commit aa3d832d5c50858b94b7dd110674192b81ebbf44 Author: HiepLM Date: Mon Dec 29 16:21:22 2025 +0700 git commit -m "first commit for v2" diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..e82b5e5 --- /dev/null +++ b/.gitignore @@ -0,0 +1,18 @@ +# ---> VisualStudioCode +.vscode/* +!.vscode/settings.json +!.vscode/tasks.json +!.vscode/launch.json +!.vscode/extensions.json +!.vscode/*.code-snippets + +# Local History for Visual Studio Code +.history/ + +# Built Visual Studio Code Extensions +*.vsix +Test +# CMakeLists.txt +Managerments/maps/activated_map.txt +Managerments/maps/initial_pose.txt +pnkx_nav/Navigations/Libraries/Ros/geometry2/tf2_ros/src/tf2_ros/* \ No newline at end of file diff --git a/Controllers/Libraries/Systems/json-3.12.0.zip b/Controllers/Libraries/Systems/json-3.12.0.zip new file mode 100644 index 0000000..e6ba6b5 Binary files /dev/null and b/Controllers/Libraries/Systems/json-3.12.0.zip differ diff --git a/Controllers/Libraries/Systems/open62541-pack-master.zip b/Controllers/Libraries/Systems/open62541-pack-master.zip new file mode 100644 index 0000000..ce24277 Binary files /dev/null and b/Controllers/Libraries/Systems/open62541-pack-master.zip differ diff --git a/Controllers/Libraries/Systems/yaml-cpp-yaml-cpp-0.6.0.zip b/Controllers/Libraries/Systems/yaml-cpp-yaml-cpp-0.6.0.zip new file mode 100644 index 0000000..05950a5 Binary files /dev/null and b/Controllers/Libraries/Systems/yaml-cpp-yaml-cpp-0.6.0.zip differ diff --git a/Controllers/Packages/amr_comunication/CMakeLists.txt b/Controllers/Packages/amr_comunication/CMakeLists.txt new file mode 100644 index 0000000..fe8e1a3 --- /dev/null +++ b/Controllers/Packages/amr_comunication/CMakeLists.txt @@ -0,0 +1,246 @@ +cmake_minimum_required(VERSION 3.0.2) +project(amr_comunication) + +## Compile as C++11, supported in ROS Kinetic and newer +add_compile_options(-std=c++17) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + roscpp + std_msgs +) + +## System dependencies are found with CMake's conventions +find_package(Boost REQUIRED COMPONENTS system thread) +find_package(PkgConfig REQUIRED) +find_package(Eigen3 REQUIRED) + +pkg_check_modules(open62541 REQUIRED open62541) +pkg_check_modules(MOSQUITTO REQUIRED libmosquitto) + +include_directories(SYSTEM ${EIGEN3_INCLUDE_DIRS}) + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a exec_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# std_msgs +# ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( + INCLUDE_DIRS include + LIBRARIES opc_ua_server vda_5050_connector ${PROJECT_NAME}_utils + CATKIN_DEPENDS roscpp std_msgs + DEPENDS Boost open62541 MOSQUITTO EIGEN3 +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIRS} +## ${MOSQUITTO_INCLUDE_DIRS} + ${BOOST_INCLUDE_DIRS} +) + +## Declare a C++ library +add_library(${PROJECT_NAME}_utils + src/vda_5050/utils/curve_common.cpp + src/vda_5050/utils/pose.cpp + src/vda_5050/utils/line_common.cpp +) + +add_library(opc_ua_server + src/opc_ua/ua_server.cpp +) + +add_library(vda_5050_connector + src/vda_5050/vda_5050_connector.cpp +) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +add_dependencies(opc_ua_server ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +add_dependencies(${PROJECT_NAME}_utils ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +add_dependencies(vda_5050_connector ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +## With catkin_make all packages are built within a single CMake context +## The recommended prefix ensures that target names across packages don't collide +add_executable(vda_5050_connector_node test/vda_5050_connector_node.cpp) + +## Rename C++ executable without prefix +## The above recommended prefix causes long target names, the following renames the +## target back to the shorter version for ease of user use +## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" +# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +target_link_libraries(opc_ua_server + ${FreeOpcUa_LIBRARIES} + ${catkin_LIBRARIES} + ${Boost_LIBRARIES} + open62541 +) + +target_link_libraries(${PROJECT_NAME}_utils + ${catkin_LIBRARIES} + ${Boost_LIBRARIES} + ${Eigen_LIBRARIES} +) + +target_link_libraries(vda_5050_connector + ${catkin_LIBRARIES} + ${Boost_LIBRARIES} + ${PROJECT_NAME}_utils + ${MOSQUITTO_LIBRARIES} +) + +target_link_libraries(vda_5050_connector_node + vda_5050_connector + ${catkin_LIBRARIES} +) +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# catkin_install_python(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html +# install(TARGETS ${PROJECT_NAME}_node +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark libraries for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html +install(TARGETS opc_ua_server vda_5050_connector ${PROJECT_NAME}_utils + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) + +## Mark cpp header files for installation +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE +) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_amr_comunication.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/Controllers/Packages/amr_comunication/include/amr_comunication/opc_ua/ua_server.h b/Controllers/Packages/amr_comunication/include/amr_comunication/opc_ua/ua_server.h new file mode 100644 index 0000000..ac0ac93 --- /dev/null +++ b/Controllers/Packages/amr_comunication/include/amr_comunication/opc_ua/ua_server.h @@ -0,0 +1,197 @@ +/** + * @file amr_server.h + * @brief This file defines the AmrOpcUAServer class for managing an OPC UA server. + * + * The AmrOpcUAServer class provides functionalities to initialize, start, stop, + * and manage access control for an OPC UA server. It includes methods for + * handling user authentication and managing server operations. + * + * This library is part of the amr_comunication package, which is designed to + * facilitate the integration of ROS (Robot Operating System) with OPC UA + * for robotic applications. + * + * @note Ensure that the Open62541 library is properly linked and configured + * to use this package effectively. + * + * @author HiepLM + * @date 16/10/2024 + */ + +#ifndef __AMR_COMUNICATION_UA_SERVER_H_INCLUDED_ +#define __AMR_COMUNICATION_UA_SERVER_H_INCLUDED_ +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include + +namespace amr_comunication +{ + class AmrOpcUAServer + { + public: + /** + * @brief Default constructor for AmrOpcUAServer + */ + AmrOpcUAServer(); + + /** + * @brief Constructor for AmrOpcUAServer with ROS node handle + * @param nh ROS NodeHandle for initialization + */ + AmrOpcUAServer(const ros::NodeHandle &nh); + + /** + * @brief Virtual destructor for AmrOpcUAServer + */ + virtual ~AmrOpcUAServer(); + + /** + * @brief Initialize the AmrOpcUAServer with a ROS node handle + * @param nh ROS NodeHandle for initialization + */ + virtual void init(const ros::NodeHandle &nh); + + /** + * @brief Stop the server operation. + * + * This method is responsible for safely shutting down the server, + * releasing any resources and stopping any ongoing processes. + */ + virtual void stop(void); + + /** + * @brief Start the server operation. + * + * This method initializes and starts the server, allowing it to + * begin processing requests from clients. + */ + virtual void start(void); + + /** + * @brief Get server object to redefine + */ + UA_Server *getServerObject(); + + /** + * @brief Get status code + */ + UA_StatusCode statusCode(); + + /** + * @brief Check server is running + */ + UA_Boolean isRunning() { return running_;} + + private: + /** + * @brief Allow adding a node to the server. + * + * This method is called to determine if a node can be added to the server. + * It is part of the access control mechanism. + * + * @param server Pointer to the UA_Server instance. + * @param ac Pointer to the UA_AccessControl instance. + * @param sessionId Pointer to the session ID of the client. + * @param sessionContext Pointer to the session context. + * @param item Pointer to the item that contains information about the node to be added. + * @return UA_Boolean Returns UA_TRUE if the addition is allowed, otherwise UA_FALSE. + */ + static UA_Boolean allowAddNode(UA_Server *server, UA_AccessControl *ac, + const UA_NodeId *sessionId, void *sessionContext, + const UA_AddNodesItem *item); + + /** + * @brief Allow adding a reference to a node. + * + * This method is called to determine if a reference can be added to a node in the server. + * It is part of the access control mechanism. + * + * @param server Pointer to the UA_Server instance. + * @param ac Pointer to the UA_AccessControl instance. + * @param sessionId Pointer to the session ID of the client. + * @param sessionContext Pointer to the session context. + * @param item Pointer to the item that contains information about the reference to be added. + * @return UA_Boolean Returns UA_TRUE if the addition is allowed, otherwise UA_FALSE. + */ + static UA_Boolean allowAddReference(UA_Server *server, UA_AccessControl *ac, + const UA_NodeId *sessionId, void *sessionContext, + const UA_AddReferencesItem *item); + + /** + * @brief Allow deleting a node from the server. + * + * This method is called to determine if a node can be deleted from the server. + * It is part of the access control mechanism. + * + * @param server Pointer to the UA_Server instance. + * @param ac Pointer to the UA_AccessControl instance. + * @param sessionId Pointer to the session ID of the client. + * @param sessionContext Pointer to the session context. + * @param item Pointer to the item that contains information about the node to be deleted. + * @return UA_Boolean Returns UA_TRUE if the deletion is allowed, otherwise UA_FALSE. + */ + static UA_Boolean allowDeleteNode(UA_Server *server, UA_AccessControl *ac, + const UA_NodeId *sessionId, void *sessionContext, + const UA_DeleteNodesItem *item); + + /** + * @brief Allow deleting a reference from a node. + * + * This method is called to determine if a reference can be deleted from a node in the server. + * It is part of the access control mechanism. + * + * @param server Pointer to the UA_Server instance. + * @param ac Pointer to the UA_AccessControl instance. + * @param sessionId Pointer to the session ID of the client. + * @param sessionContext Pointer to the session context. + * @param item Pointer to the item that contains information about the reference to be deleted. + * @return UA_Boolean Returns UA_TRUE if the deletion is allowed, otherwise UA_FALSE. + */ + static UA_Boolean allowDeleteReference(UA_Server *server, UA_AccessControl *ac, + const UA_NodeId *sessionId, void *sessionContext, + const UA_DeleteReferencesItem *item); + + /** + * @brief Handle the stop signal. + * + * This method is called when a stop signal is received (e.g., SIGINT). + * It performs necessary cleanup and stops the server operation. + * + * @param sign The signal number received. + */ + void stopHandler(int sign); + + /** + * @brief Static signal handler for stopping the server. + * + * This static method is called when a stop signal is received (e.g., SIGINT). + * It invokes the instance method stopHandler to perform the cleanup. + * + * @param sign The signal number received. + */ + static void staticSignalHandler(int sign); + + bool initalized_; ///< Indicates whether the AmrOpcUAServer has been initialized. + static volatile UA_Boolean running_; ///< Tracks the running state of the server; true if running, false if stopped. + UA_Server *server_ptr_; ///< Pointer to the UA_Server instance representing the OPC UA server. + UA_ServerConfig *config_; ///< Pointer to the server configuration (UA_ServerConfig). + UA_String url_; ///< URL. + UA_StatusCode retval_; ///< Stores the return status code from server operations. + static std::function signalHandler_; ///< Static function to handle signals for stopping the server. + std::vector logins_; ///< Vector containing user login credentials for the server. + + }; // class AMRController +} // namespace amr_comunication + +#endif // __AMR_COMUNICATION_UA_SERVER_H_INCLUDED_ \ No newline at end of file diff --git a/Controllers/Packages/amr_comunication/include/amr_comunication/vda_5050/utils/color.h b/Controllers/Packages/amr_comunication/include/amr_comunication/vda_5050/utils/color.h new file mode 100644 index 0000000..263d901 --- /dev/null +++ b/Controllers/Packages/amr_comunication/include/amr_comunication/vda_5050/utils/color.h @@ -0,0 +1,34 @@ +#ifndef COLOR_H_ +#define COLOR_H_ + +#include + +namespace agv_visualization +{ + class Color : public std_msgs::ColorRGBA + { + public: + Color() : std_msgs::ColorRGBA() {} + Color(double red, double green, double blue) : Color(red, green, blue, 1.0) {} + Color(double red, double green, double blue, double alpha) : Color() { + r = red; + g = green; + b = blue; + a = alpha; + } + + static const Color White() { return Color(1.0, 1.0, 1.0); } + static const Color Black() { return Color(0.0, 0.0, 0.0); } + static const Color Gray() { return Color(0.5, 0.5, 0.5); } + static const Color Red() { return Color(1.0, 0.0, 0.0); } + static const Color Green() { return Color(0.0, 1.0, 0.0); } + static const Color Blue() { return Color(0.0, 0.0, 1.0); } + static const Color Yellow() { return Color(1.0, 1.0, 0.0); } + static const Color Orange() { return Color(1.0, 0.5, 0.0); } + static const Color Purple() { return Color(0.5, 0.0, 1.0); } + static const Color Chartreuse() { return Color(0.5, 1.0, 0.0); } + static const Color Teal() { return Color(0.0, 1.0, 1.0); } + static const Color Pink() { return Color(1.0, 0.0, 0.5); } + }; +} +#endif //COLOR_H_ \ No newline at end of file diff --git a/Controllers/Packages/amr_comunication/include/amr_comunication/vda_5050/utils/common.h b/Controllers/Packages/amr_comunication/include/amr_comunication/vda_5050/utils/common.h new file mode 100644 index 0000000..30f4a6d --- /dev/null +++ b/Controllers/Packages/amr_comunication/include/amr_comunication/vda_5050/utils/common.h @@ -0,0 +1,472 @@ +#ifndef _VDA_5050_COMMON_H_INCLUDE_ +#define _VDA_5050_COMMON_H_INCLUDE_ +#include +#include + +namespace vda_5050 +{ + enum class OrderType + { + Base, + Horizon + }; + + struct NodePosition + { + double x, y, theta; + double allowedDeviationXY, allowedDeviationTheta; + std::string mapId, mapDescription; + }; + + struct ActionParameter + { + std::string key; + std::string value; + }; + + struct Action + { + std::string actionType; + std::string actionId; + std::string actionDescription; + std::string blockingType; + std::vector actionParameters; + }; + + struct Node + { + std::string nodeId; + int sequenceId; + std::string nodeDescription; + bool released; + NodePosition nodePosition; + std::vector> actions; + }; + + struct Corridor + { + double leftWidth; + double rightWidth; + std::string corridorRefPoint; + }; + + struct ControlPoint + { + double x, y, weight; + }; + + struct Trajectory + { + int degree; + std::vector knotVector; + std::vector controlPoints; + }; + + struct Edge + { + std::string edgeId; + int sequenceId; + std::string edgeDescription; + bool released; + std::string startNodeId; + std::string endNodeId; + double maxSpeed; + double maxHeight; + double minHeight; + double orientation; + std::string orientationType; + std::string direction; + bool rotationAllowed; + double maxRotationSpeed; + Trajectory trajectory; + double length; + Corridor corridor; + std::vector actions; + }; + + struct Order + { + int headerId; + std::string timestamp; + std::string version; + std::string manufacturer; + std::string serialNumber; + std::string orderId; + int orderUpdateId; + std::string zoneSetId; + std::vector nodes; + std::vector edges; + + OrderType getTypeOrder() + { + for (auto &edge : edges) + { + if (!edge.released) + return OrderType::Horizon; + } + for (auto &node : nodes) + { + if (!node.released) + return OrderType::Horizon; + } + return OrderType::Base; + } + }; + + struct InstantAction + { + int headerId; + std::string timestamp; + std::string version; + std::string manufacturer; + std::string serialNumber; + std::vector> actions; + }; + + struct AGVPosition + { + double x; + double y; + double theta; + std::string mapId; + bool positionInitialized; + double localizationScore; + double deviationRange; + }; + + struct Velocity + { + double vx; + double vy; + double omega; + }; + + struct BatteryState + { + double batteryCharge; + double batteryVoltage; + int8_t batteryHealth; + bool charging; + uint32_t reach; + }; + + struct SafetyState + { + std::string eStop; + bool fieldViolation; + std::string AUTO_ACK = "autoAck"; + std::string MANUAL = "manual"; + std::string REMOTE = "remote"; + std::string NONE = "none"; + }; + + struct ErrorReference + { + std::string referenceKey; + std::string referenceValue; + }; + + struct Error + { + struct ErrorLevel + { + enum ErrorLevel_EN + { + WARNING, + FATAL + }; + + operator ErrorLevel_EN() const { return level; } + + ErrorLevel &operator=(const ErrorLevel_EN &newType) + { + level = newType; + return *this; + } + + private: + ErrorLevel_EN level; + + public: + // Function to convert ActionStatus to string + std::string _toString() const + { + switch (level) + { + case WARNING: + return "WARNING"; + case FATAL: + return "FATAL"; + default: + return ""; + } + } + }; + + std::string errorType; + std::vector errorReferences; + std::string errorDescription; + ErrorLevel errorLevel; + }; + + struct NodeState + { + std::string nodeId; + int32_t sequenceId; + std::string nodeDescription; + NodePosition nodePosition; + bool released; + }; + + struct EdgeState + { + std::string edgeId; + uint32_t sequenceId; + std::string edgeDescription; + bool released; + Trajectory trajectory; + }; + + struct ActionState + { + struct ActionStatus + { + public: + enum ActionStatus_EN + { + WAITING, + INITIALIZING, + RUNNING, + PAUSED, + FINISHED, + FAILED + }; + + operator ActionStatus_EN() const { return type; } + + ActionStatus &operator=(const ActionStatus_EN &newType) + { + type = newType; + return *this; + } + + bool operator==(const ActionStatus_EN &otherType) const + { + return type == otherType; + } + + private: + ActionStatus_EN type; + + public: + // Function to convert ActionStatus to string + std::string _toString() const + { + switch (type) + { + case WAITING: + return "WAITING"; + case INITIALIZING: + return "INITIALIZING"; + case RUNNING: + return "RUNNING"; + case PAUSED: + return "PAUSED"; + case FINISHED: + return "FINISHED"; + case FAILED: + return "FAILED"; + default: + return ""; + } + } + }; + + std::string actionId; + std::string actionType; + std::string actionDescription; + ActionStatus actionStatus; + std::string resultDescription; + }; + + struct InfoReference + { + std::string referenceKey; + std::string referenceValue; + }; + + struct Info + { + std::string infoType; + std::vector infoReferences; + std::string infoDescription; + std::string infoLevel; + + std::string DEBUG = "DEBUG"; + std::string INFO = "INFO"; + }; + + struct BoundingBoxReference + { + double x; + double y; + double z; + double theta; + }; + + struct LoadDimensions + { + double length; + double width; + double height; + }; + + struct Load + { + std::string loadId; + std::string loadType; + std::string loadPosition; + BoundingBoxReference boundingBoxReference; + LoadDimensions loadDimensions; + uint32_t weight; + }; + + struct State + { + struct OperatingMode + { + enum OperatingMode_EN + { + AUTOMATIC, + SEMIAUTOMATIC, + MANUAL, + SERVICE, + TEACHING + }; + + private: + OperatingMode_EN type; + + public: + // Constructor mặc định + OperatingMode() : type(SERVICE) {} + + // Toán tử chuyển đổi sang enum + operator OperatingMode_EN() const { return type; } + + OperatingMode &operator=(const OperatingMode_EN &newType) + { + type = newType; + return *this; + } + + bool operator==(const OperatingMode_EN &otherType) const + { + return type == otherType; + } + + // Chuyển đổi sang chuỗi + std::string _toString() const + { + switch (type) + { + case AUTOMATIC: + return "AUTOMATIC"; + case SEMIAUTOMATIC: + return "SEMIAUTOMATIC"; + case MANUAL: + return "MANUAL"; + case SERVICE: + return "SERVICE"; + case TEACHING: + return "TEACHING"; + default: + return "SERVICE"; + } + } + }; + + State() + { + headerId = 1; + version = "1.0.0"; + manufacturer = "Phenikaa-X"; + serialNumber = ""; + velocity.vx = 0; + velocity.vy = 0; + velocity.omega = 0; + batteryState.batteryCharge = 100; + batteryState.batteryVoltage = 48; + batteryState.batteryHealth = 100; + batteryState.charging = false; + batteryState.reach = 0; + std::vector errors_; + errors = errors_; + } + + uint32_t headerId; + std::string timestamp; + std::string version; + std::string manufacturer; + std::string serialNumber; + std::string orderId; + uint32_t orderUpdateId; + std::string zoneSetId; + std::string lastNodeId; + uint32_t lastNodeSequenceId; + bool driving; + bool paused; + bool newBaseRequest; + double distanceSinceLastNode; + OperatingMode operatingMode; + std::vector nodeStates; + std::vector edgeStates; + std::vector> actionStates; + AGVPosition agvPosition; + Velocity velocity; + std::vector loads; + BatteryState batteryState; + std::vector errors; + std::vector information; + SafetyState safetyState; + + OrderType getTypeOrder() + { + for (auto &edge : edgeStates) + { + if (!edge.released) + return OrderType::Horizon; + } + for (auto &node : nodeStates) + { + if (!node.released) + return OrderType::Horizon; + } + return OrderType::Base; + } + }; + + struct Visualization + { + Visualization() + { + headerId = 1; + version = "1.0.0"; + manufacturer = "Phenikaa-X"; + serialNumber = "AMR2WS"; + velocity.vx = 0; + velocity.vy = 0; + velocity.omega = 0; + } + + uint32_t headerId; + std::string timestamp; + std::string version; + std::string manufacturer; + std::string serialNumber; + AGVPosition agvPosition; + Velocity velocity; + }; +} + +#endif // _VDA_5050_COMMON_H_INCLUDE_ \ No newline at end of file diff --git a/Controllers/Packages/amr_comunication/include/amr_comunication/vda_5050/utils/conversion.h b/Controllers/Packages/amr_comunication/include/amr_comunication/vda_5050/utils/conversion.h new file mode 100644 index 0000000..b70d885 --- /dev/null +++ b/Controllers/Packages/amr_comunication/include/amr_comunication/vda_5050/utils/conversion.h @@ -0,0 +1,43 @@ +#ifndef CONVERSION_H_ +#define CONVERSION_H_ +#include "curve_common.h" + +#include +#include +#include + +inline Eigen::Vector3d EigenVecter3dFromPointMsg(const geometry_msgs::Point& msg) { + return Eigen::Vector3d(msg.x, msg.y, msg.z); +} + +inline EigenTrajectoryPoint::Vector EigenTrajectoryVectorFromVector(const std::vector &plan) +{ + EigenTrajectoryPoint::Vector eigen_trajectory_point_vec; + EigenTrajectoryPoint eigen_trajectory_point; + + for(int i = 0; i < plan.size(); i++) + { + eigen_trajectory_point.position(0) = plan.at(i).pose.position.x; + eigen_trajectory_point.position(1) = plan.at(i).pose.position.y; + eigen_trajectory_point_vec.push_back(eigen_trajectory_point); + } + + return eigen_trajectory_point_vec; +} + +inline EigenTrajectoryPoint::Vector EigenTrajectoryVectorFromVector(const std::vector &discreate_point_vec) +{ + EigenTrajectoryPoint::Vector eigen_trajectory_point_vec; + EigenTrajectoryPoint eigen_trajectory_point; + + for(int i = 0; i < discreate_point_vec.size(); i++) + { + eigen_trajectory_point.position(0) = discreate_point_vec.at(i).x; + eigen_trajectory_point.position(1) = discreate_point_vec.at(i).y; + eigen_trajectory_point_vec.push_back(eigen_trajectory_point); + } + + return eigen_trajectory_point_vec; +} + +#endif //CONVERSION_H_ \ No newline at end of file diff --git a/Controllers/Packages/amr_comunication/include/amr_comunication/vda_5050/utils/curve_common.h b/Controllers/Packages/amr_comunication/include/amr_comunication/vda_5050/utils/curve_common.h new file mode 100644 index 0000000..226005b --- /dev/null +++ b/Controllers/Packages/amr_comunication/include/amr_comunication/vda_5050/utils/curve_common.h @@ -0,0 +1,81 @@ +#ifndef CURVE_COMMON_H_ +#define CURVE_COMMON_H_ + +#include +#include +#include +#include +#include "color.h" + +struct Spline_Inf +{ + int order; + std::vector knot_vector; + std::vector weight; + //std::vector N; + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + std::vector > control_point; + std::vector > N; + std::vector > dN; + std::vector > ddN; + std::vector N_double_vec; + std::vector R_double_vec; + std::vector dR_double_vec; + std::vector ddR_double_vec; + // std::vector ddN_double_vec; + //Eigen::Matrix N; //bsaic function + Eigen::MatrixXd curvefit_N; + +}; + +struct EigenTrajectoryPoint +{ + typedef std::vector > Vector; + double u_data; + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + Eigen::Vector3d position; + //Eigen::VectorXd u_data; +}; + +class CurveCommon +{ +public: + CurveCommon(); + nav_msgs::Path Generate_Line(geometry_msgs::Point start_point, geometry_msgs::Point end_point, double t_intervel, std::string frame_id); + nav_msgs::Path Generate_BezierCurve(EigenTrajectoryPoint::Vector control_point, double t_intervel, std::string frame_id); + nav_msgs::Path Generate_BsplineCurve(Spline_Inf bspline_inf, double t_intervel, std::string frame_id); + nav_msgs::Path Generate_NURBSCurve(Spline_Inf spline_inf, double t_intervel, std::string frame_id); + nav_msgs::Path Generate_DerivativeBsplineCurve(Spline_Inf bspline_inf, int differential_times, double t_intervel, std::string frame_id); + nav_msgs::Path Generate_DerivativeBasisFuncCurve(Spline_Inf bspline_inf, int differential_times, int index, double t_intervel, std::string frame_id); + + void CalculateDerivativeBasisFunc(Spline_Inf* spline_inf, double u_data, int differential_times); + geometry_msgs::Point CalculateDerivativeCurvePoint(Spline_Inf* spline_inf, double u_data, int differential_times, bool UsingNURBS); + geometry_msgs::Point CalculateCurvePoint(Spline_Inf* spline_inf, double u_data, bool UsingNURBS); + double CalculateCurvature(Spline_Inf spline_inf, double u_data, bool UsingNURBS); + double CalculateSignedCurvature(Spline_Inf spline_inf, double u_data, bool UsingNURBS); + Eigen::Vector3d CalculateCurvatureDirectionVector(Spline_Inf spline_inf, double u_data, bool UsingNURBS); + double CalculateCurvatureRadius(Spline_Inf spline_inf, double u_data, bool UsingNURBS); + double CalculateCurveLength(Spline_Inf spline_inf, double start_u, double end_u, int sub_intervals, bool UsingNURBS); + bool curveIsValid(int degree, const std::vector& knot_vector, + std::vector>& control_points); + + void ReadSplineInf(Spline_Inf* bspline_inf, int order, std::vector > control_point, std::vector knot_vector); + void ReadSplineInf(Spline_Inf* bspline_inf, std::vector weight_vector, bool use_limit_derivative_fitting); + void ReadDiscreate2DPointFromLaunch(EigenTrajectoryPoint::Vector* input_point, std::vector file_discreate_point); + void ReadDiscreate2DPointFromLaunch(std::vector >* input_point, std::vector file_discreate_point); + void ShowDiscreatePoint(visualization_msgs::Marker* points, EigenTrajectoryPoint::Vector discreate_point); + void ShowDiscreatePoint(visualization_msgs::Marker* points, std::vector > discreate_point); + + //TODO: move relate visualize function to new vislization.h + int print(); + visualization_msgs::Marker ShowDiscreatePoint(EigenTrajectoryPoint::Vector& discreate_point, const std::string& frame_id, const std::string& name, double scale); + visualization_msgs::Marker ShowDiscreatePoint2(EigenTrajectoryPoint::Vector& discreate_point, const std::string& frame_id, std_msgs::ColorRGBA point_color, const std::string& name, double scale); + visualization_msgs::Marker ShowDiscreatePoint(std::vector > discreate_point, const std::string& frame_id, const std::string& name, double scale); + visualization_msgs::Marker ShowDiscreatePoint2(std::vector > discreate_point, const std::string& frame_id, std_msgs::ColorRGBA point_color, const std::string& name, double scale); +private: + + +}; + +#endif //CURVE_COMMON_H_ \ No newline at end of file diff --git a/Controllers/Packages/amr_comunication/include/amr_comunication/vda_5050/utils/line_common.h b/Controllers/Packages/amr_comunication/include/amr_comunication/vda_5050/utils/line_common.h new file mode 100644 index 0000000..721e6cc --- /dev/null +++ b/Controllers/Packages/amr_comunication/include/amr_comunication/vda_5050/utils/line_common.h @@ -0,0 +1,14 @@ +#ifndef __LINE_COMMON_H_ +#define __LINE_COMMON_H_ + +#include +#include + +namespace vda_5050_utils +{ + double distanceFromPointToLine(double x0, double y0, double x1, double y1, double x2, double y2); + + bool isPointOnSegment(double x0, double y0, double x1, double y1, double x2, double y2, const double tolerance); +} + +#endif \ No newline at end of file diff --git a/Controllers/Packages/amr_comunication/include/amr_comunication/vda_5050/utils/pose.h b/Controllers/Packages/amr_comunication/include/amr_comunication/vda_5050/utils/pose.h new file mode 100644 index 0000000..64da7b2 --- /dev/null +++ b/Controllers/Packages/amr_comunication/include/amr_comunication/vda_5050/utils/pose.h @@ -0,0 +1,66 @@ +#ifndef AMR_CONTROL_POSE_H +#define AMR_CONTROL_POSE_H +#include +#include +#include +#include +#include +#include +#include +#include + +namespace vda_5050 +{ + class Pose { + private: + double x_, y_, yaw_; + double v_max_, accuracy_; + + void modifyYaw(void) { + while (yaw_ < -M_PI) + yaw_ += 2.0 * M_PI; + while (yaw_ > M_PI) + yaw_ -= 2.0 * M_PI; + } + + public: + Pose() : + x_(0.0), y_(0.0), yaw_(0.0), v_max_(0.0), accuracy_(0.0) { + }; + + Pose(double x, double y, double yaw) : + x_(x), y_(y), yaw_(yaw), v_max_(0.0), accuracy_(0.0) { + }; + + Pose(double x, double y, double yaw, double v_max, double accuracy) : + x_(x), y_(y), yaw_(yaw), v_max_(v_max), accuracy_(accuracy) { + }; + + ~Pose() {}; + + inline void setX(double x) { x_ = x; } + inline void setY(double y) { y_ = y; } + inline void setYaw(double yaw) { yaw_ = yaw, modifyYaw(); } + inline void setVmax(double v_max) { v_max_ = v_max; } + inline void setAccuracy(double accuracy) { accuracy_ = accuracy; } + inline void setPose(double x, double y, double yaw) { x_ = x, y_ = y, yaw_ = yaw, modifyYaw(); } + inline void setPose(double x, double y, double yaw, double v_max, double accuracy) + { + x_ = x, y_ = y, yaw_ = yaw, v_max_ = v_max, accuracy_ = accuracy, modifyYaw(); + } + inline void setPose(Pose p) { x_ = p.x_, y_ = p.y_, yaw_ = p.yaw_, modifyYaw(); } + + inline double getX(void) { return x_; } + inline double getY(void) { return y_; } + inline double getYaw(void) { return yaw_; } + inline double getVmax(void) { return v_max_; } + inline double getAccuracy(void) { return accuracy_; } + inline Pose getPose(void) { return Pose(x_, y_, yaw_, v_max_, accuracy_); } + bool SavePoseAsFile(const std::string& file_name); + bool LoadPoseFromFile(const std::string& file_name); + + }; // class Pose + +} + +#endif \ No newline at end of file diff --git a/Controllers/Packages/amr_comunication/include/amr_comunication/vda_5050/vda_5050_connector.h b/Controllers/Packages/amr_comunication/include/amr_comunication/vda_5050/vda_5050_connector.h new file mode 100644 index 0000000..c4ccc06 --- /dev/null +++ b/Controllers/Packages/amr_comunication/include/amr_comunication/vda_5050/vda_5050_connector.h @@ -0,0 +1,150 @@ +#ifndef _vda_5050_CLIENT_H_INCLUDED_ +#define _vda_5050_CLIENT_H_INCLUDED_ +#include +#include +#include +#include +#include +#include +#include "amr_comunication/vda_5050/utils/curve_common.h" +#include "amr_comunication/vda_5050/utils/pose.h" + + +namespace amr_comunication +{ + class VDA5050Connector + { + public: + struct UserParams; + + VDA5050Connector(); + virtual ~VDA5050Connector(); + + static std::unique_ptr userParams_; + static vda_5050::Order vda5050_order_; + static vda_5050::InstantAction vda5050_instant_action_; + static vda_5050::State vda5050_state_; + static vda_5050::Visualization vda5050_visualization_; + static std::mutex state_mutex; // Tạo mutex để bảo vệ vda5050_state_ + static std::mutex visualization_mutex;// Tạo mutex để bảo vệ vda5050_visualization_ + std::map> parallel_execution_list_; + + /** + * @brief execute an Order + */ + static std::function execute_order_; + + /** + * @brief execute an instant action + */ + static std::function execute_instant_action_; + + /** + * @brief execute an parallel action + */ + static std::function execute_parallel_action_; + + /** + * @brief execute an velocity max action + */ + static std::function velocity_max_action_; + + /** + * @brief execute an angular max action + */ + static std::function angular_max_action_; + + private: + + void mqtt_init(); + + void mqtt_cleanup(); + + void mqtt_connect(); + + void mqtt_reconnect(const std::string& new_host, int new_port); + + std::string getCurrentTimestamp(); + + static void mqtt_on_connect_callback(struct mosquitto* mosq, void* userdata, int result); + + static void mqtt_on_disconnect_callback(struct mosquitto* mosq, void* userdata, int rc); + + static void mqtt_message_callback(struct mosquitto* mosq, void* userdata, const struct mosquitto_message* message); + + void publishMqttTask(); + + void updatingProgress(); + + void stateInProgress(); + + void orderMsgStream(vda_5050::Order order); + + bool ParseOrderMsg(const std::string &payload, vda_5050::Order &order); + + bool ParseInstantActionMsg(const std::string& payload, vda_5050::InstantAction &instantAction); + + nlohmann::json vda5050VisualizationToJson(const vda_5050::Visualization& visualization); + + nlohmann::json vda5050StateToJson(const vda_5050::State& state); + + template bool checkObjectStructure(nlohmann::json json, std::string obj); + + static bool acceptingOrderOrOrderUpdate(const vda_5050::Order &received_order, vda_5050::Order ¤t_order); + + static bool acceptingInstantActions(const vda_5050::InstantAction &received_instantAction, vda_5050::InstantAction ¤t_instantAction); + + static bool isTheVehicleStillExecuting(); + + static bool isTheVehicleWaitingAnUpdate(); + + static bool isAGVOnNodeOrInDeviationRange(); + + static bool isAGVOnNodeOrInDeviationRangeOfNewOrder(vda_5050::Order new_order); + + static void populateNodeStates(const vda_5050::Order &receiverd_order, vda_5050::State &curent_state); + + static void appendNodeStates(const vda_5050::Order &receiverd_order, vda_5050::State &curent_state); + + static void populateEdgeStates(const vda_5050::Order &receiverd_order, vda_5050::State &curent_state); + + static void appendEdgeStates(const vda_5050::Order &receiverd_order, vda_5050::State &curent_state); + + static void populateActionStates(const vda_5050::Order &receiverd_order, vda_5050::State &curent_state); + + static void appendActionStates(const vda_5050::Order &receiverd_order, vda_5050::State &curent_state); + + static void actionsHandle(); + // properties + struct mosquitto* mosq_{ nullptr }; + std::thread publish_mqtt_thread_; + + public: + + // global variables + struct UserParams + { + UserParams() { + mqtt_hostname_ = "0.0.0.0"; + mqtt_port_ = 1883; + mqtt_keepalive_ = 60; + } + void print_params() + { + std::cout << "[amr_control_node][Param] mqtt_hostname_: " << mqtt_hostname_ << std::endl; + std::cout << "[amr_control_node][Param] mqtt_port_: " << mqtt_port_ << std::endl; + std::cout << "[amr_control_node][Param] mqtt_keepalive_: " << mqtt_keepalive_ << std::endl; + } + std::string name_; + std::string mqtt_hostname_; + std::string mqtt_client_id_; + std::string user_name_; + std::string password_; + int mqtt_port_; + int mqtt_keepalive_; + }; + }; +} // namespace amr_comunication + + +#endif // _vda_5050_CLIENT_H_INCLUDED_ \ No newline at end of file diff --git a/Controllers/Packages/amr_comunication/package.xml b/Controllers/Packages/amr_comunication/package.xml new file mode 100644 index 0000000..42b9417 --- /dev/null +++ b/Controllers/Packages/amr_comunication/package.xml @@ -0,0 +1,67 @@ + + + amr_comunication + 0.0.0 + The amr_comunication package + + + + + robotics + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + roscpp + std_msgs + + roscpp + std_msgs + + roscpp + std_msgs + + + + + + + + diff --git a/Controllers/Packages/amr_comunication/src/opc_ua/ua_server.cpp b/Controllers/Packages/amr_comunication/src/opc_ua/ua_server.cpp new file mode 100644 index 0000000..0f2a6eb --- /dev/null +++ b/Controllers/Packages/amr_comunication/src/opc_ua/ua_server.cpp @@ -0,0 +1,155 @@ +#include "amr_comunication/opc_ua/ua_server.h" +#include + +std::function amr_comunication::AmrOpcUAServer::signalHandler_; +volatile UA_Boolean amr_comunication::AmrOpcUAServer::running_ = true; + +amr_comunication::AmrOpcUAServer::AmrOpcUAServer(const ros::NodeHandle& nh) + : initalized_(false), server_ptr_(NULL), config_(NULL) +{ + if (!initalized_) this->init(nh); +} + +amr_comunication::AmrOpcUAServer::AmrOpcUAServer() + : initalized_(false), server_ptr_(NULL), config_(NULL) { +} + +amr_comunication::AmrOpcUAServer::~AmrOpcUAServer() +{ + this->stop(); +} + +void amr_comunication::AmrOpcUAServer::init(const ros::NodeHandle& nh) +{ + if (!initalized_) + { + server_ptr_ = UA_Server_new(); + config_ = UA_Server_getConfig(server_ptr_); + retval_ = UA_ServerConfig_setDefault(config_); + if (retval_ != UA_STATUSCODE_GOOD) { + UA_Server_delete(server_ptr_); + UA_ServerConfig_clean(config_); + exit(1); + } + signalHandler_ = std::bind(&amr_comunication::AmrOpcUAServer::stopHandler, this, std::placeholders::_1); + signal(SIGINT, &AmrOpcUAServer::staticSignalHandler); + signal(SIGTERM, &AmrOpcUAServer::staticSignalHandler); + + // Đặt URL máy chủ + url_ = UA_STRING((char*)"opc.tcp://0.0.0.0:4840"); + config_->serverUrls = &url_; + config_->serverUrlsSize = 1; + + // logins_.push_back({ UA_STRING_STATIC("admin"), UA_STRING_STATIC("robotics") }); + // logins_.push_back({ UA_STRING_STATIC("HiepLM"), UA_STRING_STATIC("robotics") }); + // /* Disable anonymous logins, enable two user/password logins */ + // config_->accessControl.clear(&config_->accessControl); + + + // retval_ = UA_AccessControl_default(config_, false, + // &config_->securityPolicies[config_->securityPoliciesSize - 1].policyUri, + // (int)amr_comunication::AmrOpcUAServer::logins_.size(), amr_comunication::AmrOpcUAServer::logins_.data()); + + // if (retval_ != UA_STATUSCODE_GOOD) + // exit(1); + + /* Set accessControl functions for nodeManagement */ + config_->accessControl.allowAddNode = amr_comunication::AmrOpcUAServer::allowAddNode; + config_->accessControl.allowAddReference = amr_comunication::AmrOpcUAServer::allowAddReference; + config_->accessControl.allowDeleteNode = amr_comunication::AmrOpcUAServer::allowDeleteNode; + config_->accessControl.allowDeleteReference = amr_comunication::AmrOpcUAServer::allowDeleteReference; + + // retval_ = UA_Server_run(server_ptr_, &amr_comunication::AmrOpcUAServer::running_); + initalized_ = true; + } +} + +void amr_comunication::AmrOpcUAServer::start() +{ + try + { + if (server_ptr_ != nullptr && amr_comunication::AmrOpcUAServer::running_) + { + retval_ = UA_Server_run(server_ptr_, &amr_comunication::AmrOpcUAServer::running_); + } + } + catch (const std::exception& e) + { + std::cerr << e.what() << '\n'; + } +} + +void amr_comunication::AmrOpcUAServer::stop() +{ + try + { + if (server_ptr_ != nullptr && config_ != nullptr) + { + UA_ServerConfig_clean(config_); + UA_Server_delete(server_ptr_); + } + } + catch (const std::exception& e) + { + std::cerr << e.what() << '\n'; + } +} + +UA_Server* amr_comunication::AmrOpcUAServer::getServerObject() +{ + return server_ptr_; +} + +UA_StatusCode amr_comunication::AmrOpcUAServer::statusCode() +{ + return retval_; +} + + +UA_Boolean amr_comunication::AmrOpcUAServer::allowAddNode(UA_Server* server, UA_AccessControl* ac, + const UA_NodeId* sessionId, void* sessionContext, + const UA_AddNodesItem* item) +{ + printf("Called allowAddNode\n"); + return UA_TRUE; +} + +UA_Boolean amr_comunication::AmrOpcUAServer::allowAddReference(UA_Server* server, UA_AccessControl* ac, + const UA_NodeId* sessionId, void* sessionContext, + const UA_AddReferencesItem* item) +{ + printf("Called allowAddReference\n"); + return UA_TRUE; +} + +UA_Boolean amr_comunication::AmrOpcUAServer::allowDeleteNode(UA_Server* server, UA_AccessControl* ac, + const UA_NodeId* sessionId, void* sessionContext, + const UA_DeleteNodesItem* item) +{ + printf("Called allowDeleteNode\n"); + return UA_FALSE; // Do not allow deletion from client +} + +UA_Boolean amr_comunication::AmrOpcUAServer::allowDeleteReference(UA_Server* server, UA_AccessControl* ac, + const UA_NodeId* sessionId, void* sessionContext, + const UA_DeleteReferencesItem* item) +{ + printf("Called allowDeleteReference\n"); + return UA_TRUE; +} + +void amr_comunication::AmrOpcUAServer::stopHandler(int sign) +{ + UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, (char*)"received ctrl-c"); + running_ = false; +} + +void amr_comunication::AmrOpcUAServer::staticSignalHandler(int sign) +{ + if (signalHandler_) + { + signalHandler_(sign); + } +} + + diff --git a/Controllers/Packages/amr_comunication/src/vda_5050/utils/curve_common.cpp b/Controllers/Packages/amr_comunication/src/vda_5050/utils/curve_common.cpp new file mode 100644 index 0000000..7499de2 --- /dev/null +++ b/Controllers/Packages/amr_comunication/src/vda_5050/utils/curve_common.cpp @@ -0,0 +1,1280 @@ +#include +#include "amr_comunication/vda_5050/utils/curve_common.h" +#include "amr_comunication/vda_5050/utils/conversion.h" +#include +#include + +CurveCommon::CurveCommon() +{ +} + +nav_msgs::Path CurveCommon::Generate_Line(geometry_msgs::Point start_point, geometry_msgs::Point end_point, double t_intervel, std::string frame_id) +{ + nav_msgs::Path line_result; + geometry_msgs::PoseStamped current_pose; + + line_result.header.frame_id = frame_id; + line_result.header.stamp = ros::Time::now(); + + current_pose.header.frame_id = frame_id; + + int segment = 1 / t_intervel; + double line_parameter = 0; + + for (int i = 0; i < segment; i++) + { + current_pose.header.seq = i; + current_pose.header.stamp = ros::Time::now(); + current_pose.pose.position.x = start_point.x * (1 - line_parameter) + end_point.x * (line_parameter); + current_pose.pose.position.y = start_point.y * (1 - line_parameter) + end_point.y * (line_parameter); + + line_result.poses.push_back(current_pose); + line_parameter += t_intervel; + } + + std::cout << "End cacluation" << "\n"; + return line_result; +} + +nav_msgs::Path CurveCommon::Generate_BezierCurve(EigenTrajectoryPoint::Vector control_point, double t_intervel, std::string frame_id) +{ + nav_msgs::Path bezier_curve_result; + geometry_msgs::PoseStamped current_pose; + EigenTrajectoryPoint::Vector temp_control_point_vec; + + bezier_curve_result.header.frame_id = frame_id; + bezier_curve_result.header.stamp = ros::Time::now(); + current_pose.header.frame_id = frame_id; + + int segment = 1 / t_intervel; + int control_point_length = control_point.size(); + double curve_parameter = 0; + temp_control_point_vec.reserve(control_point_length); + + for (int i = 0; i <= segment; i++) + { + temp_control_point_vec.assign(control_point.begin(), control_point.end()); + + for (int j = 1; j <= control_point_length - 1; j++) + { + for (int k = 1; k <= control_point_length - j; k++) + { + temp_control_point_vec.at(k - 1).position(0) = temp_control_point_vec.at(k - 1).position(0) * (1 - curve_parameter) + temp_control_point_vec.at(k).position(0) * curve_parameter; + temp_control_point_vec.at(k - 1).position(1) = temp_control_point_vec.at(k - 1).position(1) * (1 - curve_parameter) + temp_control_point_vec.at(k).position(1) * curve_parameter; + } + } + current_pose.header.seq = i; + current_pose.header.stamp = ros::Time::now(); + current_pose.pose.position.x = temp_control_point_vec.at(0).position(0); + current_pose.pose.position.y = temp_control_point_vec.at(0).position(1); + bezier_curve_result.poses.push_back(current_pose); + curve_parameter += t_intervel; + } + + std::cout << "End calculate Bezier Curve" << "\n"; + return bezier_curve_result; +} + +void CurveCommon::ReadDiscreate2DPointFromLaunch(EigenTrajectoryPoint::Vector *input_point, std::vector file_discreate_point) +{ + int index = 0; + Eigen::Vector3d read_point; + EigenTrajectoryPoint eigen_discreate_point; + input_point->reserve(file_discreate_point.size() / 2); + + for (int i = 0; i < file_discreate_point.size(); i++) + { + if (i % 2 == 0) + { + read_point(0) = file_discreate_point[i]; + } + else + { + read_point(1) = file_discreate_point[i]; + eigen_discreate_point.position = read_point; + // input_point->emplace_back(dis_control_point); //I don't know why have error + input_point->push_back(eigen_discreate_point); + index++; + } + } + + // Debug use + // std::cout << "input control point size : " << input_control_point->size() << "\n"; + // for(int i = 0; i < input_control_point->size(); i++) + // { + // std::cout << "control point x : " << input_control_point->at(i).position(0) << "\n"; + // std::cout << "control point y : " << input_control_point->at(i).position(1) << "\n"; + // } +} + +void CurveCommon::ReadDiscreate2DPointFromLaunch(std::vector> *input_point, std::vector file_discreate_point) +{ + Eigen::Vector3d read_point; + input_point->reserve(file_discreate_point.size() / 2); + + for (int i = 0; i < file_discreate_point.size(); i++) + { + if (i % 2 == 0) + { + read_point(0) = file_discreate_point[i]; + } + else + { + read_point(1) = file_discreate_point[i]; + input_point->push_back(read_point); + } + } + + // Debug use + // std::cout << "input control point size : " << input_point->size() << "\n"; + // for(int i = 0; i < input_point->size(); i++) + // { + // std::cout << "control point x : " << input_point->at(i)(0) << "\n"; + // std::cout << "control point y : " << input_point->at(i)(1) << "\n"; + // } +} + +visualization_msgs::Marker CurveCommon::ShowDiscreatePoint(std::vector> discreate_point, const std::string &frame_id, const std::string &name, double scale) +{ + visualization_msgs::Marker waypoints_marker; + + waypoints_marker.header.frame_id = frame_id; + waypoints_marker.header.stamp = ros::Time::now(); + waypoints_marker.type = visualization_msgs::Marker::SPHERE_LIST; + // waypoints_marker.color = color; + waypoints_marker.color.r = 1; + waypoints_marker.color.g = 1; + waypoints_marker.color.a = 0.75; + waypoints_marker.ns = name; + waypoints_marker.scale.x = scale; + waypoints_marker.scale.y = scale; + waypoints_marker.scale.z = scale; + + waypoints_marker.points.reserve(discreate_point.size()); + + geometry_msgs::Point view_point; + for (int i = 0; i < discreate_point.size(); i++) + { + view_point.x = discreate_point.at(i)(0); + view_point.y = discreate_point.at(i)(1); + waypoints_marker.points.push_back(view_point); + } + + return waypoints_marker; +} + +visualization_msgs::Marker CurveCommon::ShowDiscreatePoint2(std::vector> discreate_point, const std::string &frame_id, std_msgs::ColorRGBA point_color, const std::string &name, double scale) +{ + visualization_msgs::Marker waypoints_marker; + + waypoints_marker.header.frame_id = frame_id; + waypoints_marker.header.stamp = ros::Time::now(); + waypoints_marker.type = visualization_msgs::Marker::SPHERE_LIST; + waypoints_marker.color = point_color; + waypoints_marker.color.a = 0.75; + waypoints_marker.ns = name; + waypoints_marker.scale.x = scale; + waypoints_marker.scale.y = scale; + waypoints_marker.scale.z = scale; + + waypoints_marker.points.reserve(discreate_point.size()); + + geometry_msgs::Point view_point; + for (int i = 0; i < discreate_point.size(); i++) + { + view_point.x = discreate_point.at(i)(0); + view_point.y = discreate_point.at(i)(1); + waypoints_marker.points.push_back(view_point); + } + + return waypoints_marker; +} + +visualization_msgs::Marker CurveCommon::ShowDiscreatePoint(EigenTrajectoryPoint::Vector &discreate_point, const std::string &frame_id, const std::string &name, double scale) +{ + visualization_msgs::Marker waypoints_marker; + + waypoints_marker.header.frame_id = frame_id; + waypoints_marker.header.stamp = ros::Time::now(); + waypoints_marker.type = visualization_msgs::Marker::SPHERE_LIST; + // waypoints_marker.color = color; + waypoints_marker.color.r = 1; + waypoints_marker.color.g = 1; + waypoints_marker.color.a = 0.75; + waypoints_marker.ns = name; + waypoints_marker.scale.x = scale; + waypoints_marker.scale.y = scale; + waypoints_marker.scale.z = scale; + + waypoints_marker.points.reserve(discreate_point.size()); + + geometry_msgs::Point view_point; + for (int i = 0; i < discreate_point.size(); i++) + { + view_point.x = discreate_point.at(i).position(0); + view_point.y = discreate_point.at(i).position(1); + waypoints_marker.points.push_back(view_point); + } + + return waypoints_marker; +} + +visualization_msgs::Marker CurveCommon::ShowDiscreatePoint2(EigenTrajectoryPoint::Vector &discreate_point, const std::string &frame_id, std_msgs::ColorRGBA point_color, const std::string &name, double scale) +{ + visualization_msgs::Marker waypoints_marker; + + waypoints_marker.header.frame_id = frame_id; + waypoints_marker.header.stamp = ros::Time::now(); + waypoints_marker.type = visualization_msgs::Marker::SPHERE_LIST; + waypoints_marker.color = point_color; + // waypoints_marker.color.r = r; + // waypoints_marker.color.g = g; + // waypoints_marker.color.b = b; + waypoints_marker.color.a = 0.75; + waypoints_marker.ns = name; + waypoints_marker.scale.x = scale; + waypoints_marker.scale.y = scale; + waypoints_marker.scale.z = scale; + + waypoints_marker.points.reserve(discreate_point.size()); + + geometry_msgs::Point view_point; + for (int i = 0; i < discreate_point.size(); i++) + { + view_point.x = discreate_point.at(i).position(0); + view_point.y = discreate_point.at(i).position(1); + waypoints_marker.points.push_back(view_point); + } + + return waypoints_marker; +} + +void CurveCommon::ShowDiscreatePoint(visualization_msgs::Marker *points, EigenTrajectoryPoint::Vector discreate_point) +{ + geometry_msgs::Point view_point; + for (int i = 0; i < discreate_point.size(); i++) + { + view_point.x = discreate_point.at(i).position(0); + view_point.y = discreate_point.at(i).position(1); + points->points.push_back(view_point); + } +} + +void CurveCommon::ShowDiscreatePoint(visualization_msgs::Marker *points, std::vector> discreate_point) +{ + geometry_msgs::Point view_point; + for (int i = 0; i < discreate_point.size(); i++) + { + view_point.x = discreate_point.at(i)(0); + view_point.y = discreate_point.at(i)(1); + points->points.push_back(view_point); + } +} + +void CurveCommon::ReadSplineInf(Spline_Inf *bspline_inf, int order, std::vector> control_point, std::vector knot_vector) +{ + bspline_inf->order = order; + bspline_inf->knot_vector.assign(knot_vector.begin(), knot_vector.end()); + bspline_inf->control_point.assign(control_point.begin(), control_point.end()); + + // Debug use + // std::cout << "knot vector size : " << bspline_inf->knot_vector.size() << "\n"; + // for(int i = 0; i < bspline_inf->knot_vector.size(); i++) + // { + // std::cout << "knot vector x : " << bspline_inf->knot_vector.at(i) << "\n"; + // } + + // std::cout << "input control point size : " << bspline_inf->control_point.size() << "\n"; + // for(int i = 0; i < bspline_inf->control_point.size(); i++) + // { + // std::cout << "control point x : " << bspline_inf->control_point.at(i)(0) << ", y = " << bspline_inf->control_point.at(i)(1) << "\n"; + // } +} + +void CurveCommon::ReadSplineInf(Spline_Inf *spline_inf, std::vector weight_vector, bool use_limit_derivative_fitting) +{ + if (!use_limit_derivative_fitting) + { + if (spline_inf->control_point.size() != weight_vector.size()) + { + std::cout << "weight vector size is wrong" << "\n"; + return; + } + } + else + { + for (int i = 0; i < 2; i++) + weight_vector.push_back(1); + } + + spline_inf->weight.assign(weight_vector.begin(), weight_vector.end()); + + // Debug use + // std::cout << "knot vector size : " << spline_inf->knot_vector.size() << "\n"; + // for(int i = 0; i < spline_inf->knot_vector.size(); i++) + // { + // std::cout << "knot vector x : " << spline_inf->knot_vector.at(i) << "\n"; + // } + + // std::cout << "control point size : " << spline_inf->control_point.size() << "\n"; + // for(int i = 0; i < spline_inf->control_point.size(); i++) + // { + // std::cout << "control point x : " << spline_inf->control_point.at(i)(0) << ", y = " << spline_inf->control_point.at(i)(1) << "\n"; + // } +} + +nav_msgs::Path CurveCommon::Generate_BsplineCurve(Spline_Inf bspline_inf, double t_intervel, std::string frame_id) +{ + nav_msgs::Path bspline_curve_result; + geometry_msgs::PoseStamped current_pose; + + bspline_curve_result.header.frame_id = frame_id; + bspline_curve_result.header.stamp = ros::Time::now(); + current_pose.header.frame_id = frame_id; + + int p_degree = bspline_inf.order - 1; + int n = bspline_inf.control_point.size() - 1; + // TODO: Check knot vector size and sequence is correect + int m = bspline_inf.knot_vector.size() - 1; // The last knot vector index number + int segment = 1 / t_intervel; + double delta_t = 100; + double curve_parameter = 0; + double left_denom = 0, right_denom = 0; + double left_term = 0, right_term = 0; + double sum_x = 0, sum_y = 0; + std::vector curve_parameter_vec; + Eigen::VectorXd temp_basic_function; + std::vector> temp_basic_function_eigen; + + // Debug use + // for(int i = 0; i < bspline_inf.knot_vector.size(); i++) + // { + // std::cout << "knot vector x : " << bspline_inf.knot_vector.at(i) << "\n"; + // } + + // std::cout << "input control point size : " << bspline_inf.control_point.size() << "\n"; + // for(int i = 0; i < bspline_inf.control_point.size(); i++) + // { + // std::cout << "control point x : " << bspline_inf.control_point.at(i)(0) << ", y = " << bspline_inf.control_point.at(i)(1) << "\n"; + // } + + // TODO: Can optimal this part code + delta_t = 1 / (double)(segment - 1); + curve_parameter_vec.resize(segment); + for (int u = 0; u < segment; u++) + { + curve_parameter_vec[u] = curve_parameter; + curve_parameter += delta_t; + } + + bspline_inf.N.clear(); + temp_basic_function.resize(segment); + + // TODO: Check basic function boundary is correct + // Calculate degree = 0's basic function + for (int i = 0; i < m; i++) + { + if (bspline_inf.knot_vector.at(i) != bspline_inf.knot_vector.at(n)) + { + for (int u = 0; u < segment; u++) + { + if (bspline_inf.knot_vector.at(i) <= curve_parameter_vec[u] && curve_parameter_vec[u] < bspline_inf.knot_vector.at(i + 1)) + temp_basic_function(u) = 1; + else + temp_basic_function(u) = 0; + + // std::cout << "small temp basic function index : " << u << " small temp basic function value : " << temp_basic_function(u) << "\n"; + // std::cout << "small temp basic function value : " << temp_basic_function(u) << "\n"; + } + } + else + { + for (int u = 0; u < segment; u++) + { + if (bspline_inf.knot_vector.at(i) <= curve_parameter_vec[u] && curve_parameter_vec[u] <= bspline_inf.knot_vector.at(i + 1)) + temp_basic_function(u) = 1; + else + temp_basic_function(u) = 0; + + // std::cout << "temp basic function index : " << u << "temp basic function value : " << temp_basic_function(u) << "\n"; + // std::cout << "small temp basic function value : " << temp_basic_function(u) << "\n"; + } + } + + // if(bspline_inf.knot_vector.at(i) < bspline_inf.knot_vector.at(i+1)) + // { + // if(bspline_inf.knot_vector.at(i) != bspline_inf.knot_vector.at(n)) + // { + // for(int u = 0; u < segment; u++) + // { + // if(bspline_inf.knot_vector.at(i) <= curve_parameter_vec[u] && curve_parameter_vec[u] < bspline_inf.knot_vector.at(i+1)) + // temp_basic_function(u) = 1; + // else + // temp_basic_function(u) = 0; + + // std::cout << "small temp basic function index : " << u << " small temp basic function value : " << temp_basic_function(u) << "\n"; + // } + // } + // else + // { + // for(int u = 0; u < segment; u++) + // { + // if(bspline_inf.knot_vector.at(i) <= curve_parameter_vec[u] && curve_parameter_vec[u] <= bspline_inf.knot_vector.at(i+1)) + // temp_basic_function(u) = 1; + // else + // temp_basic_function(u) = 0; + + // std::cout << "n basic function index : " << u << " n basic function value : " << temp_basic_function(u) << "\n"; + // //std::cout << "small temp basic function value : " << temp_basic_function(u) << "\n"; + // } + // } + // } + // else + // { + // for(int u = 0; u < segment; u++) + // { + // if(bspline_inf.knot_vector.at(i) <= curve_parameter_vec[u] && curve_parameter_vec[u] <= bspline_inf.knot_vector.at(i+1)) + // temp_basic_function(u) = 1; + // else + // temp_basic_function(u) = 0; + + // std::cout << "temp basic function index : " << u << " temp basic function value : " << temp_basic_function(u) << "\n"; + // } + // } + bspline_inf.N.push_back(temp_basic_function); + // std::cout << i << "'s bspline_inf Ni,0 size " << temp_basic_function.size() << "\n"; + } + + // Calculate the rest of basic function + for (int p = 1; p <= p_degree; p++) + { + temp_basic_function_eigen.clear(); + for (int i = 0; i < (m - p); i++) + { + for (int u = 0; u < segment; u++) + { + left_denom = bspline_inf.knot_vector.at(p + i) - bspline_inf.knot_vector.at(i); + right_denom = bspline_inf.knot_vector.at(i + p + 1) - bspline_inf.knot_vector.at(i + 1); + + if (left_denom == 0) + left_term = 0; + else + left_term = (curve_parameter_vec[u] - bspline_inf.knot_vector.at(i)) * bspline_inf.N.at(i)(u) / left_denom; + + if (right_denom == 0) + right_term = 0; + else + right_term = (bspline_inf.knot_vector.at(i + p + 1) - curve_parameter_vec[u]) * bspline_inf.N.at(i + 1)(u) / right_denom; + + temp_basic_function(u) = left_term + right_term; + } + temp_basic_function_eigen.push_back(temp_basic_function); + } + bspline_inf.N = temp_basic_function_eigen; + } + + for (int u = 0; u < segment; u++) + { + sum_x = 0; + sum_y = 0; + for (int i = 0; i < bspline_inf.N.size(); i++) + { + sum_x += bspline_inf.control_point.at(i)(0) * bspline_inf.N.at(i)(u); + sum_y += bspline_inf.control_point.at(i)(1) * bspline_inf.N.at(i)(u); + // std::cout << i << "'s ," << u <<"'s bspline_inf value : " << bspline_inf.N.at(i)(u) << "\n"; + } + // std::cout << u << "'s current_pose x : " << sum_x << "\n"; + // std::cout << u << "'s current_pose y : " << sum_y << "\n"; + current_pose.header.seq = u; + current_pose.header.stamp = ros::Time::now(); + current_pose.pose.position.x = sum_x; + current_pose.pose.position.y = sum_y; + bspline_curve_result.poses.push_back(current_pose); + } + + std::cout << "End Calculate" << "\n"; + return bspline_curve_result; +} + +nav_msgs::Path CurveCommon::Generate_NURBSCurve(Spline_Inf spline_inf, double t_intervel, std::string frame_id) +{ + nav_msgs::Path nurbs_curve_result; + geometry_msgs::PoseStamped current_pose; + + nurbs_curve_result.header.frame_id = frame_id; + nurbs_curve_result.header.stamp = ros::Time::now(); + current_pose.header.frame_id = frame_id; + + int p_degree = spline_inf.order - 1; + int n = spline_inf.control_point.size() - 1; + // TODO: Check knot vector size and sequence is correect + int m = spline_inf.knot_vector.size() - 1; // The last knot vector index number + int segment = 1 / t_intervel; + double delta_t = 100; + double curve_parameter = 0; + double left_denom = 0, right_denom = 0; + double left_term = 0, right_term = 0; + double sum_nurbs_denom = 0; + double sum_x = 0, sum_y = 0; + std::vector curve_parameter_vec; + Eigen::VectorXd temp_basic_function; + std::vector> temp_basic_function_eigen; + + // Debug use + // for(int i = 0; i < bspline_inf.knot_vector.size(); i++) + // { + // std::cout << "knot vector x : " << bspline_inf.knot_vector.at(i) << "\n"; + // } + + // std::cout << "input control point size : " << bspline_inf.control_point.size() << "\n"; + // for(int i = 0; i < bspline_inf.control_point.size(); i++) + // { + // std::cout << "control point x : " << bspline_inf.control_point.at(i)(0) << ", y = " << bspline_inf.control_point.at(i)(1) << "\n"; + // } + + if (spline_inf.weight.size() == 0) + { + std::cout << "weight vector size is zero, please call read ReadSplineInf function" << "\n"; + return nurbs_curve_result; + } + + // TODO: Can optimal this part code + delta_t = 1 / (double)(segment - 1); + curve_parameter_vec.resize(segment); + for (int u = 0; u < segment; u++) + { + curve_parameter_vec[u] = curve_parameter; + curve_parameter += delta_t; + } + + spline_inf.N.clear(); + temp_basic_function.resize(segment); + + // TODO: Check basic function boundary is correct + // Calculate degree = 0's basic function + for (int i = 0; i < m; i++) + { + // if(spline_inf.knot_vector.at(i) != spline_inf.knot_vector.at(n)) + if (spline_inf.knot_vector.at(i) != spline_inf.knot_vector.at(i + 1)) + { + for (int u = 0; u < segment; u++) + { + if (spline_inf.knot_vector.at(i) <= curve_parameter_vec[u] && curve_parameter_vec[u] < spline_inf.knot_vector.at(i + 1)) + temp_basic_function(u) = 1; + else + temp_basic_function(u) = 0; + + // std::cout << "small temp basic function index : " << u << " small temp basic function value : " << temp_basic_function(u) << "\n"; + // std::cout << "small temp basic function value : " << temp_basic_function(u) << "\n"; + } + } + else + { + for (int u = 0; u < segment; u++) + { + if (spline_inf.knot_vector.at(i) <= curve_parameter_vec[u] && curve_parameter_vec[u] <= spline_inf.knot_vector.at(i + 1)) + temp_basic_function(u) = 1; + else + temp_basic_function(u) = 0; + + // std::cout << "temp basic function index : " << u << "temp basic function value : " << temp_basic_function(u) << "\n"; + // std::cout << "small temp basic function value : " << temp_basic_function(u) << "\n"; + } + } + + spline_inf.N.push_back(temp_basic_function); + // std::cout << i << "'s bspline_inf Ni,0 size " << temp_basic_function.size() << "\n"; + } + + // Calculate the rest of basic function + for (int p = 1; p <= p_degree; p++) + { + temp_basic_function_eigen.clear(); + for (int i = 0; i < (m - p); i++) // m-p is calculate how many region between basis function + { + for (int u = 0; u < segment; u++) + { + left_denom = spline_inf.knot_vector.at(p + i) - spline_inf.knot_vector.at(i); + right_denom = spline_inf.knot_vector.at(i + p + 1) - spline_inf.knot_vector.at(i + 1); + + if (left_denom == 0) + left_term = 0; + else + left_term = (curve_parameter_vec[u] - spline_inf.knot_vector.at(i)) * spline_inf.N.at(i)(u) / left_denom; + + if (right_denom == 0) + right_term = 0; + else + right_term = (spline_inf.knot_vector.at(i + p + 1) - curve_parameter_vec[u]) * spline_inf.N.at(i + 1)(u) / right_denom; + + temp_basic_function(u) = left_term + right_term; + } + temp_basic_function_eigen.push_back(temp_basic_function); + } + spline_inf.N = temp_basic_function_eigen; + } + + for (int u = 0; u < segment; u++) + { + sum_nurbs_denom = 0; + sum_x = 0; + sum_y = 0; + for (int i = 0; i < spline_inf.N.size(); i++) + { + sum_nurbs_denom += spline_inf.N.at(i)(u) * spline_inf.weight.at(i); + sum_x += spline_inf.control_point.at(i)(0) * spline_inf.N.at(i)(u) * spline_inf.weight.at(i); + sum_y += spline_inf.control_point.at(i)(1) * spline_inf.N.at(i)(u) * spline_inf.weight.at(i); + // std::cout << i << "'s ," << u <<"'s bspline_inf value : " << bspline_inf.N.at(i)(u) << "\n"; + } + sum_x /= sum_nurbs_denom; + sum_y /= sum_nurbs_denom; + // std::cout << u << "'s current_pose x : " << sum_x << "\n"; + // std::cout << u << "'s current_pose y : " << sum_y << "\n"; + current_pose.header.seq = u; + current_pose.header.stamp = ros::Time::now(); + current_pose.pose.position.x = sum_x; + current_pose.pose.position.y = sum_y; + nurbs_curve_result.poses.push_back(current_pose); + } + + std::cout << "End calculate NURBS function" << "\n"; + return nurbs_curve_result; +} + +void CurveCommon::CalculateDerivativeBasisFunc(Spline_Inf *spline_inf, double u_data, int differential_times) +{ + int p_degree = spline_inf->order - 1; + int n = spline_inf->control_point.size() - 1; + // TODO: Check knot vector size and sequence is correect + int m = spline_inf->knot_vector.size() - 1; // The last knot vector index number + double left_denom = 0, right_denom = 0; + double left_term = 0, right_term = 0; + double derivative_left_term = 0, derivative_right_term = 0; + double derivative_value = 0; + std::vector temp_basic_function; + Eigen::VectorXd derivative_temp_basic_function; + double degree_basis_value; + std::vector> derivative_basic_function_eigen; + + // Calculate degree = 0's basic function + spline_inf->N_double_vec.clear(); + spline_inf->N_double_vec.resize(m); + for (int i = 0; i < m; i++) + { + // if(spline_inf->knot_vector.at(i) != spline_inf->knot_vector.at(n)) + if (spline_inf->knot_vector.at(i) != spline_inf->knot_vector.at(i + 1)) + { + if (spline_inf->knot_vector.at(i) <= u_data && u_data < spline_inf->knot_vector.at(i + 1)) + degree_basis_value = 1; + else + degree_basis_value = 0; + } + else + { + if (spline_inf->knot_vector.at(i) <= u_data && u_data <= spline_inf->knot_vector.at(i + 1)) + degree_basis_value = 1; + else + degree_basis_value = 0; + } + + spline_inf->N_double_vec.at(i) = degree_basis_value; + } + + // Calculate the rest of basic function + spline_inf->dN.clear(); + for (int p = 1; p <= p_degree; p++) + { + temp_basic_function.clear(); + derivative_temp_basic_function.resize(m - p); + for (int i = 0; i < (m - p); i++) + { + left_denom = spline_inf->knot_vector.at(p + i) - spline_inf->knot_vector.at(i); + right_denom = spline_inf->knot_vector.at(i + p + 1) - spline_inf->knot_vector.at(i + 1); + + if (left_denom == 0) + { + left_term = 0; + derivative_left_term = 0; + } + else + { + left_term = (u_data - spline_inf->knot_vector.at(i)) * spline_inf->N_double_vec.at(i) / left_denom; + derivative_left_term = spline_inf->N_double_vec.at(i) / left_denom; + } + + if (right_denom == 0) + { + right_term = 0; + derivative_right_term = 0; + } + else + { + right_term = (spline_inf->knot_vector.at(i + p + 1) - u_data) * spline_inf->N_double_vec.at(i + 1) / right_denom; + derivative_right_term = spline_inf->N_double_vec.at(i + 1) / right_denom; + } + + derivative_value = p * (derivative_left_term - derivative_right_term); + temp_basic_function.push_back(left_term + right_term); // Calculate Ni,p + derivative_temp_basic_function(i) = derivative_value; // Calculate dNi,p + } + spline_inf->N_double_vec.assign(temp_basic_function.begin(), temp_basic_function.end()); + spline_inf->dN.push_back(derivative_temp_basic_function); + } + + if (differential_times > 1) + { + spline_inf->ddN.clear(); + for (int p = 2; p <= p_degree; p++) + { + derivative_temp_basic_function.resize(m - p); + for (int i = 0; i < (m - p); i++) + { + left_denom = spline_inf->knot_vector.at(p + i) - spline_inf->knot_vector.at(i); + right_denom = spline_inf->knot_vector.at(i + p + 1) - spline_inf->knot_vector.at(i + 1); + + if (left_denom == 0) + derivative_left_term = 0; + else + derivative_left_term = spline_inf->dN.at(p - 2)(i) / left_denom; + + if (right_denom == 0) + derivative_right_term = 0; + else + derivative_right_term = spline_inf->dN.at(p - 2)(i + 1) / right_denom; + + derivative_value = p * (derivative_left_term - derivative_right_term); + derivative_temp_basic_function(i) = derivative_value; // Calculate ddNi,p + } + spline_inf->ddN.push_back(derivative_temp_basic_function); + } + } +} + +geometry_msgs::Point CurveCommon::CalculateDerivativeCurvePoint(Spline_Inf *spline_inf, double u_data, int differential_times, bool UsingNURBS) +{ + geometry_msgs::Point derivative_curve_point; + int p_degree = spline_inf->order - 1; + double sum_x = 0, sum_y = 0; + double sum_denom = 0; + double sum_derivative_once_denom = 0; + double sum_derivative_twice_denom = 0; + double R_fraction = 0; + double dR_left_term_fraction = 0; + double dR_right_term = 0; + double ddR_left_term_fraction = 0; + double ddR_median_term_fraction = 0; + double ddR_right_term_fraction = 0; + + spline_inf->R_double_vec.clear(); + spline_inf->dR_double_vec.clear(); + spline_inf->ddR_double_vec.clear(); + + // Debug use + // std::cout << "N_double_vec :" << "\n"; + // for(int i = 0; i < spline_inf->N_double_vec.size(); i++) + // { + // std::cout << spline_inf->N_double_vec.at(i) << " "; + // } + // std::cout << "\n"; + + // std::cout << "dN :" << "\n"; + // for(int i = 0; i < spline_inf->dN.back().size(); i++) + // { + // std::cout << spline_inf->dN.back()(i) << " "; + // } + // std::cout << "\n"; + + // TODO: Safety check + if (UsingNURBS == false) + { + if (differential_times == 1) + { + for (int i = 0; i < spline_inf->control_point.size(); i++) + { + sum_x += spline_inf->control_point.at(i)(0) * spline_inf->dN.back()(i); + sum_y += spline_inf->control_point.at(i)(1) * spline_inf->dN.back()(i); + // std::cout << i << "'s spline_inf.dN : " << spline_inf->dN.back()(i) << "\n"; + } + } + if (differential_times == 2) + { + for (int i = 0; i < spline_inf->control_point.size(); i++) + { + sum_x += spline_inf->control_point.at(i)(0) * spline_inf->ddN.back()(i); + sum_y += spline_inf->control_point.at(i)(1) * spline_inf->ddN.back()(i); + // std::cout << i << "'s spline_inf.ddN : " << spline_inf->ddN.back()(i) << "\n"; + } + } + } + else + { + // TODO: Safety check differential_times + for (int i = 0; i < spline_inf->control_point.size(); i++) + { + sum_denom += spline_inf->N_double_vec.at(i) * spline_inf->weight.at(i); + sum_derivative_once_denom += spline_inf->dN.back()(i) * spline_inf->weight.at(i); + if (differential_times == 2) + sum_derivative_twice_denom += spline_inf->ddN.back()(i) * spline_inf->weight.at(i); + } + for (int i = 0; i < spline_inf->control_point.size(); i++) + { + if (sum_denom != 0) + { + R_fraction = spline_inf->N_double_vec.at(i) * spline_inf->weight.at(i); + + dR_left_term_fraction = spline_inf->dN.back()(i) * spline_inf->weight.at(i); + dR_right_term = R_fraction * sum_derivative_once_denom / std::pow(sum_denom, 2); + + if (differential_times == 2) + { + ddR_left_term_fraction = spline_inf->ddN.back()(i) * spline_inf->weight.at(i); + ddR_median_term_fraction = 2 * dR_left_term_fraction * sum_derivative_once_denom + R_fraction * sum_derivative_twice_denom; + ddR_right_term_fraction = 2 * R_fraction * std::pow(sum_derivative_once_denom, 2); + spline_inf->ddR_double_vec.push_back((ddR_left_term_fraction / sum_denom) - (ddR_median_term_fraction / std::pow(sum_denom, 2)) + (ddR_right_term_fraction / std::pow(sum_denom, 3))); + } + + spline_inf->R_double_vec.push_back(R_fraction / sum_denom); + spline_inf->dR_double_vec.push_back((dR_left_term_fraction / sum_denom) - dR_right_term); + } + else + { + spline_inf->R_double_vec.push_back(0); + spline_inf->dR_double_vec.push_back(0); + spline_inf->ddR_double_vec.push_back(0); + } + } + + if (differential_times == 1) + { + for (int i = 0; i < spline_inf->control_point.size(); i++) + { + sum_x += spline_inf->control_point.at(i)(0) * spline_inf->dR_double_vec.at(i); + sum_y += spline_inf->control_point.at(i)(1) * spline_inf->dR_double_vec.at(i); + // std::cout << i << "'s spline_inf.dN : " << spline_inf->dN.back()(i) << "\n"; + } + } + if (differential_times == 2) + { + for (int i = 0; i < spline_inf->control_point.size(); i++) + { + sum_x += spline_inf->control_point.at(i)(0) * spline_inf->ddR_double_vec.at(i); + sum_y += spline_inf->control_point.at(i)(1) * spline_inf->ddR_double_vec.at(i); + } + } + } + + derivative_curve_point.x = sum_x; + derivative_curve_point.y = sum_y; + derivative_curve_point.z = 0; + + // Debug use + // std::cout << u_data << "'s derivative current_pose x : " << derivative_curve_point.x << "\n"; + // std::cout << u_data << "'s derivative current_pose y : " << derivative_curve_point.y << "\n"; + + return derivative_curve_point; +} + +nav_msgs::Path CurveCommon::Generate_DerivativeBsplineCurve(Spline_Inf bspline_inf, int differential_times, double t_intervel, std::string frame_id) +{ + geometry_msgs::Point derivative_point_result; + nav_msgs::Path bspline_derivative_result; + geometry_msgs::PoseStamped current_pose; + + bspline_derivative_result.header.frame_id = frame_id; + bspline_derivative_result.header.stamp = ros::Time::now(); + current_pose.header.frame_id = frame_id; + + int p_degree = bspline_inf.order - 1; + int n = bspline_inf.control_point.size() - 1; + // TODO: Check knot vector size and sequence is correect + int m = bspline_inf.knot_vector.size() - 1; // The last knot vector index number + int segment = 1 / t_intervel; + double delta_t = 0; + double curve_parameter = 0; + std::vector curve_parameter_vec; + + // TODO: Can optimal this part code + delta_t = 1 / (double)(segment - 1); + curve_parameter_vec.resize(segment); + for (int u = 0; u < segment; u++) + { + curve_parameter_vec[u] = curve_parameter; + curve_parameter += delta_t; + } + + // TODO: Can support high order derivative in loop + for (int u = 0; u < segment; u++) + { + CalculateDerivativeBasisFunc(&bspline_inf, curve_parameter_vec[u], differential_times); + derivative_point_result = CalculateDerivativeCurvePoint(&bspline_inf, curve_parameter_vec[u], differential_times, false); + current_pose.header.seq = u; + current_pose.header.stamp = ros::Time::now(); + current_pose.pose.position.x = derivative_point_result.x; + current_pose.pose.position.y = derivative_point_result.y; + bspline_derivative_result.poses.push_back(current_pose); + } + + return bspline_derivative_result; +} + +nav_msgs::Path CurveCommon::Generate_DerivativeBasisFuncCurve(Spline_Inf bspline_inf, int differential_times, int index, double t_intervel, std::string frame_id) +{ + geometry_msgs::Point derivative_point_result; + nav_msgs::Path derivative_basis_result; + geometry_msgs::PoseStamped current_pose; + + derivative_basis_result.header.frame_id = frame_id; + derivative_basis_result.header.stamp = ros::Time::now(); + current_pose.header.frame_id = frame_id; + + // TODO: Add error ckeck function(below code with error) + // if(differential_times == 0 && index < bspline_inf.N_double_vec.size()) + // { + // std::cout << "Without this index's basis function, vector size is :" << bspline_inf.N_double_vec.size() << "\n"; + // return derivative_basis_result; + // } + // if(differential_times == 1 && index < bspline_inf.dN.back().size()) + // { + // std::cout << "Without this index's basis function, vector size is :" << bspline_inf.dN.back().size() << "\n"; + // return derivative_basis_result; + // } + // if(differential_times == 2 && index < bspline_inf.ddN.back().size()) + // { + // std::cout << "Without this index's basis function, vector size is :" << bspline_inf.ddN.back().size() << "\n"; + // return derivative_basis_result; + // } + + int p_degree = bspline_inf.order - 1; + int n = bspline_inf.control_point.size() - 1; + // TODO: Check knot vector size and sequence is correect + int m = bspline_inf.knot_vector.size() - 1; // The last knot vector index number + int segment = 1 / t_intervel; + double delta_t = 0; + double curve_parameter = 0; + std::vector curve_parameter_vec; + + // TODO: Can optimal this part code + delta_t = 1 / (double)(segment - 1); + curve_parameter_vec.resize(segment); + for (int u = 0; u < segment; u++) + { + curve_parameter_vec[u] = curve_parameter; + curve_parameter += delta_t; + } + + // TODO: Can support high order derivative in loop + for (int u = 0; u < segment; u++) + { + CalculateDerivativeBasisFunc(&bspline_inf, curve_parameter_vec[u], differential_times); + current_pose.header.seq = u; + current_pose.header.stamp = ros::Time::now(); + current_pose.pose.position.x = curve_parameter_vec[u]; + if (differential_times == 0) + current_pose.pose.position.y = bspline_inf.N_double_vec.at(index); + else if (differential_times == 1) + current_pose.pose.position.y = bspline_inf.dN.back()(index); + else + current_pose.pose.position.y = bspline_inf.ddN.back()(index); + derivative_basis_result.poses.push_back(current_pose); + } + + return derivative_basis_result; +} + +double CurveCommon::CalculateCurvature(Spline_Inf spline_inf, double u_data, bool UsingNURBS) +{ + if (u_data < 0) + { + std::cout << "Error, u_data need bigger than 0" << "\n"; + std::cout << "error u_data value is: " << u_data << "\n"; + return 0; + } + + if (u_data > 1) + { + std::cout << "Error!! because u_data bigger than 1, maybe is program numerical error" << "\n"; + u_data = 1; + } + + Eigen::Vector3d derivative_once_point; + Eigen::Vector3d derivative_twice_point; + + double curvature = 0; + double fraction = 0; + double denominator = 0; + + CalculateDerivativeBasisFunc(&spline_inf, u_data, 2); + derivative_once_point = EigenVecter3dFromPointMsg(CalculateDerivativeCurvePoint(&spline_inf, u_data, 1, UsingNURBS)); + derivative_twice_point = EigenVecter3dFromPointMsg(CalculateDerivativeCurvePoint(&spline_inf, u_data, 2, UsingNURBS)); + + fraction = derivative_once_point.cross(derivative_twice_point).lpNorm<2>(); + denominator = std::pow(derivative_once_point.lpNorm<2>(), 3); + + if (denominator == 0) + curvature = 0; + else + curvature = fraction / denominator; + + // TODO: Check curvature direction (but in nurbs planner not useful) + // Eigen::Vector3d direction_vector; //curvature direction + // direction_vector = derivative_once_point.cross(derivative_twice_point.cross(derivative_once_point)); + // std::cout << "direction vector x: " << direction_vector(0) << "\n"; + // std::cout << "direction vector y: " << direction_vector(1) << "\n"; + // std::cout << "direction vector z: " << direction_vector(2) << "\n"; + // if(direction_vector(0) < 0 && direction_vector(1) < 0 || direction_vector(0) > 0 && direction_vector(1) < 0) + // curvature *= -1; + + return curvature; +} + +double CurveCommon::CalculateSignedCurvature(Spline_Inf spline_inf, double u_data, bool UsingNURBS) +{ + if (u_data < 0) + { + std::cout << "Error, u_data need bigger than 0" << "\n"; + std::cout << "error u_data value is: " << u_data << "\n"; + return 0; + } + + if (u_data > 1) + { + std::cout << "Error!! because u_data bigger than 1, maybe is program numerical error" << "\n"; + u_data = 1; + } + + Eigen::Vector3d derivative_once_point; + Eigen::Vector3d derivative_twice_point; + + double curvature = 0; + double fraction = 0; + double denominator = 0; + + CalculateDerivativeBasisFunc(&spline_inf, u_data, 2); + derivative_once_point = EigenVecter3dFromPointMsg(CalculateDerivativeCurvePoint(&spline_inf, u_data, 1, UsingNURBS)); + derivative_twice_point = EigenVecter3dFromPointMsg(CalculateDerivativeCurvePoint(&spline_inf, u_data, 2, UsingNURBS)); + + fraction = derivative_once_point.cross(derivative_twice_point).lpNorm<2>(); + denominator = std::pow(derivative_once_point.lpNorm<2>(), 3); + + if (denominator == 0) + curvature = 0; + else + curvature = fraction / denominator; + + // TODO: Check curvature direction (but in nurbs planner not useful) + Eigen::Vector3d direction_vector; // curvature direction + direction_vector = derivative_once_point.cross(derivative_twice_point.cross(derivative_once_point)); + // std::cout << "direction vector x: " << direction_vector(0) << "\n"; + // std::cout << "direction vector y: " << direction_vector(1) << "\n"; + // std::cout << "direction vector z: " << direction_vector(2) << "\n"; + if (direction_vector(0) < 0 && direction_vector(1) < 0 || direction_vector(0) > 0 && direction_vector(1) < 0) + curvature *= -1; + + return curvature; +} + +Eigen::Vector3d CurveCommon::CalculateCurvatureDirectionVector(Spline_Inf spline_inf, double u_data, bool UsingNURBS) +{ + Eigen::Vector3d direction_vector; + if (u_data < 0) + { + std::cout << "Error, u_data need bigger than 0" << "\n"; + std::cout << "error u_data value is: " << u_data << "\n"; + return direction_vector; + } + + if (u_data > 1) + { + std::cout << "Error!! because u_data bigger than 1, maybe is program numerical error" << "\n"; + u_data = 1; + } + + Eigen::Vector3d derivative_once_point; + Eigen::Vector3d derivative_twice_point; + + CalculateDerivativeBasisFunc(&spline_inf, u_data, 2); + derivative_once_point = EigenVecter3dFromPointMsg(CalculateDerivativeCurvePoint(&spline_inf, u_data, 1, UsingNURBS)); + derivative_twice_point = EigenVecter3dFromPointMsg(CalculateDerivativeCurvePoint(&spline_inf, u_data, 2, UsingNURBS)); + + direction_vector = derivative_once_point.cross(derivative_twice_point.cross(derivative_once_point)); + // std::cout << "direction vector x: " << direction_vector(0) << "\n"; + // std::cout << "direction vector y: " << direction_vector(1) << "\n"; + // std::cout << "direction vector z: " << direction_vector(2) << "\n"; + + return direction_vector; +} + +double CurveCommon::CalculateCurvatureRadius(Spline_Inf spline_inf, double u_data, bool UsingNURBS) +{ + double curvature_radius = 0; + double curvature = 0; + + curvature = CalculateCurvature(spline_inf, u_data, UsingNURBS); + + if (curvature == 0) + return curvature_radius = 0; + else + return curvature_radius = 1 / curvature; +} + +double CurveCommon::CalculateCurveLength(Spline_Inf spline_inf, double start_u, double end_u, int sub_intervals, bool UsingNURBS) +{ + if (sub_intervals % 2 != 0) + { + std::cout << "Error! sub_intervals value is a even number" << "\n"; + return 0; + } + + Eigen::Vector3d eigen_derivative_point; + double sum_length = 0; + double interval = 0; + double u_index = 0; + + interval = (end_u - start_u) / (double)sub_intervals; + + for (int i = 0; i <= sub_intervals; i++) + { + u_index = start_u + interval * i; + CalculateDerivativeBasisFunc(&spline_inf, u_index, 1); + eigen_derivative_point = EigenVecter3dFromPointMsg(CalculateDerivativeCurvePoint(&spline_inf, u_index, 1, UsingNURBS)); + + if (i == 0 || i == sub_intervals) + sum_length += eigen_derivative_point.lpNorm<2>(); + else if ((i % 2) == 1) + sum_length += 4 * eigen_derivative_point.lpNorm<2>(); + else if ((i % 2) == 0) + sum_length += 2 * eigen_derivative_point.lpNorm<2>(); + } + + sum_length *= interval / 3; + + return sum_length; +} + +bool CurveCommon::curveIsValid(int degree, const std::vector &knot_vector, + std::vector> &control_points) +{ + if (degree < 1 || degree > 9) + { + ROS_WARN("degree is invalid value"); + return false; + } + if (!((knot_vector.size() - degree - 1) == control_points.size())) + { + ROS_WARN("relation between degree, number of knots, and number of control points is invalid"); + return false; + } + return true; +} + +geometry_msgs::Point CurveCommon::CalculateCurvePoint(Spline_Inf *spline_inf, double u_data, bool UsingNURBS) +{ + // TODO: Check u = 1 bug, why x=nan and y=nan? + + geometry_msgs::Point curve_point; + int p_degree = spline_inf->order - 1; + int n = spline_inf->control_point.size() - 1; + // TODO: Check knot vector size and sequence is correect + int m = spline_inf->knot_vector.size() - 1; // The last knot vector index number + double left_denom = 0, right_denom = 0; + double left_term = 0, right_term = 0; + std::vector temp_basic_function; + double degree_basis_value; + double sum_nurbs_denom = 0; + double sum_x = 0, sum_y = 0; + + // Calculate degree = 0's basic function + spline_inf->N_double_vec.clear(); + spline_inf->N_double_vec.resize(m); + for (int i = 0; i < m; i++) + { + // if(spline_inf->knot_vector.at(i) != spline_inf->knot_vector.at(n)) + if (spline_inf->knot_vector.at(i) != spline_inf->knot_vector.at(i + 1)) + { + if (spline_inf->knot_vector.at(i) <= u_data && u_data < spline_inf->knot_vector.at(i + 1)) + degree_basis_value = 1; + else + degree_basis_value = 0; + } + else + { + if (spline_inf->knot_vector.at(i) <= u_data && u_data <= spline_inf->knot_vector.at(i + 1)) + degree_basis_value = 1; + else + degree_basis_value = 0; + } + + spline_inf->N_double_vec.at(i) = degree_basis_value; + } + + // Calculate the rest of basic function + for (int p = 1; p <= p_degree; p++) + { + temp_basic_function.clear(); + for (int i = 0; i < (m - p); i++) + { + left_denom = spline_inf->knot_vector.at(p + i) - spline_inf->knot_vector.at(i); + right_denom = spline_inf->knot_vector.at(i + p + 1) - spline_inf->knot_vector.at(i + 1); + + if (left_denom == 0) + { + left_term = 0; + } + else + { + left_term = (u_data - spline_inf->knot_vector.at(i)) * spline_inf->N_double_vec.at(i) / left_denom; + } + + if (right_denom == 0) + { + right_term = 0; + } + else + { + right_term = (spline_inf->knot_vector.at(i + p + 1) - u_data) * spline_inf->N_double_vec.at(i + 1) / right_denom; + } + temp_basic_function.push_back(left_term + right_term); // Calculate Ni,p + } + spline_inf->N_double_vec.assign(temp_basic_function.begin(), temp_basic_function.end()); + } + + if (!UsingNURBS) + { + for (int i = 0; i < spline_inf->N_double_vec.size(); i++) + { + sum_x += spline_inf->control_point.at(i)(0) * spline_inf->N_double_vec.at(i); + sum_y += spline_inf->control_point.at(i)(1) * spline_inf->N_double_vec.at(i); + // std::cout << i << "'s ," << u <<"'s bspline_inf value : " << bspline_inf.N.at(i)(u) << "\n"; + } + // std::cout << u << "'s current_pose x : " << sum_x << "\n"; + // std::cout << u << "'s current_pose y : " << sum_y << "\n"; + curve_point.x = sum_x; + curve_point.y = sum_y; + } + else + { + if (spline_inf->weight.size() == 0) + { + std::cout << "weight vector size is zero, please call read ReadSplineInf function" << "\n"; + curve_point.x = 0; + curve_point.y = 0; + return curve_point; + } + + for (int i = 0; i < spline_inf->N_double_vec.size(); i++) + { + sum_nurbs_denom += spline_inf->N_double_vec.at(i) * spline_inf->weight.at(i); + sum_x += spline_inf->control_point.at(i)(0) * spline_inf->N_double_vec.at(i) * spline_inf->weight.at(i); + sum_y += spline_inf->control_point.at(i)(1) * spline_inf->N_double_vec.at(i) * spline_inf->weight.at(i); + // std::cout << i << "'s ," << u <<"'s bspline_inf value : " << bspline_inf.N.at(i)(u) << "\n"; + } + sum_x /= sum_nurbs_denom; + sum_y /= sum_nurbs_denom; + // std::cout << u << "'s current_pose x : " << sum_x << "\n"; + // std::cout << u << "'s current_pose y : " << sum_y << "\n"; + curve_point.x = sum_x; + curve_point.y = sum_y; + } + + return curve_point; +} \ No newline at end of file diff --git a/Controllers/Packages/amr_comunication/src/vda_5050/utils/line_common.cpp b/Controllers/Packages/amr_comunication/src/vda_5050/utils/line_common.cpp new file mode 100644 index 0000000..9f4df9e --- /dev/null +++ b/Controllers/Packages/amr_comunication/src/vda_5050/utils/line_common.cpp @@ -0,0 +1,26 @@ +#include "amr_comunication/vda_5050/utils/line_common.h" + +double vda_5050_utils::distanceFromPointToLine(double x0, double y0, double x1, double y1, double x2, double y2) +{ + double A = y2 - y1; + double B = -(x2 - x1); + double C = -A * x1 - B * y1; + + return fabs(A * x0 + B * y0 + C) / sqrt(A * A + B * B); +} + +bool vda_5050_utils::isPointOnSegment(double x0, double y0, double x1, double y1, double x2, double y2, const double tolerance) +{ + double crossProduct = distanceFromPointToLine(x0, y0, x1, y1, x2, y2); + if (fabs(crossProduct) > fabs(tolerance)) + return false; + + // Kiểm tra tọa độ của P có nằm giữa A và B không + if (x0 >= std::min(x1, x2) && x0 <= std::max(x1, x2) && + y0 >= std::min(y1, y2) && y0 <= std::max(y1, y2)) + { + return true; + } + + return false; +} \ No newline at end of file diff --git a/Controllers/Packages/amr_comunication/src/vda_5050/utils/pose.cpp b/Controllers/Packages/amr_comunication/src/vda_5050/utils/pose.cpp new file mode 100644 index 0000000..6df26c8 --- /dev/null +++ b/Controllers/Packages/amr_comunication/src/vda_5050/utils/pose.cpp @@ -0,0 +1,39 @@ +#include "amr_comunication/vda_5050/utils/pose.h" + + +bool vda_5050::Pose::SavePoseAsFile(const std::string& file_name) +{ + bool result = false; + std::ofstream file(file_name); + if (file.is_open()) + { + file << x_ << " " << y_ << " " << yaw_ << "\n"; + file.close(); + result = true; + } + else + { + result = false; + } + return result; +} + +bool vda_5050::Pose::LoadPoseFromFile(const std::string& file_name) +{ + bool result = false; + std::ifstream file(file_name); + if (file.is_open()) + { + double x, y, yaw; + file >> x >> y >> yaw; + setPose(x, y, yaw); + file.close(); + result = true; + } + else + { + result = false; + } + return result; +} + diff --git a/Controllers/Packages/amr_comunication/src/vda_5050/vda_5050_connector.cpp b/Controllers/Packages/amr_comunication/src/vda_5050/vda_5050_connector.cpp new file mode 100644 index 0000000..e7b6391 --- /dev/null +++ b/Controllers/Packages/amr_comunication/src/vda_5050/vda_5050_connector.cpp @@ -0,0 +1,1488 @@ +#include "amr_comunication/vda_5050/vda_5050_connector.h" +#include "amr_comunication/vda_5050/utils/line_common.h" +#include + +amr_comunication::VDA5050Connector::VDA5050Connector() +{ + try + { + this->userParams_ = std::make_unique(); + ros::NodeHandle nh_priv; + nh_priv.param("MQTT/Name", userParams_->name_, userParams_->name_); + nh_priv.param("MQTT/Host", userParams_->mqtt_hostname_, userParams_->mqtt_hostname_); + nh_priv.param("MQTT/Port", userParams_->mqtt_port_, userParams_->mqtt_port_); + nh_priv.param("MQTT/Client_ID", userParams_->mqtt_client_id_, userParams_->mqtt_client_id_); + nh_priv.param("MQTT/Username", userParams_->user_name_, userParams_->user_name_); + nh_priv.param("MQTT/Password", userParams_->password_, userParams_->password_); + nh_priv.param("MQTT/Keep_Alive", userParams_->mqtt_keepalive_, userParams_->mqtt_keepalive_); + + this->mqtt_init(); + this->mqtt_connect(); + this->publish_mqtt_thread_ = std::thread(&amr_comunication::VDA5050Connector::updatingProgress, this); + } + catch (const std::exception &e) + { + ROS_ERROR_STREAM(e.what()); + } +} + +amr_comunication::VDA5050Connector::~VDA5050Connector() +{ + this->mqtt_cleanup(); + if (this->userParams_) + this->userParams_.reset(); +} + +std::string amr_comunication::VDA5050Connector::getCurrentTimestamp() +{ + auto now = std::chrono::system_clock::now(); + + auto in_time_t = std::chrono::system_clock::to_time_t(now); + + auto microseconds = std::chrono::duration_cast(now.time_since_epoch()) % 1000000; + + std::stringstream ss; + ss << std::put_time(std::localtime(&in_time_t), "%Y-%m-%dT%H:%M:%S"); + + ss << '.' << std::setfill('0') << std::setw(6) << microseconds.count(); + + return ss.str(); +} + +void amr_comunication::VDA5050Connector::mqtt_init() +{ + mosquitto_lib_init(); + this->mosq_ = mosquitto_new(userParams_->mqtt_client_id_.c_str(), true, NULL); + if (!this->mosq_) + { + throw std::runtime_error("Failed to create mosquitto instance."); + ROS_ERROR_STREAM("Failed to create mosquitto instance."); + } + mosquitto_message_callback_set(this->mosq_, &amr_comunication::VDA5050Connector::mqtt_message_callback); + mosquitto_connect_callback_set(this->mosq_, &amr_comunication::VDA5050Connector::mqtt_on_connect_callback); + mosquitto_disconnect_callback_set(this->mosq_, &amr_comunication::VDA5050Connector::mqtt_on_disconnect_callback); +} + +void amr_comunication::VDA5050Connector::mqtt_cleanup() +{ + mosquitto_destroy(this->mosq_); + mosquitto_lib_cleanup(); +} + +void amr_comunication::VDA5050Connector::mqtt_connect() +{ + ROS_INFO("Connecting mqtt broker ..."); + int login = mosquitto_username_pw_set(this->mosq_, userParams_->user_name_.c_str(), userParams_->password_.c_str()); + if (login == MOSQ_ERR_INVAL) + throw std::runtime_error("Unable to login to broker. The input parameters were invalid"); + + if (login == MOSQ_ERR_NOMEM) + throw std::runtime_error("Unable to login to broker. An out of memory condition occurred."); + + if (mosquitto_connect(this->mosq_, userParams_->mqtt_hostname_.c_str(), userParams_->mqtt_port_, userParams_->mqtt_keepalive_)) + { + ROS_ERROR("Unable to connect to broker."); + std::this_thread::sleep_for(std::chrono::seconds(5)); + this->mqtt_reconnect(userParams_->mqtt_hostname_, userParams_->mqtt_port_); + // throw std::runtime_error("Unable to connect to broker."); + } + + mosquitto_loop_start(this->mosq_); +} + +void amr_comunication::VDA5050Connector::mqtt_reconnect(const std::string &new_host, int new_port) +{ + ROS_INFO("Reconnecting to new MQTT broker at %s:%d ...", new_host.c_str(), new_port); + + // Stop the loop + mosquitto_loop_stop(this->mosq_, true); // true = finish current messages + mosquitto_disconnect(this->mosq_); + + // Optionally, set new credentials here if needed again + int login = mosquitto_username_pw_set(this->mosq_, userParams_->user_name_.c_str(), userParams_->password_.c_str()); + if (login != MOSQ_ERR_SUCCESS) + throw std::runtime_error("Failed to set username/password: " + std::string(mosquitto_strerror(login))); + + // Connect to the new host and port + int rc = mosquitto_connect(this->mosq_, new_host.c_str(), new_port, 60); + if (rc != MOSQ_ERR_SUCCESS) + { + // throw std::runtime_error("Failed to connect to broker: " + std::string(mosquitto_strerror(rc))); + ROS_ERROR("Failed to connect to broker: %s", std::string(mosquitto_strerror(rc)).c_str()); + std::this_thread::sleep_for(std::chrono::seconds(5)); + this->mqtt_reconnect(userParams_->mqtt_hostname_, userParams_->mqtt_port_); + } + + // Start the loop again + rc = mosquitto_loop_start(this->mosq_); + if (rc != MOSQ_ERR_SUCCESS) + throw std::runtime_error("Failed to start Mosquitto loop: " + std::string(mosquitto_strerror(rc))); +} + +void amr_comunication::VDA5050Connector::mqtt_on_connect_callback(struct mosquitto *mosq, void *userdata, int result) +{ + amr_comunication::VDA5050Connector *client = static_cast(userdata); + if (result == 0) + { + ROS_INFO_STREAM("Connected to MQTT broker successfully!"); + // Subscribe lại topic sau khi kết nối lại thành công + mosquitto_subscribe(mosq, NULL, "order", 0); + mosquitto_subscribe(mosq, NULL, "instantActions", 0); + } + else + { + ROS_ERROR_STREAM("Failed to connect: " << mosquitto_strerror(result)); + } +} + +void amr_comunication::VDA5050Connector::mqtt_on_disconnect_callback(struct mosquitto *mosq, void *userdata, int rc) +{ + amr_comunication::VDA5050Connector *client = static_cast(userdata); + if (rc != 0) + { + ROS_ERROR_STREAM("Unexpected disconnect, reconnecting..."); + std::this_thread::sleep_for(std::chrono::seconds(5)); + mosquitto_reconnect(mosq); + } + else + { + ROS_INFO_STREAM("Disconnected from broker cleanly."); + } +} + +void amr_comunication::VDA5050Connector::mqtt_message_callback(struct mosquitto *mosq, void *userdata, const struct mosquitto_message *message) +{ + amr_comunication::VDA5050Connector *client = static_cast(userdata); + if (message->payloadlen) + { + std::string payload(static_cast(message->payload), message->payloadlen); + std::string mqtt_topic(message->topic); + if (mqtt_topic == "order") + { + ROS_INFO("order"); + vda_5050::Order received_order; + // All formatting and JSON data types are correct? + bool is_received_order_vaild = client->ParseOrderMsg(payload, received_order); + if (!is_received_order_vaild) + return; + + if (received_order.manufacturer != "phenikaaX") + return; + + if (received_order.serialNumber != userParams_->mqtt_client_id_) + return; + + if (!amr_comunication::VDA5050Connector::acceptingOrderOrOrderUpdate(received_order, amr_comunication::VDA5050Connector::vda5050_order_)) + { + ROS_ERROR("Reject order"); + } + else + { + if (amr_comunication::VDA5050Connector::execute_order_) + amr_comunication::VDA5050Connector::execute_order_(); + } + } + else if (mqtt_topic == "instantActions") + { + ROS_INFO("instantActions"); + vda_5050::InstantAction action; + if (!client->ParseInstantActionMsg(payload, action)) + return; + if (action.manufacturer != "phenikaaX") + return; + if (action.serialNumber != userParams_->mqtt_client_id_) + return; + + if (!amr_comunication::VDA5050Connector::acceptingInstantActions( + action, amr_comunication::VDA5050Connector::vda5050_instant_action_)) + { + ROS_ERROR("Reject instantAtion"); + } + else + { + if (amr_comunication::VDA5050Connector::execute_instant_action_) + amr_comunication::VDA5050Connector::execute_instant_action_(); + } + } + } + else + { + ROS_INFO_STREAM("Other message received"); + } +} + +void amr_comunication::VDA5050Connector::publishMqttTask() +{ + // nav_2d_msgs::Pose2DStamped robot_pose_on_map; + userParams_->mqtt_client_id_ = userParams_->mqtt_client_id_; + vda5050_visualization_.serialNumber = userParams_->mqtt_client_id_; + vda5050_visualization_.timestamp = this->getCurrentTimestamp(); + nlohmann::json virsualization_js = this->vda5050VisualizationToJson(this->vda5050_visualization_); + std::string visualization_payload = virsualization_js.dump(); + if (this->mosq_ != nullptr) + { + int pub_ret1 = mosquitto_publish(this->mosq_, NULL, "visualization", (int)visualization_payload.length(), visualization_payload.c_str(), 0, false); + } + if (vda5050_visualization_.headerId > (UINT32_MAX - 1)) + { + vda5050_visualization_.headerId = 0; + } + else + { + vda5050_visualization_.headerId++; + } + + vda5050_state_.serialNumber = userParams_->mqtt_client_id_; + vda5050_state_.timestamp = this->getCurrentTimestamp(); + nlohmann::json state_js = this->vda5050StateToJson(this->vda5050_state_); + + std::string state_payload = state_js.dump(); + if (this->mosq_ != nullptr) + { + int pub_ret2 = mosquitto_publish(this->mosq_, NULL, "state", (int)state_payload.length(), state_payload.c_str(), 0, false); + } + if (vda5050_state_.headerId > (UINT32_MAX - 1)) + { + vda5050_state_.headerId = 0; + } + else + { + vda5050_state_.headerId++; + } +} + +void amr_comunication::VDA5050Connector::updatingProgress() +{ + ros::Rate r(50); + ros::Time publish_time = ros::Time::now(); + while (ros::ok()) + { + this->stateInProgress(); + + if ((ros::Time::now() - publish_time) >= ros::Duration(0.1)) + { + this->publishMqttTask(); + publish_time = ros::Time::now(); + } + r.sleep(); + ros::spinOnce(); + } +} + +void amr_comunication::VDA5050Connector::stateInProgress() +{ + auto cache_state = amr_comunication::VDA5050Connector::vda5050_state_; + auto cache_order = amr_comunication::VDA5050Connector::vda5050_order_; + if (!cache_state.driving) + { + if (cache_state.nodeStates.size() == 1 && cache_state.edgeStates.empty()) + { + std::string end_node_state_id = cache_state.nodeStates.begin()->nodeId; + auto node_excute = std::find_if(cache_order.nodes.begin(), cache_order.nodes.end(), + [&end_node_state_id](const vda_5050::Node &n) + { + ROS_INFO("node[%s] [%s]", end_node_state_id.c_str(), n.nodeId.c_str()); + return end_node_state_id == n.nodeId; + }); + if (node_excute != cache_order.nodes.end()) + { + ROS_INFO("Excute node[%s]", node_excute->nodeDescription.c_str()); + for (auto action : node_excute->actions) + { + if (parallel_execution_list_.find(action->actionId) == parallel_execution_list_.end()) + { + parallel_execution_list_.insert({action->actionId, action}); + ROS_INFO("%s %s", action->actionId.c_str(), action->actionType.c_str()); + } + } + if (amr_comunication::VDA5050Connector::execute_parallel_action_) + { + amr_comunication::VDA5050Connector::execute_parallel_action_(); + } + } + // std::lock_guard lock(state_mutex); + auto &global_state = amr_comunication::VDA5050Connector::vda5050_state_; + global_state.nodeStates.erase(global_state.nodeStates.begin(), global_state.nodeStates.end()); + } + } + else + { + if (!cache_state.nodeStates.empty() && !cache_state.edgeStates.empty() && (int)cache_state.nodeStates.size() > 1) + { + std::map::iterator> edge_state_map; + for (auto egde_state = cache_state.edgeStates.begin(); egde_state != cache_state.edgeStates.end(); ++egde_state) + { + edge_state_map.insert({egde_state->edgeId, egde_state}); + } + + std::map::iterator> node_state_map; + for (auto node_state = cache_state.nodeStates.begin(); node_state != cache_state.nodeStates.end(); ++node_state) + { + node_state_map.insert({node_state->nodeId, node_state}); + } + + std::map::iterator> edge_map; + for (auto edge = cache_order.edges.begin(); edge != cache_order.edges.end(); ++edge) + { + edge_map.insert({edge->edgeId, edge}); + } + + std::map::iterator> node_map; + for (auto node = cache_order.nodes.begin(); node != cache_order.nodes.end(); ++node) + { + node_map.insert({node->nodeId, node}); + } + + double distance_to_start = std::numeric_limits::infinity(); + bool started_path = false; + std::vector::iterator edge_state_close; + Spline_Inf *input_spline_inf = new Spline_Inf(); + CurveCommon *curve_design = new CurveCommon(); + + // Toa do hien tai + auto position = cache_state.agvPosition; + for (auto edge_state = cache_state.edgeStates.begin(); edge_state != cache_state.edgeStates.end(); ++edge_state) + { + std::vector poses_on_edge; + std::vector> control_points; + std::vector knot_vector; + std::vector weight_vector; + int degree = 0; + int order_ = 0; + control_points.reserve(edge_state->trajectory.controlPoints.size()); + knot_vector.reserve(edge_state->trajectory.knotVector.size()); + weight_vector.reserve(edge_state->trajectory.controlPoints.size()); + for (auto ctr_point : edge_state->trajectory.controlPoints) + { + control_points.push_back(Eigen::Vector3d(ctr_point.x, ctr_point.y, 0)); + weight_vector.push_back(ctr_point.weight); + } + for (auto knotVector : edge_state->trajectory.knotVector) + { + knot_vector.push_back(knotVector); + } + degree = (int)edge_state->trajectory.degree; + if (curve_design->curveIsValid(degree, knot_vector, control_points)) + { + double step = 0.01; + order_ = degree + 1; + input_spline_inf->control_point.clear(); + input_spline_inf->knot_vector.clear(); + input_spline_inf->weight.clear(); + curve_design->ReadSplineInf(input_spline_inf, order_, control_points, knot_vector); + curve_design->ReadSplineInf(input_spline_inf, weight_vector, false); + for (double u_test = 0; u_test <= 1; u_test += step) + { + geometry_msgs::Point curve_point; + curve_point = curve_design->CalculateCurvePoint(input_spline_inf, u_test, true); + if (!std::isnan(curve_point.x) && !std::isnan(curve_point.y)) + poses_on_edge.push_back(vda_5050::Pose(curve_point.x, curve_point.y, 0)); + } + + for (auto pose : poses_on_edge) + { + const double distance = std::hypot((pose.getX() - position.x), (pose.getY() - position.y)); + if (distance_to_start > distance) + { + distance_to_start = distance; + edge_state_close = edge_state; + started_path = true; + } + } + } + } + delete (input_spline_inf); + delete (curve_design); + + if (!started_path) + { + ROS_ERROR_NAMED("VDA5050Connector", "None of the node of the edge were in the order."); + if (velocity_max_action_) + velocity_max_action_(0.3); + + if (angular_max_action_) + angular_max_action_(0.3); + } + else + { + if (auto search_edge = edge_map.find(edge_state_close->edgeId); search_edge != edge_map.end()) + { + auto start_node_m = node_map.find(search_edge->second->startNodeId); + auto end_node_m = node_map.find(search_edge->second->endNodeId); + if (start_node_m == node_map.end() || end_node_m == node_map.end()) + { + ROS_WARN_STREAM_THROTTLE(1.0, "Not found end_node_m\n"); + return; + } + // ROS_INFO_STREAM_THROTTLE(1.0, "Found " << start_node_m->second->nodeDescription << ' ' << end_node_m->second->nodeDescription); + + auto position = cache_state.agvPosition; + const double start_dx = start_node_m->second->nodePosition.x - position.x; + const double start_dy = start_node_m->second->nodePosition.y - position.y; + const double end_dx = end_node_m->second->nodePosition.x - position.x; + const double end_dy = end_node_m->second->nodePosition.y - position.y; + + auto start_node_state_m = node_state_map.find(start_node_m->second->nodeId); + auto end_node_state_m = node_state_map.find(end_node_m->second->nodeId); + auto edge_state_m = edge_state_map.find(search_edge->second->edgeId); + if (start_node_state_m != node_state_map.end() && end_node_state_m != node_state_map.end() && edge_state_m != edge_state_map.end()) + { + // std::lock_guard lock(state_mutex); + auto &global_state = amr_comunication::VDA5050Connector::vda5050_state_; + if (auto edge_order = edge_map.find(edge_state_m->first); edge_order != edge_map.end()) + { + if (velocity_max_action_) + velocity_max_action_((double)edge_order->second->maxSpeed); + + if (angular_max_action_) + angular_max_action_((double)edge_order->second->maxRotationSpeed); + } + else + { + if (velocity_max_action_) + velocity_max_action_(0.3); + if (angular_max_action_) + angular_max_action_(0.3); + } + + if (std::hypot(end_dx, end_dy) <= end_node_m->second->nodePosition.allowedDeviationXY) + { + auto node_state_erase = std::find_if(global_state.nodeStates.begin(), global_state.nodeStates.end(), + [&start_node_state_m](const vda_5050::NodeState &ns) + { + return start_node_state_m->second->nodeId == ns.nodeId; + }); + + if (node_state_erase != global_state.nodeStates.end()) + global_state.nodeStates.erase(node_state_erase); + + auto edge_state_erase = std::find_if(global_state.edgeStates.begin(), global_state.edgeStates.end(), + [&edge_state_m](const vda_5050::EdgeState &ns) + { + return edge_state_m->second->edgeId == ns.edgeId; + }); + + if (edge_state_erase != global_state.edgeStates.end()) + global_state.edgeStates.erase(edge_state_erase); + + auto node_state_excute = std::find_if(global_state.nodeStates.begin(), global_state.nodeStates.end(), + [&end_node_state_m](const vda_5050::NodeState &ns) + { + return end_node_state_m->second->nodeId == ns.nodeId; + }); + + if (node_state_erase != global_state.nodeStates.end() && + edge_state_erase != global_state.edgeStates.end() && + node_state_excute != global_state.nodeStates.end()) + { + global_state.lastNodeId = end_node_m->second->nodeId; + global_state.lastNodeSequenceId++; + global_state.distanceSinceLastNode = std::hypot(end_dx, end_dy); + + auto &global_order = amr_comunication::VDA5050Connector::vda5050_order_; + auto node_excute = std::find_if(global_order.nodes.begin(), global_order.nodes.end(), + [&node_state_excute](const vda_5050::Node &n) + { + return node_state_excute->nodeId == n.nodeId; + }); + if (node_excute != global_order.nodes.end()) + { + for (auto action : node_excute->actions) + { + if (parallel_execution_list_.find(action->actionId) == parallel_execution_list_.end()) + { + parallel_execution_list_.insert({action->actionId, action}); + } + } + if (amr_comunication::VDA5050Connector::execute_parallel_action_) + { + amr_comunication::VDA5050Connector::execute_parallel_action_(); + } + } + } + } + else + { + // std::lock_guard lock(state_mutex); + auto &global_state = amr_comunication::VDA5050Connector::vda5050_state_; + global_state.lastNodeId = start_node_m->second->nodeId; + global_state.distanceSinceLastNode = std::hypot(start_dx, start_dy); + } + } + } + else + ROS_ERROR_STREAM_THROTTLE(1.0, "Not found search_edge\n"); + } + } + } +} + +void amr_comunication::VDA5050Connector::orderMsgStream(vda_5050::Order order) +{ + std::stringstream msg; + msg << "\n" + << "headerId: " << order.headerId << "\n" + << "timestamp: " << order.timestamp << "\n" + << "version: " << order.version << "\n" + << "manufacturer: " << order.manufacturer << "\n" + << "serialNumber: " << order.serialNumber << "\n" + << "orderId: " << order.orderId << "\n" + << "orderUpdateId: " << order.orderUpdateId << "\n" + << "zoneSetId: " << order.zoneSetId << "\n"; + + msg << "Nodes[" << (int)order.nodes.size() << "]: \n"; + for (auto &n : order.nodes) + { + msg << " " << "nodeId: " << n.nodeId << "\n" + << " " << "sequenceId: " << n.sequenceId << "\n" + << " " << "nodeDescription: " << n.nodeDescription << "\n" + << " " << "released: " << n.released << "\n" + << " " << "nodePosition: " << "\n"; + + msg << " " << "x: " << n.nodePosition.x << "\n" + << " " << "y: " << n.nodePosition.y << "\n" + << " " << "theta: " << n.nodePosition.theta << "\n" + << " " << "allowedDeviationXY: " << n.nodePosition.allowedDeviationXY << "\n" + << " " << "allowedDeviationTheta: " << n.nodePosition.allowedDeviationTheta << "\n" + << " " << "mapId: " << n.nodePosition.mapId << "\n" + << " " << "mapDescription: " << n.nodePosition.mapDescription << "\n"; + + msg << " " << "actions[" << (int)n.actions.size() << "]: \n"; + for (auto &action : n.actions) + { + msg << " " << "actionType: " << action->actionType << "\n" + << " " << "actionId: " << action->actionId << "\n" + << " " << "actionDescription: " << action->actionDescription << "\n" + << " " << "blockingType: " << action->blockingType << "\n"; + msg << " " << "actionParameters[" << (int)action->actionParameters.size() << "]: \n"; + for (auto &apt : action->actionParameters) + { + msg << " " << "key: " << apt.key << "\n" + << " " << "value: " << apt.value << "\n"; + } + } + msg << std::endl; + } + ROS_INFO_STREAM(msg.str()); +} + +bool amr_comunication::VDA5050Connector::ParseOrderMsg(const std::string &payload, vda_5050::Order &order) +{ + nlohmann::json j; + try + { + j = nlohmann::json::parse(payload); + } + catch (const nlohmann::json::parse_error &e) + { + ROS_ERROR_STREAM("JSON parse error: " << e.what() << std::endl); + ROS_INFO("ParseOrderMsg : \n%s", payload.c_str()); + return false; + } + catch (const nlohmann::json::invalid_iterator &e) + { + ROS_ERROR_STREAM("JSON invalid iterator: " << e.what() << std::endl); + ROS_INFO("ParseOrderMsg : \n%s", payload.c_str()); + return false; + } + catch (const nlohmann::json::type_error &e) + { + ROS_ERROR_STREAM("JSON type error: " << e.what() << std::endl); + ROS_INFO("ParseOrderMsg : \n%s", payload.c_str()); + return false; + } + catch (const nlohmann::json::out_of_range &e) + { + ROS_ERROR_STREAM("JSON out of range: " << e.what() << std::endl); + ROS_INFO("ParseOrderMsg : \n%s", payload.c_str()); + return false; + } + catch (const nlohmann::json::other_error &e) + { + ROS_ERROR_STREAM("JSON other error: " << e.what() << std::endl); + ROS_INFO("ParseOrderMsg : \n%s", payload.c_str()); + return false; + } + + // Parse header info + if (!this->checkObjectStructure(j, "headerId")) + return false; + order.headerId = j["headerId"]; + + if (!this->checkObjectStructure(j, "timestamp")) + return false; + order.timestamp = j["timestamp"]; + + if (!this->checkObjectStructure(j, "version")) + return false; + order.version = j["version"]; + + if (!this->checkObjectStructure(j, "manufacturer")) + return false; + order.manufacturer = j["manufacturer"]; + + if (!this->checkObjectStructure(j, "serialNumber")) + return false; + order.serialNumber = j["serialNumber"]; + + if (!this->checkObjectStructure(j, "orderId")) + return false; + order.orderId = j["orderId"]; + + if (!this->checkObjectStructure(j, "orderUpdateId")) + return false; + order.orderUpdateId = j["orderUpdateId"]; + + if (!this->checkObjectStructure(j, "zoneSetId")) + return false; + order.zoneSetId = j["zoneSetId"]; + + if (!j["nodes"].is_array()) + { + ROS_WARN("'nodes' is not array type"); + return false; + } + // Parse nodes + for (const auto &nodeJson : j["nodes"]) + { + vda_5050::Node node; + if (!this->checkObjectStructure(nodeJson, "nodeId")) + return false; + node.nodeId = nodeJson["nodeId"]; + + if (!this->checkObjectStructure(nodeJson, "sequenceId")) + return false; + node.sequenceId = nodeJson["sequenceId"]; + + if (!this->checkObjectStructure(nodeJson, "nodeDescription")) + return false; + node.nodeDescription = nodeJson["nodeDescription"]; + + if (!this->checkObjectStructure(nodeJson, "released")) + return false; + node.released = nodeJson["released"]; + + // Parse node position + if (!this->checkObjectStructure(nodeJson, "nodePosition")) + return false; + + auto nodePosition = nodeJson["nodePosition"]; + + if (!this->checkObjectStructure(nodePosition, "x")) + return false; + node.nodePosition.x = nodePosition["x"]; + + if (!this->checkObjectStructure(nodePosition, "y")) + return false; + node.nodePosition.y = nodePosition["y"]; + + if (!this->checkObjectStructure(nodePosition, "theta")) + return false; + node.nodePosition.theta = nodePosition["theta"]; + + if (!this->checkObjectStructure(nodePosition, "allowedDeviationXY")) + return false; + node.nodePosition.allowedDeviationXY = nodePosition["allowedDeviationXY"]; + + if (!this->checkObjectStructure(nodePosition, "allowedDeviationTheta")) + return false; + node.nodePosition.allowedDeviationTheta = nodePosition["allowedDeviationTheta"]; + + if (!this->checkObjectStructure(nodePosition, "mapId")) + return false; + node.nodePosition.mapId = nodePosition["mapId"]; + + if (!this->checkObjectStructure(nodePosition, "mapDescription")) + return false; + node.nodePosition.mapDescription = nodePosition["mapDescription"]; + + if (!nodeJson["actions"].is_array()) + { + ROS_WARN("'actions' is not array type"); + return false; + } + // Parse actions + for (const auto &actionJson : nodeJson["actions"]) + { + vda_5050::Action action; + if (!this->checkObjectStructure(actionJson, "actionType")) + return false; + action.actionType = actionJson["actionType"]; + + if (!this->checkObjectStructure(actionJson, "actionId")) + return false; + action.actionId = actionJson["actionId"]; + + if (!this->checkObjectStructure(actionJson, "actionDescription")) + return false; + action.actionDescription = actionJson["actionDescription"]; + + if (!this->checkObjectStructure(actionJson, "blockingType")) + return false; + action.blockingType = actionJson["blockingType"]; + + if (!actionJson["actionParameters"].is_array()) + { + ROS_WARN("'actionParameters' is not array type"); + return false; + } + + // Parse action parameters + for (const auto ¶mJson : actionJson["actionParameters"]) + { + vda_5050::ActionParameter param; + if (!this->checkObjectStructure(paramJson, "key")) + return false; + param.key = paramJson["key"]; + + if (!this->checkObjectStructure(paramJson, "value")) + return false; + param.value = paramJson["value"]; + + action.actionParameters.push_back(param); + } + node.actions.push_back(std::make_shared(action)); + } + order.nodes.push_back(node); + } + + if (!j["edges"].is_array()) + { + ROS_WARN("'edges' is not array type"); + return false; + } + // Parse edges + for (const auto &edgeJson : j["edges"]) + { + vda_5050::Edge edge; + if (!this->checkObjectStructure(edgeJson, "edgeId")) + return false; + edge.edgeId = edgeJson["edgeId"]; + + if (!this->checkObjectStructure(edgeJson, "sequenceId")) + return false; + edge.sequenceId = edgeJson["sequenceId"]; + + if (!this->checkObjectStructure(edgeJson, "edgeDescription")) + return false; + edge.edgeDescription = edgeJson["edgeDescription"]; + + if (!this->checkObjectStructure(edgeJson, "released")) + return false; + edge.released = edgeJson["released"]; + + if (!this->checkObjectStructure(edgeJson, "startNodeId")) + return false; + edge.startNodeId = edgeJson["startNodeId"]; + + if (!this->checkObjectStructure(edgeJson, "endNodeId")) + return false; + edge.endNodeId = edgeJson["endNodeId"]; + + if (!this->checkObjectStructure(edgeJson, "maxSpeed")) + return false; + edge.maxSpeed = edgeJson["maxSpeed"]; + + if (!this->checkObjectStructure(edgeJson, "maxHeight")) + return false; + edge.maxHeight = edgeJson["maxHeight"]; + + if (!this->checkObjectStructure(edgeJson, "minHeight")) + return false; + edge.minHeight = edgeJson["minHeight"]; + + if (!this->checkObjectStructure(edgeJson, "orientation")) + return false; + edge.orientation = edgeJson["orientation"]; + + if (!this->checkObjectStructure(edgeJson, "orientationType")) + return false; + edge.orientationType = edgeJson["orientationType"]; + + if (!this->checkObjectStructure(edgeJson, "direction")) + return false; + edge.direction = edgeJson["direction"]; + + if (!this->checkObjectStructure(edgeJson, "rotationAllowed")) + return false; + edge.rotationAllowed = edgeJson["rotationAllowed"]; + + if (!this->checkObjectStructure(edgeJson, "maxRotationSpeed")) + return false; + edge.maxRotationSpeed = edgeJson["maxRotationSpeed"]; + + if (!this->checkObjectStructure(edgeJson, "length")) + return false; + edge.length = edgeJson["length"]; + + // Parse trajectory + if (!this->checkObjectStructure(edgeJson, "trajectory")) + return false; + auto trajectoryJson = edgeJson["trajectory"]; + + if (!this->checkObjectStructure(trajectoryJson, "degree")) + return false; + edge.trajectory.degree = trajectoryJson["degree"]; + + if (!trajectoryJson["knotVector"].is_array()) + { + ROS_WARN("'knotVector' is not array type"); + return false; + } + for (const auto &knot : trajectoryJson["knotVector"]) + { + if (!knot.is_number_float() && !knot.is_number_integer()) + { + ROS_WARN("'knot' is not double type"); + return false; + } + edge.trajectory.knotVector.push_back(knot); + } + + // Parse control points + if (!trajectoryJson["controlPoints"].is_array()) + { + ROS_WARN("'controlPoints' is not double type"); + return false; + } + for (const auto &pointJson : trajectoryJson["controlPoints"]) + { + vda_5050::ControlPoint point; + if (!this->checkObjectStructure(pointJson, "x")) + return false; + point.x = pointJson["x"]; + + if (!this->checkObjectStructure(pointJson, "y")) + return false; + point.y = pointJson["y"]; + + if (!this->checkObjectStructure(pointJson, "weight")) + return false; + point.weight = pointJson["weight"]; + + edge.trajectory.controlPoints.push_back(point); + } + + order.edges.push_back(edge); + } + return true; +} + +bool amr_comunication::VDA5050Connector::ParseInstantActionMsg(const std::string &payload, vda_5050::InstantAction &instantAction) +{ + nlohmann::json j; + try + { + j = nlohmann::json::parse(payload); + } + catch (const nlohmann::json::parse_error &e) + { + ROS_ERROR_STREAM("JSON parse error: " << e.what() << std::endl); + ROS_INFO("ParseInstantActionMsg : \n%s", payload.c_str()); + return false; + } + catch (const nlohmann::json::invalid_iterator &e) + { + ROS_ERROR_STREAM("JSON invalid iterator: " << e.what() << std::endl); + ROS_INFO("ParseInstantActionMsg : \n%s", payload.c_str()); + return false; + } + catch (const nlohmann::json::type_error &e) + { + ROS_ERROR_STREAM("JSON type error: " << e.what() << std::endl); + ROS_INFO("ParseInstantActionMsg : \n%s", payload.c_str()); + return false; + } + catch (const nlohmann::json::out_of_range &e) + { + ROS_ERROR_STREAM("JSON out of range: " << e.what() << std::endl); + ROS_INFO("ParseInstantActionMsg : \n%s", payload.c_str()); + return false; + } + catch (const nlohmann::json::other_error &e) + { + ROS_ERROR_STREAM("JSON other error: " << e.what() << std::endl); + ROS_INFO("ParseInstantActionMsg : \n%s", payload.c_str()); + return false; + } + + // Parse header info + if (!this->checkObjectStructure(j, "headerId")) + return false; + instantAction.headerId = j["headerId"]; + + if (!this->checkObjectStructure(j, "timestamp")) + return false; + instantAction.timestamp = j["timestamp"]; + + if (!this->checkObjectStructure(j, "version")) + return false; + instantAction.version = j["version"]; + + if (!this->checkObjectStructure(j, "manufacturer")) + return false; + instantAction.manufacturer = j["manufacturer"]; + + if (!this->checkObjectStructure(j, "serialNumber")) + return false; + instantAction.serialNumber = j["serialNumber"]; + + // Parse actions + if (!j["actions"].is_array()) + { + ROS_WARN("'actions' is not array type"); + return false; + } + for (const auto &actionJson : j["actions"]) + { + vda_5050::Action action; + if (!this->checkObjectStructure(actionJson, "actionType")) + return false; + action.actionType = actionJson["actionType"]; + + if (!this->checkObjectStructure(actionJson, "actionId")) + return false; + action.actionId = actionJson["actionId"]; + + if (!this->checkObjectStructure(actionJson, "actionDescription")) + return false; + action.actionDescription = actionJson["actionDescription"]; + + if (!this->checkObjectStructure(actionJson, "blockingType")) + return false; + action.blockingType = actionJson["blockingType"]; + + if (!actionJson["actionParameters"].is_array()) + { + ROS_WARN("'actionParameters' is not array type"); + return false; + } + // Parse action parameters + for (const auto ¶mJson : actionJson["actionParameters"]) + { + vda_5050::ActionParameter param; + if (!this->checkObjectStructure(paramJson, "key")) + return false; + param.key = paramJson["key"]; + + if (!this->checkObjectStructure(paramJson, "value")) + return false; + param.value = paramJson["value"]; + + action.actionParameters.push_back(param); + } + instantAction.actions.push_back(std::make_shared(action)); + } + + return true; +} + +nlohmann::json amr_comunication::VDA5050Connector::vda5050StateToJson(const vda_5050::State &state) +{ + nlohmann::json j; + + j["headerId"] = state.headerId; + j["timestamp"] = state.timestamp; + j["version"] = state.version; + j["manufacturer"] = state.manufacturer; + j["serialNumber"] = state.serialNumber; + j["orderId"] = state.orderId; + j["orderUpdateId"] = state.orderUpdateId; + j["zoneSetId"] = state.zoneSetId; + j["lastNodeId"] = state.lastNodeId; + j["lastNodeSequenceId"] = state.lastNodeSequenceId; + + // Convert nodeStates + for (const auto &nodeState : state.nodeStates) + { + j["nodeStates"].push_back({ + {"nodeId", nodeState.nodeId}, + {"sequenceId", nodeState.sequenceId}, + {"nodeDescription", nodeState.nodeDescription}, + {"released", nodeState.released}, + {"nodePosition", {{"x", nodeState.nodePosition.x}, {"y", nodeState.nodePosition.y}, {"theta", nodeState.nodePosition.theta}, {"allowedDeviationXY", nodeState.nodePosition.allowedDeviationXY}, {"allowedDeviationTheta", nodeState.nodePosition.allowedDeviationTheta}, {"mapId", nodeState.nodePosition.mapId}, {"mapDescription", nodeState.nodePosition.mapDescription}}}, + }); + } + + // Convert edgeStates + for (const auto &edgeState : state.edgeStates) + { + j["edgeStates"].push_back({{"edgeId", edgeState.edgeId}, + {"sequenceId", edgeState.sequenceId}, + {"edgeDescription", edgeState.edgeDescription}, + {"released", edgeState.released}, + {"trajectory", {{"degree", edgeState.trajectory.degree}, {"knotVector", edgeState.trajectory.knotVector}, {"controlPoints", nlohmann::json::array()}}}}); + + for (const auto &controlPoint : edgeState.trajectory.controlPoints) + { + j["edgeStates"].back()["trajectory"]["controlPoints"].push_back({{"x", controlPoint.x}, + {"y", controlPoint.y}, + {"weight", controlPoint.weight}}); + } + } + + j["agvPosition"] = { + {"x", state.agvPosition.x}, + {"y", state.agvPosition.y}, + {"theta", state.agvPosition.theta}, + {"mapId", state.agvPosition.mapId}, + {"positionInitialized", state.agvPosition.positionInitialized}, + {"localizationScore", state.agvPosition.localizationScore}, + {"deviationRange", state.agvPosition.deviationRange}}; + + j["velocity"] = { + {"vx", state.velocity.vx}, + {"vy", state.velocity.vy}, + {"omega", state.velocity.omega}}; + + // Convert loads + for (const auto &load : state.loads) + { + j["loads"].push_back({{"loadId", load.loadId}, + {"loadType", load.loadType}, + {"loadPosition", load.loadPosition}, + {"boundingBoxReference", {{"x", load.boundingBoxReference.x}, {"y", load.boundingBoxReference.y}, {"z", load.boundingBoxReference.z}, {"theta", load.boundingBoxReference.theta}}}, + {"loadDimensions", {{"length", load.loadDimensions.length}, {"width", load.loadDimensions.width}, {"height", load.loadDimensions.height}}}, + {"weight", load.weight}}); + } + + j["driving"] = state.driving; + j["paused"] = state.paused; + j["newBaseRequest"] = state.newBaseRequest; + j["distanceSinceLastNode"] = state.distanceSinceLastNode; + j["operatingMode"] = state.operatingMode._toString(); + + j["batteryState"] = { + {"batteryCharge", state.batteryState.batteryCharge}, + {"batteryVoltage", state.batteryState.batteryVoltage}, + {"batteryHealth", state.batteryState.batteryHealth}, + {"charging", state.batteryState.charging}, + {"reach", state.batteryState.reach}}; + + j["safetyState"] = { + {"eStop", state.safetyState.eStop}, + {"fieldViolation", state.safetyState.fieldViolation}}; + + // Convert actionStates + for (const auto &actionState : state.actionStates) + { + j["actionStates"].push_back({{"actionID", actionState->actionId}, + {"actionType", actionState->actionType}, + {"actionDescription", actionState->actionDescription}, + {"actionStatus", actionState->actionStatus._toString()}, + {"resultDescription", actionState->resultDescription}}); + } + + // Convert errors + for (const auto &error : state.errors) + { + nlohmann::json errorJson = { + {"errorType", error.errorType}, + {"errorDescription", error.errorDescription}, + {"errorLevel", error.errorLevel._toString()}}; + + // Convert errorReferences + for (const auto &ref : error.errorReferences) + { + errorJson["errorReferences"].push_back({{"referenceKey", ref.referenceKey}, + {"referenceValue", ref.referenceValue}}); + } + + j["errors"].push_back(errorJson); + } + + // Convert information + for (const auto &info : state.information) + { + nlohmann::json infoJson = { + {"infoType", info.infoType}, + {"infoDescription", info.infoDescription}, + {"infoLevel", info.infoLevel}}; + + // Convert infoReferences + for (const auto &ref : info.infoReferences) + { + infoJson["infoReferences"].push_back({{"referenceKey", ref.referenceKey}, + {"referenceValue", ref.referenceValue}}); + } + + j["information"].push_back(infoJson); + } + + return j; +} + +nlohmann::json amr_comunication::VDA5050Connector::vda5050VisualizationToJson(const vda_5050::Visualization &visualization) +{ + nlohmann::json j; + + j["headerId"] = visualization.headerId; + j["timestamp"] = visualization.timestamp; + j["version"] = visualization.version; + j["manufacturer"] = visualization.manufacturer; + j["serialNumber"] = visualization.serialNumber; + + j["agvPosition"] = { + {"x", visualization.agvPosition.x}, + {"y", visualization.agvPosition.y}, + {"theta", visualization.agvPosition.theta}, + {"mapId", visualization.agvPosition.mapId}, + {"positionInitialized", visualization.agvPosition.positionInitialized}, + {"localizationScore", visualization.agvPosition.localizationScore}, + {"deviationRange", visualization.agvPosition.deviationRange}}; + + j["velocity"] = { + {"vx", visualization.velocity.vx}, + {"vy", visualization.velocity.vy}, + {"omega", visualization.velocity.omega}}; + + return j; +} + +template +bool amr_comunication::VDA5050Connector::checkObjectStructure(nlohmann::json json, std::string obj) +{ + if (!json.contains(obj)) + { + ROS_WARN("Object structure '%s' is not existed", obj.c_str()); + return false; + } + + if constexpr (std::is_same::value) + { + if (!json[obj].is_number_integer()) + { + ROS_WARN("Object '%s' is not an integer", obj.c_str()); + return false; + } + } + else if constexpr (std::is_same::value) + { + if (!json[obj].is_number_float() && !json[obj].is_number_integer()) + { + ROS_WARN("Object '%s' is not a float or integer", obj.c_str()); + return false; + } + } + else if constexpr (std::is_same::value) + { + if (!json[obj].is_string()) + { + ROS_WARN("Object '%s' is not a string", obj.c_str()); + return false; + } + } + else if constexpr (std::is_same::value) + { + if (!json[obj].is_boolean()) + { + ROS_WARN("Object '%s' is not a boolean", obj.c_str()); + return false; + } + } + else if constexpr (std::is_class::value) + { + if (!json[obj].is_object()) + { + ROS_WARN("Object '%s' is not a struct", obj.c_str()); + return false; + } + } + else + { + ROS_WARN("Unknown type check for object '%s'", obj.c_str()); + return false; + } + return true; +} + +bool amr_comunication::VDA5050Connector::acceptingOrderOrOrderUpdate(const vda_5050::Order &received_order, vda_5050::Order ¤t_order) +{ + bool is_received_order_new; + // is received order new or an update of the current order? + // Is orderId of the received order different to orderId of order the vehicle currently holds? + if (received_order.orderId == current_order.orderId) + { + } + else + { + /** + * is vehicle still executing an order or waiting for an update? + * Are nodeStates not empty or are actionStates containing states which are neither 'FAILED' nor 'FINISHED'? + * Nodes and edges and the corresponding action states of the order horizon are also included inside the state. + * Vehicle might still have a horizon and therefore waiting for an update and executing an order. + */ + bool is_vehicle_still_executing_an_order = + amr_comunication::VDA5050Connector::isTheVehicleStillExecuting(); + bool waiting_for_an_update = + amr_comunication::VDA5050Connector::isTheVehicleWaitingAnUpdate(); + + auto &errors = amr_comunication::VDA5050Connector::vda5050_state_.errors; + auto error = std::find_if(errors.begin(), errors.end(), + [](const vda_5050::Error e) + { + return e.errorType == std::string("RejectOrder"); + }); + if (error != errors.end()) + { + errors.erase(error); + } + + if (is_vehicle_still_executing_an_order) + { + vda_5050::Error e; + e.errorDescription = "Is AGV still executing an order or waiting for an update?"; + e.errorLevel = vda_5050::Error::ErrorLevel::WARNING; + e.errorType = "RejectOrder"; + errors.push_back(e); + } + + if (is_vehicle_still_executing_an_order || waiting_for_an_update) + { + ROS_INFO("Is vehicle still executing an order or waiting for an update? %x %x ", is_vehicle_still_executing_an_order, waiting_for_an_update); + return false; + } + /** + * Is start of new order close enough to current position? + * Is the vehicle already standing on the node, or is it in the node's deviation range (see Section 6.6.1 Concept and logic)? + */ + bool is_start_of_new_order_close_enough_to_current_position = + amr_comunication::VDA5050Connector::isAGVOnNodeOrInDeviationRangeOfNewOrder(received_order); + + // if (!is_start_of_new_order_close_enough_to_current_position) + // { + // ROS_INFO("Is start of new order close enough to current position?"); + // return false; + // } + + // accept order + current_order = received_order; + // std::lock_guard lock(state_mutex); + auto ¤t_state = amr_comunication::VDA5050Connector::vda5050_state_; + amr_comunication::VDA5050Connector::populateNodeStates(current_order, current_state); + amr_comunication::VDA5050Connector::populateEdgeStates(current_order, current_state); + amr_comunication::VDA5050Connector::populateActionStates(current_order, current_state); + current_state.lastNodeSequenceId = 0; + current_state.lastNodeId = ""; + } + return true; +} + +bool amr_comunication::VDA5050Connector::acceptingInstantActions( + const vda_5050::InstantAction &received_instantAction, vda_5050::InstantAction ¤t_instantAction) +{ + if (received_instantAction.actions.empty()) + return false; + current_instantAction = received_instantAction; + return true; +} + +bool amr_comunication::VDA5050Connector::isTheVehicleStillExecuting() +{ + auto ¤t_state = amr_comunication::VDA5050Connector::vda5050_state_; + + // Are nodeStates not empty ? + bool is_not_node_state_empty = !current_state.nodeStates.empty(); + + // Are actionStates containing states which are neither 'FAILED' nor 'FINISHED'? + bool is_action_running = false; + for (auto &action_state : current_state.actionStates) + { + if (action_state->actionStatus != vda_5050::ActionState::ActionStatus::FINISHED && + action_state->actionStatus != vda_5050::ActionState::ActionStatus::FAILED) + { + is_action_running = true; + break; + } + } + return is_not_node_state_empty || is_action_running; +} + +bool amr_comunication::VDA5050Connector::isTheVehicleWaitingAnUpdate() +{ + auto ¤t_state = amr_comunication::VDA5050Connector::vda5050_state_; + return current_state.getTypeOrder() == vda_5050::OrderType::Horizon; +} + +bool amr_comunication::VDA5050Connector::isAGVOnNodeOrInDeviationRange() +{ + auto ¤t_state = amr_comunication::VDA5050Connector::vda5050_state_; + if (current_state.nodeStates.empty()) + return false; + + for (const auto &node : current_state.nodeStates) + { + double dx = current_state.agvPosition.x - node.nodePosition.x; + double dy = current_state.agvPosition.y - node.nodePosition.y; + double distance = std::sqrt(dx * dx + dy * dy); + + // Check if AGV is exactly on the node OR within deviation range + if (distance <= node.nodePosition.allowedDeviationXY) + { + return true; + } + } + return false; +} + +bool amr_comunication::VDA5050Connector::isAGVOnNodeOrInDeviationRangeOfNewOrder(vda_5050::Order new_order) +{ + // std::lock_guard lock(state_mutex); + auto ¤t_state = amr_comunication::VDA5050Connector::vda5050_state_; + if (new_order.nodes.empty()) + return false; + + double distance_min = std::numeric_limits::infinity(); + double allowed_xy; + std::string node_str; + for (const auto &node : new_order.nodes) + { + double dx = current_state.agvPosition.x - node.nodePosition.x; + double dy = current_state.agvPosition.y - node.nodePosition.y; + double distance = std::sqrt(dx * dx + dy * dy); + + // Check if AGV is exactly on the node OR within deviation range + if (distance <= fabs(node.nodePosition.allowedDeviationXY)) + { + return true; + } + + if (distance <= distance_min) + { + distance_min = distance; + allowed_xy = node.nodePosition.allowedDeviationXY; + node_str = std::string(node.nodeId.c_str()); + } + } + ROS_INFO("AGV distance %f to %s with allowedxy %f", distance_min, node_str.c_str(), allowed_xy); + return false; +} + +void amr_comunication::VDA5050Connector::populateNodeStates(const vda_5050::Order &receiverd_order, vda_5050::State &curent_state) +{ + curent_state.nodeStates.clear(); + for (auto &node : receiverd_order.nodes) + { + vda_5050::NodeState node_state; + node_state.nodeId = node.nodeId; + node_state.sequenceId = node.sequenceId; + node_state.nodeDescription = node.nodeDescription; + node_state.released = node_state.released; + node_state.nodePosition = node.nodePosition; + curent_state.nodeStates.push_back(node_state); + } +} + +void amr_comunication::VDA5050Connector::appendNodeStates(const vda_5050::Order &receiverd_order, vda_5050::State &curent_state) +{ +} + +void amr_comunication::VDA5050Connector::populateEdgeStates(const vda_5050::Order &receiverd_order, vda_5050::State &curent_state) +{ + curent_state.edgeStates.clear(); + int count = 0; + for (auto &edge : receiverd_order.edges) + { + vda_5050::EdgeState edge_state; + edge_state.edgeId = edge.edgeId; + edge_state.sequenceId = edge.sequenceId; + edge_state.edgeDescription = edge.edgeDescription; + edge_state.released = edge.released; + edge_state.trajectory = edge.trajectory; + curent_state.edgeStates.push_back(edge_state); + ROS_INFO("State->EdgeState[%d]: %s", count, edge_state.edgeDescription.c_str()); + count++; + } + ROS_INFO(" "); +} + +void amr_comunication::VDA5050Connector::appendEdgeStates(const vda_5050::Order &receiverd_order, vda_5050::State &curent_state) +{ +} + +void amr_comunication::VDA5050Connector::populateActionStates(const vda_5050::Order &receiverd_order, vda_5050::State &curent_state) +{ + // std::lock_guard lock(state_mutex); + curent_state.actionStates.clear(); + // for (auto &edge : receiverd_order.edges) + // { + // auto start_node = std::find_if(receiverd_order.nodes.begin(), receiverd_order.nodes.end(), + // [&edge](const vda_5050::Node &n) + // { + // return edge.startNodeId == n.nodeId; + // }); + + // auto end_node = std::find_if(receiverd_order.nodes.begin(), receiverd_order.nodes.end(), + // [&edge](const vda_5050::Node &n) + // { + // return edge.endNodeId == n.nodeId; + // }); + + // if (start_node != receiverd_order.nodes.end()) + // { + // if (!start_node->actions.empty()) + // { + // for (auto action : start_node->actions) + // { + // std::shared_ptr action_state = std::make_shared(); + // action_state->actionDescription = action->actionDescription; + // action_state->actionId = action->actionId; + // action_state->actionType = action->actionType; + // action_state->actionStatus = vda_5050::ActionState::ActionStatus::WAITING; + // curent_state.actionStates.push_back(action_state); + // } + // } + // } + + // for (auto action : edge.actions) + // { + // std::shared_ptr action_state = std::make_shared(); + // action_state->actionDescription = action.actionDescription; + // action_state->actionId = action.actionId; + // action_state->actionType = action.actionType; + // action_state->actionStatus = vda_5050::ActionState::ActionStatus::WAITING; + // curent_state.actionStates.push_back(action_state); + // } + + // if (end_node != receiverd_order.nodes.end()) + // { + // if (!end_node->actions.empty()) + // { + // for (auto action : end_node->actions) + // { + // std::shared_ptr action_state = std::make_shared(); + // action_state->actionDescription = action->actionDescription; + // action_state->actionId = action->actionId; + // action_state->actionType = action->actionType; + // action_state->actionStatus = vda_5050::ActionState::ActionStatus::WAITING; + // curent_state.actionStates.push_back(action_state); + // } + // } + // } + // } + // int count = 0; + // for (auto action : curent_state.actionStates) + // { + // ROS_INFO("State->ActionState[%d]: %s", count, action->actionType.c_str()); + // count++; + // } + // ROS_INFO(" "); +} + +void amr_comunication::VDA5050Connector::appendActionStates(const vda_5050::Order &receiverd_order, vda_5050::State &curent_state) +{ +} + +std::unique_ptr amr_comunication::VDA5050Connector::userParams_; +vda_5050::Order amr_comunication::VDA5050Connector::vda5050_order_; +vda_5050::InstantAction amr_comunication::VDA5050Connector::vda5050_instant_action_; +vda_5050::State amr_comunication::VDA5050Connector::vda5050_state_; +vda_5050::Visualization amr_comunication::VDA5050Connector::vda5050_visualization_; +std::mutex amr_comunication::VDA5050Connector::state_mutex; +std::mutex amr_comunication::VDA5050Connector::visualization_mutex; + +std::function amr_comunication::VDA5050Connector::execute_order_; +std::function amr_comunication::VDA5050Connector::execute_instant_action_; +std::function amr_comunication::VDA5050Connector::execute_parallel_action_; +std::function amr_comunication::VDA5050Connector::velocity_max_action_; +std::function amr_comunication::VDA5050Connector::angular_max_action_; \ No newline at end of file diff --git a/Controllers/Packages/amr_comunication/test/vda_5050_connector_node.cpp b/Controllers/Packages/amr_comunication/test/vda_5050_connector_node.cpp new file mode 100644 index 0000000..613a33f --- /dev/null +++ b/Controllers/Packages/amr_comunication/test/vda_5050_connector_node.cpp @@ -0,0 +1,21 @@ +#include +#include "amr_comunication/vda_5050/vda_5050_connector.h" + +std::shared_ptr client; + +void TestOrderWorker() +{ + +} + +int main(int argc, char** argv) { + // Initialize the ROS node + ros::init(argc, argv, "test_imr_nova_control"); + ros::start(); + + client = std::make_shared(); + client->execute_order_ = TestOrderWorker; + ros::spin(); + client.reset(); + return 0; +} \ No newline at end of file diff --git a/Controllers/Packages/amr_control/CMakeLists.txt b/Controllers/Packages/amr_control/CMakeLists.txt new file mode 100644 index 0000000..16095a1 --- /dev/null +++ b/Controllers/Packages/amr_control/CMakeLists.txt @@ -0,0 +1,243 @@ +cmake_minimum_required(VERSION 3.0.2) +project(amr_control) + +## Compile as C++11, supported in ROS Kinetic and newer +add_compile_options(-std=c++17) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + geometry_msgs + nav_2d_utils + roscpp + rospy + std_msgs + angles + delta_modbus + nova5_control + loc_core + move_base_core + amr_comunication + vda5050_msgs +) + + +## System dependencies are found with CMake's conventions +find_package(Boost REQUIRED COMPONENTS system thread) +find_package(PkgConfig) + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a exec_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# geometry_msgs# std_msgs +# ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( + INCLUDE_DIRS include + LIBRARIES amr_control + CATKIN_DEPENDS geometry_msgs move_base_core loc_core nav_2d_utils roscpp rospy std_msgs + DEPENDS Boost +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} +) + +## Declare a C++ library +add_library(${PROJECT_NAME} + src/amr_control.cpp + src/amr_monitor.cpp + src/amr_safety.cpp + src/amr_opc_ua_server_api.cpp + src/amr_vda_5050_client_api.cpp + src/amr_make_plan_with_order.cpp +) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +## With catkin_make all packages are built within a single CMake context +## The recommended prefix ensures that target names across packages don't collide +add_executable(${PROJECT_NAME}_node src/amr_control_node.cpp) +add_executable(vda_5050_api_test test/vda_5050_api.cpp) + +## Rename C++ executable without prefix +## The above recommended prefix causes long target names, the following renames the +## target back to the shorter version for ease of user use +## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" +#set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME ${PROJECT_NAME}) + +## Add cmake target dependencies of the executable +## same as for the library above +add_dependencies(${PROJECT_NAME}_node + ${PROJECT_NAME} + ${${PROJECT_NAME}_EXPORTED_TARGETS} + ${catkin_EXPORTED_TARGETS} +) + +add_dependencies(vda_5050_api_test + ${PROJECT_NAME} + ${${PROJECT_NAME}_EXPORTED_TARGETS} + ${catkin_EXPORTED_TARGETS} +) + +## Specify libraries to link a library or executable target against +target_link_libraries(${PROJECT_NAME} + ${FreeOpcUa_LIBRARIES} + ${catkin_LIBRARIES} + ${Boost_LIBRARIES} +) + +target_link_libraries(${PROJECT_NAME}_node + ${PROJECT_NAME} + ${catkin_LIBRARIES} + ${Boost_LIBRARIES} +) + +target_link_libraries(vda_5050_api_test + ${PROJECT_NAME} + ${catkin_LIBRARIES} + ${Boost_LIBRARIES} +) + + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# catkin_install_python(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html +install(TARGETS ${PROJECT_NAME}_node vda_5050_api_test + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +## Mark libraries for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) + +## Mark cpp header files for installation +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE +) + +## Mark other files for installation (e.g. launch and bag files, etc.) +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_amr_control.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/Controllers/Packages/amr_control/include/amr_control/amr_control.h b/Controllers/Packages/amr_control/include/amr_control/amr_control.h new file mode 100644 index 0000000..ecdc99d --- /dev/null +++ b/Controllers/Packages/amr_control/include/amr_control/amr_control.h @@ -0,0 +1,122 @@ +#ifndef __AMR_COTROLLER_H_INCLUDED_ +#define __AMR_COTROLLER_H_INCLUDED_ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "delta_modbus/delta_modbus_tcp.h" + +// API +#include "amr_control/amr_opc_ua_server_api.h" +#include "amr_control/amr_vda_5050_client_api.h" + +// Objects +#include "move_base_core/navigation.h" +#include "loc_core/localization.h" +#include "amr_control/amr_monitor.h" +#include "nova5_control/imr_nova_control.h" +#include "amr_control/amr_safety.h" + +namespace amr_control +{ + class AmrController + { + public: + AmrController(ros::NodeHandle &nh, std::shared_ptr buffer); + AmrController(); + virtual ~AmrController(); + + void init(ros::NodeHandle &nh, std::shared_ptr buffer); + + /** + * @brief Stop the ServerHandle + */ + virtual void stop(void); + + protected: + virtual void initalizingComunicationHandle(ros::NodeHandle &nh); + virtual void initalizingMoveBaseHandle(ros::NodeHandle &nh); + virtual void initalizingLocalizationHandle(ros::NodeHandle &nh); + virtual void initalizingMonitorHandle(ros::NodeHandle &nh); + + private: + void ArmCallBack(); + + void ArmDotuff(); + + void unLoadCallBack(); + + void conveyorBeltsShipping(amr_control::State &state); + + void loadCallBack(); + + void conveyorBeltsReceiving(amr_control::State &state); + + void controllerDotuff(); + + void isDetectedMakerCallback(const std_msgs::Bool::ConstPtr &msg); + + void threadHandle(); + + bool initalized_; + ros::NodeHandle nh_; + std::shared_ptr tf_; + ros::Subscriber is_detected_maker_sub_; + + std::shared_ptr opc_ua_server_api_ptr_; + std::shared_ptr vda_5050_client_api_ptr_; + std::shared_ptr server_thr_; + + std::shared_ptr monitor_ptr_; + std::shared_ptr monitor_thr_; + + std::shared_ptr move_base_ptr_; + pluginlib::ClassLoader move_base_loader_; + // Synchronous processing thread + std::shared_ptr move_base_thr_; + std::mutex init_move_base_mutex_; + std::condition_variable init_move_base_cv_; + bool move_base_initialized_; + + // Safety Speed + std::shared_ptr amr_safety_ptr_; + bool muted_; + std::mutex cmd_vel_mtx; + nav_2d_msgs::Twist2D cmd_vel_max_, cmd_vel_recommended_; + + std::shared_ptr loc_base_ptr_; + pluginlib::ClassLoader loc_base_loader_; + // Synchronous processing thread + std::shared_ptr loc_base_thr_; + std::mutex init_loc_base_mutex_; + std::condition_variable init_loc_base_cv_; + bool loc_base_initialized_; + + bool cancel_, enable_ ,pause_; + // Belt properties + std::thread belt_thread_; + amr_control::State cur_belt_state_en_; + bool belt_joined_; + std::mutex belt_mutex_; + + // amr propertise + std::thread arm_thread_; + bool arm_joined_; + std::mutex arm_mutex_; + bool arm_continue_; + bool arm_go_home_; + bool arm_power_on_; + unsigned int count_ng_max_, count_ok_max_; + unsigned int *status_code_ptr_ = new unsigned int; + + }; // class AMRController +} // namespace amr_control + +#endif // __AMR_COTROLLER_H_INCLUDED_ \ No newline at end of file diff --git a/Controllers/Packages/amr_control/include/amr_control/amr_make_plan_with_order.h b/Controllers/Packages/amr_control/include/amr_control/amr_make_plan_with_order.h new file mode 100644 index 0000000..11d942d --- /dev/null +++ b/Controllers/Packages/amr_control/include/amr_control/amr_make_plan_with_order.h @@ -0,0 +1,67 @@ +#ifndef _VDA_5050_MAKE_PLAN_WITH_ORDER_H_INCLUDE_ +#define _VDA_5050_MAKE_PLAN_WITH_ORDER_H_INCLUDE_ +#include +#include "amr_comunication/vda_5050/utils/curve_common.h" +#include "amr_comunication/vda_5050/utils/pose.h" +#include + + +namespace amr_control +{ + inline double getYawDegree(double x, double y, double z, double w) { + // yaw (z-axis rotation) + double siny_cosp = 2 * (w * z + x * y); + double cosy_cosp = 1 - 2 * (y * y + z * z); + double yaw = std::atan2(siny_cosp, cosy_cosp); + yaw = (180 * yaw) / M_PI;return yaw; + } + + inline double getYawRad(double x, double y, double z, double w) { + // yaw (z-axis rotation) + double siny_cosp = 2 * (w * z + x * y); + double cosy_cosp = 1 - 2 * (y * y + z * z); + double yaw = std::atan2(siny_cosp, cosy_cosp); + return yaw; + } + + inline double calculateAngle(double xA, double yA, double xB, double yB) { + double deltaX = xB - xA; + double deltaY = yB - yA; + double angleRad = atan2(deltaY, deltaX); + // double angleDeg = angleRad * 180.0 / M_PI; + return angleRad; + } + + inline void modifyYaw(double& yaw) { + while (yaw < -M_PI) + yaw += 2.0 * M_PI; + while (yaw > M_PI) + yaw -= 2.0 * M_PI; + }; + + double calculateDistance(vda_5050::Pose& pose1, vda_5050::Pose& pose2); + + double computeDeltaAngle(vda_5050::Pose& Pose1, vda_5050::Pose& Pose2); + + bool isThetaValid(double theta); + + bool curveIsValid(int degree, const std::vector& knot_vector, std::vector>& control_points); + + double computeDeltaAngleStartNode(double theta, vda_5050::Pose& startPose, vda_5050::Pose& next_Pose); + + double computeDeltaAngleStartNode(double thetaEnd, double thetaStart, vda_5050::Pose& Pose); + + double computeDeltaAngleStartOfPlan(double theta, geometry_msgs::Pose& startPose, geometry_msgs::Pose& next_Pose); + + double computeDeltaAngleStartOfPlan(double thetaEnd, double thetaStart, geometry_msgs::Pose& Pose); + + double computeDeltaAngleEndNode(double theta, vda_5050::Pose& endPose, vda_5050::Pose& prev_Pose); + + double computeDeltaAngleEndOfPlan(double theta, geometry_msgs::Pose& endPose, geometry_msgs::Pose& prev_Pose); + + void setYawAllPosesOnEdge(std::vector& posesOnEdge, bool reverse); + + bool MakePlanWithOrder(const vda_5050::Order order, bool is_move_backward, uint8_t& status, std::string& message, std::vector& posesOnPathWay); +} + +#endif // _VDA_5050_MAKE_PLAN_WITH_ORDER_H_INCLUDE_ \ No newline at end of file diff --git a/Controllers/Packages/amr_control/include/amr_control/amr_monitor.h b/Controllers/Packages/amr_control/include/amr_control/amr_monitor.h new file mode 100644 index 0000000..17d68e8 --- /dev/null +++ b/Controllers/Packages/amr_control/include/amr_control/amr_monitor.h @@ -0,0 +1,49 @@ +#ifndef __AMR_MONITOR_H_INCLUDED_ +#define __AMR_MONITOR_H_INCLUDED_ +#include +#include +#include +#include +#include + +namespace amr_control +{ + class AmrMonitor + { + public: + /** + * @brief Default constructor for AmrMonitor + */ + AmrMonitor(); + + /** + * @brief Constructor for AmrMonitor with ROS node handle + * @param nh ROS NodeHandle for initialization + */ + AmrMonitor(const ros::NodeHandle& nh); + + /** + * @brief Virtual destructor for AmrMonitor + */ + virtual ~AmrMonitor(); + + /** + * @brief Initialize the ServerHandle with a ROS node handle + * @param nh ROS NodeHandle for initialization + */ + virtual void init(const ros::NodeHandle& nh); + + /** + * @brief Get velocity + * @return True if get done + */ + bool getVelocity(nav_2d_msgs::Twist2D &velocity); + + private: + + std::shared_ptr odom_sub_; + bool initalized_; + }; +} // namespace amr_control + +#endif // __AMR_MONITOR_H_INCLUDED_ \ No newline at end of file diff --git a/Controllers/Packages/amr_control/include/amr_control/amr_opc_ua_server_api.h b/Controllers/Packages/amr_control/include/amr_control/amr_opc_ua_server_api.h new file mode 100644 index 0000000..fef14ff --- /dev/null +++ b/Controllers/Packages/amr_control/include/amr_control/amr_opc_ua_server_api.h @@ -0,0 +1,912 @@ +#ifndef __AMR_COTROLLER_OPC_UA_SERVER_DEFINE_API_H_INCLUDED_ +#define __AMR_COTROLLER_OPC_UA_SERVER_DEFINE_API_H_INCLUDED_ + +#include +#include "move_base_core/navigation.h" +#include "loc_core/localization.h" +#include "amr_comunication/opc_ua/ua_server.h" +#include "amr_control/amr_monitor.h" +#include "amr_control/common.h" +#include + +namespace amr_control +{ + class OpcUAServerAPI + { + public: + /** + * @brief Contructor + * @param monitor Con trỏ đến đối tượng AmrMonitor, dùng để theo dõi trạng thái và sự kiện. + * @param move_base Con trỏ đến đối tượng BaseNavigation, cho phép điều hướng robot. + * @param loc_base Con trỏ đến đối tượng LocBase. + */ + OpcUAServerAPI(const ros::NodeHandle &nh, std::shared_ptr move_base, + std::shared_ptr loc_base, std::shared_ptr monitor); + + /** + * @brief Detructor + */ + virtual ~OpcUAServerAPI(); + + /** + * @brief Start + */ + void start(); + + private: + /** + * @brief Định nghĩa các đối tượng cần thiết cho API người dùng. + * + * Hàm này cho phép người dùng định nghĩa và cấu hình các đối tượng cần thiết + * cho việc tương tác với server, monitor và hệ thống điều hướng. + * Người dùng có thể sử dụng các đối tượng này để thực hiện các hành động + * như di chuyển, nhặt đồ, thả đồ, và các hành động khác liên quan đến robot. + */ + virtual void defineObjects(); + + /** SLAM**/ + /** + * @brief Thêm phương thức bắt đầu quét map. + * Bắt đầu quá trình tạo bản đồ. Khi biến trạng thái "SlamState" đang là "Ready" thì có thể gọi được hàm này. + * Khi gọi thành công thì trạng thái biến "SlamState" sẽ chuyển thành "Mapping". + * + * @param server Con trỏ đến server mà phương thức sẽ được thêm vào. + * @param parentID ID của nút cha mà phương thức sẽ được thêm vào. + */ + static void addStartMappingMethod(UA_Server *server, UA_NodeId parentID); + + /** + * @brief Callback cho phương thức bắt đầu quét map. + * @param server Con trỏ đến server. + * @param sessionId ID của phiên làm việc. + * @param sessionContext Ngữ cảnh của phiên làm việc. + * @param methodId ID của phương thức. + * @param methodContext Ngữ cảnh của phương thức. + * @param objectId ID của đối tượng. + * @param objectContext Ngữ cảnh của đối tượng. + * @param inputSize Kích thước của dữ liệu đầu vào. + * @param input Dữ liệu đầu vào. + * @param outputSize Kích thước của dữ liệu đầu ra. + * @param output Dữ liệu đầu ra. + * @return UA_StatusCode Trạng thái của callback. + */ + static UA_StatusCode startMappingCallBack(UA_Server *server, + const UA_NodeId *sessionId, void *sessionContext, + const UA_NodeId *methodId, void *methodContext, + const UA_NodeId *objectId, void *objectContext, + size_t inputSize, const UA_Variant *input, + size_t outputSize, UA_Variant *output); + + /** + * @brief Thêm phương thức dừng quét map. + * Kết thúc quá trình tạo bản đồ, truyền vào tên map để lưu trữ. + * Nếu gọi thành công thì biến "SlamState" sẽ chuyển thành "Ready". + * + * @param server Con trỏ đến server mà phương thức sẽ được thêm vào. + * @param parentID ID của nút cha mà phương thức sẽ được thêm vào. + */ + static void addStopMappingMethod(UA_Server *server, UA_NodeId parentID); + + /** + * @brief Callback cho phương thức dừng quét map. + * @param server Con trỏ đến server. + * @param sessionId ID của phiên làm việc. + * @param sessionContext Ngữ cảnh của phiên làm việc. + * @param methodId ID của phương thức. + * @param methodContext Ngữ cảnh của phương thức. + * @param objectId ID của đối tượng. + * @param objectContext Ngữ cảnh của đối tượng. + * @param inputSize Kích thước của dữ liệu đầu vào. + * @param input Dữ liệu đầu vào. + * @param outputSize Kích thước của dữ liệu đầu ra. + * @param output Dữ liệu đầu ra. + * @return UA_StatusCode Trạng thái của callback. + */ + static UA_StatusCode stopMappingCallBack(UA_Server *server, + const UA_NodeId *sessionId, void *sessionContext, + const UA_NodeId *methodId, void *methodContext, + const UA_NodeId *objectId, void *objectContext, + size_t inputSize, const UA_Variant *input, + size_t outputSize, UA_Variant *output); + + /** + * @brief Thêm phương thức bắt đầu định vị bản đồ. + * Bắt đầu chế độ Localization. Khi biến trạng thái "SlamState" đang là "Ready" thì có thể gọi được hàm này. + * Nếu gọi thành công thì biến "SlamState" sẽ chuyển thành "Localization". + * + * @param server Con trỏ đến server mà phương thức sẽ được thêm vào. + * @param parentID ID của nút cha mà phương thức sẽ được thêm vào. + */ + static void addStartLocalizationMethod(UA_Server *server, UA_NodeId parentID); + + /** + * @brief Callback cho phương thức bắt đầu định vị bản đồ. + * @param server Con trỏ đến server. + * @param sessionId ID của phiên làm việc. + * @param sessionContext Ngữ cảnh của phiên làm việc. + * @param methodId ID của phương thức. + * @param methodContext Ngữ cảnh của phương thức. + * @param objectId ID của đối tượng. + * @param objectContext Ngữ cảnh của đối tượng. + * @param inputSize Kích thước của dữ liệu đầu vào. + * @param input Dữ liệu đầu vào. + * @param outputSize Kích thước của dữ liệu đầu ra. + * @param output Dữ liệu đầu ra. + * @return UA_StatusCode Trạng thái của callback. + */ + static UA_StatusCode startLocalizationCallBack(UA_Server *server, + const UA_NodeId *sessionId, void *sessionContext, + const UA_NodeId *methodId, void *methodContext, + const UA_NodeId *objectId, void *objectContext, + size_t inputSize, const UA_Variant *input, + size_t outputSize, UA_Variant *output); + + /** + * @brief Thêm phương thức kết thúc quá trình định vị. + * Tắt chế độ Localization.Lúc này thông tin toạ độ robot sẽ không được cập nhật. + * Nếu gọi thành công thì biến "SlamState" sẽ chuyển thành "Ready". + * + * @param server Con trỏ đến server mà phương thức sẽ được thêm vào. + * @param parentID ID của nút cha mà phương thức sẽ được thêm vào. + */ + static void addStopLocalizationMethod(UA_Server *server, UA_NodeId parentID); + + /** + * @brief Callback cho phương thức kết thúc quá trình định vị. + * @param server Con trỏ đến server. + * @param sessionId ID của phiên làm việc. + * @param sessionContext Ngữ cảnh của phiên làm việc. + * @param methodId ID của phương thức. + * @param methodContext Ngữ cảnh của phương thức. + * @param objectId ID của đối tượng. + * @param objectContext Ngữ cảnh của đối tượng. + * @param inputSize Kích thước của dữ liệu đầu vào. + * @param input Dữ liệu đầu vào. + * @param outputSize Kích thước của dữ liệu đầu ra. + * @param output Dữ liệu đầu ra. + * @return UA_StatusCode Trạng thái của callback. + */ + static UA_StatusCode stopLocalizationCallBack(UA_Server *server, + const UA_NodeId *sessionId, void *sessionContext, + const UA_NodeId *methodId, void *methodContext, + const UA_NodeId *objectId, void *objectContext, + size_t inputSize, const UA_Variant *input, + size_t outputSize, UA_Variant *output); + + /** + * @brief Thêm phương thức liệt kê các file bản đồ. + * + * @param server Con trỏ đến server mà phương thức sẽ được thêm vào. + * @param parentID ID của nút cha mà phương thức sẽ được thêm vào. + */ + static void addListMapFilesMethod(UA_Server *server, UA_NodeId parentID); + + /** + * @brief Callback cho phương thức liệt kê các file bản đồ. + * @param server Con trỏ đến server. + * @param sessionId ID của phiên làm việc. + * @param sessionContext Ngữ cảnh của phiên làm việc. + * @param methodId ID của phương thức. + * @param methodContext Ngữ cảnh của phương thức. + * @param objectId ID của đối tượng. + * @param objectContext Ngữ cảnh của đối tượng. + * @param inputSize Kích thước của dữ liệu đầu vào. + * @param input Dữ liệu đầu vào. + * @param outputSize Kích thước của dữ liệu đầu ra. + * @param output Dữ liệu đầu ra. + * @return UA_StatusCode Trạng thái của callback. + */ + static UA_StatusCode listMapFilesCallBack(UA_Server *server, + const UA_NodeId *sessionId, void *sessionContext, + const UA_NodeId *methodId, void *methodContext, + const UA_NodeId *objectId, void *objectContext, + size_t inputSize, const UA_Variant *input, + size_t outputSize, UA_Variant *output); + + /** + * @brief Thêm phương thức chọn map file. + * Chọn một map để Localization. Khi biến trạng thái "SlamState" đang là "Ready" thì có thể gọi được hàm này. + * + * @param server Con trỏ đến server mà phương thức sẽ được thêm vào. + * @param parentID ID của nút cha mà phương thức sẽ được thêm vào. + */ + static void addActivateMapMethod(UA_Server *server, UA_NodeId parentID); + + /** + * @brief Callback cho phương thức chọn map file. + * @param server Con trỏ đến server. + * @param sessionId ID của phiên làm việc. + * @param sessionContext Ngữ cảnh của phiên làm việc. + * @param methodId ID của phương thức. + * @param methodContext Ngữ cảnh của phương thức. + * @param objectId ID của đối tượng. + * @param objectContext Ngữ cảnh của đối tượng. + * @param inputSize Kích thước của dữ liệu đầu vào. + * @param input Dữ liệu đầu vào. + * @param outputSize Kích thước của dữ liệu đầu ra. + * @param output Dữ liệu đầu ra. + * @return UA_StatusCode Trạng thái của callback. + */ + static UA_StatusCode activateMapCallBack(UA_Server *server, + const UA_NodeId *sessionId, void *sessionContext, + const UA_NodeId *methodId, void *methodContext, + const UA_NodeId *objectId, void *objectContext, + size_t inputSize, const UA_Variant *input, + size_t outputSize, UA_Variant *output); + + /** + * @brief Thêm phương thức khai báo vị trí vào server. + * @param server Con trỏ đến server mà phương thức sẽ được thêm vào. + * @param parentID ID của nút cha mà phương thức sẽ được thêm vào. + */ + static void addSetInitialPoseMethod(UA_Server *server, UA_NodeId parentID); + + /** + * @brief Callback cho phương thức khai báo vị trí + * @param server Con trỏ đến server. + * @param sessionId ID của phiên làm việc. + * @param sessionContext Ngữ cảnh của phiên làm việc. + * @param methodId ID của phương thức. + * @param methodContext Ngữ cảnh của phương thức. + * @param objectId ID của đối tượng. + * @param objectContext Ngữ cảnh của đối tượng. + * @param inputSize Kích thước của dữ liệu đầu vào. + * @param input Dữ liệu đầu vào. + * @param outputSize Kích thước của dữ liệu đầu ra. + * @param output Dữ liệu đầu ra. + * @return UA_StatusCode Trạng thái của callback. + */ + static UA_StatusCode setInitialPoseCallBack(UA_Server *server, + const UA_NodeId *sessionId, void *sessionContext, + const UA_NodeId *methodId, void *methodContext, + const UA_NodeId *objectId, void *objectContext, + size_t inputSize, const UA_Variant *input, + size_t outputSize, UA_Variant *output); + + /** + * @brief Thêm thuộc tính tọa độ của robot vào server. + * @param server Con trỏ đến server mà phương thức sẽ được thêm vào. + * @param parentID ID của nút cha mà phương thức sẽ được thêm vào. + */ + static void addRobotPoseProperty(UA_Server *server, UA_NodeId parentID); + + static void slamHandle(); + + /** Navigation**/ + + /** + * @brief Thêm phương thức di chuyển đến một nút cụ thể vào server. + * @param server Con trỏ đến server mà phương thức sẽ được thêm vào. + * @param parentID ID của nút cha mà phương thức sẽ được thêm vào. + */ + static void addMoveToNodeMethod(UA_Server *server, UA_NodeId parentID); + + /** + * @brief Callback cho phương thức di chuyển đến nút. + * @param server Con trỏ đến server. + * @param sessionId ID của phiên làm việc. + * @param sessionContext Ngữ cảnh của phiên làm việc. + * @param methodId ID của phương thức. + * @param methodContext Ngữ cảnh của phương thức. + * @param objectId ID của đối tượng. + * @param objectContext Ngữ cảnh của đối tượng. + * @param inputSize Kích thước của dữ liệu đầu vào. + * @param input Dữ liệu đầu vào. + * @param outputSize Kích thước của dữ liệu đầu ra. + * @param output Dữ liệu đầu ra. + * @return UA_StatusCode Trạng thái của callback. + */ + static UA_StatusCode moveToNodeCallBack(UA_Server *server, + const UA_NodeId *sessionId, void *sessionContext, + const UA_NodeId *methodId, void *methodContext, + const UA_NodeId *objectId, void *objectContext, + size_t inputSize, const UA_Variant *input, + size_t outputSize, UA_Variant *output); + + /** + * @brief Thêm phương thức di chuyển đến vị trí nhặt đồ vào server. + * @param server Con trỏ đến server mà phương thức sẽ được thêm vào. + * @param parentID ID của nút cha mà phương thức sẽ được thêm vào. + */ + static void addDockToNodeMethod(UA_Server *server, UA_NodeId parentID); + + /** + * @brief Callback cho phương thức nhặt đồ. + * @param server Con trỏ đến server. + * @param sessionId ID của phiên làm việc. + * @param sessionContext Ngữ cảnh của phiên làm việc. + * @param methodId ID của phương thức. + * @param methodContext Ngữ cảnh của phương thức. + * @param objectId ID của đối tượng. + * @param objectContext Ngữ cảnh của đối tượng. + * @param inputSize Kích thước của dữ liệu đầu vào. + * @param input Dữ liệu đầu vào. + * @param outputSize Kích thước của dữ liệu đầu ra. + * @param output Dữ liệu đầu ra. + * @return UA_StatusCode Trạng thái của callback. + */ + static UA_StatusCode dockToNodeCallBack(UA_Server *server, + const UA_NodeId *sessionId, void *sessionContext, + const UA_NodeId *methodId, void *methodContext, + const UA_NodeId *objectId, void *objectContext, + size_t inputSize, const UA_Variant *input, + size_t outputSize, UA_Variant *output); + + + /** + * @brief Thêm phương thức quay vào server. + * @param server Con trỏ đến server mà phương thức sẽ được thêm vào. + * @param parentID ID của nút cha mà phương thức sẽ được thêm vào. + */ + static void addRotateToMethod(UA_Server *server, UA_NodeId parentID); + + /** + * @brief Callback cho phương thức quay. + * @param server Con trỏ đến server. + * @param sessionId ID của phiên làm việc. + * @param sessionContext Ngữ cảnh của phiên làm việc. + * @param methodId ID của phương thức. + * @param methodContext Ngữ cảnh của phương thức. + * @param objectId ID của đối tượng. + * @param objectContext Ngữ cảnh của đối tượng. + * @param inputSize Kích thước của dữ liệu đầu vào. + * @param input Dữ liệu đầu vào. + * @param outputSize Kích thước của dữ liệu đầu ra. + * @param output Dữ liệu đầu ra. + * @return UA_StatusCode Trạng thái của callback. + */ + static UA_StatusCode rotateToCallBack(UA_Server *server, + const UA_NodeId *sessionId, void *sessionContext, + const UA_NodeId *methodId, void *methodContext, + const UA_NodeId *objectId, void *objectContext, + size_t inputSize, const UA_Variant *input, + size_t outputSize, UA_Variant *output); + + /** + * @brief Thêm phương thức di chuyển thẳng vào server. + * @param server Con trỏ đến server mà phương thức sẽ được thêm vào. + * @param parentID ID của nút cha mà phương thức sẽ được thêm vào. + */ + static void addMoveStraightMethod(UA_Server *server, UA_NodeId parentID); + + /** + * @brief Callback cho phương thức di chuyển thẳng. + * @param server Con trỏ đến server. + * @param sessionId ID của phiên làm việc. + * @param sessionContext Ngữ cảnh của phiên làm việc. + * @param methodId ID của phương thức. + * @param methodContext Ngữ cảnh của phương thức. + * @param objectId ID của đối tượng. + * @param objectContext Ngữ cảnh của đối tượng. + * @param inputSize Kích thước của dữ liệu đầu vào. + * @param input Dữ liệu đầu vào. + * @param outputSize Kích thước của dữ liệu đầu ra. + * @param output Dữ liệu đầu ra. + * @return UA_StatusCode Trạng thái của callback. + */ + static UA_StatusCode moveStraighCallBack(UA_Server *server, + const UA_NodeId *sessionId, void *sessionContext, + const UA_NodeId *methodId, void *methodContext, + const UA_NodeId *objectId, void *objectContext, + size_t inputSize, const UA_Variant *input, + size_t outputSize, UA_Variant *output); + + /** + * @brief Thêm phương thức hủy bỏ vào server. + * @param server Con trỏ đến server mà phương thức sẽ được thêm vào. + * @param parentID ID của nút cha mà phương thức sẽ được thêm vào. + */ + static void addCancelMethod(UA_Server *server, UA_NodeId parentID); + + /** + * @brief Callback cho phương thức hủy bỏ. + * @param server Con trỏ đến server. + * @param sessionId ID của phiên làm việc. + * @param sessionContext Ngữ cảnh của phiên làm việc. + * @param methodId ID của phương thức. + * @param methodContext Ngữ cảnh của phương thức. + * @param objectId ID của đối tượng. + * @param objectContext Ngữ cảnh của đối tượng. + * @param inputSize Kích thước của dữ liệu đầu vào. + * @param input Dữ liệu đầu vào. + * @param outputSize Kích thước của dữ liệu đầu ra. + * @param output Dữ liệu đầu ra. + * @return UA_StatusCode Trạng thái của callback. + */ + static UA_StatusCode cancelCallBack(UA_Server *server, + const UA_NodeId *sessionId, void *sessionContext, + const UA_NodeId *methodId, void *methodContext, + const UA_NodeId *objectId, void *objectContext, + size_t inputSize, const UA_Variant *input, + size_t outputSize, UA_Variant *output); + + /** + * @brief Thêm phương thức tạm dừng vào server. + * @param server Con trỏ đến server mà phương thức sẽ được thêm vào. + * @param parentID ID của nút cha mà phương thức sẽ được thêm vào. + */ + static void addPauseMethod(UA_Server *server, UA_NodeId parentID); + + /** + * @brief Callback cho phương thức tạm dừng. + * @param server Con trỏ đến server. + * @param sessionId ID của phiên làm việc. + * @param sessionContext Ngữ cảnh của phiên làm việc. + * @param methodId ID của phương thức. + * @param methodContext Ngữ cảnh của phương thức. + * @param objectId ID của đối tượng. + * @param objectContext Ngữ cảnh của đối tượng. + * @param inputSize Kích thước của dữ liệu đầu vào. + * @param input Dữ liệu đầu vào. + * @param outputSize Kích thước của dữ liệu đầu ra. + * @param output Dữ liệu đầu ra. + * @return UA_StatusCode Trạng thái của callback. + */ + static UA_StatusCode pauseCallBack(UA_Server *server, + const UA_NodeId *sessionId, void *sessionContext, + const UA_NodeId *methodId, void *methodContext, + const UA_NodeId *objectId, void *objectContext, + size_t inputSize, const UA_Variant *input, + size_t outputSize, UA_Variant *output); + + /** + * @brief Thêm phương thức tiếp tục vào server. + * @param server Con trỏ đến server mà phương thức sẽ được thêm vào. + * @param parentID ID của nút cha mà phương thức sẽ được thêm vào. + */ + static void addResumeMethod(UA_Server *server, UA_NodeId parentID); + + /** + * @brief Callback cho phương thức tiếp tục. + * @param server Con trỏ đến server. + * @param sessionId ID của phiên làm việc. + * @param sessionContext Ngữ cảnh của phiên làm việc. + * @param methodId ID của phương thức. + * @param methodContext Ngữ cảnh của phương thức. + * @param objectId ID của đối tượng. + * @param objectContext Ngữ cảnh của đối tượng. + * @param inputSize Kích thước của dữ liệu đầu vào. + * @param input Dữ liệu đầu vào. + * @param outputSize Kích thước của dữ liệu đầu ra. + * @param output Dữ liệu đầu ra. + * @return UA_StatusCode Trạng thái của callback. + */ + static UA_StatusCode resumeCallBack(UA_Server *server, + const UA_NodeId *sessionId, void *sessionContext, + const UA_NodeId *methodId, void *methodContext, + const UA_NodeId *objectId, void *objectContext, + size_t inputSize, const UA_Variant *input, + size_t outputSize, UA_Variant *output); + + /** + * @brief Thêm phương thức reset vào server. + * @param server Con trỏ đến server mà phương thức sẽ được thêm vào. + * @param parentID ID của nút cha mà phương thức sẽ được thêm vào. + */ + static void addResetMethod(UA_Server *server, UA_NodeId parentID); + + /** + * @brief Callback cho phương thức reset. + * @param server Con trỏ đến server. + * @param sessionId ID của phiên làm việc. + * @param sessionContext Ngữ cảnh của phiên làm việc. + * @param methodId ID của phương thức. + * @param methodContext Ngữ cảnh của phương thức. + * @param objectId ID của đối tượng. + * @param objectContext Ngữ cảnh của đối tượng. + * @param inputSize Kích thước của dữ liệu đầu vào. + * @param input Dữ liệu đầu vào. + * @param outputSize Kích thước của dữ liệu đầu ra. + * @param output Dữ liệu đầu ra. + * @return UA_StatusCode Trạng thái của callback. + */ + static UA_StatusCode resetCallBack(UA_Server *server, + const UA_NodeId *sessionId, void *sessionContext, + const UA_NodeId *methodId, void *methodContext, + const UA_NodeId *objectId, void *objectContext, + size_t inputSize, const UA_Variant *input, + size_t outputSize, UA_Variant *output); + + /** + * @brief Thêm phương thức bật tắt vùng Muted vào server. + * @param server Con trỏ đến server mà phương thức sẽ được thêm vào. + * @param parentID ID của nút cha mà phương thức sẽ được thêm vào. + */ + static void addWriteMutedMethod(UA_Server *server, UA_NodeId parentID); + + /** + * @brief Callback cho phương thức bật tắt vùng Muted. + * @param server Con trỏ đến server. + * @param sessionId ID của phiên làm việc. + * @param sessionContext Ngữ cảnh của phiên làm việc. + * @param methodId ID của phương thức. + * @param methodContext Ngữ cảnh của phương thức. + * @param objectId ID của đối tượng. + * @param objectContext Ngữ cảnh của đối tượng. + * @param inputSize Kích thước của dữ liệu đầu vào. + * @param input Dữ liệu đầu vào. + * @param outputSize Kích thước của dữ liệu đầu ra. + * @param output Dữ liệu đầu ra. + * @return UA_StatusCode Trạng thái của callback. + */ + static UA_StatusCode writeMutedCallBack(UA_Server *server, + const UA_NodeId *sessionId, void *sessionContext, + const UA_NodeId *methodId, void *methodContext, + const UA_NodeId *objectId, void *objectContext, + size_t inputSize, const UA_Variant *input, + size_t outputSize, UA_Variant *output); + + /** + * @brief Thêm thuộc tính trạng thái của Robot khi di chuyển. + * @param server Con trỏ đến server mà phương thức sẽ được thêm vào. + * @param parentID ID của nút cha mà phương thức sẽ được thêm vào. + */ + static void addNavigationState(UA_Server *server, UA_NodeId parentID); + + static void navigationHandle(); + + /** Moritoring **/ + /** + * @brief Thêm thuộc tính vận tốc của Robot khi di chuyển. + * @param server Con trỏ đến server mà phương thức sẽ được thêm vào. + * @param parentID ID của nút cha mà phương thức sẽ được thêm vào. + */ + static void addVelocityCommand(UA_Server *server, UA_NodeId parentID); + + static void monitorHandle(); + + /** IMR Plus **/ + /** + * @brief Thêm phương thức khởi động và chạy tay máy Dobot. + * @param server Con trỏ đến server mà phương thức sẽ được thêm vào. + * @param parentID ID của nút cha mà phương thức sẽ được thêm vào. + */ + static void addPickUpDobotMethod(UA_Server *server, UA_NodeId parentID); + + /** + * @brief Callback cho phương thức khởi động và chạy tay máy Dobot. + * @param server Con trỏ đến server. + * @param sessionId ID của phiên làm việc. + * @param sessionContext Ngữ cảnh của phiên làm việc. + * @param methodId ID của phương thức. + * @param methodContext Ngữ cảnh của phương thức. + * @param objectId ID của đối tượng. + * @param objectContext Ngữ cảnh của đối tượng. + * @param inputSize Kích thước của dữ liệu đầu vào. + * @param input Dữ liệu đầu vào. + * @param outputSize Kích thước của dữ liệu đầu ra. + * @param output Dữ liệu đầu ra. + * @return UA_StatusCode Trạng thái của callback. + */ + static UA_StatusCode PickUpDobotCallBack(UA_Server *server, + const UA_NodeId *sessionId, void *sessionContext, + const UA_NodeId *methodId, void *methodContext, + const UA_NodeId *objectId, void *objectContext, + size_t inputSize, const UA_Variant *input, + size_t outputSize, UA_Variant *output); + + /** + * @brief Thêm phương thức về gốc tay máy Dobot. + * @param server Con trỏ đến server mà phương thức sẽ được thêm vào. + * @param parentID ID của nút cha mà phương thức sẽ được thêm vào. + */ + static void addGoHomeDobotMethod(UA_Server *server, UA_NodeId parentID); + + /** + * @brief Callback cho phương thức về gốc tay máy Dobot. + * @param server Con trỏ đến server. + * @param sessionId ID của phiên làm việc. + * @param sessionContext Ngữ cảnh của phiên làm việc. + * @param methodId ID của phương thức. + * @param methodContext Ngữ cảnh của phương thức. + * @param objectId ID của đối tượng. + * @param objectContext Ngữ cảnh của đối tượng. + * @param inputSize Kích thước của dữ liệu đầu vào. + * @param input Dữ liệu đầu vào. + * @param outputSize Kích thước của dữ liệu đầu ra. + * @param output Dữ liệu đầu ra. + * @return UA_StatusCode Trạng thái của callback. + */ + static UA_StatusCode goHomeDobotCallBack(UA_Server *server, + const UA_NodeId *sessionId, void *sessionContext, + const UA_NodeId *methodId, void *methodContext, + const UA_NodeId *objectId, void *objectContext, + size_t inputSize, const UA_Variant *input, + size_t outputSize, UA_Variant *output); + + /** + * @brief Thêm phương thay đổi số lượng hàng cần lấy + * @param server Con trỏ đến server mà phương thức sẽ được thêm vào. + * @param parentID ID của nút cha mà phương thức sẽ được thêm vào. + */ + static void addCountMethod(UA_Server *server, UA_NodeId parentID); + + /** + * @brief Callback cho phương thức thay đổi số lượng hàng cần lấy + * @param server Con trỏ đến server. + * @param sessionId ID của phiên làm việc. + * @param sessionContext Ngữ cảnh của phiên làm việc. + * @param methodId ID của phương thức. + * @param methodContext Ngữ cảnh của phương thức. + * @param objectId ID của đối tượng. + * @param objectContext Ngữ cảnh của đối tượng. + * @param inputSize Kích thước của dữ liệu đầu vào. + * @param input Dữ liệu đầu vào. + * @param outputSize Kích thước của dữ liệu đầu ra. + * @param output Dữ liệu đầu ra. + * @return UA_StatusCode Trạng thái của callback. + */ + static UA_StatusCode counterCallBack(UA_Server *server, + const UA_NodeId *sessionId, void *sessionContext, + const UA_NodeId *methodId, void *methodContext, + const UA_NodeId *objectId, void *objectContext, + size_t inputSize, const UA_Variant *input, + size_t outputSize, UA_Variant *output); + + /** + * @brief Thêm phương thức dừng tay máy Dobot. + * @param server Con trỏ đến server mà phương thức sẽ được thêm vào. + * @param parentID ID của nút cha mà phương thức sẽ được thêm vào. + */ + static void addCancelDobotMethod(UA_Server *server, UA_NodeId parentID); + + /** + * @brief Callback cho phương thức dừng tay máy Dobot. + * @param server Con trỏ đến server. + * @param sessionId ID của phiên làm việc. + * @param sessionContext Ngữ cảnh của phiên làm việc. + * @param methodId ID của phương thức. + * @param methodContext Ngữ cảnh của phương thức. + * @param objectId ID của đối tượng. + * @param objectContext Ngữ cảnh của đối tượng. + * @param inputSize Kích thước của dữ liệu đầu vào. + * @param input Dữ liệu đầu vào. + * @param outputSize Kích thước của dữ liệu đầu ra. + * @param output Dữ liệu đầu ra. + * @return UA_StatusCode Trạng thái của callback. + */ + static UA_StatusCode cancelDobotCallBack(UA_Server *server, + const UA_NodeId *sessionId, void *sessionContext, + const UA_NodeId *methodId, void *methodContext, + const UA_NodeId *objectId, void *objectContext, + size_t inputSize, const UA_Variant *input, + size_t outputSize, UA_Variant *output); + + /** + * @brief Thêm phương thức tiếp tục chạy tay máy Dobot. + * @param server Con trỏ đến server mà phương thức sẽ được thêm vào. + * @param parentID ID của nút cha mà phương thức sẽ được thêm vào. + */ + static void addContinueDobotMethod(UA_Server *server, UA_NodeId parentID); + + /** + * @brief Callback cho phương thức tiếp tục chạy tay máy Dobot. + * @param server Con trỏ đến server. + * @param sessionId ID của phiên làm việc. + * @param sessionContext Ngữ cảnh của phiên làm việc. + * @param methodId ID của phương thức. + * @param methodContext Ngữ cảnh của phương thức. + * @param objectId ID của đối tượng. + * @param objectContext Ngữ cảnh của đối tượng. + * @param inputSize Kích thước của dữ liệu đầu vào. + * @param input Dữ liệu đầu vào. + * @param outputSize Kích thước của dữ liệu đầu ra. + * @param output Dữ liệu đầu ra. + * @return UA_StatusCode Trạng thái của callback. + */ + static UA_StatusCode continueDobotCallBack(UA_Server *server, + const UA_NodeId *sessionId, void *sessionContext, + const UA_NodeId *methodId, void *methodContext, + const UA_NodeId *objectId, void *objectContext, + size_t inputSize, const UA_Variant *input, + size_t outputSize, UA_Variant *output); + + /** + * @brief Thêm phương thức khởi động nguồn tay máy Dobot. + * @param server Con trỏ đến server mà phương thức sẽ được thêm vào. + * @param parentID ID của nút cha mà phương thức sẽ được thêm vào. + */ + static void addPowerOnDobotMethod(UA_Server *server, UA_NodeId parentID); + + /** + * @brief Callback cho phương thức khởi động nguồn tay máy Dobot. + * @param server Con trỏ đến server. + * @param sessionId ID của phiên làm việc. + * @param sessionContext Ngữ cảnh của phiên làm việc. + * @param methodId ID của phương thức. + * @param methodContext Ngữ cảnh của phương thức. + * @param objectId ID của đối tượng. + * @param objectContext Ngữ cảnh của đối tượng. + * @param inputSize Kích thước của dữ liệu đầu vào. + * @param input Dữ liệu đầu vào. + * @param outputSize Kích thước của dữ liệu đầu ra. + * @param output Dữ liệu đầu ra. + * @return UA_StatusCode Trạng thái của callback. + */ + static UA_StatusCode powerOnDobotCallBack(UA_Server *server, + const UA_NodeId *sessionId, void *sessionContext, + const UA_NodeId *methodId, void *methodContext, + const UA_NodeId *objectId, void *objectContext, + size_t inputSize, const UA_Variant *input, + size_t outputSize, UA_Variant *output); + + /** + * @brief Thêm thuộc tính tay máy Dobot. + * @param server Con trỏ đến server mà phương thức sẽ được thêm vào. + * @param parentID ID của nút cha mà phương thức sẽ được thêm vào. + */ + static void addDobotProperties(UA_Server *server, UA_NodeId parentID); + + static void dobotPropertiesHandle(); + /** + * @brief Thêm phương thức trả hàng. + * @param server Con trỏ đến server mà phương thức sẽ được thêm vào. + * @param parentID ID của nút cha mà phương thức sẽ được thêm vào. + */ + static void addunLoadMethod(UA_Server *server, UA_NodeId parentID); + + /** + * @brief Callback cho phương thức trả hàng. + * @param server Con trỏ đến server. + * @param sessionId ID của phiên làm việc. + * @param sessionContext Ngữ cảnh của phiên làm việc. + * @param methodId ID của phương thức. + * @param methodContext Ngữ cảnh của phương thức. + * @param objectId ID của đối tượng. + * @param objectContext Ngữ cảnh của đối tượng. + * @param inputSize Kích thước của dữ liệu đầu vào. + * @param input Dữ liệu đầu vào. + * @param outputSize Kích thước của dữ liệu đầu ra. + * @param output Dữ liệu đầu ra. + * @return UA_StatusCode Trạng thái của callback. + */ + static UA_StatusCode unLoadCallBack(UA_Server *server, + const UA_NodeId *sessionId, void *sessionContext, + const UA_NodeId *methodId, void *methodContext, + const UA_NodeId *objectId, void *objectContext, + size_t inputSize, const UA_Variant *input, + size_t outputSize, UA_Variant *output); + + /** + * @brief Thêm phương thức nhận hàng. + * @param server Con trỏ đến server mà phương thức sẽ được thêm vào. + * @param parentID ID của nút cha mà phương thức sẽ được thêm vào. + */ + static void addLoadMethod(UA_Server *server, UA_NodeId parentID); + + /** + * @brief Callback cho phương thức nhận hàng. + * @param server Con trỏ đến server. + * @param sessionId ID của phiên làm việc. + * @param sessionContext Ngữ cảnh của phiên làm việc. + * @param methodId ID của phương thức. + * @param methodContext Ngữ cảnh của phương thức. + * @param objectId ID của đối tượng. + * @param objectContext Ngữ cảnh của đối tượng. + * @param inputSize Kích thước của dữ liệu đầu vào. + * @param input Dữ liệu đầu vào. + * @param outputSize Kích thước của dữ liệu đầu ra. + * @param output Dữ liệu đầu ra. + * @return UA_StatusCode Trạng thái của callback. + */ + static UA_StatusCode loadCallBack(UA_Server *server, + const UA_NodeId *sessionId, void *sessionContext, + const UA_NodeId *methodId, void *methodContext, + const UA_NodeId *objectId, void *objectContext, + size_t inputSize, const UA_Variant *input, + size_t outputSize, UA_Variant *output); + + /** + * @brief Thêm phương thức Reset Băng tải + * @param server Con trỏ đến server mà phương thức sẽ được thêm vào. + * @param parentID ID của nút cha mà phương thức sẽ được thêm vào. + */ + static void addResetConveyorBeltMethod(UA_Server *server, UA_NodeId parentID); + + /** + * @brief Callback cho phương thức Reset Băng tải + * @param server Con trỏ đến server. + * @param sessionId ID của phiên làm việc. + * @param sessionContext Ngữ cảnh của phiên làm việc. + * @param methodId ID của phương thức. + * @param methodContext Ngữ cảnh của phương thức. + * @param objectId ID của đối tượng. + * @param objectContext Ngữ cảnh của đối tượng. + * @param inputSize Kích thước của dữ liệu đầu vào. + * @param input Dữ liệu đầu vào. + * @param outputSize Kích thước của dữ liệu đầu ra. + * @param output Dữ liệu đầu ra. + * @return UA_StatusCode Trạng thái của callback. + */ + static UA_StatusCode resetConveyorBeltCallBack(UA_Server *server, + const UA_NodeId *sessionId, void *sessionContext, + const UA_NodeId *methodId, void *methodContext, + const UA_NodeId *objectId, void *objectContext, + size_t inputSize, const UA_Variant *input, + size_t outputSize, UA_Variant *output); + /** + * @brief Thêm thuộc tính trạng thái băng tải. + * @param server Con trỏ đến server mà phương thức sẽ được thêm vào. + * @param parentID ID của nút cha mà phương thức sẽ được thêm vào. + */ + static void addConveyorBeltState(UA_Server *server, UA_NodeId parentID); + + static void ConveyorBeltHandle(); + + private: + ros::NodeHandle nh_; + static ros::Publisher init_pub_; + static pthread_t hThread_; + static void *ThreadWorker(void *_); + + static std::shared_ptr server_ptr_; + static std::shared_ptr move_base_ptr_; + static std::shared_ptr loc_base_ptr_; + static std::shared_ptr monitor_ptr_; + + static UA_NodeId *robotPoseX_Id_; + static UA_NodeId *robotPoseY_Id_; + static UA_NodeId *robotPoseYaw_Id_; + static UA_NodeId *currentActiveMap_Id_; + static UA_NodeId *slamState_Id_; + static UA_NodeId *workingDirectory_Id_; + + static UA_String amr_feedback_str_; + static UA_String amr_status_str_; + static UA_UInt32 amr_status_en_; + static UA_NodeId *amr_status_str_Id_; + static UA_NodeId *amr_status_en_Id_; + static UA_NodeId *amr_robotFeedBack_Id_; + + static UA_NodeId *vx_Id_; + static UA_NodeId *vy_Id_; + static UA_NodeId *omega_Id_; + + static UA_NodeId *arm_dobot_state_str_Id_; + static UA_NodeId *arm_dobot_state_en_Id_; + static UA_NodeId *arm_dobot_count_ok_max_Id_; + static UA_NodeId *arm_dobot_count_ng_max_Id_; + static UA_NodeId *arm_dobot_mode_Id_; + static UA_NodeId *arm_dobot_status_code_Id_; + + static UA_NodeId *belt_status_str_Id_; + static UA_NodeId *belt_status_en_Id_; + static UA_NodeId *have_goods_Id_; + + public: + static void resetState(); + + /** + * @brief helper class for subscribing to odometry + */ + static nav_2d_msgs::Twist2D cmd_vel_max_; + + /** + * @brief Set muted value + */ + static bool *muted_value_; + + static std::thread *arm_thread_ptr_; + static std::function arm_function_ptr_; + static amr_control::State arm_dobot_state_en_; + static UA_String arm_dobot_state_str_; + static unsigned int *count_ng_max_, *count_ok_max_; + static bool *arm_cancel_; + static bool *arm_continue_; + static bool *arm_go_home_; + static bool *arm_power_on_; + static double *mode_ptr_; + static unsigned int *status_code_ptr_; + static unsigned int old_status_code_; + + static amr_control::State *cur_belt_state_en_; + static UA_String belt_state_str_; + static UA_Boolean have_goods_; + static bool *belt_cancel_; + static std::function unLoad_excuted_; + static std::function load_excuted_; + static std::thread *belt_thread_ptr_; + }; +} // namespace amr_control + +#endif // __AMR_COTROLLER_OPC_UA_SERVER_DEFINE_API_H_INCLUDED_ \ No newline at end of file diff --git a/Controllers/Packages/amr_control/include/amr_control/amr_safety.h b/Controllers/Packages/amr_control/include/amr_control/amr_safety.h new file mode 100644 index 0000000..7ceb506 --- /dev/null +++ b/Controllers/Packages/amr_control/include/amr_control/amr_safety.h @@ -0,0 +1,55 @@ +#ifndef __AMR_SAFETY_H_INCLUDED_ +#define __AMR_SAFETY_H_INCLUDED_ +#include +#include +#include +#include +#include "delta_modbus/delta_modbus_tcp.h" + +namespace amr_control +{ + class AmrSafety + { + public: + /** + * @brief Default constructor for AmrSafety + */ + AmrSafety(); + + /** + * @brief Constructor for AmrSafety with ROS node handle + * @param nh ROS NodeHandle for initialization + */ + AmrSafety(const ros::NodeHandle &nh); + + /** + * @brief Virtual destructor for AmrSafety + */ + virtual ~AmrSafety(); + + /** + * @brief Initialize the ServerHandle with a ROS node handle + * @param nh ROS NodeHandle for initialization + */ + virtual void init(const ros::NodeHandle &nh); + + /** + * @brief Get object PLC controller + * @param plc_controller_ptr A smart pointer of the PLC object + */ + virtual bool getController(std::shared_ptr plc_controller_ptr); + + /** + * @brief Safety Hanlde + */ + virtual void safetyHandle(nav_2d_msgs::Twist2D velocity, nav_2d_msgs::Twist2D cmd_vel_max, nav_2d_msgs::Twist2D &cmd_vel); + + virtual void writeMutesSafety(bool value); + + private: + bool initalized_; + std::shared_ptr plc_controller_ptr_; + }; +} // namespace amr_control + +#endif // __AMR_SAFETY_H_INCLUDED_ \ No newline at end of file diff --git a/Controllers/Packages/amr_control/include/amr_control/amr_vda_5050_client_api.h b/Controllers/Packages/amr_control/include/amr_control/amr_vda_5050_client_api.h new file mode 100644 index 0000000..776ba93 --- /dev/null +++ b/Controllers/Packages/amr_control/include/amr_control/amr_vda_5050_client_api.h @@ -0,0 +1,122 @@ +#ifndef __AMR_COTROLLER_VDA_5050_CLIENT_DEFINE_API_H_INCLUDED_ +#define __AMR_COTROLLER_VDA_5050_CLIENT_DEFINE_API_H_INCLUDED_ + +#include +#include "amr_comunication/vda_5050/vda_5050_connector.h" +#include "move_base_core/navigation.h" +#include "loc_core/localization.h" +#include "amr_control/amr_monitor.h" +#include "amr_control/common.h" +#include +#include + +namespace amr_control +{ + class VDA5050ClientAPI + { + public: + /** + * @brief Contructor + */ + VDA5050ClientAPI(); + + /** + * @brief Contructor + * @param monitor Con trỏ đến đối tượng AmrMonitor, dùng để theo dõi trạng thái và sự kiện. + * @param move_base Con trỏ đến đối tượng BaseNavigation, cho phép điều hướng robot. + * @param loc_base Con trỏ đến đối tượng LocBase. + */ + VDA5050ClientAPI(ros::NodeHandle nh, std::shared_ptr move_base, + std::shared_ptr loc_base, std::shared_ptr monitor); + + /** + * @brief Detructor + */ + virtual ~VDA5050ClientAPI(); + + protected: + virtual void orderWorker(); + + virtual void moveTo(vda_5050::Order order, uint8_t &status, std::string &message); + + virtual void moveToDock(vda_5050::Order order, uint8_t &status, std::string &message); + + virtual void rotateTo(vda_5050::Order order, uint8_t &status, std::string &message); + + virtual void instantActionWorker(); + + virtual void excuteAction(); + + virtual void updateVelocity(double velocity); + + virtual void updateAngular(double angular); + + virtual void cancelOrderCallBack(std::shared_ptr action_state); + + virtual void cancelOrder(std::shared_ptr action_state); + + template + void excuteNoneAction( + void (T::*excute)(std::shared_ptr, std::shared_ptr), T *obj, + std::shared_ptr action, std::shared_ptr action_state); + + virtual void unDockFromStation(std::shared_ptr action, std::shared_ptr action_state); + + virtual void dockToStaton(std::shared_ptr action, std::shared_ptr action_state); + + virtual void pickUp(std::shared_ptr action, std::shared_ptr action_state); + + virtual void unLoad(std::shared_ptr action, std::shared_ptr action_state); + + virtual void load(std::shared_ptr action, std::shared_ptr action_state); + + virtual void mutedSafetyON(std::shared_ptr action, std::shared_ptr action_state); + + virtual void mutedSafetyOFF(std::shared_ptr action, std::shared_ptr action_state); + + void updating(); + + void updatingvda5050Visualization(); + + void updatingvda5050State(); + + void resetState(); + + // properties + std::shared_ptr client_ptr_; + static std::shared_ptr move_base_ptr_; + static std::shared_ptr loc_base_ptr_; + static std::shared_ptr monitor_ptr_; + + ros::Publisher plan_pub_; + std::string global_plan_msg_type_; + std::thread update_thread_; + + std::map threads_map_; + std::mutex threads_mutex; + + public: + // Belt Convey + amr_control::State *cur_belt_state_en_; + std::function unLoad_excuted_; + std::function load_excuted_; + std::thread *belt_thread_ptr_; + std::mutex belt_mutex_; + bool belt_joined_; + + // arm control + std::thread *arm_thread_ptr_; + std::function arm_function_ptr_; + unsigned int *count_ng_max_, *count_ok_max_; + + bool *muted_value_; + bool *pause_action_; + bool *cancel_action_; + bool *enable_action_; + vda_5050::State::OperatingMode mode_; + nav_2d_msgs::Twist2D cmd_vel_max_, cmd_vel_max_saved_; + }; + +} // namespace amr_control + +#endif // __AMR_COTROLLER_VDA_5050_CLIENT_DEFINE_API_H_INCLUDED_ \ No newline at end of file diff --git a/Controllers/Packages/amr_control/include/amr_control/common.h b/Controllers/Packages/amr_control/include/amr_control/common.h new file mode 100644 index 0000000..61a3d3d --- /dev/null +++ b/Controllers/Packages/amr_control/include/amr_control/common.h @@ -0,0 +1,25 @@ +#ifndef _AMR_CONTROL_COMMON_H_INCLUDED_ +#define _AMR_CONTROL_COMMON_H_INCLUDED_ + +namespace amr_control +{ + enum State + { + WAITING, + INITIALIZING, + RUNNING, + PAUSED, + FINISHED, + FAILED + }; + + enum OperatingMode + { + AUTOMATIC, + SEMIAUTOMATIC, + MANUAL, + SERVICE, + TEACHING + }; +} +#endif \ No newline at end of file diff --git a/Controllers/Packages/amr_control/package.xml b/Controllers/Packages/amr_control/package.xml new file mode 100644 index 0000000..3be4192 --- /dev/null +++ b/Controllers/Packages/amr_control/package.xml @@ -0,0 +1,96 @@ + + + amr_control + 0.0.0 + The amr_control package + + + + + robotics + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + geometry_msgs + move_base_core + nav_2d_utils + roscpp + rospy + std_msgs + angles + delta_modbus + nova5_control + loc_core + amr_comunication + vda5050_msgs + + geometry_msgs + move_base_core + nav_2d_utils + roscpp + rospy + std_msgs + angles + delta_modbus + nova5_control + loc_core + amr_comunication + vda5050_msgs + + geometry_msgs + move_base_core + nav_2d_utils + roscpp + rospy + std_msgs + angles + delta_modbus + nova5_control + loc_core + amr_comunication + vda5050_msgs + + + + + + + diff --git a/Controllers/Packages/amr_control/src/amr_control.cpp b/Controllers/Packages/amr_control/src/amr_control.cpp new file mode 100644 index 0000000..7d813e5 --- /dev/null +++ b/Controllers/Packages/amr_control/src/amr_control.cpp @@ -0,0 +1,578 @@ +#include "amr_control/amr_control.h" +#include + +namespace amr_control +{ + AmrController::AmrController(ros::NodeHandle &nh, std::shared_ptr buffer) + : initalized_(false), + vda_5050_client_api_ptr_(nullptr), + opc_ua_server_api_ptr_(nullptr), + move_base_initialized_(false), + loc_base_initialized_(false), + monitor_ptr_(nullptr), + loc_base_ptr_(nullptr), + move_base_ptr_(nullptr), + amr_safety_ptr_(nullptr), + move_base_loader_("move_base_core", "move_base_core::BaseNavigation"), + loc_base_loader_("loc_core", "loc_core::BaseLocalization") + { + this->init(nh, buffer); + } + + AmrController::AmrController() + : initalized_(false), + move_base_initialized_(false), + loc_base_initialized_(false), + vda_5050_client_api_ptr_(nullptr), + opc_ua_server_api_ptr_(nullptr), + monitor_ptr_(nullptr), + loc_base_ptr_(nullptr), + move_base_ptr_(nullptr), + amr_safety_ptr_(nullptr), + move_base_loader_("move_base_core", "move_base_core::BaseNavigation"), + loc_base_loader_("loc_core", "loc_core::BaseLocalization") + { + } + + AmrController::~AmrController() + { + // Wait for both threads to complete + if (move_base_thr_) + { + move_base_thr_->join(); + move_base_thr_.reset(); + } + + if (loc_base_thr_) + { + loc_base_thr_->join(); + loc_base_thr_.reset(); + } + + if (server_thr_) + { + server_thr_->join(); + server_thr_.reset(); + } + + if (monitor_thr_) + { + monitor_thr_->join(); + monitor_thr_.reset(); + } + + if (opc_ua_server_api_ptr_) + { + opc_ua_server_api_ptr_.reset(); + } + + if (vda_5050_client_api_ptr_) + { + vda_5050_client_api_ptr_.reset(); + } + } + + void AmrController::init(ros::NodeHandle &nh, std::shared_ptr buffer) + { + if (!initalized_) + { + nh_ = nh; + tf_ = buffer; + + monitor_thr_ = std::make_shared( + [this]() + { + this->initalizingMonitorHandle(nh_); + this->threadHandle(); + }); + + move_base_initialized_ = false; + // Setup base localization + loc_base_thr_ = std::make_shared( + [this]() + { + this->initalizingLocalizationHandle(nh_); + { + std::unique_lock lock(init_loc_base_mutex_); + loc_base_initialized_ = true; + } + init_loc_base_cv_.notify_one(); + }); + + // Setup base navigation + move_base_thr_ = std::make_shared( + [this]() + { + std::unique_lock lock(init_loc_base_mutex_); + init_loc_base_cv_.wait(lock, [this]() + { return loc_base_initialized_; }); + + this->initalizingMoveBaseHandle(nh_); + { + std::lock_guard lock(init_move_base_mutex_); + move_base_initialized_ = true; + } + init_move_base_cv_.notify_one(); + + // this->controllerDotuff(); + }); + + // Setup server + server_thr_ = std::make_shared( + [this]() + { + std::unique_lock lock(init_move_base_mutex_); + init_move_base_cv_.wait(lock, [this]() + { return move_base_initialized_; }); + this->initalizingComunicationHandle(nh_); + }); + + ros::NodeHandle nh_core; + is_detected_maker_sub_ = nh_core.subscribe("/is_detected_maker", 1, &AmrController::isDetectedMakerCallback, this); + initalized_ = true; + } + } + + void AmrController::stop() + { + } + + void AmrController::initalizingComunicationHandle(ros::NodeHandle &nh) + { + + this->count_ng_max_ = this->count_ok_max_ = 1; + std::shared_ptr opc_ua_server_thr = std::make_shared( + [this, nh]() + { + opc_ua_server_api_ptr_ = + std::make_shared(nh, move_base_ptr_, loc_base_ptr_, monitor_ptr_); + + OpcUAServerAPI::muted_value_ = &this->muted_; + OpcUAServerAPI::unLoad_excuted_ = std::bind(&AmrController::unLoadCallBack, this); + OpcUAServerAPI::load_excuted_ = std::bind(&AmrController::loadCallBack, this); + OpcUAServerAPI::belt_thread_ptr_ = &this->belt_thread_; + OpcUAServerAPI::cur_belt_state_en_ = &this->cur_belt_state_en_; + OpcUAServerAPI::belt_cancel_ = &this->cancel_; + + OpcUAServerAPI::arm_function_ptr_ = std::bind(&AmrController::ArmCallBack, this); + OpcUAServerAPI::arm_thread_ptr_ = &this->arm_thread_; + OpcUAServerAPI::arm_cancel_ = &this->enable_; + OpcUAServerAPI::arm_go_home_ = &this->arm_go_home_; + OpcUAServerAPI::arm_continue_ = &this->arm_continue_; + OpcUAServerAPI::arm_power_on_ = &this->arm_power_on_; + OpcUAServerAPI::count_ng_max_ = &this->count_ng_max_; + OpcUAServerAPI::count_ok_max_ = &this->count_ok_max_; + OpcUAServerAPI::status_code_ptr_ = this->status_code_ptr_; + opc_ua_server_api_ptr_->start(); + }); + + std::shared_ptr vda_5050_client_thr = std::make_shared( + [this, nh]() + { + vda_5050_client_api_ptr_ = + std::make_shared(nh, move_base_ptr_, loc_base_ptr_, monitor_ptr_); + + vda_5050_client_api_ptr_->muted_value_ = &this->muted_; + vda_5050_client_api_ptr_->unLoad_excuted_ = std::bind(&AmrController::unLoadCallBack, this); + vda_5050_client_api_ptr_->load_excuted_ = std::bind(&AmrController::loadCallBack, this); + vda_5050_client_api_ptr_->cur_belt_state_en_ = &this->cur_belt_state_en_; + vda_5050_client_api_ptr_->belt_thread_ptr_ = &this->belt_thread_; + + vda_5050_client_api_ptr_->arm_function_ptr_ = std::bind(&AmrController::ArmCallBack, this); + vda_5050_client_api_ptr_->arm_thread_ptr_ = &this->arm_thread_; + vda_5050_client_api_ptr_->enable_action_ = &this->enable_; + vda_5050_client_api_ptr_->cancel_action_ = &this->cancel_; + vda_5050_client_api_ptr_->pause_action_ = &this->pause_; + vda_5050_client_api_ptr_->count_ng_max_ = &this->count_ng_max_; + vda_5050_client_api_ptr_->count_ok_max_ = &this->count_ok_max_; + }); + ROS_INFO("Initalizing comunication is done"); + opc_ua_server_thr->join(); + vda_5050_client_thr->join(); + } + + void AmrController::initalizingLocalizationHandle(ros::NodeHandle &nh) + { + std::string obj_name("loc_base::LocBase"); + if (tf_ == nullptr) + throw std::runtime_error("tf2_ros::Buffer object is null"); + try + { + loc_base_ptr_ = loc_base_loader_.createUniqueInstance(obj_name); + ros::NodeHandle node = ros::NodeHandle(nh, "global_costmap"); + loc_base_ptr_->initialize(node, tf_); + } + catch (pluginlib::PluginlibException &ex) + { + ROS_FATAL("Failed to create the %s, are you sure it is properly registered and that the containing library is built? Exception: %s", obj_name.c_str(), ex.what()); + exit(1); + } + catch (std::exception &e) + { + ROS_FATAL("Failed to create the %s, are you sure it is properly registered and that the containing library is built? Exception: %s", obj_name.c_str(), e.what()); + exit(1); + } + } + + void AmrController::initalizingMoveBaseHandle(ros::NodeHandle &nh) + { + std::string obj_name("move_base::MoveBase"); + if (tf_ == nullptr) + throw std::runtime_error("tf2_ros::Buffer object is null"); + try + { + move_base_ptr_ = move_base_loader_.createUniqueInstance(obj_name); + ROS_INFO("Created object %s susseced", obj_name.c_str()); + move_base_ptr_->initialize(tf_); + ros::Rate r(3); + do + { + r.sleep(); + ros::spinOnce(); + } while (ros::ok() && !move_base_ptr_->nav_feedback_->is_ready); + + if (move_base_ptr_ != nullptr && + move_base_ptr_->nav_feedback_ != nullptr && + move_base_ptr_->nav_feedback_->is_ready) + { + geometry_msgs::Vector3 linear; + linear.x = 0.3; + move_base_ptr_->setTwistLinear(linear); + linear.x = -0.3; + move_base_ptr_->setTwistLinear(linear); + } + } + catch (pluginlib::PluginlibException &ex) + { + ROS_FATAL("Failed to create the %s, are you sure it is properly registered and that the containing library is built? Exception: %s", obj_name.c_str(), ex.what()); + exit(1); + } + catch (std::exception &e) + { + ROS_FATAL("Failed to create the %s, are you sure it is properly registered and that the containing library is built? Exception: %s", obj_name.c_str(), e.what()); + exit(1); + } + } + + void AmrController::initalizingMonitorHandle(ros::NodeHandle &nh) + { + this->monitor_ptr_ = std::make_shared(nh); + } + + void AmrController::ArmCallBack() + { + ROS_INFO("Arm Calling"); + std::lock_guard lock(this->arm_mutex_); + { + arm_joined_ = true; + this->arm_thread_ = std::thread(&AmrController::ArmDotuff, this); + } + } + + void AmrController::ArmDotuff() + { + std::shared_ptr arm_control_ptr; + arm_control_ptr = std::make_shared(); + arm_control_ptr->enable_ = &this->enable_; + arm_control_ptr->go_home_flag_ = &this->arm_go_home_; + arm_control_ptr->continue_flag_ = &this->arm_continue_; + arm_control_ptr->power_on_flag_ = &this->arm_power_on_; + OpcUAServerAPI::mode_ptr_ = &arm_control_ptr->mode_; + this->status_code_ptr_ = reinterpret_cast(&arm_control_ptr->statusCode_); + + if (!this->arm_go_home_) + { + arm_control_ptr->ok_count_max_ = &this->count_ok_max_; + arm_control_ptr->ng_count_max_ = &this->count_ng_max_; + arm_control_ptr->startModeThread(); + } + else + { + arm_control_ptr->startHomeThread(); + } + + arm_control_ptr.reset(); + ROS_INFO("Arm Finished"); + std::lock_guard lock(this->arm_mutex_); + this->arm_joined_ = false; + } + + void AmrController::unLoadCallBack() + { + std::lock_guard lock(this->arm_mutex_); + { + ROS_INFO("Shiping call"); + this->belt_joined_ = true; + this->cancel_ = false; + this->cur_belt_state_en_ = amr_control::State::WAITING; + this->belt_thread_ = std::thread(&AmrController::conveyorBeltsShipping, this, std::ref(this->cur_belt_state_en_)); + } + } + + void AmrController::conveyorBeltsShipping(amr_control::State &state) + { + state = amr_control::State::INITIALIZING; + std::shared_ptr plc_controller_ptr_; + plc_controller_ptr_ = std::make_shared("192.168.2.5", 502); + plc_controller_ptr_->connect(); + + if (!plc_controller_ptr_->checkConnected()) + { + state = amr_control::State::FAILED; + return; + } + + if (plc_controller_ptr_ && plc_controller_ptr_->checkConnected()) + { + int shipping_regsister = 119, receiving_regsister = 116; + plc_controller_ptr_->setM(shipping_regsister); + plc_controller_ptr_->resetM(receiving_regsister); + ros::Duration(0.1).sleep(); + plc_controller_ptr_->resetM(shipping_regsister); + plc_controller_ptr_->resetM(receiving_regsister); + + ros::Rate r(5); + while (ros::ok() && plc_controller_ptr_->checkConnected() && !this->cancel_) + { + state = amr_control::State::RUNNING; + bool output_belt[2]; + plc_controller_ptr_->mulGetM(124, 125, output_belt); + + bool enable_shipping = output_belt[0]; + bool enable_receiving = output_belt[1]; + if (!enable_shipping && !enable_receiving) + { + state = amr_control::State::FINISHED; + break; + } + r.sleep(); + } + + if (cancel_ || !plc_controller_ptr_->checkConnected()) + state = amr_control::State::FAILED; + + plc_controller_ptr_->close(); + } + if (plc_controller_ptr_) + plc_controller_ptr_.reset(); + + std::lock_guard lock(this->arm_mutex_); + this->belt_joined_ = false; + } + + void AmrController::loadCallBack() + { + std::lock_guard lock(this->arm_mutex_); + { + ROS_INFO("Receiving call"); + this->belt_joined_ = true; + this->cancel_ = false; + this->cur_belt_state_en_ = amr_control::State::WAITING; + this->belt_thread_ = std::thread(&AmrController::conveyorBeltsReceiving, this, std::ref(this->cur_belt_state_en_)); + } + } + + void AmrController::conveyorBeltsReceiving(amr_control::State &state) + { + state = amr_control::State::INITIALIZING; + std::shared_ptr plc_controller_ptr_; + plc_controller_ptr_ = std::make_shared("192.168.2.5", 502); + plc_controller_ptr_->connect(); + + if (!plc_controller_ptr_->checkConnected()) + { + state = amr_control::State::FAILED; + return; + } + + if (plc_controller_ptr_ && plc_controller_ptr_->checkConnected()) + { + int shipping_regsister = 119, receiving_regsister = 116; + plc_controller_ptr_->resetM(shipping_regsister); + plc_controller_ptr_->setM(receiving_regsister); + ros::Duration(0.1).sleep(); + plc_controller_ptr_->resetM(shipping_regsister); + plc_controller_ptr_->resetM(receiving_regsister); + + ros::Rate r(5); + while (ros::ok() && plc_controller_ptr_->checkConnected() && !this->cancel_) + { + state = amr_control::State::RUNNING; + bool output_belt[2]; + plc_controller_ptr_->mulGetM(124, 125, output_belt); + + bool enable_shipping = output_belt[0]; + bool enable_receiving = output_belt[1]; + if (!enable_shipping && !enable_receiving) + { + state = amr_control::State::FINISHED; + break; + } + r.sleep(); + } + if (this->cancel_ || !plc_controller_ptr_->checkConnected()) + state = amr_control::State::FAILED; + + plc_controller_ptr_->close(); + } + if (plc_controller_ptr_) + plc_controller_ptr_.reset(); + + std::lock_guard lock(this->arm_mutex_); + this->belt_joined_ = false; + } + + void AmrController::controllerDotuff() + { + ros::Rate r(10); + while (ros::ok()) + { + std::shared_ptr plc_controller_ptr_; + plc_controller_ptr_ = std::make_shared("192.168.2.5", 502); + plc_controller_ptr_->connect(); + r.sleep(); + ros::spinOnce(); + + if (plc_controller_ptr_ == nullptr) + continue; + if (!plc_controller_ptr_->checkConnected()) + continue; + + this->amr_safety_ptr_ = std::make_shared(); + this->amr_safety_ptr_->getController(plc_controller_ptr_); + + while (ros::ok() && plc_controller_ptr_ && plc_controller_ptr_->checkConnected()) + { + if (!this->monitor_ptr_) + continue; + nav_2d_msgs::Twist2D velocity; + if (this->monitor_ptr_->getVelocity(velocity)) + { + cmd_vel_mtx.lock(); + this->amr_safety_ptr_->safetyHandle(velocity, cmd_vel_max_, cmd_vel_recommended_); + cmd_vel_mtx.unlock(); + } + + if (move_base_ptr_ != nullptr && + move_base_ptr_->nav_feedback_ != nullptr && + move_base_ptr_->nav_feedback_->is_ready) + { + if (velocity.x <= -0.01) + this->amr_safety_ptr_->writeMutesSafety(true); + else + { + this->amr_safety_ptr_->writeMutesSafety(this->muted_); + } + } + + bool have_goods; + int have_goods_regsister = 142; + if (!OpcUAServerAPI::belt_cancel_) + { + plc_controller_ptr_->getM(have_goods_regsister, have_goods); + OpcUAServerAPI::have_goods_ = have_goods; + } + else + { + plc_controller_ptr_->resetM(have_goods_regsister); + } + + if (vda_5050_client_api_ptr_) + { + amr_control::OperatingMode mode_; + bool operating_mode[4]; + plc_controller_ptr_->mulGetM(14, 17, operating_mode); + if (operating_mode[0]) + mode_ = amr_control::OperatingMode::AUTOMATIC; + else if (operating_mode[2]) + mode_ = amr_control::OperatingMode::SERVICE; + else if (operating_mode[3]) + mode_ = amr_control::OperatingMode::MANUAL; + + switch (mode_) + { + case amr_control::OperatingMode::AUTOMATIC: + vda_5050_client_api_ptr_->mode_ = vda_5050::State::OperatingMode::AUTOMATIC; + break; + case amr_control::OperatingMode::MANUAL: + vda_5050_client_api_ptr_->mode_ = vda_5050::State::OperatingMode::MANUAL; + break; + case amr_control::OperatingMode::SERVICE: + vda_5050_client_api_ptr_->mode_ = vda_5050::State::OperatingMode::SERVICE; + break; + default: + vda_5050_client_api_ptr_->mode_ = vda_5050::State::OperatingMode::SERVICE; // Default + break; + } + } + r.sleep(); + ros::spinOnce(); + } + if (plc_controller_ptr_) + plc_controller_ptr_.reset(); + if (amr_safety_ptr_) + amr_safety_ptr_.reset(); + } + } + + void AmrController::isDetectedMakerCallback(const std_msgs::Bool::ConstPtr &msg) + { + this->muted_ = msg->data; + } + + void AmrController::threadHandle() + { + ros::Rate r(5); + while (ros::ok()) + { + if (this->arm_thread_.joinable()) + { + std::lock_guard lock(this->arm_mutex_); + { + if (!this->arm_joined_) + { + this->arm_thread_.join(); + } + } + } + + if (this->belt_thread_.joinable()) + { + std::lock_guard lock(this->belt_mutex_); + { + if (!this->belt_joined_) + { + this->belt_thread_.join(); + } + } + } + + if (move_base_ptr_ != nullptr && + move_base_ptr_->nav_feedback_ != nullptr && + move_base_ptr_->nav_feedback_->is_ready) + { + nav_2d_msgs::Twist2D velocity; + if (this->monitor_ptr_ && this->monitor_ptr_->getVelocity(velocity)) + { + this->cmd_vel_max_.x = !vda_5050_client_api_ptr_ ? 0.3 : std::max(0.3, std::max(OpcUAServerAPI::cmd_vel_max_.x, vda_5050_client_api_ptr_->cmd_vel_max_.x)); + this->cmd_vel_max_.theta = !vda_5050_client_api_ptr_ ? 0.3 : std::max(0.3, vda_5050_client_api_ptr_->cmd_vel_max_.theta); + + geometry_msgs::Vector3 linear; + geometry_msgs::Vector3 angular; + + cmd_vel_mtx.lock(); + linear.x = this->amr_safety_ptr_ != nullptr ? this->cmd_vel_recommended_.x : this->cmd_vel_max_.x; + angular.z = this->cmd_vel_max_.theta; + cmd_vel_mtx.unlock(); + + this->move_base_ptr_->setTwistLinear(linear); + linear.x *= -1.0; + this->move_base_ptr_->setTwistLinear(linear); + this->move_base_ptr_->setTwistAngular(angular); + } + } + r.sleep(); + ros::spinOnce(); + } + } +} \ No newline at end of file diff --git a/Controllers/Packages/amr_control/src/amr_control_node.cpp b/Controllers/Packages/amr_control/src/amr_control_node.cpp new file mode 100644 index 0000000..2fac6d7 --- /dev/null +++ b/Controllers/Packages/amr_control/src/amr_control_node.cpp @@ -0,0 +1,27 @@ +#include +#include +#include "amr_control/amr_control.h" + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "move_base_node"); + ros::start(); + ROS_INFO("ros::this_node::getName() : %s", ros::this_node::getName().c_str()); + ros::NodeHandle nh(ros::this_node::getName()); + std::shared_ptr buffer = std::make_shared(); + tf2_ros::TransformListener tf2(*buffer); + try + { + std::shared_ptr amr_ctr_; + amr_ctr_ = std::make_shared(nh, buffer); + ros::spin(); + if(amr_ctr_ != nullptr) + amr_ctr_->stop(); + amr_ctr_.reset(); + } + catch (const std::exception& exc) + { + std::cout << exc.what() << std::endl; + } + return 0; +} \ No newline at end of file diff --git a/Controllers/Packages/amr_control/src/amr_make_plan_with_order.cpp b/Controllers/Packages/amr_control/src/amr_make_plan_with_order.cpp new file mode 100644 index 0000000..d1f2aee --- /dev/null +++ b/Controllers/Packages/amr_control/src/amr_make_plan_with_order.cpp @@ -0,0 +1,712 @@ +#include "amr_control/amr_make_plan_with_order.h" + +namespace amr_control +{ + bool MakePlanWithOrder(const vda_5050::Order order, bool is_move_backward, uint8_t &status, std::string &message, std::vector &posesOnPathWay) + { + std::map orderNodes; + Spline_Inf *input_spline_inf = new Spline_Inf(); + CurveCommon *curve_design = new CurveCommon(); + posesOnPathWay.clear(); + std::map map_str; + if ((int)order.nodes.size() == 0 || (int)order.edges.size() == 0) + { + status = 1; + message = "Nodes or Edges in Order is empty"; + return false; + } + for (auto &node : order.nodes) + { + orderNodes.insert({node.nodeId, node}); + } + for (int i = 0; i < (int)order.edges.size(); i++) + { + auto start_nodeId_it = orderNodes.find(order.edges[i].startNodeId); + auto end_nodeId_it = orderNodes.find(order.edges[i].endNodeId); + + if (start_nodeId_it != orderNodes.end() && end_nodeId_it != orderNodes.end()) + { + std::vector posesOnEdge; + std::vector> control_points; + std::vector knot_vector; + std::vector weight_vector; + int degree = 0; + int order_ = 0; + control_points.reserve(order.edges[i].trajectory.controlPoints.size()); + knot_vector.reserve(order.edges[i].trajectory.knotVector.size()); + weight_vector.reserve(order.edges[i].trajectory.controlPoints.size()); + for (int j = 0; j < (int)order.edges[i].trajectory.controlPoints.size(); j++) + { + control_points.push_back(Eigen::Vector3d(order.edges[i].trajectory.controlPoints[j].x, order.edges[i].trajectory.controlPoints[j].y, 0)); + weight_vector.push_back(order.edges[i].trajectory.controlPoints[j].weight); + } + for (int k = 0; k < (int)order.edges[i].trajectory.knotVector.size(); k++) + { + knot_vector.push_back(order.edges[i].trajectory.knotVector[k]); + } + degree = (int)order.edges[i].trajectory.degree; + if (curveIsValid(degree, knot_vector, control_points)) + { + double step = 0.01; + order_ = degree + 1; + input_spline_inf->control_point.clear(); + input_spline_inf->knot_vector.clear(); + input_spline_inf->weight.clear(); + curve_design->ReadSplineInf(input_spline_inf, order_, control_points, knot_vector); + curve_design->ReadSplineInf(input_spline_inf, weight_vector, false); + for (double u_test = 0; u_test <= 1; u_test += step) + { + geometry_msgs::Point curve_point; + curve_point = curve_design->CalculateCurvePoint(input_spline_inf, u_test, true); + if (!std::isnan(curve_point.x) && !std::isnan(curve_point.y)) + posesOnEdge.push_back(vda_5050::Pose(curve_point.x, curve_point.y, 0)); + // ROS_INFO("curve_point: %f, %f at u: %f",curve_point.x, curve_point.y, u_test); + } + if (!isThetaValid(orderNodes[order.edges[i].startNodeId].nodePosition.theta) && !isThetaValid(orderNodes[order.edges[i].endNodeId].nodePosition.theta)) + { + // if startNode of this edge and start element of posesOnEdge are the different point + if (orderNodes[order.edges[i].startNodeId].nodePosition.x != posesOnEdge.front().getX() || + orderNodes[order.edges[i].startNodeId].nodePosition.y != posesOnEdge.front().getY()) + { + posesOnEdge.insert(posesOnEdge.begin(), vda_5050::Pose(orderNodes[order.edges[i].startNodeId].nodePosition.x, orderNodes[order.edges[i].startNodeId].nodePosition.y, 0.0123443210)); + } + // if endNode of this edge and end element of posesOnEdge are the different point + if (orderNodes[order.edges[i].endNodeId].nodePosition.x != posesOnEdge.back().getX() || + orderNodes[order.edges[i].endNodeId].nodePosition.y != posesOnEdge.back().getY()) + { + posesOnEdge.insert(posesOnEdge.end(), vda_5050::Pose(orderNodes[order.edges[i].endNodeId].nodePosition.x, orderNodes[order.edges[i].endNodeId].nodePosition.y, 0.0123443210)); + } + if (!posesOnPathWay.empty()) + { + if (computeDeltaAngleStartNode(posesOnPathWay.back().getYaw(), posesOnEdge.front(), posesOnEdge[1]) <= 0.872664626) + { + setYawAllPosesOnEdge(posesOnEdge, false); + } + else if (computeDeltaAngleStartNode(posesOnPathWay.back().getYaw(), posesOnEdge.front(), posesOnEdge[1]) >= 2.2689280276) + { + setYawAllPosesOnEdge(posesOnEdge, true); + } + else + { + ROS_WARN("Trajectory of Edge: %s, startNode: %s, endNode: %s is not good", + order.edges[i].edgeId.c_str(), order.edges[i].startNodeId.c_str(), order.edges[i].endNodeId.c_str()); + status = 3; + message = "Trajectory of Edge: " + order.edges[i].edgeId + ", startNode: " + order.edges[i].startNodeId.c_str() + + ", endNode: " + order.edges[i].endNodeId.c_str() + " is not good"; + return false; + break; + } + } + else + { + if (is_move_backward == false) + { + setYawAllPosesOnEdge(posesOnEdge, false); + } + else + { + setYawAllPosesOnEdge(posesOnEdge, true); + } + } + if (!posesOnPathWay.empty()) // posesOnPathWay has datas + { + if (posesOnEdge.front().getX() == posesOnPathWay.back().getX() && + posesOnEdge.front().getY() == posesOnPathWay.back().getY()) + { + if (i != 1) + { // don't check angle of edge 1 + if (computeDeltaAngleStartNode(posesOnPathWay.back().getYaw(), posesOnEdge.front().getYaw(), posesOnEdge.front()) <= 0.872664626) // <= 50 degree + { + // if yaw angle of the end pose in posesOnPathWay is default, set it to yaw angle of start pose in posesOnEdge + posesOnPathWay.back().setYaw(posesOnEdge.front().getYaw()); + posesOnPathWay.insert(posesOnPathWay.end(), posesOnEdge.begin() + 1, posesOnEdge.end()); + } + else + { + ROS_WARN("Trajectory of Edge: %s, startNode: %s, endNode: %s is not good", + order.edges[i].edgeId.c_str(), order.edges[i].startNodeId.c_str(), order.edges[i].endNodeId.c_str()); + status = 3; + message = "Trajectory of Edge: " + order.edges[i].edgeId + ", startNode: " + order.edges[i].startNodeId.c_str() + + ", endNode: " + order.edges[i].endNodeId.c_str() + " is not good"; + return false; + break; + } + } + else + { + // if yaw angle of the end pose in posesOnPathWay is default, set it to yaw angle of start pose in posesOnEdge + posesOnPathWay.back().setYaw(posesOnEdge.front().getYaw()); + posesOnPathWay.insert(posesOnPathWay.end(), posesOnEdge.begin() + 1, posesOnEdge.end()); + } + } + else + { + ROS_WARN("Trajectory of Edge: %s. startNode: %s has posision invalid", + order.edges[i].edgeId.c_str(), order.edges[i].startNodeId.c_str()); + status = 3; + message = "Trajectory of Edge: " + order.edges[i].edgeId + ", startNode: " + order.edges[i].startNodeId.c_str() + + ", endNode: " + order.edges[i].endNodeId.c_str() + " is not good"; + return false; + break; + } + } + else // posesOnPathWay is empty + { + posesOnPathWay.insert(posesOnPathWay.end(), posesOnEdge.begin(), posesOnEdge.end()); + } + } + else if (!isThetaValid(orderNodes[order.edges[i].startNodeId].nodePosition.theta) && isThetaValid(orderNodes[order.edges[i].endNodeId].nodePosition.theta)) + { + // if startNode of this edge and start element of posesOnEdge are the different point + if (orderNodes[order.edges[i].startNodeId].nodePosition.x != posesOnEdge.front().getX() || + orderNodes[order.edges[i].startNodeId].nodePosition.y != posesOnEdge.front().getY()) + { + posesOnEdge.insert(posesOnEdge.begin(), vda_5050::Pose(orderNodes[order.edges[i].startNodeId].nodePosition.x, orderNodes[order.edges[i].startNodeId].nodePosition.y, 0.0123443210)); + } + // if endNode of this edge and end element of posesOnEdge are the different point + if (orderNodes[order.edges[i].endNodeId].nodePosition.x != posesOnEdge.back().getX() || + orderNodes[order.edges[i].endNodeId].nodePosition.y != posesOnEdge.back().getY()) + { + posesOnEdge.insert(posesOnEdge.end(), vda_5050::Pose(orderNodes[order.edges[i].endNodeId].nodePosition.x, orderNodes[order.edges[i].endNodeId].nodePosition.y, orderNodes[order.edges[i].endNodeId].nodePosition.theta)); + } + if (i == ((int)order.edges.size() - 1)) + { + if (computeDeltaAngleEndNode(orderNodes[order.edges[i].endNodeId].nodePosition.theta, posesOnEdge.back(), posesOnEdge[posesOnEdge.size() - 2]) <= 1.5707963268) // <= 90 degree + { + setYawAllPosesOnEdge(posesOnEdge, false); + posesOnEdge.back().setYaw(orderNodes[order.edges[i].endNodeId].nodePosition.theta); // set yaw angle of the end pose to endNode theta + } + else + { + setYawAllPosesOnEdge(posesOnEdge, true); + posesOnEdge.back().setYaw(orderNodes[order.edges[i].endNodeId].nodePosition.theta); // set yaw angle of the end pose to endNode theta + } + } + else + { + if (computeDeltaAngleEndNode(orderNodes[order.edges[i].endNodeId].nodePosition.theta, posesOnEdge.back(), posesOnEdge[posesOnEdge.size() - 2]) <= 0.872664626) // <= 50 degree + { + setYawAllPosesOnEdge(posesOnEdge, false); + posesOnEdge.back().setYaw(orderNodes[order.edges[i].endNodeId].nodePosition.theta); // set yaw angle of the end pose to endNode theta + } + else if (computeDeltaAngleEndNode(orderNodes[order.edges[i].endNodeId].nodePosition.theta, posesOnEdge.back(), posesOnEdge[posesOnEdge.size() - 2]) >= 2.2689280276) // >= 130 degree + { + setYawAllPosesOnEdge(posesOnEdge, true); + posesOnEdge.back().setYaw(orderNodes[order.edges[i].endNodeId].nodePosition.theta); // set yaw angle of the end pose to endNode theta + } + else + { + ROS_WARN("Trajectory of Edge: %s, startNode: %s, endNode: %s is not good", + order.edges[i].edgeId.c_str(), order.edges[i].startNodeId.c_str(), order.edges[i].endNodeId.c_str()); + status = 3; + message = "Trajectory of Edge: " + order.edges[i].edgeId + ", startNode: " + order.edges[i].startNodeId.c_str() + + ", endNode: " + order.edges[i].endNodeId.c_str() + " is not good"; + return false; + break; + } + } + if (!posesOnPathWay.empty()) // posesOnPathWay has datas + { + if (posesOnEdge.front().getX() == posesOnPathWay.back().getX() && + posesOnEdge.front().getY() == posesOnPathWay.back().getY()) + { + if (computeDeltaAngleStartNode(posesOnPathWay.back().getYaw(), posesOnEdge.front().getYaw(), posesOnEdge.front()) <= 0.872664626) // <= 50 degree + { + // if yaw angle of the end pose in posesOnPathWay is default, set it to yaw angle of start pose in posesOnEdge + posesOnPathWay.back().setYaw(posesOnEdge.front().getYaw()); + posesOnPathWay.insert(posesOnPathWay.end(), posesOnEdge.begin() + 1, posesOnEdge.end()); + } + else + { + ROS_WARN("Trajectory of Edge: %s, startNode: %s, endNode: %s is not good", + order.edges[i].edgeId.c_str(), order.edges[i].startNodeId.c_str(), order.edges[i].endNodeId.c_str()); + status = 3; + message = "Trajectory of Edge: " + order.edges[i].edgeId + ", startNode: " + order.edges[i].startNodeId.c_str() + + ", endNode: " + order.edges[i].endNodeId.c_str() + " is not good"; + return false; + break; + } + } + else + { + ROS_WARN("Trajectory of Edge: %s. startNode: %s has posision invalid", + order.edges[i].edgeId.c_str(), order.edges[i].startNodeId.c_str()); + status = 3; + message = "Trajectory of Edge: " + order.edges[i].edgeId + ", startNode: " + order.edges[i].startNodeId.c_str() + + ", endNode: " + order.edges[i].endNodeId.c_str() + " is not good"; + return false; + break; + } + } + else // posesOnPathWay is empty + { + posesOnPathWay.insert(posesOnPathWay.end(), posesOnEdge.begin(), posesOnEdge.end()); + } + } + else if (isThetaValid(orderNodes[order.edges[i].startNodeId].nodePosition.theta) && !isThetaValid(orderNodes[order.edges[i].endNodeId].nodePosition.theta)) + { + // if startNode of this edge and start element of posesOnEdge are the different point + if (orderNodes[order.edges[i].startNodeId].nodePosition.x != posesOnEdge.front().getX() || + orderNodes[order.edges[i].startNodeId].nodePosition.y != posesOnEdge.front().getY()) + { + posesOnEdge.insert(posesOnEdge.begin(), vda_5050::Pose(orderNodes[order.edges[i].startNodeId].nodePosition.x, orderNodes[order.edges[i].startNodeId].nodePosition.y, orderNodes[order.edges[i].startNodeId].nodePosition.theta)); + } + // if endNode of this edge and end element of posesOnEdge are the different point + if (orderNodes[order.edges[i].endNodeId].nodePosition.x != posesOnEdge.back().getX() || + orderNodes[order.edges[i].endNodeId].nodePosition.y != posesOnEdge.back().getY()) + { + posesOnEdge.insert(posesOnEdge.end(), vda_5050::Pose(orderNodes[order.edges[i].endNodeId].nodePosition.x, orderNodes[order.edges[i].endNodeId].nodePosition.y, 0.0123443210)); + } + if (computeDeltaAngleStartNode(orderNodes[order.edges[i].startNodeId].nodePosition.theta, posesOnEdge.front(), posesOnEdge[1]) <= 0.872664626) // <= 50 degree) + { + setYawAllPosesOnEdge(posesOnEdge, false); + posesOnEdge.front().setYaw(orderNodes[order.edges[i].startNodeId].nodePosition.theta); // set yaw angle of the start pose to startNode theta + } + else if (computeDeltaAngleStartNode(orderNodes[order.edges[i].startNodeId].nodePosition.theta, posesOnEdge.front(), posesOnEdge[1]) >= 2.2689280276) // >= 130 degree + { + setYawAllPosesOnEdge(posesOnEdge, true); + posesOnEdge.front().setYaw(orderNodes[order.edges[i].startNodeId].nodePosition.theta); // set yaw angle of the start pose to startNode theta + } + else + { + ROS_WARN("Trajectory of Edge: %s, startNode: %s, endNode: %s is not good", + order.edges[i].edgeId.c_str(), order.edges[i].startNodeId.c_str(), order.edges[i].endNodeId.c_str()); + status = 3; + message = "Trajectory of Edge: " + order.edges[i].edgeId + ", startNode: " + order.edges[i].startNodeId.c_str() + + ", endNode: " + order.edges[i].endNodeId.c_str() + " is not good"; + return false; + break; + } + if (!posesOnPathWay.empty()) // posesOnPathWay has datas + { + if (posesOnEdge.front().getX() == posesOnPathWay.back().getX() && + posesOnEdge.front().getY() == posesOnPathWay.back().getY()) + { + if (computeDeltaAngleStartNode(posesOnPathWay.back().getYaw(), posesOnEdge.front().getYaw(), posesOnEdge.front()) <= 0.872664626) // <= 50 degree + { + // if yaw angle of the end pose in posesOnPathWay is default, set it to yaw angle of start pose in posesOnEdge + posesOnPathWay.back().setYaw(posesOnEdge.front().getYaw()); + posesOnPathWay.insert(posesOnPathWay.end(), posesOnEdge.begin() + 1, posesOnEdge.end()); + } + else + { + ROS_WARN("Trajectory of Edge: %s, startNode: %s, endNode: %s is not good", + order.edges[i].edgeId.c_str(), order.edges[i].startNodeId.c_str(), order.edges[i].endNodeId.c_str()); + status = 3; + message = "Trajectory of Edge: " + order.edges[i].edgeId + ", startNode: " + order.edges[i].startNodeId.c_str() + + ", endNode: " + order.edges[i].endNodeId.c_str() + " is not good"; + return false; + break; + } + } + else + { + ROS_WARN("Trajectory of Edge: %s. startNode: %s has posision invalid", + order.edges[i].edgeId.c_str(), order.edges[i].startNodeId.c_str()); + status = 3; + message = "Trajectory of Edge: " + order.edges[i].edgeId + ", startNode: " + order.edges[i].startNodeId.c_str() + + ", endNode: " + order.edges[i].endNodeId.c_str() + " is not good"; + return false; + break; + } + } + else // posesOnPathWay is empty + { + posesOnPathWay.insert(posesOnPathWay.end(), posesOnEdge.begin(), posesOnEdge.end()); + } + } + else + { + // if startNode of this edge and start element of posesOnEdge are the different point + if (orderNodes[order.edges[i].startNodeId].nodePosition.x != posesOnEdge.front().getX() || + orderNodes[order.edges[i].startNodeId].nodePosition.y != posesOnEdge.front().getY()) + { + posesOnEdge.insert(posesOnEdge.begin(), vda_5050::Pose(orderNodes[order.edges[i].startNodeId].nodePosition.x, orderNodes[order.edges[i].startNodeId].nodePosition.y, orderNodes[order.edges[i].startNodeId].nodePosition.theta)); + } + // if endNode of this edge and end element of posesOnEdge are the different point + if (orderNodes[order.edges[i].endNodeId].nodePosition.x != posesOnEdge.back().getX() || + orderNodes[order.edges[i].endNodeId].nodePosition.y != posesOnEdge.back().getY()) + { + posesOnEdge.insert(posesOnEdge.end(), vda_5050::Pose(orderNodes[order.edges[i].endNodeId].nodePosition.x, orderNodes[order.edges[i].endNodeId].nodePosition.y, orderNodes[order.edges[i].endNodeId].nodePosition.theta)); + } + // DeltaAngleStart <= 50 degree and DeltaAngleEnd <= 50 degree + if (computeDeltaAngleStartNode(orderNodes[order.edges[i].startNodeId].nodePosition.theta, posesOnEdge.front(), posesOnEdge[1]) <= 0.872664626 && + computeDeltaAngleEndNode(orderNodes[order.edges[i].endNodeId].nodePosition.theta, posesOnEdge.back(), posesOnEdge[posesOnEdge.size() - 2]) <= 0.872664626) + { + setYawAllPosesOnEdge(posesOnEdge, false); + posesOnEdge.front().setYaw(orderNodes[order.edges[i].startNodeId].nodePosition.theta); // set yaw angle of the start pose to startNode theta + posesOnEdge.back().setYaw(orderNodes[order.edges[i].endNodeId].nodePosition.theta); // set yaw angle of the end pose to endNode theta + } + // DeltaAngleStart >= 130 degree and DeltaAngleEnd >= 130 degree + else if (computeDeltaAngleStartNode(orderNodes[order.edges[i].startNodeId].nodePosition.theta, posesOnEdge.front(), posesOnEdge[1]) >= 2.2689280276 && + computeDeltaAngleEndNode(orderNodes[order.edges[i].endNodeId].nodePosition.theta, posesOnEdge.back(), posesOnEdge[posesOnEdge.size() - 2]) >= 2.2689280276) + { + setYawAllPosesOnEdge(posesOnEdge, true); + posesOnEdge.front().setYaw(orderNodes[order.edges[i].startNodeId].nodePosition.theta); // set yaw angle of the start pose to startNode theta + posesOnEdge.back().setYaw(orderNodes[order.edges[i].endNodeId].nodePosition.theta); // set yaw angle of the end pose to endNode theta + } + if (!posesOnPathWay.empty()) // posesOnPathWay has datas + { + if (posesOnEdge.front().getX() == posesOnPathWay.back().getX() && + posesOnEdge.front().getY() == posesOnPathWay.back().getY()) + { + if (computeDeltaAngleStartNode(posesOnPathWay.back().getYaw(), posesOnEdge.front().getYaw(), posesOnEdge.front()) <= 0.872664626) // <= 50 degree + { + // if yaw angle of the end pose in posesOnPathWay is default, set it to yaw angle of start pose in posesOnEdge + posesOnPathWay.back().setYaw(posesOnEdge.front().getYaw()); + posesOnPathWay.insert(posesOnPathWay.end(), posesOnEdge.begin() + 1, posesOnEdge.end()); + } + else + { + ROS_WARN("Trajectory of Edge: %s, startNode: %s, endNode: %s is not good", + order.edges[i].edgeId.c_str(), order.edges[i].startNodeId.c_str(), order.edges[i].endNodeId.c_str()); + status = 3; + message = "Trajectory of Edge: " + order.edges[i].edgeId + ", startNode: " + order.edges[i].startNodeId.c_str() + + ", endNode: " + order.edges[i].endNodeId.c_str() + " is not good"; + return false; + break; + } + } + else + { + ROS_WARN("Trajectory of Edge: %s. startNode: %s has posision invalid", + order.edges[i].edgeId.c_str(), order.edges[i].startNodeId.c_str()); + status = 3; + message = "Trajectory of Edge: " + order.edges[i].edgeId + ", startNode: " + order.edges[i].startNodeId.c_str() + + ", endNode: " + order.edges[i].endNodeId.c_str() + " is not good"; + return false; + break; + } + } + else // posesOnPathWay is empty + { + posesOnPathWay.insert(posesOnPathWay.end(), posesOnEdge.begin(), posesOnEdge.end()); + } + } + } + else + { + ROS_WARN("Trajectory of Edge: %s, startNodeId: %s, endNodeId: %s is invalid", order.edges[i].edgeId.c_str(), + order.edges[i].startNodeId.c_str(), order.edges[i].endNodeId.c_str()); + status = 2; + message = "Trajectory of Edge: " + order.edges[i].edgeId + ", startNode: " + order.edges[i].startNodeId.c_str() + + ", endNode: " + order.edges[i].endNodeId.c_str() + " is invalid NURBS-curve"; + return false; + break; + } + } + else + { + ROS_WARN("Edge: %s not found startNodeId: %s or endNodeId: %s", order.edges[i].edgeId.c_str(), + order.edges[i].startNodeId.c_str(), order.edges[i].endNodeId.c_str()); + status = 1; + message = "Edge: " + order.edges[i].edgeId + " not found startNodeId: " + order.edges[i].startNodeId.c_str() + + " or endNodeId: " + order.edges[i].endNodeId.c_str(); + return false; + break; + } + // ROS_INFO("Finish to compute at Edge[%d]: %s", i, order.edges[i].edgeDescription.c_str()); + } + + status = 0; + message = "Success to make plan: StartNode: " + order.edges[0].startNodeId + ", EndNode: " + order.edges[order.edges.size() - 1].endNodeId; + + if (input_spline_inf) + delete (input_spline_inf); + if (curve_design) + delete (curve_design); + return true; + } + + bool isThetaValid(double theta) + { + bool result = false; + if (theta < -M_PI || theta > M_PI) + result = false; + else + result = true; + return result; + } + + bool curveIsValid(int degree, const std::vector &knot_vector, + std::vector> &control_points) + { + if (degree < 1 || degree > 9) + { + ROS_WARN("degree is invalid value"); + return false; + } + if (!((knot_vector.size() - degree - 1) == control_points.size())) + { + ROS_WARN("relation between degree, number of knots, and number of control points is invalid"); + return false; + } + // if(std::is_sorted(knot_vector.begin(), knot_vector.end())) + // { + // ROS_WARN("knot vector is not monotonic"); + // return false; + // } + return true; + } + + double computeDeltaAngleStartNode(double theta, vda_5050::Pose &startPose, vda_5050::Pose &next_Pose) + { + double delta_angle = 0; + if (isThetaValid(theta)) + { + double xAB = next_Pose.getX() - startPose.getX(); + double yAB = next_Pose.getY() - startPose.getY(); + double d = sqrt(xAB * xAB + yAB * yAB); + double xC = startPose.getX() + d * cos(theta); + double yC = startPose.getY() + d * sin(theta); + double xAC = xC - startPose.getX(); + double yAC = yC - startPose.getY(); + double dAB = sqrt(xAB * xAB + yAB * yAB); + double cos_a = (xAB * xAC + yAB * yAC) / (dAB * d); + if (cos_a > 1) + cos_a = 1; + else if (cos_a < (-1)) + cos_a = -1; + delta_angle = acos(cos_a); + // delta_angle = delta_angle*180/M_PI; + // ROS_WARN("xC: %f, yC: %f", xC, yC); + // ROS_WARN("dAB: %f", dAB); + // ROS_WARN("delta_angle: %f", delta_angle); + } + return delta_angle; + } + + double computeDeltaAngleEndNode(double theta, vda_5050::Pose &endPose, vda_5050::Pose &prev_Pose) + { + double delta_angle = 0; + if (isThetaValid(theta)) + { + double xAB = endPose.getX() - prev_Pose.getX(); + double yAB = endPose.getY() - prev_Pose.getY(); + double d = sqrt(xAB * xAB + yAB * yAB); + double xC = endPose.getX() + d * cos(theta); + double yC = endPose.getY() + d * sin(theta); + double xBC = xC - endPose.getX(); + double yBC = yC - endPose.getY(); + double dAB = sqrt(xAB * xAB + yAB * yAB); + double cos_a = (xAB * xBC + yAB * yBC) / (dAB * d); + if (cos_a > 1) + cos_a = 1; + else if (cos_a < (-1)) + cos_a = -1; + delta_angle = acos(cos_a); + // delta_angle = delta_angle*180/M_PI; + // ROS_WARN("xC: %f, yC: %f", xC, yC); + // ROS_WARN("dAB: %f", dAB); + // ROS_WARN("delta_angle: %f", delta_angle); + } + return delta_angle; + } + + double computeDeltaAngleEndOfPlan(double theta, geometry_msgs::Pose &endPose, geometry_msgs::Pose &prev_Pose) + { + double delta_angle = 0; + if (isThetaValid(theta)) + { + double xAB = endPose.position.x - prev_Pose.position.x; + double yAB = endPose.position.y - prev_Pose.position.y; + double d = sqrt(xAB * xAB + yAB * yAB); + double xC = endPose.position.x + d * cos(theta); + double yC = endPose.position.y + d * sin(theta); + double xBC = xC - endPose.position.x; + double yBC = yC - endPose.position.y; + double dAB = sqrt(xAB * xAB + yAB * yAB); + double cos_a = (xAB * xBC + yAB * yBC) / (dAB * d); + if (cos_a > 1) + cos_a = 1; + else if (cos_a < (-1)) + cos_a = -1; + delta_angle = acos(cos_a); + // delta_angle = delta_angle*180/M_PI; + // ROS_WARN("xC: %f, yC: %f", xC, yC); + // ROS_WARN("dAB: %f", dAB); + // ROS_WARN("delta_angle: %f", delta_angle); + } + return delta_angle; + } + + void setYawAllPosesOnEdge(std::vector &posesOnEdge, bool reverse) + { + if (!reverse) + { + if (!posesOnEdge.empty()) + { + if (posesOnEdge.size() > 2) + { + for (int i = 0; i < ((int)posesOnEdge.size() - 1); i++) + { + double theta = calculateAngle(posesOnEdge[i].getX(), posesOnEdge[i].getY(), + posesOnEdge[i + 1].getX(), posesOnEdge[i + 1].getY()); + posesOnEdge[i].setYaw(theta); + } + posesOnEdge.back().setYaw(posesOnEdge[posesOnEdge.size() - 2].getYaw()); + } + else if (posesOnEdge.size() == 2) + { + if (posesOnEdge[0].getX() != posesOnEdge[1].getX()) + { + double theta = calculateAngle(posesOnEdge[0].getX(), posesOnEdge[0].getY(), + posesOnEdge[1].getX(), posesOnEdge[1].getY()); + posesOnEdge[0].setYaw(theta); + posesOnEdge[1].setYaw(theta); + } + } + } + } + else + { + if (!posesOnEdge.empty()) + { + if (posesOnEdge.size() > 2) + { + for (int i = (int)posesOnEdge.size() - 1; i > 0; i--) + { + double theta = calculateAngle(posesOnEdge[i].getX(), posesOnEdge[i].getY(), + posesOnEdge[i - 1].getX(), posesOnEdge[i - 1].getY()); + posesOnEdge[i].setYaw(theta); + } + posesOnEdge.front().setYaw(posesOnEdge[1].getYaw()); + } + else if (posesOnEdge.size() == 2) + { + if (posesOnEdge[1].getX() != posesOnEdge[0].getX()) + { + double theta = calculateAngle(posesOnEdge[1].getX(), posesOnEdge[1].getY(), + posesOnEdge[0].getX(), posesOnEdge[0].getY()); + posesOnEdge[1].setYaw(theta); + posesOnEdge[0].setYaw(theta); + } + } + } + } + } + + double computeDeltaAngleStartNode(double thetaEnd, double thetaStart, vda_5050::Pose &Pose) + { + double delta_angle = 0; + if (isThetaValid(thetaEnd) && isThetaValid(thetaStart)) + { + double d = 1; + double xA = Pose.getX(); + double yA = Pose.getY(); + double xB = xA + d * cos(thetaEnd); + double yB = yA + d * sin(thetaEnd); + double xAB = xB - xA; + double yAB = yB - yA; + double xC = xA + d * cos(thetaStart); + double yC = yA + d * sin(thetaStart); + double xAC = xC - xA; + double yAC = yC - yA; + double cos_a = (xAB * xAC + yAB * yAC) / (d * d); + if (cos_a > 1) + cos_a = 1; + else if (cos_a < (-1)) + cos_a = -1; + delta_angle = acos(cos_a); + // delta_angle = delta_angle*180/M_PI; + // ROS_WARN("delta_angle: %f", delta_angle); + } + return delta_angle; + } + + double computeDeltaAngleStartOfPlan(double theta, geometry_msgs::Pose &startPose, geometry_msgs::Pose &next_Pose) + { + double delta_angle = 0; + if (isThetaValid(theta)) + { + double xAB = next_Pose.position.x - startPose.position.x; + double yAB = next_Pose.position.y - startPose.position.y; + double d = sqrt(xAB * xAB + yAB * yAB); + double xC = startPose.position.x + d * cos(theta); + double yC = startPose.position.y + d * sin(theta); + double xAC = xC - startPose.position.x; + double yAC = yC - startPose.position.y; + double dAB = sqrt(xAB * xAB + yAB * yAB); + double cos_a = (xAB * xAC + yAB * yAC) / (dAB * d); + if (cos_a > 1) + cos_a = 1; + else if (cos_a < (-1)) + cos_a = -1; + delta_angle = acos(cos_a); + // delta_angle = delta_angle*180/M_PI; + // ROS_WARN("xC: %f, yC: %f", xC, yC); + // ROS_WARN("dAB: %f", dAB); + // ROS_WARN("delta_angle: %f", delta_angle); + } + return delta_angle; + } + + double computeDeltaAngleStartOfPlan(double thetaEnd, double thetaStart, geometry_msgs::Pose &Pose) + { + double delta_angle = 0; + if (isThetaValid(thetaEnd) && isThetaValid(thetaStart)) + { + double d = 1; + double xA = Pose.position.x; + double yA = Pose.position.y; + double xB = xA + d * cos(thetaEnd); + double yB = yA + d * sin(thetaEnd); + double xAB = xB - xA; + double yAB = yB - yA; + double xC = xA + d * cos(thetaStart); + double yC = yA + d * sin(thetaStart); + double xAC = xC - xA; + double yAC = yC - yA; + double cos_a = (xAB * xAC + yAB * yAC) / (d * d); + if (cos_a > 1) + cos_a = 1; + else if (cos_a < (-1)) + cos_a = -1; + delta_angle = acos(cos_a); + // delta_angle = delta_angle*180/M_PI; + // ROS_WARN("delta_angle: %f", delta_angle); + } + return delta_angle; + } + + double calculateDistance(vda_5050::Pose &pose1, vda_5050::Pose &pose2) + { + double dx = pose1.getX() - pose2.getX(); + double dy = pose1.getY() - pose2.getY(); + return std::sqrt(dx * dx + dy * dy); + } + + double computeDeltaAngle(vda_5050::Pose &Pose1, vda_5050::Pose &Pose2) + { + double delta_angle = 0; + + double xA = Pose1.getX(); + double yA = Pose1.getY(); + double xB = Pose2.getX(); + double yB = Pose2.getY(); + double xAB = xB - xA; + double yAB = yB - yA; + double d = sqrt(xAB * xAB + yAB * yAB); + double xC = xB + d * cos(Pose2.getYaw()); + double yC = yB + d * sin(Pose2.getYaw()); + double xBC = xC - xB; + double yBC = yC - yB; + double cos_a = (xAB * xBC + yAB * yBC) / (d * d); + if (cos_a > 1) + cos_a = 1; + else if (cos_a < (-1)) + cos_a = -1; + double delta_angle_tmp = acos(cos_a); + if (delta_angle_tmp >= 1.5707963268) + { + delta_angle = M_PI - delta_angle_tmp; + } + else + { + delta_angle = delta_angle_tmp; + } + // delta_angle = delta_angle*180/M_PI; + // ROS_WARN("delta_angle: %f", delta_angle); + return delta_angle; + } +} \ No newline at end of file diff --git a/Controllers/Packages/amr_control/src/amr_monitor.cpp b/Controllers/Packages/amr_control/src/amr_monitor.cpp new file mode 100644 index 0000000..f46e5d8 --- /dev/null +++ b/Controllers/Packages/amr_control/src/amr_monitor.cpp @@ -0,0 +1,37 @@ +#include "amr_control/amr_monitor.h" + +namespace amr_control +{ + AmrMonitor::AmrMonitor() {} + + AmrMonitor::AmrMonitor(const ros::NodeHandle &nh) + { + if (!initalized_) + this->init(nh); + } + + AmrMonitor::~AmrMonitor() + { + if (odom_sub_) + odom_sub_.reset(); + } + + void AmrMonitor::init(const ros::NodeHandle &nh) + { + if (!initalized_) + { + ros::NodeHandle nh_core; + odom_sub_ = std::make_shared(nh_core); + initalized_ = true; + } + } + + bool AmrMonitor::getVelocity(nav_2d_msgs::Twist2D &velocity) + { + if (!odom_sub_) + return false; + else + velocity = odom_sub_->getTwist(); + return true; + } +} \ No newline at end of file diff --git a/Controllers/Packages/amr_control/src/amr_opc_ua_server_api.cpp b/Controllers/Packages/amr_control/src/amr_opc_ua_server_api.cpp new file mode 100644 index 0000000..4eaaf54 --- /dev/null +++ b/Controllers/Packages/amr_control/src/amr_opc_ua_server_api.cpp @@ -0,0 +1,3491 @@ + +#include +#include +#include +#include +#include +#include +#include +#include "amr_control/amr_opc_ua_server_api.h" +#include "nova5_control/imr_nova_control.h" + +#define STATUS_SUCCESSED 1 +#define STATUS_ERROR -1 + +amr_control::OpcUAServerAPI::OpcUAServerAPI(const ros::NodeHandle &nh, std::shared_ptr move_base, + std::shared_ptr loc_base, std::shared_ptr monitor) +{ + this->nh_ = nh; + amr_control::OpcUAServerAPI::monitor_ptr_ = monitor; + amr_control::OpcUAServerAPI::move_base_ptr_ = move_base; + amr_control::OpcUAServerAPI::loc_base_ptr_ = loc_base; + + ros::NodeHandle nh_node; + amr_control::OpcUAServerAPI::init_pub_ = nh_node.advertise("/initialpose", 1); +} + +amr_control::OpcUAServerAPI::~OpcUAServerAPI() +{ + pthread_join(amr_control::OpcUAServerAPI::hThread_, NULL); + if (amr_control::OpcUAServerAPI::arm_thread_ptr_) + { + delete (amr_control::OpcUAServerAPI::arm_thread_ptr_); + } + if (amr_control::OpcUAServerAPI::belt_thread_ptr_) + { + delete (amr_control::OpcUAServerAPI::belt_thread_ptr_); + } + if (amr_control::OpcUAServerAPI::server_ptr_) + { + amr_control::OpcUAServerAPI::server_ptr_->stop(); + server_ptr_.reset(); + } +} + +void amr_control::OpcUAServerAPI::start() +{ + amr_control::OpcUAServerAPI::server_ptr_ = std::make_shared(); + server_ptr_->init(this->nh_); + this->defineObjects(); + server_ptr_->start(); +} + +void amr_control::OpcUAServerAPI::defineObjects() +{ + do + { + sigset_t set; + sigemptyset(&set); + sigaddset(&set, SIGINT); + pthread_sigmask(SIG_BLOCK, &set, NULL); + pthread_create(&amr_control::OpcUAServerAPI::hThread_, NULL, &amr_control::OpcUAServerAPI::ThreadWorker, &set); + } while (0); + + UA_NodeId slam_Id; /* get the nodeid assigned by the server */ + UA_ObjectAttributes slamAttr = UA_ObjectAttributes_default; + slamAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"SLAM"); + UA_Server_addObjectNode(server_ptr_->getServerObject(), UA_NODEID_NULL, + UA_NODEID_NUMERIC(0, UA_NS0ID_OBJECTSFOLDER), + UA_NODEID_NUMERIC(0, UA_NS0ID_ORGANIZES), + UA_QUALIFIEDNAME(1, (char *)"SLAM"), + UA_NODEID_NUMERIC(0, UA_NS0ID_BASEOBJECTTYPE), + slamAttr, NULL, &slam_Id); + + // amr_control::OpcUAServerAPI::addStartMappingMethod(this->server_ptr_->getServerObject(), slam_Id); + // amr_control::OpcUAServerAPI::addStopMappingMethod(this->server_ptr_->getServerObject(), slam_Id); + // amr_control::OpcUAServerAPI::addStartLocalizationMethod(this->server_ptr_->getServerObject(), slam_Id); + // amr_control::OpcUAServerAPI::addStopLocalizationMethod(this->server_ptr_->getServerObject(), slam_Id); + amr_control::OpcUAServerAPI::addListMapFilesMethod(this->server_ptr_->getServerObject(), slam_Id); + amr_control::OpcUAServerAPI::addActivateMapMethod(this->server_ptr_->getServerObject(), slam_Id); + amr_control::OpcUAServerAPI::addRobotPoseProperty(this->server_ptr_->getServerObject(), slam_Id); + amr_control::OpcUAServerAPI::addSetInitialPoseMethod(this->server_ptr_->getServerObject(), slam_Id); + + UA_NodeId nav_Id; /* get the nodeid assigned by the server */ + UA_ObjectAttributes navAttr = UA_ObjectAttributes_default; + navAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Navigation"); + UA_Server_addObjectNode(server_ptr_->getServerObject(), UA_NODEID_NULL, + UA_NODEID_NUMERIC(0, UA_NS0ID_OBJECTSFOLDER), + UA_NODEID_NUMERIC(0, UA_NS0ID_ORGANIZES), + UA_QUALIFIEDNAME(1, (char *)"Navigation"), + UA_NODEID_NUMERIC(0, UA_NS0ID_BASEOBJECTTYPE), + navAttr, NULL, &nav_Id); + + amr_control::OpcUAServerAPI::addMoveToNodeMethod(this->server_ptr_->getServerObject(), nav_Id); + amr_control::OpcUAServerAPI::addDockToNodeMethod(this->server_ptr_->getServerObject(), nav_Id); + amr_control::OpcUAServerAPI::addRotateToMethod(this->server_ptr_->getServerObject(), nav_Id); + amr_control::OpcUAServerAPI::addMoveStraightMethod(this->server_ptr_->getServerObject(), nav_Id); + amr_control::OpcUAServerAPI::addCancelMethod(this->server_ptr_->getServerObject(), nav_Id); + amr_control::OpcUAServerAPI::addPauseMethod(this->server_ptr_->getServerObject(), nav_Id); + amr_control::OpcUAServerAPI::addResumeMethod(this->server_ptr_->getServerObject(), nav_Id); + amr_control::OpcUAServerAPI::addNavigationState(this->server_ptr_->getServerObject(), nav_Id); + amr_control::OpcUAServerAPI::addResetMethod(this->server_ptr_->getServerObject(), nav_Id); + amr_control::OpcUAServerAPI::addWriteMutedMethod(this->server_ptr_->getServerObject(), nav_Id); + + UA_NodeId monitor_ptr_Id; /* get the nodeid assigned by the server */ + UA_ObjectAttributes monitorAttr = UA_ObjectAttributes_default; + monitorAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Monitoring"); + UA_Server_addObjectNode(server_ptr_->getServerObject(), UA_NODEID_NULL, + UA_NODEID_NUMERIC(0, UA_NS0ID_OBJECTSFOLDER), + UA_NODEID_NUMERIC(0, UA_NS0ID_ORGANIZES), + UA_QUALIFIEDNAME(1, (char *)"Monitoring"), + UA_NODEID_NUMERIC(0, UA_NS0ID_BASEOBJECTTYPE), + monitorAttr, NULL, &monitor_ptr_Id); + + amr_control::OpcUAServerAPI::addVelocityCommand(this->server_ptr_->getServerObject(), monitor_ptr_Id); + + UA_NodeId amr_plus_Id; /* get the nodeid assigned by the server */ + UA_ObjectAttributes amr_plusAttr = UA_ObjectAttributes_default; + amr_plusAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"IMR+"); + UA_Server_addObjectNode(server_ptr_->getServerObject(), UA_NODEID_NULL, + UA_NODEID_NUMERIC(0, UA_NS0ID_OBJECTSFOLDER), + UA_NODEID_NUMERIC(0, UA_NS0ID_ORGANIZES), + UA_QUALIFIEDNAME(1, (char *)"IMR+"), + UA_NODEID_NUMERIC(0, UA_NS0ID_BASEOBJECTTYPE), + amr_plusAttr, NULL, &amr_plus_Id); + + UA_NodeId dobot_Id; /* get the nodeid assigned by the server */ + UA_ObjectAttributes dobotAttr = UA_ObjectAttributes_default; + dobotAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Dobot"); + UA_Server_addObjectNode(server_ptr_->getServerObject(), UA_NODEID_NULL, amr_plus_Id, + UA_NODEID_NUMERIC(0, UA_NS0ID_ORGANIZES), + UA_QUALIFIEDNAME(1, (char *)"Dobot"), + UA_NODEID_NUMERIC(0, UA_NS0ID_BASEOBJECTTYPE), + dobotAttr, NULL, &dobot_Id); + + amr_control::OpcUAServerAPI::addPickUpDobotMethod(this->server_ptr_->getServerObject(), dobot_Id); + amr_control::OpcUAServerAPI::addPowerOnDobotMethod(this->server_ptr_->getServerObject(), dobot_Id); + amr_control::OpcUAServerAPI::addContinueDobotMethod(this->server_ptr_->getServerObject(), dobot_Id); + amr_control::OpcUAServerAPI::addGoHomeDobotMethod(this->server_ptr_->getServerObject(), dobot_Id); + amr_control::OpcUAServerAPI::addCancelDobotMethod(this->server_ptr_->getServerObject(), dobot_Id); + amr_control::OpcUAServerAPI::addCountMethod(this->server_ptr_->getServerObject(), dobot_Id); + amr_control::OpcUAServerAPI::addDobotProperties(this->server_ptr_->getServerObject(), dobot_Id); + + UA_NodeId conveyor_belt_Id; /* get the nodeid assigned by the server */ + UA_ObjectAttributes conveyor_beltAttr = UA_ObjectAttributes_default; + conveyor_beltAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Conveyor Belts"); + UA_Server_addObjectNode(server_ptr_->getServerObject(), UA_NODEID_NULL, amr_plus_Id, + UA_NODEID_NUMERIC(0, UA_NS0ID_ORGANIZES), + UA_QUALIFIEDNAME(1, (char *)"Conveyor Belts"), + UA_NODEID_NUMERIC(0, UA_NS0ID_BASEOBJECTTYPE), + conveyor_beltAttr, NULL, &conveyor_belt_Id); + + amr_control::OpcUAServerAPI::addunLoadMethod(this->server_ptr_->getServerObject(), conveyor_belt_Id); + amr_control::OpcUAServerAPI::addLoadMethod(this->server_ptr_->getServerObject(), conveyor_belt_Id); + amr_control::OpcUAServerAPI::addResetConveyorBeltMethod(this->server_ptr_->getServerObject(), conveyor_belt_Id); + amr_control::OpcUAServerAPI::addConveyorBeltState(this->server_ptr_->getServerObject(), conveyor_belt_Id); +} + +void amr_control::OpcUAServerAPI::addStartMappingMethod(UA_Server *server, UA_NodeId parentID) +{ + /* One output argument */ + UA_Argument outputArguments[2]; + UA_Argument_init(&outputArguments[0]); + outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status"); + outputArguments[0].name = UA_STRING((char *)"Status"); + outputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId; + outputArguments[0].valueRank = UA_VALUERANK_SCALAR; + + UA_Argument_init(&outputArguments[1]); + outputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Message"); + outputArguments[1].name = UA_STRING((char *)"Message"); + outputArguments[1].dataType = UA_TYPES[UA_TYPES_STRING].typeId; + outputArguments[1].valueRank = UA_VALUERANK_SCALAR; + + /* Add the method node */ + UA_MethodAttributes incAttr = UA_MethodAttributes_default; + incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", + (char *)"Bắt đầu quá trình tạo bản đồ "); + incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"StartMapping"); + incAttr.executable = true; + incAttr.userExecutable = true; + UA_Server_addMethodNode(server, + UA_NODEID_STRING(1, (char *)"StartMapping"), + parentID, + UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), + UA_QUALIFIEDNAME(1, (char *)"StartMapping"), + incAttr, &amr_control::OpcUAServerAPI::startMappingCallBack, + 0, NULL, 2, outputArguments, NULL, NULL); +} + +UA_StatusCode amr_control::OpcUAServerAPI::startMappingCallBack(UA_Server *server, + const UA_NodeId *sessionId, void *sessionContext, + const UA_NodeId *methodId, void *methodContext, + const UA_NodeId *objectId, void *objectContext, + size_t inputSize, const UA_Variant *input, + size_t outputSize, UA_Variant *output) +{ + + UA_Variant_init(output); + UA_Int32 status = STATUS_SUCCESSED; + UA_String message = UA_STRING_ALLOC((char *)"Lệnh bắt đầu tạo bản đồ gọi thành công"); + + std::thread([=]() + { + if (amr_control::OpcUAServerAPI::loc_base_ptr_) + amr_control::OpcUAServerAPI::loc_base_ptr_->startMapping(); }) + .detach(); + + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh bắt đầu tạo bản đồ gọi thành công"); + return UA_STATUSCODE_GOOD; + +STATUSCODE_BADINTERNALERROR: + status = STATUS_ERROR; + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + return UA_STATUSCODE_BADINTERNALERROR; + +STATUSCODE_BADREQUESTTOOLARGE: + status = STATUS_ERROR; + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + return UA_STATUSCODE_BADREQUESTTOOLARGE; + +STATUSCODE_BADSTRUCTUREMISSING: + status = STATUS_ERROR; + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + return UA_STATUSCODE_BADSTRUCTUREMISSING; + +STATUSCODE_BAD: + status = STATUS_ERROR; + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh bắt đầu tạo bản đồ bị lỗi"); + return UA_STATUSCODE_BAD; +} + +void amr_control::OpcUAServerAPI::addStopMappingMethod(UA_Server *server, UA_NodeId parentID) +{ + /* One output argument */ + UA_Argument outputArguments[2]; + UA_Argument_init(&outputArguments[0]); + outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status"); + outputArguments[0].name = UA_STRING((char *)"Status"); + outputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId; + outputArguments[0].valueRank = UA_VALUERANK_SCALAR; + + UA_Argument_init(&outputArguments[1]); + outputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Message"); + outputArguments[1].name = UA_STRING((char *)"Message"); + outputArguments[1].dataType = UA_TYPES[UA_TYPES_STRING].typeId; + outputArguments[1].valueRank = UA_VALUERANK_SCALAR; + + /* Add the method node */ + UA_MethodAttributes incAttr = UA_MethodAttributes_default; + incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", + (char *)"Kết thúc quá trình tạo bản đồ "); + incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"StopMapping"); + incAttr.executable = true; + incAttr.userExecutable = true; + UA_Server_addMethodNode(server, + UA_NODEID_STRING(1, (char *)"StopMapping"), + parentID, + UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), + UA_QUALIFIEDNAME(1, (char *)"StopMapping"), + incAttr, &amr_control::OpcUAServerAPI::stopMappingCallBack, + 0, NULL, 2, outputArguments, NULL, NULL); +} + +UA_StatusCode amr_control::OpcUAServerAPI::stopMappingCallBack(UA_Server *server, + const UA_NodeId *sessionId, void *sessionContext, + const UA_NodeId *methodId, void *methodContext, + const UA_NodeId *objectId, void *objectContext, + size_t inputSize, const UA_Variant *input, + size_t outputSize, UA_Variant *output) +{ + UA_Variant_init(output); + UA_Int32 status = STATUS_SUCCESSED; + UA_String message = UA_STRING_ALLOC((char *)"Lệnh kết thúc quá trình tạo bản đồ gọi thành công"); + + std::thread([=]() + { + if (amr_control::OpcUAServerAPI::loc_base_ptr_) + amr_control::OpcUAServerAPI::loc_base_ptr_->stopMapping(); }) + .detach(); + + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh kết thúc quá trình tạo bản đồ gọi thành công"); + return UA_STATUSCODE_GOOD; + +STATUSCODE_BADINTERNALERROR: + status = STATUS_ERROR; + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + return UA_STATUSCODE_BADINTERNALERROR; + +STATUSCODE_BADREQUESTTOOLARGE: + status = STATUS_ERROR; + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + return UA_STATUSCODE_BADREQUESTTOOLARGE; + +STATUSCODE_BADSTRUCTUREMISSING: + status = STATUS_ERROR; + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + return UA_STATUSCODE_BADSTRUCTUREMISSING; + +STATUSCODE_BAD: + status = STATUS_ERROR; + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh kết thúc quá trình tạo bản đồ bị lỗi"); + return UA_STATUSCODE_BAD; +} + +void amr_control::OpcUAServerAPI::addStartLocalizationMethod(UA_Server *server, UA_NodeId parentID) +{ + /* One output argument */ + UA_Argument outputArguments[2]; + UA_Argument_init(&outputArguments[0]); + outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status"); + outputArguments[0].name = UA_STRING((char *)"Status"); + outputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId; + outputArguments[0].valueRank = UA_VALUERANK_SCALAR; + + UA_Argument_init(&outputArguments[1]); + outputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Message"); + outputArguments[1].name = UA_STRING((char *)"Message"); + outputArguments[1].dataType = UA_TYPES[UA_TYPES_STRING].typeId; + outputArguments[1].valueRank = UA_VALUERANK_SCALAR; + + /* Add the method node */ + UA_MethodAttributes incAttr = UA_MethodAttributes_default; + incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", + (char *)"Bắt đầu quá trình định vị bản đồ "); + incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"StartLocalization"); + incAttr.executable = true; + incAttr.userExecutable = true; + UA_Server_addMethodNode(server, + UA_NODEID_STRING(1, (char *)"StartLocalization"), + parentID, + UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), + UA_QUALIFIEDNAME(1, (char *)"StartLocalization"), + incAttr, &amr_control::OpcUAServerAPI::startLocalizationCallBack, + 0, NULL, 2, outputArguments, NULL, NULL); +} + +UA_StatusCode amr_control::OpcUAServerAPI::startLocalizationCallBack(UA_Server *server, + const UA_NodeId *sessionId, void *sessionContext, + const UA_NodeId *methodId, void *methodContext, + const UA_NodeId *objectId, void *objectContext, + size_t inputSize, const UA_Variant *input, + size_t outputSize, UA_Variant *output) +{ + UA_Variant_init(output); + UA_Int32 status = STATUS_SUCCESSED; + UA_String message = UA_STRING_ALLOC((char *)"Lệnh bắt đầu định vị bản đồ gọi thành công"); + + if (amr_control::OpcUAServerAPI::loc_base_ptr_) + amr_control::OpcUAServerAPI::loc_base_ptr_->startLocalization(); + else + { + message = UA_STRING_ALLOC((char *)"Lệnh bắt đầu định vị bản đồ bị lỗi"); + } + + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh bắt đầu định vị bản đồ gọi thành công"); + return UA_STATUSCODE_GOOD; + +STATUSCODE_BADINTERNALERROR: + status = STATUS_ERROR; + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + return UA_STATUSCODE_BADINTERNALERROR; + +STATUSCODE_BADREQUESTTOOLARGE: + status = STATUS_ERROR; + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + return UA_STATUSCODE_BADREQUESTTOOLARGE; + +STATUSCODE_BADSTRUCTUREMISSING: + status = STATUS_ERROR; + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + return UA_STATUSCODE_BADSTRUCTUREMISSING; + +STATUSCODE_BAD: + status = STATUS_ERROR; + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh bắt đầu định vị bản đồ bị lỗi"); + return UA_STATUSCODE_BAD; +} + +void amr_control::OpcUAServerAPI::addStopLocalizationMethod(UA_Server *server, UA_NodeId parentID) +{ + /* One output argument */ + UA_Argument outputArguments[2]; + UA_Argument_init(&outputArguments[0]); + outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status"); + outputArguments[0].name = UA_STRING((char *)"Status"); + outputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId; + outputArguments[0].valueRank = UA_VALUERANK_SCALAR; + + UA_Argument_init(&outputArguments[1]); + outputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Message"); + outputArguments[1].name = UA_STRING((char *)"Message"); + outputArguments[1].dataType = UA_TYPES[UA_TYPES_STRING].typeId; + outputArguments[1].valueRank = UA_VALUERANK_SCALAR; + + /* Add the method node */ + UA_MethodAttributes incAttr = UA_MethodAttributes_default; + incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", + (char *)"Kết thúc quá trình định vị bản đồ "); + incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"StopLocalization"); + incAttr.executable = true; + incAttr.userExecutable = true; + UA_Server_addMethodNode(server, + UA_NODEID_STRING(1, (char *)"StopLocalization"), + parentID, + UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), + UA_QUALIFIEDNAME(1, (char *)"StopLocalization"), + incAttr, &amr_control::OpcUAServerAPI::stopLocalizationCallBack, + 0, NULL, 2, outputArguments, NULL, NULL); +} + +UA_StatusCode amr_control::OpcUAServerAPI::stopLocalizationCallBack(UA_Server *server, + const UA_NodeId *sessionId, void *sessionContext, + const UA_NodeId *methodId, void *methodContext, + const UA_NodeId *objectId, void *objectContext, + size_t inputSize, const UA_Variant *input, + size_t outputSize, UA_Variant *output) +{ + + UA_Variant_init(output); + UA_Int32 status = STATUS_SUCCESSED; + UA_String message = UA_STRING_ALLOC((char *)"Lệnh kết thúc quá trình định vị bản đồ gọi thành công"); + + if (amr_control::OpcUAServerAPI::loc_base_ptr_) + amr_control::OpcUAServerAPI::loc_base_ptr_->stopLocalization(); + else + { + message = UA_STRING_ALLOC((char *)"Lệnh kết thúc quá trình định vị bản đồ bị lỗi"); + } + + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh kết thúc quá trình định vị bản đồ gọi thành công"); + return UA_STATUSCODE_GOOD; + +STATUSCODE_BADINTERNALERROR: + status = STATUS_ERROR; + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + return UA_STATUSCODE_BADINTERNALERROR; + +STATUSCODE_BADREQUESTTOOLARGE: + status = STATUS_ERROR; + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + return UA_STATUSCODE_BADREQUESTTOOLARGE; + +STATUSCODE_BADSTRUCTUREMISSING: + status = STATUS_ERROR; + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + return UA_STATUSCODE_BADSTRUCTUREMISSING; + +STATUSCODE_BAD: + status = STATUS_ERROR; + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh kết thúc quá trình định vị bản đồ bị lỗi"); + return UA_STATUSCODE_BAD; +} + +void amr_control::OpcUAServerAPI::addListMapFilesMethod(UA_Server *server, UA_NodeId parentID) +{ + /* One output argument */ + UA_Argument outputArguments[1]; + UA_Argument_init(&outputArguments[0]); + outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"List files"); + outputArguments[0].name = UA_STRING((char *)"List files"); + outputArguments[0].dataType = UA_TYPES[UA_TYPES_STRING].typeId; + outputArguments[0].valueRank = UA_VALUERANK_SCALAR; + + /* Add the method node */ + UA_MethodAttributes incAttr = UA_MethodAttributes_default; + incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", + (char *)"Liệt kê các file bản đồ."); + incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"GetMapFiles"); + incAttr.executable = true; + incAttr.userExecutable = true; + UA_Server_addMethodNode(server, + UA_NODEID_STRING(1, (char *)"GetMapFiles"), + parentID, + UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), + UA_QUALIFIEDNAME(1, (char *)"GetMapFiles"), + incAttr, &amr_control::OpcUAServerAPI::listMapFilesCallBack, + 0, NULL, 1, outputArguments, NULL, NULL); +} + +UA_StatusCode amr_control::OpcUAServerAPI::listMapFilesCallBack(UA_Server *server, + const UA_NodeId *sessionId, void *sessionContext, + const UA_NodeId *methodId, void *methodContext, + const UA_NodeId *objectId, void *objectContext, + size_t inputSize, const UA_Variant *input, + size_t outputSize, UA_Variant *output) +{ + + UA_Variant_init(output); + UA_String message = UA_STRING_ALLOC((char *)""); + std::stringstream result; + if (amr_control::OpcUAServerAPI::loc_base_ptr_) + { + amr_control::OpcUAServerAPI::loc_base_ptr_->listMapFiles(result); + ROS_INFO_STREAM(result.str()); + message = UA_STRING_ALLOC((char *)result.str().c_str()); + } + + UA_Variant_setScalarCopy(output, &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Hoàn thành"); + return UA_STATUSCODE_GOOD; +} + +void amr_control::OpcUAServerAPI::addActivateMapMethod(UA_Server *server, UA_NodeId parentID) +{ + /* One input argument */ + UA_Argument inputArguments[1]; + UA_Argument_init(&inputArguments[0]); + inputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"MapName"); + inputArguments[0].name = UA_STRING((char *)"MapName"); + inputArguments[0].dataType = UA_TYPES[UA_TYPES_STRING].typeId; + inputArguments[0].valueRank = UA_VALUERANK_SCALAR; + + /* One output argument */ + UA_Argument outputArguments[2]; + UA_Argument_init(&outputArguments[0]); + outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status"); + outputArguments[0].name = UA_STRING((char *)"Status"); + outputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId; + outputArguments[0].valueRank = UA_VALUERANK_SCALAR; + + UA_Argument_init(&outputArguments[1]); + outputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Message"); + outputArguments[1].name = UA_STRING((char *)"Message"); + outputArguments[1].dataType = UA_TYPES[UA_TYPES_STRING].typeId; + outputArguments[1].valueRank = UA_VALUERANK_SCALAR; + + /* Add the method node */ + UA_MethodAttributes incAttr = UA_MethodAttributes_default; + incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", + (char *)"Chọn một map file để định vị bản đồ "); + incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"ActivateMap"); + incAttr.executable = true; + incAttr.userExecutable = true; + UA_Server_addMethodNode(server, + UA_NODEID_STRING(1, (char *)"ActivateMap"), + parentID, + UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), + UA_QUALIFIEDNAME(1, (char *)"ActivateMap"), + incAttr, &amr_control::OpcUAServerAPI::activateMapCallBack, + 1, inputArguments, 2, outputArguments, NULL, NULL); +} + +UA_StatusCode amr_control::OpcUAServerAPI::activateMapCallBack(UA_Server *server, + const UA_NodeId *sessionId, void *sessionContext, + const UA_NodeId *methodId, void *methodContext, + const UA_NodeId *objectId, void *objectContext, + size_t inputSize, const UA_Variant *input, + size_t outputSize, UA_Variant *output) +{ + + UA_String map_name = *(UA_String *)input[0].data; + + UA_Variant_init(output); + UA_Int32 status = STATUS_SUCCESSED; + UA_String message = UA_STRING_ALLOC((char *)"Lệnh chọn một map file để định vị bản đồ gọi thành công"); + bool result; + if (amr_control::OpcUAServerAPI::loc_base_ptr_) + { + std::string map_name_str = std::string(reinterpret_cast(map_name.data), map_name.length); + if (!map_name_str.empty()) + result = amr_control::OpcUAServerAPI::loc_base_ptr_->changeStaticMap(map_name_str); + else + message = UA_STRING_ALLOC((char *)"MapName chưa có thông tin"); + } + + if (!result) + { + message = UA_STRING_ALLOC((char *)"Lệnh chọn một map file để định vị bản đồ bị lỗi"); + } + + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh chọn một map file để định vị bản đồ gọi thành công"); + return UA_STATUSCODE_GOOD; + +STATUSCODE_BADINTERNALERROR: + status = STATUS_ERROR; + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + return UA_STATUSCODE_BADINTERNALERROR; + +STATUSCODE_BADREQUESTTOOLARGE: + status = STATUS_ERROR; + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + return UA_STATUSCODE_BADREQUESTTOOLARGE; + +STATUSCODE_BADSTRUCTUREMISSING: + status = STATUS_ERROR; + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + return UA_STATUSCODE_BADSTRUCTUREMISSING; + +STATUSCODE_BAD: + status = STATUS_ERROR; + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh chọn một map file để định vị bản đồ bị lỗi"); + return UA_STATUSCODE_BAD; +} + +void amr_control::OpcUAServerAPI::addSetInitialPoseMethod(UA_Server *server, UA_NodeId parentID) +{ + UA_Argument inputArguments[3]; + UA_Argument_init(&inputArguments[0]); + inputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"X"); + inputArguments[0].name = UA_STRING((char *)"X"); + inputArguments[0].dataType = UA_TYPES[UA_TYPES_DOUBLE].typeId; + inputArguments[0].valueRank = UA_VALUERANK_SCALAR; + + UA_Argument_init(&inputArguments[1]); + inputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Y"); + inputArguments[1].name = UA_STRING((char *)"Y"); + inputArguments[1].dataType = UA_TYPES[UA_TYPES_DOUBLE].typeId; + inputArguments[1].valueRank = UA_VALUERANK_SCALAR; + + UA_Argument_init(&inputArguments[2]); + inputArguments[2].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"YAW"); + inputArguments[2].name = UA_STRING((char *)"YAW"); + inputArguments[2].dataType = UA_TYPES[UA_TYPES_DOUBLE].typeId; + inputArguments[2].valueRank = UA_VALUERANK_SCALAR; + + /* One output argument */ + UA_Argument outputArguments[2]; + UA_Argument_init(&outputArguments[0]); + outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status"); + outputArguments[0].name = UA_STRING((char *)"Status"); + outputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId; + outputArguments[0].valueRank = UA_VALUERANK_SCALAR; + + UA_Argument_init(&outputArguments[1]); + outputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Message"); + outputArguments[1].name = UA_STRING((char *)"Message"); + outputArguments[1].dataType = UA_TYPES[UA_TYPES_STRING].typeId; + outputArguments[1].valueRank = UA_VALUERANK_SCALAR; + + /* Add the method node */ + UA_MethodAttributes incAttr = UA_MethodAttributes_default; + incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", + (char *)"Set vị trí ban đầu của robot trên bản đồ."); + + incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Initial Pose"); + incAttr.executable = true; + incAttr.userExecutable = true; + UA_Server_addMethodNode(server, + UA_NODEID_STRING(1, (char *)"Initial Pose"), + parentID, + UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), + UA_QUALIFIEDNAME(1, (char *)"Initial Pose"), + incAttr, &amr_control::OpcUAServerAPI::setInitialPoseCallBack, + 3, inputArguments, 2, outputArguments, NULL, NULL); +} + +UA_StatusCode amr_control::OpcUAServerAPI::setInitialPoseCallBack(UA_Server *server, + const UA_NodeId *sessionId, void *sessionContext, + const UA_NodeId *methodId, void *methodContext, + const UA_NodeId *objectId, void *objectContext, + size_t inputSize, const UA_Variant *input, + size_t outputSize, UA_Variant *output) +{ + + UA_Double X = *(UA_Double *)input[0].data; + UA_Double Y = *(UA_Double *)input[1].data; + UA_Double Yaw = *(UA_Double *)input[2].data; + + std::string fixed_frame = "map"; + geometry_msgs::PoseWithCovarianceStamped pose; + pose.header.frame_id = fixed_frame; + pose.header.stamp = ros::Time::now(); + + // set x,y coord + pose.pose.pose.position.x = X; + pose.pose.pose.position.y = Y; + pose.pose.pose.position.z = 0.0; + + // set theta + tf::Quaternion quat; + quat.setRPY(0.0, 0.0, Yaw); + tf::quaternionTFToMsg(quat, pose.pose.pose.orientation); + + pose.pose.covariance[6 * 0 + 0] = 0.5 * 0.5; + pose.pose.covariance[6 * 1 + 1] = 0.5 * 0.5; + pose.pose.covariance[6 * 5 + 5] = M_PI / 12.0 * M_PI / 12.0; + + amr_control::OpcUAServerAPI::init_pub_.publish(pose); + + UA_Variant_init(output); + + UA_Int32 status = STATUS_SUCCESSED; + UA_String message = UA_STRING_ALLOC((char *)"Initial Pose gọi thành công"); + + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Initial Pose gọi thành công"); + return UA_STATUSCODE_GOOD; + +STATUSCODE_BADINTERNALERROR: + status = STATUS_ERROR; + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + return UA_STATUSCODE_BADINTERNALERROR; + +STATUSCODE_BADREQUESTTOOLARGE: + status = STATUS_ERROR; + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + return UA_STATUSCODE_BADREQUESTTOOLARGE; + +STATUSCODE_BADSTRUCTUREMISSING: + status = STATUS_ERROR; + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + return UA_STATUSCODE_BADSTRUCTUREMISSING; + +STATUSCODE_BAD: + status = STATUS_ERROR; + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Initial Pose bị lỗi"); + return UA_STATUSCODE_BAD; +} + +UA_NodeId *amr_control::OpcUAServerAPI::robotPoseX_Id_ = new UA_NodeId(); +UA_NodeId *amr_control::OpcUAServerAPI::robotPoseY_Id_ = new UA_NodeId(); +UA_NodeId *amr_control::OpcUAServerAPI::robotPoseYaw_Id_ = new UA_NodeId(); +UA_NodeId *amr_control::OpcUAServerAPI::currentActiveMap_Id_ = new UA_NodeId(); +UA_NodeId *amr_control::OpcUAServerAPI::slamState_Id_ = new UA_NodeId(); +UA_NodeId *amr_control::OpcUAServerAPI::workingDirectory_Id_ = new UA_NodeId(); +void amr_control::OpcUAServerAPI::addRobotPoseProperty(UA_Server *server, UA_NodeId parentID) +{ + UA_NodeId robotPose_Id; /* get the nodeid assigned by the server */ + UA_ObjectAttributes slamAttr = UA_ObjectAttributes_default; + slamAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"RobotPose"); + UA_Server_addObjectNode(server, UA_NODEID_NULL, parentID, + UA_NODEID_NUMERIC(0, UA_NS0ID_ORGANIZES), + UA_QUALIFIEDNAME(1, (char *)"RobotPose"), + UA_NODEID_NUMERIC(0, UA_NS0ID_BASEOBJECTTYPE), + slamAttr, NULL, &robotPose_Id); + + // UA_NodeId robotPoseX_Id_; + UA_VariableAttributes poseXAttr = UA_VariableAttributes_default; + UA_Double pose_x = 0; + UA_Variant_setScalar(&poseXAttr.value, &pose_x, &UA_TYPES[UA_TYPES_DOUBLE]); + poseXAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"X"); + poseXAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Biến thông tin tọa độ X của robot."); + poseXAttr.dataType = UA_TYPES[UA_TYPES_DOUBLE].typeId; + UA_Server_addVariableNode(server, UA_NODEID_NULL, robotPose_Id, + UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), + UA_QUALIFIEDNAME(1, (char *)"X"), + UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE), + poseXAttr, NULL, amr_control::OpcUAServerAPI::robotPoseX_Id_); + + // UA_NodeId robotPoseY_Id_; + UA_VariableAttributes poseYAttr = UA_VariableAttributes_default; + UA_Double pose_y = 0; + UA_Variant_setScalar(&poseYAttr.value, &pose_y, &UA_TYPES[UA_TYPES_DOUBLE]); + poseYAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Y"); + poseYAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Biến thông tin tọa độ Y của robot."); + poseYAttr.dataType = UA_TYPES[UA_TYPES_DOUBLE].typeId; + UA_Server_addVariableNode(server, UA_NODEID_NULL, robotPose_Id, + UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), + UA_QUALIFIEDNAME(1, (char *)"Y"), + UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE), + poseYAttr, NULL, amr_control::OpcUAServerAPI::robotPoseY_Id_); + + // UA_NodeId robotPoseYaw_Id_; + UA_VariableAttributes poseYawAttr = UA_VariableAttributes_default; + UA_Double pose_yaw = 0; + UA_Variant_setScalar(&poseYawAttr.value, &pose_yaw, &UA_TYPES[UA_TYPES_DOUBLE]); + poseYawAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Yaw"); + poseYawAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Biến thông tin tọa độ Yaw của robot."); + poseYawAttr.dataType = UA_TYPES[UA_TYPES_DOUBLE].typeId; + UA_Server_addVariableNode(server, UA_NODEID_NULL, robotPose_Id, + UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), + UA_QUALIFIEDNAME(1, (char *)"Yaw"), + UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE), + poseYawAttr, NULL, amr_control::OpcUAServerAPI::robotPoseYaw_Id_); + + // UA_NodeId currentActiveMap_Id; + UA_VariableAttributes currentActiveMapAttr = UA_VariableAttributes_default; + UA_String current_active_map = UA_STRING_ALLOC((char *)" "); + UA_Variant_setScalar(¤tActiveMapAttr.value, ¤t_active_map, &UA_TYPES[UA_TYPES_STRING]); + currentActiveMapAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"CurrentActiveMap"); + currentActiveMapAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Biến lưu trữ thông tin về map đang được active."); + currentActiveMapAttr.dataType = UA_TYPES[UA_TYPES_STRING].typeId; + UA_Server_addVariableNode(server, UA_NODEID_NULL, parentID, + UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), + UA_QUALIFIEDNAME(1, (char *)"CurrentActiveMap"), + UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE), + currentActiveMapAttr, NULL, amr_control::OpcUAServerAPI::currentActiveMap_Id_); + + // UA_NodeId slamState_Id_; + UA_VariableAttributes slamStateAttr = UA_VariableAttributes_default; + UA_String slam_state = UA_STRING_ALLOC((char *)" "); + UA_Variant_setScalar(&slamStateAttr.value, &slam_state, &UA_TYPES[UA_TYPES_STRING]); + slamStateAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"SlamState"); + slamStateAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Biến trạng thái của module slam: Mapping, Localization, Calibrations, Ready, Error"); + slamStateAttr.dataType = UA_TYPES[UA_TYPES_STRING].typeId; + UA_Server_addVariableNode(server, UA_NODEID_NULL, parentID, + UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), + UA_QUALIFIEDNAME(1, (char *)"CurrentActiveMap"), + UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE), + slamStateAttr, NULL, amr_control::OpcUAServerAPI::slamState_Id_); + + // UA_NodeId workingDirectory_Id_ + UA_VariableAttributes workingDirectoryAttr = UA_VariableAttributes_default; + UA_String working_directory = UA_STRING_ALLOC((char *)" "); + UA_Variant_setScalar(&workingDirectoryAttr.value, &working_directory, &UA_TYPES[UA_TYPES_STRING]); + workingDirectoryAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"WorkingDirectories"); + workingDirectoryAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Đường dẫn lưu trữ các file bản đồ"); + workingDirectoryAttr.dataType = UA_TYPES[UA_TYPES_STRING].typeId; + UA_Server_addVariableNode(server, UA_NODEID_NULL, parentID, + UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), + UA_QUALIFIEDNAME(1, (char *)"WorkingDirectories"), + UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE), + workingDirectoryAttr, NULL, amr_control::OpcUAServerAPI::workingDirectory_Id_); +} + +void amr_control::OpcUAServerAPI::slamHandle() +{ + if (amr_control::OpcUAServerAPI::move_base_ptr_ != nullptr && amr_control::OpcUAServerAPI::server_ptr_ != nullptr) + { + geometry_msgs::Pose2D robot_pose; + if (!amr_control::OpcUAServerAPI::move_base_ptr_->getRobotPose(robot_pose)) + { + UA_LOG_WARNING(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "getRobotPose: Is Not OKAY"); + } + else + { + UA_Double pose_x = std::round(robot_pose.x * 10000.0) / 10000.0; + UA_Double pose_y = std::round(robot_pose.y * 10000.0) / 10000.0; + UA_Double pose_yaw = std::round(robot_pose.theta * 10000.0) / 10000.0; + + if (amr_control::OpcUAServerAPI::robotPoseX_Id_ != NULL && + amr_control::OpcUAServerAPI::robotPoseY_Id_ != NULL && + amr_control::OpcUAServerAPI::robotPoseYaw_Id_ != NULL && + amr_control::OpcUAServerAPI::currentActiveMap_Id_ != NULL && + amr_control::OpcUAServerAPI::slamState_Id_ != NULL && + amr_control::OpcUAServerAPI::workingDirectory_Id_ != NULL) + { + // Cập nhật giá trị X + UA_Variant poseXVariant; + UA_Variant_setScalar(&poseXVariant, &pose_x, &UA_TYPES[UA_TYPES_DOUBLE]); + UA_Server_writeValue(amr_control::OpcUAServerAPI::server_ptr_->getServerObject(), *amr_control::OpcUAServerAPI::robotPoseX_Id_, poseXVariant); // Truyền đối tượng, không phải con trỏ + + // Cập nhật giá trị Y + UA_Variant poseYVariant; + UA_Variant_setScalar(&poseYVariant, &pose_y, &UA_TYPES[UA_TYPES_DOUBLE]); + UA_Server_writeValue(amr_control::OpcUAServerAPI::server_ptr_->getServerObject(), *amr_control::OpcUAServerAPI::robotPoseY_Id_, poseYVariant); // Truyền đối tượng, không phải con trỏ + + // Cập nhật giá trị Yaw + UA_Variant poseYawVariant; + UA_Variant_setScalar(&poseYawVariant, &pose_yaw, &UA_TYPES[UA_TYPES_DOUBLE]); + UA_Server_writeValue(amr_control::OpcUAServerAPI::server_ptr_->getServerObject(), *amr_control::OpcUAServerAPI::robotPoseYaw_Id_, poseYawVariant); + + if (amr_control::OpcUAServerAPI::loc_base_ptr_) + { + // ROS_INFO_THROTTLE(2.0, "%s", amr_control::OpcUAServerAPI::loc_base_ptr_->activated_map_filename_.c_str()); + // ROS_INFO_THROTTLE(2.0, "%s", amr_control::OpcUAServerAPI::loc_base_ptr_->working_dir_.c_str()); + UA_Variant currentActiveMapVariant; + UA_String activated_map_filename = UA_STRING_ALLOC((char *)amr_control::OpcUAServerAPI::loc_base_ptr_->activated_map_filename_.c_str()); + UA_Variant_setScalar(¤tActiveMapVariant, &activated_map_filename, &UA_TYPES[UA_TYPES_STRING]); + UA_Server_writeValue(amr_control::OpcUAServerAPI::server_ptr_->getServerObject(), *amr_control::OpcUAServerAPI::currentActiveMap_Id_, currentActiveMapVariant); + + UA_Variant workingDirectoryvariant; + UA_String working_dir = UA_STRING_ALLOC((char *)amr_control::OpcUAServerAPI::loc_base_ptr_->working_dir_.c_str()); + UA_Variant_setScalar(&workingDirectoryvariant, &working_dir, &UA_TYPES[UA_TYPES_STRING]); + UA_Server_writeValue(amr_control::OpcUAServerAPI::server_ptr_->getServerObject(), *amr_control::OpcUAServerAPI::workingDirectory_Id_, workingDirectoryvariant); + + UA_String slam_state = UA_STRING_ALLOC((char *)" "); + switch (amr_control::OpcUAServerAPI::loc_base_ptr_->getState()) + { + case loc_core::Mapping: + slam_state = UA_STRING_ALLOC((char *)"Mapping"); + break; + case loc_core::Localization: + slam_state = UA_STRING_ALLOC((char *)"Localization"); + break; + case loc_core::Calibrations: + slam_state = UA_STRING_ALLOC((char *)"Calibrations"); + break; + case loc_core::Ready: + slam_state = UA_STRING_ALLOC((char *)"Ready"); + break; + case loc_core::Error: + slam_state = UA_STRING_ALLOC((char *)"Error"); + break; + default: + break; + } + UA_Variant slamStatevariant; + UA_Variant_setScalar(&slamStatevariant, &slam_state, &UA_TYPES[UA_TYPES_STRING]); + UA_Server_writeValue(amr_control::OpcUAServerAPI::server_ptr_->getServerObject(), *amr_control::OpcUAServerAPI::slamState_Id_, slamStatevariant); + } + } + } + } +} + +void amr_control::OpcUAServerAPI::addMoveToNodeMethod(UA_Server *server, UA_NodeId parentID) +{ + UA_Argument inputArguments[2]; + UA_Argument_init(&inputArguments[0]); + inputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Kích thước N"); + inputArguments[0].name = UA_STRING((char *)"Length"); + inputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId; + inputArguments[0].valueRank = UA_VALUERANK_SCALAR; + + UA_Argument_init(&inputArguments[1]); + inputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Ma trận [Nx5][x, y, yaw, vmax, accuracy]"); + inputArguments[1].name = UA_STRING((char *)"Nodes"); + inputArguments[1].dataType = UA_TYPES[UA_TYPES_DOUBLE].typeId; + UA_UInt32 pInputDimension[2] = {static_cast(3), 5}; + inputArguments[1].valueRank = UA_VALUERANK_TWO_DIMENSIONS; + inputArguments[1].arrayDimensionsSize = 2; + inputArguments[1].arrayDimensions = pInputDimension; + + /* One output argument */ + UA_Argument outputArguments[2]; + UA_Argument_init(&outputArguments[0]); + outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status"); + outputArguments[0].name = UA_STRING((char *)"Status"); + outputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId; + outputArguments[0].valueRank = UA_VALUERANK_SCALAR; + + UA_Argument_init(&outputArguments[1]); + outputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Message"); + outputArguments[1].name = UA_STRING((char *)"Message"); + outputArguments[1].dataType = UA_TYPES[UA_TYPES_STRING].typeId; + outputArguments[1].valueRank = UA_VALUERANK_SCALAR; + + /* Add the method node */ + UA_MethodAttributes incAttr = UA_MethodAttributes_default; + incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", + (char *)"Di chuyển đến một điểm đích theo thông tin đường đi có trong order message với độ chính xác mong muốn."); + + incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"MoveToNode"); + incAttr.executable = true; + incAttr.userExecutable = true; + UA_Server_addMethodNode(server, + UA_NODEID_STRING(1, (char *)"MoveToNode"), + parentID, + UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), + UA_QUALIFIEDNAME(1, (char *)"MoveToNode"), + incAttr, &amr_control::OpcUAServerAPI::moveToNodeCallBack, + 2, inputArguments, 2, outputArguments, NULL, NULL); +} + +UA_StatusCode amr_control::OpcUAServerAPI::moveToNodeCallBack( + UA_Server *server, + const UA_NodeId *sessionId, void *sessionContext, + const UA_NodeId *methodId, void *methodContext, + const UA_NodeId *objectId, void *objectContext, + size_t inputSize, const UA_Variant *input, + size_t outputSize, UA_Variant *output) +{ + UA_UInt32 length = *(UA_UInt32 *)input[0].data; + UA_Double *nodes = (UA_Double *)input[1].data; + + UA_Variant_init(output); + UA_Int32 status = STATUS_SUCCESSED; + UA_String message = UA_STRING_ALLOC((char *)"MoveToNode gọi thành công"); + + if (input[1].arrayDimensionsSize != 2) + return UA_STATUSCODE_BADINTERNALERROR; + + const int column = length < input[1].arrayDimensions[0] ? length : input[1].arrayDimensions[0]; + const int row = input[1].arrayDimensions[1]; + + if (length > column) + { + message = UA_STRING_ALLOC((char *)"Lỗi vì biến 'Length' vượt quá ngưỡng cho phép là 500000 phần tử"); + goto STATUSCODE_BADREQUESTTOOLARGE; + } + + if (amr_control::OpcUAServerAPI::move_base_ptr_ != nullptr) + { + UA_Double matrix[column][row]; + for (int i = 0; i < column; i++) + for (int j = 0; j < row; j++) + matrix[i][j] = nodes[i * row + j]; + + geometry_msgs::Pose2D goal_2d; + goal_2d.x = matrix[column - 1][0]; + goal_2d.y = matrix[column - 1][1]; + goal_2d.theta = matrix[column - 1][2]; + const double velocity = matrix[column - 1][3]; + const double xy_tolerance = matrix[column - 1][4]; + const double yaw_tolerance = 0.025; + const geometry_msgs::PoseStamped goal = nav_2d_utils::pose2DToPoseStamped(goal_2d, "map", ros::Time::now()); + amr_control::OpcUAServerAPI::move_base_ptr_->moveTo(goal, xy_tolerance, yaw_tolerance); + amr_control::OpcUAServerAPI::cmd_vel_max_.x = velocity; + amr_control::OpcUAServerAPI::cmd_vel_max_.theta = 0.4; + geometry_msgs::Vector3 linear; + linear.x = velocity; + amr_control::OpcUAServerAPI::move_base_ptr_->setTwistLinear(linear); + } + else + { + message = UA_STRING_ALLOC((char *)"Chức năng di chuyển đang bị lỗi khởi tạo"); + goto STATUSCODE_BADSTRUCTUREMISSING; + } + + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "MoveToNode gọi thành công"); + return UA_STATUSCODE_GOOD; + +STATUSCODE_BADINTERNALERROR: + status = STATUS_ERROR; + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + return UA_STATUSCODE_BADINTERNALERROR; + +STATUSCODE_BADREQUESTTOOLARGE: + status = STATUS_ERROR; + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + return UA_STATUSCODE_BADREQUESTTOOLARGE; + +STATUSCODE_BADSTRUCTUREMISSING: + status = STATUS_ERROR; + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + return UA_STATUSCODE_BADSTRUCTUREMISSING; + +STATUSCODE_BAD: + status = STATUS_ERROR; + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "MoveToNode bị lỗi"); + return UA_STATUSCODE_BAD; +} + +void amr_control::OpcUAServerAPI::addDockToNodeMethod(UA_Server *server, UA_NodeId parentID) +{ + UA_Argument inputArguments[3]; + UA_Argument_init(&inputArguments[0]); + inputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Kích thước N"); + inputArguments[0].name = UA_STRING((char *)"Length"); + inputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId; + inputArguments[0].valueRank = UA_VALUERANK_SCALAR; + + UA_Argument_init(&inputArguments[1]); + inputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Ma trận [Nx5][x, y, yaw, vmax, accuracy]"); + inputArguments[1].name = UA_STRING((char *)"Nodes"); + inputArguments[1].dataType = UA_TYPES[UA_TYPES_DOUBLE].typeId; + UA_UInt32 pInputDimension[2] = {static_cast(3), 5}; + inputArguments[1].valueRank = UA_VALUERANK_TWO_DIMENSIONS; + inputArguments[1].arrayDimensionsSize = 2; + inputArguments[1].arrayDimensions = pInputDimension; + + UA_Argument_init(&inputArguments[2]); + inputArguments[2].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Maker"); + inputArguments[2].name = UA_STRING((char *)"Maker"); + inputArguments[2].dataType = UA_TYPES[UA_TYPES_STRING].typeId; + inputArguments[2].valueRank = UA_VALUERANK_SCALAR; + + /* One output argument */ + UA_Argument outputArguments[2]; + UA_Argument_init(&outputArguments[0]); + outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status"); + outputArguments[0].name = UA_STRING((char *)"Status"); + outputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId; + outputArguments[0].valueRank = UA_VALUERANK_SCALAR; + + UA_Argument_init(&outputArguments[1]); + outputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Message"); + outputArguments[1].name = UA_STRING((char *)"Message"); + outputArguments[1].dataType = UA_TYPES[UA_TYPES_STRING].typeId; + outputArguments[1].valueRank = UA_VALUERANK_SCALAR; + + /* Add the method node */ + UA_MethodAttributes incAttr = UA_MethodAttributes_default; + incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", + (char *)"Di chuyển đến một điểm đích theo thông tin đường đi có trong order message.\nNhiệm vụ là lấy hàng ở một vị trí lận cận với điểm đích được order."); + incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"MoveToPickup"); + incAttr.executable = true; + incAttr.userExecutable = true; + UA_Server_addMethodNode(server, + UA_NODEID_STRING(1, (char *)"MoveToPickup"), + parentID, + UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), + UA_QUALIFIEDNAME(1, (char *)"MoveToPickup"), + incAttr, &amr_control::OpcUAServerAPI::dockToNodeCallBack, + 2, inputArguments, 2, outputArguments, NULL, NULL); +} + +UA_StatusCode amr_control::OpcUAServerAPI::dockToNodeCallBack( + UA_Server *server, + const UA_NodeId *sessionId, void *sessionContext, + const UA_NodeId *methodId, void *methodContext, + const UA_NodeId *objectId, void *objectContext, + size_t inputSize, const UA_Variant *input, + size_t outputSize, UA_Variant *output) +{ + UA_UInt32 length = *(UA_UInt32 *)input[0].data; + UA_Double *nodes = (UA_Double *)input[1].data; + UA_String maker = *(UA_String *)input[2].data; + + + UA_Variant_init(output); + UA_Int32 status = STATUS_SUCCESSED; + UA_String message = UA_STRING_ALLOC((char *)"MoveToPickUp gọi thành công"); + + if (input[1].arrayDimensionsSize != 2) + return UA_STATUSCODE_BADINTERNALERROR; + + const int column = length < input[1].arrayDimensions[0] ? length : input[1].arrayDimensions[0]; + const int row = input[1].arrayDimensions[1]; + + if (length > column) + { + message = UA_STRING_ALLOC((char *)"Lỗi vì biến 'Length' vượt quá ngưỡng cho phép là 500000 phần tử"); + goto STATUSCODE_BADREQUESTTOOLARGE; + } + + if (amr_control::OpcUAServerAPI::move_base_ptr_ != nullptr) + { + UA_Double matrix[column][row]; + for (int i = 0; i < column; i++) + for (int j = 0; j < row; j++) + matrix[i][j] = nodes[i * row + j]; + + geometry_msgs::Pose2D goal_2d; + goal_2d.x = matrix[column - 1][0]; + goal_2d.y = matrix[column - 1][1]; + goal_2d.theta = matrix[column - 1][2]; + const double velocity = matrix[column - 1][3]; + const double xy_tolerance = matrix[column - 1][4]; + const double yaw_tolerance = 0.025; + const geometry_msgs::PoseStamped goal = nav_2d_utils::pose2DToPoseStamped(goal_2d, "map", ros::Time::now()); + std::string maker_str(reinterpret_cast(maker.data), maker.length); + amr_control::OpcUAServerAPI::move_base_ptr_->dockTo(maker_str, goal, xy_tolerance, yaw_tolerance); + amr_control::OpcUAServerAPI::cmd_vel_max_.x = velocity; + amr_control::OpcUAServerAPI::cmd_vel_max_.theta = 0.4; + geometry_msgs::Vector3 linear; + linear.x = velocity; + amr_control::OpcUAServerAPI::move_base_ptr_->setTwistLinear(linear); + } + else + { + message = UA_STRING_ALLOC((char *)"Chức năng di chuyển đang bị lỗi khởi tạo"); + goto STATUSCODE_BADSTRUCTUREMISSING; + } + + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "MoveToPickUp gọi thành công"); + return UA_STATUSCODE_GOOD; + +STATUSCODE_BADINTERNALERROR: + status = STATUS_ERROR; + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + return UA_STATUSCODE_BADINTERNALERROR; + +STATUSCODE_BADREQUESTTOOLARGE: + status = STATUS_ERROR; + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + return UA_STATUSCODE_BADREQUESTTOOLARGE; + +STATUSCODE_BADSTRUCTUREMISSING: + status = STATUS_ERROR; + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + return UA_STATUSCODE_BADSTRUCTUREMISSING; + +STATUSCODE_BAD: + status = STATUS_ERROR; + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "MoveToNode bị lỗi"); + return UA_STATUSCODE_BAD; +} + +void amr_control::OpcUAServerAPI::addRotateToMethod(UA_Server *server, UA_NodeId parentID) +{ + UA_Argument inputArguments[2]; + UA_Argument_init(&inputArguments[0]); + inputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Góc muốn quay trong giải từ -Pi đến Pi"); + inputArguments[0].name = UA_STRING((char *)"Yaw goal"); + inputArguments[0].dataType = UA_TYPES[UA_TYPES_DOUBLE].typeId; + inputArguments[0].valueRank = UA_VALUERANK_SCALAR; + + UA_Argument_init(&inputArguments[1]); + inputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Sai số góc cho phép"); + inputArguments[1].name = UA_STRING((char *)"Yaw tolerance"); + inputArguments[1].dataType = UA_TYPES[UA_TYPES_DOUBLE].typeId; + inputArguments[1].valueRank = UA_VALUERANK_SCALAR; + + /* One output argument */ + UA_Argument outputArguments[2]; + UA_Argument_init(&outputArguments[0]); + outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status"); + outputArguments[0].name = UA_STRING((char *)"Status"); + outputArguments[0].dataType = UA_TYPES[UA_TYPES_INT32].typeId; + outputArguments[0].valueRank = UA_VALUERANK_SCALAR; + + UA_Argument_init(&outputArguments[1]); + outputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Message"); + outputArguments[1].name = UA_STRING((char *)"Message"); + outputArguments[1].dataType = UA_TYPES[UA_TYPES_STRING].typeId; + outputArguments[1].valueRank = UA_VALUERANK_SCALAR; + + /* Add the method node */ + UA_MethodAttributes incAttr = UA_MethodAttributes_default; + incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", + (char *)"Robot quay tại chỗ một góc yaw."); + incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"RotateTo"); + incAttr.executable = true; + incAttr.userExecutable = true; + UA_Server_addMethodNode(server, + UA_NODEID_STRING(1, (char *)"RotateTo"), + parentID, + UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), + UA_QUALIFIEDNAME(1, (char *)"RotateTo"), + incAttr, &amr_control::OpcUAServerAPI::rotateToCallBack, + 2, inputArguments, 2, outputArguments, NULL, NULL); +} + +UA_StatusCode amr_control::OpcUAServerAPI::rotateToCallBack( + UA_Server *server, + const UA_NodeId *sessionId, void *sessionContext, + const UA_NodeId *methodId, void *methodContext, + const UA_NodeId *objectId, void *objectContext, + size_t inputSize, const UA_Variant *input, + size_t outputSize, UA_Variant *output) +{ + UA_Double Yaw = *(UA_Double *)input[0].data; + UA_Double tolerance = *(UA_Double *)input[1].data; + UA_Variant_init(output); + UA_Int32 status = STATUS_SUCCESSED; + UA_String message = UA_STRING_ALLOC((char *)"Successed"); + if (amr_control::OpcUAServerAPI::move_base_ptr_ != nullptr) + { + geometry_msgs::PoseStamped goal; + if (!amr_control::OpcUAServerAPI::move_base_ptr_->getRobotPose(goal)) + { + message = UA_STRING_ALLOC((char *)"Lỗi không nhận được tọa độ"); + goto STATUSCODE_BADINTERNALERROR; + } + tf2::Quaternion q; + q.setRPY(0, 0, angles::normalize_angle(Yaw)); + goal.pose.orientation.x = q.x(); + goal.pose.orientation.y = q.y(); + goal.pose.orientation.z = q.z(); + goal.pose.orientation.w = q.w(); + amr_control::OpcUAServerAPI::move_base_ptr_->rotateTo(goal, tolerance); + } + else + { + return UA_STATUSCODE_BADSTRUCTUREMISSING; + } + + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "RotateTo was called"); + return UA_STATUSCODE_GOOD; + +STATUSCODE_BADINTERNALERROR: + status = STATUS_ERROR; + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + return UA_STATUSCODE_BADINTERNALERROR; + +STATUSCODE_BAD: + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "RotateTo bị lỗi"); + return UA_STATUSCODE_BAD; +} + +void amr_control::OpcUAServerAPI::addMoveStraightMethod(UA_Server *server, UA_NodeId parentID) +{ + UA_Argument inputArguments[2]; + UA_Argument_init(&inputArguments[0]); + inputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Khoảng cách đi '+' là tiến '-' là lùi"); + inputArguments[0].name = UA_STRING((char *)"Distance"); + inputArguments[0].dataType = UA_TYPES[UA_TYPES_DOUBLE].typeId; + inputArguments[0].valueRank = UA_VALUERANK_SCALAR; + + UA_Argument_init(&inputArguments[1]); + inputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Vận tốc di chuyển"); + inputArguments[1].name = UA_STRING((char *)"Velocity linear"); + inputArguments[1].dataType = UA_TYPES[UA_TYPES_DOUBLE].typeId; + inputArguments[1].valueRank = UA_VALUERANK_SCALAR; + + /* One output argument */ + UA_Argument outputArguments[2]; + UA_Argument_init(&outputArguments[0]); + outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status"); + outputArguments[0].name = UA_STRING((char *)"Status"); + outputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId; + outputArguments[0].valueRank = UA_VALUERANK_SCALAR; + + UA_Argument_init(&outputArguments[1]); + outputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Message"); + outputArguments[1].name = UA_STRING((char *)"Message"); + outputArguments[1].dataType = UA_TYPES[UA_TYPES_STRING].typeId; + outputArguments[1].valueRank = UA_VALUERANK_SCALAR; + + /* Add the method node */ + UA_MethodAttributes incAttr = UA_MethodAttributes_default; + incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", + (char *)"Di chuyển đến một điểm đích theo thông tin đường đi có trong order message.\nNhiệm vụ là trả hàng ở một vị trí lận cận với điểm đích được order."); + incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"MoveStraight"); + incAttr.executable = true; + incAttr.userExecutable = true; + UA_Server_addMethodNode(server, + UA_NODEID_STRING(1, (char *)"MoveStraight"), + parentID, + UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), + UA_QUALIFIEDNAME(1, (char *)"MoveStraight"), + incAttr, &amr_control::OpcUAServerAPI::moveStraighCallBack, + 2, inputArguments, 2, outputArguments, NULL, NULL); +} + +UA_StatusCode amr_control::OpcUAServerAPI::moveStraighCallBack(UA_Server *server, + const UA_NodeId *sessionId, void *sessionContext, + const UA_NodeId *methodId, void *methodContext, + const UA_NodeId *objectId, void *objectContext, + size_t inputSize, const UA_Variant *input, + size_t outputSize, UA_Variant *output) +{ + UA_Double distance = *(UA_Double *)input[0].data; + UA_Double velocity = *(UA_Double *)input[1].data; + + UA_Variant_init(output); + UA_Int32 status = STATUS_SUCCESSED; + UA_String message = UA_STRING_ALLOC((char *)"MoveStraight gọi thành công"); + + if (amr_control::OpcUAServerAPI::move_base_ptr_ != nullptr) + { + geometry_msgs::PoseStamped pose; + if (!amr_control::OpcUAServerAPI::move_base_ptr_->getRobotPose(pose)) + { + message = UA_STRING_ALLOC((char *)"Lỗi không nhận được tọa độ"); + goto STATUSCODE_BADINTERNALERROR; + } + ROS_INFO_STREAM(pose); + geometry_msgs::PoseStamped goal = move_base_core::offset_goal(pose, distance); + amr_control::OpcUAServerAPI::move_base_ptr_->moveStraightTo(goal); + amr_control::OpcUAServerAPI::cmd_vel_max_.x = velocity; + amr_control::OpcUAServerAPI::cmd_vel_max_.theta = 0.4; + geometry_msgs::Vector3 linear; + linear.x = velocity; + amr_control::OpcUAServerAPI::move_base_ptr_->setTwistLinear(linear); + } + else + { + message = UA_STRING_ALLOC((char *)"Chức năng di chuyển đang bị lỗi khởi tạo"); + goto STATUSCODE_BADSTRUCTUREMISSING; + } + + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "MoveStraight gọi thành công"); + return UA_STATUSCODE_GOOD; + +STATUSCODE_BADINTERNALERROR: + status = STATUS_ERROR; + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + return UA_STATUSCODE_BADINTERNALERROR; + +STATUSCODE_BADREQUESTTOOLARGE: + status = STATUS_ERROR; + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + return UA_STATUSCODE_BADREQUESTTOOLARGE; + +STATUSCODE_BADSTRUCTUREMISSING: + status = STATUS_ERROR; + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + return UA_STATUSCODE_BADSTRUCTUREMISSING; + +STATUSCODE_BAD: + status = STATUS_ERROR; + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "MoveToNode bị lỗi"); + return UA_STATUSCODE_BAD; +} + +void amr_control::OpcUAServerAPI::addCancelMethod(UA_Server *server, UA_NodeId parentID) +{ + /* One output argument */ + UA_Argument outputArguments[2]; + UA_Argument_init(&outputArguments[0]); + outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status"); + outputArguments[0].name = UA_STRING((char *)"Status"); + outputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId; + outputArguments[0].valueRank = UA_VALUERANK_SCALAR; + + UA_Argument_init(&outputArguments[1]); + outputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Message"); + outputArguments[1].name = UA_STRING((char *)"Message"); + outputArguments[1].dataType = UA_TYPES[UA_TYPES_STRING].typeId; + outputArguments[1].valueRank = UA_VALUERANK_SCALAR; + + /* Add the method node */ + UA_MethodAttributes incAttr = UA_MethodAttributes_default; + incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", + (char *)"Hủy lệnh di chuyển của AMR"); + incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Cancel"); + incAttr.executable = true; + incAttr.userExecutable = true; + UA_Server_addMethodNode(server, + UA_NODEID_STRING(1, (char *)"Cancel"), + parentID, + UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), + UA_QUALIFIEDNAME(1, (char *)"Cancel"), + incAttr, &amr_control::OpcUAServerAPI::cancelCallBack, + 0, NULL, 2, outputArguments, NULL, NULL); +} + +UA_StatusCode amr_control::OpcUAServerAPI::cancelCallBack(UA_Server *server, + const UA_NodeId *sessionId, void *sessionContext, + const UA_NodeId *methodId, void *methodContext, + const UA_NodeId *objectId, void *objectContext, + size_t inputSize, const UA_Variant *input, + size_t outputSize, UA_Variant *output) +{ + UA_Variant_init(output); + UA_Int32 status = STATUS_SUCCESSED; + UA_String message = UA_STRING_ALLOC((char *)"Cancel gọi thành công"); + + if (amr_control::OpcUAServerAPI::move_base_ptr_ != nullptr) + { + amr_control::OpcUAServerAPI::move_base_ptr_->cancel(); + } + else + { + message = UA_STRING_ALLOC((char *)"Chức năng hủy lệnh đang bị lỗi khởi tạo"); + goto STATUSCODE_BADSTRUCTUREMISSING; + } + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Cancel gọi thành công"); + return UA_STATUSCODE_GOOD; + +STATUSCODE_BADINTERNALERROR: + status = STATUS_ERROR; + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + return UA_STATUSCODE_BADINTERNALERROR; + +STATUSCODE_BADREQUESTTOOLARGE: + status = STATUS_ERROR; + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + return UA_STATUSCODE_BADREQUESTTOOLARGE; + +STATUSCODE_BADSTRUCTUREMISSING: + status = STATUS_ERROR; + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + return UA_STATUSCODE_BADSTRUCTUREMISSING; + +STATUSCODE_BAD: + status = STATUS_ERROR; + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Cancel bị lỗi"); + return UA_STATUSCODE_BAD; +} + +void amr_control::OpcUAServerAPI::addPauseMethod(UA_Server *server, UA_NodeId parentID) +{ + /* One output argument */ + UA_Argument outputArguments[2]; + UA_Argument_init(&outputArguments[0]); + outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status"); + outputArguments[0].name = UA_STRING((char *)"Status"); + outputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId; + outputArguments[0].valueRank = UA_VALUERANK_SCALAR; + + UA_Argument_init(&outputArguments[1]); + outputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Message"); + outputArguments[1].name = UA_STRING((char *)"Message"); + outputArguments[1].dataType = UA_TYPES[UA_TYPES_STRING].typeId; + outputArguments[1].valueRank = UA_VALUERANK_SCALAR; + + /* Add the method node */ + UA_MethodAttributes incAttr = UA_MethodAttributes_default; + incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", + (char *)"Lệnh tạm dừng di chuyển của AMR"); + incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Pause"); + incAttr.executable = true; + incAttr.userExecutable = true; + UA_Server_addMethodNode(server, + UA_NODEID_STRING(1, (char *)"Pause"), + parentID, + UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), + UA_QUALIFIEDNAME(1, (char *)"Pause"), + incAttr, &amr_control::OpcUAServerAPI::pauseCallBack, + 0, NULL, 2, outputArguments, NULL, NULL); +} + +UA_StatusCode amr_control::OpcUAServerAPI::pauseCallBack(UA_Server *server, + const UA_NodeId *sessionId, void *sessionContext, + const UA_NodeId *methodId, void *methodContext, + const UA_NodeId *objectId, void *objectContext, + size_t inputSize, const UA_Variant *input, + size_t outputSize, UA_Variant *output) +{ + UA_Variant_init(output); + UA_Int32 status = STATUS_SUCCESSED; + UA_String message = UA_STRING_ALLOC((char *)"Pause gọi thành công"); + + if (amr_control::OpcUAServerAPI::move_base_ptr_ != nullptr) + { + amr_control::OpcUAServerAPI::move_base_ptr_->pause(); + } + else + { + message = UA_STRING_ALLOC((char *)"Chức năng tạm dừng đang bị lỗi khởi tạo"); + goto STATUSCODE_BADSTRUCTUREMISSING; + } + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Pause gọi thành công"); + return UA_STATUSCODE_GOOD; + +STATUSCODE_BADINTERNALERROR: + status = STATUS_ERROR; + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + return UA_STATUSCODE_BADINTERNALERROR; + +STATUSCODE_BADREQUESTTOOLARGE: + status = STATUS_ERROR; + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + return UA_STATUSCODE_BADREQUESTTOOLARGE; + +STATUSCODE_BADSTRUCTUREMISSING: + status = STATUS_ERROR; + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + return UA_STATUSCODE_BADSTRUCTUREMISSING; + +STATUSCODE_BAD: + status = STATUS_ERROR; + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Pause bị lỗi"); + return UA_STATUSCODE_BAD; +} + +void amr_control::OpcUAServerAPI::addResumeMethod(UA_Server *server, UA_NodeId parentID) +{ + /* One output argument */ + UA_Argument outputArguments[2]; + UA_Argument_init(&outputArguments[0]); + outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status"); + outputArguments[0].name = UA_STRING((char *)"Status"); + outputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId; + outputArguments[0].valueRank = UA_VALUERANK_SCALAR; + + UA_Argument_init(&outputArguments[1]); + outputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Message"); + outputArguments[1].name = UA_STRING((char *)"Message"); + outputArguments[1].dataType = UA_TYPES[UA_TYPES_STRING].typeId; + outputArguments[1].valueRank = UA_VALUERANK_SCALAR; + + /* Add the method node */ + UA_MethodAttributes incAttr = UA_MethodAttributes_default; + incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", + (char *)"Lệnh tiếp tục di chuyển của AMR"); + incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Resume"); + incAttr.executable = true; + incAttr.userExecutable = true; + UA_Server_addMethodNode(server, + UA_NODEID_STRING(1, (char *)"Resume"), + parentID, + UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), + UA_QUALIFIEDNAME(1, (char *)"Resume"), + incAttr, &amr_control::OpcUAServerAPI::resumeCallBack, + 0, NULL, 2, outputArguments, NULL, NULL); +} + +UA_StatusCode amr_control::OpcUAServerAPI::resumeCallBack(UA_Server *server, + const UA_NodeId *sessionId, void *sessionContext, + const UA_NodeId *methodId, void *methodContext, + const UA_NodeId *objectId, void *objectContext, + size_t inputSize, const UA_Variant *input, + size_t outputSize, UA_Variant *output) +{ + UA_Variant_init(output); + UA_Int32 status = STATUS_SUCCESSED; + UA_String message = UA_STRING_ALLOC((char *)"Resume gọi thành công"); + + if (amr_control::OpcUAServerAPI::move_base_ptr_ != nullptr) + { + amr_control::OpcUAServerAPI::move_base_ptr_->resume(); + } + else + { + message = UA_STRING_ALLOC((char *)"Chức năng tiếp tục đang bị lỗi khởi tạo"); + goto STATUSCODE_BADSTRUCTUREMISSING; + } + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Resume gọi thành công"); + return UA_STATUSCODE_GOOD; + +STATUSCODE_BADINTERNALERROR: + status = STATUS_ERROR; + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + return UA_STATUSCODE_BADINTERNALERROR; + +STATUSCODE_BADREQUESTTOOLARGE: + status = STATUS_ERROR; + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + return UA_STATUSCODE_BADREQUESTTOOLARGE; + +STATUSCODE_BADSTRUCTUREMISSING: + status = STATUS_ERROR; + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + return UA_STATUSCODE_BADSTRUCTUREMISSING; + +STATUSCODE_BAD: + status = STATUS_ERROR; + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Resume bị lỗi"); + return UA_STATUSCODE_BAD; +} + +void amr_control::OpcUAServerAPI::addResetMethod(UA_Server *server, UA_NodeId parentID) +{ + /* One output argument */ + UA_Argument outputArguments[2]; + UA_Argument_init(&outputArguments[0]); + outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status"); + outputArguments[0].name = UA_STRING((char *)"Status"); + outputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId; + outputArguments[0].valueRank = UA_VALUERANK_SCALAR; + + UA_Argument_init(&outputArguments[1]); + outputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Message"); + outputArguments[1].name = UA_STRING((char *)"Message"); + outputArguments[1].dataType = UA_TYPES[UA_TYPES_STRING].typeId; + outputArguments[1].valueRank = UA_VALUERANK_SCALAR; + + /* Add the method node */ + UA_MethodAttributes incAttr = UA_MethodAttributes_default; + incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", + (char *)"Lệnh reset AMR"); + incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Reset"); + incAttr.executable = true; + incAttr.userExecutable = true; + UA_Server_addMethodNode(server, + UA_NODEID_STRING(1, (char *)"Reset"), + parentID, + UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), + UA_QUALIFIEDNAME(1, (char *)"Reset"), + incAttr, &amr_control::OpcUAServerAPI::resetCallBack, + 0, NULL, 2, outputArguments, NULL, NULL); +} + +UA_StatusCode amr_control::OpcUAServerAPI::resetCallBack(UA_Server *server, + const UA_NodeId *sessionId, void *sessionContext, + const UA_NodeId *methodId, void *methodContext, + const UA_NodeId *objectId, void *objectContext, + size_t inputSize, const UA_Variant *input, + size_t outputSize, UA_Variant *output) +{ + + // sleep(5); + + UA_Variant_init(output); + UA_Int32 status = STATUS_SUCCESSED; + UA_String message = UA_STRING_ALLOC((char *)"Reset gọi thành công"); + + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Reset gọi thành công"); + return UA_STATUSCODE_GOOD; + +STATUSCODE_BADINTERNALERROR: + status = STATUS_ERROR; + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + return UA_STATUSCODE_BADINTERNALERROR; + +STATUSCODE_BADREQUESTTOOLARGE: + status = STATUS_ERROR; + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + return UA_STATUSCODE_BADREQUESTTOOLARGE; + +STATUSCODE_BADSTRUCTUREMISSING: + status = STATUS_ERROR; + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + return UA_STATUSCODE_BADSTRUCTUREMISSING; + +STATUSCODE_BAD: + status = STATUS_ERROR; + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Reset bị lỗi"); + return UA_STATUSCODE_BAD; +} + +void amr_control::OpcUAServerAPI::addWriteMutedMethod(UA_Server *server, UA_NodeId parentID) +{ + UA_Argument inputArguments[1]; + UA_Argument_init(&inputArguments[0]); + inputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Tích là bật hoặc ngược lại"); + inputArguments[0].name = UA_STRING((char *)"Input Value"); + inputArguments[0].dataType = UA_TYPES[UA_TYPES_BOOLEAN].typeId; + inputArguments[0].valueRank = UA_VALUERANK_SCALAR; + + /* One output argument */ + UA_Argument outputArguments[2]; + UA_Argument_init(&outputArguments[0]); + outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status"); + outputArguments[0].name = UA_STRING((char *)"Status"); + outputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId; + outputArguments[0].valueRank = UA_VALUERANK_SCALAR; + + UA_Argument_init(&outputArguments[1]); + outputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Message"); + outputArguments[1].name = UA_STRING((char *)"Message"); + outputArguments[1].dataType = UA_TYPES[UA_TYPES_STRING].typeId; + outputArguments[1].valueRank = UA_VALUERANK_SCALAR; + + /* Add the method node */ + UA_MethodAttributes incAttr = UA_MethodAttributes_default; + incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", + (char *)"Chức năng bật/tắt vùng Muted Safety"); + incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Muted Safety"); + incAttr.executable = true; + incAttr.userExecutable = true; + UA_Server_addMethodNode(server, + UA_NODEID_STRING(1, (char *)"Muted Safety"), + parentID, + UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), + UA_QUALIFIEDNAME(1, (char *)"Muted Safety"), + incAttr, &amr_control::OpcUAServerAPI::writeMutedCallBack, + 1, inputArguments, 2, outputArguments, NULL, NULL); +} + +UA_StatusCode amr_control::OpcUAServerAPI::writeMutedCallBack(UA_Server *server, + const UA_NodeId *sessionId, void *sessionContext, + const UA_NodeId *methodId, void *methodContext, + const UA_NodeId *objectId, void *objectContext, + size_t inputSize, const UA_Variant *input, + size_t outputSize, UA_Variant *output) +{ + *amr_control::OpcUAServerAPI::muted_value_ = *(bool *)input[0].data; + + UA_Variant_init(output); + UA_Int32 status = STATUS_SUCCESSED; + UA_String message = UA_STRING_ALLOC((char *)"Lệnh Muted Safety gọi thành công"); + + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh Muted Safety gọi thành công"); + return UA_STATUSCODE_GOOD; + +STATUSCODE_BADINTERNALERROR: + status = STATUS_ERROR; + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + return UA_STATUSCODE_BADINTERNALERROR; + +STATUSCODE_BADREQUESTTOOLARGE: + status = STATUS_ERROR; + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + return UA_STATUSCODE_BADREQUESTTOOLARGE; + +STATUSCODE_BADSTRUCTUREMISSING: + status = STATUS_ERROR; + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + return UA_STATUSCODE_BADSTRUCTUREMISSING; + +STATUSCODE_BAD: + status = STATUS_ERROR; + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh Muted Safety bị lỗi"); + return UA_STATUSCODE_BAD; +} + +UA_NodeId *amr_control::OpcUAServerAPI::amr_status_str_Id_ = new UA_NodeId(); +UA_NodeId *amr_control::OpcUAServerAPI::amr_status_en_Id_ = new UA_NodeId(); +UA_NodeId *amr_control::OpcUAServerAPI::amr_robotFeedBack_Id_ = new UA_NodeId(); +void amr_control::OpcUAServerAPI::addNavigationState(UA_Server *server, UA_NodeId parentID) +{ + UA_VariableAttributes status_str_Attr = UA_VariableAttributes_default; + UA_String status_str = UA_STRING_ALLOC((char *)""); + UA_Variant_setScalar(&status_str_Attr.value, &status_str, &UA_TYPES[UA_TYPES_STRING]); + status_str_Attr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status String"); + status_str_Attr.description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Trạng thái của robot."); + status_str_Attr.dataType = UA_TYPES[UA_TYPES_STRING].typeId; + UA_Server_addVariableNode(server, UA_NODEID_NULL, parentID, + UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), + UA_QUALIFIEDNAME(1, (char *)"Status String"), + UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE), + status_str_Attr, NULL, amr_control::OpcUAServerAPI::amr_status_str_Id_); + + UA_VariableAttributes status_en_Attr = UA_VariableAttributes_default; + UA_Int32 status_en = UA_Int32(0); + UA_Variant_setScalar(&status_en_Attr.value, &status_en, &UA_TYPES[UA_TYPES_UINT32]); + status_en_Attr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status Enum"); + status_en_Attr.description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Trạng thái của robot."); + status_en_Attr.dataType = UA_TYPES[UA_TYPES_UINT32].typeId; + UA_Server_addVariableNode(server, UA_NODEID_NULL, parentID, + UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), + UA_QUALIFIEDNAME(1, (char *)"Status Enum"), + UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE), + status_en_Attr, NULL, amr_control::OpcUAServerAPI::amr_status_en_Id_); + + UA_VariableAttributes robotFeedBackAttr = UA_VariableAttributes_default; + UA_String feed_back_str = UA_STRING_ALLOC((char *)""); + UA_Variant_setScalar(&robotFeedBackAttr.value, &feed_back_str, &UA_TYPES[UA_TYPES_STRING]); + robotFeedBackAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Feedback"); + robotFeedBackAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Thông tin phản hồi của robot."); + robotFeedBackAttr.dataType = UA_TYPES[UA_TYPES_STRING].typeId; + UA_Server_addVariableNode(server, UA_NODEID_NULL, parentID, + UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), + UA_QUALIFIEDNAME(1, (char *)"Feedback"), + UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE), + robotFeedBackAttr, NULL, amr_control::OpcUAServerAPI::amr_robotFeedBack_Id_); + + std::thread([=]() + { + ros::Rate r(10); + while (ros::ok() && amr_control::OpcUAServerAPI::server_ptr_->isRunning()) + { + r.sleep(); + ros::spinOnce(); + } }) + .detach(); +} + +void amr_control::OpcUAServerAPI::navigationHandle() +{ + if (amr_control::OpcUAServerAPI::move_base_ptr_ != nullptr && amr_control::OpcUAServerAPI::server_ptr_ != nullptr) + { + if (amr_control::OpcUAServerAPI::move_base_ptr_->nav_feedback_ != nullptr) + { + switch (amr_control::OpcUAServerAPI::move_base_ptr_->nav_feedback_->navigation_state) + { + case move_base_core::State::PENDING: + amr_control::OpcUAServerAPI::amr_status_str_ = UA_STRING_ALLOC((char *)"PENDING"); + amr_control::OpcUAServerAPI::amr_status_en_ = UA_UInt32(0); + break; + case move_base_core::State::ACTIVE: + amr_control::OpcUAServerAPI::amr_status_str_ = UA_STRING_ALLOC((char *)"ACTIVE"); + amr_control::OpcUAServerAPI::amr_status_en_ = UA_UInt32(1); + break; + case move_base_core::State::PREEMPTED: + amr_control::OpcUAServerAPI::amr_status_str_ = UA_STRING_ALLOC((char *)"PREEMPTED"); + amr_control::OpcUAServerAPI::amr_status_en_ = UA_UInt32(2); + break; + case move_base_core::State::SUCCEEDED: + amr_control::OpcUAServerAPI::amr_status_str_ = UA_STRING_ALLOC((char *)"SUCCEEDED"); + amr_control::OpcUAServerAPI::amr_status_en_ = UA_UInt32(3); + break; + case move_base_core::State::ABORTED: + amr_control::OpcUAServerAPI::amr_status_str_ = UA_STRING_ALLOC((char *)"ABORTED"); + amr_control::OpcUAServerAPI::amr_status_en_ = UA_UInt32(4); + break; + case move_base_core::State::REJECTED: + amr_control::OpcUAServerAPI::amr_status_str_ = UA_STRING_ALLOC((char *)"REJECTED"); + amr_control::OpcUAServerAPI::amr_status_en_ = UA_UInt32(5); + break; + case move_base_core::State::PREEMPTING: + amr_control::OpcUAServerAPI::amr_status_str_ = UA_STRING_ALLOC((char *)"PREEMPTING"); + amr_control::OpcUAServerAPI::amr_status_en_ = UA_UInt32(6); + break; + case move_base_core::State::RECALLING: + amr_control::OpcUAServerAPI::amr_status_str_ = UA_STRING_ALLOC((char *)"RECALLING"); + amr_control::OpcUAServerAPI::amr_status_en_ = UA_UInt32(7); + break; + case move_base_core::State::RECALLED: + amr_control::OpcUAServerAPI::amr_status_str_ = UA_STRING_ALLOC((char *)"RECALLED"); + amr_control::OpcUAServerAPI::amr_status_en_ = UA_UInt32(8); + break; + case move_base_core::State::LOST: + amr_control::OpcUAServerAPI::amr_status_str_ = UA_STRING_ALLOC((char *)"LOST"); + amr_control::OpcUAServerAPI::amr_status_en_ = UA_UInt32(9); + break; + case move_base_core::State::PLANNING: + amr_control::OpcUAServerAPI::amr_status_str_ = UA_STRING_ALLOC((char *)"PLANNING"); + amr_control::OpcUAServerAPI::amr_status_en_ = UA_UInt32(10); + break; + case move_base_core::State::CONTROLLING: + amr_control::OpcUAServerAPI::amr_status_str_ = UA_STRING_ALLOC((char *)"CONTROLLING"); + amr_control::OpcUAServerAPI::amr_status_en_ = UA_UInt32(11); + break; + case move_base_core::State::CLEARING: + amr_control::OpcUAServerAPI::amr_status_str_ = UA_STRING_ALLOC((char *)"CLEARING"); + amr_control::OpcUAServerAPI::amr_status_en_ = UA_UInt32(12); + break; + case move_base_core::State::PAUSED: + amr_control::OpcUAServerAPI::amr_status_str_ = UA_STRING_ALLOC((char *)"PAUSED"); + amr_control::OpcUAServerAPI::amr_status_en_ = UA_UInt32(13); + break; + default: + break; + } + + if (amr_control::OpcUAServerAPI::move_base_ptr_->nav_feedback_->navigation_state == move_base_core::State::CONTROLLING) + amr_control::OpcUAServerAPI::amr_feedback_str_ = UA_STRING_ALLOC((char *)""); + else + amr_control::OpcUAServerAPI::amr_feedback_str_ = + UA_STRING_ALLOC((char *)amr_control::OpcUAServerAPI::move_base_ptr_->nav_feedback_->feed_back_str.c_str()); + } + + if (amr_control::OpcUAServerAPI::amr_status_str_Id_ != NULL && amr_control::OpcUAServerAPI::amr_status_en_Id_ != NULL && amr_control::OpcUAServerAPI::amr_robotFeedBack_Id_ != NULL) + { + UA_Variant statusStrVariant; + UA_Variant_setScalar(&statusStrVariant, &amr_control::OpcUAServerAPI::amr_status_str_, &UA_TYPES[UA_TYPES_STRING]); + UA_Server_writeValue(amr_control::OpcUAServerAPI::server_ptr_->getServerObject(), *amr_control::OpcUAServerAPI::amr_status_str_Id_, statusStrVariant); // Truyền đối tượng, không phải con trỏ + + UA_Variant statusEnVariant; + UA_Variant_setScalar(&statusEnVariant, &amr_control::OpcUAServerAPI::amr_status_en_, &UA_TYPES[UA_TYPES_UINT32]); + UA_Server_writeValue(amr_control::OpcUAServerAPI::server_ptr_->getServerObject(), *amr_control::OpcUAServerAPI::amr_status_en_Id_, statusEnVariant); // Truyền đối tượng, không phải con trỏ + + UA_Variant feedbackVariant; + UA_Variant_setScalar(&feedbackVariant, &amr_control::OpcUAServerAPI::amr_feedback_str_, &UA_TYPES[UA_TYPES_STRING]); + UA_Server_writeValue(amr_control::OpcUAServerAPI::server_ptr_->getServerObject(), *amr_control::OpcUAServerAPI::amr_robotFeedBack_Id_, feedbackVariant); // Truyền đối tượng, không phải con trỏ + } + } +} + +UA_NodeId *amr_control::OpcUAServerAPI::vx_Id_ = new UA_NodeId(); +UA_NodeId *amr_control::OpcUAServerAPI::vy_Id_ = new UA_NodeId(); +UA_NodeId *amr_control::OpcUAServerAPI::omega_Id_ = new UA_NodeId(); +void amr_control::OpcUAServerAPI::addVelocityCommand(UA_Server *server, UA_NodeId parentID) +{ + UA_VariableAttributes vxAttr = UA_VariableAttributes_default; + UA_Double vx = UA_Double(0); + UA_Variant_setScalar(&vxAttr.value, &vx, &UA_TYPES[UA_TYPES_DOUBLE]); + vxAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Vx"); + vxAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Vận tốc tịnh tiến theo trục X."); + vxAttr.dataType = UA_TYPES[UA_TYPES_DOUBLE].typeId; + UA_Server_addVariableNode(server, UA_NODEID_NULL, parentID, + UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), + UA_QUALIFIEDNAME(1, (char *)"Vx"), + UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE), + vxAttr, NULL, amr_control::OpcUAServerAPI::vx_Id_); + + UA_VariableAttributes vyAttr = UA_VariableAttributes_default; + UA_Double vy = UA_Double(0); + UA_Variant_setScalar(&vyAttr.value, &vy, &UA_TYPES[UA_TYPES_DOUBLE]); + vyAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Vy"); + vyAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Vận tốc tịnh tiến theo trục Y."); + vyAttr.dataType = UA_TYPES[UA_TYPES_DOUBLE].typeId; + UA_Server_addVariableNode(server, UA_NODEID_NULL, parentID, + UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), + UA_QUALIFIEDNAME(1, (char *)"Vy"), + UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE), + vyAttr, NULL, amr_control::OpcUAServerAPI::vy_Id_); + + UA_VariableAttributes omegaAttr = UA_VariableAttributes_default; + UA_Double angular_vel = UA_Double(0); + UA_Variant_setScalar(&omegaAttr.value, &angular_vel, &UA_TYPES[UA_TYPES_DOUBLE]); + omegaAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Omega"); + omegaAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Vận tốc quay theo trục Z."); + omegaAttr.dataType = UA_TYPES[UA_TYPES_DOUBLE].typeId; + UA_Server_addVariableNode(server, UA_NODEID_NULL, parentID, + UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), + UA_QUALIFIEDNAME(1, (char *)"Omega"), + UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE), + omegaAttr, NULL, amr_control::OpcUAServerAPI::omega_Id_); +} + +void amr_control::OpcUAServerAPI::monitorHandle() +{ + if (amr_control::OpcUAServerAPI::server_ptr_ != nullptr) + { + if (amr_control::OpcUAServerAPI::monitor_ptr_ != nullptr) + { + nav_2d_msgs::Twist2D velocity; + if (monitor_ptr_->getVelocity(velocity)) + { + UA_Double vx = std::round(velocity.x * 1000.0) / 1000.0; + UA_Double vy = std::round(velocity.y * 1000.0) / 1000.0; + UA_Double omega = std::round(velocity.theta * 1000.0) / 1000.0; + + if (amr_control::OpcUAServerAPI::vx_Id_ != NULL && + amr_control::OpcUAServerAPI::vy_Id_ != NULL && + amr_control::OpcUAServerAPI::omega_Id_ != NULL) + { + UA_Variant vxVariant; + UA_Variant_setScalar(&vxVariant, &vx, &UA_TYPES[UA_TYPES_DOUBLE]); + UA_Server_writeValue(amr_control::OpcUAServerAPI::server_ptr_->getServerObject(), *amr_control::OpcUAServerAPI::vx_Id_, vxVariant); // Truyền đối tượng, không phải con trỏ + + UA_Variant vyVariant; + UA_Variant_setScalar(&vyVariant, &vy, &UA_TYPES[UA_TYPES_DOUBLE]); + UA_Server_writeValue(amr_control::OpcUAServerAPI::server_ptr_->getServerObject(), *amr_control::OpcUAServerAPI::vy_Id_, vyVariant); // Truyền đối tượng, không phải con trỏ + + UA_Variant omegaVariant; + UA_Variant_setScalar(&omegaVariant, &omega, &UA_TYPES[UA_TYPES_DOUBLE]); + UA_Server_writeValue(amr_control::OpcUAServerAPI::server_ptr_->getServerObject(), *amr_control::OpcUAServerAPI::omega_Id_, omegaVariant); // Truyền đối tượng, không phải con trỏ + } + } + } + } +} + +void amr_control::OpcUAServerAPI::addPickUpDobotMethod(UA_Server *server, UA_NodeId parentID) +{ + /* One output argument */ + UA_Argument outputArguments[2]; + UA_Argument_init(&outputArguments[0]); + outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status"); + outputArguments[0].name = UA_STRING((char *)"Status"); + outputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId; + outputArguments[0].valueRank = UA_VALUERANK_SCALAR; + + UA_Argument_init(&outputArguments[1]); + outputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Message"); + outputArguments[1].name = UA_STRING((char *)"Message"); + outputArguments[1].dataType = UA_TYPES[UA_TYPES_STRING].typeId; + outputArguments[1].valueRank = UA_VALUERANK_SCALAR; + + /* Add the method node */ + UA_MethodAttributes incAttr = UA_MethodAttributes_default; + incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", + (char *)"Chức năng lấy hàng bởi tay máy Dobot"); + incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"PickUp"); + incAttr.executable = true; + incAttr.userExecutable = true; + UA_Server_addMethodNode(server, + UA_NODEID_STRING(1, (char *)"PickUp"), + parentID, + UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), + UA_QUALIFIEDNAME(1, (char *)"PickUp"), + incAttr, &amr_control::OpcUAServerAPI::PickUpDobotCallBack, + 0, NULL, 2, outputArguments, NULL, NULL); +} + +UA_StatusCode amr_control::OpcUAServerAPI::PickUpDobotCallBack(UA_Server *server, + const UA_NodeId *sessionId, void *sessionContext, + const UA_NodeId *methodId, void *methodContext, + const UA_NodeId *objectId, void *objectContext, + size_t inputSize, const UA_Variant *input, + size_t outputSize, UA_Variant *output) +{ + + UA_Variant_init(output); + UA_Int32 status = STATUS_SUCCESSED; + UA_String message = UA_STRING_ALLOC((char *)"Lệnh khởi động và chạy tay máy Dobot gọi thành công"); + + if (!amr_control::OpcUAServerAPI::arm_function_ptr_) + { + UA_Int32 status = STATUS_ERROR; + UA_String message = UA_STRING_ALLOC((char *)"Lỗi khi khởi tạo nhiệm vụ"); + UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lỗi khi khởi tạo nhiệm vụ"); + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lỗi khi khởi tạo nhiệm vụ"); + return UA_STATUSCODE_GOOD; + } + if (amr_control::OpcUAServerAPI::arm_thread_ptr_ && amr_control::OpcUAServerAPI::arm_thread_ptr_->joinable()) + { + UA_Int32 status = STATUS_ERROR; + UA_String message = UA_STRING_ALLOC((char *)"Dobot đang bận"); + UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Dobot đang bận"); + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Dobot đang bận"); + return UA_STATUSCODE_GOOD; + } + else if (amr_control::OpcUAServerAPI::arm_thread_ptr_ && !amr_control::OpcUAServerAPI::arm_thread_ptr_->joinable()) + { + amr_control::OpcUAServerAPI::resetState(); + ROS_INFO("Dobot is running..."); + *amr_control::OpcUAServerAPI::arm_cancel_ = true; + amr_control::OpcUAServerAPI::arm_function_ptr_(); + } + else + { + UA_Int32 status = STATUS_ERROR; + UA_String message = UA_STRING_ALLOC((char *)"Lỗi khi khởi tạo nhiệm vụ"); + UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lỗi khi khởi tạo nhiệm vụ"); + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lỗi khi khởi tạo nhiệm vụ"); + return UA_STATUSCODE_GOOD; + } + + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh khởi động và chạy tay máy Dobot gọi thành công"); + return UA_STATUSCODE_GOOD; + +STATUSCODE_BADINTERNALERROR: + status = STATUS_ERROR; + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + return UA_STATUSCODE_BADINTERNALERROR; + +STATUSCODE_BADREQUESTTOOLARGE: + status = STATUS_ERROR; + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + return UA_STATUSCODE_BADREQUESTTOOLARGE; + +STATUSCODE_BADSTRUCTUREMISSING: + status = STATUS_ERROR; + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + return UA_STATUSCODE_BADSTRUCTUREMISSING; + +STATUSCODE_BAD: + status = STATUS_ERROR; + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh StartAndRunDobot bị lỗi"); + return UA_STATUSCODE_BAD; +} + +void amr_control::OpcUAServerAPI::addGoHomeDobotMethod(UA_Server *server, UA_NodeId parentID) +{ + /* One output argument */ + UA_Argument outputArguments[2]; + UA_Argument_init(&outputArguments[0]); + outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status"); + outputArguments[0].name = UA_STRING((char *)"Status"); + outputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId; + outputArguments[0].valueRank = UA_VALUERANK_SCALAR; + + UA_Argument_init(&outputArguments[1]); + outputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Message"); + outputArguments[1].name = UA_STRING((char *)"Message"); + outputArguments[1].dataType = UA_TYPES[UA_TYPES_STRING].typeId; + outputArguments[1].valueRank = UA_VALUERANK_SCALAR; + + /* Add the method node */ + UA_MethodAttributes incAttr = UA_MethodAttributes_default; + incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", + (char *)"Chức năng về gốc tay máy Dobot"); + incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"GoHomeDobot"); + incAttr.executable = true; + incAttr.userExecutable = true; + UA_Server_addMethodNode(server, + UA_NODEID_STRING(1, (char *)"GoHomeDobot"), + parentID, + UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), + UA_QUALIFIEDNAME(1, (char *)"GoHomeDobot"), + incAttr, &amr_control::OpcUAServerAPI::goHomeDobotCallBack, + 0, NULL, 2, outputArguments, NULL, NULL); +} + +UA_StatusCode amr_control::OpcUAServerAPI::goHomeDobotCallBack(UA_Server *server, + const UA_NodeId *sessionId, void *sessionContext, + const UA_NodeId *methodId, void *methodContext, + const UA_NodeId *objectId, void *objectContext, + size_t inputSize, const UA_Variant *input, + size_t outputSize, UA_Variant *output) +{ + + UA_Variant_init(output); + UA_Int32 status = STATUS_SUCCESSED; + UA_String message = UA_STRING_ALLOC((char *)"Lệnh về gốc tay máy Dobot gọi thành công"); + + if (!amr_control::OpcUAServerAPI::arm_function_ptr_) + { + UA_Int32 status = STATUS_ERROR; + UA_String message = UA_STRING_ALLOC((char *)"Lỗi khi khởi tạo nhiệm vụ"); + UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lỗi khi khởi tạo nhiệm vụ"); + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lỗi khi khởi tạo nhiệm vụ"); + return UA_STATUSCODE_GOOD; + } + + if (amr_control::OpcUAServerAPI::arm_thread_ptr_ && amr_control::OpcUAServerAPI::arm_thread_ptr_->joinable()) + { + UA_Int32 status = STATUS_ERROR; + UA_String message = UA_STRING_ALLOC((char *)"Dobot đang bận"); + UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Dobot đang bận"); + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Dobot đang bận"); + return UA_STATUSCODE_GOOD; + } + else if (amr_control::OpcUAServerAPI::arm_thread_ptr_ && !amr_control::OpcUAServerAPI::arm_thread_ptr_->joinable()) + { + amr_control::OpcUAServerAPI::resetState(); + *amr_control::OpcUAServerAPI::arm_go_home_ = true; + *amr_control::OpcUAServerAPI::arm_cancel_ = true; + ROS_INFO("Dobot is Homing..."); + amr_control::OpcUAServerAPI::arm_function_ptr_(); + } + else + { + UA_Int32 status = STATUS_ERROR; + UA_String message = UA_STRING_ALLOC((char *)"Lỗi khi khởi tạo nhiệm vụ"); + UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lỗi khi khởi tạo nhiệm vụ"); + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lỗi khi khởi tạo nhiệm vụ"); + return UA_STATUSCODE_GOOD; + } + + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh về gốc tay máy Dobot gọi thành công"); + return UA_STATUSCODE_GOOD; + +STATUSCODE_BADINTERNALERROR: + status = STATUS_ERROR; + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + return UA_STATUSCODE_BADINTERNALERROR; + +STATUSCODE_BADREQUESTTOOLARGE: + status = STATUS_ERROR; + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + return UA_STATUSCODE_BADREQUESTTOOLARGE; + +STATUSCODE_BADSTRUCTUREMISSING: + status = STATUS_ERROR; + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + return UA_STATUSCODE_BADSTRUCTUREMISSING; + +STATUSCODE_BAD: + status = STATUS_ERROR; + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh về gốc tay máy Dobot bị lỗi"); + return UA_STATUSCODE_BAD; +} + +void amr_control::OpcUAServerAPI::addCountMethod(UA_Server *server, UA_NodeId parentID) +{ + /* One output argument */ + UA_Argument inputArguments[2]; + UA_Argument_init(&inputArguments[0]); + inputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Biến số lượng hàng OK cần lấy"); + inputArguments[0].name = UA_STRING((char *)"Count OK"); + inputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId; + inputArguments[0].valueRank = UA_VALUERANK_SCALAR; + + UA_Argument_init(&inputArguments[1]); + inputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Biến số lượng hàng NG cần lấy"); + inputArguments[1].name = UA_STRING((char *)"Count NG"); + inputArguments[1].dataType = UA_TYPES[UA_TYPES_UINT32].typeId; + inputArguments[1].valueRank = UA_VALUERANK_SCALAR; + + /* One output argument */ + UA_Argument outputArguments[2]; + UA_Argument_init(&outputArguments[0]); + outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status"); + outputArguments[0].name = UA_STRING((char *)"Status"); + outputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId; + outputArguments[0].valueRank = UA_VALUERANK_SCALAR; + + UA_Argument_init(&outputArguments[1]); + outputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Message"); + outputArguments[1].name = UA_STRING((char *)"Message"); + outputArguments[1].dataType = UA_TYPES[UA_TYPES_STRING].typeId; + outputArguments[1].valueRank = UA_VALUERANK_SCALAR; + + /* Add the method node */ + UA_MethodAttributes incAttr = UA_MethodAttributes_default; + incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", + (char *)"Chức năng thay đổi số lượng hàng muốn lấy"); + incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"SetQuantityOfGoods"); + incAttr.executable = true; + incAttr.userExecutable = true; + UA_Server_addMethodNode(server, + UA_NODEID_STRING(1, (char *)"SetQuantityOfGoods"), + parentID, + UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), + UA_QUALIFIEDNAME(1, (char *)"SetQuantityOfGoods"), + incAttr, &amr_control::OpcUAServerAPI::counterCallBack, + 2, inputArguments, 2, outputArguments, NULL, NULL); +} + +UA_StatusCode amr_control::OpcUAServerAPI::counterCallBack(UA_Server *server, + const UA_NodeId *sessionId, void *sessionContext, + const UA_NodeId *methodId, void *methodContext, + const UA_NodeId *objectId, void *objectContext, + size_t inputSize, const UA_Variant *input, + size_t outputSize, UA_Variant *output) +{ + + UA_UInt32 count_ok = *(UA_UInt32 *)input[0].data; + UA_UInt32 count_ng = *(UA_UInt32 *)input[1].data; + + UA_Variant_init(output); + UA_Int32 status = STATUS_SUCCESSED; + UA_String message = UA_STRING_ALLOC((char *)"Chức năng thay đổi số lượng hàng OK muốn lấy gọi thành công"); + + *amr_control::OpcUAServerAPI::count_ok_max_ = count_ok; + *amr_control::OpcUAServerAPI::count_ng_max_ = count_ng; + + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Chức năng thay đổi số lượng hàng OK muốn lấy gọi thành công"); + return UA_STATUSCODE_GOOD; + +STATUSCODE_BADINTERNALERROR: + status = STATUS_ERROR; + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + return UA_STATUSCODE_BADINTERNALERROR; + +STATUSCODE_BADREQUESTTOOLARGE: + status = STATUS_ERROR; + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + return UA_STATUSCODE_BADREQUESTTOOLARGE; + +STATUSCODE_BADSTRUCTUREMISSING: + status = STATUS_ERROR; + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + return UA_STATUSCODE_BADSTRUCTUREMISSING; + +STATUSCODE_BAD: + status = STATUS_ERROR; + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Chức năng thay đổi số lượng hàng OK muốn lấy gọi bị lỗi"); + return UA_STATUSCODE_BAD; +} + +void amr_control::OpcUAServerAPI::addCancelDobotMethod(UA_Server *server, UA_NodeId parentID) +{ + /* One output argument */ + UA_Argument outputArguments[2]; + UA_Argument_init(&outputArguments[0]); + outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status"); + outputArguments[0].name = UA_STRING((char *)"Status"); + outputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId; + outputArguments[0].valueRank = UA_VALUERANK_SCALAR; + + UA_Argument_init(&outputArguments[1]); + outputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Message"); + outputArguments[1].name = UA_STRING((char *)"Message"); + outputArguments[1].dataType = UA_TYPES[UA_TYPES_STRING].typeId; + outputArguments[1].valueRank = UA_VALUERANK_SCALAR; + + /* Add the method node */ + UA_MethodAttributes incAttr = UA_MethodAttributes_default; + incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", + (char *)"Chức năng dừng tay máy Dobot"); + incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"CancelDobot"); + incAttr.executable = true; + incAttr.userExecutable = true; + UA_Server_addMethodNode(server, + UA_NODEID_STRING(1, (char *)"CancelDobot"), + parentID, + UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), + UA_QUALIFIEDNAME(1, (char *)"CancelDobot"), + incAttr, &amr_control::OpcUAServerAPI::cancelDobotCallBack, + 0, NULL, 2, outputArguments, NULL, NULL); +} + +UA_StatusCode amr_control::OpcUAServerAPI::cancelDobotCallBack(UA_Server *server, + const UA_NodeId *sessionId, void *sessionContext, + const UA_NodeId *methodId, void *methodContext, + const UA_NodeId *objectId, void *objectContext, + size_t inputSize, const UA_Variant *input, + size_t outputSize, UA_Variant *output) +{ + UA_Variant_init(output); + UA_Int32 status = STATUS_SUCCESSED; + UA_String message = UA_STRING_ALLOC((char *)"Lệnh dừng tay máy Dobot gọi thành công"); + + *amr_control::OpcUAServerAPI::arm_cancel_ = false; + + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh dừng tay máy Dobot gọi thành công"); + return UA_STATUSCODE_GOOD; + +STATUSCODE_BADINTERNALERROR: + status = STATUS_ERROR; + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + return UA_STATUSCODE_BADINTERNALERROR; + +STATUSCODE_BADREQUESTTOOLARGE: + status = STATUS_ERROR; + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + return UA_STATUSCODE_BADREQUESTTOOLARGE; + +STATUSCODE_BADSTRUCTUREMISSING: + status = STATUS_ERROR; + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + return UA_STATUSCODE_BADSTRUCTUREMISSING; + +STATUSCODE_BAD: + status = STATUS_ERROR; + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh dừng tay máy Dobot bị lỗi"); + return UA_STATUSCODE_BAD; +} + +void amr_control::OpcUAServerAPI::addContinueDobotMethod(UA_Server *server, UA_NodeId parentID) +{ + /* One output argument */ + UA_Argument outputArguments[2]; + UA_Argument_init(&outputArguments[0]); + outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status"); + outputArguments[0].name = UA_STRING((char *)"Status"); + outputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId; + outputArguments[0].valueRank = UA_VALUERANK_SCALAR; + + UA_Argument_init(&outputArguments[1]); + outputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Message"); + outputArguments[1].name = UA_STRING((char *)"Message"); + outputArguments[1].dataType = UA_TYPES[UA_TYPES_STRING].typeId; + outputArguments[1].valueRank = UA_VALUERANK_SCALAR; + + /* Add the method node */ + UA_MethodAttributes incAttr = UA_MethodAttributes_default; + incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", + (char *)"Chức năng tiếp tục chạy tay máy Dobot"); + incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"ContinueDobot"); + incAttr.executable = true; + incAttr.userExecutable = true; + UA_Server_addMethodNode(server, + UA_NODEID_STRING(1, (char *)"ContinueDobot"), + parentID, + UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), + UA_QUALIFIEDNAME(1, (char *)"ContinueDobot"), + incAttr, &amr_control::OpcUAServerAPI::continueDobotCallBack, + 0, NULL, 2, outputArguments, NULL, NULL); +} + +UA_StatusCode amr_control::OpcUAServerAPI::continueDobotCallBack(UA_Server *server, + const UA_NodeId *sessionId, void *sessionContext, + const UA_NodeId *methodId, void *methodContext, + const UA_NodeId *objectId, void *objectContext, + size_t inputSize, const UA_Variant *input, + size_t outputSize, UA_Variant *output) +{ + UA_Variant_init(output); + UA_Int32 status = STATUS_SUCCESSED; + UA_String message = UA_STRING_ALLOC((char *)"Lệnh tiếp tục chạy tay máy Dobot gọi thành công"); + + amr_control::OpcUAServerAPI::resetState(); + *amr_control::OpcUAServerAPI::arm_continue_ = true; + + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh tiếp tục chạy tay máy Dobot gọi thành công"); + return UA_STATUSCODE_GOOD; + +STATUSCODE_BADINTERNALERROR: + status = STATUS_ERROR; + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + return UA_STATUSCODE_BADINTERNALERROR; + +STATUSCODE_BADREQUESTTOOLARGE: + status = STATUS_ERROR; + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + return UA_STATUSCODE_BADREQUESTTOOLARGE; + +STATUSCODE_BADSTRUCTUREMISSING: + status = STATUS_ERROR; + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + return UA_STATUSCODE_BADSTRUCTUREMISSING; + +STATUSCODE_BAD: + status = STATUS_ERROR; + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh tiếp tục chạy tay máy Dobot bị lỗi"); + return UA_STATUSCODE_BAD; +} + +void amr_control::OpcUAServerAPI::addPowerOnDobotMethod(UA_Server *server, UA_NodeId parentID) +{ + /* One output argument */ + UA_Argument outputArguments[2]; + UA_Argument_init(&outputArguments[0]); + outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status"); + outputArguments[0].name = UA_STRING((char *)"Status"); + outputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId; + outputArguments[0].valueRank = UA_VALUERANK_SCALAR; + + UA_Argument_init(&outputArguments[1]); + outputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Message"); + outputArguments[1].name = UA_STRING((char *)"Message"); + outputArguments[1].dataType = UA_TYPES[UA_TYPES_STRING].typeId; + outputArguments[1].valueRank = UA_VALUERANK_SCALAR; + + /* Add the method node */ + UA_MethodAttributes incAttr = UA_MethodAttributes_default; + incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", + (char *)"Chức năng khởi động nguồn tay máy Dobot"); + incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"PowerOnDobot"); + incAttr.executable = true; + incAttr.userExecutable = true; + UA_Server_addMethodNode(server, + UA_NODEID_STRING(1, (char *)"PowerOnDobot"), + parentID, + UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), + UA_QUALIFIEDNAME(1, (char *)"PowerOnDobot"), + incAttr, &amr_control::OpcUAServerAPI::powerOnDobotCallBack, + 0, NULL, 2, outputArguments, NULL, NULL); +} + +UA_StatusCode amr_control::OpcUAServerAPI::powerOnDobotCallBack(UA_Server *server, + const UA_NodeId *sessionId, void *sessionContext, + const UA_NodeId *methodId, void *methodContext, + const UA_NodeId *objectId, void *objectContext, + size_t inputSize, const UA_Variant *input, + size_t outputSize, UA_Variant *output) +{ + UA_Variant_init(output); + UA_Int32 status = STATUS_SUCCESSED; + UA_String message = UA_STRING_ALLOC((char *)"Lệnh khởi động nguồn tay máy Dobot gọi thành công"); + + amr_control::OpcUAServerAPI::resetState(); + *amr_control::OpcUAServerAPI::arm_power_on_ = true; + + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh khởi động nguồn tay máy Dobot gọi thành công"); + return UA_STATUSCODE_GOOD; + +STATUSCODE_BADINTERNALERROR: + status = STATUS_ERROR; + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + return UA_STATUSCODE_BADINTERNALERROR; + +STATUSCODE_BADREQUESTTOOLARGE: + status = STATUS_ERROR; + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + return UA_STATUSCODE_BADREQUESTTOOLARGE; + +STATUSCODE_BADSTRUCTUREMISSING: + status = STATUS_ERROR; + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + return UA_STATUSCODE_BADSTRUCTUREMISSING; + +STATUSCODE_BAD: + status = STATUS_ERROR; + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh khởi động nguồn tay máy Dobot bị lỗi"); + return UA_STATUSCODE_BAD; +} + +UA_NodeId *amr_control::OpcUAServerAPI::arm_dobot_state_str_Id_ = new UA_NodeId(); +UA_NodeId *amr_control::OpcUAServerAPI::arm_dobot_state_en_Id_ = new UA_NodeId(); +UA_NodeId *amr_control::OpcUAServerAPI::arm_dobot_count_ok_max_Id_ = new UA_NodeId(); +UA_NodeId *amr_control::OpcUAServerAPI::arm_dobot_count_ng_max_Id_ = new UA_NodeId(); +UA_NodeId *amr_control::OpcUAServerAPI::arm_dobot_mode_Id_ = new UA_NodeId(); +UA_NodeId *amr_control::OpcUAServerAPI::arm_dobot_status_code_Id_ = new UA_NodeId(); +void amr_control::OpcUAServerAPI::addDobotProperties(UA_Server *server, UA_NodeId parentID) +{ + + UA_VariableAttributes dobot_state_str_Attr = UA_VariableAttributes_default; + UA_String dobot_state_str = UA_STRING_ALLOC((char *)"PENDING"); + UA_Variant_setScalar(&dobot_state_str_Attr.value, &dobot_state_str, &UA_TYPES[UA_TYPES_STRING]); + dobot_state_str_Attr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"State String"); + dobot_state_str_Attr.description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Trạng thái của Dobot."); + dobot_state_str_Attr.dataType = UA_TYPES[UA_TYPES_STRING].typeId; + UA_Server_addVariableNode(server, UA_NODEID_NULL, parentID, + UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), + UA_QUALIFIEDNAME(1, (char *)"State String"), + UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE), + dobot_state_str_Attr, NULL, amr_control::OpcUAServerAPI::arm_dobot_state_str_Id_); + + UA_VariableAttributes dobot_state_en_Attr = UA_VariableAttributes_default; + UA_UInt32 dobot_state_en = UA_UInt32(amr_control::State::FAILED); + UA_Variant_setScalar(&dobot_state_en_Attr.value, &dobot_state_en, &UA_TYPES[UA_TYPES_UINT32]); + dobot_state_en_Attr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"State Enum"); + dobot_state_en_Attr.description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Trạng thái của Dobot."); + dobot_state_en_Attr.dataType = UA_TYPES[UA_TYPES_UINT32].typeId; + UA_Server_addVariableNode(server, UA_NODEID_NULL, parentID, + UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), + UA_QUALIFIEDNAME(1, (char *)"State Enum"), + UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE), + dobot_state_en_Attr, NULL, amr_control::OpcUAServerAPI::arm_dobot_state_en_Id_); + + UA_VariableAttributes dobot_count_ok_max_Attr = UA_VariableAttributes_default; + UA_UInt32 count_ok_max = *amr_control::OpcUAServerAPI::count_ok_max_; + UA_Variant_setScalar(&dobot_count_ok_max_Attr.value, &count_ok_max, &UA_TYPES[UA_TYPES_UINT32]); + dobot_count_ok_max_Attr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Count OK"); + dobot_count_ok_max_Attr.description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Số lượng hàng OK cần lấy"); + dobot_count_ok_max_Attr.dataType = UA_TYPES[UA_TYPES_UINT32].typeId; + UA_Server_addVariableNode(server, UA_NODEID_NULL, parentID, + UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), + UA_QUALIFIEDNAME(1, (char *)"Count OK"), + UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE), + dobot_count_ok_max_Attr, NULL, amr_control::OpcUAServerAPI::arm_dobot_count_ok_max_Id_); + + UA_VariableAttributes dobot_count_ng_max_Attr = UA_VariableAttributes_default; + UA_UInt32 count_ng_max = *amr_control::OpcUAServerAPI::count_ng_max_; + UA_Variant_setScalar(&dobot_count_ng_max_Attr.value, &count_ng_max, &UA_TYPES[UA_TYPES_UINT32]); + dobot_count_ng_max_Attr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Count NG"); + dobot_count_ng_max_Attr.description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Số lượng hàng NG cần lấy"); + dobot_count_ng_max_Attr.dataType = UA_TYPES[UA_TYPES_UINT32].typeId; + UA_Server_addVariableNode(server, UA_NODEID_NULL, parentID, + UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), + UA_QUALIFIEDNAME(1, (char *)"Count NG"), + UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE), + dobot_count_ng_max_Attr, NULL, amr_control::OpcUAServerAPI::arm_dobot_count_ng_max_Id_); + + UA_VariableAttributes dobot_mode_ptr_Attr = UA_VariableAttributes_default; + UA_Double *mode = amr_control::OpcUAServerAPI::mode_ptr_; + UA_Variant_setScalar(&dobot_mode_ptr_Attr.value, mode, &UA_TYPES[UA_TYPES_DOUBLE]); + dobot_mode_ptr_Attr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Dobot Mode"); + dobot_mode_ptr_Attr.description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Chế độ của tay máy Dobot"); + dobot_mode_ptr_Attr.dataType = UA_TYPES[UA_TYPES_DOUBLE].typeId; + UA_Server_addVariableNode(server, UA_NODEID_NULL, parentID, + UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), + UA_QUALIFIEDNAME(1, (char *)"Dobot Mode"), + UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE), + dobot_mode_ptr_Attr, NULL, amr_control::OpcUAServerAPI::arm_dobot_status_code_Id_); + + UA_VariableAttributes dobot_status_code_ptr_Attr = UA_VariableAttributes_default; + UA_UInt32 *status_code = amr_control::OpcUAServerAPI::status_code_ptr_; + UA_Variant_setScalar(&dobot_status_code_ptr_Attr.value, status_code, &UA_TYPES[UA_TYPES_UINT32]); + dobot_status_code_ptr_Attr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Dobot Status Code"); + dobot_status_code_ptr_Attr.description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Mã trạng thái của tay máy Dobot"); + dobot_status_code_ptr_Attr.dataType = UA_TYPES[UA_TYPES_UINT32].typeId; + UA_Server_addVariableNode(server, UA_NODEID_NULL, parentID, + UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), + UA_QUALIFIEDNAME(1, (char *)"Dobot Status Code"), + UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE), + dobot_status_code_ptr_Attr, NULL, amr_control::OpcUAServerAPI::arm_dobot_status_code_Id_); +} + +void amr_control::OpcUAServerAPI::dobotPropertiesHandle() +{ + if (amr_control::OpcUAServerAPI::server_ptr_ != nullptr) + { + + if (amr_control::OpcUAServerAPI::arm_thread_ptr_) + { + // if (amr_control::OpcUAServerAPI::arm_joined_) + // { + amr_control::OpcUAServerAPI::arm_dobot_state_en_ = amr_control::State::FINISHED; + amr_control::OpcUAServerAPI::arm_dobot_state_str_ = UA_STRING_ALLOC((char *)"FINISHED"); + amr_control::OpcUAServerAPI::resetState(); + // } + + // if ( + // !amr_control::OpcUAServerAPI::arm_thread_ptr_->joinable() && + // !amr_control::OpcUAServerAPI::arm_joined_) + // { + // amr_control::OpcUAServerAPI::arm_dobot_state_en_ = amr_control::State::INITIALIZING; + // amr_control::OpcUAServerAPI::arm_dobot_state_str_ = UA_STRING_ALLOC((char *)"INITIALIZING"); + // } + // else if (amr_control::OpcUAServerAPI::arm_thread_ptr_ && + // amr_control::OpcUAServerAPI::arm_thread_ptr_->joinable() && + // !amr_control::OpcUAServerAPI::arm_joined_) + // { + // amr_control::OpcUAServerAPI::arm_dobot_state_en_ = amr_control::State::RUNNING; + // amr_control::OpcUAServerAPI::arm_dobot_state_str_ = UA_STRING_ALLOC((char *)"RUNNING"); + // } + } + else + { + amr_control::OpcUAServerAPI::arm_dobot_state_en_ = amr_control::State::WAITING; + amr_control::OpcUAServerAPI::arm_dobot_state_str_ = UA_STRING_ALLOC((char *)"WAITING"); + } + + if (amr_control::OpcUAServerAPI::arm_dobot_state_str_Id_ != NULL && + amr_control::OpcUAServerAPI::arm_dobot_state_en_Id_ != NULL && + amr_control::OpcUAServerAPI::arm_dobot_count_ok_max_Id_ != NULL && + amr_control::OpcUAServerAPI::arm_dobot_count_ng_max_Id_ != NULL && + amr_control::OpcUAServerAPI::arm_dobot_mode_Id_ != NULL && + amr_control::OpcUAServerAPI::arm_dobot_status_code_Id_ != NULL) + { + UA_Variant dobot_state_str_Variant; + UA_Variant_setScalar(&dobot_state_str_Variant, &amr_control::OpcUAServerAPI::arm_dobot_state_str_, &UA_TYPES[UA_TYPES_STRING]); + UA_Server_writeValue(amr_control::OpcUAServerAPI::server_ptr_->getServerObject(), *amr_control::OpcUAServerAPI::arm_dobot_state_str_Id_, dobot_state_str_Variant); // Truyền đối tượng, không phải con trỏ + + UA_Variant dobot_state_en_Variant; + UA_Variant_setScalar(&dobot_state_en_Variant, &amr_control::OpcUAServerAPI::arm_dobot_state_en_, &UA_TYPES[UA_TYPES_UINT32]); + UA_Server_writeValue(amr_control::OpcUAServerAPI::server_ptr_->getServerObject(), *amr_control::OpcUAServerAPI::arm_dobot_state_en_Id_, dobot_state_en_Variant); // Truyền đối tượng, không phải con trỏ + + UA_Variant dobot_count_ok_max_Variant; + UA_Variant_setScalar(&dobot_count_ok_max_Variant, &amr_control::OpcUAServerAPI::count_ok_max_, &UA_TYPES[UA_TYPES_UINT32]); + UA_Server_writeValue(amr_control::OpcUAServerAPI::server_ptr_->getServerObject(), *amr_control::OpcUAServerAPI::arm_dobot_count_ok_max_Id_, dobot_count_ok_max_Variant); // Truyền đối tượng, không phải con trỏ + + UA_Variant dobot_count_ng_max_Variant; + UA_Variant_setScalar(&dobot_count_ng_max_Variant, &amr_control::OpcUAServerAPI::count_ng_max_, &UA_TYPES[UA_TYPES_UINT32]); + UA_Server_writeValue(amr_control::OpcUAServerAPI::server_ptr_->getServerObject(), *amr_control::OpcUAServerAPI::arm_dobot_count_ng_max_Id_, dobot_count_ng_max_Variant); // Truyền đối tượng, không phải con trỏ + + if (amr_control::OpcUAServerAPI::mode_ptr_) + { + UA_Variant dobot_mode_ptr_Variant; + UA_Variant_setScalar(&dobot_mode_ptr_Variant, amr_control::OpcUAServerAPI::mode_ptr_, &UA_TYPES[UA_TYPES_DOUBLE]); + UA_Server_writeValue(amr_control::OpcUAServerAPI::server_ptr_->getServerObject(), *amr_control::OpcUAServerAPI::arm_dobot_mode_Id_, dobot_mode_ptr_Variant); + } + + if (amr_control::OpcUAServerAPI::status_code_ptr_) + { + UA_Variant dobot_status_Variant; + UA_Variant_setScalar(&dobot_status_Variant, amr_control::OpcUAServerAPI::status_code_ptr_, &UA_TYPES[UA_TYPES_UINT32]); + UA_Server_writeValue(amr_control::OpcUAServerAPI::server_ptr_->getServerObject(), *amr_control::OpcUAServerAPI::arm_dobot_status_code_Id_, dobot_status_Variant); + } + } + + if (amr_control::OpcUAServerAPI::arm_dobot_state_en_ == amr_control::State::RUNNING) + { + if (*(amr_control::OpcUAServerAPI::status_code_ptr_) != amr_control::OpcUAServerAPI::old_status_code_ && + *(amr_control::OpcUAServerAPI::status_code_ptr_) == imr_nova_control::ROBOT_DRAG) + { + amr_control::OpcUAServerAPI::resetState(); + } + } + else if (amr_control::OpcUAServerAPI::arm_dobot_state_en_ == amr_control::State::FINISHED) + { + *(amr_control::OpcUAServerAPI::status_code_ptr_) = 0; + } + amr_control::OpcUAServerAPI::old_status_code_ = *(amr_control::OpcUAServerAPI::status_code_ptr_); + } +} + +void amr_control::OpcUAServerAPI::addunLoadMethod(UA_Server *server, UA_NodeId parentID) +{ + /* One output argument */ + UA_Argument outputArguments[2]; + UA_Argument_init(&outputArguments[0]); + outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status"); + outputArguments[0].name = UA_STRING((char *)"Status"); + outputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId; + outputArguments[0].valueRank = UA_VALUERANK_SCALAR; + + UA_Argument_init(&outputArguments[1]); + outputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Message"); + outputArguments[1].name = UA_STRING((char *)"Message"); + outputArguments[1].dataType = UA_TYPES[UA_TYPES_STRING].typeId; + outputArguments[1].valueRank = UA_VALUERANK_SCALAR; + + /* Add the method node */ + UA_MethodAttributes incAttr = UA_MethodAttributes_default; + incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", + (char *)"Chức năng trả hàng"); + incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"UnLoad"); + incAttr.executable = true; + incAttr.userExecutable = true; + UA_Server_addMethodNode(server, + UA_NODEID_STRING(1, (char *)"UnLoad"), + parentID, + UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), + UA_QUALIFIEDNAME(1, (char *)"UnLoad"), + incAttr, &amr_control::OpcUAServerAPI::unLoadCallBack, + 0, NULL, 2, outputArguments, NULL, NULL); +} + +UA_StatusCode amr_control::OpcUAServerAPI::unLoadCallBack(UA_Server *server, + const UA_NodeId *sessionId, void *sessionContext, + const UA_NodeId *methodId, void *methodContext, + const UA_NodeId *objectId, void *objectContext, + size_t inputSize, const UA_Variant *input, + size_t outputSize, UA_Variant *output) +{ + UA_Variant_init(output); + UA_Int32 status = STATUS_SUCCESSED; + UA_String message = UA_STRING_ALLOC((char *)"Lệnh trả hàng gọi thành công"); + + if (!amr_control::OpcUAServerAPI::unLoad_excuted_) + { + UA_Int32 status = STATUS_ERROR; + UA_String message = UA_STRING_ALLOC((char *)"Lỗi khi khởi tạo nhiệm vụ"); + UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lỗi khi khởi tạo nhiệm vụ"); + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lỗi khi khởi tạo nhiệm vụ"); + return UA_STATUSCODE_GOOD; + } + + if (amr_control::OpcUAServerAPI::belt_thread_ptr_ && amr_control::OpcUAServerAPI::belt_thread_ptr_->joinable()) + { + UA_Int32 status = STATUS_ERROR; + UA_String message = UA_STRING_ALLOC((char *)"Bang Tai đang bận"); + UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Bang Tai đang bận"); + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Bang Tai đang bận"); + return UA_STATUSCODE_GOOD; + } + else if (amr_control::OpcUAServerAPI::belt_thread_ptr_ && !amr_control::OpcUAServerAPI::belt_thread_ptr_->joinable()) + { + amr_control::OpcUAServerAPI::unLoad_excuted_(); + } + else + { + UA_Int32 status = STATUS_ERROR; + UA_String message = UA_STRING_ALLOC((char *)"Lỗi khi khởi tạo nhiệm vụ"); + UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lỗi khi khởi tạo nhiệm vụ"); + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lỗi khi khởi tạo nhiệm vụ"); + return UA_STATUSCODE_GOOD; + } + + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh trả hàng gọi thành công"); + return UA_STATUSCODE_GOOD; + +STATUSCODE_BADINTERNALERROR: + status = STATUS_ERROR; + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + return UA_STATUSCODE_BADINTERNALERROR; + +STATUSCODE_BADREQUESTTOOLARGE: + status = STATUS_ERROR; + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + return UA_STATUSCODE_BADREQUESTTOOLARGE; + +STATUSCODE_BADSTRUCTUREMISSING: + status = STATUS_ERROR; + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + return UA_STATUSCODE_BADSTRUCTUREMISSING; + +STATUSCODE_BAD: + status = STATUS_ERROR; + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh trả hàng bị lỗi"); + return UA_STATUSCODE_BAD; +} + +void amr_control::OpcUAServerAPI::addLoadMethod(UA_Server *server, UA_NodeId parentID) +{ + /* One output argument */ + UA_Argument outputArguments[2]; + UA_Argument_init(&outputArguments[0]); + outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status"); + outputArguments[0].name = UA_STRING((char *)"Status"); + outputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId; + outputArguments[0].valueRank = UA_VALUERANK_SCALAR; + + UA_Argument_init(&outputArguments[1]); + outputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Message"); + outputArguments[1].name = UA_STRING((char *)"Message"); + outputArguments[1].dataType = UA_TYPES[UA_TYPES_STRING].typeId; + outputArguments[1].valueRank = UA_VALUERANK_SCALAR; + + /* Add the method node */ + UA_MethodAttributes incAttr = UA_MethodAttributes_default; + incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", + (char *)"Chức năng nhận hàng"); + incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Load"); + incAttr.executable = true; + incAttr.userExecutable = true; + UA_Server_addMethodNode(server, + UA_NODEID_STRING(1, (char *)"Load"), + parentID, + UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), + UA_QUALIFIEDNAME(1, (char *)"Load"), + incAttr, &amr_control::OpcUAServerAPI::loadCallBack, + 0, NULL, 2, outputArguments, NULL, NULL); +} + +UA_StatusCode amr_control::OpcUAServerAPI::loadCallBack(UA_Server *server, + const UA_NodeId *sessionId, void *sessionContext, + const UA_NodeId *methodId, void *methodContext, + const UA_NodeId *objectId, void *objectContext, + size_t inputSize, const UA_Variant *input, + size_t outputSize, UA_Variant *output) +{ + UA_Variant_init(output); + UA_Int32 status = STATUS_SUCCESSED; + UA_String message = UA_STRING_ALLOC((char *)"Lệnh nhận hàng gọi thành công"); + + if (!amr_control::OpcUAServerAPI::load_excuted_) + { + UA_Int32 status = STATUS_ERROR; + UA_String message = UA_STRING_ALLOC((char *)"Lỗi khi khởi tạo nhiệm vụ"); + UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lỗi khi khởi tạo nhiệm vụ"); + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lỗi khi khởi tạo nhiệm vụ"); + return UA_STATUSCODE_GOOD; + } + + if (amr_control::OpcUAServerAPI::belt_thread_ptr_ && amr_control::OpcUAServerAPI::belt_thread_ptr_->joinable()) + { + UA_Int32 status = STATUS_ERROR; + UA_String message = UA_STRING_ALLOC((char *)"Bang Tai đang bận"); + UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Bang Tai đang bận"); + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Bang Tai đang bận"); + return UA_STATUSCODE_GOOD; + } + else if (amr_control::OpcUAServerAPI::belt_thread_ptr_ && !amr_control::OpcUAServerAPI::belt_thread_ptr_->joinable()) + { + amr_control::OpcUAServerAPI::load_excuted_(); + } + else + { + UA_Int32 status = STATUS_ERROR; + UA_String message = UA_STRING_ALLOC((char *)"Lỗi khi khởi tạo nhiệm vụ"); + UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lỗi khi khởi tạo nhiệm vụ"); + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lỗi khi khởi tạo nhiệm vụ"); + return UA_STATUSCODE_GOOD; + } + + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh trả hàng gọi thành công"); + return UA_STATUSCODE_GOOD; + +STATUSCODE_BADINTERNALERROR: + status = STATUS_ERROR; + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + return UA_STATUSCODE_BADINTERNALERROR; + +STATUSCODE_BADREQUESTTOOLARGE: + status = STATUS_ERROR; + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + return UA_STATUSCODE_BADREQUESTTOOLARGE; + +STATUSCODE_BADSTRUCTUREMISSING: + status = STATUS_ERROR; + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + return UA_STATUSCODE_BADSTRUCTUREMISSING; + +STATUSCODE_BAD: + status = STATUS_ERROR; + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh nhận hàng bị lỗi"); + return UA_STATUSCODE_BAD; +} + +void amr_control::OpcUAServerAPI::addResetConveyorBeltMethod(UA_Server *server, UA_NodeId parentID) +{ + /* One output argument */ + UA_Argument outputArguments[2]; + UA_Argument_init(&outputArguments[0]); + outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status"); + outputArguments[0].name = UA_STRING((char *)"Status"); + outputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId; + outputArguments[0].valueRank = UA_VALUERANK_SCALAR; + + UA_Argument_init(&outputArguments[1]); + outputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Message"); + outputArguments[1].name = UA_STRING((char *)"Message"); + outputArguments[1].dataType = UA_TYPES[UA_TYPES_STRING].typeId; + outputArguments[1].valueRank = UA_VALUERANK_SCALAR; + + /* Add the method node */ + UA_MethodAttributes incAttr = UA_MethodAttributes_default; + incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", + (char *)"Chức năng reset"); + incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"ResetBelt"); + incAttr.executable = true; + incAttr.userExecutable = true; + UA_Server_addMethodNode(server, + UA_NODEID_STRING(1, (char *)"ResetBelt"), + parentID, + UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), + UA_QUALIFIEDNAME(1, (char *)"ResetBelt"), + incAttr, &amr_control::OpcUAServerAPI::resetConveyorBeltCallBack, + 0, NULL, 2, outputArguments, NULL, NULL); +} + +UA_StatusCode amr_control::OpcUAServerAPI::resetConveyorBeltCallBack(UA_Server *server, + const UA_NodeId *sessionId, void *sessionContext, + const UA_NodeId *methodId, void *methodContext, + const UA_NodeId *objectId, void *objectContext, + size_t inputSize, const UA_Variant *input, + size_t outputSize, UA_Variant *output) +{ + UA_Variant_init(output); + UA_Int32 status = STATUS_SUCCESSED; + UA_String message = UA_STRING_ALLOC((char *)"Lệnh reset gọi thành công"); + + *amr_control::OpcUAServerAPI::belt_cancel_ = true; + + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh reset gọi thành công"); + return UA_STATUSCODE_GOOD; + +STATUSCODE_BADINTERNALERROR: + status = STATUS_ERROR; + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + return UA_STATUSCODE_BADINTERNALERROR; + +STATUSCODE_BADREQUESTTOOLARGE: + status = STATUS_ERROR; + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + return UA_STATUSCODE_BADREQUESTTOOLARGE; + +STATUSCODE_BADSTRUCTUREMISSING: + status = STATUS_ERROR; + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + return UA_STATUSCODE_BADSTRUCTUREMISSING; + +STATUSCODE_BAD: + status = STATUS_ERROR; + UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); + UA_Int32_clear(&status); + UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); + UA_String_clear(&message); + UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh reset bị lỗi"); + return UA_STATUSCODE_BAD; +} + +UA_NodeId *amr_control::OpcUAServerAPI::belt_status_str_Id_ = new UA_NodeId(); +UA_NodeId *amr_control::OpcUAServerAPI::belt_status_en_Id_ = new UA_NodeId(); +UA_NodeId *amr_control::OpcUAServerAPI::have_goods_Id_ = new UA_NodeId(); +void amr_control::OpcUAServerAPI::addConveyorBeltState(UA_Server *server, UA_NodeId parentID) +{ + + UA_VariableAttributes status_str_Attr = UA_VariableAttributes_default; + UA_String status_str = UA_STRING_ALLOC((char *)"READY"); + UA_Variant_setScalar(&status_str_Attr.value, &status_str, &UA_TYPES[UA_TYPES_STRING]); + status_str_Attr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status String"); + status_str_Attr.description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Trạng thái của Conveyor Belt."); + status_str_Attr.dataType = UA_TYPES[UA_TYPES_STRING].typeId; + UA_Server_addVariableNode(server, UA_NODEID_NULL, parentID, + UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), + UA_QUALIFIEDNAME(1, (char *)"Status String"), + UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE), + status_str_Attr, NULL, amr_control::OpcUAServerAPI::belt_status_str_Id_); + + UA_VariableAttributes status_en_Attr = UA_VariableAttributes_default; + UA_Int32 status_en = UA_Int32(*cur_belt_state_en_); + UA_Variant_setScalar(&status_en_Attr.value, &status_en, &UA_TYPES[UA_TYPES_UINT32]); + status_en_Attr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status Enum"); + status_en_Attr.description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Trạng thái của Conveyor Belt."); + status_en_Attr.dataType = UA_TYPES[UA_TYPES_UINT32].typeId; + UA_Server_addVariableNode(server, UA_NODEID_NULL, parentID, + UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), + UA_QUALIFIEDNAME(1, (char *)"Status Enum"), + UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE), + status_en_Attr, NULL, amr_control::OpcUAServerAPI::belt_status_en_Id_); + + UA_VariableAttributes have_goods_Attr = UA_VariableAttributes_default; + UA_Int32 have_goods = UA_Boolean(false); + UA_Variant_setScalar(&have_goods_Attr.value, &have_goods, &UA_TYPES[UA_TYPES_BOOLEAN]); + have_goods_Attr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Have Goods"); + have_goods_Attr.description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Trạng thái đã nhận hàng chưa"); + have_goods_Attr.dataType = UA_TYPES[UA_TYPES_BOOLEAN].typeId; + UA_Server_addVariableNode(server, UA_NODEID_NULL, parentID, + UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), + UA_QUALIFIEDNAME(1, (char *)"Have Goods"), + UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE), + have_goods_Attr, NULL, amr_control::OpcUAServerAPI::have_goods_Id_); +} + +void amr_control::OpcUAServerAPI::ConveyorBeltHandle() +{ + if (amr_control::OpcUAServerAPI::server_ptr_ != nullptr) + { + switch (*amr_control::OpcUAServerAPI::cur_belt_state_en_) + { + case amr_control::WAITING: + amr_control::OpcUAServerAPI::belt_state_str_ = UA_STRING_ALLOC((char *)"WAITING"); + break; + case amr_control::INITIALIZING: + amr_control::OpcUAServerAPI::belt_state_str_ = UA_STRING_ALLOC((char *)"INITIALIZING"); + break; + case amr_control::RUNNING: + amr_control::OpcUAServerAPI::belt_state_str_ = UA_STRING_ALLOC((char *)"RUNNING"); + break; + case amr_control::PAUSED: + amr_control::OpcUAServerAPI::belt_state_str_ = UA_STRING_ALLOC((char *)"PAUSED"); + break; + case amr_control::FINISHED: + amr_control::OpcUAServerAPI::belt_state_str_ = UA_STRING_ALLOC((char *)"FINISHED"); + break; + case amr_control::FAILED: + amr_control::OpcUAServerAPI::belt_state_str_ = UA_STRING_ALLOC((char *)"FAILED"); + break; + default: + break; + } + + if (amr_control::OpcUAServerAPI::belt_status_str_Id_ != NULL && + amr_control::OpcUAServerAPI::belt_status_en_Id_ != NULL && + amr_control::OpcUAServerAPI::have_goods_Id_ != NULL) + { + UA_Variant statusStrVariant; + UA_Variant_setScalar(&statusStrVariant, &amr_control::OpcUAServerAPI::belt_state_str_, &UA_TYPES[UA_TYPES_STRING]); + UA_Server_writeValue(amr_control::OpcUAServerAPI::server_ptr_->getServerObject(), *amr_control::OpcUAServerAPI::belt_status_str_Id_, statusStrVariant); // Truyền đối tượng, không phải con trỏ + + UA_Variant statusEnVariant; + UA_Variant_setScalar(&statusEnVariant, &amr_control::OpcUAServerAPI::cur_belt_state_en_, &UA_TYPES[UA_TYPES_UINT32]); + UA_Server_writeValue(amr_control::OpcUAServerAPI::server_ptr_->getServerObject(), *amr_control::OpcUAServerAPI::belt_status_en_Id_, statusEnVariant); // Truyền đối tượng, không phải con trỏ + + UA_Variant haveGoodsVariant; + UA_Variant_setScalar(&haveGoodsVariant, &amr_control::OpcUAServerAPI::have_goods_, &UA_TYPES[UA_TYPES_BOOLEAN]); + UA_Server_writeValue(amr_control::OpcUAServerAPI::server_ptr_->getServerObject(), *amr_control::OpcUAServerAPI::have_goods_Id_, haveGoodsVariant); // Truyền đối tượng, không phải con trỏ + } + } +} + +void *amr_control::OpcUAServerAPI::ThreadWorker(void *_) +{ + ros::Rate rate(10); + + while (ros::ok() && amr_control::OpcUAServerAPI::server_ptr_->isRunning()) + { + amr_control::OpcUAServerAPI::slamHandle(); + amr_control::OpcUAServerAPI::monitorHandle(); + amr_control::OpcUAServerAPI::navigationHandle(); + amr_control::OpcUAServerAPI::dobotPropertiesHandle(); + amr_control::OpcUAServerAPI::ConveyorBeltHandle(); + + const UA_AsyncOperationRequest *request = NULL; + void *context = NULL; + UA_AsyncOperationType type; + if (UA_Server_getAsyncOperationNonBlocking(amr_control::OpcUAServerAPI::server_ptr_->getServerObject(), &type, &request, &context, NULL) == true) + { + UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "AsyncMethod_Testing: Got entry: OKAY"); + + UA_CallMethodResult response = UA_Server_call( + amr_control::OpcUAServerAPI::server_ptr_->getServerObject(), &request->callMethodRequest); + + UA_Server_setAsyncOperationResult( + amr_control::OpcUAServerAPI::server_ptr_->getServerObject(), + (UA_AsyncOperationResponse *)&response, context); + + UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "AsyncMethod_Testing: Call done: OKAY"); + UA_CallMethodResult_clear(&response); + } + rate.sleep(); + ros::spinOnce(); + } + return 0; +} + +void amr_control::OpcUAServerAPI::resetState() +{ + *amr_control::OpcUAServerAPI::arm_go_home_ = false; + *amr_control::OpcUAServerAPI::arm_continue_ = false; + *amr_control::OpcUAServerAPI::arm_power_on_ = false; +} + +ros::Publisher amr_control::OpcUAServerAPI::init_pub_; +std::shared_ptr amr_control::OpcUAServerAPI::server_ptr_ = nullptr; +std::shared_ptr amr_control::OpcUAServerAPI::move_base_ptr_ = nullptr; +std::shared_ptr amr_control::OpcUAServerAPI::loc_base_ptr_ = nullptr; +std::shared_ptr amr_control::OpcUAServerAPI::monitor_ptr_ = nullptr; +nav_2d_msgs::Twist2D amr_control::OpcUAServerAPI::cmd_vel_max_; + +std::thread *amr_control::OpcUAServerAPI::arm_thread_ptr_; +std::function amr_control::OpcUAServerAPI::arm_function_ptr_; +amr_control::State amr_control::OpcUAServerAPI::arm_dobot_state_en_; +UA_String amr_control::OpcUAServerAPI::arm_dobot_state_str_; +unsigned int *amr_control::OpcUAServerAPI::count_ng_max_ = new unsigned int; +unsigned int *amr_control::OpcUAServerAPI::count_ok_max_ = new unsigned int; +bool *amr_control::OpcUAServerAPI::arm_cancel_ = new bool; +bool *amr_control::OpcUAServerAPI::arm_continue_ = new bool; +bool *amr_control::OpcUAServerAPI::arm_go_home_ = new bool; +bool *amr_control::OpcUAServerAPI::arm_power_on_ = new bool; +double *amr_control::OpcUAServerAPI::mode_ptr_; +unsigned int *amr_control::OpcUAServerAPI::status_code_ptr_ = new unsigned int(0); +unsigned int amr_control::OpcUAServerAPI::old_status_code_ = 0; + +pthread_t amr_control::OpcUAServerAPI::hThread_; +UA_String amr_control::OpcUAServerAPI::amr_feedback_str_; +UA_String amr_control::OpcUAServerAPI::amr_status_str_; +UA_UInt32 amr_control::OpcUAServerAPI::amr_status_en_; +bool *amr_control::OpcUAServerAPI::muted_value_ = new bool; + +amr_control::State *amr_control::OpcUAServerAPI::cur_belt_state_en_ = new amr_control::State; +UA_String amr_control::OpcUAServerAPI::belt_state_str_; +UA_Boolean amr_control::OpcUAServerAPI::have_goods_; +bool *amr_control::OpcUAServerAPI::belt_cancel_ = new bool; +std::function amr_control::OpcUAServerAPI::unLoad_excuted_; +std::function amr_control::OpcUAServerAPI::load_excuted_; +std::thread *amr_control::OpcUAServerAPI::belt_thread_ptr_; diff --git a/Controllers/Packages/amr_control/src/amr_safety.cpp b/Controllers/Packages/amr_control/src/amr_safety.cpp new file mode 100644 index 0000000..dc90e37 --- /dev/null +++ b/Controllers/Packages/amr_control/src/amr_safety.cpp @@ -0,0 +1,85 @@ +#include "amr_control/amr_safety.h" + +namespace amr_control +{ + AmrSafety::AmrSafety() {} + + AmrSafety::AmrSafety(const ros::NodeHandle &nh) + { + if (!initalized_) + this->init(nh); + } + + AmrSafety::~AmrSafety() {} + + void AmrSafety::init(const ros::NodeHandle &nh) + { + if (!initalized_) + { + + initalized_ = true; + } + } + + bool AmrSafety::getController(std::shared_ptr plc_controller_ptr) + { + if (plc_controller_ptr == nullptr) + return false; + else + { + plc_controller_ptr_ = plc_controller_ptr; + return true; + } + } + + void AmrSafety::safetyHandle( + nav_2d_msgs::Twist2D velocity, nav_2d_msgs::Twist2D cmd_vel_max, nav_2d_msgs::Twist2D &cmd_vel) + { + if (plc_controller_ptr_ == nullptr) + ROS_WARN_THROTTLE(1.0, "[AmrSafety] - PLC controller is null"); + else if (!plc_controller_ptr_->checkConnected()) + ROS_WARN_THROTTLE(1.0, "[AmrSafety] - PLC controller was not conntected"); + else + { + nav_2d_msgs::Twist2D velocity_warn; + velocity_warn.x = 0.15; + velocity_warn.theta = 0.5; + nav_2d_msgs::Twist2D velocity_stop; + + // Dieu khien chuyen vung giam toc va mute + bool zone_warn_cases; + if (velocity.x >= velocity_warn.x) + zone_warn_cases = false; + else if (velocity.x < velocity_warn.x && velocity.x > 0.05) + zone_warn_cases = true; + plc_controller_ptr_->writeM(52, zone_warn_cases); + + // Nhan tin hieu 3 vung tu lidar + bool output_lidar[4]; + plc_controller_ptr_->mulGetM(30, 33, output_lidar); + if (output_lidar[0] && output_lidar[1] && output_lidar[2]) + cmd_vel = velocity_stop; + else if (output_lidar[0] && !output_lidar[1] && output_lidar[2]) + cmd_vel = velocity_warn; + else if (output_lidar[0] && !output_lidar[1] && !output_lidar[2]) + cmd_vel = cmd_vel_max; + else + cmd_vel = velocity_stop; + } + } + + void AmrSafety::writeMutesSafety(bool value) + { + if (plc_controller_ptr_ == nullptr) + ROS_WARN_THROTTLE(1.0, "[AmrSafety] - PLC controller is null"); + else if (!plc_controller_ptr_->checkConnected()) + ROS_WARN_THROTTLE(1.0, "[AmrSafety] - PLC controller was not conntected"); + else + { + bool f_data; + plc_controller_ptr_->getM(53, f_data); + if (f_data != value) + plc_controller_ptr_->writeM(53, value); + } + } +} \ No newline at end of file diff --git a/Controllers/Packages/amr_control/src/amr_vda_5050_client_api.cpp b/Controllers/Packages/amr_control/src/amr_vda_5050_client_api.cpp new file mode 100644 index 0000000..bc62625 --- /dev/null +++ b/Controllers/Packages/amr_control/src/amr_vda_5050_client_api.cpp @@ -0,0 +1,1260 @@ +#include "amr_control/amr_vda_5050_client_api.h" +#include "amr_control/amr_make_plan_with_order.h" +#include +#include +#include "vda5050_msgs/Order.h" + +namespace amr_control +{ + VDA5050ClientAPI::VDA5050ClientAPI() + { + this->client_ptr_ = std::make_shared(); + this->client_ptr_->execute_order_ = std::bind(&VDA5050ClientAPI::orderWorker, this); + this->client_ptr_->execute_instant_action_ = std::bind(&VDA5050ClientAPI::instantActionWorker, this); + + this->cancel_action_ = new bool; + this->pause_action_ = new bool; + this->enable_action_ = new bool; + this->muted_value_ = new bool; + this->count_ng_max_ = new unsigned int(1); + this->count_ok_max_ = new unsigned int(1); + } + + VDA5050ClientAPI::VDA5050ClientAPI(ros::NodeHandle nh, std::shared_ptr move_base, + std::shared_ptr loc_base, std::shared_ptr monitor) + { + VDA5050ClientAPI::move_base_ptr_ = move_base; + VDA5050ClientAPI::loc_base_ptr_ = loc_base; + VDA5050ClientAPI::monitor_ptr_ = monitor; + + this->cancel_action_ = new bool; + this->pause_action_ = new bool; + this->enable_action_ = new bool; + this->muted_value_ = new bool; + + this->client_ptr_ = std::make_shared(); + this->client_ptr_->execute_order_ = std::bind(&VDA5050ClientAPI::orderWorker, this); + this->client_ptr_->execute_instant_action_ = std::bind(&VDA5050ClientAPI::instantActionWorker, this); + this->client_ptr_->execute_parallel_action_ = std::bind(&VDA5050ClientAPI::excuteAction, this); + + this->client_ptr_->velocity_max_action_ = [this](double velocity) + { + this->updateVelocity(velocity); + }; + + this->client_ptr_->angular_max_action_ = [this](double angular) + { + this->updateAngular(angular); + }; + + std::string name; + nh.param("base_global_planner", name, name); + ros::NodeHandle nh_priv = ros::NodeHandle(nh, name); + + ros::NodeHandle nh_control = ros::NodeHandle("~"); + nh_control.param("global_plan_msg_type", global_plan_msg_type_, std::string("vda5050_msgs::Order")); + if (global_plan_msg_type_ == std::string("nav_msgs::Path")) + VDA5050ClientAPI::plan_pub_ = nh_priv.advertise("/plan_with_nav_path", 100); + else if (global_plan_msg_type_ == std::string("vda5050_msgs::Order")) + { + std::string order_topic = nh_priv.getNamespace() + "/order"; + VDA5050ClientAPI::plan_pub_ = nh_priv.advertise(order_topic, 100); + } + + this->update_thread_ = std::thread(std::bind(&VDA5050ClientAPI::updating, this)); + } + + VDA5050ClientAPI::~VDA5050ClientAPI() + { + if (this->arm_thread_ptr_) + { + delete (arm_thread_ptr_); + } + + if (this->belt_thread_ptr_) + { + delete (belt_thread_ptr_); + } + if (client_ptr_) + client_ptr_.reset(); + } + + void VDA5050ClientAPI::orderWorker() + { + if (this->client_ptr_) + { + auto global_order = VDA5050ClientAPI::client_ptr_->vda5050_order_; + + std::string goal_maker; + if (!global_order.nodes.empty()) + { // Kiểm tra map không rỗng + auto lastNodeIt = std::prev(global_order.nodes.end()); // Lấy iterator phần tử cuối + for (auto it = lastNodeIt->actions.begin(); it != lastNodeIt->actions.end(); ++it) + { + if (it->get()->actionType == "DockToStation") + goal_maker = "DockToStation"; + } + } + uint8_t status; + std::string message; + if (goal_maker.empty()) + this->moveTo(global_order, status, message); + else if (goal_maker == "DockToStation") + { + double theta = global_order.nodes.back().nodePosition.theta; + global_order.edges.erase(global_order.edges.end()); + global_order.nodes.erase(global_order.nodes.end()); + global_order.nodes.back().nodePosition.theta = theta; + + if (!global_order.edges.empty() && (unsigned int)global_order.nodes.size()) + this->moveToDock(global_order, status, message); + else + this->rotateTo(global_order, status, message); + } + } + } + + void VDA5050ClientAPI::moveTo(vda_5050::Order order, uint8_t &status, std::string &message) + { + geometry_msgs::PoseStamped goal; + if (VDA5050ClientAPI::move_base_ptr_) + { + if (global_plan_msg_type_ == std::string("nav_msgs::Path")) + { + std::vector paths; + if (MakePlanWithOrder(order, false, status, message, paths)) + { + ROS_INFO_STREAM("MakePlanWithOrder - move forward : " << "status: " << status << ", message: " << message); + } + else if (MakePlanWithOrder(order, true, status, message, paths)) + { + ROS_INFO_STREAM("MakePlanWithOrder - move backward: " << "status: " << status << ", message: " << message); + } + else + { + ROS_WARN_STREAM("MakePlanWithOrder: return false: " << "status: " << status << ", message: " << message); + this->client_ptr_->vda5050_state_.edgeStates.clear(); + this->client_ptr_->vda5050_state_.nodeStates.clear(); + this->client_ptr_->vda5050_state_.actionStates.clear(); + return; + } + if (!paths.empty()) + return; + + std::vector poses; + for (auto &p : paths) + { + geometry_msgs::Pose2D p_2d; + p_2d.x = p.getX(); + p_2d.y = p.getY(); + p_2d.theta = p.getYaw(); + poses.push_back(p_2d); + } + const nav_msgs::Path path = nav_2d_utils::poses2DToPath(poses, "map", ros::Time::now()); + goal = path.poses.back(); + VDA5050ClientAPI::plan_pub_.publish(path); + } + else if (global_plan_msg_type_ == std::string("vda5050_msgs::Order")) + { + vda5050_msgs::Order order_msg; + order_msg.headerId = order.headerId; + order_msg.timestamp = order.timestamp; + order_msg.version = order.version; + order_msg.manufacturer = order.manufacturer; + order_msg.serialNumber = order.serialNumber; + order_msg.orderId = order.orderId; + order_msg.orderUpdateId = order.orderUpdateId; + + for (auto node : order.nodes) + { + vda5050_msgs::Node node_msg; + node_msg.nodeId = node.nodeId; + node_msg.sequenceId = node.sequenceId; + node_msg.nodeDescription = node.nodeDescription; + node_msg.released = node.released; + + node_msg.nodePosition.x = node.nodePosition.x; + node_msg.nodePosition.y = node.nodePosition.y; + node_msg.nodePosition.theta = node.nodePosition.theta; + node_msg.nodePosition.allowedDeviationXY = node.nodePosition.allowedDeviationXY; + node_msg.nodePosition.allowedDeviationTheta = node.nodePosition.allowedDeviationTheta; + node_msg.nodePosition.mapId = node.nodePosition.mapId; + node_msg.nodePosition.mapDescription = node.nodePosition.mapDescription; + // vda5050_msgs/Action[] actions + order_msg.nodes.push_back(node_msg); + } + + for (auto edge : order.edges) + { + vda5050_msgs::Edge edge_msg; + edge_msg.edgeId = edge.edgeId; + edge_msg.sequenceId = edge.sequenceId; + edge_msg.edgeDescription = edge.edgeDescription; + edge_msg.released = edge.released; + edge_msg.startNodeId = edge.startNodeId; + edge_msg.endNodeId = edge.endNodeId; + edge_msg.maxSpeed = edge.maxSpeed; + edge_msg.maxHeight = edge.maxHeight; + edge_msg.minHeight = edge.minHeight; + edge_msg.orientation = edge.orientation; + edge_msg.orientationType = edge.orientationType; + edge_msg.direction = edge.direction; + edge_msg.rotationAllowed = edge.rotationAllowed; + edge_msg.maxRotationSpeed = edge.maxRotationSpeed; + edge_msg.length = edge.length; + edge_msg.trajectory.degree = edge.trajectory.degree; + edge_msg.trajectory.knotVector = edge.trajectory.knotVector; + + for (auto controlPoint : edge.trajectory.controlPoints) + { + vda5050_msgs::ControlPoint controlPoint_msg; + controlPoint_msg.x = controlPoint.x; + controlPoint_msg.y = controlPoint.y; + controlPoint_msg.weight = controlPoint.weight; + edge_msg.trajectory.controlPoints.push_back(controlPoint_msg); + } + // Corridor corridor; + // std::vector actions; + order_msg.edges.push_back(edge_msg); + } + + // order_msg.edges zoneSetId =VDA5050ClientAPI::client_ptr_->vda5050_order_. + VDA5050ClientAPI::plan_pub_.publish(order_msg); + + vda_5050::NodePosition position = order.nodes.back().nodePosition; + goal.header.stamp = ros::Time::now(); + goal.header.frame_id = "map"; + goal.pose.position.x = position.x; + goal.pose.position.y = position.y; + tf::Quaternion quat; + quat.setRPY(0.0, 0.0, position.theta); + tf::quaternionTFToMsg(quat, goal.pose.orientation); + } + else + return; + + ros::Duration(0.1).sleep(); + VDA5050ClientAPI::move_base_ptr_->moveTo(goal); + cmd_vel_max_.x = 0.2; + cmd_vel_max_.theta = 0.5; + cmd_vel_max_saved_ = cmd_vel_max_; + + geometry_msgs::Vector3 linear; + linear.x = cmd_vel_max_.x; + VDA5050ClientAPI::move_base_ptr_->setTwistLinear(linear); + linear.x = -cmd_vel_max_.x; + VDA5050ClientAPI::move_base_ptr_->setTwistLinear(linear); + + geometry_msgs::Vector3 angular; + angular.z = cmd_vel_max_.theta; + VDA5050ClientAPI::move_base_ptr_->setTwistAngular(angular); + + } + } + + void VDA5050ClientAPI::moveToDock(vda_5050::Order order, uint8_t &status, std::string &message) + { + geometry_msgs::PoseStamped goal; + if (VDA5050ClientAPI::move_base_ptr_) + { + if (global_plan_msg_type_ == std::string("nav_msgs::Path")) + { + std::vector paths; + if (MakePlanWithOrder(order, false, status, message, paths)) + { + ROS_INFO_STREAM("MakePlanWithOrder - move forward : " << "status: " << status << ", message: " << message); + } + else if (MakePlanWithOrder(order, true, status, message, paths)) + { + ROS_INFO_STREAM("MakePlanWithOrder - move backward: " << "status: " << status << ", message: " << message); + } + else + { + ROS_WARN_STREAM("MakePlanWithOrder: return false: " << "status: " << status << ", message: " << message); + this->client_ptr_->vda5050_state_.edgeStates.clear(); + this->client_ptr_->vda5050_state_.nodeStates.clear(); + this->client_ptr_->vda5050_state_.actionStates.clear(); + return; + } + if (!paths.empty()) + return; + + std::vector poses; + for (auto &p : paths) + { + geometry_msgs::Pose2D p_2d; + p_2d.x = p.getX(); + p_2d.y = p.getY(); + p_2d.theta = p.getYaw(); + poses.push_back(p_2d); + } + const nav_msgs::Path path = nav_2d_utils::poses2DToPath(poses, "map", ros::Time::now()); + goal = path.poses.back(); + VDA5050ClientAPI::plan_pub_.publish(path); + } + else if (global_plan_msg_type_ == std::string("vda5050_msgs::Order")) + { + vda5050_msgs::Order order_msg; + order_msg.headerId = order.headerId; + order_msg.timestamp = order.timestamp; + order_msg.version = order.version; + order_msg.manufacturer = order.manufacturer; + order_msg.serialNumber = order.serialNumber; + order_msg.orderId = order.orderId; + order_msg.orderUpdateId = order.orderUpdateId; + + for (auto node : order.nodes) + { + vda5050_msgs::Node node_msg; + node_msg.nodeId = node.nodeId; + node_msg.sequenceId = node.sequenceId; + node_msg.nodeDescription = node.nodeDescription; + node_msg.released = node.released; + + node_msg.nodePosition.x = node.nodePosition.x; + node_msg.nodePosition.y = node.nodePosition.y; + node_msg.nodePosition.theta = node.nodePosition.theta; + node_msg.nodePosition.allowedDeviationXY = node.nodePosition.allowedDeviationXY; + node_msg.nodePosition.allowedDeviationTheta = node.nodePosition.allowedDeviationTheta; + node_msg.nodePosition.mapId = node.nodePosition.mapId; + node_msg.nodePosition.mapDescription = node.nodePosition.mapDescription; + // vda5050_msgs/Action[] actions + order_msg.nodes.push_back(node_msg); + } + + for (auto edge : order.edges) + { + vda5050_msgs::Edge edge_msg; + edge_msg.edgeId = edge.edgeId; + edge_msg.sequenceId = edge.sequenceId; + edge_msg.edgeDescription = edge.edgeDescription; + edge_msg.released = edge.released; + edge_msg.startNodeId = edge.startNodeId; + edge_msg.endNodeId = edge.endNodeId; + edge_msg.maxSpeed = edge.maxSpeed; + edge_msg.maxHeight = edge.maxHeight; + edge_msg.minHeight = edge.minHeight; + edge_msg.orientation = edge.orientation; + edge_msg.orientationType = edge.orientationType; + edge_msg.direction = edge.direction; + edge_msg.rotationAllowed = edge.rotationAllowed; + edge_msg.maxRotationSpeed = edge.maxRotationSpeed; + edge_msg.length = edge.length; + edge_msg.trajectory.degree = edge.trajectory.degree; + edge_msg.trajectory.knotVector = edge.trajectory.knotVector; + + for (auto controlPoint : edge.trajectory.controlPoints) + { + vda5050_msgs::ControlPoint controlPoint_msg; + controlPoint_msg.x = controlPoint.x; + controlPoint_msg.y = controlPoint.y; + controlPoint_msg.weight = controlPoint.weight; + edge_msg.trajectory.controlPoints.push_back(controlPoint_msg); + } + // Corridor corridor; + // std::vector actions; + order_msg.edges.push_back(edge_msg); + } + + VDA5050ClientAPI::plan_pub_.publish(order_msg); + + vda_5050::NodePosition position = order.nodes.back().nodePosition; + goal.header.stamp = ros::Time::now(); + goal.header.frame_id = "map"; + goal.pose.position.x = position.x; + goal.pose.position.y = position.y; + tf::Quaternion quat; + quat.setRPY(0.0, 0.0, position.theta); + tf::quaternionTFToMsg(quat, goal.pose.orientation); + } + else + return; + + ros::Duration(0.1).sleep(); + VDA5050ClientAPI::move_base_ptr_->dockTo("trolley", goal, 0.2, 0.1); + cmd_vel_max_.x = 0.2; + cmd_vel_max_.theta = 0.5; + cmd_vel_max_saved_ = cmd_vel_max_; + + geometry_msgs::Vector3 linear; + linear.x = cmd_vel_max_.x; + VDA5050ClientAPI::move_base_ptr_->setTwistLinear(linear); + linear.x = -cmd_vel_max_.x; + VDA5050ClientAPI::move_base_ptr_->setTwistLinear(linear); + + geometry_msgs::Vector3 angular; + angular.z = cmd_vel_max_.theta; + VDA5050ClientAPI::move_base_ptr_->setTwistAngular(angular); + + + } + } + + void VDA5050ClientAPI::rotateTo(vda_5050::Order order, uint8_t &status, std::string &message) + { + geometry_msgs::PoseStamped goal; + + if (VDA5050ClientAPI::move_base_ptr_) + { + if (VDA5050ClientAPI::move_base_ptr_->getRobotPose(goal)) + { + vda_5050::NodePosition position = order.nodes.back().nodePosition; + goal.header.stamp = ros::Time::now(); + goal.header.frame_id = "map"; + tf::Quaternion quat; + quat.setRPY(0.0, 0.0, position.theta); + tf::quaternionTFToMsg(quat, goal.pose.orientation); + VDA5050ClientAPI::move_base_ptr_->rotateTo(goal); + } + else + return; + } + } + + void VDA5050ClientAPI::instantActionWorker() + { + + if (this->client_ptr_) + { + auto ¤t_action_states = this->client_ptr_->vda5050_state_.actionStates; + auto ¤t_actions = this->client_ptr_->vda5050_instant_action_.actions; + + for (auto action : current_actions) + { + auto same_action_state = + std::find_if(current_action_states.begin(), current_action_states.end(), + [&action](const std::shared_ptr &as) + { + return action->actionType == as->actionType; + }); + if (same_action_state != current_action_states.end()) + { + if (same_action_state->get()->actionStatus == vda_5050::ActionState::ActionStatus::INITIALIZING || + same_action_state->get()->actionStatus == vda_5050::ActionState::ActionStatus::RUNNING || + same_action_state->get()->actionStatus == vda_5050::ActionState::ActionStatus::WAITING) + continue; + } + + std::shared_ptr action_state = std::make_shared(); + action_state->actionDescription = action->actionDescription; + action_state->actionId = action->actionId; + action_state->actionType = action->actionType; + action_state->actionStatus = vda_5050::ActionState::ActionStatus::WAITING; + + *this->enable_action_ = true; + *this->cancel_action_ = false; + if (action_state->actionType == std::string("cancelOrder")) + { + current_action_states.push_back(action_state); + this->cancelOrderCallBack(action_state); + } + else if (action_state->actionType.find("UnDockFromStation") != std::string::npos) + { + current_action_states.push_back(action_state); + this->excuteNoneAction(&VDA5050ClientAPI::unDockFromStation, this, action, action_state); + } + else if (action_state->actionType == std::string("UnLoad")) + { + current_action_states.push_back(action_state); + this->excuteNoneAction(&VDA5050ClientAPI::unLoad, this, action, action_state); + } + else if (action_state->actionType == std::string("Load")) + { + current_action_states.push_back(action_state); + this->excuteNoneAction(&VDA5050ClientAPI::load, this, action, action_state); + } + else if (action_state->actionType == std::string("MutedOn")) + { + current_action_states.push_back(action_state); + this->excuteNoneAction(&VDA5050ClientAPI::mutedSafetyON, this, action, action_state); + } + else if (action_state->actionType == std::string("MutedOff")) + { + current_action_states.push_back(action_state); + this->excuteNoneAction(&VDA5050ClientAPI::mutedSafetyOFF, this, action, action_state); + } + else if (action_state->actionType == std::string("PickUp")) + { + current_action_states.push_back(action_state); + this->excuteNoneAction(&VDA5050ClientAPI::pickUp, this, action, action_state); + } + } + } + } + + void VDA5050ClientAPI::excuteAction() + { + if (this->client_ptr_) + { + if (this->client_ptr_->parallel_execution_list_.empty()) + return; + auto &curent_state = this->client_ptr_->vda5050_state_; + for (auto &action : this->client_ptr_->parallel_execution_list_) + { + std::shared_ptr action_state = std::make_shared(); + action_state->actionDescription = action.second->actionDescription; + action_state->actionId = action.second->actionId; + action_state->actionType = action.second->actionType; + action_state->actionStatus = vda_5050::ActionState::ActionStatus::WAITING; + + if (action_state->actionType == std::string("MutedOn")) + { + curent_state.actionStates.push_back(action_state); + this->excuteNoneAction(&VDA5050ClientAPI::mutedSafetyON, this, action.second, action_state); + } + else if (action_state->actionType == std::string("MutedOff")) + { + curent_state.actionStates.push_back(action_state); + this->excuteNoneAction(&VDA5050ClientAPI::mutedSafetyOFF, this, action.second, action_state); + } + else if (action_state->actionType == std::string("DockToStation")) + { + curent_state.actionStates.push_back(action_state); + this->excuteNoneAction(&VDA5050ClientAPI::dockToStaton, this, action.second, action_state); + } + } + + this->client_ptr_->parallel_execution_list_.erase( + this->client_ptr_->parallel_execution_list_.begin(), this->client_ptr_->parallel_execution_list_.end()); + } + } + + void VDA5050ClientAPI::updateVelocity(double velocity) + { + if (this->client_ptr_ && VDA5050ClientAPI::move_base_ptr_ && VDA5050ClientAPI::move_base_ptr_->nav_feedback_) + { + auto &nav_state = VDA5050ClientAPI::move_base_ptr_->nav_feedback_->navigation_state; + if (nav_state == move_base_core::State::CONTROLLING) + { + if (fabs(velocity) > 2.0) + velocity = (fabs(velocity) / velocity) * 2.0; + + if (velocity != cmd_vel_max_saved_.x) + { + cmd_vel_max_.x = velocity; + cmd_vel_max_saved_.x = cmd_vel_max_.x; + ROS_INFO("Update velocity %f", velocity); + } + } + } + } + + void VDA5050ClientAPI::updateAngular(double angular) + { + if (this->client_ptr_ && VDA5050ClientAPI::move_base_ptr_ && VDA5050ClientAPI::move_base_ptr_->nav_feedback_) + { + auto &nav_state = VDA5050ClientAPI::move_base_ptr_->nav_feedback_->navigation_state; + if (nav_state == move_base_core::State::CONTROLLING) + { + if (fabs(angular) > 1.0) + angular = (fabs(angular) / angular) * 1.0; + + if (angular != cmd_vel_max_saved_.theta) + { + cmd_vel_max_.theta = angular; + cmd_vel_max_saved_.theta = cmd_vel_max_.theta; + ROS_INFO("Update angular %f", angular); + } + } + } + } + + void VDA5050ClientAPI::cancelOrderCallBack(std::shared_ptr action_state) + { + if (this->client_ptr_) + { + // std::lock_guard lock(this->client_ptr_->state_mutex); + auto &global_state = this->client_ptr_->vda5050_state_; + + if (global_state.actionStates.empty()) + return; + + std::map>::iterator> other_cancel_action_state_mp; + for (auto as = global_state.actionStates.begin(); as != global_state.actionStates.end(); ++as) + { + if (as->get()->actionType == "cancelOrder" && as->get()->actionId != action_state->actionId) + { + other_cancel_action_state_mp.insert({as->get()->actionId, as}); + } + } + + if (action_state) + { + if (auto it = threads_map_.find(action_state->actionType); it != threads_map_.end()) + { + if (it->second.joinable()) + it->second.join(); + std::lock_guard lock(threads_mutex); + VDA5050ClientAPI::threads_map_.erase(it); + } + if (!other_cancel_action_state_mp.empty()) + { + bool prevours_order = false; + for (auto other : other_cancel_action_state_mp) + { + if (other.second->get()->actionStatus == vda_5050::ActionState::ActionStatus::FINISHED || + other.second->get()->actionStatus == vda_5050::ActionState::ActionStatus::FAILED) + { + prevours_order = true; + } + } + if (prevours_order) + { + for (auto other : other_cancel_action_state_mp) + { + global_state.actionStates.erase(other.second); + } + } + } + if (*this->cancel_action_) + { + action_state->actionStatus = vda_5050::ActionState::ActionStatus::FAILED; + action_state->resultDescription = "The previous cancelOrder was canceled"; + return; + } + + ROS_WARN("Created action [%s]: %s", action_state->actionId.c_str(), action_state->actionType.c_str()); + VDA5050ClientAPI::threads_map_.insert({action_state->actionType, + std::thread(&VDA5050ClientAPI::cancelOrder, this, action_state)}); + } + } + } + + void VDA5050ClientAPI::cancelOrder(std::shared_ptr action_state) + { + if (this->client_ptr_) + { + if (action_state->actionStatus == vda_5050::ActionState::ActionStatus::WAITING) + action_state->actionStatus = vda_5050::ActionState::ActionStatus::INITIALIZING; + // std::lock_guard lock(this->client_ptr_->state_mutex); + auto &global_state = this->client_ptr_->vda5050_state_; + + if (global_state.actionStates.size() < 1 && global_state.orderId.empty()) + { + action_state->actionStatus = vda_5050::ActionState::ActionStatus::FAILED; + action_state->resultDescription = "AGV has no order"; + vda_5050::Error error; + error.errorType = "noOrderToCancel"; + error.errorLevel = vda_5050::Error::ErrorLevel::WARNING; + error.errorDescription = "AGV has no order"; + global_state.errors.push_back(error); + } + else + { + action_state->actionStatus = vda_5050::ActionState::ActionStatus::RUNNING; + + // Can runing actions be interrupted ? + if (VDA5050ClientAPI::move_base_ptr_ != nullptr && + VDA5050ClientAPI::move_base_ptr_->nav_feedback_ != nullptr) + { + VDA5050ClientAPI::move_base_ptr_->cancel(); + *this->cancel_action_ = true; + *this->enable_action_ = false; + *this->pause_action_ = false; + ros::Rate r(5); + while (ros::ok() && + VDA5050ClientAPI::move_base_ptr_->nav_feedback_->navigation_state == move_base_core::State::CONTROLLING) + { + ROS_INFO_THROTTLE(1.0, "Waiting to cancel"); + r.sleep(); + ros::spinOnce(); + } + global_state.nodeStates.clear(); + global_state.edgeStates.clear(); + action_state->actionStatus = vda_5050::ActionState::ActionStatus::FINISHED; + action_state->resultDescription = "cancelOrder is successed"; + // ros::Duration(0.5); + // this->resetState(); + } + else + { + action_state->actionStatus = vda_5050::ActionState::ActionStatus::FAILED; + action_state->resultDescription = "AGV can not cancel when moving"; + } + } + } + } + + template + void VDA5050ClientAPI::excuteNoneAction( + void (T::*excute)(std::shared_ptr, std::shared_ptr), T *obj, + std::shared_ptr action, std::shared_ptr action_state) + { + if (this->client_ptr_) + { + // std::lock_guard lock(this->client_ptr_->state_mutex); + auto &global_state = this->client_ptr_->vda5050_state_; + + if (global_state.actionStates.empty()) + return; + + std::vector>::iterator> other_action_state_vt; + for (auto as = global_state.actionStates.begin(); as != global_state.actionStates.end(); ++as) + { + if (as->get()->actionId == action_state->actionType && + (as->get()->actionStatus == vda_5050::ActionState::ActionStatus::FAILED || as->get()->actionStatus == vda_5050::ActionState::ActionStatus::FINISHED)) + other_action_state_vt.push_back(as); + } + if (!other_action_state_vt.empty()) + { + for (auto &erase_as : other_action_state_vt) + global_state.actionStates.erase(erase_as); + } + + if (auto it = threads_map_.find(action_state->actionType); it != threads_map_.end()) + { + if (it->second.joinable()) + it->second.join(); + std::lock_guard lock(threads_mutex); + VDA5050ClientAPI::threads_map_.erase(it); + } + VDA5050ClientAPI::threads_map_.insert({action_state->actionType, std::thread(excute, obj, action, action_state)}); + } + } + + void VDA5050ClientAPI::unDockFromStation(std::shared_ptr action, std::shared_ptr action_state) + { + if (this->client_ptr_ && VDA5050ClientAPI::move_base_ptr_ && VDA5050ClientAPI::move_base_ptr_->nav_feedback_) + { + action_state->actionStatus = vda_5050::ActionState::ActionStatus::INITIALIZING; + ros::Rate r(5); + auto &nav_state = VDA5050ClientAPI::move_base_ptr_->nav_feedback_->navigation_state; + while (ros::ok() && !*this->cancel_action_ && nav_state == move_base_core::State::CONTROLLING) + { + r.sleep(); + ros::spinOnce(); + } + + geometry_msgs::PoseStamped pose; + if (!VDA5050ClientAPI::move_base_ptr_->getRobotPose(pose)) + { + action_state->actionStatus = vda_5050::ActionState::ActionStatus::FAILED; + action_state->resultDescription = "AGV is lost localization"; + return; + } + auto distance_param = std::find_if(action->actionParameters.begin(), action->actionParameters.end(), + [](const vda_5050::ActionParameter param) + { + return param.key == "distance"; + }); + double distance; + if (distance_param != action->actionParameters.end()) + { + try + { + distance = std::stod(distance_param->value); + } + catch (const std::invalid_argument &e) + { + action_state->actionStatus = vda_5050::ActionState::ActionStatus::FAILED; + std::stringstream ss; + ss << "Invalid argument: " << distance_param->key; + action_state->resultDescription = ss.str(); + return; + } + catch (const std::out_of_range &e) + { + action_state->actionStatus = vda_5050::ActionState::ActionStatus::FAILED; + std::stringstream ss; + ss << "Out of range: " << distance_param->key; + action_state->resultDescription = ss.str(); + return; + } + } + else + { + action_state->actionStatus = vda_5050::ActionState::ActionStatus::FAILED; + action_state->resultDescription = "No config 'distance' argument"; + return; + } + + auto velocity_param = std::find_if(action->actionParameters.begin(), action->actionParameters.end(), + [](const vda_5050::ActionParameter param) + { + return param.key == "velocity"; + }); + double velocity; + if (velocity_param != action->actionParameters.end()) + { + try + { + velocity = std::stod(velocity_param->value); + if (fabs(velocity) > 0.3) + { + action_state->actionStatus = vda_5050::ActionState::ActionStatus::FAILED; + action_state->resultDescription = "'velocity' argument must be smaller than 0.3 (m/s)"; + return; + } + } + catch (const std::invalid_argument &e) + { + action_state->actionStatus = vda_5050::ActionState::ActionStatus::FAILED; + std::stringstream ss; + ss << "Invalid argument: " << velocity_param->key; + return; + } + catch (const std::out_of_range &e) + { + action_state->actionStatus = vda_5050::ActionState::ActionStatus::FAILED; + std::stringstream ss; + ss << "Out of range: " << velocity_param->key; + return; + } + } + else + { + action_state->actionStatus = vda_5050::ActionState::ActionStatus::FAILED; + action_state->resultDescription = "No config 'velocity' argument"; + return; + } + + geometry_msgs::PoseStamped goal = move_base_core::offset_goal(pose, distance); + VDA5050ClientAPI::move_base_ptr_->moveStraightTo(goal); + geometry_msgs::Vector3 linear; + linear.x = fabs(velocity); + VDA5050ClientAPI::move_base_ptr_->setTwistLinear(linear); + linear.x = -fabs(velocity); + VDA5050ClientAPI::move_base_ptr_->setTwistLinear(linear); + + while (ros::ok() && !*this->cancel_action_) + { + if (nav_state == move_base_core::State::CONTROLLING) + break; + r.sleep(); + ros::spinOnce(); + } + + while (ros::ok() && !*this->cancel_action_) + { + action_state->actionStatus = vda_5050::ActionState::ActionStatus::RUNNING; + if (nav_state == move_base_core::State::SUCCEEDED || + nav_state == move_base_core::State::ABORTED || + nav_state == move_base_core::State::PREEMPTED) + break; + r.sleep(); + ros::spinOnce(); + } + + if (nav_state == move_base_core::State::SUCCEEDED) + { + action_state->actionStatus = vda_5050::ActionState::ActionStatus::FINISHED; + action_state->resultDescription = "Done"; + } + else + { + action_state->actionStatus = vda_5050::ActionState::ActionStatus::FAILED; + action_state->resultDescription = "Failed by cancel order"; + } + // this->resetState(); + } + } + + void VDA5050ClientAPI::pickUp(std::shared_ptr action, std::shared_ptr action_state) + { + if (this->client_ptr_ && VDA5050ClientAPI::move_base_ptr_ && VDA5050ClientAPI::move_base_ptr_->nav_feedback_) + { + ros::Rate r(5); + auto &nav_state = VDA5050ClientAPI::move_base_ptr_->nav_feedback_->navigation_state; + while (ros::ok() && nav_state == move_base_core::State::CONTROLLING) + { + r.sleep(); + ros::spinOnce(); + } + + auto count_ok_param = std::find_if(action->actionParameters.begin(), action->actionParameters.end(), + [](const vda_5050::ActionParameter param) + { + return param.key == "count_ok"; + }); + unsigned int count_ok; + if (count_ok_param != action->actionParameters.end()) + { + try + { + count_ok = std::stod(count_ok_param->value); + } + catch (const std::invalid_argument &e) + { + action_state->actionStatus = vda_5050::ActionState::ActionStatus::FAILED; + std::stringstream ss; + ss << "Invalid argument: " << count_ok_param->key; + action_state->resultDescription = ss.str(); + return; + } + catch (const std::out_of_range &e) + { + action_state->actionStatus = vda_5050::ActionState::ActionStatus::FAILED; + std::stringstream ss; + ss << "Out of range: " << count_ok_param->key; + action_state->resultDescription = ss.str(); + return; + } + } + else + { + action_state->actionStatus = vda_5050::ActionState::ActionStatus::FAILED; + action_state->resultDescription = "No config 'count_ok' argument"; + return; + } + + auto count_ng_param = std::find_if(action->actionParameters.begin(), action->actionParameters.end(), + [](const vda_5050::ActionParameter param) + { + return param.key == "count_ng"; + }); + double count_ng; + if (count_ng_param != action->actionParameters.end()) + { + try + { + count_ng = std::stod(count_ng_param->value); + } + catch (const std::invalid_argument &e) + { + action_state->actionStatus = vda_5050::ActionState::ActionStatus::FAILED; + std::stringstream ss; + ss << "Invalid argument: " << count_ng_param->key; + return; + } + catch (const std::out_of_range &e) + { + action_state->actionStatus = vda_5050::ActionState::ActionStatus::FAILED; + std::stringstream ss; + ss << "Out of range: " << count_ng_param->key; + return; + } + } + else + { + action_state->actionStatus = vda_5050::ActionState::ActionStatus::FAILED; + action_state->resultDescription = "No config 'count_ng' argument"; + return; + } + + action_state->actionStatus = vda_5050::ActionState::ActionStatus::INITIALIZING; + *count_ok_max_ = count_ok; + *count_ng_max_ = count_ng; + if (!this->arm_function_ptr_) + { + action_state->actionStatus = vda_5050::ActionState::ActionStatus::FAILED; + action_state->resultDescription = "Config is error"; + } + if (this->arm_thread_ptr_ && this->arm_thread_ptr_->joinable()) + { + action_state->actionStatus = vda_5050::ActionState::ActionStatus::FAILED; + action_state->resultDescription = "Arm is busy"; + } + else if (this->arm_thread_ptr_ && !this->arm_thread_ptr_->joinable()) + { + ROS_INFO("Dobot is running..."); + this->arm_function_ptr_(); + + ros::Rate r(5); + while (ros::ok() && !*this->cancel_action_) + { + action_state->actionStatus = vda_5050::ActionState::ActionStatus::RUNNING; + if (!this->arm_thread_ptr_->joinable()) + break; + r.sleep(); + ros::spinOnce(); + } + if (*this->cancel_action_) + { + action_state->actionStatus = vda_5050::ActionState::ActionStatus::FAILED; + action_state->resultDescription = "Failed by cancel order"; + } + } + else + { + action_state->actionStatus = vda_5050::ActionState::ActionStatus::FAILED; + action_state->resultDescription = "Config is error"; + } + + action_state->actionStatus = vda_5050::ActionState::ActionStatus::FINISHED; + action_state->resultDescription = "Done"; + // this->resetState(); + } + } + + void VDA5050ClientAPI::dockToStaton(std::shared_ptr action, std::shared_ptr action_state) + { + if (this->client_ptr_ && VDA5050ClientAPI::move_base_ptr_ && VDA5050ClientAPI::move_base_ptr_->nav_feedback_) + { + action_state->actionStatus = vda_5050::ActionState::ActionStatus::INITIALIZING; + ros::Rate r(5); + auto &nav_state = VDA5050ClientAPI::move_base_ptr_->nav_feedback_->navigation_state; + while (ros::ok() && nav_state == move_base_core::State::CONTROLLING) + { + action_state->actionStatus = vda_5050::ActionState::ActionStatus::RUNNING; + r.sleep(); + ros::spinOnce(); + } + if (nav_state == move_base_core::State::SUCCEEDED) + { + action_state->actionStatus = vda_5050::ActionState::ActionStatus::FINISHED; + action_state->resultDescription = "Done"; + } + else + action_state->actionStatus = vda_5050::ActionState::ActionStatus::FAILED; + // this->resetState(); + } + } + + void VDA5050ClientAPI::unLoad(std::shared_ptr action, std::shared_ptr action_state) + { + ROS_INFO("UnLoad running"); + if (this->client_ptr_ && VDA5050ClientAPI::move_base_ptr_ && VDA5050ClientAPI::move_base_ptr_->nav_feedback_) + { + ros::Rate r(5); + auto &nav_state = VDA5050ClientAPI::move_base_ptr_->nav_feedback_->navigation_state; + while (ros::ok() && !*enable_action_ && !*this->cancel_action_) + { + if (nav_state == move_base_core::State::ABORTED || + nav_state == move_base_core::State::REJECTED) + { + if (action_state != nullptr) + { + action_state->actionStatus = vda_5050::ActionState::ActionStatus::FAILED; + action_state->resultDescription = VDA5050ClientAPI::move_base_ptr_->nav_feedback_->feed_back_str; + } + return; + } + if (nav_state == move_base_core::State::SUCCEEDED) + break; + r.sleep(); + } + + if (!this->unLoad_excuted_) + { + if (action_state != nullptr) + { + action_state->actionStatus = vda_5050::ActionState::ActionStatus::FAILED; + action_state->resultDescription = "Config Error"; + } + return; + } + if (this->belt_thread_ptr_ && this->belt_thread_ptr_->joinable()) + { + if (action_state != nullptr) + { + action_state->actionStatus = vda_5050::ActionState::ActionStatus::FAILED; + action_state->resultDescription = "The belt is not be Finished by before action"; + } + } + else if (this->belt_thread_ptr_ && !this->belt_thread_ptr_->joinable()) + { + this->unLoad_excuted_(); + } + else + { + if (action_state != nullptr) + { + action_state->actionStatus = vda_5050::ActionState::ActionStatus::FAILED; + action_state->resultDescription = "Config Error"; + } + } + while (ros::ok() && !*this->cancel_action_) + { + if (action_state != nullptr) + action_state->actionStatus = static_cast(*this->cur_belt_state_en_); + if (*this->cur_belt_state_en_ == amr_control::State::FAILED || *this->cur_belt_state_en_ == amr_control::State::FINISHED) + { + break; + } + r.sleep(); + ros::spinOnce(); + } + if (*this->cancel_action_) + { + action_state->actionStatus = vda_5050::ActionState::ActionStatus::FAILED; + action_state->resultDescription = "Failed by cancel order"; + } + else if (action_state != nullptr) + { + action_state->actionStatus = vda_5050::ActionState::ActionStatus::FINISHED; + action_state->resultDescription = "Done"; + } + // this->resetState(); + } + } + + void VDA5050ClientAPI::load(std::shared_ptr action, std::shared_ptr action_state) + { + ROS_INFO("Load running"); + if (this->client_ptr_ && VDA5050ClientAPI::move_base_ptr_ && VDA5050ClientAPI::move_base_ptr_->nav_feedback_) + { + ros::Rate r(5); + auto &nav_state = VDA5050ClientAPI::move_base_ptr_->nav_feedback_->navigation_state; + while (ros::ok() && !*enable_action_ && !*this->cancel_action_) + { + if (nav_state == move_base_core::State::ABORTED || + nav_state == move_base_core::State::REJECTED) + { + if (action_state != nullptr) + { + action_state->actionStatus = vda_5050::ActionState::ActionStatus::FAILED; + action_state->resultDescription = VDA5050ClientAPI::move_base_ptr_->nav_feedback_->feed_back_str; + } + return; + } + if (nav_state == move_base_core::State::SUCCEEDED) + break; + r.sleep(); + } + + if (!this->load_excuted_) + { + if (action_state != nullptr) + { + action_state->actionStatus = vda_5050::ActionState::ActionStatus::FAILED; + action_state->resultDescription = "Config Error"; + } + return; + } + if (this->belt_thread_ptr_ && this->belt_thread_ptr_->joinable()) + { + if (action_state != nullptr) + { + action_state->actionStatus = vda_5050::ActionState::ActionStatus::INITIALIZING; + action_state->resultDescription = "The belt is not be Finished by before action"; + } + } + else if (this->belt_thread_ptr_ && !this->belt_thread_ptr_->joinable()) + { + this->load_excuted_(); + } + + while (ros::ok() && !*this->cancel_action_) + { + if (action_state != nullptr) + action_state->actionStatus = static_cast(*this->cur_belt_state_en_); + if (*this->cur_belt_state_en_ == amr_control::State::FAILED || *this->cur_belt_state_en_ == amr_control::State::FINISHED) + { + break; + } + r.sleep(); + ros::spinOnce(); + } + if (*this->cancel_action_) + { + action_state->actionStatus = vda_5050::ActionState::ActionStatus::FAILED; + action_state->resultDescription = "Failed by cancel order"; + } + else if (action_state != nullptr) + { + action_state->actionStatus = vda_5050::ActionState::ActionStatus::FINISHED; + action_state->resultDescription = "Done"; + } + // this->resetState(); + } + } + + void VDA5050ClientAPI::mutedSafetyON(std::shared_ptr action, std::shared_ptr action_state) + { + *this->muted_value_ = true; + action_state->actionStatus = vda_5050::ActionState::ActionStatus::FINISHED; + action_state->resultDescription = "Muted is On"; + } + + void VDA5050ClientAPI::mutedSafetyOFF(std::shared_ptr action, std::shared_ptr action_state) + { + *this->muted_value_ = false; + action_state->actionStatus = vda_5050::ActionState::ActionStatus::FINISHED; + action_state->resultDescription = "Muted is Off"; + } + + void VDA5050ClientAPI::updating() + { + ros::Rate r(10); + while (ros::ok) + { + if (this->client_ptr_) + { + this->updatingvda5050Visualization(); + this->updatingvda5050State(); + } + r.sleep(); + ros::spinOnce(); + } + } + + void VDA5050ClientAPI::updatingvda5050Visualization() + { + std::lock_guard lock(this->client_ptr_->visualization_mutex); + + auto &global_visualization = this->client_ptr_->vda5050_visualization_; + if (VDA5050ClientAPI::move_base_ptr_) + { + geometry_msgs::Pose2D robot_pose; + if (VDA5050ClientAPI::move_base_ptr_->getRobotPose(robot_pose)) + { + global_visualization.agvPosition.x = robot_pose.x; + global_visualization.agvPosition.y = robot_pose.y; + global_visualization.agvPosition.theta = robot_pose.theta; + } + } + + if (VDA5050ClientAPI::monitor_ptr_) + { + nav_2d_msgs::Twist2D velocity; + if (VDA5050ClientAPI::monitor_ptr_->getVelocity(velocity)) + { + global_visualization.velocity.vx = std::round(velocity.x * 1000.0) / 1000.0; + global_visualization.velocity.vy = std::round(velocity.y * 1000.0) / 1000.0; + global_visualization.velocity.omega = std::round(velocity.theta * 1000.0) / 1000.0; + } + } + } + + void VDA5050ClientAPI::updatingvda5050State() + { + std::lock_guard lock(this->client_ptr_->state_mutex); + auto &global_state = this->client_ptr_->vda5050_state_; + if (VDA5050ClientAPI::move_base_ptr_ != nullptr && + VDA5050ClientAPI::move_base_ptr_->nav_feedback_ != nullptr) + { + geometry_msgs::Pose2D robot_pose; + if (VDA5050ClientAPI::move_base_ptr_->getRobotPose(robot_pose)) + { + global_state.agvPosition.x = robot_pose.x; + global_state.agvPosition.y = robot_pose.y; + global_state.agvPosition.theta = robot_pose.theta; + } + + move_base_core::State nav_state = VDA5050ClientAPI::move_base_ptr_->nav_feedback_->navigation_state; + if (nav_state == move_base_core::State::CONTROLLING) + { + global_state.driving = true; + global_state.paused = false; + global_state.newBaseRequest = false; + } + else if (nav_state == move_base_core::State::PAUSED) + { + global_state.driving = false; + global_state.paused = true; + global_state.newBaseRequest = false; + } + else + { + global_state.driving = false; + global_state.paused = false; + global_state.newBaseRequest = true; + } + } + + global_state.orderId = this->client_ptr_->vda5050_order_.orderId; + global_state.orderUpdateId = this->client_ptr_->vda5050_order_.orderUpdateId; + global_state.operatingMode = this->mode_; + } + + void VDA5050ClientAPI::resetState() + { + *this->cancel_action_ = false; + *this->enable_action_ = false; + *this->pause_action_ = false; + } + + std::shared_ptr VDA5050ClientAPI::move_base_ptr_ = nullptr; + std::shared_ptr VDA5050ClientAPI::loc_base_ptr_ = nullptr; + std::shared_ptr VDA5050ClientAPI::monitor_ptr_ = nullptr; +} \ No newline at end of file diff --git a/Controllers/Packages/amr_control/test/test_move_base.cpp b/Controllers/Packages/amr_control/test/test_move_base.cpp new file mode 100644 index 0000000..50fe4ef --- /dev/null +++ b/Controllers/Packages/amr_control/test/test_move_base.cpp @@ -0,0 +1,279 @@ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +move_base_core::BaseNavigation::Ptr move_base = nullptr; +geometry_msgs::PoseStamped amcl_pose; +boost::mutex ctr_mutex; +bool pub_order = false; +amr_control::moveTo order; +bool pub_cancel = false; + +void moveToCallback(const amr_control::moveTo::ConstPtr& msg) +{ + // boost::unique_lock lock(ctr_mutex); + if(move_base) + { + try + { + geometry_msgs::PoseStamped goal = nav_2d_utils::pose2DToPoseStamped(msg->goal, "map", ros::Time::now()); + geometry_msgs::PoseStamped of_goal = move_base_core::offset_goal(amcl_pose, msg->goal.x); + double xy_goal_tolerance = 0.04; + double yaw_goal_tolerance = 0.06; + // ROS_INFO_STREAM(of_goal); + switch ((int)msg->maker.data) + { + case 1: + move_base->moveTo(goal, move_base_core::PICK_UP, xy_goal_tolerance, yaw_goal_tolerance); + break; + case 2: + move_base->moveTo(goal, move_base_core::POP_DOWN, xy_goal_tolerance, yaw_goal_tolerance); + break; + case 3: + move_base->moveTo(goal, move_base_core::POSITION, xy_goal_tolerance, yaw_goal_tolerance); + break; + case 4: + move_base->moveTo(goal, move_base_core::CHARGER, xy_goal_tolerance, yaw_goal_tolerance); + break; + case 5: + move_base->moveStraightTo(of_goal, xy_goal_tolerance); + break; + case 6: + move_base->rotateTo(goal, yaw_goal_tolerance); + break; + default: + throw std::bad_function_call(); + break; + } + + geometry_msgs::Vector3 linear; + linear.x = 1.1; + move_base->setTwistLinear(linear); + linear.x = -0.5; + move_base->setTwistLinear(linear); + } + catch(const std::exception& e) + { + ROS_ERROR_STREAM(e.what() << "\n"); + } + } +} + +void getposeCallback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg) +{ + amcl_pose.header = msg->header; + amcl_pose.pose.position = msg->pose.pose.position; + amcl_pose.pose.orientation = msg->pose.pose.orientation; +} + +void setTiwstCallback(const geometry_msgs::Vector3::ConstPtr& msg) +{ + if(move_base) + { + move_base->setTwistLinear(*msg); + } +} + +void cancelCallback(const std_msgs::Bool::ConstPtr& msg) +{ + move_base->cancel(); +} + +void pauseCallback(const std_msgs::Bool::ConstPtr& msg) +{ + move_base->pause(); +} + +void resumeCallback(const std_msgs::Bool::ConstPtr& msg) +{ + move_base->resume(); +} + +void order_msg_handle(const vda5050_msgs::Order::ConstPtr& msg) +{ + if(msg->serialNumber == "AMR-150") + { + ros::Duration(0.5).sleep(); + if(!msg->nodes.empty()) + { + order.header.frame_id = "map"; + order.header.stamp = ros::Time::now(); + order.goal.x = msg->nodes.back().nodePosition.x; + order.goal.y = msg->nodes.back().nodePosition.y; + order.goal.theta = msg->nodes.back().nodePosition.theta; + if(!msg->nodes.back().actions.empty()) + { + if(msg->nodes.back().actions.front().actionType == "startInPallet") + order.maker.data = 1; + else + order.maker.data = 3; + } + else + order.maker.data = 3; + pub_order = true; + } + } +} + +void instantActions_msg_handle(const vda5050_msgs::InstantActions::ConstPtr& msg) +{ + if(msg->serialNumber == "AMR-150") + { + for(int i=0; i< msg->actions.size(); i++) + { + if(msg->actions[i].actionType == "cancelOrder") pub_cancel = true; + } + } +} + +bool getRobotPose(geometry_msgs::PoseStamped& global_pose, std::shared_ptr tf_) +{ + std::string global_frame_ = "map"; + std::string robot_base_frame_ = "base_link"; + // if(name_.find("global_costmap")) ROS_WARN("Costmap2DROS::getRobotPose"); + tf2::toMsg(tf2::Transform::getIdentity(), global_pose.pose); + geometry_msgs::PoseStamped robot_pose; + tf2::toMsg(tf2::Transform::getIdentity(), robot_pose.pose); + robot_pose.header.frame_id = robot_base_frame_; + robot_pose.header.stamp = ros::Time(); + ros::Time current_time = ros::Time::now(); // save time for checking tf delay later + + // get the global pose of the robot + try + { + // use current time if possible (makes sure it's not in the future) + if (tf_->canTransform(global_frame_, robot_base_frame_, current_time)) + { + geometry_msgs::TransformStamped transform = tf_->lookupTransform(global_frame_, robot_base_frame_, current_time); + tf2::doTransform(robot_pose, global_pose, transform); + } + // use the latest otherwise + else + { + tf_->transform(robot_pose, global_pose, global_frame_); + } + } + catch (tf2::LookupException& ex) + { + ROS_ERROR_THROTTLE(1.0, "No Transform available Error looking up robot pose: %s\n", ex.what()); + return false; + } + catch (tf2::ConnectivityException& ex) + { + ROS_ERROR_THROTTLE(1.0, "Connectivity Error looking up robot pose: %s\n", ex.what()); + return false; + } + catch (tf2::ExtrapolationException& ex) + { + ROS_ERROR_THROTTLE(1.0, "Extrapolation Error looking up robot pose: %s\n", ex.what()); + return false; + } + // ROS_INFO_THROTTLE(1.0, "Time Delay %f , p %f %f", current_time.toSec() - global_pose.header.stamp.toSec(), global_pose.pose.position.x, global_pose.pose.position.y); + // check global_pose timeout + if (!global_pose.header.stamp.isZero() && current_time.toSec() - global_pose.header.stamp.toSec() > 1.0) + { + ROS_WARN_THROTTLE(1.0, + // ROS_WARN( + "Costmap2DROS transform timeout. Current time: %.4f, global_pose stamp: %.4f, tolerance: %.4f", + current_time.toSec(), global_pose.header.stamp.toSec(), 1.0); + return false; + } + + return true; +} + + +int main(int argc, char** argv){ + ros::init(argc, argv, "move_base_node"); + ros::NodeHandle nh = ros::NodeHandle("move_base_node"); + ros::NodeHandle nh_; + std::shared_ptr buffer = std::make_shared(); + tf2_ros::TransformListener tf2(*buffer); + + pluginlib::ClassLoader mb_loader("move_base_core", "move_base_core::BaseNavigation"); + + std::string obj_name("move_base::MoveBase"); + try + { + move_base = mb_loader.createUniqueInstance(obj_name); + ROS_INFO("Created object %s susseced", obj_name.c_str()); + move_base->initialize(buffer); + } + catch (pluginlib::PluginlibException &ex) + { + ROS_FATAL("Failed to create the %s, are you sure it is properly registered and that the containing library is built? Exception: %s", obj_name.c_str(), ex.what()); + return 1; + } + catch(std::exception &e) + { + ROS_FATAL("Failed to create the %s, are you sure it is properly registered and that the containing library is built? Exception: %s", obj_name.c_str(), e.what()); + return 1; + } + ros::Subscriber moveto_sub = nh.subscribe("moveTo", 1000, moveToCallback); + ros::Subscriber set_twist_sub = nh.subscribe("setTiwst", 1000, setTiwstCallback); + ros::Subscriber cancel_sub = nh.subscribe("cancel", 1000, cancelCallback); + ros::Subscriber pause_sub = nh.subscribe("pause", 1000, pauseCallback); + ros::Subscriber resume_sub = nh.subscribe("resume", 1000, resumeCallback); + ros::Subscriber amcl_pose_sub = nh.subscribe("amcl_pose", 1000, getposeCallback); + ros::Subscriber order_msg_sub_ = nh_.subscribe("/order",1000, order_msg_handle); + ros::Subscriber instantAc_msg_sub_ = nh_.subscribe("/instantActions",1000, instantActions_msg_handle); + ros::Publisher moveto_pub = nh.advertise("moveTo", 1000); + ros::Publisher cancel_pub = nh.advertise("cancel", 1000); + ros::Rate r(10); + + while (ros::ok()) + { + if(pub_order) + { + moveto_pub.publish(order); + pub_order = false; + } + else if (pub_cancel) + { + std_msgs::Bool value; + value.data = true; + cancel_pub.publish(value); + pub_cancel = false; + } + getRobotPose(amcl_pose, buffer); + // boost::unique_lock lock(ctr_mutex); + // { + if(move_base && move_base->nav_feedback_) + { + // if(move_base->nav_feedback_->navigation_state == move_base_core::State::ABORTED) + // ROS_ERROR("ABORTED %s", move_base->nav_feedback_->feed_back_str.c_str()); + // if(move_base->nav_feedback_->navigation_state == move_base_core::State::PAUSED) + // ROS_WARN("PAUSED %s", move_base->nav_feedback_->feed_back_str.c_str()); + // if(move_base->nav_feedback_->navigation_state == move_base_core::State::PREEMPTED) + // ROS_WARN("PREEMPTED %s", move_base->nav_feedback_->feed_back_str.c_str()); + // if(move_base->nav_feedback_->navigation_state == move_base_core::State::SUCCEEDED) + // ROS_INFO_STREAM("SUCCEEDED \n" << move_base->nav_feedback_->current_pose); + } + // } + ros::spinOnce(); + r.sleep(); + } + + ros::spin(); + return(0); +} diff --git a/Controllers/Packages/amr_control/test/vda_5050_api.cpp b/Controllers/Packages/amr_control/test/vda_5050_api.cpp new file mode 100644 index 0000000..b78a938 --- /dev/null +++ b/Controllers/Packages/amr_control/test/vda_5050_api.cpp @@ -0,0 +1,15 @@ +#include +#include "amr_control/amr_vda_5050_client_api.h" + +std::shared_ptr client; + +int main(int argc, char** argv) { + // Initialize the ROS node + ros::init(argc, argv, "test_imr_nova_control"); + ros::start(); + + client = std::make_shared(); + ros::spin(); + client.reset(); + return 0; +} \ No newline at end of file diff --git a/Controllers/Packages/amr_startup/CMakeLists.txt b/Controllers/Packages/amr_startup/CMakeLists.txt new file mode 100644 index 0000000..b2c0f81 --- /dev/null +++ b/Controllers/Packages/amr_startup/CMakeLists.txt @@ -0,0 +1,205 @@ +cmake_minimum_required(VERSION 3.0.2) +project(amr_startup) + +## Compile as C++11, supported in ROS Kinetic and newer +# add_compile_options(-std=c++11) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a exec_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# std_msgs # Or other packages containing msgs +# ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES amr_startup +# CATKIN_DEPENDS other_catkin_pkg +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( +# include +# ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +# add_library(${PROJECT_NAME} +# src/${PROJECT_NAME}/amr_startup.cpp +# ) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +## With catkin_make all packages are built within a single CMake context +## The recommended prefix ensures that target names across packages don't collide +# add_executable(${PROJECT_NAME}_node src/amr_startup_node.cpp) + +## Rename C++ executable without prefix +## The above recommended prefix causes long target names, the following renames the +## target back to the shorter version for ease of user use +## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" +# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +# target_link_libraries(${PROJECT_NAME}_node +# ${catkin_LIBRARIES} +# ) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# catkin_install_python(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html +# install(TARGETS ${PROJECT_NAME}_node +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark libraries for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html +# install(TARGETS ${PROJECT_NAME} +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +install(DIRECTORY + config + launch + rviz + sdf + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + ) + + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_amr_startup.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/Controllers/Packages/amr_startup/config/base_local_planner_params.yaml b/Controllers/Packages/amr_startup/config/base_local_planner_params.yaml new file mode 100644 index 0000000..853cace --- /dev/null +++ b/Controllers/Packages/amr_startup/config/base_local_planner_params.yaml @@ -0,0 +1,48 @@ +#For full documentation of the parameters in this file, and a list of all the +#parameters available for TrajectoryPlannerROS, please see +#http://www.ros.org/wiki/base_local_planner +TrajectoryPlannerROS: + #Set the acceleration limits of the robot + acc_lim_th: 3.2 + acc_lim_x: 0.5 + acc_lim_y: 0.5 + + #Set the velocity limits of the robot + max_vel_x: 0.4 + min_vel_x: 0.1 + max_rotational_vel: 1.0 + min_in_place_rotational_vel: 0.4 + + #The velocity the robot will command when trying to escape from a stuck situation + escape_vel: -0.2 + + #For this example, we'll use a holonomic robot + holonomic_robot: false + + #Since we're using a holonomic robot, we'll set the set of y velocities it will sample + y_vels: [-0.3, -0.1, 0.1, 0.3] + + #Set the tolerance on achieving a goal + xy_goal_tolerance: 0.1 + yaw_goal_tolerance: 0.05 + + #We'll configure how long and with what granularity we'll forward simulate trajectories + sim_time: 3.0 + sim_granularity: 0.025 + vx_samples: 5 + vtheta_samples: 20 + + #Parameters for scoring trajectories + goal_distance_bias: 0.4 + path_distance_bias: 0.7 + occdist_scale: 0.3 + heading_lookahead: 0.325 + + #We'll use the Dynamic Window Approach to control instead of Trajectory Rollout for this example + dwa: false + + #How far the robot must travel before oscillation flags are reset + oscillation_reset_dist: 0.01 + + #Eat up the plan as the robot moves along it + prune_plan: false \ No newline at end of file diff --git a/Controllers/Packages/amr_startup/config/costmap_common_params.yaml b/Controllers/Packages/amr_startup/config/costmap_common_params.yaml new file mode 100755 index 0000000..af52a10 --- /dev/null +++ b/Controllers/Packages/amr_startup/config/costmap_common_params.yaml @@ -0,0 +1,54 @@ +robot_base_frame: base_footprint +transform_tolerance: 1.0 +obstacle_range: 3.0 +#mark_threshold: 1 +publish_voxel_map: true +footprint_padding: 0.0 + +navigation_map: + map_topic: /map + map_pkg: managerments + map_file: maze + +virtual_walls_map: + map_topic: /virtual_walls/map + namespace: /virtual_walls + map_pkg: managerments + map_file: maze + use_maximum: true + lethal_cost_threshold: 100 + +obstacles: + observation_sources: f_scan_marking f_scan_clearing b_scan_marking b_scan_clearing + f_scan_marking: + topic: /f_scan + data_type: LaserScan + clearing: false + marking: true + inf_is_valid: false + min_obstacle_height: 0.0 + max_obstacle_height: 0.25 + f_scan_clearing: + topic: /f_scan + data_type: LaserScan + clearing: true + marking: false + inf_is_valid: false + min_obstacle_height: 0.0 + max_obstacle_height: 0.25 + b_scan_marking: + topic: /b_scan + data_type: LaserScan + clearing: false + marking: true + inf_is_valid: false + min_obstacle_height: 0.0 + max_obstacle_height: 0.25 + b_scan_clearing: + topic: /b_scan + data_type: LaserScan + clearing: true + marking: false + inf_is_valid: false + min_obstacle_height: 0.0 + max_obstacle_height: 0.25 diff --git a/Controllers/Packages/amr_startup/config/costmap_global_params.yaml b/Controllers/Packages/amr_startup/config/costmap_global_params.yaml new file mode 100755 index 0000000..ae6d769 --- /dev/null +++ b/Controllers/Packages/amr_startup/config/costmap_global_params.yaml @@ -0,0 +1,13 @@ +global_costmap: + global_frame: map + update_frequency: 1.0 + publish_frequency: 1.0 + raytrace_range: 2.0 + resolution: 0.05 + z_resolution: 0.2 + rolling_window: false + z_voxels: 10 + inflation: + cost_scaling_factor: 10.0 # Exponential rate at which the obstacle cost drops off (default: 10). Must be chosen so that the cost value is > 0 at robot's circumscribed radius. + inflation_radius: 0.6 # Max. distance from an obstacle at which costs are incurred for planning paths. Must be > robot's circumscribed radius. + diff --git a/Controllers/Packages/amr_startup/config/costmap_global_params_plugins_no_virtual_walls.yaml b/Controllers/Packages/amr_startup/config/costmap_global_params_plugins_no_virtual_walls.yaml new file mode 100755 index 0000000..06d0037 --- /dev/null +++ b/Controllers/Packages/amr_startup/config/costmap_global_params_plugins_no_virtual_walls.yaml @@ -0,0 +1,10 @@ +global_costmap: + frame_id: map + plugins: + - {name: navigation_map, type: "costmap_2d::StaticLayer" } + - {name: virtual_walls_map, type: "costmap_2d::StaticLayer" } + - {name: obstacles, type: "costmap_2d::VoxelLayer" } + - {name: inflation, type: "costmap_2d::InflationLayer" } + obstacles: + enabled: false + footprint_clearing_enabled: false \ No newline at end of file diff --git a/Controllers/Packages/amr_startup/config/costmap_local_params.yaml b/Controllers/Packages/amr_startup/config/costmap_local_params.yaml new file mode 100755 index 0000000..49d126f --- /dev/null +++ b/Controllers/Packages/amr_startup/config/costmap_local_params.yaml @@ -0,0 +1,16 @@ +local_costmap: + global_frame: odom + update_frequency: 5.0 + publish_frequency: 5.0 + rolling_window: true + raytrace_range: 2.0 + resolution: 0.05 + z_resolution: 0.15 + z_voxels: 8 + inflation: + cost_scaling_factor: 10.0 # Exponential rate at which the obstacle cost drops off (default: 10). Must be chosen so that the cost value is > 0 at robot's circumscribed radius. + inflation_radius: 0.3 # Max. distance from an obstacle at which costs are incurred for planning paths. Must be > robot's circumscribed radius. + width: 8.0 + height: 8.0 + origin_x: 0.0 + origin_y: 0.0 \ No newline at end of file diff --git a/Controllers/Packages/amr_startup/config/costmap_local_params_plugins_no_virtual_walls.yaml b/Controllers/Packages/amr_startup/config/costmap_local_params_plugins_no_virtual_walls.yaml new file mode 100755 index 0000000..6ef480b --- /dev/null +++ b/Controllers/Packages/amr_startup/config/costmap_local_params_plugins_no_virtual_walls.yaml @@ -0,0 +1,8 @@ +local_costmap: + frame_id: odom + plugins: + - {name: obstacles, type: "costmap_2d::VoxelLayer" } + - {name: inflation, type: "costmap_2d::InflationLayer" } + obstacles: + enabled: true + footprint_clearing_enabled: true diff --git a/Controllers/Packages/amr_startup/config/custom_global_params.yaml b/Controllers/Packages/amr_startup/config/custom_global_params.yaml new file mode 100644 index 0000000..6536a6e --- /dev/null +++ b/Controllers/Packages/amr_startup/config/custom_global_params.yaml @@ -0,0 +1,17 @@ +base_global_planner: CustomPlanner +CustomPlanner: + environment_type: XYThetaLattice + planner_type: ARAPlanner + allocated_time: 10.0 + initial_epsilon: 1.0 + force_scratch_limit: 10000 + forward_search: false + nominalvel_mpersecs: 0.8 + timetoturn45degsinplace_secs: 1.31 # = 0.6 rad/s + allow_unknown: true + directory_to_save_paths: "/init/paths" + pathway_filename: "pathway.txt" + current_pose_topic_name: "/amcl_pose" + map_frame_id: "map" + base_frame_id: "base_link" + diff --git a/Controllers/Packages/amr_startup/config/dwa_local_planner_params.yaml b/Controllers/Packages/amr_startup/config/dwa_local_planner_params.yaml new file mode 100755 index 0000000..b8d766a --- /dev/null +++ b/Controllers/Packages/amr_startup/config/dwa_local_planner_params.yaml @@ -0,0 +1,55 @@ +base_local_planner: dwa_local_planner/DWAPlannerROS +DWAPlannerROS: + # Robot configuration + max_vel_x: 0.8 + min_vel_x: -0.2 + + max_vel_y: 0.0 # diff drive robot + min_vel_y: 0.0 # diff drive robot + + max_trans_vel: 0.8 # choose slightly less than the base's capability + min_trans_vel: 0.1 # this is the min trans velocity when there is negligible rotational velocity + trans_stopped_vel: 0.03 + + # Warning! + # do not set min_trans_vel to 0.0 otherwise dwa will always think translational velocities + # are non-negligible and small in place rotational velocities will be created. + + max_rot_vel: 1.0 # choose slightly less than the base's capability + min_rot_vel: 0.1 # this is the min angular velocity when there is negligible translational velocity + rot_stopped_vel: 0.1 + + acc_lim_x: 1.5 + acc_lim_y: 0.0 # diff drive robot + acc_limit_trans: 1.5 + acc_lim_theta: 2.0 + + # Goal tolerance + yaw_goal_tolerance: 0.03 # yaw_goal_tolerance > (sim_time * min_rot_vel) / 2 (from Navigation Tuning Guide) + xy_goal_tolerance: 0.08 # xy_goal_tolerance > (sim_time * min_vel_x) / 2 + latch_xy_goal_tolerance: true + + # Forward simulation + sim_time: 1.2 + vx_samples: 15 + vy_samples: 1 # diff drive robot, there is only one sample + vtheta_samples: 20 + + # Trajectory scoring + path_distance_bias: 64.0 # default: 32.0 mir: 32.0 - weighting for how much it should stick to the global path plan + goal_distance_bias: 12.0 # default: 24.0 mir: 48.0 - weighting for how much it should attempt to reach its goal + occdist_scale: 0.5 # default: 0.01 mir: 0.01 - weighting for how much the controller should avoid obstacles + forward_point_distance: 0.325 # default: 0.325 mir: 0.325 - how far along to place an additional scoring point + stop_time_buffer: 0.2 # default: 0.2 mir: 0.2 - amount of time a robot must stop before colliding for a valid traj. + scaling_speed: 0.25 # default: 0.25 mir: 0.25 - absolute velocity at which to start scaling the robot's footprint + max_scaling_factor: 0.2 # default: 0.2 mir: 0.2 - how much to scale the robot's footprint when at speed. + prune_plan: true + + # Oscillation prevention + oscillation_reset_dist: 0.05 # 0.05 - how far to travel before resetting oscillation flags, in m + oscillation_reset_angle: 0.2 # 0.2 - the angle the robot must turn before resetting Oscillation flags, in rad + + # Debugging + publish_traj_pc : true + publish_cost_grid_pc: true + global_frame_id: /odom # or /odom diff --git a/Controllers/Packages/amr_startup/config/ekf.yaml b/Controllers/Packages/amr_startup/config/ekf.yaml new file mode 100755 index 0000000..7344ee3 --- /dev/null +++ b/Controllers/Packages/amr_startup/config/ekf.yaml @@ -0,0 +1,217 @@ +# The frequency, in Hz, at which the filter will output a position estimate. Note that the filter will not begin +# computation until it receives at least one message from one of the inputs. It will then run continuously at the +# frequency specified here, regardless of whether it receives more measurements. Defaults to 30 if unspecified. +frequency: 50 + +# The period, in seconds, after which we consider a sensor to have timed out. In this event, we carry out a predict +# cycle on the EKF without correcting it. This parameter can be thought of as the minimum frequency with which the +# filter will generate new output. Defaults to 1 / frequency if not specified. +sensor_timeout: 0.1 + +# ekf_localization_node and ukf_localization_node both use a 3D omnidirectional motion model. If this parameter is +# set to true, no 3D information will be used in your state estimate. Use this if you are operating in a planar +# environment and want to ignore the effect of small variations in the ground plane that might otherwise be detected +# by, for example, an IMU. Defaults to false if unspecified. +two_d_mode: true + +# Use this parameter to provide an offset to the transform generated by ekf_localization_node. This can be used for +# future dating the transform, which is required for interaction with some other packages. Defaults to 0.0 if +# unspecified. +transform_time_offset: 0.0 + +# Use this parameter to specify how long the tf listener should wait for a transform to become available. +# Defaults to 0.0 if unspecified. +transform_timeout: 0.0 + +# If you're having trouble, try setting this to true, and then echo the /diagnostics_agg topic to see if the node is +# unhappy with any settings or data. +print_diagnostics: true + +# Debug settings. Not for the faint of heart. Outputs a ludicrous amount of information to the file specified by +# debug_out_file. I hope you like matrices! Please note that setting this to true will have strongly deleterious +# effects on the performance of the node. Defaults to false if unspecified. +debug: false + +# Defaults to "robot_localization_debug.txt" if unspecified. Please specify the full path. +debug_out_file: /path/to/debug/file.txt + +# Whether to broadcast the transformation over the /tf topic. Defaults to true if unspecified. +publish_tf: true + +# Whether to publish the acceleration state. Defaults to false if unspecified. +publish_acceleration: false + +# REP-105 (http://www.ros.org/reps/rep-0105.html) specifies four principal coordinate frames: base_link, odom, map, and +# earth. base_link is the coordinate frame that is affixed to the robot. Both odom and map are world-fixed frames. +# The robot's position in the odom frame will drift over time, but is accurate in the short term and should be +# continuous. The odom frame is therefore the best frame for executing local motion plans. The map frame, like the odom +# frame, is a world-fixed coordinate frame, and while it contains the most globally accurate position estimate for your +# robot, it is subject to discrete jumps, e.g., due to the fusion of GPS data or a correction from a map-based +# localization node. The earth frame is used to relate multiple map frames by giving them a common reference frame. +# ekf_localization_node and ukf_localization_node are not concerned with the earth frame. +# Here is how to use the following settings: +# 1. Set the map_frame, odom_frame, and base_link frames to the appropriate frame names for your system. +# 1a. If your system does not have a map_frame, just remove it, and make sure "world_frame" is set to the value of +# odom_frame. +# 2. If you are fusing continuous position data such as wheel encoder odometry, visual odometry, or IMU data, set +# "world_frame" to your odom_frame value. This is the default behavior for robot_localization's state estimation nodes. +# 3. If you are fusing global absolute position data that is subject to discrete jumps (e.g., GPS or position updates +# from landmark observations) then: +# 3a. Set your "world_frame" to your map_frame value +# 3b. MAKE SURE something else is generating the odom->base_link transform. Note that this can even be another state +# estimation node from robot_localization! However, that instance should *not* fuse the global data. +map_frame: map # Defaults to "map" if unspecified +odom_frame: $(arg tf_prefix)odom # Defaults to "odom" if unspecified +base_link_frame: $(arg tf_prefix)base_footprint # Defaults to "base_link" if unspecified +world_frame: $(arg tf_prefix)odom # Defaults to the value of odom_frame if unspecified + +# The filter accepts an arbitrary number of inputs from each input message type (nav_msgs/Odometry, +# geometry_msgs/PoseWithCovarianceStamped, geometry_msgs/TwistWithCovarianceStamped, +# sensor_msgs/Imu). To add an input, simply append the next number in the sequence to its "base" name, e.g., odom0, +# odom1, twist0, twist1, imu0, imu1, imu2, etc. The value should be the topic name. These parameters obviously have no +# default values, and must be specified. +odom0: odom + +# Each sensor reading updates some or all of the filter's state. These options give you greater control over which +# values from each measurement are fed to the filter. For example, if you have an odometry message as input, but only +# want to use its Z position value, then set the entire vector to false, except for the third entry. The order of the +# values is x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Note that not some message types +# do not provide some of the state variables estimated by the filter. For example, a TwistWithCovarianceStamped message +# has no pose information, so the first six values would be meaningless in that case. Each vector defaults to all false +# if unspecified, effectively making this parameter required for each sensor. +# see http://docs.ros.org/melodic/api/robot_localization/html/configuring_robot_localization.html +odom0_config: [false, false, false, # x y z + false, false, false, # roll pitch yaw + true, true, false, # vx vy vz + false, false, true, # vroll vpitch vyaw + false, false, false] # ax ay az + +# If you have high-frequency data or are running with a low frequency parameter value, then you may want to increase +# the size of the subscription queue so that more measurements are fused. +odom0_queue_size: 10 + +# [ADVANCED] Large messages in ROS can exhibit strange behavior when they arrive at a high frequency. This is a result +# of Nagle's algorithm. This option tells the ROS subscriber to use the tcpNoDelay option, which disables Nagle's +# algorithm. +odom0_nodelay: false + +# [ADVANCED] When measuring one pose variable with two sensors, a situation can arise in which both sensors under- +# report their covariances. This can lead to the filter rapidly jumping back and forth between each measurement as they +# arrive. In these cases, it often makes sense to (a) correct the measurement covariances, or (b) if velocity is also +# measured by one of the sensors, let one sensor measure pose, and the other velocity. However, doing (a) or (b) isn't +# always feasible, and so we expose the differential parameter. When differential mode is enabled, all absolute pose +# data is converted to velocity data by differentiating the absolute pose measurements. These velocities are then +# integrated as usual. NOTE: this only applies to sensors that provide pose measurements; setting differential to true +# for twist measurements has no effect. +odom0_differential: false + +# [ADVANCED] When the node starts, if this parameter is true, then the first measurement is treated as a "zero point" +# for all future measurements. While you can achieve the same effect with the differential paremeter, the key +# difference is that the relative parameter doesn't cause the measurement to be converted to a velocity before +# integrating it. If you simply want your measurements to start at 0 for a given sensor, set this to true. +odom0_relative: false + +# [ADVANCED] If your data is subject to outliers, use these threshold settings, expressed as Mahalanobis distances, to +# control how far away from the current vehicle state a sensor measurement is permitted to be. Each defaults to +# numeric_limits::max() if unspecified. It is strongly recommended that these parameters be removed if not +# required. Data is specified at the level of pose and twist variables, rather than for each variable in isolation. +# For messages that have both pose and twist data, the parameter specifies to which part of the message we are applying +# the thresholds. +#odom0_pose_rejection_threshold: 5 +#odom0_twist_rejection_threshold: 1 + +# Further input parameter examples +# see http://docs.ros.org/melodic/api/robot_localization/html/configuring_robot_localization.html +imu0: imu_data +imu0_config: [false, false, false, # x y z + false, false, true, # roll pitch yaw + false, false, false, # vx vy vz + false, false, true, # vroll vpitch vyaw + true, false, false] # ax ay az +imu0_nodelay: false +imu0_differential: false +imu0_relative: true +imu0_queue_size: 10 +#imu0_pose_rejection_threshold: 0.8 # Note the difference in parameter names +#imu0_twist_rejection_threshold: 0.8 # +#imu0_linear_acceleration_rejection_threshold: 0.8 # + +# [ADVANCED] Some IMUs automatically remove acceleration due to gravity, and others don't. If yours doesn't, please set +# this to true, and *make sure* your data conforms to REP-103, specifically, that the data is in ENU frame. +imu0_remove_gravitational_acceleration: false + +# [ADVANCED] The EKF and UKF models follow a standard predict/correct cycle. During prediction, if there is no +# acceleration reference, the velocity at time t+1 is simply predicted to be the same as the velocity at time t. During +# correction, this predicted value is fused with the measured value to produce the new velocity estimate. This can be +# problematic, as the final velocity will effectively be a weighted average of the old velocity and the new one. When +# this velocity is the integrated into a new pose, the result can be sluggish covergence. This effect is especially +# noticeable with LIDAR data during rotations. To get around it, users can try inflating the process_noise_covariance +# for the velocity variable in question, or decrease the variance of the variable in question in the measurement +# itself. In addition, users can also take advantage of the control command being issued to the robot at the time we +# make the prediction. If control is used, it will get converted into an acceleration term, which will be used during +# predicition. Note that if an acceleration measurement for the variable in question is available from one of the +# inputs, the control term will be ignored. +# Whether or not we use the control input during predicition. Defaults to false. +use_control: false +# Whether the input (assumed to be cmd_vel) is a geometry_msgs/Twist or geometry_msgs/TwistStamped message. Defaults to +# false. +stamped_control: false +# The last issued control command will be used in prediction for this period. Defaults to 0.2. +control_timeout: 0.2 +# Which velocities are being controlled. Order is vx, vy, vz, vroll, vpitch, vyaw. +control_config: [true, false, false, false, false, true] +# Places limits on how large the acceleration term will be. Should match your robot's kinematics. +acceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 3.4] +# Acceleration and deceleration limits are not always the same for robots. +deceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 4.5] +# If your robot cannot instantaneously reach its acceleration limit, the permitted change can be controlled with these +# gains +acceleration_gains: [0.8, 0.0, 0.0, 0.0, 0.0, 0.9] +# If your robot cannot instantaneously reach its deceleration limit, the permitted change can be controlled with these +# gains +deceleration_gains: [1.0, 0.0, 0.0, 0.0, 0.0, 1.0] + +# [ADVANCED] The process noise covariance matrix can be difficult to tune, and can vary for each application, so it is +# exposed as a configuration parameter. This matrix represents the noise we add to the total error after each +# prediction step. The better the omnidirectional motion model matches your system, the smaller these values can be. +# However, if users find that a given variable is slow to converge, one approach is to increase the +# process_noise_covariance diagonal value for the variable in question, which will cause the filter's predicted error +# to be larger, which will cause the filter to trust the incoming measurement more during correction. The values are +# ordered as x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Defaults to the matrix below if +# unspecified. +process_noise_covariance: [0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0.06, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0.06, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0.04, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.02, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.015] + +# [ADVANCED] This represents the initial value for the state estimate error covariance matrix. Setting a diagonal +# value (variance) to a large value will result in rapid convergence for initial measurements of the variable in +# question. Users should take care not to use large values for variables that will not be measured directly. The values +# are ordered as x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Defaults to the matrix below +#if unspecified. +initial_estimate_covariance: [100.0,0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 100.0,0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 1.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 10.0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 10.0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9] diff --git a/Controllers/Packages/amr_startup/config/localization.yaml b/Controllers/Packages/amr_startup/config/localization.yaml new file mode 100644 index 0000000..677cb96 --- /dev/null +++ b/Controllers/Packages/amr_startup/config/localization.yaml @@ -0,0 +1,36 @@ +Amcl: + use_map_topic: true + odom_model_type: "diff-corrected" + gui_publish_rate: 5.0 + save_pose_rate: 0.5 + laser_max_beams: 300 + laser_min_range: -1.0 + laser_max_range: -1.0 + min_particles: 1000 + max_particles: 3000 + kld_err: 0.05 + kld_z: 0.99 + odom_alpha1: 0.02 + odom_alpha2: 0.01 + odom_alpha3: 0.01 + odom_alpha4: 0.02 + laser_z_hit: 0.5 + laser_z_short: 0.05 + laser_z_max: 0.05 + laser_z_rand: 0.5 + laser_sigma_hit: 0.2 + laser_lambda_short: 0.1 + laser_model_type: "likelihood_field" + laser_likelihood_max_dist: 1.0 + update_min_d: 0.05 + update_min_a: 0.05 + odom_frame_id: odom + base_frame_id: base_footprint + global_frame_id: map + resample_interval: 1 + transform_tolerance: 0.2 + recovery_alpha_slow: 0.001 + recovery_alpha_fast: 0.001 + initial_pose_x: 0.0 + initial_pose_y: 0.0 + initial_pose_a: 0.0 diff --git a/Controllers/Packages/amr_startup/config/maker_sources.yaml b/Controllers/Packages/amr_startup/config/maker_sources.yaml new file mode 100644 index 0000000..5dd2276 --- /dev/null +++ b/Controllers/Packages/amr_startup/config/maker_sources.yaml @@ -0,0 +1,120 @@ +maker_sources: trolley charger dock_station undock_station +trolley: + plugins: + - {name: 4legs, docking_planner: "DockPlanner", docking_nav: ""} + - {name: qrcode, docking_planner: "TwoPointsPlanner", docking_nav: "" } + + 4legs: + maker_goal_frame: trolley_goal + footprint: [[0.583,-0.48],[0.583,0.48],[-0.583,0.48],[-0.583,-0.48]] + delay: 2.0 + timeout: 60.0 + vel_x: 0.1 + vel_theta: 0.3 + yaw_goal_tolerance: 0.017 + xy_goal_tolerance: 0.01 + min_lookahead_dist: 0.4 + max_lookahead_dist: 1.0 + lookahead_time: 1.5 + angle_threshold: 0.36 + + qrcode: + maker_goal_frame: qr_trolley + delay: 2.0 + timeout: 60.0 + vel_x: 0.05 + vel_theta: 0.2 + allow_rotate: true + yaw_goal_tolerance: 0.017 + xy_goal_tolerance: 0.01 + min_lookahead_dist: 0.4 + max_lookahead_dist: 1.0 + lookahead_time: 1.5 + angle_threshold: 0.36 + +charger: + plugins: + - {name: charger, docking_planner: "DockPlanner", docking_nav: ""} + + charger: + maker_goal_frame: charger_goal + footprint: [[0.583,-0.48],[0.583,0.48],[-0.583,0.48],[-0.583,-0.48]] + delay: 2 + timeout: 60 + vel_x: 0.1 + yaw_goal_tolerance: 0.017 + xy_goal_tolerance: 0.01 + min_lookahead_dist: 0.4 + max_lookahead_dist: 1.0 + lookahead_time: 1.5 + angle_threshold: 0.36 + +dock_station: + plugins: + - {name: station, docking_planner: "DockPlanner", docking_nav: ""} + + station: + maker_goal_frame: dock_station_goal + footprint: [[1.15,-0.55],[1.15,0.55],[-1.15,0.55],[-1.15,-0.55]] + delay: 2 + timeout: 60 + vel_x: 0.15 + vel_theta: 0.3 + yaw_goal_tolerance: 0.01 + xy_goal_tolerance: 0.01 + min_lookahead_dist: 0.4 + max_lookahead_dist: 1.0 + lookahead_time: 1.5 + angle_threshold: 0.36 + + +dock_station_2: + plugins: + - {name: station, docking_planner: "DockPlanner", docking_nav: ""} + + station: + maker_goal_frame: dock_station_goal_2 + footprint: [[1.15,-0.55],[1.15,0.55],[-1.15,0.55],[-1.15,-0.55]] + delay: 2 + timeout: 60 + vel_x: 0.15 + vel_theta: 0.3 + yaw_goal_tolerance: 0.01 + xy_goal_tolerance: 0.01 + min_lookahead_dist: 0.4 + max_lookahead_dist: 1.0 + lookahead_time: 1.5 + angle_threshold: 0.36 + +undock_station: + plugins: + - {name: station, docking_planner: "DockPlanner", docking_nav: ""} + - {name: qrcode, docking_planner: "TwoPointsPlanner", docking_nav: "" } + + station: + maker_goal_frame: undock_station_goal + footprint: [[0.583,-0.48],[0.583,0.48],[-0.583,0.48],[-0.583,-0.48]] + delay: 2.0 + timeout: 60.0 + vel_x: 0.15 + vel_theta: 0.3 + yaw_goal_tolerance: 0.017 + xy_goal_tolerance: 0.01 + min_lookahead_dist: 0.4 + max_lookahead_dist: 1.0 + lookahead_time: 1.5 + angle_threshold: 0.36 + + qrcode: + maker_goal_frame: qr_station + delay: 2.0 + timeout: 60.0 + vel_x: 0.05 + vel_theta: 0.2 + allow_rotate: true + yaw_goal_tolerance: 0.01 + xy_goal_tolerance: 0.01 + min_lookahead_dist: 0.4 + max_lookahead_dist: 1.0 + lookahead_time: 1.5 + angle_threshold: 0.36 \ No newline at end of file diff --git a/Controllers/Packages/amr_startup/config/mapping.yaml b/Controllers/Packages/amr_startup/config/mapping.yaml new file mode 100644 index 0000000..e58fce3 --- /dev/null +++ b/Controllers/Packages/amr_startup/config/mapping.yaml @@ -0,0 +1,63 @@ +SlamToolBox: + # Plugin params + solver_plugin: solver_plugins::CeresSolver + ceres_linear_solver: SPARSE_NORMAL_CHOLESKY + ceres_preconditioner: SCHUR_JACOBI + ceres_trust_strategy: LEVENBERG_MARQUARDT + ceres_dogleg_type: TRADITIONAL_DOGLEG + ceres_loss_function: None + + # ROS Parameters + odom_frame: odom + map_frame: map + base_frame: base_link + scan_topic: /scan + mode: mapping #localization + debug_logging: false + throttle_scans: 1 + transform_publish_period: 0.02 #if 0 never publishes odometry + map_update_interval: 10.0 + resolution: 0.05 + max_laser_range: 20.0 #for rastering images + minimum_time_interval: 0.5 + transform_timeout: 0.2 + tf_buffer_duration: 14400. + stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps + enable_interactive_mode: true + + # General Parameters + use_scan_matching: true + use_scan_barycenter: true + minimum_travel_distance: 0.5 + minimum_travel_heading: 0.5 + scan_buffer_size: 10 + scan_buffer_maximum_scan_distance: 10 + link_match_minimum_response_fine: 0.1 + link_scan_maximum_distance: 1.5 + loop_search_maximum_distance: 3.0 + do_loop_closing: true + loop_match_minimum_chain_size: 10 + loop_match_maximum_variance_coarse: 3.0 + loop_match_minimum_response_coarse: 0.35 + loop_match_minimum_response_fine: 0.45 + + # Correlation Parameters - Correlation Parameters + correlation_search_space_dimension: 0.5 + correlation_search_space_resolution: 0.01 + correlation_search_space_smear_deviation: 0.1 + + # Correlation Parameters - Loop Closure Parameters + loop_search_space_dimension: 8.0 + loop_search_space_resolution: 0.05 + loop_search_space_smear_deviation: 0.03 + + # Scan Matcher Parameters + distance_variance_penalty: 0.5 + angle_variance_penalty: 1.0 + + fine_search_angle_offset: 0.00349 + coarse_search_angle_offset: 0.349 + coarse_angle_resolution: 0.0349 + minimum_angle_penalty: 0.9 + minimum_distance_penalty: 0.5 + use_response_expansion: true diff --git a/Controllers/Packages/amr_startup/config/move_base_common_params.yaml b/Controllers/Packages/amr_startup/config/move_base_common_params.yaml new file mode 100755 index 0000000..6d275d8 --- /dev/null +++ b/Controllers/Packages/amr_startup/config/move_base_common_params.yaml @@ -0,0 +1,24 @@ + +### replanning +controller_frequency: 20.0 # run controller at 15.0 Hz +controller_patience: 0.0 # if the controller failed, clear obstacles and retry; after 15.0 s, abort and replan +planner_frequency: 0.0 # don't continually replan (only when controller failed) +planner_patience: 5.0 # if the first planning attempt failed, abort planning retries after 5.0 s... +max_planning_retries: 5 # ... or after 10 attempts (whichever happens first) +oscillation_timeout: -1.0 # abort controller and trigger recovery behaviors after 30.0 s +oscillation_distance: 1.5 +### recovery behaviors +recovery_behavior_enabled: false +recovery_behaviors: [ + {name: aggressive_reset, type: clear_costmap_recovery/ClearCostmapRecovery}, + {name: conservative_reset, type: clear_costmap_recovery/ClearCostmapRecovery}, + ] + +conservative_reset: + reset_distance: 3.0 # clear obstacles farther away than 3.0 m + invert_area_to_clear: true + +aggressive_reset: + reset_distance: 3.0 + + diff --git a/Controllers/Packages/amr_startup/config/mpc_local_planner_params.yaml b/Controllers/Packages/amr_startup/config/mpc_local_planner_params.yaml new file mode 100755 index 0000000..aa6a592 --- /dev/null +++ b/Controllers/Packages/amr_startup/config/mpc_local_planner_params.yaml @@ -0,0 +1,106 @@ +base_local_planner: mpc_local_planner/MpcLocalPlannerROS +MpcLocalPlannerROS: + + odom_topic: odom + + ## Robot settings + robot: + type: "unicycle" + unicycle: + max_vel_x: 0.5 + max_vel_x_backwards: 0.3 + max_vel_theta: 0.3 + acc_lim_x: 0.4 # deactive bounds with zero + dec_lim_x: 0.4 # deactive bounds with zero + acc_lim_theta: 0.6 # deactivate bounds with zero + + ## Footprint model for collision avoidance + footprint_model: + type: "point" + is_footprint_dynamic: False + + ## Collision avoidance + collision_avoidance: + min_obstacle_dist: 0.5 # Note, this parameter must be chosen w.r.t. the footprint_model + enable_dynamic_obstacles: False + force_inclusion_dist: 0.5 + cutoff_dist: 2.0 + include_costmap_obstacles: True + costmap_obstacles_behind_robot_dist: 1.5 + collision_check_no_poses: 5 + + ## Planning grid + grid: + type: "fd_grid" + grid_size_ref: 20 # Set horizon length here (T = (grid_size_ref-1) * dt_ref); Note, also check max_global_plan_lookahead_dist + dt_ref: 0.3 # and here the corresponding temporal resolution + xf_fixed: [False, False, False] # Unfix final state -> we use terminal cost below + warm_start: True + collocation_method: "forward_differences" + cost_integration_method: "left_sum" + variable_grid: + enable: False # We want a fixed grid + min_dt: 0.0; + max_dt: 10.0; + grid_adaptation: + enable: True + dt_hyst_ratio: 0.1 + min_grid_size: 2 + max_grid_size: 50 + + ## Planning options + planning: + objective: + type: "quadratic_form" # minimum_time requires grid/variable_grid/enable=True and grid/xf_fixed set properly + quadratic_form: + state_weights: [2.0, 2.0, 0.25] + control_weights: [0.1, 0.05] + integral_form: False + terminal_cost: + type: "quadratic" # can be "none" + quadratic: + final_state_weights: [10.0, 10.0, 0.5] + terminal_constraint: + type: "none" # can be "none" + l2_ball: + weight_matrix: [1.0, 1.0, 1.0] + radius: 5 + + ## Controller options + controller: + outer_ocp_iterations: 1 + xy_goal_tolerance: 0.05 + yaw_goal_tolerance: 0.04 + global_plan_overwrite_orientation: False + global_plan_prune_distance: 1.5 + allow_init_with_backward_motion: True + max_global_plan_lookahead_dist: 1.0 # Check horizon length + force_reinit_new_goal_dist: 1.0 + force_reinit_new_goal_angular: 1.57 + force_reinit_num_steps: 0 + prefer_x_feedback: False + publish_ocp_results: True + + ## Solver settings + solver: + type: "ipopt" + ipopt: + iterations: 100 + max_cpu_time: -1.0 + ipopt_numeric_options: + tol: 1e-3 + ipopt_string_options: + linear_solver: "mumps" + hessian_approximation: "exact" # exact or limited-memory + lsq_lm: + iterations: 10 + weight_init_eq: 2 + weight_init_ineq: 2 + weight_init_bounds: 2 + weight_adapt_factor_eq: 1.5 + weight_adapt_factor_ineq: 1.5 + weight_adapt_factor_bounds: 1.5 + weight_adapt_max_eq: 500 + weight_adapt_max_ineq: 500 + weight_adapt_max_bounds: 500 + diff --git a/Controllers/Packages/amr_startup/config/mprim/genmprim_unicycle_highcost_5cm.m b/Controllers/Packages/amr_startup/config/mprim/genmprim_unicycle_highcost_5cm.m new file mode 100755 index 0000000..2810593 --- /dev/null +++ b/Controllers/Packages/amr_startup/config/mprim/genmprim_unicycle_highcost_5cm.m @@ -0,0 +1,280 @@ +% /* +% * Copyright (c) 2008, Maxim Likhachev +% * All rights reserved. +% * +% * Redistribution and use in source and binary forms, with or without +% * modification, are permitted provided that the following conditions are met: +% * +% * * Redistributions of source code must retain the above copyright +% * notice, this list of conditions and the following disclaimer. +% * * Redistributions in binary form must reproduce the above copyright +% * notice, this list of conditions and the following disclaimer in the +% * documentation and/or other materials provided with the distribution. +% * * Neither the name of the Carnegie Mellon University nor the names of its +% * contributors may be used to endorse or promote products derived from +% * this software without specific prior written permission. +% * +% * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +% * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +% * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +% * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +% * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +% * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +% * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +% * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +% * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +% * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +% * POSSIBILITY OF SUCH DAMAGE. +% */ + +function[] = genmprim_unicycle_highcost_5cm(outfilename) + +% +%generates motion primitives and saves them into file +% +%written by Maxim Likhachev +%--------------------------------------------------- +% + +%defines + +UNICYCLE_MPRIM_16DEGS = 1; + + +if UNICYCLE_MPRIM_16DEGS == 1 + resolution = 0.05; + numberofangles = 16; %preferably a power of 2, definitely multiple of 8 + numberofprimsperangle = 7; + + %multipliers (multiplier is used as costmult*cost) + forwardcostmult = 1; + backwardcostmult = 40; + forwardandturncostmult = 2; + sidestepcostmult = 10; + turninplacecostmult = 20; + + %note, what is shown x,y,theta changes (not absolute numbers) + + %0 degreees + basemprimendpts0_c = zeros(numberofprimsperangle, 4); %x,y,theta,costmult + %x aligned with the heading of the robot, angles are positive + %counterclockwise + %0 theta change + basemprimendpts0_c(1,:) = [1 0 0 forwardcostmult]; + basemprimendpts0_c(2,:) = [8 0 0 forwardcostmult]; + basemprimendpts0_c(3,:) = [-1 0 0 backwardcostmult]; + %1/16 theta change + basemprimendpts0_c(4,:) = [8 1 1 forwardandturncostmult]; + basemprimendpts0_c(5,:) = [8 -1 -1 forwardandturncostmult]; + %turn in place + basemprimendpts0_c(6,:) = [0 0 1 turninplacecostmult]; + basemprimendpts0_c(7,:) = [0 0 -1 turninplacecostmult]; + + %45 degrees + basemprimendpts45_c = zeros(numberofprimsperangle, 4); %x,y,theta,costmult (multiplier is used as costmult*cost) + %x aligned with the heading of the robot, angles are positive + %counterclockwise + %0 theta change + basemprimendpts45_c(1,:) = [1 1 0 forwardcostmult]; + basemprimendpts45_c(2,:) = [6 6 0 forwardcostmult]; + basemprimendpts45_c(3,:) = [-1 -1 0 backwardcostmult]; + %1/16 theta change + basemprimendpts45_c(4,:) = [5 7 1 forwardandturncostmult]; + basemprimendpts45_c(5,:) = [7 5 -1 forwardandturncostmult]; + %turn in place + basemprimendpts45_c(6,:) = [0 0 1 turninplacecostmult]; + basemprimendpts45_c(7,:) = [0 0 -1 turninplacecostmult]; + + %22.5 degrees + basemprimendpts22p5_c = zeros(numberofprimsperangle, 4); %x,y,theta,costmult (multiplier is used as costmult*cost) + %x aligned with the heading of the robot, angles are positive + %counterclockwise + %0 theta change + basemprimendpts22p5_c(1,:) = [2 1 0 forwardcostmult]; + basemprimendpts22p5_c(2,:) = [6 3 0 forwardcostmult]; + basemprimendpts22p5_c(3,:) = [-2 -1 0 backwardcostmult]; + %1/16 theta change + basemprimendpts22p5_c(4,:) = [5 4 1 forwardandturncostmult]; + basemprimendpts22p5_c(5,:) = [7 2 -1 forwardandturncostmult]; + %turn in place + basemprimendpts22p5_c(6,:) = [0 0 1 turninplacecostmult]; + basemprimendpts22p5_c(7,:) = [0 0 -1 turninplacecostmult]; + +else + fprintf(1, 'ERROR: undefined mprims type\n'); + return; +end; + + +fout = fopen(outfilename, 'w'); + + +%write the header +fprintf(fout, 'resolution_m: %f\n', resolution); +fprintf(fout, 'numberofangles: %d\n', numberofangles); +fprintf(fout, 'totalnumberofprimitives: %d\n', numberofprimsperangle*numberofangles); + +%iterate over angles +for angleind = 1:numberofangles + + figure(1); + hold off; + + text(0, 0, int2str(angleind)); + + %iterate over primitives + for primind = 1:numberofprimsperangle + fprintf(fout, 'primID: %d\n', primind-1); + fprintf(fout, 'startangle_c: %d\n', angleind-1); + + %current angle + currentangle = (angleind-1)*2*pi/numberofangles; + currentangle_36000int = round((angleind-1)*36000/numberofangles); + + %compute which template to use + if (rem(currentangle_36000int, 9000) == 0) + basemprimendpts_c = basemprimendpts0_c(primind,:); + angle = currentangle; + elseif (rem(currentangle_36000int, 4500) == 0) + basemprimendpts_c = basemprimendpts45_c(primind,:); + angle = currentangle - 45*pi/180; + elseif (rem(currentangle_36000int-7875, 9000) == 0) + basemprimendpts_c = basemprimendpts33p75_c(primind,:); + basemprimendpts_c(1) = basemprimendpts33p75_c(primind, 2); %reverse x and y + basemprimendpts_c(2) = basemprimendpts33p75_c(primind, 1); + basemprimendpts_c(3) = -basemprimendpts33p75_c(primind, 3); %reverse the angle as well + angle = currentangle - 78.75*pi/180; + fprintf(1, '78p75\n'); + elseif (rem(currentangle_36000int-6750, 9000) == 0) + basemprimendpts_c = basemprimendpts22p5_c(primind,:); + basemprimendpts_c(1) = basemprimendpts22p5_c(primind, 2); %reverse x and y + basemprimendpts_c(2) = basemprimendpts22p5_c(primind, 1); + basemprimendpts_c(3) = -basemprimendpts22p5_c(primind, 3); %reverse the angle as well + %fprintf(1, '%d %d %d onto %d %d %d\n', basemprimendpts22p5_c(1), basemprimendpts22p5_c(2), basemprimendpts22p5_c(3), ... + % basemprimendpts_c(1), basemprimendpts_c(2), basemprimendpts_c(3)); + angle = currentangle - 67.5*pi/180; + fprintf(1, '67p5\n'); + elseif (rem(currentangle_36000int-5625, 9000) == 0) + basemprimendpts_c = basemprimendpts11p25_c(primind,:); + basemprimendpts_c(1) = basemprimendpts11p25_c(primind, 2); %reverse x and y + basemprimendpts_c(2) = basemprimendpts11p25_c(primind, 1); + basemprimendpts_c(3) = -basemprimendpts11p25_c(primind, 3); %reverse the angle as well + angle = currentangle - 56.25*pi/180; + fprintf(1, '56p25\n'); + elseif (rem(currentangle_36000int-3375, 9000) == 0) + basemprimendpts_c = basemprimendpts33p75_c(primind,:); + angle = currentangle - 33.75*pi/180; + fprintf(1, '33p75\n'); + elseif (rem(currentangle_36000int-2250, 9000) == 0) + basemprimendpts_c = basemprimendpts22p5_c(primind,:); + angle = currentangle - 22.5*pi/180; + fprintf(1, '22p5\n'); + elseif (rem(currentangle_36000int-1125, 9000) == 0) + basemprimendpts_c = basemprimendpts11p25_c(primind,:); + angle = currentangle - 11.25*pi/180; + fprintf(1, '11p25\n'); + else + fprintf(1, 'ERROR: invalid angular resolution. angle = %d\n', currentangle_36000int); + return; + end; + + %now figure out what action will be + baseendpose_c = basemprimendpts_c(1:3); + additionalactioncostmult = basemprimendpts_c(4); + endx_c = round(baseendpose_c(1)*cos(angle) - baseendpose_c(2)*sin(angle)); + endy_c = round(baseendpose_c(1)*sin(angle) + baseendpose_c(2)*cos(angle)); + endtheta_c = rem(angleind - 1 + baseendpose_c(3), numberofangles); + endpose_c = [endx_c endy_c endtheta_c]; + + fprintf(1, 'rotation angle=%f\n', angle*180/pi); + + if baseendpose_c(2) == 0 & baseendpose_c(3) == 0 + %fprintf(1, 'endpose=%d %d %d\n', endpose_c(1), endpose_c(2), endpose_c(3)); + end; + + %generate intermediate poses (remember they are w.r.t 0,0 (and not + %centers of the cells) + numofsamples = 10; + intermcells_m = zeros(numofsamples,3); + if UNICYCLE_MPRIM_16DEGS == 1 + startpt = [0 0 currentangle]; + endpt = [endpose_c(1)*resolution endpose_c(2)*resolution ... + rem(angleind - 1 + baseendpose_c(3), numberofangles)*2*pi/numberofangles]; + intermcells_m = zeros(numofsamples,3); + if ((endx_c == 0 & endy_c == 0) | baseendpose_c(3) == 0) %turn in place or move forward + for iind = 1:numofsamples + intermcells_m(iind,:) = [startpt(1) + (endpt(1) - startpt(1))*(iind-1)/(numofsamples-1) ... + startpt(2) + (endpt(2) - startpt(2))*(iind-1)/(numofsamples-1) ... + 0]; + rotation_angle = (baseendpose_c(3) ) * (2*pi/numberofangles); + intermcells_m(iind,3) = rem(startpt(3) + (rotation_angle)*(iind-1)/(numofsamples-1), 2*pi); + + end; + else %unicycle-based move forward or backward + R = [cos(startpt(3)) sin(endpt(3)) - sin(startpt(3)); + sin(startpt(3)) -(cos(endpt(3)) - cos(startpt(3)))]; + S = pinv(R)*[endpt(1) - startpt(1); endpt(2) - startpt(2)]; + l = S(1); + tvoverrv = S(2); + rv = (baseendpose_c(3)*2*pi/numberofangles + l/tvoverrv); + tv = tvoverrv*rv; + + if l < 0 + fprintf(1, 'WARNING: l = %d < 0 -> bad action start/end points\n', l); + l = 0; + end; + %compute rv + %rv = baseendpose_c(3)*2*pi/numberofangles; + %compute tv + %tvx = (endpt(1) - startpt(1))*rv/(sin(endpt(3)) - sin(startpt(3))) + %tvy = -(endpt(2) - startpt(2))*rv/(cos(endpt(3)) - cos(startpt(3))) + %tv = (tvx + tvy)/2.0; + %generate samples + for iind = 1:numofsamples + dt = (iind-1)/(numofsamples-1); + + %dtheta = rv*dt + startpt(3); + %intermcells_m(iind,:) = [startpt(1) + tv/rv*(sin(dtheta) - sin(startpt(3))) ... + % startpt(2) - tv/rv*(cos(dtheta) - cos(startpt(3))) ... + % dtheta]; + + if(dt*tv < l) + intermcells_m(iind,:) = [startpt(1) + dt*tv*cos(startpt(3)) ... + startpt(2) + dt*tv*sin(startpt(3)) ... + startpt(3)]; + else + dtheta = rv*(dt - l/tv) + startpt(3); + intermcells_m(iind,:) = [startpt(1) + l*cos(startpt(3)) + tvoverrv*(sin(dtheta) - sin(startpt(3))) ... + startpt(2) + l*sin(startpt(3)) - tvoverrv*(cos(dtheta) - cos(startpt(3))) ... + dtheta]; + end; + end; + %correct + errorxy = [endpt(1) - intermcells_m(numofsamples,1) ... + endpt(2) - intermcells_m(numofsamples,2)]; + fprintf(1, 'l=%f errx=%f erry=%f\n', l, errorxy(1), errorxy(2)); + interpfactor = [0:1/(numofsamples-1):1]; + intermcells_m(:,1) = intermcells_m(:,1) + errorxy(1)*interpfactor'; + intermcells_m(:,2) = intermcells_m(:,2) + errorxy(2)*interpfactor'; + end; + end; + + %write out + fprintf(fout, 'endpose_c: %d %d %d\n', endpose_c(1), endpose_c(2), endpose_c(3)); + fprintf(fout, 'additionalactioncostmult: %d\n', additionalactioncostmult); + fprintf(fout, 'intermediateposes: %d\n', size(intermcells_m,1)); + for interind = 1:size(intermcells_m, 1) + fprintf(fout, '%.4f %.4f %.4f\n', intermcells_m(interind,1), intermcells_m(interind,2), intermcells_m(interind,3)); + end; + + plot(intermcells_m(:,1), intermcells_m(:,2)); + axis([-0.3 0.3 -0.3 0.3]); + text(intermcells_m(numofsamples,1), intermcells_m(numofsamples,2), int2str(endpose_c(3))); + hold on; + + end; + grid; + pause; +end; + +fclose('all'); diff --git a/Controllers/Packages/amr_startup/config/mprim/genmprim_unicycle_highcost_5cm.py b/Controllers/Packages/amr_startup/config/mprim/genmprim_unicycle_highcost_5cm.py new file mode 100755 index 0000000..c8801c0 --- /dev/null +++ b/Controllers/Packages/amr_startup/config/mprim/genmprim_unicycle_highcost_5cm.py @@ -0,0 +1,416 @@ +#!/usr/bin/env python3 +# +# Copyright (c) 2016, David Conner (Christopher Newport University) +# Based on genmprim_unicycle.m +# Copyright (c) 2008, Maxim Likhachev +# All rights reserved. +# converted by libermate utility (https://github.com/awesomebytes/libermate) +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# * Neither the name of the Carnegie Mellon University nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +import numpy as np +import rospkg + +# if available import pylab (from matlibplot) +matplotlib_found = False +try: + import matplotlib.pylab as plt + + matplotlib_found = True +except ImportError: + pass + + +def matrix_size(mat, elem=None): + if not elem: + return mat.shape + else: + return mat.shape[int(elem) - 1] + + +def genmprim_unicycle(outfilename, visualize=False, separate_plots=False): + visualize = matplotlib_found and visualize # Plot the primitives + + # Local Variables: basemprimendpts22p5_c, endtheta_c, endx_c, + # baseendpose_c, additionalactioncostmult, fout, numofsamples, + # basemprimendpts45_c, primind, basemprimendpts0_c, rv, angle, outfilename, + # numberofangles, startpt, UNICYCLE_MPRIM_16DEGS, sidestepcostmult, + # rotation_angle, basemprimendpts_c, forwardandturncostmult, + # forwardcostmult, turninplacecostmult, endpose_c, backwardcostmult, + # interpfactor, S, R, tvoverrv, dtheta, intermcells_m, tv, dt, + # currentangle, numberofprimsperangle, resolution, currentangle_36000int, + # l, iind, errorxy, interind, endy_c, angleind, endpt + # Function calls: plot, cos, pi, grid, figure, genmprim_unicycle, text, + # int2str, pause, axis, sin, pinv, fprintf, fclose, rem, zeros, fopen, + # round, size + # % + # %generates motion primitives and saves them into file + # % + # %written by Maxim Likhachev + # %--------------------------------------------------- + # % + # %defines + UNICYCLE_MPRIM_16DEGS = 1.0 + if UNICYCLE_MPRIM_16DEGS == 1.0: + resolution = 0.05 + numberofangles = 16 + # %preferably a power of 2, definitely multiple of 8 + numberofprimsperangle = 7 + # %multipliers (multiplier is used as costmult*cost) + forwardcostmult = 1.0 + backwardcostmult = 40.0 + forwardandturncostmult = 2.0 + # sidestepcostmult = 10.0 + turninplacecostmult = 20.0 + # %note, what is shown x,y,theta changes (not absolute numbers) + # %0 degreees + basemprimendpts0_c = np.zeros((numberofprimsperangle, 4)) + # %x,y,theta,costmult + # %x aligned with the heading of the robot, angles are positive + # %counterclockwise + # %0 theta change + basemprimendpts0_c[0, :] = np.array(np.hstack((1.0, 0.0, 0.0, forwardcostmult))) + basemprimendpts0_c[1, :] = np.array(np.hstack((8.0, 0.0, 0.0, forwardcostmult))) + basemprimendpts0_c[2, :] = np.array(np.hstack((-1.0, 0.0, 0.0, backwardcostmult))) + # %1/16 theta change + basemprimendpts0_c[3, :] = np.array(np.hstack((8.0, 1.0, 1.0, forwardandturncostmult))) + basemprimendpts0_c[4, :] = np.array(np.hstack((8.0, -1.0, -1.0, forwardandturncostmult))) + # %turn in place + basemprimendpts0_c[5, :] = np.array(np.hstack((0.0, 0.0, 1.0, turninplacecostmult))) + basemprimendpts0_c[6, :] = np.array(np.hstack((0.0, 0.0, -1.0, turninplacecostmult))) + # %45 degrees + basemprimendpts45_c = np.zeros((numberofprimsperangle, 4)) + # %x,y,theta,costmult (multiplier is used as costmult*cost) + # %x aligned with the heading of the robot, angles are positive + # %counterclockwise + # %0 theta change + basemprimendpts45_c[0, :] = np.array(np.hstack((1.0, 1.0, 0.0, forwardcostmult))) + basemprimendpts45_c[1, :] = np.array(np.hstack((6.0, 6.0, 0.0, forwardcostmult))) + basemprimendpts45_c[2, :] = np.array(np.hstack((-1.0, -1.0, 0.0, backwardcostmult))) + # %1/16 theta change + basemprimendpts45_c[3, :] = np.array(np.hstack((5.0, 7.0, 1.0, forwardandturncostmult))) + basemprimendpts45_c[4, :] = np.array(np.hstack((7.0, 5.0, -1.0, forwardandturncostmult))) + # %turn in place + basemprimendpts45_c[5, :] = np.array(np.hstack((0.0, 0.0, 1.0, turninplacecostmult))) + basemprimendpts45_c[6, :] = np.array(np.hstack((0.0, 0.0, -1.0, turninplacecostmult))) + # %22.5 degrees + basemprimendpts22p5_c = np.zeros((numberofprimsperangle, 4)) + # %x,y,theta,costmult (multiplier is used as costmult*cost) + # %x aligned with the heading of the robot, angles are positive + # %counterclockwise + # %0 theta change + basemprimendpts22p5_c[0, :] = np.array(np.hstack((2.0, 1.0, 0.0, forwardcostmult))) + basemprimendpts22p5_c[1, :] = np.array(np.hstack((6.0, 3.0, 0.0, forwardcostmult))) + basemprimendpts22p5_c[2, :] = np.array(np.hstack((-2.0, -1.0, 0.0, backwardcostmult))) + # %1/16 theta change + basemprimendpts22p5_c[3, :] = np.array(np.hstack((5.0, 4.0, 1.0, forwardandturncostmult))) + basemprimendpts22p5_c[4, :] = np.array(np.hstack((7.0, 2.0, -1.0, forwardandturncostmult))) + # %turn in place + basemprimendpts22p5_c[5, :] = np.array(np.hstack((0.0, 0.0, 1.0, turninplacecostmult))) + basemprimendpts22p5_c[6, :] = np.array(np.hstack((0.0, 0.0, -1.0, turninplacecostmult))) + else: + print('ERROR: undefined mprims type\n') + return [] + + fout = open(outfilename, 'w') + # %write the header + fout.write('resolution_m: %f\n' % (resolution)) + fout.write('numberofangles: %d\n' % (numberofangles)) + fout.write('totalnumberofprimitives: %d\n' % (numberofprimsperangle * numberofangles)) + # %iterate over angles + for angleind in np.arange(1.0, (numberofangles) + 1): + currentangle = ((angleind - 1) * 2.0 * np.pi) / numberofangles + currentangle_36000int = np.round((angleind - 1) * 36000.0 / numberofangles) + if visualize: + if separate_plots: + fig = plt.figure(angleind) + plt.title('angle {:2.0f} (= {:3.1f} degrees)'.format(angleind - 1, currentangle_36000int / 100.0)) + else: + fig = plt.figure(1) + + plt.axis('equal') + plt.axis([-10 * resolution, 10 * resolution, -10 * resolution, 10 * resolution]) + ax = fig.add_subplot(1, 1, 1) + major_ticks = np.arange(-8 * resolution, 9 * resolution, 4 * resolution) + minor_ticks = np.arange(-8 * resolution, 9 * resolution, resolution) + ax.set_xticks(major_ticks) + ax.set_xticks(minor_ticks, minor=True) + ax.set_yticks(major_ticks) + ax.set_yticks(minor_ticks, minor=True) + ax.grid(which='minor', alpha=0.5) + ax.grid(which='major', alpha=0.9) + + # %iterate over primitives + for primind in np.arange(1.0, (numberofprimsperangle) + 1): + fout.write('primID: %d\n' % (primind - 1)) + fout.write('startangle_c: %d\n' % (angleind - 1)) + # %current angle + # %compute which template to use + if (currentangle_36000int % 9000) == 0: + basemprimendpts_c = basemprimendpts0_c[int(primind) - 1, :] + angle = currentangle + elif (currentangle_36000int % 4500) == 0: + basemprimendpts_c = basemprimendpts45_c[int(primind) - 1, :] + angle = currentangle - 45.0 * np.pi / 180.0 + + # commented out because basemprimendpts33p75_c is undefined + # elif ((currentangle_36000int - 7875) % 9000) == 0: + # basemprimendpts_c = ( + # 1 * basemprimendpts33p75_c[primind, :] + # ) # 1* to force deep copy to avoid reference update below + # basemprimendpts_c[0] = basemprimendpts33p75_c[primind, 1] + # # %reverse x and y + # basemprimendpts_c[1] = basemprimendpts33p75_c[primind, 0] + # basemprimendpts_c[2] = -basemprimendpts33p75_c[primind, 2] + # # %reverse the angle as well + # angle = currentangle - (78.75 * np.pi) / 180.0 + # print('78p75\n') + + elif ((currentangle_36000int - 6750) % 9000) == 0: + basemprimendpts_c = ( + 1 * basemprimendpts22p5_c[int(primind) - 1, :] + ) # 1* to force deep copy to avoid reference update below + basemprimendpts_c[0] = basemprimendpts22p5_c[int(primind) - 1, 1] + # %reverse x and y + basemprimendpts_c[1] = basemprimendpts22p5_c[int(primind) - 1, 0] + basemprimendpts_c[2] = -basemprimendpts22p5_c[int(primind) - 1, 2] + # %reverse the angle as well + # print( + # '%d : %d %d %d onto %d %d %d\n' + # % ( + # primind - 1, + # basemprimendpts22p5_c[int(primind) - 1, 0], + # basemprimendpts22p5_c[int(primind) - 1, 1], + # basemprimendpts22p5_c[int(primind) - 1, 2], + # basemprimendpts_c[0], + # basemprimendpts_c[1], + # basemprimendpts_c[2], + # ) + # ) + angle = currentangle - (67.5 * np.pi) / 180.0 + print('67p5\n') + + # commented out because basemprimendpts11p25_c is undefined + # elif ((currentangle_36000int - 5625) % 9000) == 0: + # basemprimendpts_c = ( + # 1 * basemprimendpts11p25_c[primind, :] + # ) # 1* to force deep copy to avoid reference update below + # basemprimendpts_c[0] = basemprimendpts11p25_c[primind, 1] + # # %reverse x and y + # basemprimendpts_c[1] = basemprimendpts11p25_c[primind, 0] + # basemprimendpts_c[2] = -basemprimendpts11p25_c[primind, 2] + # # %reverse the angle as well + # angle = currentangle - (56.25 * np.pi) / 180.0 + # print('56p25\n') + + # commented out because basemprimendpts33p75_c is undefined + # elif ((currentangle_36000int - 3375) % 9000) == 0: + # basemprimendpts_c = basemprimendpts33p75_c[int(primind), :] + # angle = currentangle - (33.75 * np.pi) / 180.0 + # print('33p75\n') + + elif ((currentangle_36000int - 2250) % 9000) == 0: + basemprimendpts_c = basemprimendpts22p5_c[int(primind) - 1, :] + angle = currentangle - (22.5 * np.pi) / 180.0 + print('22p5\n') + + # commented out because basemprimendpts11p25_c is undefined + # elif ((currentangle_36000int - 1125) % 9000) == 0: + # basemprimendpts_c = basemprimendpts11p25_c[int(primind), :] + # angle = currentangle - (11.25 * np.pi) / 180.0 + # print('11p25\n') + + else: + print('ERROR: invalid angular resolution. angle = %d\n' % currentangle_36000int) + return [] + + # %now figure out what action will be + baseendpose_c = basemprimendpts_c[0:3] + additionalactioncostmult = basemprimendpts_c[3] + endx_c = np.round((baseendpose_c[0] * np.cos(angle)) - (baseendpose_c[1] * np.sin(angle))) + endy_c = np.round((baseendpose_c[0] * np.sin(angle)) + (baseendpose_c[1] * np.cos(angle))) + endtheta_c = np.fmod(angleind - 1 + baseendpose_c[2], numberofangles) + endpose_c = np.array(np.hstack((endx_c, endy_c, endtheta_c))) + print("endpose_c=", endpose_c) + print(('rotation angle=%f\n' % (angle * 180.0 / np.pi))) + # if np.logical_and(baseendpose_c[1] == 0., baseendpose_c[2] == 0.): + # %fprintf(1, 'endpose=%d %d %d\n', endpose_c(1), endpose_c(2), endpose_c(3)); + + # %generate intermediate poses (remember they are w.r.t 0,0 (and not + # %centers of the cells) + numofsamples = 10 + intermcells_m = np.zeros((numofsamples, 3)) + if UNICYCLE_MPRIM_16DEGS == 1.0: + startpt = np.array(np.hstack((0.0, 0.0, currentangle))) + endpt = np.array( + np.hstack( + ( + (endpose_c[0] * resolution), + (endpose_c[1] * resolution), + ( + ((np.fmod(angleind - 1 + baseendpose_c[2], numberofangles)) * 2.0 * np.pi) + / numberofangles + ), + ) + ) + ) + + print("startpt =", startpt) + print("endpt =", endpt) + intermcells_m = np.zeros((numofsamples, 3)) + if np.logical_or(np.logical_and(endx_c == 0.0, endy_c == 0.0), baseendpose_c[2] == 0.0): + # %turn in place or move forward + for iind in np.arange(1.0, (numofsamples) + 1): + fraction = float(iind - 1) / (numofsamples - 1) + intermcells_m[int(iind) - 1, :] = np.array( + ( + startpt[0] + (endpt[0] - startpt[0]) * fraction, + startpt[1] + (endpt[1] - startpt[1]) * fraction, + 0, + ) + ) + rotation_angle = baseendpose_c[2] * (2.0 * np.pi / numberofangles) + intermcells_m[int(iind) - 1, 2] = np.fmod(startpt[2] + rotation_angle * fraction, (2.0 * np.pi)) + # print " ",iind," of ",numofsamples," fraction=",fraction," rotation=",rotation_angle + + else: + # %unicycle-based move forward or backward (http://sbpl.net/node/53) + R = np.array( + np.vstack( + ( + np.hstack((np.cos(startpt[2]), np.sin(endpt[2]) - np.sin(startpt[2]))), + np.hstack((np.sin(startpt[2]), -np.cos(endpt[2]) + np.cos(startpt[2]))), + ) + ) + ) + + S = np.dot(np.linalg.pinv(R), np.array(np.vstack((endpt[0] - startpt[0], endpt[1] - startpt[1])))) + l = S[0] + tvoverrv = S[1] + rv = (baseendpose_c[2] * 2.0 * np.pi / numberofangles) + l / tvoverrv + tv = tvoverrv * rv + + # print "R=\n",R + # print "Rpi=\n",np.linalg.pinv(R) + # print "S=\n",S + # print "l=",l + # print "tvoverrv=",tvoverrv + # print "rv=",rv + # print "tv=",tv + + if l < 0.0: + print(('WARNING: l = %f < 0 -> bad action start/end points\n' % (l))) + l = 0.0 + + # %compute rv + # %rv = baseendpose_c(3)*2*pi/numberofangles; + # %compute tv + # %tvx = (endpt(1) - startpt(1))*rv/(sin(endpt(3)) - sin(startpt(3))) + # %tvy = -(endpt(2) - startpt(2))*rv/(cos(endpt(3)) - cos(startpt(3))) + # %tv = (tvx + tvy)/2.0; + # %generate samples + for iind in np.arange(1, numofsamples + 1): + dt = (iind - 1) / (numofsamples - 1) + # %dtheta = rv*dt + startpt(3); + # %intermcells_m(iind,:) = [startpt(1) + tv/rv*(sin(dtheta) - sin(startpt(3))) ... + # % startpt(2) - tv/rv*(cos(dtheta) - cos(startpt(3))) ... + # % dtheta]; + if (dt * tv) < l: + intermcells_m[int(iind) - 1, :] = np.array( + np.hstack( + ( + startpt[0] + dt * tv * np.cos(startpt[2]), + startpt[1] + dt * tv * np.sin(startpt[2]), + startpt[2], + ) + ) + ) + else: + dtheta = rv * (dt - l / tv) + startpt[2] + intermcells_m[int(iind) - 1, :] = np.array( + np.hstack( + ( + startpt[0] + + l * np.cos(startpt[2]) + + tvoverrv * (np.sin(dtheta) - np.sin(startpt[2])), + startpt[1] + + l * np.sin(startpt[2]) + - tvoverrv * (np.cos(dtheta) - np.cos(startpt[2])), + dtheta, + ) + ) + ) + + # %correct + errorxy = np.array( + np.hstack( + ( + endpt[0] - intermcells_m[int(numofsamples) - 1, 0], + endpt[1] - intermcells_m[int(numofsamples) - 1, 1], + ) + ) + ) + # print('l=%f errx=%f erry=%f\n'%(l, errorxy[0], errorxy[1])) + interpfactor = np.array( + np.hstack((np.arange(0.0, 1.0 + (1.0 / (numofsamples)), 1.0 / (numofsamples - 1)))) + ) + + # print "intermcells_m=",intermcells_m + # print "interp'=",interpfactor.conj().T + + intermcells_m[:, 0] = intermcells_m[:, 0] + errorxy[0] * interpfactor.conj().T + intermcells_m[:, 1] = intermcells_m[:, 1] + errorxy[1] * interpfactor.conj().T + + # %write out + fout.write('endpose_c: %d %d %d\n' % (endpose_c[0], endpose_c[1], endpose_c[2])) + fout.write('additionalactioncostmult: %d\n' % (additionalactioncostmult)) + fout.write('intermediateposes: %d\n' % (matrix_size(intermcells_m, 1.0))) + for interind in np.arange(1.0, (matrix_size(intermcells_m, 1.0)) + 1): + fout.write( + '%.4f %.4f %.4f\n' + % ( + intermcells_m[int(interind) - 1, 0], + intermcells_m[int(interind) - 1, 1], + intermcells_m[int(interind) - 1, 2], + ) + ) + + if visualize: + plt.plot(intermcells_m[:, 0], intermcells_m[:, 1], linestyle="-", marker="o") + plt.text(endpt[0], endpt[1], '{:2.0f}'.format(endpose_c[2])) + # if (visualize): + # plt.waitforbuttonpress() # uncomment to plot each primitive set one at a time + + fout.close() + if visualize: + # plt.waitforbuttonpress() # hold until buttom pressed + plt.show() # Keep windows open until the program is terminated + return [] + + +if __name__ == "__main__": + rospack = rospkg.RosPack() + outfilename = rospack.get_path('mir_navigation') + '/mprim/unicycle_highcost_5cm.mprim' + genmprim_unicycle(outfilename, visualize=True) diff --git a/Controllers/Packages/amr_startup/config/mprim/unicycle_5cm.mprim b/Controllers/Packages/amr_startup/config/mprim/unicycle_5cm.mprim new file mode 100755 index 0000000..cb56cd0 --- /dev/null +++ b/Controllers/Packages/amr_startup/config/mprim/unicycle_5cm.mprim @@ -0,0 +1,1203 @@ +resolution_m: 0.050000 +numberofangles: 16 +totalnumberofprimitives: 80 +primID: 0 +startangle_c: 0 +endpose_c: 1 0 0 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 0.0000 +0.0056 0.0000 0.0000 +0.0111 0.0000 0.0000 +0.0167 0.0000 0.0000 +0.0222 0.0000 0.0000 +0.0278 0.0000 0.0000 +0.0333 0.0000 0.0000 +0.0389 0.0000 0.0000 +0.0444 0.0000 0.0000 +0.0500 0.0000 0.0000 +primID: 1 +startangle_c: 0 +endpose_c: 8 0 0 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 0.0000 +0.0444 0.0000 0.0000 +0.0889 0.0000 0.0000 +0.1333 0.0000 0.0000 +0.1778 0.0000 0.0000 +0.2222 0.0000 0.0000 +0.2667 0.0000 0.0000 +0.3111 0.0000 0.0000 +0.3556 0.0000 0.0000 +0.4000 0.0000 0.0000 +primID: 2 +startangle_c: 0 +endpose_c: -1 0 0 +additionalactioncostmult: 5 +intermediateposes: 10 +0.0000 0.0000 0.0000 +-0.0056 0.0000 0.0000 +-0.0111 0.0000 0.0000 +-0.0167 0.0000 0.0000 +-0.0222 0.0000 0.0000 +-0.0278 0.0000 0.0000 +-0.0333 0.0000 0.0000 +-0.0389 0.0000 0.0000 +-0.0444 0.0000 0.0000 +-0.0500 0.0000 0.0000 +primID: 3 +startangle_c: 0 +endpose_c: 8 1 1 +additionalactioncostmult: 2 +intermediateposes: 10 +0.0000 0.0000 0.0000 +0.0452 -0.0000 0.0000 +0.0904 -0.0000 0.0000 +0.1355 -0.0000 0.0000 +0.1807 0.0008 0.0488 +0.2257 0.0045 0.1176 +0.2703 0.0114 0.1864 +0.3144 0.0213 0.2551 +0.3577 0.0342 0.3239 +0.4000 0.0500 0.3927 +primID: 4 +startangle_c: 0 +endpose_c: 8 -1 -1 +additionalactioncostmult: 2 +intermediateposes: 10 +0.0000 0.0000 0.0000 +0.0452 0.0000 0.0000 +0.0904 0.0000 0.0000 +0.1355 0.0000 0.0000 +0.1807 -0.0008 -0.0488 +0.2257 -0.0045 -0.1176 +0.2703 -0.0114 -0.1864 +0.3144 -0.0213 -0.2551 +0.3577 -0.0342 -0.3239 +0.4000 -0.0500 -0.3927 +primID: 0 +startangle_c: 1 +endpose_c: 2 1 1 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 0.3927 +0.0111 0.0056 0.3927 +0.0222 0.0111 0.3927 +0.0333 0.0167 0.3927 +0.0444 0.0222 0.3927 +0.0556 0.0278 0.3927 +0.0667 0.0333 0.3927 +0.0778 0.0389 0.3927 +0.0889 0.0444 0.3927 +0.1000 0.0500 0.3927 +primID: 1 +startangle_c: 1 +endpose_c: 6 3 1 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 0.3927 +0.0333 0.0167 0.3927 +0.0667 0.0333 0.3927 +0.1000 0.0500 0.3927 +0.1333 0.0667 0.3927 +0.1667 0.0833 0.3927 +0.2000 0.1000 0.3927 +0.2333 0.1167 0.3927 +0.2667 0.1333 0.3927 +0.3000 0.1500 0.3927 +primID: 2 +startangle_c: 1 +endpose_c: -2 -1 1 +additionalactioncostmult: 5 +intermediateposes: 10 +0.0000 0.0000 0.3927 +-0.0111 -0.0056 0.3927 +-0.0222 -0.0111 0.3927 +-0.0333 -0.0167 0.3927 +-0.0444 -0.0222 0.3927 +-0.0556 -0.0278 0.3927 +-0.0667 -0.0333 0.3927 +-0.0778 -0.0389 0.3927 +-0.0889 -0.0444 0.3927 +-0.1000 -0.0500 0.3927 +primID: 3 +startangle_c: 1 +endpose_c: 5 4 2 +additionalactioncostmult: 2 +intermediateposes: 10 +0.0000 0.0000 0.3927 +0.0298 0.0184 0.4230 +0.0592 0.0379 0.4533 +0.0881 0.0583 0.4836 +0.1165 0.0796 0.5139 +0.1444 0.1019 0.5442 +0.1717 0.1251 0.5745 +0.1984 0.1492 0.6048 +0.2245 0.1742 0.6351 +0.2500 0.2000 0.6654 +primID: 4 +startangle_c: 1 +endpose_c: 7 2 0 +additionalactioncostmult: 2 +intermediateposes: 10 +0.0000 0.0000 0.3927 +0.0377 0.0156 0.3927 +0.0754 0.0312 0.3927 +0.1131 0.0468 0.3927 +0.1508 0.0623 0.3736 +0.1893 0.0758 0.2989 +0.2287 0.0863 0.2242 +0.2687 0.0939 0.1494 +0.3092 0.0985 0.0747 +0.3500 0.1000 0.0000 +primID: 0 +startangle_c: 2 +endpose_c: 1 1 2 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 0.7854 +0.0056 0.0056 0.7854 +0.0111 0.0111 0.7854 +0.0167 0.0167 0.7854 +0.0222 0.0222 0.7854 +0.0278 0.0278 0.7854 +0.0333 0.0333 0.7854 +0.0389 0.0389 0.7854 +0.0444 0.0444 0.7854 +0.0500 0.0500 0.7854 +primID: 1 +startangle_c: 2 +endpose_c: 6 6 2 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 0.7854 +0.0333 0.0333 0.7854 +0.0667 0.0667 0.7854 +0.1000 0.1000 0.7854 +0.1333 0.1333 0.7854 +0.1667 0.1667 0.7854 +0.2000 0.2000 0.7854 +0.2333 0.2333 0.7854 +0.2667 0.2667 0.7854 +0.3000 0.3000 0.7854 +primID: 2 +startangle_c: 2 +endpose_c: -1 -1 2 +additionalactioncostmult: 5 +intermediateposes: 10 +0.0000 0.0000 0.7854 +-0.0056 -0.0056 0.7854 +-0.0111 -0.0111 0.7854 +-0.0167 -0.0167 0.7854 +-0.0222 -0.0222 0.7854 +-0.0278 -0.0278 0.7854 +-0.0333 -0.0333 0.7854 +-0.0389 -0.0389 0.7854 +-0.0444 -0.0444 0.7854 +-0.0500 -0.0500 0.7854 +primID: 3 +startangle_c: 2 +endpose_c: 5 7 3 +additionalactioncostmult: 2 +intermediateposes: 10 +0.0000 0.0000 0.7854 +0.0341 0.0341 0.7854 +0.0678 0.0684 0.8151 +0.1000 0.1043 0.8669 +0.1302 0.1418 0.9188 +0.1584 0.1809 0.9707 +0.1846 0.2213 1.0225 +0.2086 0.2631 1.0744 +0.2304 0.3060 1.1262 +0.2500 0.3500 1.1781 +primID: 4 +startangle_c: 2 +endpose_c: 7 5 1 +additionalactioncostmult: 2 +intermediateposes: 10 +0.0000 0.0000 0.7854 +0.0341 0.0341 0.7854 +0.0684 0.0678 0.7557 +0.1043 0.1000 0.7039 +0.1418 0.1302 0.6520 +0.1809 0.1584 0.6001 +0.2213 0.1846 0.5483 +0.2631 0.2086 0.4964 +0.3060 0.2304 0.4446 +0.3500 0.2500 0.3927 +primID: 0 +startangle_c: 3 +endpose_c: 1 2 3 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 1.1781 +0.0056 0.0111 1.1781 +0.0111 0.0222 1.1781 +0.0167 0.0333 1.1781 +0.0222 0.0444 1.1781 +0.0278 0.0556 1.1781 +0.0333 0.0667 1.1781 +0.0389 0.0778 1.1781 +0.0444 0.0889 1.1781 +0.0500 0.1000 1.1781 +primID: 1 +startangle_c: 3 +endpose_c: 3 6 3 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 1.1781 +0.0167 0.0333 1.1781 +0.0333 0.0667 1.1781 +0.0500 0.1000 1.1781 +0.0667 0.1333 1.1781 +0.0833 0.1667 1.1781 +0.1000 0.2000 1.1781 +0.1167 0.2333 1.1781 +0.1333 0.2667 1.1781 +0.1500 0.3000 1.1781 +primID: 2 +startangle_c: 3 +endpose_c: -1 -2 3 +additionalactioncostmult: 5 +intermediateposes: 10 +0.0000 0.0000 1.1781 +-0.0056 -0.0111 1.1781 +-0.0111 -0.0222 1.1781 +-0.0167 -0.0333 1.1781 +-0.0222 -0.0444 1.1781 +-0.0278 -0.0556 1.1781 +-0.0333 -0.0667 1.1781 +-0.0389 -0.0778 1.1781 +-0.0444 -0.0889 1.1781 +-0.0500 -0.1000 1.1781 +primID: 3 +startangle_c: 3 +endpose_c: 4 5 2 +additionalactioncostmult: 2 +intermediateposes: 10 +0.0000 0.0000 1.1781 +0.0184 0.0298 1.1478 +0.0379 0.0592 1.1175 +0.0583 0.0881 1.0872 +0.0796 0.1165 1.0569 +0.1019 0.1444 1.0266 +0.1251 0.1717 0.9963 +0.1492 0.1984 0.9660 +0.1742 0.2245 0.9357 +0.2000 0.2500 0.9054 +primID: 4 +startangle_c: 3 +endpose_c: 2 7 4 +additionalactioncostmult: 2 +intermediateposes: 10 +0.0000 0.0000 1.1781 +0.0156 0.0377 1.1781 +0.0312 0.0754 1.1781 +0.0468 0.1131 1.1781 +0.0623 0.1508 1.1972 +0.0758 0.1893 1.2719 +0.0863 0.2287 1.3466 +0.0939 0.2687 1.4214 +0.0985 0.3092 1.4961 +0.1000 0.3500 1.5708 +primID: 0 +startangle_c: 4 +endpose_c: 0 1 4 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 1.5708 +0.0000 0.0056 1.5708 +0.0000 0.0111 1.5708 +0.0000 0.0167 1.5708 +0.0000 0.0222 1.5708 +0.0000 0.0278 1.5708 +0.0000 0.0333 1.5708 +0.0000 0.0389 1.5708 +0.0000 0.0444 1.5708 +0.0000 0.0500 1.5708 +primID: 1 +startangle_c: 4 +endpose_c: 0 8 4 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 1.5708 +0.0000 0.0444 1.5708 +0.0000 0.0889 1.5708 +0.0000 0.1333 1.5708 +0.0000 0.1778 1.5708 +0.0000 0.2222 1.5708 +0.0000 0.2667 1.5708 +0.0000 0.3111 1.5708 +0.0000 0.3556 1.5708 +0.0000 0.4000 1.5708 +primID: 2 +startangle_c: 4 +endpose_c: 0 -1 4 +additionalactioncostmult: 5 +intermediateposes: 10 +0.0000 0.0000 1.5708 +0.0000 -0.0056 1.5708 +0.0000 -0.0111 1.5708 +0.0000 -0.0167 1.5708 +0.0000 -0.0222 1.5708 +0.0000 -0.0278 1.5708 +0.0000 -0.0333 1.5708 +0.0000 -0.0389 1.5708 +0.0000 -0.0444 1.5708 +0.0000 -0.0500 1.5708 +primID: 3 +startangle_c: 4 +endpose_c: -1 8 5 +additionalactioncostmult: 2 +intermediateposes: 10 +0.0000 0.0000 1.5708 +0.0000 0.0452 1.5708 +0.0000 0.0904 1.5708 +0.0000 0.1355 1.5708 +-0.0008 0.1807 1.6196 +-0.0045 0.2257 1.6884 +-0.0114 0.2703 1.7572 +-0.0213 0.3144 1.8259 +-0.0342 0.3577 1.8947 +-0.0500 0.4000 1.9635 +primID: 4 +startangle_c: 4 +endpose_c: 1 8 3 +additionalactioncostmult: 2 +intermediateposes: 10 +0.0000 0.0000 1.5708 +-0.0000 0.0452 1.5708 +-0.0000 0.0904 1.5708 +-0.0000 0.1355 1.5708 +0.0008 0.1807 1.5220 +0.0045 0.2257 1.4532 +0.0114 0.2703 1.3844 +0.0213 0.3144 1.3156 +0.0342 0.3577 1.2469 +0.0500 0.4000 1.1781 +primID: 0 +startangle_c: 5 +endpose_c: -1 2 5 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 1.9635 +-0.0056 0.0111 1.9635 +-0.0111 0.0222 1.9635 +-0.0167 0.0333 1.9635 +-0.0222 0.0444 1.9635 +-0.0278 0.0556 1.9635 +-0.0333 0.0667 1.9635 +-0.0389 0.0778 1.9635 +-0.0444 0.0889 1.9635 +-0.0500 0.1000 1.9635 +primID: 1 +startangle_c: 5 +endpose_c: -3 6 5 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 1.9635 +-0.0167 0.0333 1.9635 +-0.0333 0.0667 1.9635 +-0.0500 0.1000 1.9635 +-0.0667 0.1333 1.9635 +-0.0833 0.1667 1.9635 +-0.1000 0.2000 1.9635 +-0.1167 0.2333 1.9635 +-0.1333 0.2667 1.9635 +-0.1500 0.3000 1.9635 +primID: 2 +startangle_c: 5 +endpose_c: 1 -2 5 +additionalactioncostmult: 5 +intermediateposes: 10 +0.0000 0.0000 1.9635 +0.0056 -0.0111 1.9635 +0.0111 -0.0222 1.9635 +0.0167 -0.0333 1.9635 +0.0222 -0.0444 1.9635 +0.0278 -0.0556 1.9635 +0.0333 -0.0667 1.9635 +0.0389 -0.0778 1.9635 +0.0444 -0.0889 1.9635 +0.0500 -0.1000 1.9635 +primID: 3 +startangle_c: 5 +endpose_c: -4 5 6 +additionalactioncostmult: 2 +intermediateposes: 10 +0.0000 0.0000 1.9635 +-0.0184 0.0298 1.9938 +-0.0379 0.0592 2.0241 +-0.0583 0.0881 2.0544 +-0.0796 0.1165 2.0847 +-0.1019 0.1444 2.1150 +-0.1251 0.1717 2.1453 +-0.1492 0.1984 2.1756 +-0.1742 0.2245 2.2059 +-0.2000 0.2500 2.2362 +primID: 4 +startangle_c: 5 +endpose_c: -2 7 4 +additionalactioncostmult: 2 +intermediateposes: 10 +0.0000 0.0000 1.9635 +-0.0156 0.0377 1.9635 +-0.0312 0.0754 1.9635 +-0.0468 0.1131 1.9635 +-0.0623 0.1508 1.9444 +-0.0758 0.1893 1.8697 +-0.0863 0.2287 1.7950 +-0.0939 0.2687 1.7202 +-0.0985 0.3092 1.6455 +-0.1000 0.3500 1.5708 +primID: 0 +startangle_c: 6 +endpose_c: -1 1 6 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 2.3562 +-0.0056 0.0056 2.3562 +-0.0111 0.0111 2.3562 +-0.0167 0.0167 2.3562 +-0.0222 0.0222 2.3562 +-0.0278 0.0278 2.3562 +-0.0333 0.0333 2.3562 +-0.0389 0.0389 2.3562 +-0.0444 0.0444 2.3562 +-0.0500 0.0500 2.3562 +primID: 1 +startangle_c: 6 +endpose_c: -6 6 6 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 2.3562 +-0.0333 0.0333 2.3562 +-0.0667 0.0667 2.3562 +-0.1000 0.1000 2.3562 +-0.1333 0.1333 2.3562 +-0.1667 0.1667 2.3562 +-0.2000 0.2000 2.3562 +-0.2333 0.2333 2.3562 +-0.2667 0.2667 2.3562 +-0.3000 0.3000 2.3562 +primID: 2 +startangle_c: 6 +endpose_c: 1 -1 6 +additionalactioncostmult: 5 +intermediateposes: 10 +0.0000 0.0000 2.3562 +0.0056 -0.0056 2.3562 +0.0111 -0.0111 2.3562 +0.0167 -0.0167 2.3562 +0.0222 -0.0222 2.3562 +0.0278 -0.0278 2.3562 +0.0333 -0.0333 2.3562 +0.0389 -0.0389 2.3562 +0.0444 -0.0444 2.3562 +0.0500 -0.0500 2.3562 +primID: 3 +startangle_c: 6 +endpose_c: -7 5 7 +additionalactioncostmult: 2 +intermediateposes: 10 +0.0000 0.0000 2.3562 +-0.0341 0.0341 2.3562 +-0.0684 0.0678 2.3859 +-0.1043 0.1000 2.4377 +-0.1418 0.1302 2.4896 +-0.1809 0.1584 2.5415 +-0.2213 0.1846 2.5933 +-0.2631 0.2086 2.6452 +-0.3060 0.2304 2.6970 +-0.3500 0.2500 2.7489 +primID: 4 +startangle_c: 6 +endpose_c: -5 7 5 +additionalactioncostmult: 2 +intermediateposes: 10 +0.0000 0.0000 2.3562 +-0.0341 0.0341 2.3562 +-0.0678 0.0684 2.3265 +-0.1000 0.1043 2.2747 +-0.1302 0.1418 2.2228 +-0.1584 0.1809 2.1709 +-0.1846 0.2213 2.1191 +-0.2086 0.2631 2.0672 +-0.2304 0.3060 2.0154 +-0.2500 0.3500 1.9635 +primID: 0 +startangle_c: 7 +endpose_c: -2 1 7 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 2.7489 +-0.0111 0.0056 2.7489 +-0.0222 0.0111 2.7489 +-0.0333 0.0167 2.7489 +-0.0444 0.0222 2.7489 +-0.0556 0.0278 2.7489 +-0.0667 0.0333 2.7489 +-0.0778 0.0389 2.7489 +-0.0889 0.0444 2.7489 +-0.1000 0.0500 2.7489 +primID: 1 +startangle_c: 7 +endpose_c: -6 3 7 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 2.7489 +-0.0333 0.0167 2.7489 +-0.0667 0.0333 2.7489 +-0.1000 0.0500 2.7489 +-0.1333 0.0667 2.7489 +-0.1667 0.0833 2.7489 +-0.2000 0.1000 2.7489 +-0.2333 0.1167 2.7489 +-0.2667 0.1333 2.7489 +-0.3000 0.1500 2.7489 +primID: 2 +startangle_c: 7 +endpose_c: 2 -1 7 +additionalactioncostmult: 5 +intermediateposes: 10 +0.0000 0.0000 2.7489 +0.0111 -0.0056 2.7489 +0.0222 -0.0111 2.7489 +0.0333 -0.0167 2.7489 +0.0444 -0.0222 2.7489 +0.0556 -0.0278 2.7489 +0.0667 -0.0333 2.7489 +0.0778 -0.0389 2.7489 +0.0889 -0.0444 2.7489 +0.1000 -0.0500 2.7489 +primID: 3 +startangle_c: 7 +endpose_c: -5 4 6 +additionalactioncostmult: 2 +intermediateposes: 10 +0.0000 0.0000 2.7489 +-0.0298 0.0184 2.7186 +-0.0592 0.0379 2.6883 +-0.0881 0.0583 2.6580 +-0.1165 0.0796 2.6277 +-0.1444 0.1019 2.5974 +-0.1717 0.1251 2.5671 +-0.1984 0.1492 2.5368 +-0.2245 0.1742 2.5065 +-0.2500 0.2000 2.4762 +primID: 4 +startangle_c: 7 +endpose_c: -7 2 8 +additionalactioncostmult: 2 +intermediateposes: 10 +0.0000 0.0000 2.7489 +-0.0377 0.0156 2.7489 +-0.0754 0.0312 2.7489 +-0.1131 0.0468 2.7489 +-0.1508 0.0623 2.7680 +-0.1893 0.0758 2.8427 +-0.2287 0.0863 2.9174 +-0.2687 0.0939 2.9921 +-0.3092 0.0985 3.0669 +-0.3500 0.1000 3.1416 +primID: 0 +startangle_c: 8 +endpose_c: -1 0 8 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 3.1416 +-0.0056 0.0000 3.1416 +-0.0111 0.0000 3.1416 +-0.0167 0.0000 3.1416 +-0.0222 0.0000 3.1416 +-0.0278 0.0000 3.1416 +-0.0333 0.0000 3.1416 +-0.0389 0.0000 3.1416 +-0.0444 0.0000 3.1416 +-0.0500 0.0000 3.1416 +primID: 1 +startangle_c: 8 +endpose_c: -8 0 8 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 3.1416 +-0.0444 0.0000 3.1416 +-0.0889 0.0000 3.1416 +-0.1333 0.0000 3.1416 +-0.1778 0.0000 3.1416 +-0.2222 0.0000 3.1416 +-0.2667 0.0000 3.1416 +-0.3111 0.0000 3.1416 +-0.3556 0.0000 3.1416 +-0.4000 0.0000 3.1416 +primID: 2 +startangle_c: 8 +endpose_c: 1 0 8 +additionalactioncostmult: 5 +intermediateposes: 10 +0.0000 0.0000 3.1416 +0.0056 0.0000 3.1416 +0.0111 0.0000 3.1416 +0.0167 0.0000 3.1416 +0.0222 0.0000 3.1416 +0.0278 0.0000 3.1416 +0.0333 0.0000 3.1416 +0.0389 0.0000 3.1416 +0.0444 0.0000 3.1416 +0.0500 0.0000 3.1416 +primID: 3 +startangle_c: 8 +endpose_c: -8 -1 9 +additionalactioncostmult: 2 +intermediateposes: 10 +0.0000 0.0000 3.1416 +-0.0452 0.0000 3.1416 +-0.0904 0.0000 3.1416 +-0.1355 0.0000 3.1416 +-0.1807 -0.0008 3.1904 +-0.2257 -0.0045 3.2592 +-0.2703 -0.0114 3.3280 +-0.3144 -0.0213 3.3967 +-0.3577 -0.0342 3.4655 +-0.4000 -0.0500 3.5343 +primID: 4 +startangle_c: 8 +endpose_c: -8 1 7 +additionalactioncostmult: 2 +intermediateposes: 10 +0.0000 0.0000 3.1416 +-0.0452 0.0000 3.1416 +-0.0904 0.0000 3.1416 +-0.1355 0.0000 3.1416 +-0.1807 0.0008 3.0928 +-0.2257 0.0045 3.0240 +-0.2703 0.0114 2.9552 +-0.3144 0.0213 2.8864 +-0.3577 0.0342 2.8177 +-0.4000 0.0500 2.7489 +primID: 0 +startangle_c: 9 +endpose_c: -2 -1 9 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 3.5343 +-0.0111 -0.0056 3.5343 +-0.0222 -0.0111 3.5343 +-0.0333 -0.0167 3.5343 +-0.0444 -0.0222 3.5343 +-0.0556 -0.0278 3.5343 +-0.0667 -0.0333 3.5343 +-0.0778 -0.0389 3.5343 +-0.0889 -0.0444 3.5343 +-0.1000 -0.0500 3.5343 +primID: 1 +startangle_c: 9 +endpose_c: -6 -3 9 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 3.5343 +-0.0333 -0.0167 3.5343 +-0.0667 -0.0333 3.5343 +-0.1000 -0.0500 3.5343 +-0.1333 -0.0667 3.5343 +-0.1667 -0.0833 3.5343 +-0.2000 -0.1000 3.5343 +-0.2333 -0.1167 3.5343 +-0.2667 -0.1333 3.5343 +-0.3000 -0.1500 3.5343 +primID: 2 +startangle_c: 9 +endpose_c: 2 1 9 +additionalactioncostmult: 5 +intermediateposes: 10 +0.0000 0.0000 3.5343 +0.0111 0.0056 3.5343 +0.0222 0.0111 3.5343 +0.0333 0.0167 3.5343 +0.0444 0.0222 3.5343 +0.0556 0.0278 3.5343 +0.0667 0.0333 3.5343 +0.0778 0.0389 3.5343 +0.0889 0.0444 3.5343 +0.1000 0.0500 3.5343 +primID: 3 +startangle_c: 9 +endpose_c: -5 -4 10 +additionalactioncostmult: 2 +intermediateposes: 10 +0.0000 0.0000 3.5343 +-0.0298 -0.0184 3.5646 +-0.0592 -0.0379 3.5949 +-0.0881 -0.0583 3.6252 +-0.1165 -0.0796 3.6555 +-0.1444 -0.1019 3.6858 +-0.1717 -0.1251 3.7161 +-0.1984 -0.1492 3.7464 +-0.2245 -0.1742 3.7767 +-0.2500 -0.2000 3.8070 +primID: 4 +startangle_c: 9 +endpose_c: -7 -2 8 +additionalactioncostmult: 2 +intermediateposes: 10 +0.0000 0.0000 3.5343 +-0.0377 -0.0156 3.5343 +-0.0754 -0.0312 3.5343 +-0.1131 -0.0468 3.5343 +-0.1508 -0.0623 3.5152 +-0.1893 -0.0758 3.4405 +-0.2287 -0.0863 3.3658 +-0.2687 -0.0939 3.2910 +-0.3092 -0.0985 3.2163 +-0.3500 -0.1000 3.1416 +primID: 0 +startangle_c: 10 +endpose_c: -1 -1 10 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 3.9270 +-0.0056 -0.0056 3.9270 +-0.0111 -0.0111 3.9270 +-0.0167 -0.0167 3.9270 +-0.0222 -0.0222 3.9270 +-0.0278 -0.0278 3.9270 +-0.0333 -0.0333 3.9270 +-0.0389 -0.0389 3.9270 +-0.0444 -0.0444 3.9270 +-0.0500 -0.0500 3.9270 +primID: 1 +startangle_c: 10 +endpose_c: -6 -6 10 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 3.9270 +-0.0333 -0.0333 3.9270 +-0.0667 -0.0667 3.9270 +-0.1000 -0.1000 3.9270 +-0.1333 -0.1333 3.9270 +-0.1667 -0.1667 3.9270 +-0.2000 -0.2000 3.9270 +-0.2333 -0.2333 3.9270 +-0.2667 -0.2667 3.9270 +-0.3000 -0.3000 3.9270 +primID: 2 +startangle_c: 10 +endpose_c: 1 1 10 +additionalactioncostmult: 5 +intermediateposes: 10 +0.0000 0.0000 3.9270 +0.0056 0.0056 3.9270 +0.0111 0.0111 3.9270 +0.0167 0.0167 3.9270 +0.0222 0.0222 3.9270 +0.0278 0.0278 3.9270 +0.0333 0.0333 3.9270 +0.0389 0.0389 3.9270 +0.0444 0.0444 3.9270 +0.0500 0.0500 3.9270 +primID: 3 +startangle_c: 10 +endpose_c: -5 -7 11 +additionalactioncostmult: 2 +intermediateposes: 10 +0.0000 0.0000 3.9270 +-0.0341 -0.0341 3.9270 +-0.0678 -0.0684 3.9567 +-0.1000 -0.1043 4.0085 +-0.1302 -0.1418 4.0604 +-0.1584 -0.1809 4.1123 +-0.1846 -0.2213 4.1641 +-0.2086 -0.2631 4.2160 +-0.2304 -0.3060 4.2678 +-0.2500 -0.3500 4.3197 +primID: 4 +startangle_c: 10 +endpose_c: -7 -5 9 +additionalactioncostmult: 2 +intermediateposes: 10 +0.0000 0.0000 3.9270 +-0.0341 -0.0341 3.9270 +-0.0684 -0.0678 3.8973 +-0.1043 -0.1000 3.8455 +-0.1418 -0.1302 3.7936 +-0.1809 -0.1584 3.7417 +-0.2213 -0.1846 3.6899 +-0.2631 -0.2086 3.6380 +-0.3060 -0.2304 3.5862 +-0.3500 -0.2500 3.5343 +primID: 0 +startangle_c: 11 +endpose_c: -1 -2 11 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 4.3197 +-0.0056 -0.0111 4.3197 +-0.0111 -0.0222 4.3197 +-0.0167 -0.0333 4.3197 +-0.0222 -0.0444 4.3197 +-0.0278 -0.0556 4.3197 +-0.0333 -0.0667 4.3197 +-0.0389 -0.0778 4.3197 +-0.0444 -0.0889 4.3197 +-0.0500 -0.1000 4.3197 +primID: 1 +startangle_c: 11 +endpose_c: -3 -6 11 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 4.3197 +-0.0167 -0.0333 4.3197 +-0.0333 -0.0667 4.3197 +-0.0500 -0.1000 4.3197 +-0.0667 -0.1333 4.3197 +-0.0833 -0.1667 4.3197 +-0.1000 -0.2000 4.3197 +-0.1167 -0.2333 4.3197 +-0.1333 -0.2667 4.3197 +-0.1500 -0.3000 4.3197 +primID: 2 +startangle_c: 11 +endpose_c: 1 2 11 +additionalactioncostmult: 5 +intermediateposes: 10 +0.0000 0.0000 4.3197 +0.0056 0.0111 4.3197 +0.0111 0.0222 4.3197 +0.0167 0.0333 4.3197 +0.0222 0.0444 4.3197 +0.0278 0.0556 4.3197 +0.0333 0.0667 4.3197 +0.0389 0.0778 4.3197 +0.0444 0.0889 4.3197 +0.0500 0.1000 4.3197 +primID: 3 +startangle_c: 11 +endpose_c: -4 -5 10 +additionalactioncostmult: 2 +intermediateposes: 10 +0.0000 0.0000 4.3197 +-0.0184 -0.0298 4.2894 +-0.0379 -0.0592 4.2591 +-0.0583 -0.0881 4.2288 +-0.0796 -0.1165 4.1985 +-0.1019 -0.1444 4.1682 +-0.1251 -0.1717 4.1379 +-0.1492 -0.1984 4.1076 +-0.1742 -0.2245 4.0773 +-0.2000 -0.2500 4.0470 +primID: 4 +startangle_c: 11 +endpose_c: -2 -7 12 +additionalactioncostmult: 2 +intermediateposes: 10 +0.0000 0.0000 4.3197 +-0.0156 -0.0377 4.3197 +-0.0312 -0.0754 4.3197 +-0.0468 -0.1131 4.3197 +-0.0623 -0.1508 4.3388 +-0.0758 -0.1893 4.4135 +-0.0863 -0.2287 4.4882 +-0.0939 -0.2687 4.5629 +-0.0985 -0.3092 4.6377 +-0.1000 -0.3500 4.7124 +primID: 0 +startangle_c: 12 +endpose_c: 0 -1 12 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 4.7124 +0.0000 -0.0056 4.7124 +0.0000 -0.0111 4.7124 +0.0000 -0.0167 4.7124 +0.0000 -0.0222 4.7124 +0.0000 -0.0278 4.7124 +0.0000 -0.0333 4.7124 +0.0000 -0.0389 4.7124 +0.0000 -0.0444 4.7124 +0.0000 -0.0500 4.7124 +primID: 1 +startangle_c: 12 +endpose_c: 0 -8 12 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 4.7124 +0.0000 -0.0444 4.7124 +0.0000 -0.0889 4.7124 +0.0000 -0.1333 4.7124 +0.0000 -0.1778 4.7124 +0.0000 -0.2222 4.7124 +0.0000 -0.2667 4.7124 +0.0000 -0.3111 4.7124 +0.0000 -0.3556 4.7124 +0.0000 -0.4000 4.7124 +primID: 2 +startangle_c: 12 +endpose_c: 0 1 12 +additionalactioncostmult: 5 +intermediateposes: 10 +0.0000 0.0000 4.7124 +0.0000 0.0056 4.7124 +0.0000 0.0111 4.7124 +0.0000 0.0167 4.7124 +0.0000 0.0222 4.7124 +0.0000 0.0278 4.7124 +0.0000 0.0333 4.7124 +0.0000 0.0389 4.7124 +0.0000 0.0444 4.7124 +0.0000 0.0500 4.7124 +primID: 3 +startangle_c: 12 +endpose_c: 1 -8 13 +additionalactioncostmult: 2 +intermediateposes: 10 +0.0000 0.0000 4.7124 +-0.0000 -0.0452 4.7124 +-0.0000 -0.0904 4.7124 +-0.0000 -0.1355 4.7124 +0.0008 -0.1807 4.7612 +0.0045 -0.2257 4.8300 +0.0114 -0.2703 4.8988 +0.0213 -0.3144 4.9675 +0.0342 -0.3577 5.0363 +0.0500 -0.4000 5.1051 +primID: 4 +startangle_c: 12 +endpose_c: -1 -8 11 +additionalactioncostmult: 2 +intermediateposes: 10 +0.0000 0.0000 4.7124 +-0.0000 -0.0452 4.7124 +-0.0000 -0.0904 4.7124 +-0.0000 -0.1355 4.7124 +-0.0008 -0.1807 4.6636 +-0.0045 -0.2257 4.5948 +-0.0114 -0.2703 4.5260 +-0.0213 -0.3144 4.4572 +-0.0342 -0.3577 4.3885 +-0.0500 -0.4000 4.3197 +primID: 0 +startangle_c: 13 +endpose_c: 1 -2 13 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 5.1051 +0.0056 -0.0111 5.1051 +0.0111 -0.0222 5.1051 +0.0167 -0.0333 5.1051 +0.0222 -0.0444 5.1051 +0.0278 -0.0556 5.1051 +0.0333 -0.0667 5.1051 +0.0389 -0.0778 5.1051 +0.0444 -0.0889 5.1051 +0.0500 -0.1000 5.1051 +primID: 1 +startangle_c: 13 +endpose_c: 3 -6 13 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 5.1051 +0.0167 -0.0333 5.1051 +0.0333 -0.0667 5.1051 +0.0500 -0.1000 5.1051 +0.0667 -0.1333 5.1051 +0.0833 -0.1667 5.1051 +0.1000 -0.2000 5.1051 +0.1167 -0.2333 5.1051 +0.1333 -0.2667 5.1051 +0.1500 -0.3000 5.1051 +primID: 2 +startangle_c: 13 +endpose_c: -1 2 13 +additionalactioncostmult: 5 +intermediateposes: 10 +0.0000 0.0000 5.1051 +-0.0056 0.0111 5.1051 +-0.0111 0.0222 5.1051 +-0.0167 0.0333 5.1051 +-0.0222 0.0444 5.1051 +-0.0278 0.0556 5.1051 +-0.0333 0.0667 5.1051 +-0.0389 0.0778 5.1051 +-0.0444 0.0889 5.1051 +-0.0500 0.1000 5.1051 +primID: 3 +startangle_c: 13 +endpose_c: 4 -5 14 +additionalactioncostmult: 2 +intermediateposes: 10 +0.0000 0.0000 5.1051 +0.0184 -0.0298 5.1354 +0.0379 -0.0592 5.1657 +0.0583 -0.0881 5.1960 +0.0796 -0.1165 5.2263 +0.1019 -0.1444 5.2566 +0.1251 -0.1717 5.2869 +0.1492 -0.1984 5.3172 +0.1742 -0.2245 5.3475 +0.2000 -0.2500 5.3778 +primID: 4 +startangle_c: 13 +endpose_c: 2 -7 12 +additionalactioncostmult: 2 +intermediateposes: 10 +0.0000 0.0000 5.1051 +0.0156 -0.0377 5.1051 +0.0312 -0.0754 5.1051 +0.0468 -0.1131 5.1051 +0.0623 -0.1508 5.0860 +0.0758 -0.1893 5.0113 +0.0863 -0.2287 4.9366 +0.0939 -0.2687 4.8618 +0.0985 -0.3092 4.7871 +0.1000 -0.3500 4.7124 +primID: 0 +startangle_c: 14 +endpose_c: 1 -1 14 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 5.4978 +0.0056 -0.0056 5.4978 +0.0111 -0.0111 5.4978 +0.0167 -0.0167 5.4978 +0.0222 -0.0222 5.4978 +0.0278 -0.0278 5.4978 +0.0333 -0.0333 5.4978 +0.0389 -0.0389 5.4978 +0.0444 -0.0444 5.4978 +0.0500 -0.0500 5.4978 +primID: 1 +startangle_c: 14 +endpose_c: 6 -6 14 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 5.4978 +0.0333 -0.0333 5.4978 +0.0667 -0.0667 5.4978 +0.1000 -0.1000 5.4978 +0.1333 -0.1333 5.4978 +0.1667 -0.1667 5.4978 +0.2000 -0.2000 5.4978 +0.2333 -0.2333 5.4978 +0.2667 -0.2667 5.4978 +0.3000 -0.3000 5.4978 +primID: 2 +startangle_c: 14 +endpose_c: -1 1 14 +additionalactioncostmult: 5 +intermediateposes: 10 +0.0000 0.0000 5.4978 +-0.0056 0.0056 5.4978 +-0.0111 0.0111 5.4978 +-0.0167 0.0167 5.4978 +-0.0222 0.0222 5.4978 +-0.0278 0.0278 5.4978 +-0.0333 0.0333 5.4978 +-0.0389 0.0389 5.4978 +-0.0444 0.0444 5.4978 +-0.0500 0.0500 5.4978 +primID: 3 +startangle_c: 14 +endpose_c: 7 -5 15 +additionalactioncostmult: 2 +intermediateposes: 10 +0.0000 0.0000 5.4978 +0.0341 -0.0341 5.4978 +0.0684 -0.0678 5.5275 +0.1043 -0.1000 5.5793 +0.1418 -0.1302 5.6312 +0.1809 -0.1584 5.6830 +0.2213 -0.1846 5.7349 +0.2631 -0.2086 5.7868 +0.3060 -0.2304 5.8386 +0.3500 -0.2500 5.8905 +primID: 4 +startangle_c: 14 +endpose_c: 5 -7 13 +additionalactioncostmult: 2 +intermediateposes: 10 +0.0000 0.0000 5.4978 +0.0341 -0.0341 5.4978 +0.0678 -0.0684 5.4681 +0.1000 -0.1043 5.4162 +0.1302 -0.1418 5.3644 +0.1584 -0.1809 5.3125 +0.1846 -0.2213 5.2607 +0.2086 -0.2631 5.2088 +0.2304 -0.3060 5.1569 +0.2500 -0.3500 5.1051 +primID: 0 +startangle_c: 15 +endpose_c: 2 -1 15 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 5.8905 +0.0111 -0.0056 5.8905 +0.0222 -0.0111 5.8905 +0.0333 -0.0167 5.8905 +0.0444 -0.0222 5.8905 +0.0556 -0.0278 5.8905 +0.0667 -0.0333 5.8905 +0.0778 -0.0389 5.8905 +0.0889 -0.0444 5.8905 +0.1000 -0.0500 5.8905 +primID: 1 +startangle_c: 15 +endpose_c: 6 -3 15 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 5.8905 +0.0333 -0.0167 5.8905 +0.0667 -0.0333 5.8905 +0.1000 -0.0500 5.8905 +0.1333 -0.0667 5.8905 +0.1667 -0.0833 5.8905 +0.2000 -0.1000 5.8905 +0.2333 -0.1167 5.8905 +0.2667 -0.1333 5.8905 +0.3000 -0.1500 5.8905 +primID: 2 +startangle_c: 15 +endpose_c: -2 1 15 +additionalactioncostmult: 5 +intermediateposes: 10 +0.0000 0.0000 5.8905 +-0.0111 0.0056 5.8905 +-0.0222 0.0111 5.8905 +-0.0333 0.0167 5.8905 +-0.0444 0.0222 5.8905 +-0.0556 0.0278 5.8905 +-0.0667 0.0333 5.8905 +-0.0778 0.0389 5.8905 +-0.0889 0.0444 5.8905 +-0.1000 0.0500 5.8905 +primID: 3 +startangle_c: 15 +endpose_c: 5 -4 14 +additionalactioncostmult: 2 +intermediateposes: 10 +0.0000 0.0000 5.8905 +0.0298 -0.0184 5.8602 +0.0592 -0.0379 5.8299 +0.0881 -0.0583 5.7996 +0.1165 -0.0796 5.7693 +0.1444 -0.1019 5.7390 +0.1717 -0.1251 5.7087 +0.1984 -0.1492 5.6784 +0.2245 -0.1742 5.6481 +0.2500 -0.2000 5.6178 +primID: 4 +startangle_c: 15 +endpose_c: 7 -2 0 +additionalactioncostmult: 2 +intermediateposes: 10 +0.0000 0.0000 5.8905 +0.0377 -0.0156 5.8905 +0.0754 -0.0312 5.8905 +0.1131 -0.0468 5.8905 +0.1508 -0.0623 5.9096 +0.1893 -0.0758 5.9843 +0.2287 -0.0863 6.0590 +0.2687 -0.0939 6.1337 +0.3092 -0.0985 6.2085 +0.3500 -0.1000 6.2832 diff --git a/Controllers/Packages/amr_startup/config/mprim/unicycle_5cm_expensive_turn_in_place.mprim b/Controllers/Packages/amr_startup/config/mprim/unicycle_5cm_expensive_turn_in_place.mprim new file mode 100755 index 0000000..f27907b --- /dev/null +++ b/Controllers/Packages/amr_startup/config/mprim/unicycle_5cm_expensive_turn_in_place.mprim @@ -0,0 +1,1683 @@ +resolution_m: 0.050000 +numberofangles: 16 +totalnumberofprimitives: 112 +primID: 0 +startangle_c: 0 +endpose_c: 1 0 0 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 0.0000 +0.0056 0.0000 0.0000 +0.0111 0.0000 0.0000 +0.0167 0.0000 0.0000 +0.0222 0.0000 0.0000 +0.0278 0.0000 0.0000 +0.0333 0.0000 0.0000 +0.0389 0.0000 0.0000 +0.0444 0.0000 0.0000 +0.0500 0.0000 0.0000 +primID: 1 +startangle_c: 0 +endpose_c: 8 0 0 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 0.0000 +0.0444 0.0000 0.0000 +0.0889 0.0000 0.0000 +0.1333 0.0000 0.0000 +0.1778 0.0000 0.0000 +0.2222 0.0000 0.0000 +0.2667 0.0000 0.0000 +0.3111 0.0000 0.0000 +0.3556 0.0000 0.0000 +0.4000 0.0000 0.0000 +primID: 2 +startangle_c: 0 +endpose_c: -1 0 0 +additionalactioncostmult: 5 +intermediateposes: 10 +0.0000 0.0000 0.0000 +-0.0056 0.0000 0.0000 +-0.0111 0.0000 0.0000 +-0.0167 0.0000 0.0000 +-0.0222 0.0000 0.0000 +-0.0278 0.0000 0.0000 +-0.0333 0.0000 0.0000 +-0.0389 0.0000 0.0000 +-0.0444 0.0000 0.0000 +-0.0500 0.0000 0.0000 +primID: 3 +startangle_c: 0 +endpose_c: 8 1 1 +additionalactioncostmult: 2 +intermediateposes: 10 +0.0000 0.0000 0.0000 +0.0452 -0.0000 0.0000 +0.0904 -0.0000 0.0000 +0.1355 -0.0000 0.0000 +0.1807 0.0008 0.0488 +0.2257 0.0045 0.1176 +0.2703 0.0114 0.1864 +0.3144 0.0213 0.2551 +0.3577 0.0342 0.3239 +0.4000 0.0500 0.3927 +primID: 4 +startangle_c: 0 +endpose_c: 8 -1 -1 +additionalactioncostmult: 2 +intermediateposes: 10 +0.0000 0.0000 0.0000 +0.0452 0.0000 0.0000 +0.0904 0.0000 0.0000 +0.1355 0.0000 0.0000 +0.1807 -0.0008 -0.0488 +0.2257 -0.0045 -0.1176 +0.2703 -0.0114 -0.1864 +0.3144 -0.0213 -0.2551 +0.3577 -0.0342 -0.3239 +0.4000 -0.0500 -0.3927 +primID: 5 +startangle_c: 0 +endpose_c: 0 0 1 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 0.0000 +0.0000 0.0000 0.0436 +0.0000 0.0000 0.0873 +0.0000 0.0000 0.1309 +0.0000 0.0000 0.1745 +0.0000 0.0000 0.2182 +0.0000 0.0000 0.2618 +0.0000 0.0000 0.3054 +0.0000 0.0000 0.3491 +0.0000 0.0000 0.3927 +primID: 6 +startangle_c: 0 +endpose_c: 0 0 -1 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 0.0000 +0.0000 0.0000 -0.0436 +0.0000 0.0000 -0.0873 +0.0000 0.0000 -0.1309 +0.0000 0.0000 -0.1745 +0.0000 0.0000 -0.2182 +0.0000 0.0000 -0.2618 +0.0000 0.0000 -0.3054 +0.0000 0.0000 -0.3491 +0.0000 0.0000 -0.3927 +primID: 0 +startangle_c: 1 +endpose_c: 2 1 1 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 0.3927 +0.0111 0.0056 0.3927 +0.0222 0.0111 0.3927 +0.0333 0.0167 0.3927 +0.0444 0.0222 0.3927 +0.0556 0.0278 0.3927 +0.0667 0.0333 0.3927 +0.0778 0.0389 0.3927 +0.0889 0.0444 0.3927 +0.1000 0.0500 0.3927 +primID: 1 +startangle_c: 1 +endpose_c: 6 3 1 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 0.3927 +0.0333 0.0167 0.3927 +0.0667 0.0333 0.3927 +0.1000 0.0500 0.3927 +0.1333 0.0667 0.3927 +0.1667 0.0833 0.3927 +0.2000 0.1000 0.3927 +0.2333 0.1167 0.3927 +0.2667 0.1333 0.3927 +0.3000 0.1500 0.3927 +primID: 2 +startangle_c: 1 +endpose_c: -2 -1 1 +additionalactioncostmult: 5 +intermediateposes: 10 +0.0000 0.0000 0.3927 +-0.0111 -0.0056 0.3927 +-0.0222 -0.0111 0.3927 +-0.0333 -0.0167 0.3927 +-0.0444 -0.0222 0.3927 +-0.0556 -0.0278 0.3927 +-0.0667 -0.0333 0.3927 +-0.0778 -0.0389 0.3927 +-0.0889 -0.0444 0.3927 +-0.1000 -0.0500 0.3927 +primID: 3 +startangle_c: 1 +endpose_c: 5 4 2 +additionalactioncostmult: 2 +intermediateposes: 10 +0.0000 0.0000 0.3927 +0.0298 0.0184 0.4230 +0.0592 0.0379 0.4533 +0.0881 0.0583 0.4836 +0.1165 0.0796 0.5139 +0.1444 0.1019 0.5442 +0.1717 0.1251 0.5745 +0.1984 0.1492 0.6048 +0.2245 0.1742 0.6351 +0.2500 0.2000 0.6654 +primID: 4 +startangle_c: 1 +endpose_c: 7 2 0 +additionalactioncostmult: 2 +intermediateposes: 10 +0.0000 0.0000 0.3927 +0.0377 0.0156 0.3927 +0.0754 0.0312 0.3927 +0.1131 0.0468 0.3927 +0.1508 0.0623 0.3736 +0.1893 0.0758 0.2989 +0.2287 0.0863 0.2242 +0.2687 0.0939 0.1494 +0.3092 0.0985 0.0747 +0.3500 0.1000 -0.0000 +primID: 5 +startangle_c: 1 +endpose_c: 0 0 2 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 0.3927 +0.0000 0.0000 0.4363 +0.0000 0.0000 0.4800 +0.0000 0.0000 0.5236 +0.0000 0.0000 0.5672 +0.0000 0.0000 0.6109 +0.0000 0.0000 0.6545 +0.0000 0.0000 0.6981 +0.0000 0.0000 0.7418 +0.0000 0.0000 0.7854 +primID: 6 +startangle_c: 1 +endpose_c: 0 0 0 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 0.3927 +0.0000 0.0000 0.3491 +0.0000 0.0000 0.3054 +0.0000 0.0000 0.2618 +0.0000 0.0000 0.2182 +0.0000 0.0000 0.1745 +0.0000 0.0000 0.1309 +0.0000 0.0000 0.0873 +0.0000 0.0000 0.0436 +0.0000 0.0000 0.0000 +primID: 0 +startangle_c: 2 +endpose_c: 1 1 2 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 0.7854 +0.0056 0.0056 0.7854 +0.0111 0.0111 0.7854 +0.0167 0.0167 0.7854 +0.0222 0.0222 0.7854 +0.0278 0.0278 0.7854 +0.0333 0.0333 0.7854 +0.0389 0.0389 0.7854 +0.0444 0.0444 0.7854 +0.0500 0.0500 0.7854 +primID: 1 +startangle_c: 2 +endpose_c: 6 6 2 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 0.7854 +0.0333 0.0333 0.7854 +0.0667 0.0667 0.7854 +0.1000 0.1000 0.7854 +0.1333 0.1333 0.7854 +0.1667 0.1667 0.7854 +0.2000 0.2000 0.7854 +0.2333 0.2333 0.7854 +0.2667 0.2667 0.7854 +0.3000 0.3000 0.7854 +primID: 2 +startangle_c: 2 +endpose_c: -1 -1 2 +additionalactioncostmult: 5 +intermediateposes: 10 +0.0000 0.0000 0.7854 +-0.0056 -0.0056 0.7854 +-0.0111 -0.0111 0.7854 +-0.0167 -0.0167 0.7854 +-0.0222 -0.0222 0.7854 +-0.0278 -0.0278 0.7854 +-0.0333 -0.0333 0.7854 +-0.0389 -0.0389 0.7854 +-0.0444 -0.0444 0.7854 +-0.0500 -0.0500 0.7854 +primID: 3 +startangle_c: 2 +endpose_c: 5 7 3 +additionalactioncostmult: 2 +intermediateposes: 10 +0.0000 0.0000 0.7854 +0.0341 0.0341 0.7854 +0.0678 0.0684 0.8151 +0.1000 0.1043 0.8669 +0.1302 0.1418 0.9188 +0.1584 0.1809 0.9707 +0.1846 0.2213 1.0225 +0.2086 0.2631 1.0744 +0.2304 0.3060 1.1262 +0.2500 0.3500 1.1781 +primID: 4 +startangle_c: 2 +endpose_c: 7 5 1 +additionalactioncostmult: 2 +intermediateposes: 10 +0.0000 0.0000 0.7854 +0.0341 0.0341 0.7854 +0.0684 0.0678 0.7557 +0.1043 0.1000 0.7039 +0.1418 0.1302 0.6520 +0.1809 0.1584 0.6001 +0.2213 0.1846 0.5483 +0.2631 0.2086 0.4964 +0.3060 0.2304 0.4446 +0.3500 0.2500 0.3927 +primID: 5 +startangle_c: 2 +endpose_c: 0 0 3 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 0.7854 +0.0000 0.0000 0.8290 +0.0000 0.0000 0.8727 +0.0000 0.0000 0.9163 +0.0000 0.0000 0.9599 +0.0000 0.0000 1.0036 +0.0000 0.0000 1.0472 +0.0000 0.0000 1.0908 +0.0000 0.0000 1.1345 +0.0000 0.0000 1.1781 +primID: 6 +startangle_c: 2 +endpose_c: 0 0 1 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 0.7854 +0.0000 0.0000 0.7418 +0.0000 0.0000 0.6981 +0.0000 0.0000 0.6545 +0.0000 0.0000 0.6109 +0.0000 0.0000 0.5672 +0.0000 0.0000 0.5236 +0.0000 0.0000 0.4800 +0.0000 0.0000 0.4363 +0.0000 0.0000 0.3927 +primID: 0 +startangle_c: 3 +endpose_c: 1 2 3 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 1.1781 +0.0056 0.0111 1.1781 +0.0111 0.0222 1.1781 +0.0167 0.0333 1.1781 +0.0222 0.0444 1.1781 +0.0278 0.0556 1.1781 +0.0333 0.0667 1.1781 +0.0389 0.0778 1.1781 +0.0444 0.0889 1.1781 +0.0500 0.1000 1.1781 +primID: 1 +startangle_c: 3 +endpose_c: 3 6 3 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 1.1781 +0.0167 0.0333 1.1781 +0.0333 0.0667 1.1781 +0.0500 0.1000 1.1781 +0.0667 0.1333 1.1781 +0.0833 0.1667 1.1781 +0.1000 0.2000 1.1781 +0.1167 0.2333 1.1781 +0.1333 0.2667 1.1781 +0.1500 0.3000 1.1781 +primID: 2 +startangle_c: 3 +endpose_c: -1 -2 3 +additionalactioncostmult: 5 +intermediateposes: 10 +0.0000 0.0000 1.1781 +-0.0056 -0.0111 1.1781 +-0.0111 -0.0222 1.1781 +-0.0167 -0.0333 1.1781 +-0.0222 -0.0444 1.1781 +-0.0278 -0.0556 1.1781 +-0.0333 -0.0667 1.1781 +-0.0389 -0.0778 1.1781 +-0.0444 -0.0889 1.1781 +-0.0500 -0.1000 1.1781 +primID: 3 +startangle_c: 3 +endpose_c: 4 5 2 +additionalactioncostmult: 2 +intermediateposes: 10 +0.0000 0.0000 1.1781 +0.0184 0.0298 1.1478 +0.0379 0.0592 1.1175 +0.0583 0.0881 1.0872 +0.0796 0.1165 1.0569 +0.1019 0.1444 1.0266 +0.1251 0.1717 0.9963 +0.1492 0.1984 0.9660 +0.1742 0.2245 0.9357 +0.2000 0.2500 0.9054 +primID: 4 +startangle_c: 3 +endpose_c: 2 7 4 +additionalactioncostmult: 2 +intermediateposes: 10 +0.0000 0.0000 1.1781 +0.0156 0.0377 1.1781 +0.0312 0.0754 1.1781 +0.0468 0.1131 1.1781 +0.0623 0.1508 1.1972 +0.0758 0.1893 1.2719 +0.0863 0.2287 1.3466 +0.0939 0.2687 1.4214 +0.0985 0.3092 1.4961 +0.1000 0.3500 1.5708 +primID: 5 +startangle_c: 3 +endpose_c: 0 0 2 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 1.1781 +0.0000 0.0000 1.1345 +0.0000 0.0000 1.0908 +0.0000 0.0000 1.0472 +0.0000 0.0000 1.0036 +0.0000 0.0000 0.9599 +0.0000 0.0000 0.9163 +0.0000 0.0000 0.8727 +0.0000 0.0000 0.8290 +0.0000 0.0000 0.7854 +primID: 6 +startangle_c: 3 +endpose_c: 0 0 4 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 1.1781 +0.0000 0.0000 1.2217 +0.0000 0.0000 1.2654 +0.0000 0.0000 1.3090 +0.0000 0.0000 1.3526 +0.0000 0.0000 1.3963 +0.0000 0.0000 1.4399 +0.0000 0.0000 1.4835 +0.0000 0.0000 1.5272 +0.0000 0.0000 1.5708 +primID: 0 +startangle_c: 4 +endpose_c: 0 1 4 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 1.5708 +0.0000 0.0056 1.5708 +0.0000 0.0111 1.5708 +0.0000 0.0167 1.5708 +0.0000 0.0222 1.5708 +0.0000 0.0278 1.5708 +0.0000 0.0333 1.5708 +0.0000 0.0389 1.5708 +0.0000 0.0444 1.5708 +0.0000 0.0500 1.5708 +primID: 1 +startangle_c: 4 +endpose_c: 0 8 4 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 1.5708 +0.0000 0.0444 1.5708 +0.0000 0.0889 1.5708 +0.0000 0.1333 1.5708 +0.0000 0.1778 1.5708 +0.0000 0.2222 1.5708 +0.0000 0.2667 1.5708 +0.0000 0.3111 1.5708 +0.0000 0.3556 1.5708 +0.0000 0.4000 1.5708 +primID: 2 +startangle_c: 4 +endpose_c: 0 -1 4 +additionalactioncostmult: 5 +intermediateposes: 10 +0.0000 0.0000 1.5708 +0.0000 -0.0056 1.5708 +0.0000 -0.0111 1.5708 +0.0000 -0.0167 1.5708 +0.0000 -0.0222 1.5708 +0.0000 -0.0278 1.5708 +0.0000 -0.0333 1.5708 +0.0000 -0.0389 1.5708 +0.0000 -0.0444 1.5708 +0.0000 -0.0500 1.5708 +primID: 3 +startangle_c: 4 +endpose_c: -1 8 5 +additionalactioncostmult: 2 +intermediateposes: 10 +0.0000 0.0000 1.5708 +0.0000 0.0452 1.5708 +0.0000 0.0904 1.5708 +0.0000 0.1355 1.5708 +-0.0008 0.1807 1.6196 +-0.0045 0.2257 1.6884 +-0.0114 0.2703 1.7572 +-0.0213 0.3144 1.8259 +-0.0342 0.3577 1.8947 +-0.0500 0.4000 1.9635 +primID: 4 +startangle_c: 4 +endpose_c: 1 8 3 +additionalactioncostmult: 2 +intermediateposes: 10 +0.0000 0.0000 1.5708 +0.0000 0.0452 1.5708 +0.0000 0.0904 1.5708 +0.0000 0.1355 1.5708 +0.0008 0.1807 1.5220 +0.0045 0.2257 1.4532 +0.0114 0.2703 1.3844 +0.0213 0.3144 1.3156 +0.0342 0.3577 1.2469 +0.0500 0.4000 1.1781 +primID: 5 +startangle_c: 4 +endpose_c: 0 0 5 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 1.5708 +0.0000 0.0000 1.6144 +0.0000 0.0000 1.6581 +0.0000 0.0000 1.7017 +0.0000 0.0000 1.7453 +0.0000 0.0000 1.7890 +0.0000 0.0000 1.8326 +0.0000 0.0000 1.8762 +0.0000 0.0000 1.9199 +0.0000 0.0000 1.9635 +primID: 6 +startangle_c: 4 +endpose_c: 0 0 3 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 1.5708 +0.0000 0.0000 1.5272 +0.0000 0.0000 1.4835 +0.0000 0.0000 1.4399 +0.0000 0.0000 1.3963 +0.0000 0.0000 1.3526 +0.0000 0.0000 1.3090 +0.0000 0.0000 1.2654 +0.0000 0.0000 1.2217 +0.0000 0.0000 1.1781 +primID: 0 +startangle_c: 5 +endpose_c: -1 2 5 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 1.9635 +-0.0056 0.0111 1.9635 +-0.0111 0.0222 1.9635 +-0.0167 0.0333 1.9635 +-0.0222 0.0444 1.9635 +-0.0278 0.0556 1.9635 +-0.0333 0.0667 1.9635 +-0.0389 0.0778 1.9635 +-0.0444 0.0889 1.9635 +-0.0500 0.1000 1.9635 +primID: 1 +startangle_c: 5 +endpose_c: -3 6 5 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 1.9635 +-0.0167 0.0333 1.9635 +-0.0333 0.0667 1.9635 +-0.0500 0.1000 1.9635 +-0.0667 0.1333 1.9635 +-0.0833 0.1667 1.9635 +-0.1000 0.2000 1.9635 +-0.1167 0.2333 1.9635 +-0.1333 0.2667 1.9635 +-0.1500 0.3000 1.9635 +primID: 2 +startangle_c: 5 +endpose_c: 1 -2 5 +additionalactioncostmult: 5 +intermediateposes: 10 +0.0000 0.0000 1.9635 +0.0056 -0.0111 1.9635 +0.0111 -0.0222 1.9635 +0.0167 -0.0333 1.9635 +0.0222 -0.0444 1.9635 +0.0278 -0.0556 1.9635 +0.0333 -0.0667 1.9635 +0.0389 -0.0778 1.9635 +0.0444 -0.0889 1.9635 +0.0500 -0.1000 1.9635 +primID: 3 +startangle_c: 5 +endpose_c: -4 5 6 +additionalactioncostmult: 2 +intermediateposes: 10 +0.0000 0.0000 1.9635 +-0.0184 0.0298 1.9938 +-0.0379 0.0592 2.0241 +-0.0583 0.0881 2.0544 +-0.0796 0.1165 2.0847 +-0.1019 0.1444 2.1150 +-0.1251 0.1717 2.1453 +-0.1492 0.1984 2.1756 +-0.1742 0.2245 2.2059 +-0.2000 0.2500 2.2362 +primID: 4 +startangle_c: 5 +endpose_c: -2 7 4 +additionalactioncostmult: 2 +intermediateposes: 10 +0.0000 0.0000 1.9635 +-0.0156 0.0377 1.9635 +-0.0312 0.0754 1.9635 +-0.0468 0.1131 1.9635 +-0.0623 0.1508 1.9444 +-0.0758 0.1893 1.8697 +-0.0863 0.2287 1.7950 +-0.0939 0.2687 1.7202 +-0.0985 0.3092 1.6455 +-0.1000 0.3500 1.5708 +primID: 5 +startangle_c: 5 +endpose_c: 0 0 6 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 1.9635 +0.0000 0.0000 2.0071 +0.0000 0.0000 2.0508 +0.0000 0.0000 2.0944 +0.0000 0.0000 2.1380 +0.0000 0.0000 2.1817 +0.0000 0.0000 2.2253 +0.0000 0.0000 2.2689 +0.0000 0.0000 2.3126 +0.0000 0.0000 2.3562 +primID: 6 +startangle_c: 5 +endpose_c: 0 0 4 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 1.9635 +0.0000 0.0000 1.9199 +0.0000 0.0000 1.8762 +0.0000 0.0000 1.8326 +0.0000 0.0000 1.7890 +0.0000 0.0000 1.7453 +0.0000 0.0000 1.7017 +0.0000 0.0000 1.6581 +0.0000 0.0000 1.6144 +0.0000 0.0000 1.5708 +primID: 0 +startangle_c: 6 +endpose_c: -1 1 6 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 2.3562 +-0.0056 0.0056 2.3562 +-0.0111 0.0111 2.3562 +-0.0167 0.0167 2.3562 +-0.0222 0.0222 2.3562 +-0.0278 0.0278 2.3562 +-0.0333 0.0333 2.3562 +-0.0389 0.0389 2.3562 +-0.0444 0.0444 2.3562 +-0.0500 0.0500 2.3562 +primID: 1 +startangle_c: 6 +endpose_c: -6 6 6 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 2.3562 +-0.0333 0.0333 2.3562 +-0.0667 0.0667 2.3562 +-0.1000 0.1000 2.3562 +-0.1333 0.1333 2.3562 +-0.1667 0.1667 2.3562 +-0.2000 0.2000 2.3562 +-0.2333 0.2333 2.3562 +-0.2667 0.2667 2.3562 +-0.3000 0.3000 2.3562 +primID: 2 +startangle_c: 6 +endpose_c: 1 -1 6 +additionalactioncostmult: 5 +intermediateposes: 10 +0.0000 0.0000 2.3562 +0.0056 -0.0056 2.3562 +0.0111 -0.0111 2.3562 +0.0167 -0.0167 2.3562 +0.0222 -0.0222 2.3562 +0.0278 -0.0278 2.3562 +0.0333 -0.0333 2.3562 +0.0389 -0.0389 2.3562 +0.0444 -0.0444 2.3562 +0.0500 -0.0500 2.3562 +primID: 3 +startangle_c: 6 +endpose_c: -7 5 7 +additionalactioncostmult: 2 +intermediateposes: 10 +0.0000 0.0000 2.3562 +-0.0341 0.0341 2.3562 +-0.0684 0.0678 2.3859 +-0.1043 0.1000 2.4377 +-0.1418 0.1302 2.4896 +-0.1809 0.1584 2.5415 +-0.2213 0.1846 2.5933 +-0.2631 0.2086 2.6452 +-0.3060 0.2304 2.6970 +-0.3500 0.2500 2.7489 +primID: 4 +startangle_c: 6 +endpose_c: -5 7 5 +additionalactioncostmult: 2 +intermediateposes: 10 +0.0000 0.0000 2.3562 +-0.0341 0.0341 2.3562 +-0.0678 0.0684 2.3265 +-0.1000 0.1043 2.2747 +-0.1302 0.1418 2.2228 +-0.1584 0.1809 2.1709 +-0.1846 0.2213 2.1191 +-0.2086 0.2631 2.0672 +-0.2304 0.3060 2.0154 +-0.2500 0.3500 1.9635 +primID: 5 +startangle_c: 6 +endpose_c: 0 0 7 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 2.3562 +0.0000 0.0000 2.3998 +0.0000 0.0000 2.4435 +0.0000 0.0000 2.4871 +0.0000 0.0000 2.5307 +0.0000 0.0000 2.5744 +0.0000 0.0000 2.6180 +0.0000 0.0000 2.6616 +0.0000 0.0000 2.7053 +0.0000 0.0000 2.7489 +primID: 6 +startangle_c: 6 +endpose_c: 0 0 5 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 2.3562 +0.0000 0.0000 2.3126 +0.0000 0.0000 2.2689 +0.0000 0.0000 2.2253 +0.0000 0.0000 2.1817 +0.0000 0.0000 2.1380 +0.0000 0.0000 2.0944 +0.0000 0.0000 2.0508 +0.0000 0.0000 2.0071 +0.0000 0.0000 1.9635 +primID: 0 +startangle_c: 7 +endpose_c: -2 1 7 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 2.7489 +-0.0111 0.0056 2.7489 +-0.0222 0.0111 2.7489 +-0.0333 0.0167 2.7489 +-0.0444 0.0222 2.7489 +-0.0556 0.0278 2.7489 +-0.0667 0.0333 2.7489 +-0.0778 0.0389 2.7489 +-0.0889 0.0444 2.7489 +-0.1000 0.0500 2.7489 +primID: 1 +startangle_c: 7 +endpose_c: -6 3 7 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 2.7489 +-0.0333 0.0167 2.7489 +-0.0667 0.0333 2.7489 +-0.1000 0.0500 2.7489 +-0.1333 0.0667 2.7489 +-0.1667 0.0833 2.7489 +-0.2000 0.1000 2.7489 +-0.2333 0.1167 2.7489 +-0.2667 0.1333 2.7489 +-0.3000 0.1500 2.7489 +primID: 2 +startangle_c: 7 +endpose_c: 2 -1 7 +additionalactioncostmult: 5 +intermediateposes: 10 +0.0000 0.0000 2.7489 +0.0111 -0.0056 2.7489 +0.0222 -0.0111 2.7489 +0.0333 -0.0167 2.7489 +0.0444 -0.0222 2.7489 +0.0556 -0.0278 2.7489 +0.0667 -0.0333 2.7489 +0.0778 -0.0389 2.7489 +0.0889 -0.0444 2.7489 +0.1000 -0.0500 2.7489 +primID: 3 +startangle_c: 7 +endpose_c: -5 4 6 +additionalactioncostmult: 2 +intermediateposes: 10 +0.0000 0.0000 2.7489 +-0.0298 0.0184 2.7186 +-0.0592 0.0379 2.6883 +-0.0881 0.0583 2.6580 +-0.1165 0.0796 2.6277 +-0.1444 0.1019 2.5974 +-0.1717 0.1251 2.5671 +-0.1984 0.1492 2.5368 +-0.2245 0.1742 2.5065 +-0.2500 0.2000 2.4762 +primID: 4 +startangle_c: 7 +endpose_c: -7 2 8 +additionalactioncostmult: 2 +intermediateposes: 10 +0.0000 0.0000 2.7489 +-0.0377 0.0156 2.7489 +-0.0754 0.0312 2.7489 +-0.1131 0.0468 2.7489 +-0.1508 0.0623 2.7680 +-0.1893 0.0758 2.8427 +-0.2287 0.0863 2.9174 +-0.2687 0.0939 2.9921 +-0.3092 0.0985 3.0669 +-0.3500 0.1000 3.1416 +primID: 5 +startangle_c: 7 +endpose_c: 0 0 6 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 2.7489 +0.0000 0.0000 2.7053 +0.0000 0.0000 2.6616 +0.0000 0.0000 2.6180 +0.0000 0.0000 2.5744 +0.0000 0.0000 2.5307 +0.0000 0.0000 2.4871 +0.0000 0.0000 2.4435 +0.0000 0.0000 2.3998 +0.0000 0.0000 2.3562 +primID: 6 +startangle_c: 7 +endpose_c: 0 0 8 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 2.7489 +0.0000 0.0000 2.7925 +0.0000 0.0000 2.8362 +0.0000 0.0000 2.8798 +0.0000 0.0000 2.9234 +0.0000 0.0000 2.9671 +0.0000 0.0000 3.0107 +0.0000 0.0000 3.0543 +0.0000 0.0000 3.0980 +0.0000 0.0000 3.1416 +primID: 0 +startangle_c: 8 +endpose_c: -1 0 8 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 3.1416 +-0.0056 0.0000 3.1416 +-0.0111 0.0000 3.1416 +-0.0167 0.0000 3.1416 +-0.0222 0.0000 3.1416 +-0.0278 0.0000 3.1416 +-0.0333 0.0000 3.1416 +-0.0389 0.0000 3.1416 +-0.0444 0.0000 3.1416 +-0.0500 0.0000 3.1416 +primID: 1 +startangle_c: 8 +endpose_c: -8 0 8 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 3.1416 +-0.0444 0.0000 3.1416 +-0.0889 0.0000 3.1416 +-0.1333 0.0000 3.1416 +-0.1778 0.0000 3.1416 +-0.2222 0.0000 3.1416 +-0.2667 0.0000 3.1416 +-0.3111 0.0000 3.1416 +-0.3556 0.0000 3.1416 +-0.4000 0.0000 3.1416 +primID: 2 +startangle_c: 8 +endpose_c: 1 0 8 +additionalactioncostmult: 5 +intermediateposes: 10 +0.0000 0.0000 3.1416 +0.0056 0.0000 3.1416 +0.0111 0.0000 3.1416 +0.0167 0.0000 3.1416 +0.0222 0.0000 3.1416 +0.0278 0.0000 3.1416 +0.0333 0.0000 3.1416 +0.0389 0.0000 3.1416 +0.0444 0.0000 3.1416 +0.0500 0.0000 3.1416 +primID: 3 +startangle_c: 8 +endpose_c: -8 -1 9 +additionalactioncostmult: 2 +intermediateposes: 10 +0.0000 0.0000 3.1416 +-0.0452 0.0000 3.1416 +-0.0904 0.0000 3.1416 +-0.1355 0.0000 3.1416 +-0.1807 -0.0008 3.1904 +-0.2257 -0.0045 3.2592 +-0.2703 -0.0114 3.3280 +-0.3144 -0.0213 3.3967 +-0.3577 -0.0342 3.4655 +-0.4000 -0.0500 3.5343 +primID: 4 +startangle_c: 8 +endpose_c: -8 1 7 +additionalactioncostmult: 2 +intermediateposes: 10 +0.0000 0.0000 3.1416 +-0.0452 0.0000 3.1416 +-0.0904 0.0000 3.1416 +-0.1355 0.0000 3.1416 +-0.1807 0.0008 3.0928 +-0.2257 0.0045 3.0240 +-0.2703 0.0114 2.9552 +-0.3144 0.0213 2.8864 +-0.3577 0.0342 2.8177 +-0.4000 0.0500 2.7489 +primID: 5 +startangle_c: 8 +endpose_c: 0 0 9 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 3.1416 +0.0000 0.0000 3.1852 +0.0000 0.0000 3.2289 +0.0000 0.0000 3.2725 +0.0000 0.0000 3.3161 +0.0000 0.0000 3.3598 +0.0000 0.0000 3.4034 +0.0000 0.0000 3.4470 +0.0000 0.0000 3.4907 +0.0000 0.0000 3.5343 +primID: 6 +startangle_c: 8 +endpose_c: 0 0 7 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 3.1416 +0.0000 0.0000 3.0980 +0.0000 0.0000 3.0543 +0.0000 0.0000 3.0107 +0.0000 0.0000 2.9671 +0.0000 0.0000 2.9234 +0.0000 0.0000 2.8798 +0.0000 0.0000 2.8362 +0.0000 0.0000 2.7925 +0.0000 0.0000 2.7489 +primID: 0 +startangle_c: 9 +endpose_c: -2 -1 9 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 3.5343 +-0.0111 -0.0056 3.5343 +-0.0222 -0.0111 3.5343 +-0.0333 -0.0167 3.5343 +-0.0444 -0.0222 3.5343 +-0.0556 -0.0278 3.5343 +-0.0667 -0.0333 3.5343 +-0.0778 -0.0389 3.5343 +-0.0889 -0.0444 3.5343 +-0.1000 -0.0500 3.5343 +primID: 1 +startangle_c: 9 +endpose_c: -6 -3 9 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 3.5343 +-0.0333 -0.0167 3.5343 +-0.0667 -0.0333 3.5343 +-0.1000 -0.0500 3.5343 +-0.1333 -0.0667 3.5343 +-0.1667 -0.0833 3.5343 +-0.2000 -0.1000 3.5343 +-0.2333 -0.1167 3.5343 +-0.2667 -0.1333 3.5343 +-0.3000 -0.1500 3.5343 +primID: 2 +startangle_c: 9 +endpose_c: 2 1 9 +additionalactioncostmult: 5 +intermediateposes: 10 +0.0000 0.0000 3.5343 +0.0111 0.0056 3.5343 +0.0222 0.0111 3.5343 +0.0333 0.0167 3.5343 +0.0444 0.0222 3.5343 +0.0556 0.0278 3.5343 +0.0667 0.0333 3.5343 +0.0778 0.0389 3.5343 +0.0889 0.0444 3.5343 +0.1000 0.0500 3.5343 +primID: 3 +startangle_c: 9 +endpose_c: -5 -4 10 +additionalactioncostmult: 2 +intermediateposes: 10 +0.0000 0.0000 3.5343 +-0.0298 -0.0184 3.5646 +-0.0592 -0.0379 3.5949 +-0.0881 -0.0583 3.6252 +-0.1165 -0.0796 3.6555 +-0.1444 -0.1019 3.6858 +-0.1717 -0.1251 3.7161 +-0.1984 -0.1492 3.7464 +-0.2245 -0.1742 3.7767 +-0.2500 -0.2000 3.8070 +primID: 4 +startangle_c: 9 +endpose_c: -7 -2 8 +additionalactioncostmult: 2 +intermediateposes: 10 +0.0000 0.0000 3.5343 +-0.0377 -0.0156 3.5343 +-0.0754 -0.0312 3.5343 +-0.1131 -0.0468 3.5343 +-0.1508 -0.0623 3.5152 +-0.1893 -0.0758 3.4405 +-0.2287 -0.0863 3.3658 +-0.2687 -0.0939 3.2910 +-0.3092 -0.0985 3.2163 +-0.3500 -0.1000 3.1416 +primID: 5 +startangle_c: 9 +endpose_c: 0 0 10 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 3.5343 +0.0000 0.0000 3.5779 +0.0000 0.0000 3.6216 +0.0000 0.0000 3.6652 +0.0000 0.0000 3.7088 +0.0000 0.0000 3.7525 +0.0000 0.0000 3.7961 +0.0000 0.0000 3.8397 +0.0000 0.0000 3.8834 +0.0000 0.0000 3.9270 +primID: 6 +startangle_c: 9 +endpose_c: 0 0 8 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 3.5343 +0.0000 0.0000 3.4907 +0.0000 0.0000 3.4470 +0.0000 0.0000 3.4034 +0.0000 0.0000 3.3598 +0.0000 0.0000 3.3161 +0.0000 0.0000 3.2725 +0.0000 0.0000 3.2289 +0.0000 0.0000 3.1852 +0.0000 0.0000 3.1416 +primID: 0 +startangle_c: 10 +endpose_c: -1 -1 10 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 3.9270 +-0.0056 -0.0056 3.9270 +-0.0111 -0.0111 3.9270 +-0.0167 -0.0167 3.9270 +-0.0222 -0.0222 3.9270 +-0.0278 -0.0278 3.9270 +-0.0333 -0.0333 3.9270 +-0.0389 -0.0389 3.9270 +-0.0444 -0.0444 3.9270 +-0.0500 -0.0500 3.9270 +primID: 1 +startangle_c: 10 +endpose_c: -6 -6 10 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 3.9270 +-0.0333 -0.0333 3.9270 +-0.0667 -0.0667 3.9270 +-0.1000 -0.1000 3.9270 +-0.1333 -0.1333 3.9270 +-0.1667 -0.1667 3.9270 +-0.2000 -0.2000 3.9270 +-0.2333 -0.2333 3.9270 +-0.2667 -0.2667 3.9270 +-0.3000 -0.3000 3.9270 +primID: 2 +startangle_c: 10 +endpose_c: 1 1 10 +additionalactioncostmult: 5 +intermediateposes: 10 +0.0000 0.0000 3.9270 +0.0056 0.0056 3.9270 +0.0111 0.0111 3.9270 +0.0167 0.0167 3.9270 +0.0222 0.0222 3.9270 +0.0278 0.0278 3.9270 +0.0333 0.0333 3.9270 +0.0389 0.0389 3.9270 +0.0444 0.0444 3.9270 +0.0500 0.0500 3.9270 +primID: 3 +startangle_c: 10 +endpose_c: -5 -7 11 +additionalactioncostmult: 2 +intermediateposes: 10 +0.0000 0.0000 3.9270 +-0.0341 -0.0341 3.9270 +-0.0678 -0.0684 3.9567 +-0.1000 -0.1043 4.0085 +-0.1302 -0.1418 4.0604 +-0.1584 -0.1809 4.1123 +-0.1846 -0.2213 4.1641 +-0.2086 -0.2631 4.2160 +-0.2304 -0.3060 4.2678 +-0.2500 -0.3500 4.3197 +primID: 4 +startangle_c: 10 +endpose_c: -7 -5 9 +additionalactioncostmult: 2 +intermediateposes: 10 +0.0000 0.0000 3.9270 +-0.0341 -0.0341 3.9270 +-0.0684 -0.0678 3.8973 +-0.1043 -0.1000 3.8455 +-0.1418 -0.1302 3.7936 +-0.1809 -0.1584 3.7417 +-0.2213 -0.1846 3.6899 +-0.2631 -0.2086 3.6380 +-0.3060 -0.2304 3.5862 +-0.3500 -0.2500 3.5343 +primID: 5 +startangle_c: 10 +endpose_c: 0 0 11 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 3.9270 +0.0000 0.0000 3.9706 +0.0000 0.0000 4.0143 +0.0000 0.0000 4.0579 +0.0000 0.0000 4.1015 +0.0000 0.0000 4.1452 +0.0000 0.0000 4.1888 +0.0000 0.0000 4.2324 +0.0000 0.0000 4.2761 +0.0000 0.0000 4.3197 +primID: 6 +startangle_c: 10 +endpose_c: 0 0 9 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 3.9270 +0.0000 0.0000 3.8834 +0.0000 0.0000 3.8397 +0.0000 0.0000 3.7961 +0.0000 0.0000 3.7525 +0.0000 0.0000 3.7088 +0.0000 0.0000 3.6652 +0.0000 0.0000 3.6216 +0.0000 0.0000 3.5779 +0.0000 0.0000 3.5343 +primID: 0 +startangle_c: 11 +endpose_c: -1 -2 11 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 4.3197 +-0.0056 -0.0111 4.3197 +-0.0111 -0.0222 4.3197 +-0.0167 -0.0333 4.3197 +-0.0222 -0.0444 4.3197 +-0.0278 -0.0556 4.3197 +-0.0333 -0.0667 4.3197 +-0.0389 -0.0778 4.3197 +-0.0444 -0.0889 4.3197 +-0.0500 -0.1000 4.3197 +primID: 1 +startangle_c: 11 +endpose_c: -3 -6 11 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 4.3197 +-0.0167 -0.0333 4.3197 +-0.0333 -0.0667 4.3197 +-0.0500 -0.1000 4.3197 +-0.0667 -0.1333 4.3197 +-0.0833 -0.1667 4.3197 +-0.1000 -0.2000 4.3197 +-0.1167 -0.2333 4.3197 +-0.1333 -0.2667 4.3197 +-0.1500 -0.3000 4.3197 +primID: 2 +startangle_c: 11 +endpose_c: 1 2 11 +additionalactioncostmult: 5 +intermediateposes: 10 +0.0000 0.0000 4.3197 +0.0056 0.0111 4.3197 +0.0111 0.0222 4.3197 +0.0167 0.0333 4.3197 +0.0222 0.0444 4.3197 +0.0278 0.0556 4.3197 +0.0333 0.0667 4.3197 +0.0389 0.0778 4.3197 +0.0444 0.0889 4.3197 +0.0500 0.1000 4.3197 +primID: 3 +startangle_c: 11 +endpose_c: -4 -5 10 +additionalactioncostmult: 2 +intermediateposes: 10 +0.0000 0.0000 4.3197 +-0.0184 -0.0298 4.2894 +-0.0379 -0.0592 4.2591 +-0.0583 -0.0881 4.2288 +-0.0796 -0.1165 4.1985 +-0.1019 -0.1444 4.1682 +-0.1251 -0.1717 4.1379 +-0.1492 -0.1984 4.1076 +-0.1742 -0.2245 4.0773 +-0.2000 -0.2500 4.0470 +primID: 4 +startangle_c: 11 +endpose_c: -2 -7 12 +additionalactioncostmult: 2 +intermediateposes: 10 +0.0000 0.0000 4.3197 +-0.0156 -0.0377 4.3197 +-0.0312 -0.0754 4.3197 +-0.0468 -0.1131 4.3197 +-0.0623 -0.1508 4.3388 +-0.0758 -0.1893 4.4135 +-0.0863 -0.2287 4.4882 +-0.0939 -0.2687 4.5629 +-0.0985 -0.3092 4.6377 +-0.1000 -0.3500 4.7124 +primID: 5 +startangle_c: 11 +endpose_c: 0 0 10 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 4.3197 +0.0000 0.0000 4.2761 +0.0000 0.0000 4.2324 +0.0000 0.0000 4.1888 +0.0000 0.0000 4.1452 +0.0000 0.0000 4.1015 +0.0000 0.0000 4.0579 +0.0000 0.0000 4.0143 +0.0000 0.0000 3.9706 +0.0000 0.0000 3.9270 +primID: 6 +startangle_c: 11 +endpose_c: 0 0 12 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 4.3197 +0.0000 0.0000 4.3633 +0.0000 0.0000 4.4070 +0.0000 0.0000 4.4506 +0.0000 0.0000 4.4942 +0.0000 0.0000 4.5379 +0.0000 0.0000 4.5815 +0.0000 0.0000 4.6251 +0.0000 0.0000 4.6688 +0.0000 0.0000 4.7124 +primID: 0 +startangle_c: 12 +endpose_c: 0 -1 12 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 4.7124 +0.0000 -0.0056 4.7124 +0.0000 -0.0111 4.7124 +0.0000 -0.0167 4.7124 +0.0000 -0.0222 4.7124 +0.0000 -0.0278 4.7124 +0.0000 -0.0333 4.7124 +0.0000 -0.0389 4.7124 +0.0000 -0.0444 4.7124 +0.0000 -0.0500 4.7124 +primID: 1 +startangle_c: 12 +endpose_c: 0 -8 12 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 4.7124 +0.0000 -0.0444 4.7124 +0.0000 -0.0889 4.7124 +0.0000 -0.1333 4.7124 +0.0000 -0.1778 4.7124 +0.0000 -0.2222 4.7124 +0.0000 -0.2667 4.7124 +0.0000 -0.3111 4.7124 +0.0000 -0.3556 4.7124 +0.0000 -0.4000 4.7124 +primID: 2 +startangle_c: 12 +endpose_c: 0 1 12 +additionalactioncostmult: 5 +intermediateposes: 10 +0.0000 0.0000 4.7124 +0.0000 0.0056 4.7124 +0.0000 0.0111 4.7124 +0.0000 0.0167 4.7124 +0.0000 0.0222 4.7124 +0.0000 0.0278 4.7124 +0.0000 0.0333 4.7124 +0.0000 0.0389 4.7124 +0.0000 0.0444 4.7124 +0.0000 0.0500 4.7124 +primID: 3 +startangle_c: 12 +endpose_c: 1 -8 13 +additionalactioncostmult: 2 +intermediateposes: 10 +0.0000 0.0000 4.7124 +0.0000 -0.0452 4.7124 +0.0000 -0.0904 4.7124 +0.0000 -0.1355 4.7124 +0.0008 -0.1807 4.7612 +0.0045 -0.2257 4.8300 +0.0114 -0.2703 4.8988 +0.0213 -0.3144 4.9675 +0.0342 -0.3577 5.0363 +0.0500 -0.4000 5.1051 +primID: 4 +startangle_c: 12 +endpose_c: -1 -8 11 +additionalactioncostmult: 2 +intermediateposes: 10 +0.0000 0.0000 4.7124 +-0.0000 -0.0452 4.7124 +-0.0000 -0.0904 4.7124 +-0.0000 -0.1355 4.7124 +-0.0008 -0.1807 4.6636 +-0.0045 -0.2257 4.5948 +-0.0114 -0.2703 4.5260 +-0.0213 -0.3144 4.4572 +-0.0342 -0.3577 4.3885 +-0.0500 -0.4000 4.3197 +primID: 5 +startangle_c: 12 +endpose_c: 0 0 13 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 4.7124 +0.0000 0.0000 4.7560 +0.0000 0.0000 4.7997 +0.0000 0.0000 4.8433 +0.0000 0.0000 4.8869 +0.0000 0.0000 4.9306 +0.0000 0.0000 4.9742 +0.0000 0.0000 5.0178 +0.0000 0.0000 5.0615 +0.0000 0.0000 5.1051 +primID: 6 +startangle_c: 12 +endpose_c: 0 0 11 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 4.7124 +0.0000 0.0000 4.6688 +0.0000 0.0000 4.6251 +0.0000 0.0000 4.5815 +0.0000 0.0000 4.5379 +0.0000 0.0000 4.4942 +0.0000 0.0000 4.4506 +0.0000 0.0000 4.4070 +0.0000 0.0000 4.3633 +0.0000 0.0000 4.3197 +primID: 0 +startangle_c: 13 +endpose_c: 1 -2 13 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 5.1051 +0.0056 -0.0111 5.1051 +0.0111 -0.0222 5.1051 +0.0167 -0.0333 5.1051 +0.0222 -0.0444 5.1051 +0.0278 -0.0556 5.1051 +0.0333 -0.0667 5.1051 +0.0389 -0.0778 5.1051 +0.0444 -0.0889 5.1051 +0.0500 -0.1000 5.1051 +primID: 1 +startangle_c: 13 +endpose_c: 3 -6 13 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 5.1051 +0.0167 -0.0333 5.1051 +0.0333 -0.0667 5.1051 +0.0500 -0.1000 5.1051 +0.0667 -0.1333 5.1051 +0.0833 -0.1667 5.1051 +0.1000 -0.2000 5.1051 +0.1167 -0.2333 5.1051 +0.1333 -0.2667 5.1051 +0.1500 -0.3000 5.1051 +primID: 2 +startangle_c: 13 +endpose_c: -1 2 13 +additionalactioncostmult: 5 +intermediateposes: 10 +0.0000 0.0000 5.1051 +-0.0056 0.0111 5.1051 +-0.0111 0.0222 5.1051 +-0.0167 0.0333 5.1051 +-0.0222 0.0444 5.1051 +-0.0278 0.0556 5.1051 +-0.0333 0.0667 5.1051 +-0.0389 0.0778 5.1051 +-0.0444 0.0889 5.1051 +-0.0500 0.1000 5.1051 +primID: 3 +startangle_c: 13 +endpose_c: 4 -5 14 +additionalactioncostmult: 2 +intermediateposes: 10 +0.0000 0.0000 5.1051 +0.0184 -0.0298 5.1354 +0.0379 -0.0592 5.1657 +0.0583 -0.0881 5.1960 +0.0796 -0.1165 5.2263 +0.1019 -0.1444 5.2566 +0.1251 -0.1717 5.2869 +0.1492 -0.1984 5.3172 +0.1742 -0.2245 5.3475 +0.2000 -0.2500 5.3778 +primID: 4 +startangle_c: 13 +endpose_c: 2 -7 12 +additionalactioncostmult: 2 +intermediateposes: 10 +0.0000 0.0000 5.1051 +0.0156 -0.0377 5.1051 +0.0312 -0.0754 5.1051 +0.0468 -0.1131 5.1051 +0.0623 -0.1508 5.0860 +0.0758 -0.1893 5.0113 +0.0863 -0.2287 4.9366 +0.0939 -0.2687 4.8618 +0.0985 -0.3092 4.7871 +0.1000 -0.3500 4.7124 +primID: 5 +startangle_c: 13 +endpose_c: 0 0 14 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 5.1051 +0.0000 0.0000 5.1487 +0.0000 0.0000 5.1924 +0.0000 0.0000 5.2360 +0.0000 0.0000 5.2796 +0.0000 0.0000 5.3233 +0.0000 0.0000 5.3669 +0.0000 0.0000 5.4105 +0.0000 0.0000 5.4542 +0.0000 0.0000 5.4978 +primID: 6 +startangle_c: 13 +endpose_c: 0 0 12 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 5.1051 +0.0000 0.0000 5.0615 +0.0000 0.0000 5.0178 +0.0000 0.0000 4.9742 +0.0000 0.0000 4.9306 +0.0000 0.0000 4.8869 +0.0000 0.0000 4.8433 +0.0000 0.0000 4.7997 +0.0000 0.0000 4.7560 +0.0000 0.0000 4.7124 +primID: 0 +startangle_c: 14 +endpose_c: 1 -1 14 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 5.4978 +0.0056 -0.0056 5.4978 +0.0111 -0.0111 5.4978 +0.0167 -0.0167 5.4978 +0.0222 -0.0222 5.4978 +0.0278 -0.0278 5.4978 +0.0333 -0.0333 5.4978 +0.0389 -0.0389 5.4978 +0.0444 -0.0444 5.4978 +0.0500 -0.0500 5.4978 +primID: 1 +startangle_c: 14 +endpose_c: 6 -6 14 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 5.4978 +0.0333 -0.0333 5.4978 +0.0667 -0.0667 5.4978 +0.1000 -0.1000 5.4978 +0.1333 -0.1333 5.4978 +0.1667 -0.1667 5.4978 +0.2000 -0.2000 5.4978 +0.2333 -0.2333 5.4978 +0.2667 -0.2667 5.4978 +0.3000 -0.3000 5.4978 +primID: 2 +startangle_c: 14 +endpose_c: -1 1 14 +additionalactioncostmult: 5 +intermediateposes: 10 +0.0000 0.0000 5.4978 +-0.0056 0.0056 5.4978 +-0.0111 0.0111 5.4978 +-0.0167 0.0167 5.4978 +-0.0222 0.0222 5.4978 +-0.0278 0.0278 5.4978 +-0.0333 0.0333 5.4978 +-0.0389 0.0389 5.4978 +-0.0444 0.0444 5.4978 +-0.0500 0.0500 5.4978 +primID: 3 +startangle_c: 14 +endpose_c: 7 -5 15 +additionalactioncostmult: 2 +intermediateposes: 10 +0.0000 0.0000 5.4978 +0.0341 -0.0341 5.4978 +0.0684 -0.0678 5.5275 +0.1043 -0.1000 5.5793 +0.1418 -0.1302 5.6312 +0.1809 -0.1584 5.6830 +0.2213 -0.1846 5.7349 +0.2631 -0.2086 5.7868 +0.3060 -0.2304 5.8386 +0.3500 -0.2500 5.8905 +primID: 4 +startangle_c: 14 +endpose_c: 5 -7 13 +additionalactioncostmult: 2 +intermediateposes: 10 +0.0000 0.0000 5.4978 +0.0341 -0.0341 5.4978 +0.0678 -0.0684 5.4681 +0.1000 -0.1043 5.4162 +0.1302 -0.1418 5.3644 +0.1584 -0.1809 5.3125 +0.1846 -0.2213 5.2607 +0.2086 -0.2631 5.2088 +0.2304 -0.3060 5.1569 +0.2500 -0.3500 5.1051 +primID: 5 +startangle_c: 14 +endpose_c: 0 0 15 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 5.4978 +0.0000 0.0000 5.5414 +0.0000 0.0000 5.5851 +0.0000 0.0000 5.6287 +0.0000 0.0000 5.6723 +0.0000 0.0000 5.7160 +0.0000 0.0000 5.7596 +0.0000 0.0000 5.8032 +0.0000 0.0000 5.8469 +0.0000 0.0000 5.8905 +primID: 6 +startangle_c: 14 +endpose_c: 0 0 13 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 5.4978 +0.0000 0.0000 5.4542 +0.0000 0.0000 5.4105 +0.0000 0.0000 5.3669 +0.0000 0.0000 5.3233 +0.0000 0.0000 5.2796 +0.0000 0.0000 5.2360 +0.0000 0.0000 5.1924 +0.0000 0.0000 5.1487 +0.0000 0.0000 5.1051 +primID: 0 +startangle_c: 15 +endpose_c: 2 -1 15 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 5.8905 +0.0111 -0.0056 5.8905 +0.0222 -0.0111 5.8905 +0.0333 -0.0167 5.8905 +0.0444 -0.0222 5.8905 +0.0556 -0.0278 5.8905 +0.0667 -0.0333 5.8905 +0.0778 -0.0389 5.8905 +0.0889 -0.0444 5.8905 +0.1000 -0.0500 5.8905 +primID: 1 +startangle_c: 15 +endpose_c: 6 -3 15 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 5.8905 +0.0333 -0.0167 5.8905 +0.0667 -0.0333 5.8905 +0.1000 -0.0500 5.8905 +0.1333 -0.0667 5.8905 +0.1667 -0.0833 5.8905 +0.2000 -0.1000 5.8905 +0.2333 -0.1167 5.8905 +0.2667 -0.1333 5.8905 +0.3000 -0.1500 5.8905 +primID: 2 +startangle_c: 15 +endpose_c: -2 1 15 +additionalactioncostmult: 5 +intermediateposes: 10 +0.0000 0.0000 5.8905 +-0.0111 0.0056 5.8905 +-0.0222 0.0111 5.8905 +-0.0333 0.0167 5.8905 +-0.0444 0.0222 5.8905 +-0.0556 0.0278 5.8905 +-0.0667 0.0333 5.8905 +-0.0778 0.0389 5.8905 +-0.0889 0.0444 5.8905 +-0.1000 0.0500 5.8905 +primID: 3 +startangle_c: 15 +endpose_c: 5 -4 14 +additionalactioncostmult: 2 +intermediateposes: 10 +0.0000 0.0000 5.8905 +0.0298 -0.0184 5.8602 +0.0592 -0.0379 5.8299 +0.0881 -0.0583 5.7996 +0.1165 -0.0796 5.7693 +0.1444 -0.1019 5.7390 +0.1717 -0.1251 5.7087 +0.1984 -0.1492 5.6784 +0.2245 -0.1742 5.6481 +0.2500 -0.2000 5.6178 +primID: 4 +startangle_c: 15 +endpose_c: 7 -2 0 +additionalactioncostmult: 2 +intermediateposes: 10 +0.0000 0.0000 5.8905 +0.0377 -0.0156 5.8905 +0.0754 -0.0312 5.8905 +0.1131 -0.0468 5.8905 +0.1508 -0.0623 5.9096 +0.1893 -0.0758 5.9843 +0.2287 -0.0863 6.0590 +0.2687 -0.0939 6.1337 +0.3092 -0.0985 6.2085 +0.3500 -0.1000 6.2832 +primID: 5 +startangle_c: 15 +endpose_c: 0 0 14 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 5.8905 +0.0000 0.0000 5.8469 +0.0000 0.0000 5.8032 +0.0000 0.0000 5.7596 +0.0000 0.0000 5.7160 +0.0000 0.0000 5.6723 +0.0000 0.0000 5.6287 +0.0000 0.0000 5.5851 +0.0000 0.0000 5.5414 +0.0000 0.0000 5.4978 +primID: 6 +startangle_c: 15 +endpose_c: 0 0 0 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 5.8905 +0.0000 0.0000 5.2360 +0.0000 0.0000 4.5815 +0.0000 0.0000 3.9270 +0.0000 0.0000 3.2725 +0.0000 0.0000 2.6180 +0.0000 0.0000 1.9635 +0.0000 0.0000 1.3090 +0.0000 0.0000 0.6545 +0.0000 0.0000 0.0000 diff --git a/Controllers/Packages/amr_startup/config/mprim/unicycle_5cm_expensive_turn_in_place_highcost.mprim b/Controllers/Packages/amr_startup/config/mprim/unicycle_5cm_expensive_turn_in_place_highcost.mprim new file mode 100755 index 0000000..0f11e5d --- /dev/null +++ b/Controllers/Packages/amr_startup/config/mprim/unicycle_5cm_expensive_turn_in_place_highcost.mprim @@ -0,0 +1,1683 @@ +resolution_m: 0.050000 +numberofangles: 16 +totalnumberofprimitives: 112 +primID: 0 +startangle_c: 0 +endpose_c: 1 0 0 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 0.0000 +0.0056 0.0000 0.0000 +0.0111 0.0000 0.0000 +0.0167 0.0000 0.0000 +0.0222 0.0000 0.0000 +0.0278 0.0000 0.0000 +0.0333 0.0000 0.0000 +0.0389 0.0000 0.0000 +0.0444 0.0000 0.0000 +0.0500 0.0000 0.0000 +primID: 1 +startangle_c: 0 +endpose_c: 8 0 0 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 0.0000 +0.0444 0.0000 0.0000 +0.0889 0.0000 0.0000 +0.1333 0.0000 0.0000 +0.1778 0.0000 0.0000 +0.2222 0.0000 0.0000 +0.2667 0.0000 0.0000 +0.3111 0.0000 0.0000 +0.3556 0.0000 0.0000 +0.4000 0.0000 0.0000 +primID: 2 +startangle_c: 0 +endpose_c: -1 0 0 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 0.0000 +-0.0056 0.0000 0.0000 +-0.0111 0.0000 0.0000 +-0.0167 0.0000 0.0000 +-0.0222 0.0000 0.0000 +-0.0278 0.0000 0.0000 +-0.0333 0.0000 0.0000 +-0.0389 0.0000 0.0000 +-0.0444 0.0000 0.0000 +-0.0500 0.0000 0.0000 +primID: 3 +startangle_c: 0 +endpose_c: 8 1 1 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 0.0000 +0.0452 -0.0000 0.0000 +0.0904 -0.0000 0.0000 +0.1355 -0.0000 0.0000 +0.1807 0.0008 0.0488 +0.2257 0.0045 0.1176 +0.2703 0.0114 0.1864 +0.3144 0.0213 0.2551 +0.3577 0.0342 0.3239 +0.4000 0.0500 0.3927 +primID: 4 +startangle_c: 0 +endpose_c: 8 -1 -1 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 0.0000 +0.0452 0.0000 0.0000 +0.0904 0.0000 0.0000 +0.1355 0.0000 0.0000 +0.1807 -0.0008 -0.0488 +0.2257 -0.0045 -0.1176 +0.2703 -0.0114 -0.1864 +0.3144 -0.0213 -0.2551 +0.3577 -0.0342 -0.3239 +0.4000 -0.0500 -0.3927 +primID: 5 +startangle_c: 0 +endpose_c: 0 0 1 +additionalactioncostmult: 200 +intermediateposes: 10 +0.0000 0.0000 0.0000 +0.0000 0.0000 0.0436 +0.0000 0.0000 0.0873 +0.0000 0.0000 0.1309 +0.0000 0.0000 0.1745 +0.0000 0.0000 0.2182 +0.0000 0.0000 0.2618 +0.0000 0.0000 0.3054 +0.0000 0.0000 0.3491 +0.0000 0.0000 0.3927 +primID: 6 +startangle_c: 0 +endpose_c: 0 0 -1 +additionalactioncostmult: 200 +intermediateposes: 10 +0.0000 0.0000 0.0000 +0.0000 0.0000 -0.0436 +0.0000 0.0000 -0.0873 +0.0000 0.0000 -0.1309 +0.0000 0.0000 -0.1745 +0.0000 0.0000 -0.2182 +0.0000 0.0000 -0.2618 +0.0000 0.0000 -0.3054 +0.0000 0.0000 -0.3491 +0.0000 0.0000 -0.3927 +primID: 0 +startangle_c: 1 +endpose_c: 2 1 1 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 0.3927 +0.0111 0.0056 0.3927 +0.0222 0.0111 0.3927 +0.0333 0.0167 0.3927 +0.0444 0.0222 0.3927 +0.0556 0.0278 0.3927 +0.0667 0.0333 0.3927 +0.0778 0.0389 0.3927 +0.0889 0.0444 0.3927 +0.1000 0.0500 0.3927 +primID: 1 +startangle_c: 1 +endpose_c: 6 3 1 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 0.3927 +0.0333 0.0167 0.3927 +0.0667 0.0333 0.3927 +0.1000 0.0500 0.3927 +0.1333 0.0667 0.3927 +0.1667 0.0833 0.3927 +0.2000 0.1000 0.3927 +0.2333 0.1167 0.3927 +0.2667 0.1333 0.3927 +0.3000 0.1500 0.3927 +primID: 2 +startangle_c: 1 +endpose_c: -2 -1 1 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 0.3927 +-0.0111 -0.0056 0.3927 +-0.0222 -0.0111 0.3927 +-0.0333 -0.0167 0.3927 +-0.0444 -0.0222 0.3927 +-0.0556 -0.0278 0.3927 +-0.0667 -0.0333 0.3927 +-0.0778 -0.0389 0.3927 +-0.0889 -0.0444 0.3927 +-0.1000 -0.0500 0.3927 +primID: 3 +startangle_c: 1 +endpose_c: 5 4 2 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 0.3927 +0.0298 0.0184 0.4230 +0.0592 0.0379 0.4533 +0.0881 0.0583 0.4836 +0.1165 0.0796 0.5139 +0.1444 0.1019 0.5442 +0.1717 0.1251 0.5745 +0.1984 0.1492 0.6048 +0.2245 0.1742 0.6351 +0.2500 0.2000 0.6654 +primID: 4 +startangle_c: 1 +endpose_c: 7 2 0 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 0.3927 +0.0377 0.0156 0.3927 +0.0754 0.0312 0.3927 +0.1131 0.0468 0.3927 +0.1508 0.0623 0.3736 +0.1893 0.0758 0.2989 +0.2287 0.0863 0.2242 +0.2687 0.0939 0.1494 +0.3092 0.0985 0.0747 +0.3500 0.1000 -0.0000 +primID: 5 +startangle_c: 1 +endpose_c: 0 0 2 +additionalactioncostmult: 200 +intermediateposes: 10 +0.0000 0.0000 0.3927 +0.0000 0.0000 0.4363 +0.0000 0.0000 0.4800 +0.0000 0.0000 0.5236 +0.0000 0.0000 0.5672 +0.0000 0.0000 0.6109 +0.0000 0.0000 0.6545 +0.0000 0.0000 0.6981 +0.0000 0.0000 0.7418 +0.0000 0.0000 0.7854 +primID: 6 +startangle_c: 1 +endpose_c: 0 0 0 +additionalactioncostmult: 200 +intermediateposes: 10 +0.0000 0.0000 0.3927 +0.0000 0.0000 0.3491 +0.0000 0.0000 0.3054 +0.0000 0.0000 0.2618 +0.0000 0.0000 0.2182 +0.0000 0.0000 0.1745 +0.0000 0.0000 0.1309 +0.0000 0.0000 0.0873 +0.0000 0.0000 0.0436 +0.0000 0.0000 0.0000 +primID: 0 +startangle_c: 2 +endpose_c: 1 1 2 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 0.7854 +0.0056 0.0056 0.7854 +0.0111 0.0111 0.7854 +0.0167 0.0167 0.7854 +0.0222 0.0222 0.7854 +0.0278 0.0278 0.7854 +0.0333 0.0333 0.7854 +0.0389 0.0389 0.7854 +0.0444 0.0444 0.7854 +0.0500 0.0500 0.7854 +primID: 1 +startangle_c: 2 +endpose_c: 6 6 2 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 0.7854 +0.0333 0.0333 0.7854 +0.0667 0.0667 0.7854 +0.1000 0.1000 0.7854 +0.1333 0.1333 0.7854 +0.1667 0.1667 0.7854 +0.2000 0.2000 0.7854 +0.2333 0.2333 0.7854 +0.2667 0.2667 0.7854 +0.3000 0.3000 0.7854 +primID: 2 +startangle_c: 2 +endpose_c: -1 -1 2 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 0.7854 +-0.0056 -0.0056 0.7854 +-0.0111 -0.0111 0.7854 +-0.0167 -0.0167 0.7854 +-0.0222 -0.0222 0.7854 +-0.0278 -0.0278 0.7854 +-0.0333 -0.0333 0.7854 +-0.0389 -0.0389 0.7854 +-0.0444 -0.0444 0.7854 +-0.0500 -0.0500 0.7854 +primID: 3 +startangle_c: 2 +endpose_c: 5 7 3 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 0.7854 +0.0341 0.0341 0.7854 +0.0678 0.0684 0.8151 +0.1000 0.1043 0.8669 +0.1302 0.1418 0.9188 +0.1584 0.1809 0.9707 +0.1846 0.2213 1.0225 +0.2086 0.2631 1.0744 +0.2304 0.3060 1.1262 +0.2500 0.3500 1.1781 +primID: 4 +startangle_c: 2 +endpose_c: 7 5 1 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 0.7854 +0.0341 0.0341 0.7854 +0.0684 0.0678 0.7557 +0.1043 0.1000 0.7039 +0.1418 0.1302 0.6520 +0.1809 0.1584 0.6001 +0.2213 0.1846 0.5483 +0.2631 0.2086 0.4964 +0.3060 0.2304 0.4446 +0.3500 0.2500 0.3927 +primID: 5 +startangle_c: 2 +endpose_c: 0 0 3 +additionalactioncostmult: 200 +intermediateposes: 10 +0.0000 0.0000 0.7854 +0.0000 0.0000 0.8290 +0.0000 0.0000 0.8727 +0.0000 0.0000 0.9163 +0.0000 0.0000 0.9599 +0.0000 0.0000 1.0036 +0.0000 0.0000 1.0472 +0.0000 0.0000 1.0908 +0.0000 0.0000 1.1345 +0.0000 0.0000 1.1781 +primID: 6 +startangle_c: 2 +endpose_c: 0 0 1 +additionalactioncostmult: 200 +intermediateposes: 10 +0.0000 0.0000 0.7854 +0.0000 0.0000 0.7418 +0.0000 0.0000 0.6981 +0.0000 0.0000 0.6545 +0.0000 0.0000 0.6109 +0.0000 0.0000 0.5672 +0.0000 0.0000 0.5236 +0.0000 0.0000 0.4800 +0.0000 0.0000 0.4363 +0.0000 0.0000 0.3927 +primID: 0 +startangle_c: 3 +endpose_c: 1 2 3 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 1.1781 +0.0056 0.0111 1.1781 +0.0111 0.0222 1.1781 +0.0167 0.0333 1.1781 +0.0222 0.0444 1.1781 +0.0278 0.0556 1.1781 +0.0333 0.0667 1.1781 +0.0389 0.0778 1.1781 +0.0444 0.0889 1.1781 +0.0500 0.1000 1.1781 +primID: 1 +startangle_c: 3 +endpose_c: 3 6 3 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 1.1781 +0.0167 0.0333 1.1781 +0.0333 0.0667 1.1781 +0.0500 0.1000 1.1781 +0.0667 0.1333 1.1781 +0.0833 0.1667 1.1781 +0.1000 0.2000 1.1781 +0.1167 0.2333 1.1781 +0.1333 0.2667 1.1781 +0.1500 0.3000 1.1781 +primID: 2 +startangle_c: 3 +endpose_c: -1 -2 3 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 1.1781 +-0.0056 -0.0111 1.1781 +-0.0111 -0.0222 1.1781 +-0.0167 -0.0333 1.1781 +-0.0222 -0.0444 1.1781 +-0.0278 -0.0556 1.1781 +-0.0333 -0.0667 1.1781 +-0.0389 -0.0778 1.1781 +-0.0444 -0.0889 1.1781 +-0.0500 -0.1000 1.1781 +primID: 3 +startangle_c: 3 +endpose_c: 4 5 2 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 1.1781 +0.0184 0.0298 1.1478 +0.0379 0.0592 1.1175 +0.0583 0.0881 1.0872 +0.0796 0.1165 1.0569 +0.1019 0.1444 1.0266 +0.1251 0.1717 0.9963 +0.1492 0.1984 0.9660 +0.1742 0.2245 0.9357 +0.2000 0.2500 0.9054 +primID: 4 +startangle_c: 3 +endpose_c: 2 7 4 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 1.1781 +0.0156 0.0377 1.1781 +0.0312 0.0754 1.1781 +0.0468 0.1131 1.1781 +0.0623 0.1508 1.1972 +0.0758 0.1893 1.2719 +0.0863 0.2287 1.3466 +0.0939 0.2687 1.4214 +0.0985 0.3092 1.4961 +0.1000 0.3500 1.5708 +primID: 5 +startangle_c: 3 +endpose_c: 0 0 2 +additionalactioncostmult: 200 +intermediateposes: 10 +0.0000 0.0000 1.1781 +0.0000 0.0000 1.1345 +0.0000 0.0000 1.0908 +0.0000 0.0000 1.0472 +0.0000 0.0000 1.0036 +0.0000 0.0000 0.9599 +0.0000 0.0000 0.9163 +0.0000 0.0000 0.8727 +0.0000 0.0000 0.8290 +0.0000 0.0000 0.7854 +primID: 6 +startangle_c: 3 +endpose_c: 0 0 4 +additionalactioncostmult: 200 +intermediateposes: 10 +0.0000 0.0000 1.1781 +0.0000 0.0000 1.2217 +0.0000 0.0000 1.2654 +0.0000 0.0000 1.3090 +0.0000 0.0000 1.3526 +0.0000 0.0000 1.3963 +0.0000 0.0000 1.4399 +0.0000 0.0000 1.4835 +0.0000 0.0000 1.5272 +0.0000 0.0000 1.5708 +primID: 0 +startangle_c: 4 +endpose_c: 0 1 4 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 1.5708 +0.0000 0.0056 1.5708 +0.0000 0.0111 1.5708 +0.0000 0.0167 1.5708 +0.0000 0.0222 1.5708 +0.0000 0.0278 1.5708 +0.0000 0.0333 1.5708 +0.0000 0.0389 1.5708 +0.0000 0.0444 1.5708 +0.0000 0.0500 1.5708 +primID: 1 +startangle_c: 4 +endpose_c: 0 8 4 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 1.5708 +0.0000 0.0444 1.5708 +0.0000 0.0889 1.5708 +0.0000 0.1333 1.5708 +0.0000 0.1778 1.5708 +0.0000 0.2222 1.5708 +0.0000 0.2667 1.5708 +0.0000 0.3111 1.5708 +0.0000 0.3556 1.5708 +0.0000 0.4000 1.5708 +primID: 2 +startangle_c: 4 +endpose_c: 0 -1 4 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 1.5708 +0.0000 -0.0056 1.5708 +0.0000 -0.0111 1.5708 +0.0000 -0.0167 1.5708 +0.0000 -0.0222 1.5708 +0.0000 -0.0278 1.5708 +0.0000 -0.0333 1.5708 +0.0000 -0.0389 1.5708 +0.0000 -0.0444 1.5708 +0.0000 -0.0500 1.5708 +primID: 3 +startangle_c: 4 +endpose_c: -1 8 5 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 1.5708 +0.0000 0.0452 1.5708 +0.0000 0.0904 1.5708 +0.0000 0.1355 1.5708 +-0.0008 0.1807 1.6196 +-0.0045 0.2257 1.6884 +-0.0114 0.2703 1.7572 +-0.0213 0.3144 1.8259 +-0.0342 0.3577 1.8947 +-0.0500 0.4000 1.9635 +primID: 4 +startangle_c: 4 +endpose_c: 1 8 3 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 1.5708 +0.0000 0.0452 1.5708 +0.0000 0.0904 1.5708 +0.0000 0.1355 1.5708 +0.0008 0.1807 1.5220 +0.0045 0.2257 1.4532 +0.0114 0.2703 1.3844 +0.0213 0.3144 1.3156 +0.0342 0.3577 1.2469 +0.0500 0.4000 1.1781 +primID: 5 +startangle_c: 4 +endpose_c: 0 0 5 +additionalactioncostmult: 200 +intermediateposes: 10 +0.0000 0.0000 1.5708 +0.0000 0.0000 1.6144 +0.0000 0.0000 1.6581 +0.0000 0.0000 1.7017 +0.0000 0.0000 1.7453 +0.0000 0.0000 1.7890 +0.0000 0.0000 1.8326 +0.0000 0.0000 1.8762 +0.0000 0.0000 1.9199 +0.0000 0.0000 1.9635 +primID: 6 +startangle_c: 4 +endpose_c: 0 0 3 +additionalactioncostmult: 200 +intermediateposes: 10 +0.0000 0.0000 1.5708 +0.0000 0.0000 1.5272 +0.0000 0.0000 1.4835 +0.0000 0.0000 1.4399 +0.0000 0.0000 1.3963 +0.0000 0.0000 1.3526 +0.0000 0.0000 1.3090 +0.0000 0.0000 1.2654 +0.0000 0.0000 1.2217 +0.0000 0.0000 1.1781 +primID: 0 +startangle_c: 5 +endpose_c: -1 2 5 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 1.9635 +-0.0056 0.0111 1.9635 +-0.0111 0.0222 1.9635 +-0.0167 0.0333 1.9635 +-0.0222 0.0444 1.9635 +-0.0278 0.0556 1.9635 +-0.0333 0.0667 1.9635 +-0.0389 0.0778 1.9635 +-0.0444 0.0889 1.9635 +-0.0500 0.1000 1.9635 +primID: 1 +startangle_c: 5 +endpose_c: -3 6 5 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 1.9635 +-0.0167 0.0333 1.9635 +-0.0333 0.0667 1.9635 +-0.0500 0.1000 1.9635 +-0.0667 0.1333 1.9635 +-0.0833 0.1667 1.9635 +-0.1000 0.2000 1.9635 +-0.1167 0.2333 1.9635 +-0.1333 0.2667 1.9635 +-0.1500 0.3000 1.9635 +primID: 2 +startangle_c: 5 +endpose_c: 1 -2 5 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 1.9635 +0.0056 -0.0111 1.9635 +0.0111 -0.0222 1.9635 +0.0167 -0.0333 1.9635 +0.0222 -0.0444 1.9635 +0.0278 -0.0556 1.9635 +0.0333 -0.0667 1.9635 +0.0389 -0.0778 1.9635 +0.0444 -0.0889 1.9635 +0.0500 -0.1000 1.9635 +primID: 3 +startangle_c: 5 +endpose_c: -4 5 6 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 1.9635 +-0.0184 0.0298 1.9938 +-0.0379 0.0592 2.0241 +-0.0583 0.0881 2.0544 +-0.0796 0.1165 2.0847 +-0.1019 0.1444 2.1150 +-0.1251 0.1717 2.1453 +-0.1492 0.1984 2.1756 +-0.1742 0.2245 2.2059 +-0.2000 0.2500 2.2362 +primID: 4 +startangle_c: 5 +endpose_c: -2 7 4 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 1.9635 +-0.0156 0.0377 1.9635 +-0.0312 0.0754 1.9635 +-0.0468 0.1131 1.9635 +-0.0623 0.1508 1.9444 +-0.0758 0.1893 1.8697 +-0.0863 0.2287 1.7950 +-0.0939 0.2687 1.7202 +-0.0985 0.3092 1.6455 +-0.1000 0.3500 1.5708 +primID: 5 +startangle_c: 5 +endpose_c: 0 0 6 +additionalactioncostmult: 200 +intermediateposes: 10 +0.0000 0.0000 1.9635 +0.0000 0.0000 2.0071 +0.0000 0.0000 2.0508 +0.0000 0.0000 2.0944 +0.0000 0.0000 2.1380 +0.0000 0.0000 2.1817 +0.0000 0.0000 2.2253 +0.0000 0.0000 2.2689 +0.0000 0.0000 2.3126 +0.0000 0.0000 2.3562 +primID: 6 +startangle_c: 5 +endpose_c: 0 0 4 +additionalactioncostmult: 200 +intermediateposes: 10 +0.0000 0.0000 1.9635 +0.0000 0.0000 1.9199 +0.0000 0.0000 1.8762 +0.0000 0.0000 1.8326 +0.0000 0.0000 1.7890 +0.0000 0.0000 1.7453 +0.0000 0.0000 1.7017 +0.0000 0.0000 1.6581 +0.0000 0.0000 1.6144 +0.0000 0.0000 1.5708 +primID: 0 +startangle_c: 6 +endpose_c: -1 1 6 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 2.3562 +-0.0056 0.0056 2.3562 +-0.0111 0.0111 2.3562 +-0.0167 0.0167 2.3562 +-0.0222 0.0222 2.3562 +-0.0278 0.0278 2.3562 +-0.0333 0.0333 2.3562 +-0.0389 0.0389 2.3562 +-0.0444 0.0444 2.3562 +-0.0500 0.0500 2.3562 +primID: 1 +startangle_c: 6 +endpose_c: -6 6 6 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 2.3562 +-0.0333 0.0333 2.3562 +-0.0667 0.0667 2.3562 +-0.1000 0.1000 2.3562 +-0.1333 0.1333 2.3562 +-0.1667 0.1667 2.3562 +-0.2000 0.2000 2.3562 +-0.2333 0.2333 2.3562 +-0.2667 0.2667 2.3562 +-0.3000 0.3000 2.3562 +primID: 2 +startangle_c: 6 +endpose_c: 1 -1 6 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 2.3562 +0.0056 -0.0056 2.3562 +0.0111 -0.0111 2.3562 +0.0167 -0.0167 2.3562 +0.0222 -0.0222 2.3562 +0.0278 -0.0278 2.3562 +0.0333 -0.0333 2.3562 +0.0389 -0.0389 2.3562 +0.0444 -0.0444 2.3562 +0.0500 -0.0500 2.3562 +primID: 3 +startangle_c: 6 +endpose_c: -7 5 7 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 2.3562 +-0.0341 0.0341 2.3562 +-0.0684 0.0678 2.3859 +-0.1043 0.1000 2.4377 +-0.1418 0.1302 2.4896 +-0.1809 0.1584 2.5415 +-0.2213 0.1846 2.5933 +-0.2631 0.2086 2.6452 +-0.3060 0.2304 2.6970 +-0.3500 0.2500 2.7489 +primID: 4 +startangle_c: 6 +endpose_c: -5 7 5 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 2.3562 +-0.0341 0.0341 2.3562 +-0.0678 0.0684 2.3265 +-0.1000 0.1043 2.2747 +-0.1302 0.1418 2.2228 +-0.1584 0.1809 2.1709 +-0.1846 0.2213 2.1191 +-0.2086 0.2631 2.0672 +-0.2304 0.3060 2.0154 +-0.2500 0.3500 1.9635 +primID: 5 +startangle_c: 6 +endpose_c: 0 0 7 +additionalactioncostmult: 200 +intermediateposes: 10 +0.0000 0.0000 2.3562 +0.0000 0.0000 2.3998 +0.0000 0.0000 2.4435 +0.0000 0.0000 2.4871 +0.0000 0.0000 2.5307 +0.0000 0.0000 2.5744 +0.0000 0.0000 2.6180 +0.0000 0.0000 2.6616 +0.0000 0.0000 2.7053 +0.0000 0.0000 2.7489 +primID: 6 +startangle_c: 6 +endpose_c: 0 0 5 +additionalactioncostmult: 200 +intermediateposes: 10 +0.0000 0.0000 2.3562 +0.0000 0.0000 2.3126 +0.0000 0.0000 2.2689 +0.0000 0.0000 2.2253 +0.0000 0.0000 2.1817 +0.0000 0.0000 2.1380 +0.0000 0.0000 2.0944 +0.0000 0.0000 2.0508 +0.0000 0.0000 2.0071 +0.0000 0.0000 1.9635 +primID: 0 +startangle_c: 7 +endpose_c: -2 1 7 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 2.7489 +-0.0111 0.0056 2.7489 +-0.0222 0.0111 2.7489 +-0.0333 0.0167 2.7489 +-0.0444 0.0222 2.7489 +-0.0556 0.0278 2.7489 +-0.0667 0.0333 2.7489 +-0.0778 0.0389 2.7489 +-0.0889 0.0444 2.7489 +-0.1000 0.0500 2.7489 +primID: 1 +startangle_c: 7 +endpose_c: -6 3 7 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 2.7489 +-0.0333 0.0167 2.7489 +-0.0667 0.0333 2.7489 +-0.1000 0.0500 2.7489 +-0.1333 0.0667 2.7489 +-0.1667 0.0833 2.7489 +-0.2000 0.1000 2.7489 +-0.2333 0.1167 2.7489 +-0.2667 0.1333 2.7489 +-0.3000 0.1500 2.7489 +primID: 2 +startangle_c: 7 +endpose_c: 2 -1 7 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 2.7489 +0.0111 -0.0056 2.7489 +0.0222 -0.0111 2.7489 +0.0333 -0.0167 2.7489 +0.0444 -0.0222 2.7489 +0.0556 -0.0278 2.7489 +0.0667 -0.0333 2.7489 +0.0778 -0.0389 2.7489 +0.0889 -0.0444 2.7489 +0.1000 -0.0500 2.7489 +primID: 3 +startangle_c: 7 +endpose_c: -5 4 6 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 2.7489 +-0.0298 0.0184 2.7186 +-0.0592 0.0379 2.6883 +-0.0881 0.0583 2.6580 +-0.1165 0.0796 2.6277 +-0.1444 0.1019 2.5974 +-0.1717 0.1251 2.5671 +-0.1984 0.1492 2.5368 +-0.2245 0.1742 2.5065 +-0.2500 0.2000 2.4762 +primID: 4 +startangle_c: 7 +endpose_c: -7 2 8 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 2.7489 +-0.0377 0.0156 2.7489 +-0.0754 0.0312 2.7489 +-0.1131 0.0468 2.7489 +-0.1508 0.0623 2.7680 +-0.1893 0.0758 2.8427 +-0.2287 0.0863 2.9174 +-0.2687 0.0939 2.9921 +-0.3092 0.0985 3.0669 +-0.3500 0.1000 3.1416 +primID: 5 +startangle_c: 7 +endpose_c: 0 0 6 +additionalactioncostmult: 200 +intermediateposes: 10 +0.0000 0.0000 2.7489 +0.0000 0.0000 2.7053 +0.0000 0.0000 2.6616 +0.0000 0.0000 2.6180 +0.0000 0.0000 2.5744 +0.0000 0.0000 2.5307 +0.0000 0.0000 2.4871 +0.0000 0.0000 2.4435 +0.0000 0.0000 2.3998 +0.0000 0.0000 2.3562 +primID: 6 +startangle_c: 7 +endpose_c: 0 0 8 +additionalactioncostmult: 200 +intermediateposes: 10 +0.0000 0.0000 2.7489 +0.0000 0.0000 2.7925 +0.0000 0.0000 2.8362 +0.0000 0.0000 2.8798 +0.0000 0.0000 2.9234 +0.0000 0.0000 2.9671 +0.0000 0.0000 3.0107 +0.0000 0.0000 3.0543 +0.0000 0.0000 3.0980 +0.0000 0.0000 3.1416 +primID: 0 +startangle_c: 8 +endpose_c: -1 0 8 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 3.1416 +-0.0056 0.0000 3.1416 +-0.0111 0.0000 3.1416 +-0.0167 0.0000 3.1416 +-0.0222 0.0000 3.1416 +-0.0278 0.0000 3.1416 +-0.0333 0.0000 3.1416 +-0.0389 0.0000 3.1416 +-0.0444 0.0000 3.1416 +-0.0500 0.0000 3.1416 +primID: 1 +startangle_c: 8 +endpose_c: -8 0 8 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 3.1416 +-0.0444 0.0000 3.1416 +-0.0889 0.0000 3.1416 +-0.1333 0.0000 3.1416 +-0.1778 0.0000 3.1416 +-0.2222 0.0000 3.1416 +-0.2667 0.0000 3.1416 +-0.3111 0.0000 3.1416 +-0.3556 0.0000 3.1416 +-0.4000 0.0000 3.1416 +primID: 2 +startangle_c: 8 +endpose_c: 1 0 8 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 3.1416 +0.0056 0.0000 3.1416 +0.0111 0.0000 3.1416 +0.0167 0.0000 3.1416 +0.0222 0.0000 3.1416 +0.0278 0.0000 3.1416 +0.0333 0.0000 3.1416 +0.0389 0.0000 3.1416 +0.0444 0.0000 3.1416 +0.0500 0.0000 3.1416 +primID: 3 +startangle_c: 8 +endpose_c: -8 -1 9 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 3.1416 +-0.0452 0.0000 3.1416 +-0.0904 0.0000 3.1416 +-0.1355 0.0000 3.1416 +-0.1807 -0.0008 3.1904 +-0.2257 -0.0045 3.2592 +-0.2703 -0.0114 3.3280 +-0.3144 -0.0213 3.3967 +-0.3577 -0.0342 3.4655 +-0.4000 -0.0500 3.5343 +primID: 4 +startangle_c: 8 +endpose_c: -8 1 7 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 3.1416 +-0.0452 0.0000 3.1416 +-0.0904 0.0000 3.1416 +-0.1355 0.0000 3.1416 +-0.1807 0.0008 3.0928 +-0.2257 0.0045 3.0240 +-0.2703 0.0114 2.9552 +-0.3144 0.0213 2.8864 +-0.3577 0.0342 2.8177 +-0.4000 0.0500 2.7489 +primID: 5 +startangle_c: 8 +endpose_c: 0 0 9 +additionalactioncostmult: 200 +intermediateposes: 10 +0.0000 0.0000 3.1416 +0.0000 0.0000 3.1852 +0.0000 0.0000 3.2289 +0.0000 0.0000 3.2725 +0.0000 0.0000 3.3161 +0.0000 0.0000 3.3598 +0.0000 0.0000 3.4034 +0.0000 0.0000 3.4470 +0.0000 0.0000 3.4907 +0.0000 0.0000 3.5343 +primID: 6 +startangle_c: 8 +endpose_c: 0 0 7 +additionalactioncostmult: 200 +intermediateposes: 10 +0.0000 0.0000 3.1416 +0.0000 0.0000 3.0980 +0.0000 0.0000 3.0543 +0.0000 0.0000 3.0107 +0.0000 0.0000 2.9671 +0.0000 0.0000 2.9234 +0.0000 0.0000 2.8798 +0.0000 0.0000 2.8362 +0.0000 0.0000 2.7925 +0.0000 0.0000 2.7489 +primID: 0 +startangle_c: 9 +endpose_c: -2 -1 9 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 3.5343 +-0.0111 -0.0056 3.5343 +-0.0222 -0.0111 3.5343 +-0.0333 -0.0167 3.5343 +-0.0444 -0.0222 3.5343 +-0.0556 -0.0278 3.5343 +-0.0667 -0.0333 3.5343 +-0.0778 -0.0389 3.5343 +-0.0889 -0.0444 3.5343 +-0.1000 -0.0500 3.5343 +primID: 1 +startangle_c: 9 +endpose_c: -6 -3 9 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 3.5343 +-0.0333 -0.0167 3.5343 +-0.0667 -0.0333 3.5343 +-0.1000 -0.0500 3.5343 +-0.1333 -0.0667 3.5343 +-0.1667 -0.0833 3.5343 +-0.2000 -0.1000 3.5343 +-0.2333 -0.1167 3.5343 +-0.2667 -0.1333 3.5343 +-0.3000 -0.1500 3.5343 +primID: 2 +startangle_c: 9 +endpose_c: 2 1 9 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 3.5343 +0.0111 0.0056 3.5343 +0.0222 0.0111 3.5343 +0.0333 0.0167 3.5343 +0.0444 0.0222 3.5343 +0.0556 0.0278 3.5343 +0.0667 0.0333 3.5343 +0.0778 0.0389 3.5343 +0.0889 0.0444 3.5343 +0.1000 0.0500 3.5343 +primID: 3 +startangle_c: 9 +endpose_c: -5 -4 10 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 3.5343 +-0.0298 -0.0184 3.5646 +-0.0592 -0.0379 3.5949 +-0.0881 -0.0583 3.6252 +-0.1165 -0.0796 3.6555 +-0.1444 -0.1019 3.6858 +-0.1717 -0.1251 3.7161 +-0.1984 -0.1492 3.7464 +-0.2245 -0.1742 3.7767 +-0.2500 -0.2000 3.8070 +primID: 4 +startangle_c: 9 +endpose_c: -7 -2 8 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 3.5343 +-0.0377 -0.0156 3.5343 +-0.0754 -0.0312 3.5343 +-0.1131 -0.0468 3.5343 +-0.1508 -0.0623 3.5152 +-0.1893 -0.0758 3.4405 +-0.2287 -0.0863 3.3658 +-0.2687 -0.0939 3.2910 +-0.3092 -0.0985 3.2163 +-0.3500 -0.1000 3.1416 +primID: 5 +startangle_c: 9 +endpose_c: 0 0 10 +additionalactioncostmult: 200 +intermediateposes: 10 +0.0000 0.0000 3.5343 +0.0000 0.0000 3.5779 +0.0000 0.0000 3.6216 +0.0000 0.0000 3.6652 +0.0000 0.0000 3.7088 +0.0000 0.0000 3.7525 +0.0000 0.0000 3.7961 +0.0000 0.0000 3.8397 +0.0000 0.0000 3.8834 +0.0000 0.0000 3.9270 +primID: 6 +startangle_c: 9 +endpose_c: 0 0 8 +additionalactioncostmult: 200 +intermediateposes: 10 +0.0000 0.0000 3.5343 +0.0000 0.0000 3.4907 +0.0000 0.0000 3.4470 +0.0000 0.0000 3.4034 +0.0000 0.0000 3.3598 +0.0000 0.0000 3.3161 +0.0000 0.0000 3.2725 +0.0000 0.0000 3.2289 +0.0000 0.0000 3.1852 +0.0000 0.0000 3.1416 +primID: 0 +startangle_c: 10 +endpose_c: -1 -1 10 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 3.9270 +-0.0056 -0.0056 3.9270 +-0.0111 -0.0111 3.9270 +-0.0167 -0.0167 3.9270 +-0.0222 -0.0222 3.9270 +-0.0278 -0.0278 3.9270 +-0.0333 -0.0333 3.9270 +-0.0389 -0.0389 3.9270 +-0.0444 -0.0444 3.9270 +-0.0500 -0.0500 3.9270 +primID: 1 +startangle_c: 10 +endpose_c: -6 -6 10 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 3.9270 +-0.0333 -0.0333 3.9270 +-0.0667 -0.0667 3.9270 +-0.1000 -0.1000 3.9270 +-0.1333 -0.1333 3.9270 +-0.1667 -0.1667 3.9270 +-0.2000 -0.2000 3.9270 +-0.2333 -0.2333 3.9270 +-0.2667 -0.2667 3.9270 +-0.3000 -0.3000 3.9270 +primID: 2 +startangle_c: 10 +endpose_c: 1 1 10 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 3.9270 +0.0056 0.0056 3.9270 +0.0111 0.0111 3.9270 +0.0167 0.0167 3.9270 +0.0222 0.0222 3.9270 +0.0278 0.0278 3.9270 +0.0333 0.0333 3.9270 +0.0389 0.0389 3.9270 +0.0444 0.0444 3.9270 +0.0500 0.0500 3.9270 +primID: 3 +startangle_c: 10 +endpose_c: -5 -7 11 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 3.9270 +-0.0341 -0.0341 3.9270 +-0.0678 -0.0684 3.9567 +-0.1000 -0.1043 4.0085 +-0.1302 -0.1418 4.0604 +-0.1584 -0.1809 4.1123 +-0.1846 -0.2213 4.1641 +-0.2086 -0.2631 4.2160 +-0.2304 -0.3060 4.2678 +-0.2500 -0.3500 4.3197 +primID: 4 +startangle_c: 10 +endpose_c: -7 -5 9 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 3.9270 +-0.0341 -0.0341 3.9270 +-0.0684 -0.0678 3.8973 +-0.1043 -0.1000 3.8455 +-0.1418 -0.1302 3.7936 +-0.1809 -0.1584 3.7417 +-0.2213 -0.1846 3.6899 +-0.2631 -0.2086 3.6380 +-0.3060 -0.2304 3.5862 +-0.3500 -0.2500 3.5343 +primID: 5 +startangle_c: 10 +endpose_c: 0 0 11 +additionalactioncostmult: 200 +intermediateposes: 10 +0.0000 0.0000 3.9270 +0.0000 0.0000 3.9706 +0.0000 0.0000 4.0143 +0.0000 0.0000 4.0579 +0.0000 0.0000 4.1015 +0.0000 0.0000 4.1452 +0.0000 0.0000 4.1888 +0.0000 0.0000 4.2324 +0.0000 0.0000 4.2761 +0.0000 0.0000 4.3197 +primID: 6 +startangle_c: 10 +endpose_c: 0 0 9 +additionalactioncostmult: 200 +intermediateposes: 10 +0.0000 0.0000 3.9270 +0.0000 0.0000 3.8834 +0.0000 0.0000 3.8397 +0.0000 0.0000 3.7961 +0.0000 0.0000 3.7525 +0.0000 0.0000 3.7088 +0.0000 0.0000 3.6652 +0.0000 0.0000 3.6216 +0.0000 0.0000 3.5779 +0.0000 0.0000 3.5343 +primID: 0 +startangle_c: 11 +endpose_c: -1 -2 11 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 4.3197 +-0.0056 -0.0111 4.3197 +-0.0111 -0.0222 4.3197 +-0.0167 -0.0333 4.3197 +-0.0222 -0.0444 4.3197 +-0.0278 -0.0556 4.3197 +-0.0333 -0.0667 4.3197 +-0.0389 -0.0778 4.3197 +-0.0444 -0.0889 4.3197 +-0.0500 -0.1000 4.3197 +primID: 1 +startangle_c: 11 +endpose_c: -3 -6 11 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 4.3197 +-0.0167 -0.0333 4.3197 +-0.0333 -0.0667 4.3197 +-0.0500 -0.1000 4.3197 +-0.0667 -0.1333 4.3197 +-0.0833 -0.1667 4.3197 +-0.1000 -0.2000 4.3197 +-0.1167 -0.2333 4.3197 +-0.1333 -0.2667 4.3197 +-0.1500 -0.3000 4.3197 +primID: 2 +startangle_c: 11 +endpose_c: 1 2 11 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 4.3197 +0.0056 0.0111 4.3197 +0.0111 0.0222 4.3197 +0.0167 0.0333 4.3197 +0.0222 0.0444 4.3197 +0.0278 0.0556 4.3197 +0.0333 0.0667 4.3197 +0.0389 0.0778 4.3197 +0.0444 0.0889 4.3197 +0.0500 0.1000 4.3197 +primID: 3 +startangle_c: 11 +endpose_c: -4 -5 10 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 4.3197 +-0.0184 -0.0298 4.2894 +-0.0379 -0.0592 4.2591 +-0.0583 -0.0881 4.2288 +-0.0796 -0.1165 4.1985 +-0.1019 -0.1444 4.1682 +-0.1251 -0.1717 4.1379 +-0.1492 -0.1984 4.1076 +-0.1742 -0.2245 4.0773 +-0.2000 -0.2500 4.0470 +primID: 4 +startangle_c: 11 +endpose_c: -2 -7 12 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 4.3197 +-0.0156 -0.0377 4.3197 +-0.0312 -0.0754 4.3197 +-0.0468 -0.1131 4.3197 +-0.0623 -0.1508 4.3388 +-0.0758 -0.1893 4.4135 +-0.0863 -0.2287 4.4882 +-0.0939 -0.2687 4.5629 +-0.0985 -0.3092 4.6377 +-0.1000 -0.3500 4.7124 +primID: 5 +startangle_c: 11 +endpose_c: 0 0 10 +additionalactioncostmult: 200 +intermediateposes: 10 +0.0000 0.0000 4.3197 +0.0000 0.0000 4.2761 +0.0000 0.0000 4.2324 +0.0000 0.0000 4.1888 +0.0000 0.0000 4.1452 +0.0000 0.0000 4.1015 +0.0000 0.0000 4.0579 +0.0000 0.0000 4.0143 +0.0000 0.0000 3.9706 +0.0000 0.0000 3.9270 +primID: 6 +startangle_c: 11 +endpose_c: 0 0 12 +additionalactioncostmult: 200 +intermediateposes: 10 +0.0000 0.0000 4.3197 +0.0000 0.0000 4.3633 +0.0000 0.0000 4.4070 +0.0000 0.0000 4.4506 +0.0000 0.0000 4.4942 +0.0000 0.0000 4.5379 +0.0000 0.0000 4.5815 +0.0000 0.0000 4.6251 +0.0000 0.0000 4.6688 +0.0000 0.0000 4.7124 +primID: 0 +startangle_c: 12 +endpose_c: 0 -1 12 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 4.7124 +0.0000 -0.0056 4.7124 +0.0000 -0.0111 4.7124 +0.0000 -0.0167 4.7124 +0.0000 -0.0222 4.7124 +0.0000 -0.0278 4.7124 +0.0000 -0.0333 4.7124 +0.0000 -0.0389 4.7124 +0.0000 -0.0444 4.7124 +0.0000 -0.0500 4.7124 +primID: 1 +startangle_c: 12 +endpose_c: 0 -8 12 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 4.7124 +0.0000 -0.0444 4.7124 +0.0000 -0.0889 4.7124 +0.0000 -0.1333 4.7124 +0.0000 -0.1778 4.7124 +0.0000 -0.2222 4.7124 +0.0000 -0.2667 4.7124 +0.0000 -0.3111 4.7124 +0.0000 -0.3556 4.7124 +0.0000 -0.4000 4.7124 +primID: 2 +startangle_c: 12 +endpose_c: 0 1 12 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 4.7124 +0.0000 0.0056 4.7124 +0.0000 0.0111 4.7124 +0.0000 0.0167 4.7124 +0.0000 0.0222 4.7124 +0.0000 0.0278 4.7124 +0.0000 0.0333 4.7124 +0.0000 0.0389 4.7124 +0.0000 0.0444 4.7124 +0.0000 0.0500 4.7124 +primID: 3 +startangle_c: 12 +endpose_c: 1 -8 13 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 4.7124 +0.0000 -0.0452 4.7124 +0.0000 -0.0904 4.7124 +0.0000 -0.1355 4.7124 +0.0008 -0.1807 4.7612 +0.0045 -0.2257 4.8300 +0.0114 -0.2703 4.8988 +0.0213 -0.3144 4.9675 +0.0342 -0.3577 5.0363 +0.0500 -0.4000 5.1051 +primID: 4 +startangle_c: 12 +endpose_c: -1 -8 11 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 4.7124 +-0.0000 -0.0452 4.7124 +-0.0000 -0.0904 4.7124 +-0.0000 -0.1355 4.7124 +-0.0008 -0.1807 4.6636 +-0.0045 -0.2257 4.5948 +-0.0114 -0.2703 4.5260 +-0.0213 -0.3144 4.4572 +-0.0342 -0.3577 4.3885 +-0.0500 -0.4000 4.3197 +primID: 5 +startangle_c: 12 +endpose_c: 0 0 13 +additionalactioncostmult: 200 +intermediateposes: 10 +0.0000 0.0000 4.7124 +0.0000 0.0000 4.7560 +0.0000 0.0000 4.7997 +0.0000 0.0000 4.8433 +0.0000 0.0000 4.8869 +0.0000 0.0000 4.9306 +0.0000 0.0000 4.9742 +0.0000 0.0000 5.0178 +0.0000 0.0000 5.0615 +0.0000 0.0000 5.1051 +primID: 6 +startangle_c: 12 +endpose_c: 0 0 11 +additionalactioncostmult: 200 +intermediateposes: 10 +0.0000 0.0000 4.7124 +0.0000 0.0000 4.6688 +0.0000 0.0000 4.6251 +0.0000 0.0000 4.5815 +0.0000 0.0000 4.5379 +0.0000 0.0000 4.4942 +0.0000 0.0000 4.4506 +0.0000 0.0000 4.4070 +0.0000 0.0000 4.3633 +0.0000 0.0000 4.3197 +primID: 0 +startangle_c: 13 +endpose_c: 1 -2 13 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 5.1051 +0.0056 -0.0111 5.1051 +0.0111 -0.0222 5.1051 +0.0167 -0.0333 5.1051 +0.0222 -0.0444 5.1051 +0.0278 -0.0556 5.1051 +0.0333 -0.0667 5.1051 +0.0389 -0.0778 5.1051 +0.0444 -0.0889 5.1051 +0.0500 -0.1000 5.1051 +primID: 1 +startangle_c: 13 +endpose_c: 3 -6 13 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 5.1051 +0.0167 -0.0333 5.1051 +0.0333 -0.0667 5.1051 +0.0500 -0.1000 5.1051 +0.0667 -0.1333 5.1051 +0.0833 -0.1667 5.1051 +0.1000 -0.2000 5.1051 +0.1167 -0.2333 5.1051 +0.1333 -0.2667 5.1051 +0.1500 -0.3000 5.1051 +primID: 2 +startangle_c: 13 +endpose_c: -1 2 13 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 5.1051 +-0.0056 0.0111 5.1051 +-0.0111 0.0222 5.1051 +-0.0167 0.0333 5.1051 +-0.0222 0.0444 5.1051 +-0.0278 0.0556 5.1051 +-0.0333 0.0667 5.1051 +-0.0389 0.0778 5.1051 +-0.0444 0.0889 5.1051 +-0.0500 0.1000 5.1051 +primID: 3 +startangle_c: 13 +endpose_c: 4 -5 14 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 5.1051 +0.0184 -0.0298 5.1354 +0.0379 -0.0592 5.1657 +0.0583 -0.0881 5.1960 +0.0796 -0.1165 5.2263 +0.1019 -0.1444 5.2566 +0.1251 -0.1717 5.2869 +0.1492 -0.1984 5.3172 +0.1742 -0.2245 5.3475 +0.2000 -0.2500 5.3778 +primID: 4 +startangle_c: 13 +endpose_c: 2 -7 12 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 5.1051 +0.0156 -0.0377 5.1051 +0.0312 -0.0754 5.1051 +0.0468 -0.1131 5.1051 +0.0623 -0.1508 5.0860 +0.0758 -0.1893 5.0113 +0.0863 -0.2287 4.9366 +0.0939 -0.2687 4.8618 +0.0985 -0.3092 4.7871 +0.1000 -0.3500 4.7124 +primID: 5 +startangle_c: 13 +endpose_c: 0 0 14 +additionalactioncostmult: 200 +intermediateposes: 10 +0.0000 0.0000 5.1051 +0.0000 0.0000 5.1487 +0.0000 0.0000 5.1924 +0.0000 0.0000 5.2360 +0.0000 0.0000 5.2796 +0.0000 0.0000 5.3233 +0.0000 0.0000 5.3669 +0.0000 0.0000 5.4105 +0.0000 0.0000 5.4542 +0.0000 0.0000 5.4978 +primID: 6 +startangle_c: 13 +endpose_c: 0 0 12 +additionalactioncostmult: 200 +intermediateposes: 10 +0.0000 0.0000 5.1051 +0.0000 0.0000 5.0615 +0.0000 0.0000 5.0178 +0.0000 0.0000 4.9742 +0.0000 0.0000 4.9306 +0.0000 0.0000 4.8869 +0.0000 0.0000 4.8433 +0.0000 0.0000 4.7997 +0.0000 0.0000 4.7560 +0.0000 0.0000 4.7124 +primID: 0 +startangle_c: 14 +endpose_c: 1 -1 14 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 5.4978 +0.0056 -0.0056 5.4978 +0.0111 -0.0111 5.4978 +0.0167 -0.0167 5.4978 +0.0222 -0.0222 5.4978 +0.0278 -0.0278 5.4978 +0.0333 -0.0333 5.4978 +0.0389 -0.0389 5.4978 +0.0444 -0.0444 5.4978 +0.0500 -0.0500 5.4978 +primID: 1 +startangle_c: 14 +endpose_c: 6 -6 14 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 5.4978 +0.0333 -0.0333 5.4978 +0.0667 -0.0667 5.4978 +0.1000 -0.1000 5.4978 +0.1333 -0.1333 5.4978 +0.1667 -0.1667 5.4978 +0.2000 -0.2000 5.4978 +0.2333 -0.2333 5.4978 +0.2667 -0.2667 5.4978 +0.3000 -0.3000 5.4978 +primID: 2 +startangle_c: 14 +endpose_c: -1 1 14 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 5.4978 +-0.0056 0.0056 5.4978 +-0.0111 0.0111 5.4978 +-0.0167 0.0167 5.4978 +-0.0222 0.0222 5.4978 +-0.0278 0.0278 5.4978 +-0.0333 0.0333 5.4978 +-0.0389 0.0389 5.4978 +-0.0444 0.0444 5.4978 +-0.0500 0.0500 5.4978 +primID: 3 +startangle_c: 14 +endpose_c: 7 -5 15 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 5.4978 +0.0341 -0.0341 5.4978 +0.0684 -0.0678 5.5275 +0.1043 -0.1000 5.5793 +0.1418 -0.1302 5.6312 +0.1809 -0.1584 5.6830 +0.2213 -0.1846 5.7349 +0.2631 -0.2086 5.7868 +0.3060 -0.2304 5.8386 +0.3500 -0.2500 5.8905 +primID: 4 +startangle_c: 14 +endpose_c: 5 -7 13 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 5.4978 +0.0341 -0.0341 5.4978 +0.0678 -0.0684 5.4681 +0.1000 -0.1043 5.4162 +0.1302 -0.1418 5.3644 +0.1584 -0.1809 5.3125 +0.1846 -0.2213 5.2607 +0.2086 -0.2631 5.2088 +0.2304 -0.3060 5.1569 +0.2500 -0.3500 5.1051 +primID: 5 +startangle_c: 14 +endpose_c: 0 0 15 +additionalactioncostmult: 200 +intermediateposes: 10 +0.0000 0.0000 5.4978 +0.0000 0.0000 5.5414 +0.0000 0.0000 5.5851 +0.0000 0.0000 5.6287 +0.0000 0.0000 5.6723 +0.0000 0.0000 5.7160 +0.0000 0.0000 5.7596 +0.0000 0.0000 5.8032 +0.0000 0.0000 5.8469 +0.0000 0.0000 5.8905 +primID: 6 +startangle_c: 14 +endpose_c: 0 0 13 +additionalactioncostmult: 200 +intermediateposes: 10 +0.0000 0.0000 5.4978 +0.0000 0.0000 5.4542 +0.0000 0.0000 5.4105 +0.0000 0.0000 5.3669 +0.0000 0.0000 5.3233 +0.0000 0.0000 5.2796 +0.0000 0.0000 5.2360 +0.0000 0.0000 5.1924 +0.0000 0.0000 5.1487 +0.0000 0.0000 5.1051 +primID: 0 +startangle_c: 15 +endpose_c: 2 -1 15 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 5.8905 +0.0111 -0.0056 5.8905 +0.0222 -0.0111 5.8905 +0.0333 -0.0167 5.8905 +0.0444 -0.0222 5.8905 +0.0556 -0.0278 5.8905 +0.0667 -0.0333 5.8905 +0.0778 -0.0389 5.8905 +0.0889 -0.0444 5.8905 +0.1000 -0.0500 5.8905 +primID: 1 +startangle_c: 15 +endpose_c: 6 -3 15 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 5.8905 +0.0333 -0.0167 5.8905 +0.0667 -0.0333 5.8905 +0.1000 -0.0500 5.8905 +0.1333 -0.0667 5.8905 +0.1667 -0.0833 5.8905 +0.2000 -0.1000 5.8905 +0.2333 -0.1167 5.8905 +0.2667 -0.1333 5.8905 +0.3000 -0.1500 5.8905 +primID: 2 +startangle_c: 15 +endpose_c: -2 1 15 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 5.8905 +-0.0111 0.0056 5.8905 +-0.0222 0.0111 5.8905 +-0.0333 0.0167 5.8905 +-0.0444 0.0222 5.8905 +-0.0556 0.0278 5.8905 +-0.0667 0.0333 5.8905 +-0.0778 0.0389 5.8905 +-0.0889 0.0444 5.8905 +-0.1000 0.0500 5.8905 +primID: 3 +startangle_c: 15 +endpose_c: 5 -4 14 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 5.8905 +0.0298 -0.0184 5.8602 +0.0592 -0.0379 5.8299 +0.0881 -0.0583 5.7996 +0.1165 -0.0796 5.7693 +0.1444 -0.1019 5.7390 +0.1717 -0.1251 5.7087 +0.1984 -0.1492 5.6784 +0.2245 -0.1742 5.6481 +0.2500 -0.2000 5.6178 +primID: 4 +startangle_c: 15 +endpose_c: 7 -2 0 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 5.8905 +0.0377 -0.0156 5.8905 +0.0754 -0.0312 5.8905 +0.1131 -0.0468 5.8905 +0.1508 -0.0623 5.9096 +0.1893 -0.0758 5.9843 +0.2287 -0.0863 6.0590 +0.2687 -0.0939 6.1337 +0.3092 -0.0985 6.2085 +0.3500 -0.1000 6.2832 +primID: 5 +startangle_c: 15 +endpose_c: 0 0 14 +additionalactioncostmult: 200 +intermediateposes: 10 +0.0000 0.0000 5.8905 +0.0000 0.0000 5.8469 +0.0000 0.0000 5.8032 +0.0000 0.0000 5.7596 +0.0000 0.0000 5.7160 +0.0000 0.0000 5.6723 +0.0000 0.0000 5.6287 +0.0000 0.0000 5.5851 +0.0000 0.0000 5.5414 +0.0000 0.0000 5.4978 +primID: 6 +startangle_c: 15 +endpose_c: 0 0 0 +additionalactioncostmult: 200 +intermediateposes: 10 +0.0000 0.0000 5.8905 +0.0000 0.0000 5.2360 +0.0000 0.0000 4.5815 +0.0000 0.0000 3.9270 +0.0000 0.0000 3.2725 +0.0000 0.0000 2.6180 +0.0000 0.0000 1.9635 +0.0000 0.0000 1.3090 +0.0000 0.0000 0.6545 +0.0000 0.0000 0.0000 diff --git a/Controllers/Packages/amr_startup/config/mprim/unicycle_5cm_noreverse_trolley.mprim b/Controllers/Packages/amr_startup/config/mprim/unicycle_5cm_noreverse_trolley.mprim new file mode 100755 index 0000000..596f5c5 --- /dev/null +++ b/Controllers/Packages/amr_startup/config/mprim/unicycle_5cm_noreverse_trolley.mprim @@ -0,0 +1,2403 @@ +resolution_m: 0.050000 +numberofangles: 16 +totalnumberofprimitives: 160 +primID: 0 +startangle_c: 0 +endpose_c: 1 0 0 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 0.0000 +0.0056 0.0000 0.0000 +0.0111 0.0000 0.0000 +0.0167 0.0000 0.0000 +0.0222 0.0000 0.0000 +0.0278 0.0000 0.0000 +0.0333 0.0000 0.0000 +0.0389 0.0000 0.0000 +0.0444 0.0000 0.0000 +0.0500 0.0000 0.0000 +primID: 1 +startangle_c: 0 +endpose_c: 8 0 0 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 0.0000 +0.0444 0.0000 0.0000 +0.0889 0.0000 0.0000 +0.1333 0.0000 0.0000 +0.1778 0.0000 0.0000 +0.2222 0.0000 0.0000 +0.2667 0.0000 0.0000 +0.3111 0.0000 0.0000 +0.3556 0.0000 0.0000 +0.4000 0.0000 0.0000 +primID: 2 +startangle_c: 0 +endpose_c: 16 4 1 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 0.0000 +0.0904 0.0095 0.0349 +0.1806 0.0222 0.0699 +0.2707 0.0381 0.1048 +0.3604 0.0572 0.1398 +0.4496 0.0795 0.1747 +0.5383 0.1050 0.2097 +0.6264 0.1335 0.2446 +0.7136 0.1652 0.2796 +0.8000 0.2000 0.3145 +primID: 3 +startangle_c: 0 +endpose_c: 16 -4 -1 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 0.0000 +0.0904 -0.0095 -0.0349 +0.1806 -0.0222 -0.0699 +0.2707 -0.0381 -0.1048 +0.3604 -0.0572 -0.1398 +0.4496 -0.0795 -0.1747 +0.5383 -0.1050 -0.2097 +0.6264 -0.1335 -0.2446 +0.7136 -0.1652 -0.2796 +0.8000 -0.2000 -0.3145 +primID: 4 +startangle_c: 0 +endpose_c: 5 2 1 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 0.0000 +0.0280 0.0085 0.0223 +0.0559 0.0177 0.0445 +0.0839 0.0275 0.0668 +0.1117 0.0380 0.0890 +0.1396 0.0491 0.1113 +0.1673 0.0609 0.1335 +0.1950 0.0733 0.1558 +0.2225 0.0863 0.1781 +0.2500 0.1000 0.2003 +primID: 5 +startangle_c: 0 +endpose_c: 5 -2 -1 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 0.0000 +0.0280 -0.0085 -0.0223 +0.0559 -0.0177 -0.0445 +0.0839 -0.0275 -0.0668 +0.1117 -0.0380 -0.0890 +0.1396 -0.0491 -0.1113 +0.1673 -0.0609 -0.1335 +0.1950 -0.0733 -0.1558 +0.2225 -0.0863 -0.1781 +0.2500 -0.1000 -0.2003 +primID: 6 +startangle_c: 0 +endpose_c: 0 0 1 +additionalactioncostmult: 1000 +intermediateposes: 10 +0.0000 0.0000 0.0000 +0.0000 0.0000 0.0436 +0.0000 0.0000 0.0873 +0.0000 0.0000 0.1309 +0.0000 0.0000 0.1745 +0.0000 0.0000 0.2182 +0.0000 0.0000 0.2618 +0.0000 0.0000 0.3054 +0.0000 0.0000 0.3491 +0.0000 0.0000 0.3927 +primID: 7 +startangle_c: 0 +endpose_c: 0 0 -1 +additionalactioncostmult: 1000 +intermediateposes: 10 +0.0000 0.0000 0.0000 +0.0000 0.0000 -0.0436 +0.0000 0.0000 -0.0873 +0.0000 0.0000 -0.1309 +0.0000 0.0000 -0.1745 +0.0000 0.0000 -0.2182 +0.0000 0.0000 -0.2618 +0.0000 0.0000 -0.3054 +0.0000 0.0000 -0.3491 +0.0000 0.0000 -0.3927 +primID: 8 +startangle_c: 0 +endpose_c: 8 1 0 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 0.0000 +0.0444 0.0056 0.0000 +0.0889 0.0111 0.0000 +0.1333 0.0167 0.0000 +0.1778 0.0222 0.0000 +0.2222 0.0278 0.0000 +0.2667 0.0333 0.0000 +0.3111 0.0389 0.0000 +0.3556 0.0444 0.0000 +0.4000 0.0500 0.0000 +primID: 9 +startangle_c: 0 +endpose_c: 8 -1 0 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 0.0000 +0.0444 -0.0056 0.0000 +0.0889 -0.0111 0.0000 +0.1333 -0.0167 0.0000 +0.1778 -0.0222 0.0000 +0.2222 -0.0278 0.0000 +0.2667 -0.0333 0.0000 +0.3111 -0.0389 0.0000 +0.3556 -0.0444 0.0000 +0.4000 -0.0500 0.0000 +primID: 0 +startangle_c: 1 +endpose_c: 2 1 1 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 0.3927 +0.0111 0.0056 0.3927 +0.0222 0.0111 0.3927 +0.0333 0.0167 0.3927 +0.0444 0.0222 0.3927 +0.0556 0.0278 0.3927 +0.0667 0.0333 0.3927 +0.0778 0.0389 0.3927 +0.0889 0.0444 0.3927 +0.1000 0.0500 0.3927 +primID: 1 +startangle_c: 1 +endpose_c: 6 3 1 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 0.3927 +0.0333 0.0167 0.3927 +0.0667 0.0333 0.3927 +0.1000 0.0500 0.3927 +0.1333 0.0667 0.3927 +0.1667 0.0833 0.3927 +0.2000 0.1000 0.3927 +0.2333 0.1167 0.3927 +0.2667 0.1333 0.3927 +0.3000 0.1500 0.3927 +primID: 2 +startangle_c: 1 +endpose_c: 13 9 2 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 0.3927 +0.0793 0.0378 0.4329 +0.1572 0.0787 0.4732 +0.2334 0.1229 0.5134 +0.3079 0.1701 0.5537 +0.3805 0.2204 0.5939 +0.4512 0.2736 0.6342 +0.5197 0.3296 0.6744 +0.5860 0.3885 0.7147 +0.6500 0.4500 0.7549 +primID: 3 +startangle_c: 1 +endpose_c: 14 3 0 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 0.3927 +0.0741 0.0305 0.3774 +0.1493 0.0582 0.3302 +0.2256 0.0824 0.2830 +0.3031 0.1030 0.2359 +0.3814 0.1199 0.1887 +0.4604 0.1330 0.1415 +0.5400 0.1424 0.0943 +0.6199 0.1481 0.0472 +0.7000 0.1500 -0.0000 +primID: 4 +startangle_c: 1 +endpose_c: 5 4 2 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 0.3927 +0.0298 0.0184 0.4230 +0.0592 0.0379 0.4533 +0.0881 0.0583 0.4836 +0.1165 0.0796 0.5139 +0.1444 0.1019 0.5442 +0.1717 0.1251 0.5745 +0.1984 0.1492 0.6048 +0.2245 0.1742 0.6351 +0.2500 0.2000 0.6654 +primID: 5 +startangle_c: 1 +endpose_c: 6 1 0 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 0.3927 +0.0320 0.0105 0.3551 +0.0644 0.0197 0.3174 +0.0973 0.0278 0.2798 +0.1304 0.0346 0.2421 +0.1639 0.0402 0.2045 +0.1977 0.0446 0.1669 +0.2316 0.0476 0.1292 +0.2658 0.0495 0.0916 +0.3000 0.0500 0.0540 +primID: 6 +startangle_c: 1 +endpose_c: 0 0 2 +additionalactioncostmult: 1000 +intermediateposes: 10 +0.0000 0.0000 0.3927 +0.0000 0.0000 0.4363 +0.0000 0.0000 0.4800 +0.0000 0.0000 0.5236 +0.0000 0.0000 0.5672 +0.0000 0.0000 0.6109 +0.0000 0.0000 0.6545 +0.0000 0.0000 0.6981 +0.0000 0.0000 0.7418 +0.0000 0.0000 0.7854 +primID: 7 +startangle_c: 1 +endpose_c: 0 0 0 +additionalactioncostmult: 1000 +intermediateposes: 10 +0.0000 0.0000 0.3927 +0.0000 0.0000 0.3491 +0.0000 0.0000 0.3054 +0.0000 0.0000 0.2618 +0.0000 0.0000 0.2182 +0.0000 0.0000 0.1745 +0.0000 0.0000 0.1309 +0.0000 0.0000 0.0873 +0.0000 0.0000 0.0436 +0.0000 0.0000 0.0000 +primID: 8 +startangle_c: 1 +endpose_c: 7 2 1 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 0.3927 +0.0389 0.0111 0.3927 +0.0778 0.0222 0.3927 +0.1167 0.0333 0.3927 +0.1556 0.0444 0.3927 +0.1944 0.0556 0.3927 +0.2333 0.0667 0.3927 +0.2722 0.0778 0.3927 +0.3111 0.0889 0.3927 +0.3500 0.1000 0.3927 +primID: 9 +startangle_c: 1 +endpose_c: 5 4 1 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 0.3927 +0.0278 0.0222 0.3927 +0.0556 0.0444 0.3927 +0.0833 0.0667 0.3927 +0.1111 0.0889 0.3927 +0.1389 0.1111 0.3927 +0.1667 0.1333 0.3927 +0.1944 0.1556 0.3927 +0.2222 0.1778 0.3927 +0.2500 0.2000 0.3927 +primID: 0 +startangle_c: 2 +endpose_c: 1 1 2 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 0.7854 +0.0056 0.0056 0.7854 +0.0111 0.0111 0.7854 +0.0167 0.0167 0.7854 +0.0222 0.0222 0.7854 +0.0278 0.0278 0.7854 +0.0333 0.0333 0.7854 +0.0389 0.0389 0.7854 +0.0444 0.0444 0.7854 +0.0500 0.0500 0.7854 +primID: 1 +startangle_c: 2 +endpose_c: 6 6 2 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 0.7854 +0.0333 0.0333 0.7854 +0.0667 0.0667 0.7854 +0.1000 0.1000 0.7854 +0.1333 0.1333 0.7854 +0.1667 0.1667 0.7854 +0.2000 0.2000 0.7854 +0.2333 0.2333 0.7854 +0.2667 0.2667 0.7854 +0.3000 0.3000 0.7854 +primID: 2 +startangle_c: 2 +endpose_c: 11 14 3 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 0.7854 +0.0705 0.0705 0.7854 +0.1411 0.1411 0.7854 +0.2116 0.2116 0.7854 +0.2816 0.2828 0.8201 +0.3469 0.3581 0.8917 +0.4068 0.4379 0.9633 +0.4607 0.5218 1.0349 +0.5086 0.6093 1.1065 +0.5500 0.7000 1.1781 +primID: 3 +startangle_c: 2 +endpose_c: 14 11 1 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 0.7854 +0.0705 0.0705 0.7854 +0.1411 0.1411 0.7854 +0.2116 0.2116 0.7854 +0.2828 0.2816 0.7507 +0.3581 0.3469 0.6791 +0.4379 0.4068 0.6075 +0.5218 0.4607 0.5359 +0.6093 0.5086 0.4643 +0.7000 0.5500 0.3927 +primID: 4 +startangle_c: 2 +endpose_c: 4 6 3 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 0.7854 +0.0278 0.0292 0.8288 +0.0543 0.0595 0.8722 +0.0795 0.0910 0.9156 +0.1033 0.1235 0.9590 +0.1257 0.1571 1.0024 +0.1466 0.1916 1.0458 +0.1659 0.2269 1.0892 +0.1838 0.2631 1.1326 +0.2000 0.3000 1.1760 +primID: 5 +startangle_c: 2 +endpose_c: 6 4 1 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 0.7854 +0.0292 0.0278 0.7420 +0.0595 0.0543 0.6986 +0.0910 0.0795 0.6552 +0.1235 0.1033 0.6118 +0.1571 0.1257 0.5684 +0.1916 0.1466 0.5250 +0.2269 0.1659 0.4816 +0.2631 0.1838 0.4382 +0.3000 0.2000 0.3948 +primID: 6 +startangle_c: 2 +endpose_c: 0 0 3 +additionalactioncostmult: 1000 +intermediateposes: 10 +0.0000 0.0000 0.7854 +0.0000 0.0000 0.8290 +0.0000 0.0000 0.8727 +0.0000 0.0000 0.9163 +0.0000 0.0000 0.9599 +0.0000 0.0000 1.0036 +0.0000 0.0000 1.0472 +0.0000 0.0000 1.0908 +0.0000 0.0000 1.1345 +0.0000 0.0000 1.1781 +primID: 7 +startangle_c: 2 +endpose_c: 0 0 1 +additionalactioncostmult: 1000 +intermediateposes: 10 +0.0000 0.0000 0.7854 +0.0000 0.0000 0.7418 +0.0000 0.0000 0.6981 +0.0000 0.0000 0.6545 +0.0000 0.0000 0.6109 +0.0000 0.0000 0.5672 +0.0000 0.0000 0.5236 +0.0000 0.0000 0.4800 +0.0000 0.0000 0.4363 +0.0000 0.0000 0.3927 +primID: 8 +startangle_c: 2 +endpose_c: 6 7 2 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 0.7854 +0.0333 0.0389 0.7854 +0.0667 0.0778 0.7854 +0.1000 0.1167 0.7854 +0.1333 0.1556 0.7854 +0.1667 0.1944 0.7854 +0.2000 0.2333 0.7854 +0.2333 0.2722 0.7854 +0.2667 0.3111 0.7854 +0.3000 0.3500 0.7854 +primID: 9 +startangle_c: 2 +endpose_c: 7 6 2 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 0.7854 +0.0389 0.0333 0.7854 +0.0778 0.0667 0.7854 +0.1167 0.1000 0.7854 +0.1556 0.1333 0.7854 +0.1944 0.1667 0.7854 +0.2333 0.2000 0.7854 +0.2722 0.2333 0.7854 +0.3111 0.2667 0.7854 +0.3500 0.3000 0.7854 +primID: 0 +startangle_c: 3 +endpose_c: 1 2 3 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 1.1781 +0.0056 0.0111 1.1781 +0.0111 0.0222 1.1781 +0.0167 0.0333 1.1781 +0.0222 0.0444 1.1781 +0.0278 0.0556 1.1781 +0.0333 0.0667 1.1781 +0.0389 0.0778 1.1781 +0.0444 0.0889 1.1781 +0.0500 0.1000 1.1781 +primID: 1 +startangle_c: 3 +endpose_c: 3 6 3 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 1.1781 +0.0167 0.0333 1.1781 +0.0333 0.0667 1.1781 +0.0500 0.1000 1.1781 +0.0667 0.1333 1.1781 +0.0833 0.1667 1.1781 +0.1000 0.2000 1.1781 +0.1167 0.2333 1.1781 +0.1333 0.2667 1.1781 +0.1500 0.3000 1.1781 +primID: 2 +startangle_c: 3 +endpose_c: 9 13 2 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 1.1781 +0.0378 0.0793 1.1378 +0.0787 0.1572 1.0976 +0.1229 0.2334 1.0574 +0.1701 0.3079 1.0171 +0.2204 0.3805 0.9769 +0.2736 0.4512 0.9366 +0.3296 0.5197 0.8964 +0.3885 0.5860 0.8561 +0.4500 0.6500 0.8159 +primID: 3 +startangle_c: 3 +endpose_c: 3 14 4 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 1.1781 +0.0305 0.0741 1.1934 +0.0582 0.1493 1.2406 +0.0824 0.2256 1.2878 +0.1030 0.3031 1.3349 +0.1199 0.3814 1.3821 +0.1330 0.4604 1.4293 +0.1424 0.5400 1.4765 +0.1481 0.6199 1.5236 +0.1500 0.7000 1.5708 +primID: 4 +startangle_c: 3 +endpose_c: 4 5 2 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 1.1781 +0.0184 0.0298 1.1478 +0.0379 0.0592 1.1175 +0.0583 0.0881 1.0872 +0.0796 0.1165 1.0569 +0.1019 0.1444 1.0266 +0.1251 0.1717 0.9963 +0.1492 0.1984 0.9660 +0.1742 0.2245 0.9357 +0.2000 0.2500 0.9054 +primID: 5 +startangle_c: 3 +endpose_c: 1 6 4 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 1.1781 +0.0105 0.0320 1.2157 +0.0197 0.0644 1.2534 +0.0278 0.0973 1.2910 +0.0346 0.1304 1.3286 +0.0402 0.1639 1.3663 +0.0446 0.1977 1.4039 +0.0476 0.2316 1.4416 +0.0495 0.2658 1.4792 +0.0500 0.3000 1.5168 +primID: 6 +startangle_c: 3 +endpose_c: 0 0 2 +additionalactioncostmult: 1000 +intermediateposes: 10 +0.0000 0.0000 1.1781 +0.0000 0.0000 1.1345 +0.0000 0.0000 1.0908 +0.0000 0.0000 1.0472 +0.0000 0.0000 1.0036 +0.0000 0.0000 0.9599 +0.0000 0.0000 0.9163 +0.0000 0.0000 0.8727 +0.0000 0.0000 0.8290 +0.0000 0.0000 0.7854 +primID: 7 +startangle_c: 3 +endpose_c: 0 0 4 +additionalactioncostmult: 1000 +intermediateposes: 10 +0.0000 0.0000 1.1781 +0.0000 0.0000 1.2217 +0.0000 0.0000 1.2654 +0.0000 0.0000 1.3090 +0.0000 0.0000 1.3526 +0.0000 0.0000 1.3963 +0.0000 0.0000 1.4399 +0.0000 0.0000 1.4835 +0.0000 0.0000 1.5272 +0.0000 0.0000 1.5708 +primID: 8 +startangle_c: 3 +endpose_c: 2 7 3 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 1.1781 +0.0111 0.0389 1.1781 +0.0222 0.0778 1.1781 +0.0333 0.1167 1.1781 +0.0444 0.1556 1.1781 +0.0556 0.1944 1.1781 +0.0667 0.2333 1.1781 +0.0778 0.2722 1.1781 +0.0889 0.3111 1.1781 +0.1000 0.3500 1.1781 +primID: 9 +startangle_c: 3 +endpose_c: 4 5 3 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 1.1781 +0.0222 0.0278 1.1781 +0.0444 0.0556 1.1781 +0.0667 0.0833 1.1781 +0.0889 0.1111 1.1781 +0.1111 0.1389 1.1781 +0.1333 0.1667 1.1781 +0.1556 0.1944 1.1781 +0.1778 0.2222 1.1781 +0.2000 0.2500 1.1781 +primID: 0 +startangle_c: 4 +endpose_c: 0 1 4 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 1.5708 +0.0000 0.0056 1.5708 +0.0000 0.0111 1.5708 +0.0000 0.0167 1.5708 +0.0000 0.0222 1.5708 +0.0000 0.0278 1.5708 +0.0000 0.0333 1.5708 +0.0000 0.0389 1.5708 +0.0000 0.0444 1.5708 +0.0000 0.0500 1.5708 +primID: 1 +startangle_c: 4 +endpose_c: 0 8 4 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 1.5708 +0.0000 0.0444 1.5708 +0.0000 0.0889 1.5708 +0.0000 0.1333 1.5708 +0.0000 0.1778 1.5708 +0.0000 0.2222 1.5708 +0.0000 0.2667 1.5708 +0.0000 0.3111 1.5708 +0.0000 0.3556 1.5708 +0.0000 0.4000 1.5708 +primID: 2 +startangle_c: 4 +endpose_c: -4 16 5 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 1.5708 +-0.0095 0.0904 1.6057 +-0.0222 0.1806 1.6407 +-0.0381 0.2707 1.6756 +-0.0572 0.3604 1.7106 +-0.0795 0.4496 1.7455 +-0.1050 0.5383 1.7805 +-0.1335 0.6264 1.8154 +-0.1652 0.7136 1.8503 +-0.2000 0.8000 1.8853 +primID: 3 +startangle_c: 4 +endpose_c: 4 16 3 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 1.5708 +0.0095 0.0904 1.5359 +0.0222 0.1806 1.5009 +0.0381 0.2707 1.4660 +0.0572 0.3604 1.4310 +0.0795 0.4496 1.3961 +0.1050 0.5383 1.3611 +0.1335 0.6264 1.3262 +0.1652 0.7136 1.2912 +0.2000 0.8000 1.2563 +primID: 4 +startangle_c: 4 +endpose_c: -2 5 5 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 1.5708 +-0.0085 0.0280 1.5931 +-0.0177 0.0559 1.6153 +-0.0275 0.0839 1.6376 +-0.0380 0.1117 1.6598 +-0.0491 0.1396 1.6821 +-0.0609 0.1673 1.7043 +-0.0733 0.1950 1.7266 +-0.0863 0.2225 1.7489 +-0.1000 0.2500 1.7711 +primID: 5 +startangle_c: 4 +endpose_c: 2 5 3 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 1.5708 +0.0085 0.0280 1.5485 +0.0177 0.0559 1.5263 +0.0275 0.0839 1.5040 +0.0380 0.1117 1.4818 +0.0491 0.1396 1.4595 +0.0609 0.1673 1.4373 +0.0733 0.1950 1.4150 +0.0863 0.2225 1.3927 +0.1000 0.2500 1.3705 +primID: 6 +startangle_c: 4 +endpose_c: 0 0 5 +additionalactioncostmult: 1000 +intermediateposes: 10 +0.0000 0.0000 1.5708 +0.0000 0.0000 1.6144 +0.0000 0.0000 1.6581 +0.0000 0.0000 1.7017 +0.0000 0.0000 1.7453 +0.0000 0.0000 1.7890 +0.0000 0.0000 1.8326 +0.0000 0.0000 1.8762 +0.0000 0.0000 1.9199 +0.0000 0.0000 1.9635 +primID: 7 +startangle_c: 4 +endpose_c: 0 0 3 +additionalactioncostmult: 1000 +intermediateposes: 10 +0.0000 0.0000 1.5708 +0.0000 0.0000 1.5272 +0.0000 0.0000 1.4835 +0.0000 0.0000 1.4399 +0.0000 0.0000 1.3963 +0.0000 0.0000 1.3526 +0.0000 0.0000 1.3090 +0.0000 0.0000 1.2654 +0.0000 0.0000 1.2217 +0.0000 0.0000 1.1781 +primID: 8 +startangle_c: 4 +endpose_c: -1 8 4 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 1.5708 +-0.0056 0.0444 1.5708 +-0.0111 0.0889 1.5708 +-0.0167 0.1333 1.5708 +-0.0222 0.1778 1.5708 +-0.0278 0.2222 1.5708 +-0.0333 0.2667 1.5708 +-0.0389 0.3111 1.5708 +-0.0444 0.3556 1.5708 +-0.0500 0.4000 1.5708 +primID: 9 +startangle_c: 4 +endpose_c: 1 8 4 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 1.5708 +0.0056 0.0444 1.5708 +0.0111 0.0889 1.5708 +0.0167 0.1333 1.5708 +0.0222 0.1778 1.5708 +0.0278 0.2222 1.5708 +0.0333 0.2667 1.5708 +0.0389 0.3111 1.5708 +0.0444 0.3556 1.5708 +0.0500 0.4000 1.5708 +primID: 0 +startangle_c: 5 +endpose_c: -1 2 5 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 1.9635 +-0.0056 0.0111 1.9635 +-0.0111 0.0222 1.9635 +-0.0167 0.0333 1.9635 +-0.0222 0.0444 1.9635 +-0.0278 0.0556 1.9635 +-0.0333 0.0667 1.9635 +-0.0389 0.0778 1.9635 +-0.0444 0.0889 1.9635 +-0.0500 0.1000 1.9635 +primID: 1 +startangle_c: 5 +endpose_c: -3 6 5 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 1.9635 +-0.0167 0.0333 1.9635 +-0.0333 0.0667 1.9635 +-0.0500 0.1000 1.9635 +-0.0667 0.1333 1.9635 +-0.0833 0.1667 1.9635 +-0.1000 0.2000 1.9635 +-0.1167 0.2333 1.9635 +-0.1333 0.2667 1.9635 +-0.1500 0.3000 1.9635 +primID: 2 +startangle_c: 5 +endpose_c: -9 13 6 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 1.9635 +-0.0378 0.0793 2.0037 +-0.0787 0.1572 2.0440 +-0.1229 0.2334 2.0842 +-0.1701 0.3079 2.1245 +-0.2204 0.3805 2.1647 +-0.2736 0.4512 2.2050 +-0.3296 0.5197 2.2452 +-0.3885 0.5860 2.2855 +-0.4500 0.6500 2.3257 +primID: 3 +startangle_c: 5 +endpose_c: -3 14 4 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 1.9635 +-0.0305 0.0741 1.9482 +-0.0582 0.1493 1.9010 +-0.0824 0.2256 1.8538 +-0.1030 0.3031 1.8067 +-0.1199 0.3814 1.7595 +-0.1330 0.4604 1.7123 +-0.1424 0.5400 1.6651 +-0.1481 0.6199 1.6180 +-0.1500 0.7000 1.5708 +primID: 4 +startangle_c: 5 +endpose_c: -4 5 6 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 1.9635 +-0.0184 0.0298 1.9938 +-0.0379 0.0592 2.0241 +-0.0583 0.0881 2.0544 +-0.0796 0.1165 2.0847 +-0.1019 0.1444 2.1150 +-0.1251 0.1717 2.1453 +-0.1492 0.1984 2.1756 +-0.1742 0.2245 2.2059 +-0.2000 0.2500 2.2362 +primID: 5 +startangle_c: 5 +endpose_c: -1 6 4 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 1.9635 +-0.0105 0.0320 1.9259 +-0.0197 0.0644 1.8882 +-0.0278 0.0973 1.8506 +-0.0346 0.1304 1.8129 +-0.0402 0.1639 1.7753 +-0.0446 0.1977 1.7377 +-0.0476 0.2316 1.7000 +-0.0495 0.2658 1.6624 +-0.0500 0.3000 1.6248 +primID: 6 +startangle_c: 5 +endpose_c: 0 0 6 +additionalactioncostmult: 1000 +intermediateposes: 10 +0.0000 0.0000 1.9635 +0.0000 0.0000 2.0071 +0.0000 0.0000 2.0508 +0.0000 0.0000 2.0944 +0.0000 0.0000 2.1380 +0.0000 0.0000 2.1817 +0.0000 0.0000 2.2253 +0.0000 0.0000 2.2689 +0.0000 0.0000 2.3126 +0.0000 0.0000 2.3562 +primID: 7 +startangle_c: 5 +endpose_c: 0 0 4 +additionalactioncostmult: 1000 +intermediateposes: 10 +0.0000 0.0000 1.9635 +0.0000 0.0000 1.9199 +0.0000 0.0000 1.8762 +0.0000 0.0000 1.8326 +0.0000 0.0000 1.7890 +0.0000 0.0000 1.7453 +0.0000 0.0000 1.7017 +0.0000 0.0000 1.6581 +0.0000 0.0000 1.6144 +0.0000 0.0000 1.5708 +primID: 8 +startangle_c: 5 +endpose_c: -2 7 5 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 1.9635 +-0.0111 0.0389 1.9635 +-0.0222 0.0778 1.9635 +-0.0333 0.1167 1.9635 +-0.0444 0.1556 1.9635 +-0.0556 0.1944 1.9635 +-0.0667 0.2333 1.9635 +-0.0778 0.2722 1.9635 +-0.0889 0.3111 1.9635 +-0.1000 0.3500 1.9635 +primID: 9 +startangle_c: 5 +endpose_c: -4 5 5 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 1.9635 +-0.0222 0.0278 1.9635 +-0.0444 0.0556 1.9635 +-0.0667 0.0833 1.9635 +-0.0889 0.1111 1.9635 +-0.1111 0.1389 1.9635 +-0.1333 0.1667 1.9635 +-0.1556 0.1944 1.9635 +-0.1778 0.2222 1.9635 +-0.2000 0.2500 1.9635 +primID: 0 +startangle_c: 6 +endpose_c: -1 1 6 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 2.3562 +-0.0056 0.0056 2.3562 +-0.0111 0.0111 2.3562 +-0.0167 0.0167 2.3562 +-0.0222 0.0222 2.3562 +-0.0278 0.0278 2.3562 +-0.0333 0.0333 2.3562 +-0.0389 0.0389 2.3562 +-0.0444 0.0444 2.3562 +-0.0500 0.0500 2.3562 +primID: 1 +startangle_c: 6 +endpose_c: -6 6 6 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 2.3562 +-0.0333 0.0333 2.3562 +-0.0667 0.0667 2.3562 +-0.1000 0.1000 2.3562 +-0.1333 0.1333 2.3562 +-0.1667 0.1667 2.3562 +-0.2000 0.2000 2.3562 +-0.2333 0.2333 2.3562 +-0.2667 0.2667 2.3562 +-0.3000 0.3000 2.3562 +primID: 2 +startangle_c: 6 +endpose_c: -14 11 7 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 2.3562 +-0.0705 0.0705 2.3562 +-0.1411 0.1411 2.3562 +-0.2116 0.2116 2.3562 +-0.2828 0.2816 2.3909 +-0.3581 0.3469 2.4625 +-0.4379 0.4068 2.5341 +-0.5218 0.4607 2.6057 +-0.6093 0.5086 2.6773 +-0.7000 0.5500 2.7489 +primID: 3 +startangle_c: 6 +endpose_c: -11 14 5 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 2.3562 +-0.0705 0.0705 2.3562 +-0.1411 0.1411 2.3562 +-0.2116 0.2116 2.3562 +-0.2816 0.2828 2.3215 +-0.3469 0.3581 2.2499 +-0.4068 0.4379 2.1783 +-0.4607 0.5218 2.1067 +-0.5086 0.6093 2.0351 +-0.5500 0.7000 1.9635 +primID: 4 +startangle_c: 6 +endpose_c: -6 4 7 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 2.3562 +-0.0292 0.0278 2.3996 +-0.0595 0.0543 2.4430 +-0.0910 0.0795 2.4864 +-0.1235 0.1033 2.5298 +-0.1571 0.1257 2.5732 +-0.1916 0.1466 2.6166 +-0.2269 0.1659 2.6600 +-0.2631 0.1838 2.7034 +-0.3000 0.2000 2.7468 +primID: 5 +startangle_c: 6 +endpose_c: -4 6 5 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 2.3562 +-0.0278 0.0292 2.3128 +-0.0543 0.0595 2.2694 +-0.0795 0.0910 2.2260 +-0.1033 0.1235 2.1826 +-0.1257 0.1571 2.1392 +-0.1466 0.1916 2.0958 +-0.1659 0.2269 2.0524 +-0.1838 0.2631 2.0090 +-0.2000 0.3000 1.9656 +primID: 6 +startangle_c: 6 +endpose_c: 0 0 7 +additionalactioncostmult: 1000 +intermediateposes: 10 +0.0000 0.0000 2.3562 +0.0000 0.0000 2.3998 +0.0000 0.0000 2.4435 +0.0000 0.0000 2.4871 +0.0000 0.0000 2.5307 +0.0000 0.0000 2.5744 +0.0000 0.0000 2.6180 +0.0000 0.0000 2.6616 +0.0000 0.0000 2.7053 +0.0000 0.0000 2.7489 +primID: 7 +startangle_c: 6 +endpose_c: 0 0 5 +additionalactioncostmult: 1000 +intermediateposes: 10 +0.0000 0.0000 2.3562 +0.0000 0.0000 2.3126 +0.0000 0.0000 2.2689 +0.0000 0.0000 2.2253 +0.0000 0.0000 2.1817 +0.0000 0.0000 2.1380 +0.0000 0.0000 2.0944 +0.0000 0.0000 2.0508 +0.0000 0.0000 2.0071 +0.0000 0.0000 1.9635 +primID: 8 +startangle_c: 6 +endpose_c: -7 6 6 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 2.3562 +-0.0389 0.0333 2.3562 +-0.0778 0.0667 2.3562 +-0.1167 0.1000 2.3562 +-0.1556 0.1333 2.3562 +-0.1944 0.1667 2.3562 +-0.2333 0.2000 2.3562 +-0.2722 0.2333 2.3562 +-0.3111 0.2667 2.3562 +-0.3500 0.3000 2.3562 +primID: 9 +startangle_c: 6 +endpose_c: -6 7 6 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 2.3562 +-0.0333 0.0389 2.3562 +-0.0667 0.0778 2.3562 +-0.1000 0.1167 2.3562 +-0.1333 0.1556 2.3562 +-0.1667 0.1944 2.3562 +-0.2000 0.2333 2.3562 +-0.2333 0.2722 2.3562 +-0.2667 0.3111 2.3562 +-0.3000 0.3500 2.3562 +primID: 0 +startangle_c: 7 +endpose_c: -2 1 7 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 2.7489 +-0.0111 0.0056 2.7489 +-0.0222 0.0111 2.7489 +-0.0333 0.0167 2.7489 +-0.0444 0.0222 2.7489 +-0.0556 0.0278 2.7489 +-0.0667 0.0333 2.7489 +-0.0778 0.0389 2.7489 +-0.0889 0.0444 2.7489 +-0.1000 0.0500 2.7489 +primID: 1 +startangle_c: 7 +endpose_c: -6 3 7 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 2.7489 +-0.0333 0.0167 2.7489 +-0.0667 0.0333 2.7489 +-0.1000 0.0500 2.7489 +-0.1333 0.0667 2.7489 +-0.1667 0.0833 2.7489 +-0.2000 0.1000 2.7489 +-0.2333 0.1167 2.7489 +-0.2667 0.1333 2.7489 +-0.3000 0.1500 2.7489 +primID: 2 +startangle_c: 7 +endpose_c: -13 9 6 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 2.7489 +-0.0793 0.0378 2.7086 +-0.1572 0.0787 2.6684 +-0.2334 0.1229 2.6281 +-0.3079 0.1701 2.5879 +-0.3805 0.2204 2.5477 +-0.4512 0.2736 2.5074 +-0.5197 0.3296 2.4672 +-0.5860 0.3885 2.4269 +-0.6500 0.4500 2.3867 +primID: 3 +startangle_c: 7 +endpose_c: -14 3 8 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 2.7489 +-0.0741 0.0305 2.7642 +-0.1493 0.0582 2.8114 +-0.2256 0.0824 2.8586 +-0.3031 0.1030 2.9057 +-0.3814 0.1199 2.9529 +-0.4604 0.1330 3.0001 +-0.5400 0.1424 3.0472 +-0.6199 0.1481 3.0944 +-0.7000 0.1500 3.1416 +primID: 4 +startangle_c: 7 +endpose_c: -5 4 6 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 2.7489 +-0.0298 0.0184 2.7186 +-0.0592 0.0379 2.6883 +-0.0881 0.0583 2.6580 +-0.1165 0.0796 2.6277 +-0.1444 0.1019 2.5974 +-0.1717 0.1251 2.5671 +-0.1984 0.1492 2.5368 +-0.2245 0.1742 2.5065 +-0.2500 0.2000 2.4762 +primID: 5 +startangle_c: 7 +endpose_c: -6 1 8 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 2.7489 +-0.0320 0.0105 2.7865 +-0.0644 0.0197 2.8242 +-0.0973 0.0278 2.8618 +-0.1304 0.0346 2.8994 +-0.1639 0.0402 2.9371 +-0.1977 0.0446 2.9747 +-0.2316 0.0476 3.0124 +-0.2658 0.0495 3.0500 +-0.3000 0.0500 3.0876 +primID: 6 +startangle_c: 7 +endpose_c: 0 0 6 +additionalactioncostmult: 1000 +intermediateposes: 10 +0.0000 0.0000 2.7489 +0.0000 0.0000 2.7053 +0.0000 0.0000 2.6616 +0.0000 0.0000 2.6180 +0.0000 0.0000 2.5744 +0.0000 0.0000 2.5307 +0.0000 0.0000 2.4871 +0.0000 0.0000 2.4435 +0.0000 0.0000 2.3998 +0.0000 0.0000 2.3562 +primID: 7 +startangle_c: 7 +endpose_c: 0 0 8 +additionalactioncostmult: 1000 +intermediateposes: 10 +0.0000 0.0000 2.7489 +0.0000 0.0000 2.7925 +0.0000 0.0000 2.8362 +0.0000 0.0000 2.8798 +0.0000 0.0000 2.9234 +0.0000 0.0000 2.9671 +0.0000 0.0000 3.0107 +0.0000 0.0000 3.0543 +0.0000 0.0000 3.0980 +0.0000 0.0000 3.1416 +primID: 8 +startangle_c: 7 +endpose_c: -7 2 7 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 2.7489 +-0.0389 0.0111 2.7489 +-0.0778 0.0222 2.7489 +-0.1167 0.0333 2.7489 +-0.1556 0.0444 2.7489 +-0.1944 0.0556 2.7489 +-0.2333 0.0667 2.7489 +-0.2722 0.0778 2.7489 +-0.3111 0.0889 2.7489 +-0.3500 0.1000 2.7489 +primID: 9 +startangle_c: 7 +endpose_c: -5 4 7 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 2.7489 +-0.0278 0.0222 2.7489 +-0.0556 0.0444 2.7489 +-0.0833 0.0667 2.7489 +-0.1111 0.0889 2.7489 +-0.1389 0.1111 2.7489 +-0.1667 0.1333 2.7489 +-0.1944 0.1556 2.7489 +-0.2222 0.1778 2.7489 +-0.2500 0.2000 2.7489 +primID: 0 +startangle_c: 8 +endpose_c: -1 0 8 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 3.1416 +-0.0056 0.0000 3.1416 +-0.0111 0.0000 3.1416 +-0.0167 0.0000 3.1416 +-0.0222 0.0000 3.1416 +-0.0278 0.0000 3.1416 +-0.0333 0.0000 3.1416 +-0.0389 0.0000 3.1416 +-0.0444 0.0000 3.1416 +-0.0500 0.0000 3.1416 +primID: 1 +startangle_c: 8 +endpose_c: -8 0 8 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 3.1416 +-0.0444 0.0000 3.1416 +-0.0889 0.0000 3.1416 +-0.1333 0.0000 3.1416 +-0.1778 0.0000 3.1416 +-0.2222 0.0000 3.1416 +-0.2667 0.0000 3.1416 +-0.3111 0.0000 3.1416 +-0.3556 0.0000 3.1416 +-0.4000 0.0000 3.1416 +primID: 2 +startangle_c: 8 +endpose_c: -16 -4 9 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 3.1416 +-0.0904 -0.0095 3.1765 +-0.1806 -0.0222 3.2115 +-0.2707 -0.0381 3.2464 +-0.3604 -0.0572 3.2814 +-0.4496 -0.0795 3.3163 +-0.5383 -0.1050 3.3513 +-0.6264 -0.1335 3.3862 +-0.7136 -0.1652 3.4211 +-0.8000 -0.2000 3.4561 +primID: 3 +startangle_c: 8 +endpose_c: -16 4 7 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 3.1416 +-0.0904 0.0095 3.1066 +-0.1806 0.0222 3.0717 +-0.2707 0.0381 3.0368 +-0.3604 0.0572 3.0018 +-0.4496 0.0795 2.9669 +-0.5383 0.1050 2.9319 +-0.6264 0.1335 2.8970 +-0.7136 0.1652 2.8620 +-0.8000 0.2000 2.8271 +primID: 4 +startangle_c: 8 +endpose_c: -5 -2 9 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 3.1416 +-0.0280 -0.0085 3.1639 +-0.0559 -0.0177 3.1861 +-0.0839 -0.0275 3.2084 +-0.1117 -0.0380 3.2306 +-0.1396 -0.0491 3.2529 +-0.1673 -0.0609 3.2751 +-0.1950 -0.0733 3.2974 +-0.2225 -0.0863 3.3197 +-0.2500 -0.1000 3.3419 +primID: 5 +startangle_c: 8 +endpose_c: -5 2 7 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 3.1416 +-0.0280 0.0085 3.1193 +-0.0559 0.0177 3.0971 +-0.0839 0.0275 3.0748 +-0.1117 0.0380 3.0526 +-0.1396 0.0491 3.0303 +-0.1673 0.0609 3.0080 +-0.1950 0.0733 2.9858 +-0.2225 0.0863 2.9635 +-0.2500 0.1000 2.9413 +primID: 6 +startangle_c: 8 +endpose_c: 0 0 9 +additionalactioncostmult: 1000 +intermediateposes: 10 +0.0000 0.0000 3.1416 +0.0000 0.0000 3.1852 +0.0000 0.0000 3.2289 +0.0000 0.0000 3.2725 +0.0000 0.0000 3.3161 +0.0000 0.0000 3.3598 +0.0000 0.0000 3.4034 +0.0000 0.0000 3.4470 +0.0000 0.0000 3.4907 +0.0000 0.0000 3.5343 +primID: 7 +startangle_c: 8 +endpose_c: 0 0 7 +additionalactioncostmult: 1000 +intermediateposes: 10 +0.0000 0.0000 3.1416 +0.0000 0.0000 3.0980 +0.0000 0.0000 3.0543 +0.0000 0.0000 3.0107 +0.0000 0.0000 2.9671 +0.0000 0.0000 2.9234 +0.0000 0.0000 2.8798 +0.0000 0.0000 2.8362 +0.0000 0.0000 2.7925 +0.0000 0.0000 2.7489 +primID: 8 +startangle_c: 8 +endpose_c: -8 -1 8 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 3.1416 +-0.0444 -0.0056 3.1416 +-0.0889 -0.0111 3.1416 +-0.1333 -0.0167 3.1416 +-0.1778 -0.0222 3.1416 +-0.2222 -0.0278 3.1416 +-0.2667 -0.0333 3.1416 +-0.3111 -0.0389 3.1416 +-0.3556 -0.0444 3.1416 +-0.4000 -0.0500 3.1416 +primID: 9 +startangle_c: 8 +endpose_c: -8 1 8 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 3.1416 +-0.0444 0.0056 3.1416 +-0.0889 0.0111 3.1416 +-0.1333 0.0167 3.1416 +-0.1778 0.0222 3.1416 +-0.2222 0.0278 3.1416 +-0.2667 0.0333 3.1416 +-0.3111 0.0389 3.1416 +-0.3556 0.0444 3.1416 +-0.4000 0.0500 3.1416 +primID: 0 +startangle_c: 9 +endpose_c: -2 -1 9 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 3.5343 +-0.0111 -0.0056 3.5343 +-0.0222 -0.0111 3.5343 +-0.0333 -0.0167 3.5343 +-0.0444 -0.0222 3.5343 +-0.0556 -0.0278 3.5343 +-0.0667 -0.0333 3.5343 +-0.0778 -0.0389 3.5343 +-0.0889 -0.0444 3.5343 +-0.1000 -0.0500 3.5343 +primID: 1 +startangle_c: 9 +endpose_c: -6 -3 9 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 3.5343 +-0.0333 -0.0167 3.5343 +-0.0667 -0.0333 3.5343 +-0.1000 -0.0500 3.5343 +-0.1333 -0.0667 3.5343 +-0.1667 -0.0833 3.5343 +-0.2000 -0.1000 3.5343 +-0.2333 -0.1167 3.5343 +-0.2667 -0.1333 3.5343 +-0.3000 -0.1500 3.5343 +primID: 2 +startangle_c: 9 +endpose_c: -13 -9 10 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 3.5343 +-0.0793 -0.0378 3.5745 +-0.1572 -0.0787 3.6148 +-0.2334 -0.1229 3.6550 +-0.3079 -0.1701 3.6953 +-0.3805 -0.2204 3.7355 +-0.4512 -0.2736 3.7758 +-0.5197 -0.3296 3.8160 +-0.5860 -0.3885 3.8563 +-0.6500 -0.4500 3.8965 +primID: 3 +startangle_c: 9 +endpose_c: -14 -3 8 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 3.5343 +-0.0741 -0.0305 3.5190 +-0.1493 -0.0582 3.4718 +-0.2256 -0.0824 3.4246 +-0.3031 -0.1030 3.3775 +-0.3814 -0.1199 3.3303 +-0.4604 -0.1330 3.2831 +-0.5400 -0.1424 3.2359 +-0.6199 -0.1481 3.1888 +-0.7000 -0.1500 3.1416 +primID: 4 +startangle_c: 9 +endpose_c: -5 -4 10 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 3.5343 +-0.0298 -0.0184 3.5646 +-0.0592 -0.0379 3.5949 +-0.0881 -0.0583 3.6252 +-0.1165 -0.0796 3.6555 +-0.1444 -0.1019 3.6858 +-0.1717 -0.1251 3.7161 +-0.1984 -0.1492 3.7464 +-0.2245 -0.1742 3.7767 +-0.2500 -0.2000 3.8070 +primID: 5 +startangle_c: 9 +endpose_c: -6 -1 8 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 3.5343 +-0.0320 -0.0105 3.4967 +-0.0644 -0.0197 3.4590 +-0.0973 -0.0278 3.4214 +-0.1304 -0.0346 3.3837 +-0.1639 -0.0402 3.3461 +-0.1977 -0.0446 3.3085 +-0.2316 -0.0476 3.2708 +-0.2658 -0.0495 3.2332 +-0.3000 -0.0500 3.1955 +primID: 6 +startangle_c: 9 +endpose_c: 0 0 10 +additionalactioncostmult: 1000 +intermediateposes: 10 +0.0000 0.0000 3.5343 +0.0000 0.0000 3.5779 +0.0000 0.0000 3.6216 +0.0000 0.0000 3.6652 +0.0000 0.0000 3.7088 +0.0000 0.0000 3.7525 +0.0000 0.0000 3.7961 +0.0000 0.0000 3.8397 +0.0000 0.0000 3.8834 +0.0000 0.0000 3.9270 +primID: 7 +startangle_c: 9 +endpose_c: 0 0 8 +additionalactioncostmult: 1000 +intermediateposes: 10 +0.0000 0.0000 3.5343 +0.0000 0.0000 3.4907 +0.0000 0.0000 3.4470 +0.0000 0.0000 3.4034 +0.0000 0.0000 3.3598 +0.0000 0.0000 3.3161 +0.0000 0.0000 3.2725 +0.0000 0.0000 3.2289 +0.0000 0.0000 3.1852 +0.0000 0.0000 3.1416 +primID: 8 +startangle_c: 9 +endpose_c: -7 -2 9 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 3.5343 +-0.0389 -0.0111 3.5343 +-0.0778 -0.0222 3.5343 +-0.1167 -0.0333 3.5343 +-0.1556 -0.0444 3.5343 +-0.1944 -0.0556 3.5343 +-0.2333 -0.0667 3.5343 +-0.2722 -0.0778 3.5343 +-0.3111 -0.0889 3.5343 +-0.3500 -0.1000 3.5343 +primID: 9 +startangle_c: 9 +endpose_c: -5 -4 9 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 3.5343 +-0.0278 -0.0222 3.5343 +-0.0556 -0.0444 3.5343 +-0.0833 -0.0667 3.5343 +-0.1111 -0.0889 3.5343 +-0.1389 -0.1111 3.5343 +-0.1667 -0.1333 3.5343 +-0.1944 -0.1556 3.5343 +-0.2222 -0.1778 3.5343 +-0.2500 -0.2000 3.5343 +primID: 0 +startangle_c: 10 +endpose_c: -1 -1 10 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 3.9270 +-0.0056 -0.0056 3.9270 +-0.0111 -0.0111 3.9270 +-0.0167 -0.0167 3.9270 +-0.0222 -0.0222 3.9270 +-0.0278 -0.0278 3.9270 +-0.0333 -0.0333 3.9270 +-0.0389 -0.0389 3.9270 +-0.0444 -0.0444 3.9270 +-0.0500 -0.0500 3.9270 +primID: 1 +startangle_c: 10 +endpose_c: -6 -6 10 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 3.9270 +-0.0333 -0.0333 3.9270 +-0.0667 -0.0667 3.9270 +-0.1000 -0.1000 3.9270 +-0.1333 -0.1333 3.9270 +-0.1667 -0.1667 3.9270 +-0.2000 -0.2000 3.9270 +-0.2333 -0.2333 3.9270 +-0.2667 -0.2667 3.9270 +-0.3000 -0.3000 3.9270 +primID: 2 +startangle_c: 10 +endpose_c: -11 -14 11 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 3.9270 +-0.0705 -0.0705 3.9270 +-0.1411 -0.1411 3.9270 +-0.2116 -0.2116 3.9270 +-0.2816 -0.2828 3.9617 +-0.3469 -0.3581 4.0333 +-0.4068 -0.4379 4.1049 +-0.4607 -0.5218 4.1765 +-0.5086 -0.6093 4.2481 +-0.5500 -0.7000 4.3197 +primID: 3 +startangle_c: 10 +endpose_c: -14 -11 9 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 3.9270 +-0.0705 -0.0705 3.9270 +-0.1411 -0.1411 3.9270 +-0.2116 -0.2116 3.9270 +-0.2828 -0.2816 3.8923 +-0.3581 -0.3469 3.8207 +-0.4379 -0.4068 3.7491 +-0.5218 -0.4607 3.6775 +-0.6093 -0.5086 3.6059 +-0.7000 -0.5500 3.5343 +primID: 4 +startangle_c: 10 +endpose_c: -4 -6 11 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 3.9270 +-0.0278 -0.0292 3.9704 +-0.0543 -0.0595 4.0138 +-0.0795 -0.0910 4.0572 +-0.1033 -0.1235 4.1006 +-0.1257 -0.1571 4.1440 +-0.1466 -0.1916 4.1874 +-0.1659 -0.2269 4.2308 +-0.1838 -0.2631 4.2742 +-0.2000 -0.3000 4.3176 +primID: 5 +startangle_c: 10 +endpose_c: -6 -4 9 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 3.9270 +-0.0292 -0.0278 3.8836 +-0.0595 -0.0543 3.8402 +-0.0910 -0.0795 3.7968 +-0.1235 -0.1033 3.7534 +-0.1571 -0.1257 3.7100 +-0.1916 -0.1466 3.6666 +-0.2269 -0.1659 3.6232 +-0.2631 -0.1838 3.5798 +-0.3000 -0.2000 3.5364 +primID: 6 +startangle_c: 10 +endpose_c: 0 0 11 +additionalactioncostmult: 1000 +intermediateposes: 10 +0.0000 0.0000 3.9270 +0.0000 0.0000 3.9706 +0.0000 0.0000 4.0143 +0.0000 0.0000 4.0579 +0.0000 0.0000 4.1015 +0.0000 0.0000 4.1452 +0.0000 0.0000 4.1888 +0.0000 0.0000 4.2324 +0.0000 0.0000 4.2761 +0.0000 0.0000 4.3197 +primID: 7 +startangle_c: 10 +endpose_c: 0 0 9 +additionalactioncostmult: 1000 +intermediateposes: 10 +0.0000 0.0000 3.9270 +0.0000 0.0000 3.8834 +0.0000 0.0000 3.8397 +0.0000 0.0000 3.7961 +0.0000 0.0000 3.7525 +0.0000 0.0000 3.7088 +0.0000 0.0000 3.6652 +0.0000 0.0000 3.6216 +0.0000 0.0000 3.5779 +0.0000 0.0000 3.5343 +primID: 8 +startangle_c: 10 +endpose_c: -6 -7 10 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 3.9270 +-0.0333 -0.0389 3.9270 +-0.0667 -0.0778 3.9270 +-0.1000 -0.1167 3.9270 +-0.1333 -0.1556 3.9270 +-0.1667 -0.1944 3.9270 +-0.2000 -0.2333 3.9270 +-0.2333 -0.2722 3.9270 +-0.2667 -0.3111 3.9270 +-0.3000 -0.3500 3.9270 +primID: 9 +startangle_c: 10 +endpose_c: -7 -6 10 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 3.9270 +-0.0389 -0.0333 3.9270 +-0.0778 -0.0667 3.9270 +-0.1167 -0.1000 3.9270 +-0.1556 -0.1333 3.9270 +-0.1944 -0.1667 3.9270 +-0.2333 -0.2000 3.9270 +-0.2722 -0.2333 3.9270 +-0.3111 -0.2667 3.9270 +-0.3500 -0.3000 3.9270 +primID: 0 +startangle_c: 11 +endpose_c: -1 -2 11 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 4.3197 +-0.0056 -0.0111 4.3197 +-0.0111 -0.0222 4.3197 +-0.0167 -0.0333 4.3197 +-0.0222 -0.0444 4.3197 +-0.0278 -0.0556 4.3197 +-0.0333 -0.0667 4.3197 +-0.0389 -0.0778 4.3197 +-0.0444 -0.0889 4.3197 +-0.0500 -0.1000 4.3197 +primID: 1 +startangle_c: 11 +endpose_c: -3 -6 11 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 4.3197 +-0.0167 -0.0333 4.3197 +-0.0333 -0.0667 4.3197 +-0.0500 -0.1000 4.3197 +-0.0667 -0.1333 4.3197 +-0.0833 -0.1667 4.3197 +-0.1000 -0.2000 4.3197 +-0.1167 -0.2333 4.3197 +-0.1333 -0.2667 4.3197 +-0.1500 -0.3000 4.3197 +primID: 2 +startangle_c: 11 +endpose_c: -9 -13 10 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 4.3197 +-0.0378 -0.0793 4.2794 +-0.0787 -0.1572 4.2392 +-0.1229 -0.2334 4.1989 +-0.1701 -0.3079 4.1587 +-0.2204 -0.3805 4.1185 +-0.2736 -0.4512 4.0782 +-0.3296 -0.5197 4.0380 +-0.3885 -0.5860 3.9977 +-0.4500 -0.6500 3.9575 +primID: 3 +startangle_c: 11 +endpose_c: -3 -14 12 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 4.3197 +-0.0305 -0.0741 4.3350 +-0.0582 -0.1493 4.3822 +-0.0824 -0.2256 4.4294 +-0.1030 -0.3031 4.4765 +-0.1199 -0.3814 4.5237 +-0.1330 -0.4604 4.5709 +-0.1424 -0.5400 4.6180 +-0.1481 -0.6199 4.6652 +-0.1500 -0.7000 4.7124 +primID: 4 +startangle_c: 11 +endpose_c: -4 -5 10 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 4.3197 +-0.0184 -0.0298 4.2894 +-0.0379 -0.0592 4.2591 +-0.0583 -0.0881 4.2288 +-0.0796 -0.1165 4.1985 +-0.1019 -0.1444 4.1682 +-0.1251 -0.1717 4.1379 +-0.1492 -0.1984 4.1076 +-0.1742 -0.2245 4.0773 +-0.2000 -0.2500 4.0470 +primID: 5 +startangle_c: 11 +endpose_c: -1 -6 12 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 4.3197 +-0.0105 -0.0320 4.3573 +-0.0197 -0.0644 4.3950 +-0.0278 -0.0973 4.4326 +-0.0346 -0.1304 4.4702 +-0.0402 -0.1639 4.5079 +-0.0446 -0.1977 4.5455 +-0.0476 -0.2316 4.5832 +-0.0495 -0.2658 4.6208 +-0.0500 -0.3000 4.6584 +primID: 6 +startangle_c: 11 +endpose_c: 0 0 10 +additionalactioncostmult: 1000 +intermediateposes: 10 +0.0000 0.0000 4.3197 +0.0000 0.0000 4.2761 +0.0000 0.0000 4.2324 +0.0000 0.0000 4.1888 +0.0000 0.0000 4.1452 +0.0000 0.0000 4.1015 +0.0000 0.0000 4.0579 +0.0000 0.0000 4.0143 +0.0000 0.0000 3.9706 +0.0000 0.0000 3.9270 +primID: 7 +startangle_c: 11 +endpose_c: 0 0 12 +additionalactioncostmult: 1000 +intermediateposes: 10 +0.0000 0.0000 4.3197 +0.0000 0.0000 4.3633 +0.0000 0.0000 4.4070 +0.0000 0.0000 4.4506 +0.0000 0.0000 4.4942 +0.0000 0.0000 4.5379 +0.0000 0.0000 4.5815 +0.0000 0.0000 4.6251 +0.0000 0.0000 4.6688 +0.0000 0.0000 4.7124 +primID: 8 +startangle_c: 11 +endpose_c: -2 -7 11 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 4.3197 +-0.0111 -0.0389 4.3197 +-0.0222 -0.0778 4.3197 +-0.0333 -0.1167 4.3197 +-0.0444 -0.1556 4.3197 +-0.0556 -0.1944 4.3197 +-0.0667 -0.2333 4.3197 +-0.0778 -0.2722 4.3197 +-0.0889 -0.3111 4.3197 +-0.1000 -0.3500 4.3197 +primID: 9 +startangle_c: 11 +endpose_c: -4 -5 11 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 4.3197 +-0.0222 -0.0278 4.3197 +-0.0444 -0.0556 4.3197 +-0.0667 -0.0833 4.3197 +-0.0889 -0.1111 4.3197 +-0.1111 -0.1389 4.3197 +-0.1333 -0.1667 4.3197 +-0.1556 -0.1944 4.3197 +-0.1778 -0.2222 4.3197 +-0.2000 -0.2500 4.3197 +primID: 0 +startangle_c: 12 +endpose_c: 0 -1 12 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 4.7124 +0.0000 -0.0056 4.7124 +0.0000 -0.0111 4.7124 +0.0000 -0.0167 4.7124 +0.0000 -0.0222 4.7124 +0.0000 -0.0278 4.7124 +0.0000 -0.0333 4.7124 +0.0000 -0.0389 4.7124 +0.0000 -0.0444 4.7124 +0.0000 -0.0500 4.7124 +primID: 1 +startangle_c: 12 +endpose_c: 0 -8 12 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 4.7124 +0.0000 -0.0444 4.7124 +0.0000 -0.0889 4.7124 +0.0000 -0.1333 4.7124 +0.0000 -0.1778 4.7124 +0.0000 -0.2222 4.7124 +0.0000 -0.2667 4.7124 +0.0000 -0.3111 4.7124 +0.0000 -0.3556 4.7124 +0.0000 -0.4000 4.7124 +primID: 2 +startangle_c: 12 +endpose_c: 4 -16 13 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 4.7124 +0.0095 -0.0904 4.7473 +0.0222 -0.1806 4.7823 +0.0381 -0.2707 4.8172 +0.0572 -0.3604 4.8522 +0.0795 -0.4496 4.8871 +0.1050 -0.5383 4.9221 +0.1335 -0.6264 4.9570 +0.1652 -0.7136 4.9919 +0.2000 -0.8000 5.0269 +primID: 3 +startangle_c: 12 +endpose_c: -4 -16 11 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 4.7124 +-0.0095 -0.0904 4.6774 +-0.0222 -0.1806 4.6425 +-0.0381 -0.2707 4.6076 +-0.0572 -0.3604 4.5726 +-0.0795 -0.4496 4.5377 +-0.1050 -0.5383 4.5027 +-0.1335 -0.6264 4.4678 +-0.1652 -0.7136 4.4328 +-0.2000 -0.8000 4.3979 +primID: 4 +startangle_c: 12 +endpose_c: 2 -5 13 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 4.7124 +0.0085 -0.0280 4.7346 +0.0177 -0.0559 4.7569 +0.0275 -0.0839 4.7792 +0.0380 -0.1117 4.8014 +0.0491 -0.1396 4.8237 +0.0609 -0.1673 4.8459 +0.0733 -0.1950 4.8682 +0.0863 -0.2225 4.8904 +0.1000 -0.2500 4.9127 +primID: 5 +startangle_c: 12 +endpose_c: -2 -5 11 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 4.7124 +-0.0085 -0.0280 4.6901 +-0.0177 -0.0559 4.6679 +-0.0275 -0.0839 4.6456 +-0.0380 -0.1117 4.6234 +-0.0491 -0.1396 4.6011 +-0.0609 -0.1673 4.5788 +-0.0733 -0.1950 4.5566 +-0.0863 -0.2225 4.5343 +-0.1000 -0.2500 4.5121 +primID: 6 +startangle_c: 12 +endpose_c: 0 0 13 +additionalactioncostmult: 1000 +intermediateposes: 10 +0.0000 0.0000 4.7124 +0.0000 0.0000 4.7560 +0.0000 0.0000 4.7997 +0.0000 0.0000 4.8433 +0.0000 0.0000 4.8869 +0.0000 0.0000 4.9306 +0.0000 0.0000 4.9742 +0.0000 0.0000 5.0178 +0.0000 0.0000 5.0615 +0.0000 0.0000 5.1051 +primID: 7 +startangle_c: 12 +endpose_c: 0 0 11 +additionalactioncostmult: 1000 +intermediateposes: 10 +0.0000 0.0000 4.7124 +0.0000 0.0000 4.6688 +0.0000 0.0000 4.6251 +0.0000 0.0000 4.5815 +0.0000 0.0000 4.5379 +0.0000 0.0000 4.4942 +0.0000 0.0000 4.4506 +0.0000 0.0000 4.4070 +0.0000 0.0000 4.3633 +0.0000 0.0000 4.3197 +primID: 8 +startangle_c: 12 +endpose_c: 1 -8 12 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 4.7124 +0.0056 -0.0444 4.7124 +0.0111 -0.0889 4.7124 +0.0167 -0.1333 4.7124 +0.0222 -0.1778 4.7124 +0.0278 -0.2222 4.7124 +0.0333 -0.2667 4.7124 +0.0389 -0.3111 4.7124 +0.0444 -0.3556 4.7124 +0.0500 -0.4000 4.7124 +primID: 9 +startangle_c: 12 +endpose_c: -1 -8 12 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 4.7124 +-0.0056 -0.0444 4.7124 +-0.0111 -0.0889 4.7124 +-0.0167 -0.1333 4.7124 +-0.0222 -0.1778 4.7124 +-0.0278 -0.2222 4.7124 +-0.0333 -0.2667 4.7124 +-0.0389 -0.3111 4.7124 +-0.0444 -0.3556 4.7124 +-0.0500 -0.4000 4.7124 +primID: 0 +startangle_c: 13 +endpose_c: 1 -2 13 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 5.1051 +0.0056 -0.0111 5.1051 +0.0111 -0.0222 5.1051 +0.0167 -0.0333 5.1051 +0.0222 -0.0444 5.1051 +0.0278 -0.0556 5.1051 +0.0333 -0.0667 5.1051 +0.0389 -0.0778 5.1051 +0.0444 -0.0889 5.1051 +0.0500 -0.1000 5.1051 +primID: 1 +startangle_c: 13 +endpose_c: 3 -6 13 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 5.1051 +0.0167 -0.0333 5.1051 +0.0333 -0.0667 5.1051 +0.0500 -0.1000 5.1051 +0.0667 -0.1333 5.1051 +0.0833 -0.1667 5.1051 +0.1000 -0.2000 5.1051 +0.1167 -0.2333 5.1051 +0.1333 -0.2667 5.1051 +0.1500 -0.3000 5.1051 +primID: 2 +startangle_c: 13 +endpose_c: 9 -13 14 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 5.1051 +0.0378 -0.0793 5.1453 +0.0787 -0.1572 5.1856 +0.1229 -0.2334 5.2258 +0.1701 -0.3079 5.2661 +0.2204 -0.3805 5.3063 +0.2736 -0.4512 5.3466 +0.3296 -0.5197 5.3868 +0.3885 -0.5860 5.4271 +0.4500 -0.6500 5.4673 +primID: 3 +startangle_c: 13 +endpose_c: 3 -14 12 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 5.1051 +0.0305 -0.0741 5.0898 +0.0582 -0.1493 5.0426 +0.0824 -0.2256 4.9954 +0.1030 -0.3031 4.9482 +0.1199 -0.3814 4.9011 +0.1330 -0.4604 4.8539 +0.1424 -0.5400 4.8067 +0.1481 -0.6199 4.7596 +0.1500 -0.7000 4.7124 +primID: 4 +startangle_c: 13 +endpose_c: 4 -5 14 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 5.1051 +0.0184 -0.0298 5.1354 +0.0379 -0.0592 5.1657 +0.0583 -0.0881 5.1960 +0.0796 -0.1165 5.2263 +0.1019 -0.1444 5.2566 +0.1251 -0.1717 5.2869 +0.1492 -0.1984 5.3172 +0.1742 -0.2245 5.3475 +0.2000 -0.2500 5.3778 +primID: 5 +startangle_c: 13 +endpose_c: 1 -6 12 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 5.1051 +0.0105 -0.0320 5.0674 +0.0197 -0.0644 5.0298 +0.0278 -0.0973 4.9922 +0.0346 -0.1304 4.9545 +0.0402 -0.1639 4.9169 +0.0446 -0.1977 4.8793 +0.0476 -0.2316 4.8416 +0.0495 -0.2658 4.8040 +0.0500 -0.3000 4.7663 +primID: 6 +startangle_c: 13 +endpose_c: 0 0 14 +additionalactioncostmult: 1000 +intermediateposes: 10 +0.0000 0.0000 5.1051 +0.0000 0.0000 5.1487 +0.0000 0.0000 5.1924 +0.0000 0.0000 5.2360 +0.0000 0.0000 5.2796 +0.0000 0.0000 5.3233 +0.0000 0.0000 5.3669 +0.0000 0.0000 5.4105 +0.0000 0.0000 5.4542 +0.0000 0.0000 5.4978 +primID: 7 +startangle_c: 13 +endpose_c: 0 0 12 +additionalactioncostmult: 1000 +intermediateposes: 10 +0.0000 0.0000 5.1051 +0.0000 0.0000 5.0615 +0.0000 0.0000 5.0178 +0.0000 0.0000 4.9742 +0.0000 0.0000 4.9306 +0.0000 0.0000 4.8869 +0.0000 0.0000 4.8433 +0.0000 0.0000 4.7997 +0.0000 0.0000 4.7560 +0.0000 0.0000 4.7124 +primID: 8 +startangle_c: 13 +endpose_c: 2 -7 13 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 5.1051 +0.0111 -0.0389 5.1051 +0.0222 -0.0778 5.1051 +0.0333 -0.1167 5.1051 +0.0444 -0.1556 5.1051 +0.0556 -0.1944 5.1051 +0.0667 -0.2333 5.1051 +0.0778 -0.2722 5.1051 +0.0889 -0.3111 5.1051 +0.1000 -0.3500 5.1051 +primID: 9 +startangle_c: 13 +endpose_c: 4 -5 13 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 5.1051 +0.0222 -0.0278 5.1051 +0.0444 -0.0556 5.1051 +0.0667 -0.0833 5.1051 +0.0889 -0.1111 5.1051 +0.1111 -0.1389 5.1051 +0.1333 -0.1667 5.1051 +0.1556 -0.1944 5.1051 +0.1778 -0.2222 5.1051 +0.2000 -0.2500 5.1051 +primID: 0 +startangle_c: 14 +endpose_c: 1 -1 14 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 5.4978 +0.0056 -0.0056 5.4978 +0.0111 -0.0111 5.4978 +0.0167 -0.0167 5.4978 +0.0222 -0.0222 5.4978 +0.0278 -0.0278 5.4978 +0.0333 -0.0333 5.4978 +0.0389 -0.0389 5.4978 +0.0444 -0.0444 5.4978 +0.0500 -0.0500 5.4978 +primID: 1 +startangle_c: 14 +endpose_c: 6 -6 14 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 5.4978 +0.0333 -0.0333 5.4978 +0.0667 -0.0667 5.4978 +0.1000 -0.1000 5.4978 +0.1333 -0.1333 5.4978 +0.1667 -0.1667 5.4978 +0.2000 -0.2000 5.4978 +0.2333 -0.2333 5.4978 +0.2667 -0.2667 5.4978 +0.3000 -0.3000 5.4978 +primID: 2 +startangle_c: 14 +endpose_c: 14 -11 15 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 5.4978 +0.0705 -0.0705 5.4978 +0.1411 -0.1411 5.4978 +0.2116 -0.2116 5.4978 +0.2828 -0.2816 5.5325 +0.3581 -0.3469 5.6041 +0.4379 -0.4068 5.6757 +0.5218 -0.4607 5.7473 +0.6093 -0.5086 5.8189 +0.7000 -0.5500 5.8905 +primID: 3 +startangle_c: 14 +endpose_c: 11 -14 13 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 5.4978 +0.0705 -0.0705 5.4978 +0.1411 -0.1411 5.4978 +0.2116 -0.2116 5.4978 +0.2816 -0.2828 5.4631 +0.3469 -0.3581 5.3915 +0.4068 -0.4379 5.3199 +0.4607 -0.5218 5.2483 +0.5086 -0.6093 5.1767 +0.5500 -0.7000 5.1051 +primID: 4 +startangle_c: 14 +endpose_c: 6 -4 15 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 5.4978 +0.0292 -0.0278 5.5412 +0.0595 -0.0543 5.5846 +0.0910 -0.0795 5.6280 +0.1235 -0.1033 5.6714 +0.1571 -0.1257 5.7148 +0.1916 -0.1466 5.7582 +0.2269 -0.1659 5.8016 +0.2631 -0.1838 5.8450 +0.3000 -0.2000 5.8884 +primID: 5 +startangle_c: 14 +endpose_c: 4 -6 13 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 5.4978 +0.0278 -0.0292 5.4544 +0.0543 -0.0595 5.4110 +0.0795 -0.0910 5.3676 +0.1033 -0.1235 5.3242 +0.1257 -0.1571 5.2808 +0.1466 -0.1916 5.2374 +0.1659 -0.2269 5.1940 +0.1838 -0.2631 5.1506 +0.2000 -0.3000 5.1072 +primID: 6 +startangle_c: 14 +endpose_c: 0 0 15 +additionalactioncostmult: 1000 +intermediateposes: 10 +0.0000 0.0000 5.4978 +0.0000 0.0000 5.5414 +0.0000 0.0000 5.5851 +0.0000 0.0000 5.6287 +0.0000 0.0000 5.6723 +0.0000 0.0000 5.7160 +0.0000 0.0000 5.7596 +0.0000 0.0000 5.8032 +0.0000 0.0000 5.8469 +0.0000 0.0000 5.8905 +primID: 7 +startangle_c: 14 +endpose_c: 0 0 13 +additionalactioncostmult: 1000 +intermediateposes: 10 +0.0000 0.0000 5.4978 +0.0000 0.0000 5.4542 +0.0000 0.0000 5.4105 +0.0000 0.0000 5.3669 +0.0000 0.0000 5.3233 +0.0000 0.0000 5.2796 +0.0000 0.0000 5.2360 +0.0000 0.0000 5.1924 +0.0000 0.0000 5.1487 +0.0000 0.0000 5.1051 +primID: 8 +startangle_c: 14 +endpose_c: 7 -6 14 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 5.4978 +0.0389 -0.0333 5.4978 +0.0778 -0.0667 5.4978 +0.1167 -0.1000 5.4978 +0.1556 -0.1333 5.4978 +0.1944 -0.1667 5.4978 +0.2333 -0.2000 5.4978 +0.2722 -0.2333 5.4978 +0.3111 -0.2667 5.4978 +0.3500 -0.3000 5.4978 +primID: 9 +startangle_c: 14 +endpose_c: 6 -7 14 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 5.4978 +0.0333 -0.0389 5.4978 +0.0667 -0.0778 5.4978 +0.1000 -0.1167 5.4978 +0.1333 -0.1556 5.4978 +0.1667 -0.1944 5.4978 +0.2000 -0.2333 5.4978 +0.2333 -0.2722 5.4978 +0.2667 -0.3111 5.4978 +0.3000 -0.3500 5.4978 +primID: 0 +startangle_c: 15 +endpose_c: 2 -1 15 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 5.8905 +0.0111 -0.0056 5.8905 +0.0222 -0.0111 5.8905 +0.0333 -0.0167 5.8905 +0.0444 -0.0222 5.8905 +0.0556 -0.0278 5.8905 +0.0667 -0.0333 5.8905 +0.0778 -0.0389 5.8905 +0.0889 -0.0444 5.8905 +0.1000 -0.0500 5.8905 +primID: 1 +startangle_c: 15 +endpose_c: 6 -3 15 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 5.8905 +0.0333 -0.0167 5.8905 +0.0667 -0.0333 5.8905 +0.1000 -0.0500 5.8905 +0.1333 -0.0667 5.8905 +0.1667 -0.0833 5.8905 +0.2000 -0.1000 5.8905 +0.2333 -0.1167 5.8905 +0.2667 -0.1333 5.8905 +0.3000 -0.1500 5.8905 +primID: 2 +startangle_c: 15 +endpose_c: 13 -9 14 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 5.8905 +0.0793 -0.0378 5.8502 +0.1572 -0.0787 5.8100 +0.2334 -0.1229 5.7697 +0.3079 -0.1701 5.7295 +0.3805 -0.2204 5.6892 +0.4512 -0.2736 5.6490 +0.5197 -0.3296 5.6088 +0.5860 -0.3885 5.5685 +0.6500 -0.4500 5.5283 +primID: 3 +startangle_c: 15 +endpose_c: 14 -3 0 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 5.8905 +0.0741 -0.0305 5.9058 +0.1493 -0.0582 5.9530 +0.2256 -0.0824 6.0002 +0.3031 -0.1030 6.0473 +0.3814 -0.1199 6.0945 +0.4604 -0.1330 6.1417 +0.5400 -0.1424 6.1888 +0.6199 -0.1481 6.2360 +0.7000 -0.1500 6.2832 +primID: 4 +startangle_c: 15 +endpose_c: 5 -4 14 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 5.8905 +0.0298 -0.0184 5.8602 +0.0592 -0.0379 5.8299 +0.0881 -0.0583 5.7996 +0.1165 -0.0796 5.7693 +0.1444 -0.1019 5.7390 +0.1717 -0.1251 5.7087 +0.1984 -0.1492 5.6784 +0.2245 -0.1742 5.6481 +0.2500 -0.2000 5.6178 +primID: 5 +startangle_c: 15 +endpose_c: 6 -1 0 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 5.8905 +0.0320 -0.0105 5.9281 +0.0644 -0.0197 5.9658 +0.0973 -0.0278 6.0034 +0.1304 -0.0346 6.0410 +0.1639 -0.0402 6.0787 +0.1977 -0.0446 6.1163 +0.2316 -0.0476 6.1540 +0.2658 -0.0495 6.1916 +0.3000 -0.0500 6.2292 +primID: 6 +startangle_c: 15 +endpose_c: 0 0 14 +additionalactioncostmult: 1000 +intermediateposes: 10 +0.0000 0.0000 5.8905 +0.0000 0.0000 5.8469 +0.0000 0.0000 5.8032 +0.0000 0.0000 5.7596 +0.0000 0.0000 5.7160 +0.0000 0.0000 5.6723 +0.0000 0.0000 5.6287 +0.0000 0.0000 5.5851 +0.0000 0.0000 5.5414 +0.0000 0.0000 5.4978 +primID: 7 +startangle_c: 15 +endpose_c: 0 0 0 +additionalactioncostmult: 1000 +intermediateposes: 10 +0.0000 0.0000 5.8905 +0.0000 0.0000 5.2360 +0.0000 0.0000 4.5815 +0.0000 0.0000 3.9270 +0.0000 0.0000 3.2725 +0.0000 0.0000 2.6180 +0.0000 0.0000 1.9635 +0.0000 0.0000 1.3090 +0.0000 0.0000 0.6545 +0.0000 0.0000 0.0000 +primID: 8 +startangle_c: 15 +endpose_c: 7 -2 15 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 5.8905 +0.0389 -0.0111 5.8905 +0.0778 -0.0222 5.8905 +0.1167 -0.0333 5.8905 +0.1556 -0.0444 5.8905 +0.1944 -0.0556 5.8905 +0.2333 -0.0667 5.8905 +0.2722 -0.0778 5.8905 +0.3111 -0.0889 5.8905 +0.3500 -0.1000 5.8905 +primID: 9 +startangle_c: 15 +endpose_c: 5 -4 15 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 5.8905 +0.0278 -0.0222 5.8905 +0.0556 -0.0444 5.8905 +0.0833 -0.0667 5.8905 +0.1111 -0.0889 5.8905 +0.1389 -0.1111 5.8905 +0.1667 -0.1333 5.8905 +0.1944 -0.1556 5.8905 +0.2222 -0.1778 5.8905 +0.2500 -0.2000 5.8905 diff --git a/Controllers/Packages/amr_startup/config/mprim/unicycle_highcost_10cm.mprim b/Controllers/Packages/amr_startup/config/mprim/unicycle_highcost_10cm.mprim new file mode 100755 index 0000000..de16976 --- /dev/null +++ b/Controllers/Packages/amr_startup/config/mprim/unicycle_highcost_10cm.mprim @@ -0,0 +1,1683 @@ +resolution_m: 0.100000 +numberofangles: 16 +totalnumberofprimitives: 112 +primID: 0 +startangle_c: 0 +endpose_c: 1 0 0 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 0.0000 +0.0111 0.0000 0.0000 +0.0222 0.0000 0.0000 +0.0333 0.0000 0.0000 +0.0444 0.0000 0.0000 +0.0556 0.0000 0.0000 +0.0667 0.0000 0.0000 +0.0778 0.0000 0.0000 +0.0889 0.0000 0.0000 +0.1000 0.0000 0.0000 +primID: 1 +startangle_c: 0 +endpose_c: 4 0 0 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 0.0000 +0.0444 0.0000 0.0000 +0.0889 0.0000 0.0000 +0.1333 0.0000 0.0000 +0.1778 0.0000 0.0000 +0.2222 0.0000 0.0000 +0.2667 0.0000 0.0000 +0.3111 0.0000 0.0000 +0.3556 0.0000 0.0000 +0.4000 0.0000 0.0000 +primID: 2 +startangle_c: 0 +endpose_c: -1 0 0 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 0.0000 +-0.0111 0.0000 0.0000 +-0.0222 0.0000 0.0000 +-0.0333 0.0000 0.0000 +-0.0444 0.0000 0.0000 +-0.0556 0.0000 0.0000 +-0.0667 0.0000 0.0000 +-0.0778 0.0000 0.0000 +-0.0889 0.0000 0.0000 +-0.1000 0.0000 0.0000 +primID: 3 +startangle_c: 0 +endpose_c: 4 1 1 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 0.0000 +0.0452 0.0048 0.0349 +0.0903 0.0111 0.0699 +0.1353 0.0191 0.1048 +0.1802 0.0286 0.1398 +0.2248 0.0398 0.1747 +0.2692 0.0525 0.2097 +0.3132 0.0668 0.2446 +0.3568 0.0826 0.2796 +0.4000 0.1000 0.3145 +primID: 4 +startangle_c: 0 +endpose_c: 4 -1 -1 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 0.0000 +0.0452 -0.0048 -0.0349 +0.0903 -0.0111 -0.0699 +0.1353 -0.0191 -0.1048 +0.1802 -0.0286 -0.1398 +0.2248 -0.0398 -0.1747 +0.2692 -0.0525 -0.2097 +0.3132 -0.0668 -0.2446 +0.3568 -0.0826 -0.2796 +0.4000 -0.1000 -0.3145 +primID: 5 +startangle_c: 0 +endpose_c: 0 0 1 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 0.0000 +0.0000 0.0000 0.0436 +0.0000 0.0000 0.0873 +0.0000 0.0000 0.1309 +0.0000 0.0000 0.1745 +0.0000 0.0000 0.2182 +0.0000 0.0000 0.2618 +0.0000 0.0000 0.3054 +0.0000 0.0000 0.3491 +0.0000 0.0000 0.3927 +primID: 6 +startangle_c: 0 +endpose_c: 0 0 -1 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 0.0000 +0.0000 0.0000 -0.0436 +0.0000 0.0000 -0.0873 +0.0000 0.0000 -0.1309 +0.0000 0.0000 -0.1745 +0.0000 0.0000 -0.2182 +0.0000 0.0000 -0.2618 +0.0000 0.0000 -0.3054 +0.0000 0.0000 -0.3491 +0.0000 0.0000 -0.3927 +primID: 0 +startangle_c: 1 +endpose_c: 2 1 1 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 0.3927 +0.0222 0.0111 0.3927 +0.0444 0.0222 0.3927 +0.0667 0.0333 0.3927 +0.0889 0.0444 0.3927 +0.1111 0.0556 0.3927 +0.1333 0.0667 0.3927 +0.1556 0.0778 0.3927 +0.1778 0.0889 0.3927 +0.2000 0.1000 0.3927 +primID: 1 +startangle_c: 1 +endpose_c: 6 3 1 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 0.3927 +0.0667 0.0333 0.3927 +0.1333 0.0667 0.3927 +0.2000 0.1000 0.3927 +0.2667 0.1333 0.3927 +0.3333 0.1667 0.3927 +0.4000 0.2000 0.3927 +0.4667 0.2333 0.3927 +0.5333 0.2667 0.3927 +0.6000 0.3000 0.3927 +primID: 2 +startangle_c: 1 +endpose_c: -2 -1 1 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 0.3927 +-0.0222 -0.0111 0.3927 +-0.0444 -0.0222 0.3927 +-0.0667 -0.0333 0.3927 +-0.0889 -0.0444 0.3927 +-0.1111 -0.0556 0.3927 +-0.1333 -0.0667 0.3927 +-0.1556 -0.0778 0.3927 +-0.1778 -0.0889 0.3927 +-0.2000 -0.1000 0.3927 +primID: 3 +startangle_c: 1 +endpose_c: 3 2 2 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 0.3927 +0.0369 0.0162 0.4345 +0.0731 0.0339 0.4783 +0.1085 0.0533 0.5222 +0.1430 0.0741 0.5661 +0.1766 0.0965 0.6099 +0.2091 0.1203 0.6538 +0.2405 0.1455 0.6977 +0.2709 0.1721 0.7415 +0.3000 0.2000 0.7854 +primID: 4 +startangle_c: 1 +endpose_c: 4 1 0 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 0.3927 +0.0427 0.0177 0.3927 +0.0854 0.0354 0.3927 +0.1283 0.0523 0.3477 +0.1722 0.0668 0.2898 +0.2168 0.0787 0.2318 +0.2621 0.0880 0.1739 +0.3078 0.0947 0.1159 +0.3538 0.0987 0.0580 +0.4000 0.1000 0.0000 +primID: 5 +startangle_c: 1 +endpose_c: 0 0 2 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 0.3927 +0.0000 0.0000 0.4363 +0.0000 0.0000 0.4800 +0.0000 0.0000 0.5236 +0.0000 0.0000 0.5672 +0.0000 0.0000 0.6109 +0.0000 0.0000 0.6545 +0.0000 0.0000 0.6981 +0.0000 0.0000 0.7418 +0.0000 0.0000 0.7854 +primID: 6 +startangle_c: 1 +endpose_c: 0 0 0 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 0.3927 +0.0000 0.0000 0.3491 +0.0000 0.0000 0.3054 +0.0000 0.0000 0.2618 +0.0000 0.0000 0.2182 +0.0000 0.0000 0.1745 +0.0000 0.0000 0.1309 +0.0000 0.0000 0.0873 +0.0000 0.0000 0.0436 +0.0000 0.0000 0.0000 +primID: 0 +startangle_c: 2 +endpose_c: 1 1 2 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 0.7854 +0.0111 0.0111 0.7854 +0.0222 0.0222 0.7854 +0.0333 0.0333 0.7854 +0.0444 0.0444 0.7854 +0.0556 0.0556 0.7854 +0.0667 0.0667 0.7854 +0.0778 0.0778 0.7854 +0.0889 0.0889 0.7854 +0.1000 0.1000 0.7854 +primID: 1 +startangle_c: 2 +endpose_c: 3 3 2 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 0.7854 +0.0333 0.0333 0.7854 +0.0667 0.0667 0.7854 +0.1000 0.1000 0.7854 +0.1333 0.1333 0.7854 +0.1667 0.1667 0.7854 +0.2000 0.2000 0.7854 +0.2333 0.2333 0.7854 +0.2667 0.2667 0.7854 +0.3000 0.3000 0.7854 +primID: 2 +startangle_c: 2 +endpose_c: -1 -1 2 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 0.7854 +-0.0111 -0.0111 0.7854 +-0.0222 -0.0222 0.7854 +-0.0333 -0.0333 0.7854 +-0.0444 -0.0444 0.7854 +-0.0556 -0.0556 0.7854 +-0.0667 -0.0667 0.7854 +-0.0778 -0.0778 0.7854 +-0.0889 -0.0889 0.7854 +-0.1000 -0.1000 0.7854 +primID: 3 +startangle_c: 2 +endpose_c: 2 4 3 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 0.7854 +0.0262 0.0411 0.8119 +0.0515 0.0831 0.8384 +0.0758 0.1260 0.8649 +0.0991 0.1697 0.8913 +0.1214 0.2142 0.9178 +0.1426 0.2595 0.9443 +0.1628 0.3056 0.9708 +0.1820 0.3525 0.9973 +0.2000 0.4000 1.0238 +primID: 4 +startangle_c: 2 +endpose_c: 4 2 1 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 0.7854 +0.0411 0.0262 0.7589 +0.0831 0.0515 0.7324 +0.1260 0.0758 0.7059 +0.1697 0.0991 0.6795 +0.2142 0.1214 0.6530 +0.2595 0.1426 0.6265 +0.3056 0.1628 0.6000 +0.3525 0.1820 0.5735 +0.4000 0.2000 0.5470 +primID: 5 +startangle_c: 2 +endpose_c: 0 0 3 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 0.7854 +0.0000 0.0000 0.8290 +0.0000 0.0000 0.8727 +0.0000 0.0000 0.9163 +0.0000 0.0000 0.9599 +0.0000 0.0000 1.0036 +0.0000 0.0000 1.0472 +0.0000 0.0000 1.0908 +0.0000 0.0000 1.1345 +0.0000 0.0000 1.1781 +primID: 6 +startangle_c: 2 +endpose_c: 0 0 1 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 0.7854 +0.0000 0.0000 0.7418 +0.0000 0.0000 0.6981 +0.0000 0.0000 0.6545 +0.0000 0.0000 0.6109 +0.0000 0.0000 0.5672 +0.0000 0.0000 0.5236 +0.0000 0.0000 0.4800 +0.0000 0.0000 0.4363 +0.0000 0.0000 0.3927 +primID: 0 +startangle_c: 3 +endpose_c: 1 2 3 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 1.1781 +0.0111 0.0222 1.1781 +0.0222 0.0444 1.1781 +0.0333 0.0667 1.1781 +0.0444 0.0889 1.1781 +0.0556 0.1111 1.1781 +0.0667 0.1333 1.1781 +0.0778 0.1556 1.1781 +0.0889 0.1778 1.1781 +0.1000 0.2000 1.1781 +primID: 1 +startangle_c: 3 +endpose_c: 3 6 3 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 1.1781 +0.0333 0.0667 1.1781 +0.0667 0.1333 1.1781 +0.1000 0.2000 1.1781 +0.1333 0.2667 1.1781 +0.1667 0.3333 1.1781 +0.2000 0.4000 1.1781 +0.2333 0.4667 1.1781 +0.2667 0.5333 1.1781 +0.3000 0.6000 1.1781 +primID: 2 +startangle_c: 3 +endpose_c: -1 -2 3 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 1.1781 +-0.0111 -0.0222 1.1781 +-0.0222 -0.0444 1.1781 +-0.0333 -0.0667 1.1781 +-0.0444 -0.0889 1.1781 +-0.0556 -0.1111 1.1781 +-0.0667 -0.1333 1.1781 +-0.0778 -0.1556 1.1781 +-0.0889 -0.1778 1.1781 +-0.1000 -0.2000 1.1781 +primID: 3 +startangle_c: 3 +endpose_c: 2 3 2 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 1.1781 +0.0162 0.0369 1.1363 +0.0339 0.0731 1.0925 +0.0533 0.1085 1.0486 +0.0741 0.1430 1.0047 +0.0965 0.1766 0.9609 +0.1203 0.2091 0.9170 +0.1455 0.2405 0.8731 +0.1721 0.2709 0.8293 +0.2000 0.3000 0.7854 +primID: 4 +startangle_c: 3 +endpose_c: 1 4 4 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 1.1781 +0.0177 0.0427 1.1781 +0.0354 0.0854 1.1781 +0.0523 0.1283 1.2231 +0.0668 0.1722 1.2810 +0.0787 0.2168 1.3390 +0.0880 0.2621 1.3969 +0.0947 0.3078 1.4549 +0.0987 0.3538 1.5128 +0.1000 0.4000 1.5708 +primID: 5 +startangle_c: 3 +endpose_c: 0 0 2 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 1.1781 +0.0000 0.0000 1.1345 +0.0000 0.0000 1.0908 +0.0000 0.0000 1.0472 +0.0000 0.0000 1.0036 +0.0000 0.0000 0.9599 +0.0000 0.0000 0.9163 +0.0000 0.0000 0.8727 +0.0000 0.0000 0.8290 +0.0000 0.0000 0.7854 +primID: 6 +startangle_c: 3 +endpose_c: 0 0 4 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 1.1781 +0.0000 0.0000 1.2217 +0.0000 0.0000 1.2654 +0.0000 0.0000 1.3090 +0.0000 0.0000 1.3526 +0.0000 0.0000 1.3963 +0.0000 0.0000 1.4399 +0.0000 0.0000 1.4835 +0.0000 0.0000 1.5272 +0.0000 0.0000 1.5708 +primID: 0 +startangle_c: 4 +endpose_c: 0 1 4 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 1.5708 +0.0000 0.0111 1.5708 +0.0000 0.0222 1.5708 +0.0000 0.0333 1.5708 +0.0000 0.0444 1.5708 +0.0000 0.0556 1.5708 +0.0000 0.0667 1.5708 +0.0000 0.0778 1.5708 +0.0000 0.0889 1.5708 +0.0000 0.1000 1.5708 +primID: 1 +startangle_c: 4 +endpose_c: 0 4 4 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 1.5708 +0.0000 0.0444 1.5708 +0.0000 0.0889 1.5708 +0.0000 0.1333 1.5708 +0.0000 0.1778 1.5708 +0.0000 0.2222 1.5708 +0.0000 0.2667 1.5708 +0.0000 0.3111 1.5708 +0.0000 0.3556 1.5708 +0.0000 0.4000 1.5708 +primID: 2 +startangle_c: 4 +endpose_c: 0 -1 4 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 1.5708 +0.0000 -0.0111 1.5708 +0.0000 -0.0222 1.5708 +0.0000 -0.0333 1.5708 +0.0000 -0.0444 1.5708 +0.0000 -0.0556 1.5708 +0.0000 -0.0667 1.5708 +0.0000 -0.0778 1.5708 +0.0000 -0.0889 1.5708 +0.0000 -0.1000 1.5708 +primID: 3 +startangle_c: 4 +endpose_c: -1 4 5 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 1.5708 +-0.0048 0.0452 1.6057 +-0.0111 0.0903 1.6407 +-0.0191 0.1353 1.6756 +-0.0286 0.1802 1.7106 +-0.0398 0.2248 1.7455 +-0.0525 0.2692 1.7805 +-0.0668 0.3132 1.8154 +-0.0826 0.3568 1.8503 +-0.1000 0.4000 1.8853 +primID: 4 +startangle_c: 4 +endpose_c: 1 4 3 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 1.5708 +0.0048 0.0452 1.5359 +0.0111 0.0903 1.5009 +0.0191 0.1353 1.4660 +0.0286 0.1802 1.4310 +0.0398 0.2248 1.3961 +0.0525 0.2692 1.3611 +0.0668 0.3132 1.3262 +0.0826 0.3568 1.2912 +0.1000 0.4000 1.2563 +primID: 5 +startangle_c: 4 +endpose_c: 0 0 5 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 1.5708 +0.0000 0.0000 1.6144 +0.0000 0.0000 1.6581 +0.0000 0.0000 1.7017 +0.0000 0.0000 1.7453 +0.0000 0.0000 1.7890 +0.0000 0.0000 1.8326 +0.0000 0.0000 1.8762 +0.0000 0.0000 1.9199 +0.0000 0.0000 1.9635 +primID: 6 +startangle_c: 4 +endpose_c: 0 0 3 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 1.5708 +0.0000 0.0000 1.5272 +0.0000 0.0000 1.4835 +0.0000 0.0000 1.4399 +0.0000 0.0000 1.3963 +0.0000 0.0000 1.3526 +0.0000 0.0000 1.3090 +0.0000 0.0000 1.2654 +0.0000 0.0000 1.2217 +0.0000 0.0000 1.1781 +primID: 0 +startangle_c: 5 +endpose_c: -1 2 5 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 1.9635 +-0.0111 0.0222 1.9635 +-0.0222 0.0444 1.9635 +-0.0333 0.0667 1.9635 +-0.0444 0.0889 1.9635 +-0.0556 0.1111 1.9635 +-0.0667 0.1333 1.9635 +-0.0778 0.1556 1.9635 +-0.0889 0.1778 1.9635 +-0.1000 0.2000 1.9635 +primID: 1 +startangle_c: 5 +endpose_c: -3 6 5 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 1.9635 +-0.0333 0.0667 1.9635 +-0.0667 0.1333 1.9635 +-0.1000 0.2000 1.9635 +-0.1333 0.2667 1.9635 +-0.1667 0.3333 1.9635 +-0.2000 0.4000 1.9635 +-0.2333 0.4667 1.9635 +-0.2667 0.5333 1.9635 +-0.3000 0.6000 1.9635 +primID: 2 +startangle_c: 5 +endpose_c: 1 -2 5 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 1.9635 +0.0111 -0.0222 1.9635 +0.0222 -0.0444 1.9635 +0.0333 -0.0667 1.9635 +0.0444 -0.0889 1.9635 +0.0556 -0.1111 1.9635 +0.0667 -0.1333 1.9635 +0.0778 -0.1556 1.9635 +0.0889 -0.1778 1.9635 +0.1000 -0.2000 1.9635 +primID: 3 +startangle_c: 5 +endpose_c: -2 3 6 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 1.9635 +-0.0162 0.0369 2.0053 +-0.0339 0.0731 2.0491 +-0.0533 0.1085 2.0930 +-0.0741 0.1430 2.1369 +-0.0965 0.1766 2.1807 +-0.1203 0.2091 2.2246 +-0.1455 0.2405 2.2685 +-0.1721 0.2709 2.3123 +-0.2000 0.3000 2.3562 +primID: 4 +startangle_c: 5 +endpose_c: -1 4 4 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 1.9635 +-0.0177 0.0427 1.9635 +-0.0354 0.0854 1.9635 +-0.0523 0.1283 1.9185 +-0.0668 0.1722 1.8606 +-0.0787 0.2168 1.8026 +-0.0880 0.2621 1.7447 +-0.0947 0.3078 1.6867 +-0.0987 0.3538 1.6287 +-0.1000 0.4000 1.5708 +primID: 5 +startangle_c: 5 +endpose_c: 0 0 6 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 1.9635 +0.0000 0.0000 2.0071 +0.0000 0.0000 2.0508 +0.0000 0.0000 2.0944 +0.0000 0.0000 2.1380 +0.0000 0.0000 2.1817 +0.0000 0.0000 2.2253 +0.0000 0.0000 2.2689 +0.0000 0.0000 2.3126 +0.0000 0.0000 2.3562 +primID: 6 +startangle_c: 5 +endpose_c: 0 0 4 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 1.9635 +0.0000 0.0000 1.9199 +0.0000 0.0000 1.8762 +0.0000 0.0000 1.8326 +0.0000 0.0000 1.7890 +0.0000 0.0000 1.7453 +0.0000 0.0000 1.7017 +0.0000 0.0000 1.6581 +0.0000 0.0000 1.6144 +0.0000 0.0000 1.5708 +primID: 0 +startangle_c: 6 +endpose_c: -1 1 6 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 2.3562 +-0.0111 0.0111 2.3562 +-0.0222 0.0222 2.3562 +-0.0333 0.0333 2.3562 +-0.0444 0.0444 2.3562 +-0.0556 0.0556 2.3562 +-0.0667 0.0667 2.3562 +-0.0778 0.0778 2.3562 +-0.0889 0.0889 2.3562 +-0.1000 0.1000 2.3562 +primID: 1 +startangle_c: 6 +endpose_c: -3 3 6 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 2.3562 +-0.0333 0.0333 2.3562 +-0.0667 0.0667 2.3562 +-0.1000 0.1000 2.3562 +-0.1333 0.1333 2.3562 +-0.1667 0.1667 2.3562 +-0.2000 0.2000 2.3562 +-0.2333 0.2333 2.3562 +-0.2667 0.2667 2.3562 +-0.3000 0.3000 2.3562 +primID: 2 +startangle_c: 6 +endpose_c: 1 -1 6 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 2.3562 +0.0111 -0.0111 2.3562 +0.0222 -0.0222 2.3562 +0.0333 -0.0333 2.3562 +0.0444 -0.0444 2.3562 +0.0556 -0.0556 2.3562 +0.0667 -0.0667 2.3562 +0.0778 -0.0778 2.3562 +0.0889 -0.0889 2.3562 +0.1000 -0.1000 2.3562 +primID: 3 +startangle_c: 6 +endpose_c: -4 2 7 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 2.3562 +-0.0411 0.0262 2.3827 +-0.0831 0.0515 2.4092 +-0.1260 0.0758 2.4357 +-0.1697 0.0991 2.4621 +-0.2142 0.1214 2.4886 +-0.2595 0.1426 2.5151 +-0.3056 0.1628 2.5416 +-0.3525 0.1820 2.5681 +-0.4000 0.2000 2.5946 +primID: 4 +startangle_c: 6 +endpose_c: -2 4 5 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 2.3562 +-0.0262 0.0411 2.3297 +-0.0515 0.0831 2.3032 +-0.0758 0.1260 2.2767 +-0.0991 0.1697 2.2502 +-0.1214 0.2142 2.2238 +-0.1426 0.2595 2.1973 +-0.1628 0.3056 2.1708 +-0.1820 0.3525 2.1443 +-0.2000 0.4000 2.1178 +primID: 5 +startangle_c: 6 +endpose_c: 0 0 7 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 2.3562 +0.0000 0.0000 2.3998 +0.0000 0.0000 2.4435 +0.0000 0.0000 2.4871 +0.0000 0.0000 2.5307 +0.0000 0.0000 2.5744 +0.0000 0.0000 2.6180 +0.0000 0.0000 2.6616 +0.0000 0.0000 2.7053 +0.0000 0.0000 2.7489 +primID: 6 +startangle_c: 6 +endpose_c: 0 0 5 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 2.3562 +0.0000 0.0000 2.3126 +0.0000 0.0000 2.2689 +0.0000 0.0000 2.2253 +0.0000 0.0000 2.1817 +0.0000 0.0000 2.1380 +0.0000 0.0000 2.0944 +0.0000 0.0000 2.0508 +0.0000 0.0000 2.0071 +0.0000 0.0000 1.9635 +primID: 0 +startangle_c: 7 +endpose_c: -2 1 7 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 2.7489 +-0.0222 0.0111 2.7489 +-0.0444 0.0222 2.7489 +-0.0667 0.0333 2.7489 +-0.0889 0.0444 2.7489 +-0.1111 0.0556 2.7489 +-0.1333 0.0667 2.7489 +-0.1556 0.0778 2.7489 +-0.1778 0.0889 2.7489 +-0.2000 0.1000 2.7489 +primID: 1 +startangle_c: 7 +endpose_c: -6 3 7 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 2.7489 +-0.0667 0.0333 2.7489 +-0.1333 0.0667 2.7489 +-0.2000 0.1000 2.7489 +-0.2667 0.1333 2.7489 +-0.3333 0.1667 2.7489 +-0.4000 0.2000 2.7489 +-0.4667 0.2333 2.7489 +-0.5333 0.2667 2.7489 +-0.6000 0.3000 2.7489 +primID: 2 +startangle_c: 7 +endpose_c: 2 -1 7 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 2.7489 +0.0222 -0.0111 2.7489 +0.0444 -0.0222 2.7489 +0.0667 -0.0333 2.7489 +0.0889 -0.0444 2.7489 +0.1111 -0.0556 2.7489 +0.1333 -0.0667 2.7489 +0.1556 -0.0778 2.7489 +0.1778 -0.0889 2.7489 +0.2000 -0.1000 2.7489 +primID: 3 +startangle_c: 7 +endpose_c: -3 2 6 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 2.7489 +-0.0369 0.0162 2.7071 +-0.0731 0.0339 2.6633 +-0.1085 0.0533 2.6194 +-0.1430 0.0741 2.5755 +-0.1766 0.0965 2.5317 +-0.2091 0.1203 2.4878 +-0.2405 0.1455 2.4439 +-0.2709 0.1721 2.4001 +-0.3000 0.2000 2.3562 +primID: 4 +startangle_c: 7 +endpose_c: -4 1 8 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 2.7489 +-0.0427 0.0177 2.7489 +-0.0854 0.0354 2.7489 +-0.1283 0.0523 2.7939 +-0.1722 0.0668 2.8518 +-0.2168 0.0787 2.9098 +-0.2621 0.0880 2.9677 +-0.3078 0.0947 3.0257 +-0.3538 0.0987 3.0836 +-0.4000 0.1000 3.1416 +primID: 5 +startangle_c: 7 +endpose_c: 0 0 6 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 2.7489 +0.0000 0.0000 2.7053 +0.0000 0.0000 2.6616 +0.0000 0.0000 2.6180 +0.0000 0.0000 2.5744 +0.0000 0.0000 2.5307 +0.0000 0.0000 2.4871 +0.0000 0.0000 2.4435 +0.0000 0.0000 2.3998 +0.0000 0.0000 2.3562 +primID: 6 +startangle_c: 7 +endpose_c: 0 0 8 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 2.7489 +0.0000 0.0000 2.7925 +0.0000 0.0000 2.8362 +0.0000 0.0000 2.8798 +0.0000 0.0000 2.9234 +0.0000 0.0000 2.9671 +0.0000 0.0000 3.0107 +0.0000 0.0000 3.0543 +0.0000 0.0000 3.0980 +0.0000 0.0000 3.1416 +primID: 0 +startangle_c: 8 +endpose_c: -1 0 8 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 3.1416 +-0.0111 0.0000 3.1416 +-0.0222 0.0000 3.1416 +-0.0333 0.0000 3.1416 +-0.0444 0.0000 3.1416 +-0.0556 0.0000 3.1416 +-0.0667 0.0000 3.1416 +-0.0778 0.0000 3.1416 +-0.0889 0.0000 3.1416 +-0.1000 0.0000 3.1416 +primID: 1 +startangle_c: 8 +endpose_c: -4 0 8 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 3.1416 +-0.0444 0.0000 3.1416 +-0.0889 0.0000 3.1416 +-0.1333 0.0000 3.1416 +-0.1778 0.0000 3.1416 +-0.2222 0.0000 3.1416 +-0.2667 0.0000 3.1416 +-0.3111 0.0000 3.1416 +-0.3556 0.0000 3.1416 +-0.4000 0.0000 3.1416 +primID: 2 +startangle_c: 8 +endpose_c: 1 0 8 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 3.1416 +0.0111 0.0000 3.1416 +0.0222 0.0000 3.1416 +0.0333 0.0000 3.1416 +0.0444 0.0000 3.1416 +0.0556 0.0000 3.1416 +0.0667 0.0000 3.1416 +0.0778 0.0000 3.1416 +0.0889 0.0000 3.1416 +0.1000 0.0000 3.1416 +primID: 3 +startangle_c: 8 +endpose_c: -4 -1 9 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 3.1416 +-0.0452 -0.0048 3.1765 +-0.0903 -0.0111 3.2115 +-0.1353 -0.0191 3.2464 +-0.1802 -0.0286 3.2814 +-0.2248 -0.0398 3.3163 +-0.2692 -0.0525 3.3513 +-0.3132 -0.0668 3.3862 +-0.3568 -0.0826 3.4211 +-0.4000 -0.1000 3.4561 +primID: 4 +startangle_c: 8 +endpose_c: -4 1 7 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 3.1416 +-0.0452 0.0048 3.1066 +-0.0903 0.0111 3.0717 +-0.1353 0.0191 3.0368 +-0.1802 0.0286 3.0018 +-0.2248 0.0398 2.9669 +-0.2692 0.0525 2.9319 +-0.3132 0.0668 2.8970 +-0.3568 0.0826 2.8620 +-0.4000 0.1000 2.8271 +primID: 5 +startangle_c: 8 +endpose_c: 0 0 9 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 3.1416 +0.0000 0.0000 3.1852 +0.0000 0.0000 3.2289 +0.0000 0.0000 3.2725 +0.0000 0.0000 3.3161 +0.0000 0.0000 3.3598 +0.0000 0.0000 3.4034 +0.0000 0.0000 3.4470 +0.0000 0.0000 3.4907 +0.0000 0.0000 3.5343 +primID: 6 +startangle_c: 8 +endpose_c: 0 0 7 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 3.1416 +0.0000 0.0000 3.0980 +0.0000 0.0000 3.0543 +0.0000 0.0000 3.0107 +0.0000 0.0000 2.9671 +0.0000 0.0000 2.9234 +0.0000 0.0000 2.8798 +0.0000 0.0000 2.8362 +0.0000 0.0000 2.7925 +0.0000 0.0000 2.7489 +primID: 0 +startangle_c: 9 +endpose_c: -2 -1 9 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 3.5343 +-0.0222 -0.0111 3.5343 +-0.0444 -0.0222 3.5343 +-0.0667 -0.0333 3.5343 +-0.0889 -0.0444 3.5343 +-0.1111 -0.0556 3.5343 +-0.1333 -0.0667 3.5343 +-0.1556 -0.0778 3.5343 +-0.1778 -0.0889 3.5343 +-0.2000 -0.1000 3.5343 +primID: 1 +startangle_c: 9 +endpose_c: -6 -3 9 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 3.5343 +-0.0667 -0.0333 3.5343 +-0.1333 -0.0667 3.5343 +-0.2000 -0.1000 3.5343 +-0.2667 -0.1333 3.5343 +-0.3333 -0.1667 3.5343 +-0.4000 -0.2000 3.5343 +-0.4667 -0.2333 3.5343 +-0.5333 -0.2667 3.5343 +-0.6000 -0.3000 3.5343 +primID: 2 +startangle_c: 9 +endpose_c: 2 1 9 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 3.5343 +0.0222 0.0111 3.5343 +0.0444 0.0222 3.5343 +0.0667 0.0333 3.5343 +0.0889 0.0444 3.5343 +0.1111 0.0556 3.5343 +0.1333 0.0667 3.5343 +0.1556 0.0778 3.5343 +0.1778 0.0889 3.5343 +0.2000 0.1000 3.5343 +primID: 3 +startangle_c: 9 +endpose_c: -3 -2 10 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 3.5343 +-0.0369 -0.0162 3.5761 +-0.0731 -0.0339 3.6199 +-0.1085 -0.0533 3.6638 +-0.1430 -0.0741 3.7077 +-0.1766 -0.0965 3.7515 +-0.2091 -0.1203 3.7954 +-0.2405 -0.1455 3.8393 +-0.2709 -0.1721 3.8831 +-0.3000 -0.2000 3.9270 +primID: 4 +startangle_c: 9 +endpose_c: -4 -1 8 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 3.5343 +-0.0427 -0.0177 3.5343 +-0.0854 -0.0354 3.5343 +-0.1283 -0.0523 3.4893 +-0.1722 -0.0668 3.4313 +-0.2168 -0.0787 3.3734 +-0.2621 -0.0880 3.3154 +-0.3078 -0.0947 3.2575 +-0.3538 -0.0987 3.1995 +-0.4000 -0.1000 3.1416 +primID: 5 +startangle_c: 9 +endpose_c: 0 0 10 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 3.5343 +0.0000 0.0000 3.5779 +0.0000 0.0000 3.6216 +0.0000 0.0000 3.6652 +0.0000 0.0000 3.7088 +0.0000 0.0000 3.7525 +0.0000 0.0000 3.7961 +0.0000 0.0000 3.8397 +0.0000 0.0000 3.8834 +0.0000 0.0000 3.9270 +primID: 6 +startangle_c: 9 +endpose_c: 0 0 8 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 3.5343 +0.0000 0.0000 3.4907 +0.0000 0.0000 3.4470 +0.0000 0.0000 3.4034 +0.0000 0.0000 3.3598 +0.0000 0.0000 3.3161 +0.0000 0.0000 3.2725 +0.0000 0.0000 3.2289 +0.0000 0.0000 3.1852 +0.0000 0.0000 3.1416 +primID: 0 +startangle_c: 10 +endpose_c: -1 -1 10 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 3.9270 +-0.0111 -0.0111 3.9270 +-0.0222 -0.0222 3.9270 +-0.0333 -0.0333 3.9270 +-0.0444 -0.0444 3.9270 +-0.0556 -0.0556 3.9270 +-0.0667 -0.0667 3.9270 +-0.0778 -0.0778 3.9270 +-0.0889 -0.0889 3.9270 +-0.1000 -0.1000 3.9270 +primID: 1 +startangle_c: 10 +endpose_c: -3 -3 10 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 3.9270 +-0.0333 -0.0333 3.9270 +-0.0667 -0.0667 3.9270 +-0.1000 -0.1000 3.9270 +-0.1333 -0.1333 3.9270 +-0.1667 -0.1667 3.9270 +-0.2000 -0.2000 3.9270 +-0.2333 -0.2333 3.9270 +-0.2667 -0.2667 3.9270 +-0.3000 -0.3000 3.9270 +primID: 2 +startangle_c: 10 +endpose_c: 1 1 10 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 3.9270 +0.0111 0.0111 3.9270 +0.0222 0.0222 3.9270 +0.0333 0.0333 3.9270 +0.0444 0.0444 3.9270 +0.0556 0.0556 3.9270 +0.0667 0.0667 3.9270 +0.0778 0.0778 3.9270 +0.0889 0.0889 3.9270 +0.1000 0.1000 3.9270 +primID: 3 +startangle_c: 10 +endpose_c: -2 -4 11 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 3.9270 +-0.0262 -0.0411 3.9535 +-0.0515 -0.0831 3.9800 +-0.0758 -0.1260 4.0064 +-0.0991 -0.1697 4.0329 +-0.1214 -0.2142 4.0594 +-0.1426 -0.2595 4.0859 +-0.1628 -0.3056 4.1124 +-0.1820 -0.3525 4.1389 +-0.2000 -0.4000 4.1654 +primID: 4 +startangle_c: 10 +endpose_c: -4 -2 9 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 3.9270 +-0.0411 -0.0262 3.9005 +-0.0831 -0.0515 3.8740 +-0.1260 -0.0758 3.8475 +-0.1697 -0.0991 3.8210 +-0.2142 -0.1214 3.7946 +-0.2595 -0.1426 3.7681 +-0.3056 -0.1628 3.7416 +-0.3525 -0.1820 3.7151 +-0.4000 -0.2000 3.6886 +primID: 5 +startangle_c: 10 +endpose_c: 0 0 11 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 3.9270 +0.0000 0.0000 3.9706 +0.0000 0.0000 4.0143 +0.0000 0.0000 4.0579 +0.0000 0.0000 4.1015 +0.0000 0.0000 4.1452 +0.0000 0.0000 4.1888 +0.0000 0.0000 4.2324 +0.0000 0.0000 4.2761 +0.0000 0.0000 4.3197 +primID: 6 +startangle_c: 10 +endpose_c: 0 0 9 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 3.9270 +0.0000 0.0000 3.8834 +0.0000 0.0000 3.8397 +0.0000 0.0000 3.7961 +0.0000 0.0000 3.7525 +0.0000 0.0000 3.7088 +0.0000 0.0000 3.6652 +0.0000 0.0000 3.6216 +0.0000 0.0000 3.5779 +0.0000 0.0000 3.5343 +primID: 0 +startangle_c: 11 +endpose_c: -1 -2 11 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 4.3197 +-0.0111 -0.0222 4.3197 +-0.0222 -0.0444 4.3197 +-0.0333 -0.0667 4.3197 +-0.0444 -0.0889 4.3197 +-0.0556 -0.1111 4.3197 +-0.0667 -0.1333 4.3197 +-0.0778 -0.1556 4.3197 +-0.0889 -0.1778 4.3197 +-0.1000 -0.2000 4.3197 +primID: 1 +startangle_c: 11 +endpose_c: -3 -6 11 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 4.3197 +-0.0333 -0.0667 4.3197 +-0.0667 -0.1333 4.3197 +-0.1000 -0.2000 4.3197 +-0.1333 -0.2667 4.3197 +-0.1667 -0.3333 4.3197 +-0.2000 -0.4000 4.3197 +-0.2333 -0.4667 4.3197 +-0.2667 -0.5333 4.3197 +-0.3000 -0.6000 4.3197 +primID: 2 +startangle_c: 11 +endpose_c: 1 2 11 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 4.3197 +0.0111 0.0222 4.3197 +0.0222 0.0444 4.3197 +0.0333 0.0667 4.3197 +0.0444 0.0889 4.3197 +0.0556 0.1111 4.3197 +0.0667 0.1333 4.3197 +0.0778 0.1556 4.3197 +0.0889 0.1778 4.3197 +0.1000 0.2000 4.3197 +primID: 3 +startangle_c: 11 +endpose_c: -2 -3 10 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 4.3197 +-0.0162 -0.0369 4.2779 +-0.0339 -0.0731 4.2341 +-0.0533 -0.1085 4.1902 +-0.0741 -0.1430 4.1463 +-0.0965 -0.1766 4.1025 +-0.1203 -0.2091 4.0586 +-0.1455 -0.2405 4.0147 +-0.1721 -0.2709 3.9709 +-0.2000 -0.3000 3.9270 +primID: 4 +startangle_c: 11 +endpose_c: -1 -4 12 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 4.3197 +-0.0177 -0.0427 4.3197 +-0.0354 -0.0854 4.3197 +-0.0523 -0.1283 4.3647 +-0.0668 -0.1722 4.4226 +-0.0787 -0.2168 4.4806 +-0.0880 -0.2621 4.5385 +-0.0947 -0.3078 4.5965 +-0.0987 -0.3538 4.6544 +-0.1000 -0.4000 4.7124 +primID: 5 +startangle_c: 11 +endpose_c: 0 0 10 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 4.3197 +0.0000 0.0000 4.2761 +0.0000 0.0000 4.2324 +0.0000 0.0000 4.1888 +0.0000 0.0000 4.1452 +0.0000 0.0000 4.1015 +0.0000 0.0000 4.0579 +0.0000 0.0000 4.0143 +0.0000 0.0000 3.9706 +0.0000 0.0000 3.9270 +primID: 6 +startangle_c: 11 +endpose_c: 0 0 12 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 4.3197 +0.0000 0.0000 4.3633 +0.0000 0.0000 4.4070 +0.0000 0.0000 4.4506 +0.0000 0.0000 4.4942 +0.0000 0.0000 4.5379 +0.0000 0.0000 4.5815 +0.0000 0.0000 4.6251 +0.0000 0.0000 4.6688 +0.0000 0.0000 4.7124 +primID: 0 +startangle_c: 12 +endpose_c: 0 -1 12 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 4.7124 +0.0000 -0.0111 4.7124 +0.0000 -0.0222 4.7124 +0.0000 -0.0333 4.7124 +0.0000 -0.0444 4.7124 +0.0000 -0.0556 4.7124 +0.0000 -0.0667 4.7124 +0.0000 -0.0778 4.7124 +0.0000 -0.0889 4.7124 +0.0000 -0.1000 4.7124 +primID: 1 +startangle_c: 12 +endpose_c: 0 -4 12 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 4.7124 +0.0000 -0.0444 4.7124 +0.0000 -0.0889 4.7124 +0.0000 -0.1333 4.7124 +0.0000 -0.1778 4.7124 +0.0000 -0.2222 4.7124 +0.0000 -0.2667 4.7124 +0.0000 -0.3111 4.7124 +0.0000 -0.3556 4.7124 +0.0000 -0.4000 4.7124 +primID: 2 +startangle_c: 12 +endpose_c: 0 1 12 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 4.7124 +0.0000 0.0111 4.7124 +0.0000 0.0222 4.7124 +0.0000 0.0333 4.7124 +0.0000 0.0444 4.7124 +0.0000 0.0556 4.7124 +0.0000 0.0667 4.7124 +0.0000 0.0778 4.7124 +0.0000 0.0889 4.7124 +0.0000 0.1000 4.7124 +primID: 3 +startangle_c: 12 +endpose_c: 1 -4 13 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 4.7124 +0.0048 -0.0452 4.7473 +0.0111 -0.0903 4.7823 +0.0191 -0.1353 4.8172 +0.0286 -0.1802 4.8522 +0.0398 -0.2248 4.8871 +0.0525 -0.2692 4.9221 +0.0668 -0.3132 4.9570 +0.0826 -0.3568 4.9919 +0.1000 -0.4000 5.0269 +primID: 4 +startangle_c: 12 +endpose_c: -1 -4 11 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 4.7124 +-0.0048 -0.0452 4.6774 +-0.0111 -0.0903 4.6425 +-0.0191 -0.1353 4.6076 +-0.0286 -0.1802 4.5726 +-0.0398 -0.2248 4.5377 +-0.0525 -0.2692 4.5027 +-0.0668 -0.3132 4.4678 +-0.0826 -0.3568 4.4328 +-0.1000 -0.4000 4.3979 +primID: 5 +startangle_c: 12 +endpose_c: 0 0 13 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 4.7124 +0.0000 0.0000 4.7560 +0.0000 0.0000 4.7997 +0.0000 0.0000 4.8433 +0.0000 0.0000 4.8869 +0.0000 0.0000 4.9306 +0.0000 0.0000 4.9742 +0.0000 0.0000 5.0178 +0.0000 0.0000 5.0615 +0.0000 0.0000 5.1051 +primID: 6 +startangle_c: 12 +endpose_c: 0 0 11 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 4.7124 +0.0000 0.0000 4.6688 +0.0000 0.0000 4.6251 +0.0000 0.0000 4.5815 +0.0000 0.0000 4.5379 +0.0000 0.0000 4.4942 +0.0000 0.0000 4.4506 +0.0000 0.0000 4.4070 +0.0000 0.0000 4.3633 +0.0000 0.0000 4.3197 +primID: 0 +startangle_c: 13 +endpose_c: 1 -2 13 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 5.1051 +0.0111 -0.0222 5.1051 +0.0222 -0.0444 5.1051 +0.0333 -0.0667 5.1051 +0.0444 -0.0889 5.1051 +0.0556 -0.1111 5.1051 +0.0667 -0.1333 5.1051 +0.0778 -0.1556 5.1051 +0.0889 -0.1778 5.1051 +0.1000 -0.2000 5.1051 +primID: 1 +startangle_c: 13 +endpose_c: 3 -6 13 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 5.1051 +0.0333 -0.0667 5.1051 +0.0667 -0.1333 5.1051 +0.1000 -0.2000 5.1051 +0.1333 -0.2667 5.1051 +0.1667 -0.3333 5.1051 +0.2000 -0.4000 5.1051 +0.2333 -0.4667 5.1051 +0.2667 -0.5333 5.1051 +0.3000 -0.6000 5.1051 +primID: 2 +startangle_c: 13 +endpose_c: -1 2 13 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 5.1051 +-0.0111 0.0222 5.1051 +-0.0222 0.0444 5.1051 +-0.0333 0.0667 5.1051 +-0.0444 0.0889 5.1051 +-0.0556 0.1111 5.1051 +-0.0667 0.1333 5.1051 +-0.0778 0.1556 5.1051 +-0.0889 0.1778 5.1051 +-0.1000 0.2000 5.1051 +primID: 3 +startangle_c: 13 +endpose_c: 2 -3 14 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 5.1051 +0.0162 -0.0369 5.1469 +0.0339 -0.0731 5.1907 +0.0533 -0.1085 5.2346 +0.0741 -0.1430 5.2785 +0.0965 -0.1766 5.3223 +0.1203 -0.2091 5.3662 +0.1455 -0.2405 5.4101 +0.1721 -0.2709 5.4539 +0.2000 -0.3000 5.4978 +primID: 4 +startangle_c: 13 +endpose_c: 1 -4 12 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 5.1051 +0.0177 -0.0427 5.1051 +0.0354 -0.0854 5.1051 +0.0523 -0.1283 5.0601 +0.0668 -0.1722 5.0021 +0.0787 -0.2168 4.9442 +0.0880 -0.2621 4.8862 +0.0947 -0.3078 4.8283 +0.0987 -0.3538 4.7703 +0.1000 -0.4000 4.7124 +primID: 5 +startangle_c: 13 +endpose_c: 0 0 14 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 5.1051 +0.0000 0.0000 5.1487 +0.0000 0.0000 5.1924 +0.0000 0.0000 5.2360 +0.0000 0.0000 5.2796 +0.0000 0.0000 5.3233 +0.0000 0.0000 5.3669 +0.0000 0.0000 5.4105 +0.0000 0.0000 5.4542 +0.0000 0.0000 5.4978 +primID: 6 +startangle_c: 13 +endpose_c: 0 0 12 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 5.1051 +0.0000 0.0000 5.0615 +0.0000 0.0000 5.0178 +0.0000 0.0000 4.9742 +0.0000 0.0000 4.9306 +0.0000 0.0000 4.8869 +0.0000 0.0000 4.8433 +0.0000 0.0000 4.7997 +0.0000 0.0000 4.7560 +0.0000 0.0000 4.7124 +primID: 0 +startangle_c: 14 +endpose_c: 1 -1 14 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 5.4978 +0.0111 -0.0111 5.4978 +0.0222 -0.0222 5.4978 +0.0333 -0.0333 5.4978 +0.0444 -0.0444 5.4978 +0.0556 -0.0556 5.4978 +0.0667 -0.0667 5.4978 +0.0778 -0.0778 5.4978 +0.0889 -0.0889 5.4978 +0.1000 -0.1000 5.4978 +primID: 1 +startangle_c: 14 +endpose_c: 3 -3 14 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 5.4978 +0.0333 -0.0333 5.4978 +0.0667 -0.0667 5.4978 +0.1000 -0.1000 5.4978 +0.1333 -0.1333 5.4978 +0.1667 -0.1667 5.4978 +0.2000 -0.2000 5.4978 +0.2333 -0.2333 5.4978 +0.2667 -0.2667 5.4978 +0.3000 -0.3000 5.4978 +primID: 2 +startangle_c: 14 +endpose_c: -1 1 14 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 5.4978 +-0.0111 0.0111 5.4978 +-0.0222 0.0222 5.4978 +-0.0333 0.0333 5.4978 +-0.0444 0.0444 5.4978 +-0.0556 0.0556 5.4978 +-0.0667 0.0667 5.4978 +-0.0778 0.0778 5.4978 +-0.0889 0.0889 5.4978 +-0.1000 0.1000 5.4978 +primID: 3 +startangle_c: 14 +endpose_c: 4 -2 15 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 5.4978 +0.0411 -0.0262 5.5243 +0.0831 -0.0515 5.5508 +0.1260 -0.0758 5.5772 +0.1697 -0.0991 5.6037 +0.2142 -0.1214 5.6302 +0.2595 -0.1426 5.6567 +0.3056 -0.1628 5.6832 +0.3525 -0.1820 5.7097 +0.4000 -0.2000 5.7362 +primID: 4 +startangle_c: 14 +endpose_c: 2 -4 13 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 5.4978 +0.0262 -0.0411 5.4713 +0.0515 -0.0831 5.4448 +0.0758 -0.1260 5.4183 +0.0991 -0.1697 5.3918 +0.1214 -0.2142 5.3654 +0.1426 -0.2595 5.3389 +0.1628 -0.3056 5.3124 +0.1820 -0.3525 5.2859 +0.2000 -0.4000 5.2594 +primID: 5 +startangle_c: 14 +endpose_c: 0 0 15 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 5.4978 +0.0000 0.0000 5.5414 +0.0000 0.0000 5.5851 +0.0000 0.0000 5.6287 +0.0000 0.0000 5.6723 +0.0000 0.0000 5.7160 +0.0000 0.0000 5.7596 +0.0000 0.0000 5.8032 +0.0000 0.0000 5.8469 +0.0000 0.0000 5.8905 +primID: 6 +startangle_c: 14 +endpose_c: 0 0 13 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 5.4978 +0.0000 0.0000 5.4542 +0.0000 0.0000 5.4105 +0.0000 0.0000 5.3669 +0.0000 0.0000 5.3233 +0.0000 0.0000 5.2796 +0.0000 0.0000 5.2360 +0.0000 0.0000 5.1924 +0.0000 0.0000 5.1487 +0.0000 0.0000 5.1051 +primID: 0 +startangle_c: 15 +endpose_c: 2 -1 15 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 5.8905 +0.0222 -0.0111 5.8905 +0.0444 -0.0222 5.8905 +0.0667 -0.0333 5.8905 +0.0889 -0.0444 5.8905 +0.1111 -0.0556 5.8905 +0.1333 -0.0667 5.8905 +0.1556 -0.0778 5.8905 +0.1778 -0.0889 5.8905 +0.2000 -0.1000 5.8905 +primID: 1 +startangle_c: 15 +endpose_c: 6 -3 15 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 5.8905 +0.0667 -0.0333 5.8905 +0.1333 -0.0667 5.8905 +0.2000 -0.1000 5.8905 +0.2667 -0.1333 5.8905 +0.3333 -0.1667 5.8905 +0.4000 -0.2000 5.8905 +0.4667 -0.2333 5.8905 +0.5333 -0.2667 5.8905 +0.6000 -0.3000 5.8905 +primID: 2 +startangle_c: 15 +endpose_c: -2 1 15 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 5.8905 +-0.0222 0.0111 5.8905 +-0.0444 0.0222 5.8905 +-0.0667 0.0333 5.8905 +-0.0889 0.0444 5.8905 +-0.1111 0.0556 5.8905 +-0.1333 0.0667 5.8905 +-0.1556 0.0778 5.8905 +-0.1778 0.0889 5.8905 +-0.2000 0.1000 5.8905 +primID: 3 +startangle_c: 15 +endpose_c: 3 -2 14 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 5.8905 +0.0369 -0.0162 5.8487 +0.0731 -0.0339 5.8049 +0.1085 -0.0533 5.7610 +0.1430 -0.0741 5.7171 +0.1766 -0.0965 5.6733 +0.2091 -0.1203 5.6294 +0.2405 -0.1455 5.5855 +0.2709 -0.1721 5.5417 +0.3000 -0.2000 5.4978 +primID: 4 +startangle_c: 15 +endpose_c: 4 -1 0 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 5.8905 +0.0427 -0.0177 5.8905 +0.0854 -0.0354 5.8905 +0.1283 -0.0523 5.9355 +0.1722 -0.0668 5.9934 +0.2168 -0.0787 6.0514 +0.2621 -0.0880 6.1093 +0.3078 -0.0947 6.1673 +0.3538 -0.0987 6.2252 +0.4000 -0.1000 6.2832 +primID: 5 +startangle_c: 15 +endpose_c: 0 0 14 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 5.8905 +0.0000 0.0000 5.8469 +0.0000 0.0000 5.8032 +0.0000 0.0000 5.7596 +0.0000 0.0000 5.7160 +0.0000 0.0000 5.6723 +0.0000 0.0000 5.6287 +0.0000 0.0000 5.5851 +0.0000 0.0000 5.5414 +0.0000 0.0000 5.4978 +primID: 6 +startangle_c: 15 +endpose_c: 0 0 0 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 5.8905 +0.0000 0.0000 5.2360 +0.0000 0.0000 4.5815 +0.0000 0.0000 3.9270 +0.0000 0.0000 3.2725 +0.0000 0.0000 2.6180 +0.0000 0.0000 1.9635 +0.0000 0.0000 1.3090 +0.0000 0.0000 0.6545 +0.0000 0.0000 0.0000 diff --git a/Controllers/Packages/amr_startup/config/mprim/unicycle_highcost_1cm.mprim b/Controllers/Packages/amr_startup/config/mprim/unicycle_highcost_1cm.mprim new file mode 100755 index 0000000..aa9670d --- /dev/null +++ b/Controllers/Packages/amr_startup/config/mprim/unicycle_highcost_1cm.mprim @@ -0,0 +1,1683 @@ +resolution_m: 0.010000 +numberofangles: 16 +totalnumberofprimitives: 112 +primID: 0 +startangle_c: 0 +endpose_c: 1 0 0 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 0.0000 +0.0011 0.0000 0.0000 +0.0022 0.0000 0.0000 +0.0033 0.0000 0.0000 +0.0044 0.0000 0.0000 +0.0056 0.0000 0.0000 +0.0067 0.0000 0.0000 +0.0078 0.0000 0.0000 +0.0089 0.0000 0.0000 +0.0100 0.0000 0.0000 +primID: 1 +startangle_c: 0 +endpose_c: 20 0 0 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 0.0000 +0.0222 0.0000 0.0000 +0.0444 0.0000 0.0000 +0.0667 0.0000 0.0000 +0.0889 0.0000 0.0000 +0.1111 0.0000 0.0000 +0.1333 0.0000 0.0000 +0.1556 0.0000 0.0000 +0.1778 0.0000 0.0000 +0.2000 0.0000 0.0000 +primID: 2 +startangle_c: 0 +endpose_c: -1 0 0 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 0.0000 +-0.0011 0.0000 0.0000 +-0.0022 0.0000 0.0000 +-0.0033 0.0000 0.0000 +-0.0044 0.0000 0.0000 +-0.0056 0.0000 0.0000 +-0.0067 0.0000 0.0000 +-0.0078 0.0000 0.0000 +-0.0089 0.0000 0.0000 +-0.0100 0.0000 0.0000 +primID: 3 +startangle_c: 0 +endpose_c: 40 5 1 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 0.0000 +0.0452 -0.0000 0.0000 +0.0904 -0.0000 0.0000 +0.1355 -0.0000 0.0000 +0.1807 0.0008 0.0488 +0.2257 0.0045 0.1176 +0.2703 0.0114 0.1864 +0.3144 0.0213 0.2551 +0.3577 0.0342 0.3239 +0.4000 0.0500 0.3927 +primID: 4 +startangle_c: 0 +endpose_c: 40 -5 -1 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 0.0000 +0.0452 0.0000 0.0000 +0.0904 0.0000 0.0000 +0.1355 0.0000 0.0000 +0.1807 -0.0008 -0.0488 +0.2257 -0.0045 -0.1176 +0.2703 -0.0114 -0.1864 +0.3144 -0.0213 -0.2551 +0.3577 -0.0342 -0.3239 +0.4000 -0.0500 -0.3927 +primID: 5 +startangle_c: 0 +endpose_c: 0 0 1 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 0.0000 +0.0000 0.0000 0.0436 +0.0000 0.0000 0.0873 +0.0000 0.0000 0.1309 +0.0000 0.0000 0.1745 +0.0000 0.0000 0.2182 +0.0000 0.0000 0.2618 +0.0000 0.0000 0.3054 +0.0000 0.0000 0.3491 +0.0000 0.0000 0.3927 +primID: 6 +startangle_c: 0 +endpose_c: 0 0 -1 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 0.0000 +0.0000 0.0000 -0.0436 +0.0000 0.0000 -0.0873 +0.0000 0.0000 -0.1309 +0.0000 0.0000 -0.1745 +0.0000 0.0000 -0.2182 +0.0000 0.0000 -0.2618 +0.0000 0.0000 -0.3054 +0.0000 0.0000 -0.3491 +0.0000 0.0000 -0.3927 +primID: 0 +startangle_c: 1 +endpose_c: 2 1 1 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 0.3927 +0.0022 0.0011 0.3927 +0.0044 0.0022 0.3927 +0.0067 0.0033 0.3927 +0.0089 0.0044 0.3927 +0.0111 0.0056 0.3927 +0.0133 0.0067 0.3927 +0.0156 0.0078 0.3927 +0.0178 0.0089 0.3927 +0.0200 0.0100 0.3927 +primID: 1 +startangle_c: 1 +endpose_c: 20 10 1 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 0.3927 +0.0222 0.0111 0.3927 +0.0444 0.0222 0.3927 +0.0667 0.0333 0.3927 +0.0889 0.0444 0.3927 +0.1111 0.0556 0.3927 +0.1333 0.0667 0.3927 +0.1556 0.0778 0.3927 +0.1778 0.0889 0.3927 +0.2000 0.1000 0.3927 +primID: 2 +startangle_c: 1 +endpose_c: -2 -1 1 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 0.3927 +-0.0022 -0.0011 0.3927 +-0.0044 -0.0022 0.3927 +-0.0067 -0.0033 0.3927 +-0.0089 -0.0044 0.3927 +-0.0111 -0.0056 0.3927 +-0.0133 -0.0067 0.3927 +-0.0156 -0.0078 0.3927 +-0.0178 -0.0089 0.3927 +-0.0200 -0.0100 0.3927 +primID: 3 +startangle_c: 1 +endpose_c: 25 20 2 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 0.3927 +0.0298 0.0184 0.4230 +0.0592 0.0379 0.4533 +0.0881 0.0583 0.4836 +0.1165 0.0796 0.5139 +0.1444 0.1019 0.5442 +0.1717 0.1251 0.5745 +0.1984 0.1492 0.6048 +0.2245 0.1742 0.6351 +0.2500 0.2000 0.6654 +primID: 4 +startangle_c: 1 +endpose_c: 35 10 0 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 0.3927 +0.0377 0.0156 0.3927 +0.0754 0.0312 0.3927 +0.1131 0.0468 0.3927 +0.1508 0.0623 0.3736 +0.1893 0.0758 0.2989 +0.2287 0.0863 0.2242 +0.2687 0.0939 0.1494 +0.3092 0.0985 0.0747 +0.3500 0.1000 -0.0000 +primID: 5 +startangle_c: 1 +endpose_c: 0 0 2 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 0.3927 +0.0000 0.0000 0.4363 +0.0000 0.0000 0.4800 +0.0000 0.0000 0.5236 +0.0000 0.0000 0.5672 +0.0000 0.0000 0.6109 +0.0000 0.0000 0.6545 +0.0000 0.0000 0.6981 +0.0000 0.0000 0.7418 +0.0000 0.0000 0.7854 +primID: 6 +startangle_c: 1 +endpose_c: 0 0 0 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 0.3927 +0.0000 0.0000 0.3491 +0.0000 0.0000 0.3054 +0.0000 0.0000 0.2618 +0.0000 0.0000 0.2182 +0.0000 0.0000 0.1745 +0.0000 0.0000 0.1309 +0.0000 0.0000 0.0873 +0.0000 0.0000 0.0436 +0.0000 0.0000 0.0000 +primID: 0 +startangle_c: 2 +endpose_c: 1 1 2 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 0.7854 +0.0011 0.0011 0.7854 +0.0022 0.0022 0.7854 +0.0033 0.0033 0.7854 +0.0044 0.0044 0.7854 +0.0056 0.0056 0.7854 +0.0067 0.0067 0.7854 +0.0078 0.0078 0.7854 +0.0089 0.0089 0.7854 +0.0100 0.0100 0.7854 +primID: 1 +startangle_c: 2 +endpose_c: 10 10 2 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 0.7854 +0.0111 0.0111 0.7854 +0.0222 0.0222 0.7854 +0.0333 0.0333 0.7854 +0.0444 0.0444 0.7854 +0.0556 0.0556 0.7854 +0.0667 0.0667 0.7854 +0.0778 0.0778 0.7854 +0.0889 0.0889 0.7854 +0.1000 0.1000 0.7854 +primID: 2 +startangle_c: 2 +endpose_c: -1 -1 2 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 0.7854 +-0.0011 -0.0011 0.7854 +-0.0022 -0.0022 0.7854 +-0.0033 -0.0033 0.7854 +-0.0044 -0.0044 0.7854 +-0.0056 -0.0056 0.7854 +-0.0067 -0.0067 0.7854 +-0.0078 -0.0078 0.7854 +-0.0089 -0.0089 0.7854 +-0.0100 -0.0100 0.7854 +primID: 3 +startangle_c: 2 +endpose_c: 25 35 3 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 0.7854 +0.0341 0.0341 0.7854 +0.0678 0.0684 0.8151 +0.1000 0.1043 0.8669 +0.1302 0.1418 0.9188 +0.1584 0.1809 0.9707 +0.1846 0.2213 1.0225 +0.2086 0.2631 1.0744 +0.2304 0.3060 1.1262 +0.2500 0.3500 1.1781 +primID: 4 +startangle_c: 2 +endpose_c: 35 25 1 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 0.7854 +0.0341 0.0341 0.7854 +0.0684 0.0678 0.7557 +0.1043 0.1000 0.7039 +0.1418 0.1302 0.6520 +0.1809 0.1584 0.6001 +0.2213 0.1846 0.5483 +0.2631 0.2086 0.4964 +0.3060 0.2304 0.4446 +0.3500 0.2500 0.3927 +primID: 5 +startangle_c: 2 +endpose_c: 0 0 3 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 0.7854 +0.0000 0.0000 0.8290 +0.0000 0.0000 0.8727 +0.0000 0.0000 0.9163 +0.0000 0.0000 0.9599 +0.0000 0.0000 1.0036 +0.0000 0.0000 1.0472 +0.0000 0.0000 1.0908 +0.0000 0.0000 1.1345 +0.0000 0.0000 1.1781 +primID: 6 +startangle_c: 2 +endpose_c: 0 0 1 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 0.7854 +0.0000 0.0000 0.7418 +0.0000 0.0000 0.6981 +0.0000 0.0000 0.6545 +0.0000 0.0000 0.6109 +0.0000 0.0000 0.5672 +0.0000 0.0000 0.5236 +0.0000 0.0000 0.4800 +0.0000 0.0000 0.4363 +0.0000 0.0000 0.3927 +primID: 0 +startangle_c: 3 +endpose_c: 1 2 3 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 1.1781 +0.0011 0.0022 1.1781 +0.0022 0.0044 1.1781 +0.0033 0.0067 1.1781 +0.0044 0.0089 1.1781 +0.0056 0.0111 1.1781 +0.0067 0.0133 1.1781 +0.0078 0.0156 1.1781 +0.0089 0.0178 1.1781 +0.0100 0.0200 1.1781 +primID: 1 +startangle_c: 3 +endpose_c: 10 20 3 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 1.1781 +0.0111 0.0222 1.1781 +0.0222 0.0444 1.1781 +0.0333 0.0667 1.1781 +0.0444 0.0889 1.1781 +0.0556 0.1111 1.1781 +0.0667 0.1333 1.1781 +0.0778 0.1556 1.1781 +0.0889 0.1778 1.1781 +0.1000 0.2000 1.1781 +primID: 2 +startangle_c: 3 +endpose_c: -1 -2 3 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 1.1781 +-0.0011 -0.0022 1.1781 +-0.0022 -0.0044 1.1781 +-0.0033 -0.0067 1.1781 +-0.0044 -0.0089 1.1781 +-0.0056 -0.0111 1.1781 +-0.0067 -0.0133 1.1781 +-0.0078 -0.0156 1.1781 +-0.0089 -0.0178 1.1781 +-0.0100 -0.0200 1.1781 +primID: 3 +startangle_c: 3 +endpose_c: 20 25 2 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 1.1781 +0.0184 0.0298 1.1478 +0.0379 0.0592 1.1175 +0.0583 0.0881 1.0872 +0.0796 0.1165 1.0569 +0.1019 0.1444 1.0266 +0.1251 0.1717 0.9963 +0.1492 0.1984 0.9660 +0.1742 0.2245 0.9357 +0.2000 0.2500 0.9054 +primID: 4 +startangle_c: 3 +endpose_c: 10 35 4 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 1.1781 +0.0156 0.0377 1.1781 +0.0312 0.0754 1.1781 +0.0468 0.1131 1.1781 +0.0623 0.1508 1.1972 +0.0758 0.1893 1.2719 +0.0863 0.2287 1.3466 +0.0939 0.2687 1.4214 +0.0985 0.3092 1.4961 +0.1000 0.3500 1.5708 +primID: 5 +startangle_c: 3 +endpose_c: 0 0 2 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 1.1781 +0.0000 0.0000 1.1345 +0.0000 0.0000 1.0908 +0.0000 0.0000 1.0472 +0.0000 0.0000 1.0036 +0.0000 0.0000 0.9599 +0.0000 0.0000 0.9163 +0.0000 0.0000 0.8727 +0.0000 0.0000 0.8290 +0.0000 0.0000 0.7854 +primID: 6 +startangle_c: 3 +endpose_c: 0 0 4 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 1.1781 +0.0000 0.0000 1.2217 +0.0000 0.0000 1.2654 +0.0000 0.0000 1.3090 +0.0000 0.0000 1.3526 +0.0000 0.0000 1.3963 +0.0000 0.0000 1.4399 +0.0000 0.0000 1.4835 +0.0000 0.0000 1.5272 +0.0000 0.0000 1.5708 +primID: 0 +startangle_c: 4 +endpose_c: 0 1 4 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 1.5708 +0.0000 0.0011 1.5708 +0.0000 0.0022 1.5708 +0.0000 0.0033 1.5708 +0.0000 0.0044 1.5708 +0.0000 0.0056 1.5708 +0.0000 0.0067 1.5708 +0.0000 0.0078 1.5708 +0.0000 0.0089 1.5708 +0.0000 0.0100 1.5708 +primID: 1 +startangle_c: 4 +endpose_c: 0 20 4 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 1.5708 +0.0000 0.0222 1.5708 +0.0000 0.0444 1.5708 +0.0000 0.0667 1.5708 +0.0000 0.0889 1.5708 +0.0000 0.1111 1.5708 +0.0000 0.1333 1.5708 +0.0000 0.1556 1.5708 +0.0000 0.1778 1.5708 +0.0000 0.2000 1.5708 +primID: 2 +startangle_c: 4 +endpose_c: 0 -1 4 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 1.5708 +0.0000 -0.0011 1.5708 +0.0000 -0.0022 1.5708 +0.0000 -0.0033 1.5708 +0.0000 -0.0044 1.5708 +0.0000 -0.0056 1.5708 +0.0000 -0.0067 1.5708 +0.0000 -0.0078 1.5708 +0.0000 -0.0089 1.5708 +0.0000 -0.0100 1.5708 +primID: 3 +startangle_c: 4 +endpose_c: -5 40 5 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 1.5708 +0.0000 0.0452 1.5708 +0.0000 0.0904 1.5708 +0.0000 0.1355 1.5708 +-0.0008 0.1807 1.6196 +-0.0045 0.2257 1.6884 +-0.0114 0.2703 1.7572 +-0.0213 0.3144 1.8259 +-0.0342 0.3577 1.8947 +-0.0500 0.4000 1.9635 +primID: 4 +startangle_c: 4 +endpose_c: 5 40 3 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 1.5708 +0.0000 0.0452 1.5708 +0.0000 0.0904 1.5708 +0.0000 0.1355 1.5708 +0.0008 0.1807 1.5220 +0.0045 0.2257 1.4532 +0.0114 0.2703 1.3844 +0.0213 0.3144 1.3156 +0.0342 0.3577 1.2469 +0.0500 0.4000 1.1781 +primID: 5 +startangle_c: 4 +endpose_c: 0 0 5 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 1.5708 +0.0000 0.0000 1.6144 +0.0000 0.0000 1.6581 +0.0000 0.0000 1.7017 +0.0000 0.0000 1.7453 +0.0000 0.0000 1.7890 +0.0000 0.0000 1.8326 +0.0000 0.0000 1.8762 +0.0000 0.0000 1.9199 +0.0000 0.0000 1.9635 +primID: 6 +startangle_c: 4 +endpose_c: 0 0 3 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 1.5708 +0.0000 0.0000 1.5272 +0.0000 0.0000 1.4835 +0.0000 0.0000 1.4399 +0.0000 0.0000 1.3963 +0.0000 0.0000 1.3526 +0.0000 0.0000 1.3090 +0.0000 0.0000 1.2654 +0.0000 0.0000 1.2217 +0.0000 0.0000 1.1781 +primID: 0 +startangle_c: 5 +endpose_c: -1 2 5 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 1.9635 +-0.0011 0.0022 1.9635 +-0.0022 0.0044 1.9635 +-0.0033 0.0067 1.9635 +-0.0044 0.0089 1.9635 +-0.0056 0.0111 1.9635 +-0.0067 0.0133 1.9635 +-0.0078 0.0156 1.9635 +-0.0089 0.0178 1.9635 +-0.0100 0.0200 1.9635 +primID: 1 +startangle_c: 5 +endpose_c: -10 20 5 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 1.9635 +-0.0111 0.0222 1.9635 +-0.0222 0.0444 1.9635 +-0.0333 0.0667 1.9635 +-0.0444 0.0889 1.9635 +-0.0556 0.1111 1.9635 +-0.0667 0.1333 1.9635 +-0.0778 0.1556 1.9635 +-0.0889 0.1778 1.9635 +-0.1000 0.2000 1.9635 +primID: 2 +startangle_c: 5 +endpose_c: 1 -2 5 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 1.9635 +0.0011 -0.0022 1.9635 +0.0022 -0.0044 1.9635 +0.0033 -0.0067 1.9635 +0.0044 -0.0089 1.9635 +0.0056 -0.0111 1.9635 +0.0067 -0.0133 1.9635 +0.0078 -0.0156 1.9635 +0.0089 -0.0178 1.9635 +0.0100 -0.0200 1.9635 +primID: 3 +startangle_c: 5 +endpose_c: -20 25 6 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 1.9635 +-0.0184 0.0298 1.9938 +-0.0379 0.0592 2.0241 +-0.0583 0.0881 2.0544 +-0.0796 0.1165 2.0847 +-0.1019 0.1444 2.1150 +-0.1251 0.1717 2.1453 +-0.1492 0.1984 2.1756 +-0.1742 0.2245 2.2059 +-0.2000 0.2500 2.2362 +primID: 4 +startangle_c: 5 +endpose_c: -10 35 4 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 1.9635 +-0.0156 0.0377 1.9635 +-0.0312 0.0754 1.9635 +-0.0468 0.1131 1.9635 +-0.0623 0.1508 1.9444 +-0.0758 0.1893 1.8697 +-0.0863 0.2287 1.7950 +-0.0939 0.2687 1.7202 +-0.0985 0.3092 1.6455 +-0.1000 0.3500 1.5708 +primID: 5 +startangle_c: 5 +endpose_c: 0 0 6 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 1.9635 +0.0000 0.0000 2.0071 +0.0000 0.0000 2.0508 +0.0000 0.0000 2.0944 +0.0000 0.0000 2.1380 +0.0000 0.0000 2.1817 +0.0000 0.0000 2.2253 +0.0000 0.0000 2.2689 +0.0000 0.0000 2.3126 +0.0000 0.0000 2.3562 +primID: 6 +startangle_c: 5 +endpose_c: 0 0 4 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 1.9635 +0.0000 0.0000 1.9199 +0.0000 0.0000 1.8762 +0.0000 0.0000 1.8326 +0.0000 0.0000 1.7890 +0.0000 0.0000 1.7453 +0.0000 0.0000 1.7017 +0.0000 0.0000 1.6581 +0.0000 0.0000 1.6144 +0.0000 0.0000 1.5708 +primID: 0 +startangle_c: 6 +endpose_c: -1 1 6 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 2.3562 +-0.0011 0.0011 2.3562 +-0.0022 0.0022 2.3562 +-0.0033 0.0033 2.3562 +-0.0044 0.0044 2.3562 +-0.0056 0.0056 2.3562 +-0.0067 0.0067 2.3562 +-0.0078 0.0078 2.3562 +-0.0089 0.0089 2.3562 +-0.0100 0.0100 2.3562 +primID: 1 +startangle_c: 6 +endpose_c: -10 10 6 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 2.3562 +-0.0111 0.0111 2.3562 +-0.0222 0.0222 2.3562 +-0.0333 0.0333 2.3562 +-0.0444 0.0444 2.3562 +-0.0556 0.0556 2.3562 +-0.0667 0.0667 2.3562 +-0.0778 0.0778 2.3562 +-0.0889 0.0889 2.3562 +-0.1000 0.1000 2.3562 +primID: 2 +startangle_c: 6 +endpose_c: 1 -1 6 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 2.3562 +0.0011 -0.0011 2.3562 +0.0022 -0.0022 2.3562 +0.0033 -0.0033 2.3562 +0.0044 -0.0044 2.3562 +0.0056 -0.0056 2.3562 +0.0067 -0.0067 2.3562 +0.0078 -0.0078 2.3562 +0.0089 -0.0089 2.3562 +0.0100 -0.0100 2.3562 +primID: 3 +startangle_c: 6 +endpose_c: -35 25 7 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 2.3562 +-0.0341 0.0341 2.3562 +-0.0684 0.0678 2.3859 +-0.1043 0.1000 2.4377 +-0.1418 0.1302 2.4896 +-0.1809 0.1584 2.5415 +-0.2213 0.1846 2.5933 +-0.2631 0.2086 2.6452 +-0.3060 0.2304 2.6970 +-0.3500 0.2500 2.7489 +primID: 4 +startangle_c: 6 +endpose_c: -25 35 5 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 2.3562 +-0.0341 0.0341 2.3562 +-0.0678 0.0684 2.3265 +-0.1000 0.1043 2.2747 +-0.1302 0.1418 2.2228 +-0.1584 0.1809 2.1709 +-0.1846 0.2213 2.1191 +-0.2086 0.2631 2.0672 +-0.2304 0.3060 2.0154 +-0.2500 0.3500 1.9635 +primID: 5 +startangle_c: 6 +endpose_c: 0 0 7 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 2.3562 +0.0000 0.0000 2.3998 +0.0000 0.0000 2.4435 +0.0000 0.0000 2.4871 +0.0000 0.0000 2.5307 +0.0000 0.0000 2.5744 +0.0000 0.0000 2.6180 +0.0000 0.0000 2.6616 +0.0000 0.0000 2.7053 +0.0000 0.0000 2.7489 +primID: 6 +startangle_c: 6 +endpose_c: 0 0 5 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 2.3562 +0.0000 0.0000 2.3126 +0.0000 0.0000 2.2689 +0.0000 0.0000 2.2253 +0.0000 0.0000 2.1817 +0.0000 0.0000 2.1380 +0.0000 0.0000 2.0944 +0.0000 0.0000 2.0508 +0.0000 0.0000 2.0071 +0.0000 0.0000 1.9635 +primID: 0 +startangle_c: 7 +endpose_c: -2 1 7 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 2.7489 +-0.0022 0.0011 2.7489 +-0.0044 0.0022 2.7489 +-0.0067 0.0033 2.7489 +-0.0089 0.0044 2.7489 +-0.0111 0.0056 2.7489 +-0.0133 0.0067 2.7489 +-0.0156 0.0078 2.7489 +-0.0178 0.0089 2.7489 +-0.0200 0.0100 2.7489 +primID: 1 +startangle_c: 7 +endpose_c: -20 10 7 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 2.7489 +-0.0222 0.0111 2.7489 +-0.0444 0.0222 2.7489 +-0.0667 0.0333 2.7489 +-0.0889 0.0444 2.7489 +-0.1111 0.0556 2.7489 +-0.1333 0.0667 2.7489 +-0.1556 0.0778 2.7489 +-0.1778 0.0889 2.7489 +-0.2000 0.1000 2.7489 +primID: 2 +startangle_c: 7 +endpose_c: 2 -1 7 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 2.7489 +0.0022 -0.0011 2.7489 +0.0044 -0.0022 2.7489 +0.0067 -0.0033 2.7489 +0.0089 -0.0044 2.7489 +0.0111 -0.0056 2.7489 +0.0133 -0.0067 2.7489 +0.0156 -0.0078 2.7489 +0.0178 -0.0089 2.7489 +0.0200 -0.0100 2.7489 +primID: 3 +startangle_c: 7 +endpose_c: -25 20 6 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 2.7489 +-0.0298 0.0184 2.7186 +-0.0592 0.0379 2.6883 +-0.0881 0.0583 2.6580 +-0.1165 0.0796 2.6277 +-0.1444 0.1019 2.5974 +-0.1717 0.1251 2.5671 +-0.1984 0.1492 2.5368 +-0.2245 0.1742 2.5065 +-0.2500 0.2000 2.4762 +primID: 4 +startangle_c: 7 +endpose_c: -35 10 8 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 2.7489 +-0.0377 0.0156 2.7489 +-0.0754 0.0312 2.7489 +-0.1131 0.0468 2.7489 +-0.1508 0.0623 2.7680 +-0.1893 0.0758 2.8427 +-0.2287 0.0863 2.9174 +-0.2687 0.0939 2.9921 +-0.3092 0.0985 3.0669 +-0.3500 0.1000 3.1416 +primID: 5 +startangle_c: 7 +endpose_c: 0 0 6 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 2.7489 +0.0000 0.0000 2.7053 +0.0000 0.0000 2.6616 +0.0000 0.0000 2.6180 +0.0000 0.0000 2.5744 +0.0000 0.0000 2.5307 +0.0000 0.0000 2.4871 +0.0000 0.0000 2.4435 +0.0000 0.0000 2.3998 +0.0000 0.0000 2.3562 +primID: 6 +startangle_c: 7 +endpose_c: 0 0 8 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 2.7489 +0.0000 0.0000 2.7925 +0.0000 0.0000 2.8362 +0.0000 0.0000 2.8798 +0.0000 0.0000 2.9234 +0.0000 0.0000 2.9671 +0.0000 0.0000 3.0107 +0.0000 0.0000 3.0543 +0.0000 0.0000 3.0980 +0.0000 0.0000 3.1416 +primID: 0 +startangle_c: 8 +endpose_c: -1 0 8 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 3.1416 +-0.0011 0.0000 3.1416 +-0.0022 0.0000 3.1416 +-0.0033 0.0000 3.1416 +-0.0044 0.0000 3.1416 +-0.0056 0.0000 3.1416 +-0.0067 0.0000 3.1416 +-0.0078 0.0000 3.1416 +-0.0089 0.0000 3.1416 +-0.0100 0.0000 3.1416 +primID: 1 +startangle_c: 8 +endpose_c: -20 0 8 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 3.1416 +-0.0222 0.0000 3.1416 +-0.0444 0.0000 3.1416 +-0.0667 0.0000 3.1416 +-0.0889 0.0000 3.1416 +-0.1111 0.0000 3.1416 +-0.1333 0.0000 3.1416 +-0.1556 0.0000 3.1416 +-0.1778 0.0000 3.1416 +-0.2000 0.0000 3.1416 +primID: 2 +startangle_c: 8 +endpose_c: 1 0 8 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 3.1416 +0.0011 0.0000 3.1416 +0.0022 0.0000 3.1416 +0.0033 0.0000 3.1416 +0.0044 0.0000 3.1416 +0.0056 0.0000 3.1416 +0.0067 0.0000 3.1416 +0.0078 0.0000 3.1416 +0.0089 0.0000 3.1416 +0.0100 0.0000 3.1416 +primID: 3 +startangle_c: 8 +endpose_c: -40 -5 9 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 3.1416 +-0.0452 0.0000 3.1416 +-0.0904 0.0000 3.1416 +-0.1355 0.0000 3.1416 +-0.1807 -0.0008 3.1904 +-0.2257 -0.0045 3.2592 +-0.2703 -0.0114 3.3280 +-0.3144 -0.0213 3.3967 +-0.3577 -0.0342 3.4655 +-0.4000 -0.0500 3.5343 +primID: 4 +startangle_c: 8 +endpose_c: -40 5 7 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 3.1416 +-0.0452 0.0000 3.1416 +-0.0904 0.0000 3.1416 +-0.1355 0.0000 3.1416 +-0.1807 0.0008 3.0928 +-0.2257 0.0045 3.0240 +-0.2703 0.0114 2.9552 +-0.3144 0.0213 2.8864 +-0.3577 0.0342 2.8177 +-0.4000 0.0500 2.7489 +primID: 5 +startangle_c: 8 +endpose_c: 0 0 9 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 3.1416 +0.0000 0.0000 3.1852 +0.0000 0.0000 3.2289 +0.0000 0.0000 3.2725 +0.0000 0.0000 3.3161 +0.0000 0.0000 3.3598 +0.0000 0.0000 3.4034 +0.0000 0.0000 3.4470 +0.0000 0.0000 3.4907 +0.0000 0.0000 3.5343 +primID: 6 +startangle_c: 8 +endpose_c: 0 0 7 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 3.1416 +0.0000 0.0000 3.0980 +0.0000 0.0000 3.0543 +0.0000 0.0000 3.0107 +0.0000 0.0000 2.9671 +0.0000 0.0000 2.9234 +0.0000 0.0000 2.8798 +0.0000 0.0000 2.8362 +0.0000 0.0000 2.7925 +0.0000 0.0000 2.7489 +primID: 0 +startangle_c: 9 +endpose_c: -2 -1 9 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 3.5343 +-0.0022 -0.0011 3.5343 +-0.0044 -0.0022 3.5343 +-0.0067 -0.0033 3.5343 +-0.0089 -0.0044 3.5343 +-0.0111 -0.0056 3.5343 +-0.0133 -0.0067 3.5343 +-0.0156 -0.0078 3.5343 +-0.0178 -0.0089 3.5343 +-0.0200 -0.0100 3.5343 +primID: 1 +startangle_c: 9 +endpose_c: -20 -10 9 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 3.5343 +-0.0222 -0.0111 3.5343 +-0.0444 -0.0222 3.5343 +-0.0667 -0.0333 3.5343 +-0.0889 -0.0444 3.5343 +-0.1111 -0.0556 3.5343 +-0.1333 -0.0667 3.5343 +-0.1556 -0.0778 3.5343 +-0.1778 -0.0889 3.5343 +-0.2000 -0.1000 3.5343 +primID: 2 +startangle_c: 9 +endpose_c: 2 1 9 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 3.5343 +0.0022 0.0011 3.5343 +0.0044 0.0022 3.5343 +0.0067 0.0033 3.5343 +0.0089 0.0044 3.5343 +0.0111 0.0056 3.5343 +0.0133 0.0067 3.5343 +0.0156 0.0078 3.5343 +0.0178 0.0089 3.5343 +0.0200 0.0100 3.5343 +primID: 3 +startangle_c: 9 +endpose_c: -25 -20 10 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 3.5343 +-0.0298 -0.0184 3.5646 +-0.0592 -0.0379 3.5949 +-0.0881 -0.0583 3.6252 +-0.1165 -0.0796 3.6555 +-0.1444 -0.1019 3.6858 +-0.1717 -0.1251 3.7161 +-0.1984 -0.1492 3.7464 +-0.2245 -0.1742 3.7767 +-0.2500 -0.2000 3.8070 +primID: 4 +startangle_c: 9 +endpose_c: -35 -10 8 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 3.5343 +-0.0377 -0.0156 3.5343 +-0.0754 -0.0312 3.5343 +-0.1131 -0.0468 3.5343 +-0.1508 -0.0623 3.5152 +-0.1893 -0.0758 3.4405 +-0.2287 -0.0863 3.3658 +-0.2687 -0.0939 3.2910 +-0.3092 -0.0985 3.2163 +-0.3500 -0.1000 3.1416 +primID: 5 +startangle_c: 9 +endpose_c: 0 0 10 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 3.5343 +0.0000 0.0000 3.5779 +0.0000 0.0000 3.6216 +0.0000 0.0000 3.6652 +0.0000 0.0000 3.7088 +0.0000 0.0000 3.7525 +0.0000 0.0000 3.7961 +0.0000 0.0000 3.8397 +0.0000 0.0000 3.8834 +0.0000 0.0000 3.9270 +primID: 6 +startangle_c: 9 +endpose_c: 0 0 8 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 3.5343 +0.0000 0.0000 3.4907 +0.0000 0.0000 3.4470 +0.0000 0.0000 3.4034 +0.0000 0.0000 3.3598 +0.0000 0.0000 3.3161 +0.0000 0.0000 3.2725 +0.0000 0.0000 3.2289 +0.0000 0.0000 3.1852 +0.0000 0.0000 3.1416 +primID: 0 +startangle_c: 10 +endpose_c: -1 -1 10 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 3.9270 +-0.0011 -0.0011 3.9270 +-0.0022 -0.0022 3.9270 +-0.0033 -0.0033 3.9270 +-0.0044 -0.0044 3.9270 +-0.0056 -0.0056 3.9270 +-0.0067 -0.0067 3.9270 +-0.0078 -0.0078 3.9270 +-0.0089 -0.0089 3.9270 +-0.0100 -0.0100 3.9270 +primID: 1 +startangle_c: 10 +endpose_c: -10 -10 10 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 3.9270 +-0.0111 -0.0111 3.9270 +-0.0222 -0.0222 3.9270 +-0.0333 -0.0333 3.9270 +-0.0444 -0.0444 3.9270 +-0.0556 -0.0556 3.9270 +-0.0667 -0.0667 3.9270 +-0.0778 -0.0778 3.9270 +-0.0889 -0.0889 3.9270 +-0.1000 -0.1000 3.9270 +primID: 2 +startangle_c: 10 +endpose_c: 1 1 10 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 3.9270 +0.0011 0.0011 3.9270 +0.0022 0.0022 3.9270 +0.0033 0.0033 3.9270 +0.0044 0.0044 3.9270 +0.0056 0.0056 3.9270 +0.0067 0.0067 3.9270 +0.0078 0.0078 3.9270 +0.0089 0.0089 3.9270 +0.0100 0.0100 3.9270 +primID: 3 +startangle_c: 10 +endpose_c: -25 -35 11 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 3.9270 +-0.0341 -0.0341 3.9270 +-0.0678 -0.0684 3.9567 +-0.1000 -0.1043 4.0085 +-0.1302 -0.1418 4.0604 +-0.1584 -0.1809 4.1123 +-0.1846 -0.2213 4.1641 +-0.2086 -0.2631 4.2160 +-0.2304 -0.3060 4.2678 +-0.2500 -0.3500 4.3197 +primID: 4 +startangle_c: 10 +endpose_c: -35 -25 9 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 3.9270 +-0.0341 -0.0341 3.9270 +-0.0684 -0.0678 3.8973 +-0.1043 -0.1000 3.8455 +-0.1418 -0.1302 3.7936 +-0.1809 -0.1584 3.7417 +-0.2213 -0.1846 3.6899 +-0.2631 -0.2086 3.6380 +-0.3060 -0.2304 3.5862 +-0.3500 -0.2500 3.5343 +primID: 5 +startangle_c: 10 +endpose_c: 0 0 11 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 3.9270 +0.0000 0.0000 3.9706 +0.0000 0.0000 4.0143 +0.0000 0.0000 4.0579 +0.0000 0.0000 4.1015 +0.0000 0.0000 4.1452 +0.0000 0.0000 4.1888 +0.0000 0.0000 4.2324 +0.0000 0.0000 4.2761 +0.0000 0.0000 4.3197 +primID: 6 +startangle_c: 10 +endpose_c: 0 0 9 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 3.9270 +0.0000 0.0000 3.8834 +0.0000 0.0000 3.8397 +0.0000 0.0000 3.7961 +0.0000 0.0000 3.7525 +0.0000 0.0000 3.7088 +0.0000 0.0000 3.6652 +0.0000 0.0000 3.6216 +0.0000 0.0000 3.5779 +0.0000 0.0000 3.5343 +primID: 0 +startangle_c: 11 +endpose_c: -1 -2 11 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 4.3197 +-0.0011 -0.0022 4.3197 +-0.0022 -0.0044 4.3197 +-0.0033 -0.0067 4.3197 +-0.0044 -0.0089 4.3197 +-0.0056 -0.0111 4.3197 +-0.0067 -0.0133 4.3197 +-0.0078 -0.0156 4.3197 +-0.0089 -0.0178 4.3197 +-0.0100 -0.0200 4.3197 +primID: 1 +startangle_c: 11 +endpose_c: -10 -20 11 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 4.3197 +-0.0111 -0.0222 4.3197 +-0.0222 -0.0444 4.3197 +-0.0333 -0.0667 4.3197 +-0.0444 -0.0889 4.3197 +-0.0556 -0.1111 4.3197 +-0.0667 -0.1333 4.3197 +-0.0778 -0.1556 4.3197 +-0.0889 -0.1778 4.3197 +-0.1000 -0.2000 4.3197 +primID: 2 +startangle_c: 11 +endpose_c: 1 2 11 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 4.3197 +0.0011 0.0022 4.3197 +0.0022 0.0044 4.3197 +0.0033 0.0067 4.3197 +0.0044 0.0089 4.3197 +0.0056 0.0111 4.3197 +0.0067 0.0133 4.3197 +0.0078 0.0156 4.3197 +0.0089 0.0178 4.3197 +0.0100 0.0200 4.3197 +primID: 3 +startangle_c: 11 +endpose_c: -20 -25 10 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 4.3197 +-0.0184 -0.0298 4.2894 +-0.0379 -0.0592 4.2591 +-0.0583 -0.0881 4.2288 +-0.0796 -0.1165 4.1985 +-0.1019 -0.1444 4.1682 +-0.1251 -0.1717 4.1379 +-0.1492 -0.1984 4.1076 +-0.1742 -0.2245 4.0773 +-0.2000 -0.2500 4.0470 +primID: 4 +startangle_c: 11 +endpose_c: -10 -35 12 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 4.3197 +-0.0156 -0.0377 4.3197 +-0.0312 -0.0754 4.3197 +-0.0468 -0.1131 4.3197 +-0.0623 -0.1508 4.3388 +-0.0758 -0.1893 4.4135 +-0.0863 -0.2287 4.4882 +-0.0939 -0.2687 4.5629 +-0.0985 -0.3092 4.6377 +-0.1000 -0.3500 4.7124 +primID: 5 +startangle_c: 11 +endpose_c: 0 0 10 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 4.3197 +0.0000 0.0000 4.2761 +0.0000 0.0000 4.2324 +0.0000 0.0000 4.1888 +0.0000 0.0000 4.1452 +0.0000 0.0000 4.1015 +0.0000 0.0000 4.0579 +0.0000 0.0000 4.0143 +0.0000 0.0000 3.9706 +0.0000 0.0000 3.9270 +primID: 6 +startangle_c: 11 +endpose_c: 0 0 12 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 4.3197 +0.0000 0.0000 4.3633 +0.0000 0.0000 4.4070 +0.0000 0.0000 4.4506 +0.0000 0.0000 4.4942 +0.0000 0.0000 4.5379 +0.0000 0.0000 4.5815 +0.0000 0.0000 4.6251 +0.0000 0.0000 4.6688 +0.0000 0.0000 4.7124 +primID: 0 +startangle_c: 12 +endpose_c: 0 -1 12 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 4.7124 +0.0000 -0.0011 4.7124 +0.0000 -0.0022 4.7124 +0.0000 -0.0033 4.7124 +0.0000 -0.0044 4.7124 +0.0000 -0.0056 4.7124 +0.0000 -0.0067 4.7124 +0.0000 -0.0078 4.7124 +0.0000 -0.0089 4.7124 +0.0000 -0.0100 4.7124 +primID: 1 +startangle_c: 12 +endpose_c: 0 -20 12 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 4.7124 +0.0000 -0.0222 4.7124 +0.0000 -0.0444 4.7124 +0.0000 -0.0667 4.7124 +0.0000 -0.0889 4.7124 +0.0000 -0.1111 4.7124 +0.0000 -0.1333 4.7124 +0.0000 -0.1556 4.7124 +0.0000 -0.1778 4.7124 +0.0000 -0.2000 4.7124 +primID: 2 +startangle_c: 12 +endpose_c: 0 1 12 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 4.7124 +0.0000 0.0011 4.7124 +0.0000 0.0022 4.7124 +0.0000 0.0033 4.7124 +0.0000 0.0044 4.7124 +0.0000 0.0056 4.7124 +0.0000 0.0067 4.7124 +0.0000 0.0078 4.7124 +0.0000 0.0089 4.7124 +0.0000 0.0100 4.7124 +primID: 3 +startangle_c: 12 +endpose_c: 5 -40 13 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 4.7124 +0.0000 -0.0452 4.7124 +0.0000 -0.0904 4.7124 +0.0000 -0.1355 4.7124 +0.0008 -0.1807 4.7612 +0.0045 -0.2257 4.8300 +0.0114 -0.2703 4.8988 +0.0213 -0.3144 4.9675 +0.0342 -0.3577 5.0363 +0.0500 -0.4000 5.1051 +primID: 4 +startangle_c: 12 +endpose_c: -5 -40 11 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 4.7124 +-0.0000 -0.0452 4.7124 +-0.0000 -0.0904 4.7124 +-0.0000 -0.1355 4.7124 +-0.0008 -0.1807 4.6636 +-0.0045 -0.2257 4.5948 +-0.0114 -0.2703 4.5260 +-0.0213 -0.3144 4.4572 +-0.0342 -0.3577 4.3885 +-0.0500 -0.4000 4.3197 +primID: 5 +startangle_c: 12 +endpose_c: 0 0 13 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 4.7124 +0.0000 0.0000 4.7560 +0.0000 0.0000 4.7997 +0.0000 0.0000 4.8433 +0.0000 0.0000 4.8869 +0.0000 0.0000 4.9306 +0.0000 0.0000 4.9742 +0.0000 0.0000 5.0178 +0.0000 0.0000 5.0615 +0.0000 0.0000 5.1051 +primID: 6 +startangle_c: 12 +endpose_c: 0 0 11 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 4.7124 +0.0000 0.0000 4.6688 +0.0000 0.0000 4.6251 +0.0000 0.0000 4.5815 +0.0000 0.0000 4.5379 +0.0000 0.0000 4.4942 +0.0000 0.0000 4.4506 +0.0000 0.0000 4.4070 +0.0000 0.0000 4.3633 +0.0000 0.0000 4.3197 +primID: 0 +startangle_c: 13 +endpose_c: 1 -2 13 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 5.1051 +0.0011 -0.0022 5.1051 +0.0022 -0.0044 5.1051 +0.0033 -0.0067 5.1051 +0.0044 -0.0089 5.1051 +0.0056 -0.0111 5.1051 +0.0067 -0.0133 5.1051 +0.0078 -0.0156 5.1051 +0.0089 -0.0178 5.1051 +0.0100 -0.0200 5.1051 +primID: 1 +startangle_c: 13 +endpose_c: 10 -20 13 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 5.1051 +0.0111 -0.0222 5.1051 +0.0222 -0.0444 5.1051 +0.0333 -0.0667 5.1051 +0.0444 -0.0889 5.1051 +0.0556 -0.1111 5.1051 +0.0667 -0.1333 5.1051 +0.0778 -0.1556 5.1051 +0.0889 -0.1778 5.1051 +0.1000 -0.2000 5.1051 +primID: 2 +startangle_c: 13 +endpose_c: -1 2 13 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 5.1051 +-0.0011 0.0022 5.1051 +-0.0022 0.0044 5.1051 +-0.0033 0.0067 5.1051 +-0.0044 0.0089 5.1051 +-0.0056 0.0111 5.1051 +-0.0067 0.0133 5.1051 +-0.0078 0.0156 5.1051 +-0.0089 0.0178 5.1051 +-0.0100 0.0200 5.1051 +primID: 3 +startangle_c: 13 +endpose_c: 20 -25 14 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 5.1051 +0.0184 -0.0298 5.1354 +0.0379 -0.0592 5.1657 +0.0583 -0.0881 5.1960 +0.0796 -0.1165 5.2263 +0.1019 -0.1444 5.2566 +0.1251 -0.1717 5.2869 +0.1492 -0.1984 5.3172 +0.1742 -0.2245 5.3475 +0.2000 -0.2500 5.3778 +primID: 4 +startangle_c: 13 +endpose_c: 10 -35 12 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 5.1051 +0.0156 -0.0377 5.1051 +0.0312 -0.0754 5.1051 +0.0468 -0.1131 5.1051 +0.0623 -0.1508 5.0860 +0.0758 -0.1893 5.0113 +0.0863 -0.2287 4.9366 +0.0939 -0.2687 4.8618 +0.0985 -0.3092 4.7871 +0.1000 -0.3500 4.7124 +primID: 5 +startangle_c: 13 +endpose_c: 0 0 14 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 5.1051 +0.0000 0.0000 5.1487 +0.0000 0.0000 5.1924 +0.0000 0.0000 5.2360 +0.0000 0.0000 5.2796 +0.0000 0.0000 5.3233 +0.0000 0.0000 5.3669 +0.0000 0.0000 5.4105 +0.0000 0.0000 5.4542 +0.0000 0.0000 5.4978 +primID: 6 +startangle_c: 13 +endpose_c: 0 0 12 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 5.1051 +0.0000 0.0000 5.0615 +0.0000 0.0000 5.0178 +0.0000 0.0000 4.9742 +0.0000 0.0000 4.9306 +0.0000 0.0000 4.8869 +0.0000 0.0000 4.8433 +0.0000 0.0000 4.7997 +0.0000 0.0000 4.7560 +0.0000 0.0000 4.7124 +primID: 0 +startangle_c: 14 +endpose_c: 1 -1 14 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 5.4978 +0.0011 -0.0011 5.4978 +0.0022 -0.0022 5.4978 +0.0033 -0.0033 5.4978 +0.0044 -0.0044 5.4978 +0.0056 -0.0056 5.4978 +0.0067 -0.0067 5.4978 +0.0078 -0.0078 5.4978 +0.0089 -0.0089 5.4978 +0.0100 -0.0100 5.4978 +primID: 1 +startangle_c: 14 +endpose_c: 10 -10 14 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 5.4978 +0.0111 -0.0111 5.4978 +0.0222 -0.0222 5.4978 +0.0333 -0.0333 5.4978 +0.0444 -0.0444 5.4978 +0.0556 -0.0556 5.4978 +0.0667 -0.0667 5.4978 +0.0778 -0.0778 5.4978 +0.0889 -0.0889 5.4978 +0.1000 -0.1000 5.4978 +primID: 2 +startangle_c: 14 +endpose_c: -1 1 14 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 5.4978 +-0.0011 0.0011 5.4978 +-0.0022 0.0022 5.4978 +-0.0033 0.0033 5.4978 +-0.0044 0.0044 5.4978 +-0.0056 0.0056 5.4978 +-0.0067 0.0067 5.4978 +-0.0078 0.0078 5.4978 +-0.0089 0.0089 5.4978 +-0.0100 0.0100 5.4978 +primID: 3 +startangle_c: 14 +endpose_c: 35 -25 15 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 5.4978 +0.0341 -0.0341 5.4978 +0.0684 -0.0678 5.5275 +0.1043 -0.1000 5.5793 +0.1418 -0.1302 5.6312 +0.1809 -0.1584 5.6830 +0.2213 -0.1846 5.7349 +0.2631 -0.2086 5.7868 +0.3060 -0.2304 5.8386 +0.3500 -0.2500 5.8905 +primID: 4 +startangle_c: 14 +endpose_c: 25 -35 13 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 5.4978 +0.0341 -0.0341 5.4978 +0.0678 -0.0684 5.4681 +0.1000 -0.1043 5.4162 +0.1302 -0.1418 5.3644 +0.1584 -0.1809 5.3125 +0.1846 -0.2213 5.2607 +0.2086 -0.2631 5.2088 +0.2304 -0.3060 5.1569 +0.2500 -0.3500 5.1051 +primID: 5 +startangle_c: 14 +endpose_c: 0 0 15 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 5.4978 +0.0000 0.0000 5.5414 +0.0000 0.0000 5.5851 +0.0000 0.0000 5.6287 +0.0000 0.0000 5.6723 +0.0000 0.0000 5.7160 +0.0000 0.0000 5.7596 +0.0000 0.0000 5.8032 +0.0000 0.0000 5.8469 +0.0000 0.0000 5.8905 +primID: 6 +startangle_c: 14 +endpose_c: 0 0 13 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 5.4978 +0.0000 0.0000 5.4542 +0.0000 0.0000 5.4105 +0.0000 0.0000 5.3669 +0.0000 0.0000 5.3233 +0.0000 0.0000 5.2796 +0.0000 0.0000 5.2360 +0.0000 0.0000 5.1924 +0.0000 0.0000 5.1487 +0.0000 0.0000 5.1051 +primID: 0 +startangle_c: 15 +endpose_c: 2 -1 15 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 5.8905 +0.0022 -0.0011 5.8905 +0.0044 -0.0022 5.8905 +0.0067 -0.0033 5.8905 +0.0089 -0.0044 5.8905 +0.0111 -0.0056 5.8905 +0.0133 -0.0067 5.8905 +0.0156 -0.0078 5.8905 +0.0178 -0.0089 5.8905 +0.0200 -0.0100 5.8905 +primID: 1 +startangle_c: 15 +endpose_c: 20 -10 15 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 5.8905 +0.0222 -0.0111 5.8905 +0.0444 -0.0222 5.8905 +0.0667 -0.0333 5.8905 +0.0889 -0.0444 5.8905 +0.1111 -0.0556 5.8905 +0.1333 -0.0667 5.8905 +0.1556 -0.0778 5.8905 +0.1778 -0.0889 5.8905 +0.2000 -0.1000 5.8905 +primID: 2 +startangle_c: 15 +endpose_c: -2 1 15 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 5.8905 +-0.0022 0.0011 5.8905 +-0.0044 0.0022 5.8905 +-0.0067 0.0033 5.8905 +-0.0089 0.0044 5.8905 +-0.0111 0.0056 5.8905 +-0.0133 0.0067 5.8905 +-0.0156 0.0078 5.8905 +-0.0178 0.0089 5.8905 +-0.0200 0.0100 5.8905 +primID: 3 +startangle_c: 15 +endpose_c: 25 -20 14 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 5.8905 +0.0298 -0.0184 5.8602 +0.0592 -0.0379 5.8299 +0.0881 -0.0583 5.7996 +0.1165 -0.0796 5.7693 +0.1444 -0.1019 5.7390 +0.1717 -0.1251 5.7087 +0.1984 -0.1492 5.6784 +0.2245 -0.1742 5.6481 +0.2500 -0.2000 5.6178 +primID: 4 +startangle_c: 15 +endpose_c: 35 -10 0 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 5.8905 +0.0377 -0.0156 5.8905 +0.0754 -0.0312 5.8905 +0.1131 -0.0468 5.8905 +0.1508 -0.0623 5.9096 +0.1893 -0.0758 5.9843 +0.2287 -0.0863 6.0590 +0.2687 -0.0939 6.1337 +0.3092 -0.0985 6.2085 +0.3500 -0.1000 6.2832 +primID: 5 +startangle_c: 15 +endpose_c: 0 0 14 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 5.8905 +0.0000 0.0000 5.8469 +0.0000 0.0000 5.8032 +0.0000 0.0000 5.7596 +0.0000 0.0000 5.7160 +0.0000 0.0000 5.6723 +0.0000 0.0000 5.6287 +0.0000 0.0000 5.5851 +0.0000 0.0000 5.5414 +0.0000 0.0000 5.4978 +primID: 6 +startangle_c: 15 +endpose_c: 0 0 0 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 5.8905 +0.0000 0.0000 5.2360 +0.0000 0.0000 4.5815 +0.0000 0.0000 3.9270 +0.0000 0.0000 3.2725 +0.0000 0.0000 2.6180 +0.0000 0.0000 1.9635 +0.0000 0.0000 1.3090 +0.0000 0.0000 0.6545 +0.0000 0.0000 0.0000 diff --git a/Controllers/Packages/amr_startup/config/mprim/unicycle_highcost_2_5cm.mprim b/Controllers/Packages/amr_startup/config/mprim/unicycle_highcost_2_5cm.mprim new file mode 100755 index 0000000..6bf962e --- /dev/null +++ b/Controllers/Packages/amr_startup/config/mprim/unicycle_highcost_2_5cm.mprim @@ -0,0 +1,1683 @@ +resolution_m: 0.025000 +numberofangles: 16 +totalnumberofprimitives: 112 +primID: 0 +startangle_c: 0 +endpose_c: 1 0 0 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 0.0000 +0.0028 0.0000 0.0000 +0.0056 0.0000 0.0000 +0.0083 0.0000 0.0000 +0.0111 0.0000 0.0000 +0.0139 0.0000 0.0000 +0.0167 0.0000 0.0000 +0.0194 0.0000 0.0000 +0.0222 0.0000 0.0000 +0.0250 0.0000 0.0000 +primID: 1 +startangle_c: 0 +endpose_c: 12 0 0 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 0.0000 +0.0333 0.0000 0.0000 +0.0667 0.0000 0.0000 +0.1000 0.0000 0.0000 +0.1333 0.0000 0.0000 +0.1667 0.0000 0.0000 +0.2000 0.0000 0.0000 +0.2333 0.0000 0.0000 +0.2667 0.0000 0.0000 +0.3000 0.0000 0.0000 +primID: 2 +startangle_c: 0 +endpose_c: -1 0 0 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 0.0000 +-0.0028 0.0000 0.0000 +-0.0056 0.0000 0.0000 +-0.0083 0.0000 0.0000 +-0.0111 0.0000 0.0000 +-0.0139 0.0000 0.0000 +-0.0167 0.0000 0.0000 +-0.0194 0.0000 0.0000 +-0.0222 0.0000 0.0000 +-0.0250 0.0000 0.0000 +primID: 3 +startangle_c: 0 +endpose_c: 16 2 1 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 0.0000 +0.0452 -0.0000 0.0000 +0.0904 -0.0000 0.0000 +0.1355 -0.0000 0.0000 +0.1807 0.0008 0.0488 +0.2257 0.0045 0.1176 +0.2703 0.0114 0.1864 +0.3144 0.0213 0.2551 +0.3577 0.0342 0.3239 +0.4000 0.0500 0.3927 +primID: 4 +startangle_c: 0 +endpose_c: 16 -2 -1 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 0.0000 +0.0452 0.0000 0.0000 +0.0904 0.0000 0.0000 +0.1355 0.0000 0.0000 +0.1807 -0.0008 -0.0488 +0.2257 -0.0045 -0.1176 +0.2703 -0.0114 -0.1864 +0.3144 -0.0213 -0.2551 +0.3577 -0.0342 -0.3239 +0.4000 -0.0500 -0.3927 +primID: 5 +startangle_c: 0 +endpose_c: 0 0 1 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 0.0000 +0.0000 0.0000 0.0436 +0.0000 0.0000 0.0873 +0.0000 0.0000 0.1309 +0.0000 0.0000 0.1745 +0.0000 0.0000 0.2182 +0.0000 0.0000 0.2618 +0.0000 0.0000 0.3054 +0.0000 0.0000 0.3491 +0.0000 0.0000 0.3927 +primID: 6 +startangle_c: 0 +endpose_c: 0 0 -1 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 0.0000 +0.0000 0.0000 -0.0436 +0.0000 0.0000 -0.0873 +0.0000 0.0000 -0.1309 +0.0000 0.0000 -0.1745 +0.0000 0.0000 -0.2182 +0.0000 0.0000 -0.2618 +0.0000 0.0000 -0.3054 +0.0000 0.0000 -0.3491 +0.0000 0.0000 -0.3927 +primID: 0 +startangle_c: 1 +endpose_c: 2 1 1 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 0.3927 +0.0056 0.0028 0.3927 +0.0111 0.0056 0.3927 +0.0167 0.0083 0.3927 +0.0222 0.0111 0.3927 +0.0278 0.0139 0.3927 +0.0333 0.0167 0.3927 +0.0389 0.0194 0.3927 +0.0444 0.0222 0.3927 +0.0500 0.0250 0.3927 +primID: 1 +startangle_c: 1 +endpose_c: 12 6 1 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 0.3927 +0.0333 0.0167 0.3927 +0.0667 0.0333 0.3927 +0.1000 0.0500 0.3927 +0.1333 0.0667 0.3927 +0.1667 0.0833 0.3927 +0.2000 0.1000 0.3927 +0.2333 0.1167 0.3927 +0.2667 0.1333 0.3927 +0.3000 0.1500 0.3927 +primID: 2 +startangle_c: 1 +endpose_c: -2 -1 1 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 0.3927 +-0.0056 -0.0028 0.3927 +-0.0111 -0.0056 0.3927 +-0.0167 -0.0083 0.3927 +-0.0222 -0.0111 0.3927 +-0.0278 -0.0139 0.3927 +-0.0333 -0.0167 0.3927 +-0.0389 -0.0194 0.3927 +-0.0444 -0.0222 0.3927 +-0.0500 -0.0250 0.3927 +primID: 3 +startangle_c: 1 +endpose_c: 10 8 2 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 0.3927 +0.0298 0.0184 0.4230 +0.0592 0.0379 0.4533 +0.0881 0.0583 0.4836 +0.1165 0.0796 0.5139 +0.1444 0.1019 0.5442 +0.1717 0.1251 0.5745 +0.1984 0.1492 0.6048 +0.2245 0.1742 0.6351 +0.2500 0.2000 0.6654 +primID: 4 +startangle_c: 1 +endpose_c: 14 4 0 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 0.3927 +0.0377 0.0156 0.3927 +0.0754 0.0312 0.3927 +0.1131 0.0468 0.3927 +0.1508 0.0623 0.3736 +0.1893 0.0758 0.2989 +0.2287 0.0863 0.2242 +0.2687 0.0939 0.1494 +0.3092 0.0985 0.0747 +0.3500 0.1000 -0.0000 +primID: 5 +startangle_c: 1 +endpose_c: 0 0 2 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 0.3927 +0.0000 0.0000 0.4363 +0.0000 0.0000 0.4800 +0.0000 0.0000 0.5236 +0.0000 0.0000 0.5672 +0.0000 0.0000 0.6109 +0.0000 0.0000 0.6545 +0.0000 0.0000 0.6981 +0.0000 0.0000 0.7418 +0.0000 0.0000 0.7854 +primID: 6 +startangle_c: 1 +endpose_c: 0 0 0 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 0.3927 +0.0000 0.0000 0.3491 +0.0000 0.0000 0.3054 +0.0000 0.0000 0.2618 +0.0000 0.0000 0.2182 +0.0000 0.0000 0.1745 +0.0000 0.0000 0.1309 +0.0000 0.0000 0.0873 +0.0000 0.0000 0.0436 +0.0000 0.0000 0.0000 +primID: 0 +startangle_c: 2 +endpose_c: 1 1 2 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 0.7854 +0.0028 0.0028 0.7854 +0.0056 0.0056 0.7854 +0.0083 0.0083 0.7854 +0.0111 0.0111 0.7854 +0.0139 0.0139 0.7854 +0.0167 0.0167 0.7854 +0.0194 0.0194 0.7854 +0.0222 0.0222 0.7854 +0.0250 0.0250 0.7854 +primID: 1 +startangle_c: 2 +endpose_c: 12 12 2 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 0.7854 +0.0333 0.0333 0.7854 +0.0667 0.0667 0.7854 +0.1000 0.1000 0.7854 +0.1333 0.1333 0.7854 +0.1667 0.1667 0.7854 +0.2000 0.2000 0.7854 +0.2333 0.2333 0.7854 +0.2667 0.2667 0.7854 +0.3000 0.3000 0.7854 +primID: 2 +startangle_c: 2 +endpose_c: -1 -1 2 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 0.7854 +-0.0028 -0.0028 0.7854 +-0.0056 -0.0056 0.7854 +-0.0083 -0.0083 0.7854 +-0.0111 -0.0111 0.7854 +-0.0139 -0.0139 0.7854 +-0.0167 -0.0167 0.7854 +-0.0194 -0.0194 0.7854 +-0.0222 -0.0222 0.7854 +-0.0250 -0.0250 0.7854 +primID: 3 +startangle_c: 2 +endpose_c: 10 14 3 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 0.7854 +0.0341 0.0341 0.7854 +0.0678 0.0684 0.8151 +0.1000 0.1043 0.8669 +0.1302 0.1418 0.9188 +0.1584 0.1809 0.9707 +0.1846 0.2213 1.0225 +0.2086 0.2631 1.0744 +0.2304 0.3060 1.1262 +0.2500 0.3500 1.1781 +primID: 4 +startangle_c: 2 +endpose_c: 14 10 1 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 0.7854 +0.0341 0.0341 0.7854 +0.0684 0.0678 0.7557 +0.1043 0.1000 0.7039 +0.1418 0.1302 0.6520 +0.1809 0.1584 0.6001 +0.2213 0.1846 0.5483 +0.2631 0.2086 0.4964 +0.3060 0.2304 0.4446 +0.3500 0.2500 0.3927 +primID: 5 +startangle_c: 2 +endpose_c: 0 0 3 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 0.7854 +0.0000 0.0000 0.8290 +0.0000 0.0000 0.8727 +0.0000 0.0000 0.9163 +0.0000 0.0000 0.9599 +0.0000 0.0000 1.0036 +0.0000 0.0000 1.0472 +0.0000 0.0000 1.0908 +0.0000 0.0000 1.1345 +0.0000 0.0000 1.1781 +primID: 6 +startangle_c: 2 +endpose_c: 0 0 1 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 0.7854 +0.0000 0.0000 0.7418 +0.0000 0.0000 0.6981 +0.0000 0.0000 0.6545 +0.0000 0.0000 0.6109 +0.0000 0.0000 0.5672 +0.0000 0.0000 0.5236 +0.0000 0.0000 0.4800 +0.0000 0.0000 0.4363 +0.0000 0.0000 0.3927 +primID: 0 +startangle_c: 3 +endpose_c: 1 2 3 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 1.1781 +0.0028 0.0056 1.1781 +0.0056 0.0111 1.1781 +0.0083 0.0167 1.1781 +0.0111 0.0222 1.1781 +0.0139 0.0278 1.1781 +0.0167 0.0333 1.1781 +0.0194 0.0389 1.1781 +0.0222 0.0444 1.1781 +0.0250 0.0500 1.1781 +primID: 1 +startangle_c: 3 +endpose_c: 6 12 3 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 1.1781 +0.0167 0.0333 1.1781 +0.0333 0.0667 1.1781 +0.0500 0.1000 1.1781 +0.0667 0.1333 1.1781 +0.0833 0.1667 1.1781 +0.1000 0.2000 1.1781 +0.1167 0.2333 1.1781 +0.1333 0.2667 1.1781 +0.1500 0.3000 1.1781 +primID: 2 +startangle_c: 3 +endpose_c: -1 -2 3 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 1.1781 +-0.0028 -0.0056 1.1781 +-0.0056 -0.0111 1.1781 +-0.0083 -0.0167 1.1781 +-0.0111 -0.0222 1.1781 +-0.0139 -0.0278 1.1781 +-0.0167 -0.0333 1.1781 +-0.0194 -0.0389 1.1781 +-0.0222 -0.0444 1.1781 +-0.0250 -0.0500 1.1781 +primID: 3 +startangle_c: 3 +endpose_c: 8 10 2 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 1.1781 +0.0184 0.0298 1.1478 +0.0379 0.0592 1.1175 +0.0583 0.0881 1.0872 +0.0796 0.1165 1.0569 +0.1019 0.1444 1.0266 +0.1251 0.1717 0.9963 +0.1492 0.1984 0.9660 +0.1742 0.2245 0.9357 +0.2000 0.2500 0.9054 +primID: 4 +startangle_c: 3 +endpose_c: 4 14 4 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 1.1781 +0.0156 0.0377 1.1781 +0.0312 0.0754 1.1781 +0.0468 0.1131 1.1781 +0.0623 0.1508 1.1972 +0.0758 0.1893 1.2719 +0.0863 0.2287 1.3466 +0.0939 0.2687 1.4214 +0.0985 0.3092 1.4961 +0.1000 0.3500 1.5708 +primID: 5 +startangle_c: 3 +endpose_c: 0 0 2 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 1.1781 +0.0000 0.0000 1.1345 +0.0000 0.0000 1.0908 +0.0000 0.0000 1.0472 +0.0000 0.0000 1.0036 +0.0000 0.0000 0.9599 +0.0000 0.0000 0.9163 +0.0000 0.0000 0.8727 +0.0000 0.0000 0.8290 +0.0000 0.0000 0.7854 +primID: 6 +startangle_c: 3 +endpose_c: 0 0 4 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 1.1781 +0.0000 0.0000 1.2217 +0.0000 0.0000 1.2654 +0.0000 0.0000 1.3090 +0.0000 0.0000 1.3526 +0.0000 0.0000 1.3963 +0.0000 0.0000 1.4399 +0.0000 0.0000 1.4835 +0.0000 0.0000 1.5272 +0.0000 0.0000 1.5708 +primID: 0 +startangle_c: 4 +endpose_c: 0 1 4 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 1.5708 +0.0000 0.0028 1.5708 +0.0000 0.0056 1.5708 +0.0000 0.0083 1.5708 +0.0000 0.0111 1.5708 +0.0000 0.0139 1.5708 +0.0000 0.0167 1.5708 +0.0000 0.0194 1.5708 +0.0000 0.0222 1.5708 +0.0000 0.0250 1.5708 +primID: 1 +startangle_c: 4 +endpose_c: 0 12 4 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 1.5708 +0.0000 0.0333 1.5708 +0.0000 0.0667 1.5708 +0.0000 0.1000 1.5708 +0.0000 0.1333 1.5708 +0.0000 0.1667 1.5708 +0.0000 0.2000 1.5708 +0.0000 0.2333 1.5708 +0.0000 0.2667 1.5708 +0.0000 0.3000 1.5708 +primID: 2 +startangle_c: 4 +endpose_c: 0 -1 4 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 1.5708 +0.0000 -0.0028 1.5708 +0.0000 -0.0056 1.5708 +0.0000 -0.0083 1.5708 +0.0000 -0.0111 1.5708 +0.0000 -0.0139 1.5708 +0.0000 -0.0167 1.5708 +0.0000 -0.0194 1.5708 +0.0000 -0.0222 1.5708 +0.0000 -0.0250 1.5708 +primID: 3 +startangle_c: 4 +endpose_c: -2 16 5 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 1.5708 +0.0000 0.0452 1.5708 +0.0000 0.0904 1.5708 +0.0000 0.1355 1.5708 +-0.0008 0.1807 1.6196 +-0.0045 0.2257 1.6884 +-0.0114 0.2703 1.7572 +-0.0213 0.3144 1.8259 +-0.0342 0.3577 1.8947 +-0.0500 0.4000 1.9635 +primID: 4 +startangle_c: 4 +endpose_c: 2 16 3 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 1.5708 +0.0000 0.0452 1.5708 +0.0000 0.0904 1.5708 +0.0000 0.1355 1.5708 +0.0008 0.1807 1.5220 +0.0045 0.2257 1.4532 +0.0114 0.2703 1.3844 +0.0213 0.3144 1.3156 +0.0342 0.3577 1.2469 +0.0500 0.4000 1.1781 +primID: 5 +startangle_c: 4 +endpose_c: 0 0 5 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 1.5708 +0.0000 0.0000 1.6144 +0.0000 0.0000 1.6581 +0.0000 0.0000 1.7017 +0.0000 0.0000 1.7453 +0.0000 0.0000 1.7890 +0.0000 0.0000 1.8326 +0.0000 0.0000 1.8762 +0.0000 0.0000 1.9199 +0.0000 0.0000 1.9635 +primID: 6 +startangle_c: 4 +endpose_c: 0 0 3 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 1.5708 +0.0000 0.0000 1.5272 +0.0000 0.0000 1.4835 +0.0000 0.0000 1.4399 +0.0000 0.0000 1.3963 +0.0000 0.0000 1.3526 +0.0000 0.0000 1.3090 +0.0000 0.0000 1.2654 +0.0000 0.0000 1.2217 +0.0000 0.0000 1.1781 +primID: 0 +startangle_c: 5 +endpose_c: -1 2 5 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 1.9635 +-0.0028 0.0056 1.9635 +-0.0056 0.0111 1.9635 +-0.0083 0.0167 1.9635 +-0.0111 0.0222 1.9635 +-0.0139 0.0278 1.9635 +-0.0167 0.0333 1.9635 +-0.0194 0.0389 1.9635 +-0.0222 0.0444 1.9635 +-0.0250 0.0500 1.9635 +primID: 1 +startangle_c: 5 +endpose_c: -6 12 5 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 1.9635 +-0.0167 0.0333 1.9635 +-0.0333 0.0667 1.9635 +-0.0500 0.1000 1.9635 +-0.0667 0.1333 1.9635 +-0.0833 0.1667 1.9635 +-0.1000 0.2000 1.9635 +-0.1167 0.2333 1.9635 +-0.1333 0.2667 1.9635 +-0.1500 0.3000 1.9635 +primID: 2 +startangle_c: 5 +endpose_c: 1 -2 5 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 1.9635 +0.0028 -0.0056 1.9635 +0.0056 -0.0111 1.9635 +0.0083 -0.0167 1.9635 +0.0111 -0.0222 1.9635 +0.0139 -0.0278 1.9635 +0.0167 -0.0333 1.9635 +0.0194 -0.0389 1.9635 +0.0222 -0.0444 1.9635 +0.0250 -0.0500 1.9635 +primID: 3 +startangle_c: 5 +endpose_c: -8 10 6 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 1.9635 +-0.0184 0.0298 1.9938 +-0.0379 0.0592 2.0241 +-0.0583 0.0881 2.0544 +-0.0796 0.1165 2.0847 +-0.1019 0.1444 2.1150 +-0.1251 0.1717 2.1453 +-0.1492 0.1984 2.1756 +-0.1742 0.2245 2.2059 +-0.2000 0.2500 2.2362 +primID: 4 +startangle_c: 5 +endpose_c: -4 14 4 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 1.9635 +-0.0156 0.0377 1.9635 +-0.0312 0.0754 1.9635 +-0.0468 0.1131 1.9635 +-0.0623 0.1508 1.9444 +-0.0758 0.1893 1.8697 +-0.0863 0.2287 1.7950 +-0.0939 0.2687 1.7202 +-0.0985 0.3092 1.6455 +-0.1000 0.3500 1.5708 +primID: 5 +startangle_c: 5 +endpose_c: 0 0 6 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 1.9635 +0.0000 0.0000 2.0071 +0.0000 0.0000 2.0508 +0.0000 0.0000 2.0944 +0.0000 0.0000 2.1380 +0.0000 0.0000 2.1817 +0.0000 0.0000 2.2253 +0.0000 0.0000 2.2689 +0.0000 0.0000 2.3126 +0.0000 0.0000 2.3562 +primID: 6 +startangle_c: 5 +endpose_c: 0 0 4 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 1.9635 +0.0000 0.0000 1.9199 +0.0000 0.0000 1.8762 +0.0000 0.0000 1.8326 +0.0000 0.0000 1.7890 +0.0000 0.0000 1.7453 +0.0000 0.0000 1.7017 +0.0000 0.0000 1.6581 +0.0000 0.0000 1.6144 +0.0000 0.0000 1.5708 +primID: 0 +startangle_c: 6 +endpose_c: -1 1 6 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 2.3562 +-0.0028 0.0028 2.3562 +-0.0056 0.0056 2.3562 +-0.0083 0.0083 2.3562 +-0.0111 0.0111 2.3562 +-0.0139 0.0139 2.3562 +-0.0167 0.0167 2.3562 +-0.0194 0.0194 2.3562 +-0.0222 0.0222 2.3562 +-0.0250 0.0250 2.3562 +primID: 1 +startangle_c: 6 +endpose_c: -12 12 6 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 2.3562 +-0.0333 0.0333 2.3562 +-0.0667 0.0667 2.3562 +-0.1000 0.1000 2.3562 +-0.1333 0.1333 2.3562 +-0.1667 0.1667 2.3562 +-0.2000 0.2000 2.3562 +-0.2333 0.2333 2.3562 +-0.2667 0.2667 2.3562 +-0.3000 0.3000 2.3562 +primID: 2 +startangle_c: 6 +endpose_c: 1 -1 6 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 2.3562 +0.0028 -0.0028 2.3562 +0.0056 -0.0056 2.3562 +0.0083 -0.0083 2.3562 +0.0111 -0.0111 2.3562 +0.0139 -0.0139 2.3562 +0.0167 -0.0167 2.3562 +0.0194 -0.0194 2.3562 +0.0222 -0.0222 2.3562 +0.0250 -0.0250 2.3562 +primID: 3 +startangle_c: 6 +endpose_c: -14 10 7 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 2.3562 +-0.0341 0.0341 2.3562 +-0.0684 0.0678 2.3859 +-0.1043 0.1000 2.4377 +-0.1418 0.1302 2.4896 +-0.1809 0.1584 2.5415 +-0.2213 0.1846 2.5933 +-0.2631 0.2086 2.6452 +-0.3060 0.2304 2.6970 +-0.3500 0.2500 2.7489 +primID: 4 +startangle_c: 6 +endpose_c: -10 14 5 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 2.3562 +-0.0341 0.0341 2.3562 +-0.0678 0.0684 2.3265 +-0.1000 0.1043 2.2747 +-0.1302 0.1418 2.2228 +-0.1584 0.1809 2.1709 +-0.1846 0.2213 2.1191 +-0.2086 0.2631 2.0672 +-0.2304 0.3060 2.0154 +-0.2500 0.3500 1.9635 +primID: 5 +startangle_c: 6 +endpose_c: 0 0 7 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 2.3562 +0.0000 0.0000 2.3998 +0.0000 0.0000 2.4435 +0.0000 0.0000 2.4871 +0.0000 0.0000 2.5307 +0.0000 0.0000 2.5744 +0.0000 0.0000 2.6180 +0.0000 0.0000 2.6616 +0.0000 0.0000 2.7053 +0.0000 0.0000 2.7489 +primID: 6 +startangle_c: 6 +endpose_c: 0 0 5 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 2.3562 +0.0000 0.0000 2.3126 +0.0000 0.0000 2.2689 +0.0000 0.0000 2.2253 +0.0000 0.0000 2.1817 +0.0000 0.0000 2.1380 +0.0000 0.0000 2.0944 +0.0000 0.0000 2.0508 +0.0000 0.0000 2.0071 +0.0000 0.0000 1.9635 +primID: 0 +startangle_c: 7 +endpose_c: -2 1 7 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 2.7489 +-0.0056 0.0028 2.7489 +-0.0111 0.0056 2.7489 +-0.0167 0.0083 2.7489 +-0.0222 0.0111 2.7489 +-0.0278 0.0139 2.7489 +-0.0333 0.0167 2.7489 +-0.0389 0.0194 2.7489 +-0.0444 0.0222 2.7489 +-0.0500 0.0250 2.7489 +primID: 1 +startangle_c: 7 +endpose_c: -12 6 7 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 2.7489 +-0.0333 0.0167 2.7489 +-0.0667 0.0333 2.7489 +-0.1000 0.0500 2.7489 +-0.1333 0.0667 2.7489 +-0.1667 0.0833 2.7489 +-0.2000 0.1000 2.7489 +-0.2333 0.1167 2.7489 +-0.2667 0.1333 2.7489 +-0.3000 0.1500 2.7489 +primID: 2 +startangle_c: 7 +endpose_c: 2 -1 7 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 2.7489 +0.0056 -0.0028 2.7489 +0.0111 -0.0056 2.7489 +0.0167 -0.0083 2.7489 +0.0222 -0.0111 2.7489 +0.0278 -0.0139 2.7489 +0.0333 -0.0167 2.7489 +0.0389 -0.0194 2.7489 +0.0444 -0.0222 2.7489 +0.0500 -0.0250 2.7489 +primID: 3 +startangle_c: 7 +endpose_c: -10 8 6 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 2.7489 +-0.0298 0.0184 2.7186 +-0.0592 0.0379 2.6883 +-0.0881 0.0583 2.6580 +-0.1165 0.0796 2.6277 +-0.1444 0.1019 2.5974 +-0.1717 0.1251 2.5671 +-0.1984 0.1492 2.5368 +-0.2245 0.1742 2.5065 +-0.2500 0.2000 2.4762 +primID: 4 +startangle_c: 7 +endpose_c: -14 4 8 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 2.7489 +-0.0377 0.0156 2.7489 +-0.0754 0.0312 2.7489 +-0.1131 0.0468 2.7489 +-0.1508 0.0623 2.7680 +-0.1893 0.0758 2.8427 +-0.2287 0.0863 2.9174 +-0.2687 0.0939 2.9921 +-0.3092 0.0985 3.0669 +-0.3500 0.1000 3.1416 +primID: 5 +startangle_c: 7 +endpose_c: 0 0 6 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 2.7489 +0.0000 0.0000 2.7053 +0.0000 0.0000 2.6616 +0.0000 0.0000 2.6180 +0.0000 0.0000 2.5744 +0.0000 0.0000 2.5307 +0.0000 0.0000 2.4871 +0.0000 0.0000 2.4435 +0.0000 0.0000 2.3998 +0.0000 0.0000 2.3562 +primID: 6 +startangle_c: 7 +endpose_c: 0 0 8 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 2.7489 +0.0000 0.0000 2.7925 +0.0000 0.0000 2.8362 +0.0000 0.0000 2.8798 +0.0000 0.0000 2.9234 +0.0000 0.0000 2.9671 +0.0000 0.0000 3.0107 +0.0000 0.0000 3.0543 +0.0000 0.0000 3.0980 +0.0000 0.0000 3.1416 +primID: 0 +startangle_c: 8 +endpose_c: -1 0 8 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 3.1416 +-0.0028 0.0000 3.1416 +-0.0056 0.0000 3.1416 +-0.0083 0.0000 3.1416 +-0.0111 0.0000 3.1416 +-0.0139 0.0000 3.1416 +-0.0167 0.0000 3.1416 +-0.0194 0.0000 3.1416 +-0.0222 0.0000 3.1416 +-0.0250 0.0000 3.1416 +primID: 1 +startangle_c: 8 +endpose_c: -12 0 8 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 3.1416 +-0.0333 0.0000 3.1416 +-0.0667 0.0000 3.1416 +-0.1000 0.0000 3.1416 +-0.1333 0.0000 3.1416 +-0.1667 0.0000 3.1416 +-0.2000 0.0000 3.1416 +-0.2333 0.0000 3.1416 +-0.2667 0.0000 3.1416 +-0.3000 0.0000 3.1416 +primID: 2 +startangle_c: 8 +endpose_c: 1 0 8 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 3.1416 +0.0028 0.0000 3.1416 +0.0056 0.0000 3.1416 +0.0083 0.0000 3.1416 +0.0111 0.0000 3.1416 +0.0139 0.0000 3.1416 +0.0167 0.0000 3.1416 +0.0194 0.0000 3.1416 +0.0222 0.0000 3.1416 +0.0250 0.0000 3.1416 +primID: 3 +startangle_c: 8 +endpose_c: -16 -2 9 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 3.1416 +-0.0452 0.0000 3.1416 +-0.0904 0.0000 3.1416 +-0.1355 0.0000 3.1416 +-0.1807 -0.0008 3.1904 +-0.2257 -0.0045 3.2592 +-0.2703 -0.0114 3.3280 +-0.3144 -0.0213 3.3967 +-0.3577 -0.0342 3.4655 +-0.4000 -0.0500 3.5343 +primID: 4 +startangle_c: 8 +endpose_c: -16 2 7 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 3.1416 +-0.0452 0.0000 3.1416 +-0.0904 0.0000 3.1416 +-0.1355 0.0000 3.1416 +-0.1807 0.0008 3.0928 +-0.2257 0.0045 3.0240 +-0.2703 0.0114 2.9552 +-0.3144 0.0213 2.8864 +-0.3577 0.0342 2.8177 +-0.4000 0.0500 2.7489 +primID: 5 +startangle_c: 8 +endpose_c: 0 0 9 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 3.1416 +0.0000 0.0000 3.1852 +0.0000 0.0000 3.2289 +0.0000 0.0000 3.2725 +0.0000 0.0000 3.3161 +0.0000 0.0000 3.3598 +0.0000 0.0000 3.4034 +0.0000 0.0000 3.4470 +0.0000 0.0000 3.4907 +0.0000 0.0000 3.5343 +primID: 6 +startangle_c: 8 +endpose_c: 0 0 7 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 3.1416 +0.0000 0.0000 3.0980 +0.0000 0.0000 3.0543 +0.0000 0.0000 3.0107 +0.0000 0.0000 2.9671 +0.0000 0.0000 2.9234 +0.0000 0.0000 2.8798 +0.0000 0.0000 2.8362 +0.0000 0.0000 2.7925 +0.0000 0.0000 2.7489 +primID: 0 +startangle_c: 9 +endpose_c: -2 -1 9 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 3.5343 +-0.0056 -0.0028 3.5343 +-0.0111 -0.0056 3.5343 +-0.0167 -0.0083 3.5343 +-0.0222 -0.0111 3.5343 +-0.0278 -0.0139 3.5343 +-0.0333 -0.0167 3.5343 +-0.0389 -0.0194 3.5343 +-0.0444 -0.0222 3.5343 +-0.0500 -0.0250 3.5343 +primID: 1 +startangle_c: 9 +endpose_c: -12 -6 9 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 3.5343 +-0.0333 -0.0167 3.5343 +-0.0667 -0.0333 3.5343 +-0.1000 -0.0500 3.5343 +-0.1333 -0.0667 3.5343 +-0.1667 -0.0833 3.5343 +-0.2000 -0.1000 3.5343 +-0.2333 -0.1167 3.5343 +-0.2667 -0.1333 3.5343 +-0.3000 -0.1500 3.5343 +primID: 2 +startangle_c: 9 +endpose_c: 2 1 9 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 3.5343 +0.0056 0.0028 3.5343 +0.0111 0.0056 3.5343 +0.0167 0.0083 3.5343 +0.0222 0.0111 3.5343 +0.0278 0.0139 3.5343 +0.0333 0.0167 3.5343 +0.0389 0.0194 3.5343 +0.0444 0.0222 3.5343 +0.0500 0.0250 3.5343 +primID: 3 +startangle_c: 9 +endpose_c: -10 -8 10 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 3.5343 +-0.0298 -0.0184 3.5646 +-0.0592 -0.0379 3.5949 +-0.0881 -0.0583 3.6252 +-0.1165 -0.0796 3.6555 +-0.1444 -0.1019 3.6858 +-0.1717 -0.1251 3.7161 +-0.1984 -0.1492 3.7464 +-0.2245 -0.1742 3.7767 +-0.2500 -0.2000 3.8070 +primID: 4 +startangle_c: 9 +endpose_c: -14 -4 8 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 3.5343 +-0.0377 -0.0156 3.5343 +-0.0754 -0.0312 3.5343 +-0.1131 -0.0468 3.5343 +-0.1508 -0.0623 3.5152 +-0.1893 -0.0758 3.4405 +-0.2287 -0.0863 3.3658 +-0.2687 -0.0939 3.2910 +-0.3092 -0.0985 3.2163 +-0.3500 -0.1000 3.1416 +primID: 5 +startangle_c: 9 +endpose_c: 0 0 10 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 3.5343 +0.0000 0.0000 3.5779 +0.0000 0.0000 3.6216 +0.0000 0.0000 3.6652 +0.0000 0.0000 3.7088 +0.0000 0.0000 3.7525 +0.0000 0.0000 3.7961 +0.0000 0.0000 3.8397 +0.0000 0.0000 3.8834 +0.0000 0.0000 3.9270 +primID: 6 +startangle_c: 9 +endpose_c: 0 0 8 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 3.5343 +0.0000 0.0000 3.4907 +0.0000 0.0000 3.4470 +0.0000 0.0000 3.4034 +0.0000 0.0000 3.3598 +0.0000 0.0000 3.3161 +0.0000 0.0000 3.2725 +0.0000 0.0000 3.2289 +0.0000 0.0000 3.1852 +0.0000 0.0000 3.1416 +primID: 0 +startangle_c: 10 +endpose_c: -1 -1 10 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 3.9270 +-0.0028 -0.0028 3.9270 +-0.0056 -0.0056 3.9270 +-0.0083 -0.0083 3.9270 +-0.0111 -0.0111 3.9270 +-0.0139 -0.0139 3.9270 +-0.0167 -0.0167 3.9270 +-0.0194 -0.0194 3.9270 +-0.0222 -0.0222 3.9270 +-0.0250 -0.0250 3.9270 +primID: 1 +startangle_c: 10 +endpose_c: -12 -12 10 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 3.9270 +-0.0333 -0.0333 3.9270 +-0.0667 -0.0667 3.9270 +-0.1000 -0.1000 3.9270 +-0.1333 -0.1333 3.9270 +-0.1667 -0.1667 3.9270 +-0.2000 -0.2000 3.9270 +-0.2333 -0.2333 3.9270 +-0.2667 -0.2667 3.9270 +-0.3000 -0.3000 3.9270 +primID: 2 +startangle_c: 10 +endpose_c: 1 1 10 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 3.9270 +0.0028 0.0028 3.9270 +0.0056 0.0056 3.9270 +0.0083 0.0083 3.9270 +0.0111 0.0111 3.9270 +0.0139 0.0139 3.9270 +0.0167 0.0167 3.9270 +0.0194 0.0194 3.9270 +0.0222 0.0222 3.9270 +0.0250 0.0250 3.9270 +primID: 3 +startangle_c: 10 +endpose_c: -10 -14 11 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 3.9270 +-0.0341 -0.0341 3.9270 +-0.0678 -0.0684 3.9567 +-0.1000 -0.1043 4.0085 +-0.1302 -0.1418 4.0604 +-0.1584 -0.1809 4.1123 +-0.1846 -0.2213 4.1641 +-0.2086 -0.2631 4.2160 +-0.2304 -0.3060 4.2678 +-0.2500 -0.3500 4.3197 +primID: 4 +startangle_c: 10 +endpose_c: -14 -10 9 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 3.9270 +-0.0341 -0.0341 3.9270 +-0.0684 -0.0678 3.8973 +-0.1043 -0.1000 3.8455 +-0.1418 -0.1302 3.7936 +-0.1809 -0.1584 3.7417 +-0.2213 -0.1846 3.6899 +-0.2631 -0.2086 3.6380 +-0.3060 -0.2304 3.5862 +-0.3500 -0.2500 3.5343 +primID: 5 +startangle_c: 10 +endpose_c: 0 0 11 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 3.9270 +0.0000 0.0000 3.9706 +0.0000 0.0000 4.0143 +0.0000 0.0000 4.0579 +0.0000 0.0000 4.1015 +0.0000 0.0000 4.1452 +0.0000 0.0000 4.1888 +0.0000 0.0000 4.2324 +0.0000 0.0000 4.2761 +0.0000 0.0000 4.3197 +primID: 6 +startangle_c: 10 +endpose_c: 0 0 9 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 3.9270 +0.0000 0.0000 3.8834 +0.0000 0.0000 3.8397 +0.0000 0.0000 3.7961 +0.0000 0.0000 3.7525 +0.0000 0.0000 3.7088 +0.0000 0.0000 3.6652 +0.0000 0.0000 3.6216 +0.0000 0.0000 3.5779 +0.0000 0.0000 3.5343 +primID: 0 +startangle_c: 11 +endpose_c: -1 -2 11 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 4.3197 +-0.0028 -0.0056 4.3197 +-0.0056 -0.0111 4.3197 +-0.0083 -0.0167 4.3197 +-0.0111 -0.0222 4.3197 +-0.0139 -0.0278 4.3197 +-0.0167 -0.0333 4.3197 +-0.0194 -0.0389 4.3197 +-0.0222 -0.0444 4.3197 +-0.0250 -0.0500 4.3197 +primID: 1 +startangle_c: 11 +endpose_c: -6 -12 11 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 4.3197 +-0.0167 -0.0333 4.3197 +-0.0333 -0.0667 4.3197 +-0.0500 -0.1000 4.3197 +-0.0667 -0.1333 4.3197 +-0.0833 -0.1667 4.3197 +-0.1000 -0.2000 4.3197 +-0.1167 -0.2333 4.3197 +-0.1333 -0.2667 4.3197 +-0.1500 -0.3000 4.3197 +primID: 2 +startangle_c: 11 +endpose_c: 1 2 11 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 4.3197 +0.0028 0.0056 4.3197 +0.0056 0.0111 4.3197 +0.0083 0.0167 4.3197 +0.0111 0.0222 4.3197 +0.0139 0.0278 4.3197 +0.0167 0.0333 4.3197 +0.0194 0.0389 4.3197 +0.0222 0.0444 4.3197 +0.0250 0.0500 4.3197 +primID: 3 +startangle_c: 11 +endpose_c: -8 -10 10 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 4.3197 +-0.0184 -0.0298 4.2894 +-0.0379 -0.0592 4.2591 +-0.0583 -0.0881 4.2288 +-0.0796 -0.1165 4.1985 +-0.1019 -0.1444 4.1682 +-0.1251 -0.1717 4.1379 +-0.1492 -0.1984 4.1076 +-0.1742 -0.2245 4.0773 +-0.2000 -0.2500 4.0470 +primID: 4 +startangle_c: 11 +endpose_c: -4 -14 12 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 4.3197 +-0.0156 -0.0377 4.3197 +-0.0312 -0.0754 4.3197 +-0.0468 -0.1131 4.3197 +-0.0623 -0.1508 4.3388 +-0.0758 -0.1893 4.4135 +-0.0863 -0.2287 4.4882 +-0.0939 -0.2687 4.5629 +-0.0985 -0.3092 4.6377 +-0.1000 -0.3500 4.7124 +primID: 5 +startangle_c: 11 +endpose_c: 0 0 10 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 4.3197 +0.0000 0.0000 4.2761 +0.0000 0.0000 4.2324 +0.0000 0.0000 4.1888 +0.0000 0.0000 4.1452 +0.0000 0.0000 4.1015 +0.0000 0.0000 4.0579 +0.0000 0.0000 4.0143 +0.0000 0.0000 3.9706 +0.0000 0.0000 3.9270 +primID: 6 +startangle_c: 11 +endpose_c: 0 0 12 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 4.3197 +0.0000 0.0000 4.3633 +0.0000 0.0000 4.4070 +0.0000 0.0000 4.4506 +0.0000 0.0000 4.4942 +0.0000 0.0000 4.5379 +0.0000 0.0000 4.5815 +0.0000 0.0000 4.6251 +0.0000 0.0000 4.6688 +0.0000 0.0000 4.7124 +primID: 0 +startangle_c: 12 +endpose_c: 0 -1 12 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 4.7124 +0.0000 -0.0028 4.7124 +0.0000 -0.0056 4.7124 +0.0000 -0.0083 4.7124 +0.0000 -0.0111 4.7124 +0.0000 -0.0139 4.7124 +0.0000 -0.0167 4.7124 +0.0000 -0.0194 4.7124 +0.0000 -0.0222 4.7124 +0.0000 -0.0250 4.7124 +primID: 1 +startangle_c: 12 +endpose_c: 0 -12 12 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 4.7124 +0.0000 -0.0333 4.7124 +0.0000 -0.0667 4.7124 +0.0000 -0.1000 4.7124 +0.0000 -0.1333 4.7124 +0.0000 -0.1667 4.7124 +0.0000 -0.2000 4.7124 +0.0000 -0.2333 4.7124 +0.0000 -0.2667 4.7124 +0.0000 -0.3000 4.7124 +primID: 2 +startangle_c: 12 +endpose_c: 0 1 12 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 4.7124 +0.0000 0.0028 4.7124 +0.0000 0.0056 4.7124 +0.0000 0.0083 4.7124 +0.0000 0.0111 4.7124 +0.0000 0.0139 4.7124 +0.0000 0.0167 4.7124 +0.0000 0.0194 4.7124 +0.0000 0.0222 4.7124 +0.0000 0.0250 4.7124 +primID: 3 +startangle_c: 12 +endpose_c: 2 -16 13 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 4.7124 +0.0000 -0.0452 4.7124 +0.0000 -0.0904 4.7124 +0.0000 -0.1355 4.7124 +0.0008 -0.1807 4.7612 +0.0045 -0.2257 4.8300 +0.0114 -0.2703 4.8988 +0.0213 -0.3144 4.9675 +0.0342 -0.3577 5.0363 +0.0500 -0.4000 5.1051 +primID: 4 +startangle_c: 12 +endpose_c: -2 -16 11 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 4.7124 +-0.0000 -0.0452 4.7124 +-0.0000 -0.0904 4.7124 +-0.0000 -0.1355 4.7124 +-0.0008 -0.1807 4.6636 +-0.0045 -0.2257 4.5948 +-0.0114 -0.2703 4.5260 +-0.0213 -0.3144 4.4572 +-0.0342 -0.3577 4.3885 +-0.0500 -0.4000 4.3197 +primID: 5 +startangle_c: 12 +endpose_c: 0 0 13 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 4.7124 +0.0000 0.0000 4.7560 +0.0000 0.0000 4.7997 +0.0000 0.0000 4.8433 +0.0000 0.0000 4.8869 +0.0000 0.0000 4.9306 +0.0000 0.0000 4.9742 +0.0000 0.0000 5.0178 +0.0000 0.0000 5.0615 +0.0000 0.0000 5.1051 +primID: 6 +startangle_c: 12 +endpose_c: 0 0 11 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 4.7124 +0.0000 0.0000 4.6688 +0.0000 0.0000 4.6251 +0.0000 0.0000 4.5815 +0.0000 0.0000 4.5379 +0.0000 0.0000 4.4942 +0.0000 0.0000 4.4506 +0.0000 0.0000 4.4070 +0.0000 0.0000 4.3633 +0.0000 0.0000 4.3197 +primID: 0 +startangle_c: 13 +endpose_c: 1 -2 13 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 5.1051 +0.0028 -0.0056 5.1051 +0.0056 -0.0111 5.1051 +0.0083 -0.0167 5.1051 +0.0111 -0.0222 5.1051 +0.0139 -0.0278 5.1051 +0.0167 -0.0333 5.1051 +0.0194 -0.0389 5.1051 +0.0222 -0.0444 5.1051 +0.0250 -0.0500 5.1051 +primID: 1 +startangle_c: 13 +endpose_c: 6 -12 13 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 5.1051 +0.0167 -0.0333 5.1051 +0.0333 -0.0667 5.1051 +0.0500 -0.1000 5.1051 +0.0667 -0.1333 5.1051 +0.0833 -0.1667 5.1051 +0.1000 -0.2000 5.1051 +0.1167 -0.2333 5.1051 +0.1333 -0.2667 5.1051 +0.1500 -0.3000 5.1051 +primID: 2 +startangle_c: 13 +endpose_c: -1 2 13 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 5.1051 +-0.0028 0.0056 5.1051 +-0.0056 0.0111 5.1051 +-0.0083 0.0167 5.1051 +-0.0111 0.0222 5.1051 +-0.0139 0.0278 5.1051 +-0.0167 0.0333 5.1051 +-0.0194 0.0389 5.1051 +-0.0222 0.0444 5.1051 +-0.0250 0.0500 5.1051 +primID: 3 +startangle_c: 13 +endpose_c: 8 -10 14 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 5.1051 +0.0184 -0.0298 5.1354 +0.0379 -0.0592 5.1657 +0.0583 -0.0881 5.1960 +0.0796 -0.1165 5.2263 +0.1019 -0.1444 5.2566 +0.1251 -0.1717 5.2869 +0.1492 -0.1984 5.3172 +0.1742 -0.2245 5.3475 +0.2000 -0.2500 5.3778 +primID: 4 +startangle_c: 13 +endpose_c: 4 -14 12 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 5.1051 +0.0156 -0.0377 5.1051 +0.0312 -0.0754 5.1051 +0.0468 -0.1131 5.1051 +0.0623 -0.1508 5.0860 +0.0758 -0.1893 5.0113 +0.0863 -0.2287 4.9366 +0.0939 -0.2687 4.8618 +0.0985 -0.3092 4.7871 +0.1000 -0.3500 4.7124 +primID: 5 +startangle_c: 13 +endpose_c: 0 0 14 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 5.1051 +0.0000 0.0000 5.1487 +0.0000 0.0000 5.1924 +0.0000 0.0000 5.2360 +0.0000 0.0000 5.2796 +0.0000 0.0000 5.3233 +0.0000 0.0000 5.3669 +0.0000 0.0000 5.4105 +0.0000 0.0000 5.4542 +0.0000 0.0000 5.4978 +primID: 6 +startangle_c: 13 +endpose_c: 0 0 12 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 5.1051 +0.0000 0.0000 5.0615 +0.0000 0.0000 5.0178 +0.0000 0.0000 4.9742 +0.0000 0.0000 4.9306 +0.0000 0.0000 4.8869 +0.0000 0.0000 4.8433 +0.0000 0.0000 4.7997 +0.0000 0.0000 4.7560 +0.0000 0.0000 4.7124 +primID: 0 +startangle_c: 14 +endpose_c: 1 -1 14 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 5.4978 +0.0028 -0.0028 5.4978 +0.0056 -0.0056 5.4978 +0.0083 -0.0083 5.4978 +0.0111 -0.0111 5.4978 +0.0139 -0.0139 5.4978 +0.0167 -0.0167 5.4978 +0.0194 -0.0194 5.4978 +0.0222 -0.0222 5.4978 +0.0250 -0.0250 5.4978 +primID: 1 +startangle_c: 14 +endpose_c: 12 -12 14 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 5.4978 +0.0333 -0.0333 5.4978 +0.0667 -0.0667 5.4978 +0.1000 -0.1000 5.4978 +0.1333 -0.1333 5.4978 +0.1667 -0.1667 5.4978 +0.2000 -0.2000 5.4978 +0.2333 -0.2333 5.4978 +0.2667 -0.2667 5.4978 +0.3000 -0.3000 5.4978 +primID: 2 +startangle_c: 14 +endpose_c: -1 1 14 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 5.4978 +-0.0028 0.0028 5.4978 +-0.0056 0.0056 5.4978 +-0.0083 0.0083 5.4978 +-0.0111 0.0111 5.4978 +-0.0139 0.0139 5.4978 +-0.0167 0.0167 5.4978 +-0.0194 0.0194 5.4978 +-0.0222 0.0222 5.4978 +-0.0250 0.0250 5.4978 +primID: 3 +startangle_c: 14 +endpose_c: 14 -10 15 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 5.4978 +0.0341 -0.0341 5.4978 +0.0684 -0.0678 5.5275 +0.1043 -0.1000 5.5793 +0.1418 -0.1302 5.6312 +0.1809 -0.1584 5.6830 +0.2213 -0.1846 5.7349 +0.2631 -0.2086 5.7868 +0.3060 -0.2304 5.8386 +0.3500 -0.2500 5.8905 +primID: 4 +startangle_c: 14 +endpose_c: 10 -14 13 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 5.4978 +0.0341 -0.0341 5.4978 +0.0678 -0.0684 5.4681 +0.1000 -0.1043 5.4162 +0.1302 -0.1418 5.3644 +0.1584 -0.1809 5.3125 +0.1846 -0.2213 5.2607 +0.2086 -0.2631 5.2088 +0.2304 -0.3060 5.1569 +0.2500 -0.3500 5.1051 +primID: 5 +startangle_c: 14 +endpose_c: 0 0 15 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 5.4978 +0.0000 0.0000 5.5414 +0.0000 0.0000 5.5851 +0.0000 0.0000 5.6287 +0.0000 0.0000 5.6723 +0.0000 0.0000 5.7160 +0.0000 0.0000 5.7596 +0.0000 0.0000 5.8032 +0.0000 0.0000 5.8469 +0.0000 0.0000 5.8905 +primID: 6 +startangle_c: 14 +endpose_c: 0 0 13 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 5.4978 +0.0000 0.0000 5.4542 +0.0000 0.0000 5.4105 +0.0000 0.0000 5.3669 +0.0000 0.0000 5.3233 +0.0000 0.0000 5.2796 +0.0000 0.0000 5.2360 +0.0000 0.0000 5.1924 +0.0000 0.0000 5.1487 +0.0000 0.0000 5.1051 +primID: 0 +startangle_c: 15 +endpose_c: 2 -1 15 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 5.8905 +0.0056 -0.0028 5.8905 +0.0111 -0.0056 5.8905 +0.0167 -0.0083 5.8905 +0.0222 -0.0111 5.8905 +0.0278 -0.0139 5.8905 +0.0333 -0.0167 5.8905 +0.0389 -0.0194 5.8905 +0.0444 -0.0222 5.8905 +0.0500 -0.0250 5.8905 +primID: 1 +startangle_c: 15 +endpose_c: 12 -6 15 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 5.8905 +0.0333 -0.0167 5.8905 +0.0667 -0.0333 5.8905 +0.1000 -0.0500 5.8905 +0.1333 -0.0667 5.8905 +0.1667 -0.0833 5.8905 +0.2000 -0.1000 5.8905 +0.2333 -0.1167 5.8905 +0.2667 -0.1333 5.8905 +0.3000 -0.1500 5.8905 +primID: 2 +startangle_c: 15 +endpose_c: -2 1 15 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 5.8905 +-0.0056 0.0028 5.8905 +-0.0111 0.0056 5.8905 +-0.0167 0.0083 5.8905 +-0.0222 0.0111 5.8905 +-0.0278 0.0139 5.8905 +-0.0333 0.0167 5.8905 +-0.0389 0.0194 5.8905 +-0.0444 0.0222 5.8905 +-0.0500 0.0250 5.8905 +primID: 3 +startangle_c: 15 +endpose_c: 10 -8 14 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 5.8905 +0.0298 -0.0184 5.8602 +0.0592 -0.0379 5.8299 +0.0881 -0.0583 5.7996 +0.1165 -0.0796 5.7693 +0.1444 -0.1019 5.7390 +0.1717 -0.1251 5.7087 +0.1984 -0.1492 5.6784 +0.2245 -0.1742 5.6481 +0.2500 -0.2000 5.6178 +primID: 4 +startangle_c: 15 +endpose_c: 14 -4 0 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 5.8905 +0.0377 -0.0156 5.8905 +0.0754 -0.0312 5.8905 +0.1131 -0.0468 5.8905 +0.1508 -0.0623 5.9096 +0.1893 -0.0758 5.9843 +0.2287 -0.0863 6.0590 +0.2687 -0.0939 6.1337 +0.3092 -0.0985 6.2085 +0.3500 -0.1000 6.2832 +primID: 5 +startangle_c: 15 +endpose_c: 0 0 14 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 5.8905 +0.0000 0.0000 5.8469 +0.0000 0.0000 5.8032 +0.0000 0.0000 5.7596 +0.0000 0.0000 5.7160 +0.0000 0.0000 5.6723 +0.0000 0.0000 5.6287 +0.0000 0.0000 5.5851 +0.0000 0.0000 5.5414 +0.0000 0.0000 5.4978 +primID: 6 +startangle_c: 15 +endpose_c: 0 0 0 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 5.8905 +0.0000 0.0000 5.2360 +0.0000 0.0000 4.5815 +0.0000 0.0000 3.9270 +0.0000 0.0000 3.2725 +0.0000 0.0000 2.6180 +0.0000 0.0000 1.9635 +0.0000 0.0000 1.3090 +0.0000 0.0000 0.6545 +0.0000 0.0000 0.0000 diff --git a/Controllers/Packages/amr_startup/config/mprim/unicycle_highcost_2cm.mprim b/Controllers/Packages/amr_startup/config/mprim/unicycle_highcost_2cm.mprim new file mode 100755 index 0000000..b87c310 --- /dev/null +++ b/Controllers/Packages/amr_startup/config/mprim/unicycle_highcost_2cm.mprim @@ -0,0 +1,1683 @@ +resolution_m: 0.020000 +numberofangles: 16 +totalnumberofprimitives: 112 +primID: 0 +startangle_c: 0 +endpose_c: 1 0 0 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 0.0000 +0.0022 0.0000 0.0000 +0.0044 0.0000 0.0000 +0.0067 0.0000 0.0000 +0.0089 0.0000 0.0000 +0.0111 0.0000 0.0000 +0.0133 0.0000 0.0000 +0.0156 0.0000 0.0000 +0.0178 0.0000 0.0000 +0.0200 0.0000 0.0000 +primID: 1 +startangle_c: 0 +endpose_c: 20 0 0 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 0.0000 +0.0444 0.0000 0.0000 +0.0889 0.0000 0.0000 +0.1333 0.0000 0.0000 +0.1778 0.0000 0.0000 +0.2222 0.0000 0.0000 +0.2667 0.0000 0.0000 +0.3111 0.0000 0.0000 +0.3556 0.0000 0.0000 +0.4000 0.0000 0.0000 +primID: 2 +startangle_c: 0 +endpose_c: -1 0 0 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 0.0000 +-0.0022 0.0000 0.0000 +-0.0044 0.0000 0.0000 +-0.0067 0.0000 0.0000 +-0.0089 0.0000 0.0000 +-0.0111 0.0000 0.0000 +-0.0133 0.0000 0.0000 +-0.0156 0.0000 0.0000 +-0.0178 0.0000 0.0000 +-0.0200 0.0000 0.0000 +primID: 3 +startangle_c: 0 +endpose_c: 15 3 1 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 0.0000 +0.0342 0.0008 0.0434 +0.0683 0.0031 0.0868 +0.1023 0.0069 0.1302 +0.1361 0.0121 0.1736 +0.1696 0.0188 0.2170 +0.2029 0.0270 0.2604 +0.2357 0.0366 0.3038 +0.2681 0.0476 0.3472 +0.3000 0.0600 0.3906 +primID: 4 +startangle_c: 0 +endpose_c: 15 -3 -1 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 0.0000 +0.0342 -0.0008 -0.0434 +0.0683 -0.0031 -0.0868 +0.1023 -0.0069 -0.1302 +0.1361 -0.0121 -0.1736 +0.1696 -0.0188 -0.2170 +0.2029 -0.0270 -0.2604 +0.2357 -0.0366 -0.3038 +0.2681 -0.0476 -0.3472 +0.3000 -0.0600 -0.3906 +primID: 5 +startangle_c: 0 +endpose_c: 0 0 1 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 0.0000 +0.0000 0.0000 0.0436 +0.0000 0.0000 0.0873 +0.0000 0.0000 0.1309 +0.0000 0.0000 0.1745 +0.0000 0.0000 0.2182 +0.0000 0.0000 0.2618 +0.0000 0.0000 0.3054 +0.0000 0.0000 0.3491 +0.0000 0.0000 0.3927 +primID: 6 +startangle_c: 0 +endpose_c: 0 0 -1 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 0.0000 +0.0000 0.0000 -0.0436 +0.0000 0.0000 -0.0873 +0.0000 0.0000 -0.1309 +0.0000 0.0000 -0.1745 +0.0000 0.0000 -0.2182 +0.0000 0.0000 -0.2618 +0.0000 0.0000 -0.3054 +0.0000 0.0000 -0.3491 +0.0000 0.0000 -0.3927 +primID: 0 +startangle_c: 1 +endpose_c: 2 1 1 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 0.3927 +0.0044 0.0022 0.3927 +0.0089 0.0044 0.3927 +0.0133 0.0067 0.3927 +0.0178 0.0089 0.3927 +0.0222 0.0111 0.3927 +0.0267 0.0133 0.3927 +0.0311 0.0156 0.3927 +0.0356 0.0178 0.3927 +0.0400 0.0200 0.3927 +primID: 1 +startangle_c: 1 +endpose_c: 15 8 1 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 0.3927 +0.0333 0.0178 0.3927 +0.0667 0.0356 0.3927 +0.1000 0.0533 0.3927 +0.1333 0.0711 0.3927 +0.1667 0.0889 0.3927 +0.2000 0.1067 0.3927 +0.2333 0.1244 0.3927 +0.2667 0.1422 0.3927 +0.3000 0.1600 0.3927 +primID: 2 +startangle_c: 1 +endpose_c: -2 -1 1 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 0.3927 +-0.0044 -0.0022 0.3927 +-0.0089 -0.0044 0.3927 +-0.0133 -0.0067 0.3927 +-0.0178 -0.0089 0.3927 +-0.0222 -0.0111 0.3927 +-0.0267 -0.0133 0.3927 +-0.0311 -0.0156 0.3927 +-0.0356 -0.0178 0.3927 +-0.0400 -0.0200 0.3927 +primID: 3 +startangle_c: 1 +endpose_c: 12 10 2 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 0.3927 +0.0285 0.0188 0.4210 +0.0566 0.0385 0.4492 +0.0842 0.0590 0.4775 +0.1115 0.0804 0.5057 +0.1382 0.1027 0.5340 +0.1645 0.1258 0.5623 +0.1902 0.1497 0.5905 +0.2154 0.1745 0.6188 +0.2400 0.2000 0.6470 +primID: 4 +startangle_c: 1 +endpose_c: 18 5 0 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 0.3927 +0.0387 0.0160 0.3927 +0.0774 0.0320 0.3927 +0.1161 0.0481 0.3927 +0.1549 0.0636 0.3512 +0.1947 0.0766 0.2809 +0.2353 0.0868 0.2107 +0.2765 0.0941 0.1405 +0.3182 0.0985 0.0702 +0.3600 0.1000 0.0000 +primID: 5 +startangle_c: 1 +endpose_c: 0 0 2 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 0.3927 +0.0000 0.0000 0.4363 +0.0000 0.0000 0.4800 +0.0000 0.0000 0.5236 +0.0000 0.0000 0.5672 +0.0000 0.0000 0.6109 +0.0000 0.0000 0.6545 +0.0000 0.0000 0.6981 +0.0000 0.0000 0.7418 +0.0000 0.0000 0.7854 +primID: 6 +startangle_c: 1 +endpose_c: 0 0 0 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 0.3927 +0.0000 0.0000 0.3491 +0.0000 0.0000 0.3054 +0.0000 0.0000 0.2618 +0.0000 0.0000 0.2182 +0.0000 0.0000 0.1745 +0.0000 0.0000 0.1309 +0.0000 0.0000 0.0873 +0.0000 0.0000 0.0436 +0.0000 0.0000 0.0000 +primID: 0 +startangle_c: 2 +endpose_c: 1 1 2 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 0.7854 +0.0022 0.0022 0.7854 +0.0044 0.0044 0.7854 +0.0067 0.0067 0.7854 +0.0089 0.0089 0.7854 +0.0111 0.0111 0.7854 +0.0133 0.0133 0.7854 +0.0156 0.0156 0.7854 +0.0178 0.0178 0.7854 +0.0200 0.0200 0.7854 +primID: 1 +startangle_c: 2 +endpose_c: 15 15 2 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 0.7854 +0.0333 0.0333 0.7854 +0.0667 0.0667 0.7854 +0.1000 0.1000 0.7854 +0.1333 0.1333 0.7854 +0.1667 0.1667 0.7854 +0.2000 0.2000 0.7854 +0.2333 0.2333 0.7854 +0.2667 0.2667 0.7854 +0.3000 0.3000 0.7854 +primID: 2 +startangle_c: 2 +endpose_c: -1 -1 2 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 0.7854 +-0.0022 -0.0022 0.7854 +-0.0044 -0.0044 0.7854 +-0.0067 -0.0067 0.7854 +-0.0089 -0.0089 0.7854 +-0.0111 -0.0111 0.7854 +-0.0133 -0.0133 0.7854 +-0.0156 -0.0156 0.7854 +-0.0178 -0.0178 0.7854 +-0.0200 -0.0200 0.7854 +primID: 3 +startangle_c: 2 +endpose_c: 12 18 3 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 0.7854 +0.0334 0.0350 0.8288 +0.0652 0.0714 0.8722 +0.0954 0.1092 0.9156 +0.1240 0.1482 0.9590 +0.1508 0.1885 1.0024 +0.1759 0.2299 1.0458 +0.1991 0.2723 1.0892 +0.2205 0.3157 1.1326 +0.2400 0.3600 1.1760 +primID: 4 +startangle_c: 2 +endpose_c: 18 12 1 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 0.7854 +0.0350 0.0334 0.7420 +0.0714 0.0652 0.6986 +0.1092 0.0954 0.6552 +0.1482 0.1240 0.6118 +0.1885 0.1508 0.5684 +0.2299 0.1759 0.5250 +0.2723 0.1991 0.4816 +0.3157 0.2205 0.4382 +0.3600 0.2400 0.3948 +primID: 5 +startangle_c: 2 +endpose_c: 0 0 3 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 0.7854 +0.0000 0.0000 0.8290 +0.0000 0.0000 0.8727 +0.0000 0.0000 0.9163 +0.0000 0.0000 0.9599 +0.0000 0.0000 1.0036 +0.0000 0.0000 1.0472 +0.0000 0.0000 1.0908 +0.0000 0.0000 1.1345 +0.0000 0.0000 1.1781 +primID: 6 +startangle_c: 2 +endpose_c: 0 0 1 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 0.7854 +0.0000 0.0000 0.7418 +0.0000 0.0000 0.6981 +0.0000 0.0000 0.6545 +0.0000 0.0000 0.6109 +0.0000 0.0000 0.5672 +0.0000 0.0000 0.5236 +0.0000 0.0000 0.4800 +0.0000 0.0000 0.4363 +0.0000 0.0000 0.3927 +primID: 0 +startangle_c: 3 +endpose_c: 1 2 3 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 1.1781 +0.0022 0.0044 1.1781 +0.0044 0.0089 1.1781 +0.0067 0.0133 1.1781 +0.0089 0.0178 1.1781 +0.0111 0.0222 1.1781 +0.0133 0.0267 1.1781 +0.0156 0.0311 1.1781 +0.0178 0.0356 1.1781 +0.0200 0.0400 1.1781 +primID: 1 +startangle_c: 3 +endpose_c: 8 15 3 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 1.1781 +0.0178 0.0333 1.1781 +0.0356 0.0667 1.1781 +0.0533 0.1000 1.1781 +0.0711 0.1333 1.1781 +0.0889 0.1667 1.1781 +0.1067 0.2000 1.1781 +0.1244 0.2333 1.1781 +0.1422 0.2667 1.1781 +0.1600 0.3000 1.1781 +primID: 2 +startangle_c: 3 +endpose_c: -1 -2 3 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 1.1781 +-0.0022 -0.0044 1.1781 +-0.0044 -0.0089 1.1781 +-0.0067 -0.0133 1.1781 +-0.0089 -0.0178 1.1781 +-0.0111 -0.0222 1.1781 +-0.0133 -0.0267 1.1781 +-0.0156 -0.0311 1.1781 +-0.0178 -0.0356 1.1781 +-0.0200 -0.0400 1.1781 +primID: 3 +startangle_c: 3 +endpose_c: 10 12 2 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 1.1781 +0.0188 0.0285 1.1498 +0.0385 0.0566 1.1216 +0.0590 0.0842 1.0933 +0.0804 0.1115 1.0651 +0.1027 0.1382 1.0368 +0.1258 0.1645 1.0085 +0.1497 0.1902 0.9803 +0.1745 0.2154 0.9520 +0.2000 0.2400 0.9238 +primID: 4 +startangle_c: 3 +endpose_c: 5 18 4 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 1.1781 +0.0160 0.0387 1.1781 +0.0320 0.0774 1.1781 +0.0481 0.1161 1.1781 +0.0636 0.1549 1.2196 +0.0766 0.1947 1.2898 +0.0868 0.2353 1.3601 +0.0941 0.2765 1.4303 +0.0985 0.3182 1.5006 +0.1000 0.3600 1.5708 +primID: 5 +startangle_c: 3 +endpose_c: 0 0 2 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 1.1781 +0.0000 0.0000 1.1345 +0.0000 0.0000 1.0908 +0.0000 0.0000 1.0472 +0.0000 0.0000 1.0036 +0.0000 0.0000 0.9599 +0.0000 0.0000 0.9163 +0.0000 0.0000 0.8727 +0.0000 0.0000 0.8290 +0.0000 0.0000 0.7854 +primID: 6 +startangle_c: 3 +endpose_c: 0 0 4 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 1.1781 +0.0000 0.0000 1.2217 +0.0000 0.0000 1.2654 +0.0000 0.0000 1.3090 +0.0000 0.0000 1.3526 +0.0000 0.0000 1.3963 +0.0000 0.0000 1.4399 +0.0000 0.0000 1.4835 +0.0000 0.0000 1.5272 +0.0000 0.0000 1.5708 +primID: 0 +startangle_c: 4 +endpose_c: 0 1 4 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 1.5708 +0.0000 0.0022 1.5708 +0.0000 0.0044 1.5708 +0.0000 0.0067 1.5708 +0.0000 0.0089 1.5708 +0.0000 0.0111 1.5708 +0.0000 0.0133 1.5708 +0.0000 0.0156 1.5708 +0.0000 0.0178 1.5708 +0.0000 0.0200 1.5708 +primID: 1 +startangle_c: 4 +endpose_c: 0 20 4 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 1.5708 +0.0000 0.0444 1.5708 +0.0000 0.0889 1.5708 +0.0000 0.1333 1.5708 +0.0000 0.1778 1.5708 +0.0000 0.2222 1.5708 +0.0000 0.2667 1.5708 +0.0000 0.3111 1.5708 +0.0000 0.3556 1.5708 +0.0000 0.4000 1.5708 +primID: 2 +startangle_c: 4 +endpose_c: 0 -1 4 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 1.5708 +0.0000 -0.0022 1.5708 +0.0000 -0.0044 1.5708 +0.0000 -0.0067 1.5708 +0.0000 -0.0089 1.5708 +0.0000 -0.0111 1.5708 +0.0000 -0.0133 1.5708 +0.0000 -0.0156 1.5708 +0.0000 -0.0178 1.5708 +0.0000 -0.0200 1.5708 +primID: 3 +startangle_c: 4 +endpose_c: -3 15 5 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 1.5708 +-0.0008 0.0342 1.6142 +-0.0031 0.0683 1.6576 +-0.0069 0.1023 1.7010 +-0.0121 0.1361 1.7444 +-0.0188 0.1696 1.7878 +-0.0270 0.2029 1.8312 +-0.0366 0.2357 1.8746 +-0.0476 0.2681 1.9180 +-0.0600 0.3000 1.9614 +primID: 4 +startangle_c: 4 +endpose_c: 3 15 3 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 1.5708 +0.0008 0.0342 1.5274 +0.0031 0.0683 1.4840 +0.0069 0.1023 1.4406 +0.0121 0.1361 1.3972 +0.0188 0.1696 1.3538 +0.0270 0.2029 1.3104 +0.0366 0.2357 1.2670 +0.0476 0.2681 1.2236 +0.0600 0.3000 1.1802 +primID: 5 +startangle_c: 4 +endpose_c: 0 0 5 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 1.5708 +0.0000 0.0000 1.6144 +0.0000 0.0000 1.6581 +0.0000 0.0000 1.7017 +0.0000 0.0000 1.7453 +0.0000 0.0000 1.7890 +0.0000 0.0000 1.8326 +0.0000 0.0000 1.8762 +0.0000 0.0000 1.9199 +0.0000 0.0000 1.9635 +primID: 6 +startangle_c: 4 +endpose_c: 0 0 3 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 1.5708 +0.0000 0.0000 1.5272 +0.0000 0.0000 1.4835 +0.0000 0.0000 1.4399 +0.0000 0.0000 1.3963 +0.0000 0.0000 1.3526 +0.0000 0.0000 1.3090 +0.0000 0.0000 1.2654 +0.0000 0.0000 1.2217 +0.0000 0.0000 1.1781 +primID: 0 +startangle_c: 5 +endpose_c: -1 2 5 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 1.9635 +-0.0022 0.0044 1.9635 +-0.0044 0.0089 1.9635 +-0.0067 0.0133 1.9635 +-0.0089 0.0178 1.9635 +-0.0111 0.0222 1.9635 +-0.0133 0.0267 1.9635 +-0.0156 0.0311 1.9635 +-0.0178 0.0356 1.9635 +-0.0200 0.0400 1.9635 +primID: 1 +startangle_c: 5 +endpose_c: -8 15 5 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 1.9635 +-0.0178 0.0333 1.9635 +-0.0356 0.0667 1.9635 +-0.0533 0.1000 1.9635 +-0.0711 0.1333 1.9635 +-0.0889 0.1667 1.9635 +-0.1067 0.2000 1.9635 +-0.1244 0.2333 1.9635 +-0.1422 0.2667 1.9635 +-0.1600 0.3000 1.9635 +primID: 2 +startangle_c: 5 +endpose_c: 1 -2 5 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 1.9635 +0.0022 -0.0044 1.9635 +0.0044 -0.0089 1.9635 +0.0067 -0.0133 1.9635 +0.0089 -0.0178 1.9635 +0.0111 -0.0222 1.9635 +0.0133 -0.0267 1.9635 +0.0156 -0.0311 1.9635 +0.0178 -0.0356 1.9635 +0.0200 -0.0400 1.9635 +primID: 3 +startangle_c: 5 +endpose_c: -10 12 6 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 1.9635 +-0.0188 0.0285 1.9918 +-0.0385 0.0566 2.0200 +-0.0590 0.0842 2.0483 +-0.0804 0.1115 2.0765 +-0.1027 0.1382 2.1048 +-0.1258 0.1645 2.1330 +-0.1497 0.1902 2.1613 +-0.1745 0.2154 2.1896 +-0.2000 0.2400 2.2178 +primID: 4 +startangle_c: 5 +endpose_c: -5 18 4 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 1.9635 +-0.0160 0.0387 1.9635 +-0.0320 0.0774 1.9635 +-0.0481 0.1161 1.9635 +-0.0636 0.1549 1.9220 +-0.0766 0.1947 1.8517 +-0.0868 0.2353 1.7815 +-0.0941 0.2765 1.7113 +-0.0985 0.3182 1.6410 +-0.1000 0.3600 1.5708 +primID: 5 +startangle_c: 5 +endpose_c: 0 0 6 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 1.9635 +0.0000 0.0000 2.0071 +0.0000 0.0000 2.0508 +0.0000 0.0000 2.0944 +0.0000 0.0000 2.1380 +0.0000 0.0000 2.1817 +0.0000 0.0000 2.2253 +0.0000 0.0000 2.2689 +0.0000 0.0000 2.3126 +0.0000 0.0000 2.3562 +primID: 6 +startangle_c: 5 +endpose_c: 0 0 4 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 1.9635 +0.0000 0.0000 1.9199 +0.0000 0.0000 1.8762 +0.0000 0.0000 1.8326 +0.0000 0.0000 1.7890 +0.0000 0.0000 1.7453 +0.0000 0.0000 1.7017 +0.0000 0.0000 1.6581 +0.0000 0.0000 1.6144 +0.0000 0.0000 1.5708 +primID: 0 +startangle_c: 6 +endpose_c: -1 1 6 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 2.3562 +-0.0022 0.0022 2.3562 +-0.0044 0.0044 2.3562 +-0.0067 0.0067 2.3562 +-0.0089 0.0089 2.3562 +-0.0111 0.0111 2.3562 +-0.0133 0.0133 2.3562 +-0.0156 0.0156 2.3562 +-0.0178 0.0178 2.3562 +-0.0200 0.0200 2.3562 +primID: 1 +startangle_c: 6 +endpose_c: -15 15 6 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 2.3562 +-0.0333 0.0333 2.3562 +-0.0667 0.0667 2.3562 +-0.1000 0.1000 2.3562 +-0.1333 0.1333 2.3562 +-0.1667 0.1667 2.3562 +-0.2000 0.2000 2.3562 +-0.2333 0.2333 2.3562 +-0.2667 0.2667 2.3562 +-0.3000 0.3000 2.3562 +primID: 2 +startangle_c: 6 +endpose_c: 1 -1 6 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 2.3562 +0.0022 -0.0022 2.3562 +0.0044 -0.0044 2.3562 +0.0067 -0.0067 2.3562 +0.0089 -0.0089 2.3562 +0.0111 -0.0111 2.3562 +0.0133 -0.0133 2.3562 +0.0156 -0.0156 2.3562 +0.0178 -0.0178 2.3562 +0.0200 -0.0200 2.3562 +primID: 3 +startangle_c: 6 +endpose_c: -18 12 7 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 2.3562 +-0.0350 0.0334 2.3996 +-0.0714 0.0652 2.4430 +-0.1092 0.0954 2.4864 +-0.1482 0.1240 2.5298 +-0.1885 0.1508 2.5732 +-0.2299 0.1759 2.6166 +-0.2723 0.1991 2.6600 +-0.3157 0.2205 2.7034 +-0.3600 0.2400 2.7468 +primID: 4 +startangle_c: 6 +endpose_c: -12 18 5 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 2.3562 +-0.0334 0.0350 2.3128 +-0.0652 0.0714 2.2694 +-0.0954 0.1092 2.2260 +-0.1240 0.1482 2.1826 +-0.1508 0.1885 2.1392 +-0.1759 0.2299 2.0958 +-0.1991 0.2723 2.0524 +-0.2205 0.3157 2.0090 +-0.2400 0.3600 1.9656 +primID: 5 +startangle_c: 6 +endpose_c: 0 0 7 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 2.3562 +0.0000 0.0000 2.3998 +0.0000 0.0000 2.4435 +0.0000 0.0000 2.4871 +0.0000 0.0000 2.5307 +0.0000 0.0000 2.5744 +0.0000 0.0000 2.6180 +0.0000 0.0000 2.6616 +0.0000 0.0000 2.7053 +0.0000 0.0000 2.7489 +primID: 6 +startangle_c: 6 +endpose_c: 0 0 5 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 2.3562 +0.0000 0.0000 2.3126 +0.0000 0.0000 2.2689 +0.0000 0.0000 2.2253 +0.0000 0.0000 2.1817 +0.0000 0.0000 2.1380 +0.0000 0.0000 2.0944 +0.0000 0.0000 2.0508 +0.0000 0.0000 2.0071 +0.0000 0.0000 1.9635 +primID: 0 +startangle_c: 7 +endpose_c: -2 1 7 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 2.7489 +-0.0044 0.0022 2.7489 +-0.0089 0.0044 2.7489 +-0.0133 0.0067 2.7489 +-0.0178 0.0089 2.7489 +-0.0222 0.0111 2.7489 +-0.0267 0.0133 2.7489 +-0.0311 0.0156 2.7489 +-0.0356 0.0178 2.7489 +-0.0400 0.0200 2.7489 +primID: 1 +startangle_c: 7 +endpose_c: -15 8 7 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 2.7489 +-0.0333 0.0178 2.7489 +-0.0667 0.0356 2.7489 +-0.1000 0.0533 2.7489 +-0.1333 0.0711 2.7489 +-0.1667 0.0889 2.7489 +-0.2000 0.1067 2.7489 +-0.2333 0.1244 2.7489 +-0.2667 0.1422 2.7489 +-0.3000 0.1600 2.7489 +primID: 2 +startangle_c: 7 +endpose_c: 2 -1 7 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 2.7489 +0.0044 -0.0022 2.7489 +0.0089 -0.0044 2.7489 +0.0133 -0.0067 2.7489 +0.0178 -0.0089 2.7489 +0.0222 -0.0111 2.7489 +0.0267 -0.0133 2.7489 +0.0311 -0.0156 2.7489 +0.0356 -0.0178 2.7489 +0.0400 -0.0200 2.7489 +primID: 3 +startangle_c: 7 +endpose_c: -12 10 6 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 2.7489 +-0.0285 0.0188 2.7206 +-0.0566 0.0385 2.6924 +-0.0842 0.0590 2.6641 +-0.1115 0.0804 2.6359 +-0.1382 0.1027 2.6076 +-0.1645 0.1258 2.5793 +-0.1902 0.1497 2.5511 +-0.2154 0.1745 2.5228 +-0.2400 0.2000 2.4946 +primID: 4 +startangle_c: 7 +endpose_c: -18 5 8 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 2.7489 +-0.0387 0.0160 2.7489 +-0.0774 0.0320 2.7489 +-0.1161 0.0481 2.7489 +-0.1549 0.0636 2.7904 +-0.1947 0.0766 2.8606 +-0.2353 0.0868 2.9309 +-0.2765 0.0941 3.0011 +-0.3182 0.0985 3.0714 +-0.3600 0.1000 3.1416 +primID: 5 +startangle_c: 7 +endpose_c: 0 0 6 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 2.7489 +0.0000 0.0000 2.7053 +0.0000 0.0000 2.6616 +0.0000 0.0000 2.6180 +0.0000 0.0000 2.5744 +0.0000 0.0000 2.5307 +0.0000 0.0000 2.4871 +0.0000 0.0000 2.4435 +0.0000 0.0000 2.3998 +0.0000 0.0000 2.3562 +primID: 6 +startangle_c: 7 +endpose_c: 0 0 8 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 2.7489 +0.0000 0.0000 2.7925 +0.0000 0.0000 2.8362 +0.0000 0.0000 2.8798 +0.0000 0.0000 2.9234 +0.0000 0.0000 2.9671 +0.0000 0.0000 3.0107 +0.0000 0.0000 3.0543 +0.0000 0.0000 3.0980 +0.0000 0.0000 3.1416 +primID: 0 +startangle_c: 8 +endpose_c: -1 0 8 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 3.1416 +-0.0022 0.0000 3.1416 +-0.0044 0.0000 3.1416 +-0.0067 0.0000 3.1416 +-0.0089 0.0000 3.1416 +-0.0111 0.0000 3.1416 +-0.0133 0.0000 3.1416 +-0.0156 0.0000 3.1416 +-0.0178 0.0000 3.1416 +-0.0200 0.0000 3.1416 +primID: 1 +startangle_c: 8 +endpose_c: -20 0 8 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 3.1416 +-0.0444 0.0000 3.1416 +-0.0889 0.0000 3.1416 +-0.1333 0.0000 3.1416 +-0.1778 0.0000 3.1416 +-0.2222 0.0000 3.1416 +-0.2667 0.0000 3.1416 +-0.3111 0.0000 3.1416 +-0.3556 0.0000 3.1416 +-0.4000 0.0000 3.1416 +primID: 2 +startangle_c: 8 +endpose_c: 1 0 8 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 3.1416 +0.0022 0.0000 3.1416 +0.0044 0.0000 3.1416 +0.0067 0.0000 3.1416 +0.0089 0.0000 3.1416 +0.0111 0.0000 3.1416 +0.0133 0.0000 3.1416 +0.0156 0.0000 3.1416 +0.0178 0.0000 3.1416 +0.0200 0.0000 3.1416 +primID: 3 +startangle_c: 8 +endpose_c: -15 -3 9 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 3.1416 +-0.0342 -0.0008 3.1850 +-0.0683 -0.0031 3.2284 +-0.1023 -0.0069 3.2718 +-0.1361 -0.0121 3.3152 +-0.1696 -0.0188 3.3586 +-0.2029 -0.0270 3.4020 +-0.2357 -0.0366 3.4454 +-0.2681 -0.0476 3.4888 +-0.3000 -0.0600 3.5322 +primID: 4 +startangle_c: 8 +endpose_c: -15 3 7 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 3.1416 +-0.0342 0.0008 3.0982 +-0.0683 0.0031 3.0548 +-0.1023 0.0069 3.0114 +-0.1361 0.0121 2.9680 +-0.1696 0.0188 2.9246 +-0.2029 0.0270 2.8812 +-0.2357 0.0366 2.8378 +-0.2681 0.0476 2.7944 +-0.3000 0.0600 2.7510 +primID: 5 +startangle_c: 8 +endpose_c: 0 0 9 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 3.1416 +0.0000 0.0000 3.1852 +0.0000 0.0000 3.2289 +0.0000 0.0000 3.2725 +0.0000 0.0000 3.3161 +0.0000 0.0000 3.3598 +0.0000 0.0000 3.4034 +0.0000 0.0000 3.4470 +0.0000 0.0000 3.4907 +0.0000 0.0000 3.5343 +primID: 6 +startangle_c: 8 +endpose_c: 0 0 7 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 3.1416 +0.0000 0.0000 3.0980 +0.0000 0.0000 3.0543 +0.0000 0.0000 3.0107 +0.0000 0.0000 2.9671 +0.0000 0.0000 2.9234 +0.0000 0.0000 2.8798 +0.0000 0.0000 2.8362 +0.0000 0.0000 2.7925 +0.0000 0.0000 2.7489 +primID: 0 +startangle_c: 9 +endpose_c: -2 -1 9 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 3.5343 +-0.0044 -0.0022 3.5343 +-0.0089 -0.0044 3.5343 +-0.0133 -0.0067 3.5343 +-0.0178 -0.0089 3.5343 +-0.0222 -0.0111 3.5343 +-0.0267 -0.0133 3.5343 +-0.0311 -0.0156 3.5343 +-0.0356 -0.0178 3.5343 +-0.0400 -0.0200 3.5343 +primID: 1 +startangle_c: 9 +endpose_c: -15 -8 9 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 3.5343 +-0.0333 -0.0178 3.5343 +-0.0667 -0.0356 3.5343 +-0.1000 -0.0533 3.5343 +-0.1333 -0.0711 3.5343 +-0.1667 -0.0889 3.5343 +-0.2000 -0.1067 3.5343 +-0.2333 -0.1244 3.5343 +-0.2667 -0.1422 3.5343 +-0.3000 -0.1600 3.5343 +primID: 2 +startangle_c: 9 +endpose_c: 2 1 9 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 3.5343 +0.0044 0.0022 3.5343 +0.0089 0.0044 3.5343 +0.0133 0.0067 3.5343 +0.0178 0.0089 3.5343 +0.0222 0.0111 3.5343 +0.0267 0.0133 3.5343 +0.0311 0.0156 3.5343 +0.0356 0.0178 3.5343 +0.0400 0.0200 3.5343 +primID: 3 +startangle_c: 9 +endpose_c: -12 -10 10 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 3.5343 +-0.0285 -0.0188 3.5626 +-0.0566 -0.0385 3.5908 +-0.0842 -0.0590 3.6191 +-0.1115 -0.0804 3.6473 +-0.1382 -0.1027 3.6756 +-0.1645 -0.1258 3.7038 +-0.1902 -0.1497 3.7321 +-0.2154 -0.1745 3.7604 +-0.2400 -0.2000 3.7886 +primID: 4 +startangle_c: 9 +endpose_c: -18 -5 8 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 3.5343 +-0.0387 -0.0160 3.5343 +-0.0774 -0.0320 3.5343 +-0.1161 -0.0481 3.5343 +-0.1549 -0.0636 3.4928 +-0.1947 -0.0766 3.4225 +-0.2353 -0.0868 3.3523 +-0.2765 -0.0941 3.2821 +-0.3182 -0.0985 3.2118 +-0.3600 -0.1000 3.1416 +primID: 5 +startangle_c: 9 +endpose_c: 0 0 10 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 3.5343 +0.0000 0.0000 3.5779 +0.0000 0.0000 3.6216 +0.0000 0.0000 3.6652 +0.0000 0.0000 3.7088 +0.0000 0.0000 3.7525 +0.0000 0.0000 3.7961 +0.0000 0.0000 3.8397 +0.0000 0.0000 3.8834 +0.0000 0.0000 3.9270 +primID: 6 +startangle_c: 9 +endpose_c: 0 0 8 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 3.5343 +0.0000 0.0000 3.4907 +0.0000 0.0000 3.4470 +0.0000 0.0000 3.4034 +0.0000 0.0000 3.3598 +0.0000 0.0000 3.3161 +0.0000 0.0000 3.2725 +0.0000 0.0000 3.2289 +0.0000 0.0000 3.1852 +0.0000 0.0000 3.1416 +primID: 0 +startangle_c: 10 +endpose_c: -1 -1 10 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 3.9270 +-0.0022 -0.0022 3.9270 +-0.0044 -0.0044 3.9270 +-0.0067 -0.0067 3.9270 +-0.0089 -0.0089 3.9270 +-0.0111 -0.0111 3.9270 +-0.0133 -0.0133 3.9270 +-0.0156 -0.0156 3.9270 +-0.0178 -0.0178 3.9270 +-0.0200 -0.0200 3.9270 +primID: 1 +startangle_c: 10 +endpose_c: -15 -15 10 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 3.9270 +-0.0333 -0.0333 3.9270 +-0.0667 -0.0667 3.9270 +-0.1000 -0.1000 3.9270 +-0.1333 -0.1333 3.9270 +-0.1667 -0.1667 3.9270 +-0.2000 -0.2000 3.9270 +-0.2333 -0.2333 3.9270 +-0.2667 -0.2667 3.9270 +-0.3000 -0.3000 3.9270 +primID: 2 +startangle_c: 10 +endpose_c: 1 1 10 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 3.9270 +0.0022 0.0022 3.9270 +0.0044 0.0044 3.9270 +0.0067 0.0067 3.9270 +0.0089 0.0089 3.9270 +0.0111 0.0111 3.9270 +0.0133 0.0133 3.9270 +0.0156 0.0156 3.9270 +0.0178 0.0178 3.9270 +0.0200 0.0200 3.9270 +primID: 3 +startangle_c: 10 +endpose_c: -12 -18 11 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 3.9270 +-0.0334 -0.0350 3.9704 +-0.0652 -0.0714 4.0138 +-0.0954 -0.1092 4.0572 +-0.1240 -0.1482 4.1006 +-0.1508 -0.1885 4.1440 +-0.1759 -0.2299 4.1874 +-0.1991 -0.2723 4.2308 +-0.2205 -0.3157 4.2742 +-0.2400 -0.3600 4.3176 +primID: 4 +startangle_c: 10 +endpose_c: -18 -12 9 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 3.9270 +-0.0350 -0.0334 3.8836 +-0.0714 -0.0652 3.8402 +-0.1092 -0.0954 3.7968 +-0.1482 -0.1240 3.7534 +-0.1885 -0.1508 3.7100 +-0.2299 -0.1759 3.6666 +-0.2723 -0.1991 3.6232 +-0.3157 -0.2205 3.5798 +-0.3600 -0.2400 3.5364 +primID: 5 +startangle_c: 10 +endpose_c: 0 0 11 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 3.9270 +0.0000 0.0000 3.9706 +0.0000 0.0000 4.0143 +0.0000 0.0000 4.0579 +0.0000 0.0000 4.1015 +0.0000 0.0000 4.1452 +0.0000 0.0000 4.1888 +0.0000 0.0000 4.2324 +0.0000 0.0000 4.2761 +0.0000 0.0000 4.3197 +primID: 6 +startangle_c: 10 +endpose_c: 0 0 9 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 3.9270 +0.0000 0.0000 3.8834 +0.0000 0.0000 3.8397 +0.0000 0.0000 3.7961 +0.0000 0.0000 3.7525 +0.0000 0.0000 3.7088 +0.0000 0.0000 3.6652 +0.0000 0.0000 3.6216 +0.0000 0.0000 3.5779 +0.0000 0.0000 3.5343 +primID: 0 +startangle_c: 11 +endpose_c: -1 -2 11 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 4.3197 +-0.0022 -0.0044 4.3197 +-0.0044 -0.0089 4.3197 +-0.0067 -0.0133 4.3197 +-0.0089 -0.0178 4.3197 +-0.0111 -0.0222 4.3197 +-0.0133 -0.0267 4.3197 +-0.0156 -0.0311 4.3197 +-0.0178 -0.0356 4.3197 +-0.0200 -0.0400 4.3197 +primID: 1 +startangle_c: 11 +endpose_c: -8 -15 11 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 4.3197 +-0.0178 -0.0333 4.3197 +-0.0356 -0.0667 4.3197 +-0.0533 -0.1000 4.3197 +-0.0711 -0.1333 4.3197 +-0.0889 -0.1667 4.3197 +-0.1067 -0.2000 4.3197 +-0.1244 -0.2333 4.3197 +-0.1422 -0.2667 4.3197 +-0.1600 -0.3000 4.3197 +primID: 2 +startangle_c: 11 +endpose_c: 1 2 11 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 4.3197 +0.0022 0.0044 4.3197 +0.0044 0.0089 4.3197 +0.0067 0.0133 4.3197 +0.0089 0.0178 4.3197 +0.0111 0.0222 4.3197 +0.0133 0.0267 4.3197 +0.0156 0.0311 4.3197 +0.0178 0.0356 4.3197 +0.0200 0.0400 4.3197 +primID: 3 +startangle_c: 11 +endpose_c: -10 -12 10 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 4.3197 +-0.0188 -0.0285 4.2914 +-0.0385 -0.0566 4.2632 +-0.0590 -0.0842 4.2349 +-0.0804 -0.1115 4.2067 +-0.1027 -0.1382 4.1784 +-0.1258 -0.1645 4.1501 +-0.1497 -0.1902 4.1219 +-0.1745 -0.2154 4.0936 +-0.2000 -0.2400 4.0654 +primID: 4 +startangle_c: 11 +endpose_c: -5 -18 12 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 4.3197 +-0.0160 -0.0387 4.3197 +-0.0320 -0.0774 4.3197 +-0.0481 -0.1161 4.3197 +-0.0636 -0.1549 4.3612 +-0.0766 -0.1947 4.4314 +-0.0868 -0.2353 4.5017 +-0.0941 -0.2765 4.5719 +-0.0985 -0.3182 4.6422 +-0.1000 -0.3600 4.7124 +primID: 5 +startangle_c: 11 +endpose_c: 0 0 10 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 4.3197 +0.0000 0.0000 4.2761 +0.0000 0.0000 4.2324 +0.0000 0.0000 4.1888 +0.0000 0.0000 4.1452 +0.0000 0.0000 4.1015 +0.0000 0.0000 4.0579 +0.0000 0.0000 4.0143 +0.0000 0.0000 3.9706 +0.0000 0.0000 3.9270 +primID: 6 +startangle_c: 11 +endpose_c: 0 0 12 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 4.3197 +0.0000 0.0000 4.3633 +0.0000 0.0000 4.4070 +0.0000 0.0000 4.4506 +0.0000 0.0000 4.4942 +0.0000 0.0000 4.5379 +0.0000 0.0000 4.5815 +0.0000 0.0000 4.6251 +0.0000 0.0000 4.6688 +0.0000 0.0000 4.7124 +primID: 0 +startangle_c: 12 +endpose_c: 0 -1 12 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 4.7124 +0.0000 -0.0022 4.7124 +0.0000 -0.0044 4.7124 +0.0000 -0.0067 4.7124 +0.0000 -0.0089 4.7124 +0.0000 -0.0111 4.7124 +0.0000 -0.0133 4.7124 +0.0000 -0.0156 4.7124 +0.0000 -0.0178 4.7124 +0.0000 -0.0200 4.7124 +primID: 1 +startangle_c: 12 +endpose_c: 0 -20 12 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 4.7124 +0.0000 -0.0444 4.7124 +0.0000 -0.0889 4.7124 +0.0000 -0.1333 4.7124 +0.0000 -0.1778 4.7124 +0.0000 -0.2222 4.7124 +0.0000 -0.2667 4.7124 +0.0000 -0.3111 4.7124 +0.0000 -0.3556 4.7124 +0.0000 -0.4000 4.7124 +primID: 2 +startangle_c: 12 +endpose_c: 0 1 12 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 4.7124 +0.0000 0.0022 4.7124 +0.0000 0.0044 4.7124 +0.0000 0.0067 4.7124 +0.0000 0.0089 4.7124 +0.0000 0.0111 4.7124 +0.0000 0.0133 4.7124 +0.0000 0.0156 4.7124 +0.0000 0.0178 4.7124 +0.0000 0.0200 4.7124 +primID: 3 +startangle_c: 12 +endpose_c: 3 -15 13 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 4.7124 +0.0008 -0.0342 4.7558 +0.0031 -0.0683 4.7992 +0.0069 -0.1023 4.8426 +0.0121 -0.1361 4.8860 +0.0188 -0.1696 4.9294 +0.0270 -0.2029 4.9728 +0.0366 -0.2357 5.0162 +0.0476 -0.2681 5.0596 +0.0600 -0.3000 5.1030 +primID: 4 +startangle_c: 12 +endpose_c: -3 -15 11 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 4.7124 +-0.0008 -0.0342 4.6690 +-0.0031 -0.0683 4.6256 +-0.0069 -0.1023 4.5822 +-0.0121 -0.1361 4.5388 +-0.0188 -0.1696 4.4954 +-0.0270 -0.2029 4.4520 +-0.0366 -0.2357 4.4086 +-0.0476 -0.2681 4.3652 +-0.0600 -0.3000 4.3218 +primID: 5 +startangle_c: 12 +endpose_c: 0 0 13 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 4.7124 +0.0000 0.0000 4.7560 +0.0000 0.0000 4.7997 +0.0000 0.0000 4.8433 +0.0000 0.0000 4.8869 +0.0000 0.0000 4.9306 +0.0000 0.0000 4.9742 +0.0000 0.0000 5.0178 +0.0000 0.0000 5.0615 +0.0000 0.0000 5.1051 +primID: 6 +startangle_c: 12 +endpose_c: 0 0 11 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 4.7124 +0.0000 0.0000 4.6688 +0.0000 0.0000 4.6251 +0.0000 0.0000 4.5815 +0.0000 0.0000 4.5379 +0.0000 0.0000 4.4942 +0.0000 0.0000 4.4506 +0.0000 0.0000 4.4070 +0.0000 0.0000 4.3633 +0.0000 0.0000 4.3197 +primID: 0 +startangle_c: 13 +endpose_c: 1 -2 13 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 5.1051 +0.0022 -0.0044 5.1051 +0.0044 -0.0089 5.1051 +0.0067 -0.0133 5.1051 +0.0089 -0.0178 5.1051 +0.0111 -0.0222 5.1051 +0.0133 -0.0267 5.1051 +0.0156 -0.0311 5.1051 +0.0178 -0.0356 5.1051 +0.0200 -0.0400 5.1051 +primID: 1 +startangle_c: 13 +endpose_c: 8 -15 13 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 5.1051 +0.0178 -0.0333 5.1051 +0.0356 -0.0667 5.1051 +0.0533 -0.1000 5.1051 +0.0711 -0.1333 5.1051 +0.0889 -0.1667 5.1051 +0.1067 -0.2000 5.1051 +0.1244 -0.2333 5.1051 +0.1422 -0.2667 5.1051 +0.1600 -0.3000 5.1051 +primID: 2 +startangle_c: 13 +endpose_c: -1 2 13 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 5.1051 +-0.0022 0.0044 5.1051 +-0.0044 0.0089 5.1051 +-0.0067 0.0133 5.1051 +-0.0089 0.0178 5.1051 +-0.0111 0.0222 5.1051 +-0.0133 0.0267 5.1051 +-0.0156 0.0311 5.1051 +-0.0178 0.0356 5.1051 +-0.0200 0.0400 5.1051 +primID: 3 +startangle_c: 13 +endpose_c: 10 -12 14 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 5.1051 +0.0188 -0.0285 5.1333 +0.0385 -0.0566 5.1616 +0.0590 -0.0842 5.1899 +0.0804 -0.1115 5.2181 +0.1027 -0.1382 5.2464 +0.1258 -0.1645 5.2746 +0.1497 -0.1902 5.3029 +0.1745 -0.2154 5.3312 +0.2000 -0.2400 5.3594 +primID: 4 +startangle_c: 13 +endpose_c: 5 -18 12 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 5.1051 +0.0160 -0.0387 5.1051 +0.0320 -0.0774 5.1051 +0.0481 -0.1161 5.1051 +0.0636 -0.1549 5.0636 +0.0766 -0.1947 4.9933 +0.0868 -0.2353 4.9231 +0.0941 -0.2765 4.8529 +0.0985 -0.3182 4.7826 +0.1000 -0.3600 4.7124 +primID: 5 +startangle_c: 13 +endpose_c: 0 0 14 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 5.1051 +0.0000 0.0000 5.1487 +0.0000 0.0000 5.1924 +0.0000 0.0000 5.2360 +0.0000 0.0000 5.2796 +0.0000 0.0000 5.3233 +0.0000 0.0000 5.3669 +0.0000 0.0000 5.4105 +0.0000 0.0000 5.4542 +0.0000 0.0000 5.4978 +primID: 6 +startangle_c: 13 +endpose_c: 0 0 12 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 5.1051 +0.0000 0.0000 5.0615 +0.0000 0.0000 5.0178 +0.0000 0.0000 4.9742 +0.0000 0.0000 4.9306 +0.0000 0.0000 4.8869 +0.0000 0.0000 4.8433 +0.0000 0.0000 4.7997 +0.0000 0.0000 4.7560 +0.0000 0.0000 4.7124 +primID: 0 +startangle_c: 14 +endpose_c: 1 -1 14 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 5.4978 +0.0022 -0.0022 5.4978 +0.0044 -0.0044 5.4978 +0.0067 -0.0067 5.4978 +0.0089 -0.0089 5.4978 +0.0111 -0.0111 5.4978 +0.0133 -0.0133 5.4978 +0.0156 -0.0156 5.4978 +0.0178 -0.0178 5.4978 +0.0200 -0.0200 5.4978 +primID: 1 +startangle_c: 14 +endpose_c: 15 -15 14 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 5.4978 +0.0333 -0.0333 5.4978 +0.0667 -0.0667 5.4978 +0.1000 -0.1000 5.4978 +0.1333 -0.1333 5.4978 +0.1667 -0.1667 5.4978 +0.2000 -0.2000 5.4978 +0.2333 -0.2333 5.4978 +0.2667 -0.2667 5.4978 +0.3000 -0.3000 5.4978 +primID: 2 +startangle_c: 14 +endpose_c: -1 1 14 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 5.4978 +-0.0022 0.0022 5.4978 +-0.0044 0.0044 5.4978 +-0.0067 0.0067 5.4978 +-0.0089 0.0089 5.4978 +-0.0111 0.0111 5.4978 +-0.0133 0.0133 5.4978 +-0.0156 0.0156 5.4978 +-0.0178 0.0178 5.4978 +-0.0200 0.0200 5.4978 +primID: 3 +startangle_c: 14 +endpose_c: 18 -12 15 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 5.4978 +0.0350 -0.0334 5.5412 +0.0714 -0.0652 5.5846 +0.1092 -0.0954 5.6280 +0.1482 -0.1240 5.6714 +0.1885 -0.1508 5.7148 +0.2299 -0.1759 5.7582 +0.2723 -0.1991 5.8016 +0.3157 -0.2205 5.8450 +0.3600 -0.2400 5.8884 +primID: 4 +startangle_c: 14 +endpose_c: 12 -18 13 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 5.4978 +0.0334 -0.0350 5.4544 +0.0652 -0.0714 5.4110 +0.0954 -0.1092 5.3676 +0.1240 -0.1482 5.3242 +0.1508 -0.1885 5.2808 +0.1759 -0.2299 5.2374 +0.1991 -0.2723 5.1940 +0.2205 -0.3157 5.1506 +0.2400 -0.3600 5.1072 +primID: 5 +startangle_c: 14 +endpose_c: 0 0 15 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 5.4978 +0.0000 0.0000 5.5414 +0.0000 0.0000 5.5851 +0.0000 0.0000 5.6287 +0.0000 0.0000 5.6723 +0.0000 0.0000 5.7160 +0.0000 0.0000 5.7596 +0.0000 0.0000 5.8032 +0.0000 0.0000 5.8469 +0.0000 0.0000 5.8905 +primID: 6 +startangle_c: 14 +endpose_c: 0 0 13 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 5.4978 +0.0000 0.0000 5.4542 +0.0000 0.0000 5.4105 +0.0000 0.0000 5.3669 +0.0000 0.0000 5.3233 +0.0000 0.0000 5.2796 +0.0000 0.0000 5.2360 +0.0000 0.0000 5.1924 +0.0000 0.0000 5.1487 +0.0000 0.0000 5.1051 +primID: 0 +startangle_c: 15 +endpose_c: 2 -1 15 +additionalactioncostmult: 10 +intermediateposes: 10 +0.0000 0.0000 5.8905 +0.0044 -0.0022 5.8905 +0.0089 -0.0044 5.8905 +0.0133 -0.0067 5.8905 +0.0178 -0.0089 5.8905 +0.0222 -0.0111 5.8905 +0.0267 -0.0133 5.8905 +0.0311 -0.0156 5.8905 +0.0356 -0.0178 5.8905 +0.0400 -0.0200 5.8905 +primID: 1 +startangle_c: 15 +endpose_c: 15 -8 15 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 5.8905 +0.0333 -0.0178 5.8905 +0.0667 -0.0356 5.8905 +0.1000 -0.0533 5.8905 +0.1333 -0.0711 5.8905 +0.1667 -0.0889 5.8905 +0.2000 -0.1067 5.8905 +0.2333 -0.1244 5.8905 +0.2667 -0.1422 5.8905 +0.3000 -0.1600 5.8905 +primID: 2 +startangle_c: 15 +endpose_c: -2 1 15 +additionalactioncostmult: 50 +intermediateposes: 10 +0.0000 0.0000 5.8905 +-0.0044 0.0022 5.8905 +-0.0089 0.0044 5.8905 +-0.0133 0.0067 5.8905 +-0.0178 0.0089 5.8905 +-0.0222 0.0111 5.8905 +-0.0267 0.0133 5.8905 +-0.0311 0.0156 5.8905 +-0.0356 0.0178 5.8905 +-0.0400 0.0200 5.8905 +primID: 3 +startangle_c: 15 +endpose_c: 12 -10 14 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 5.8905 +0.0285 -0.0188 5.8622 +0.0566 -0.0385 5.8340 +0.0842 -0.0590 5.8057 +0.1115 -0.0804 5.7775 +0.1382 -0.1027 5.7492 +0.1645 -0.1258 5.7209 +0.1902 -0.1497 5.6927 +0.2154 -0.1745 5.6644 +0.2400 -0.2000 5.6362 +primID: 4 +startangle_c: 15 +endpose_c: 18 -5 0 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 5.8905 +0.0387 -0.0160 5.8905 +0.0774 -0.0320 5.8905 +0.1161 -0.0481 5.8905 +0.1549 -0.0636 5.9320 +0.1947 -0.0766 6.0022 +0.2353 -0.0868 6.0725 +0.2765 -0.0941 6.1427 +0.3182 -0.0985 6.2129 +0.3600 -0.1000 6.2832 +primID: 5 +startangle_c: 15 +endpose_c: 0 0 14 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 5.8905 +0.0000 0.0000 5.8469 +0.0000 0.0000 5.8032 +0.0000 0.0000 5.7596 +0.0000 0.0000 5.7160 +0.0000 0.0000 5.6723 +0.0000 0.0000 5.6287 +0.0000 0.0000 5.5851 +0.0000 0.0000 5.5414 +0.0000 0.0000 5.4978 +primID: 6 +startangle_c: 15 +endpose_c: 0 0 0 +additionalactioncostmult: 100 +intermediateposes: 10 +0.0000 0.0000 5.8905 +0.0000 0.0000 5.2360 +0.0000 0.0000 4.5815 +0.0000 0.0000 3.9270 +0.0000 0.0000 3.2725 +0.0000 0.0000 2.6180 +0.0000 0.0000 1.9635 +0.0000 0.0000 1.3090 +0.0000 0.0000 0.6545 +0.0000 0.0000 0.0000 diff --git a/Controllers/Packages/amr_startup/config/mprim/unicycle_highcost_5cm.mprim b/Controllers/Packages/amr_startup/config/mprim/unicycle_highcost_5cm.mprim new file mode 100755 index 0000000..bff2688 --- /dev/null +++ b/Controllers/Packages/amr_startup/config/mprim/unicycle_highcost_5cm.mprim @@ -0,0 +1,1683 @@ +resolution_m: 0.050000 +numberofangles: 16 +totalnumberofprimitives: 112 +primID: 0 +startangle_c: 0 +endpose_c: 1 0 0 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 0.0000 +0.0056 0.0000 0.0000 +0.0111 0.0000 0.0000 +0.0167 0.0000 0.0000 +0.0222 0.0000 0.0000 +0.0278 0.0000 0.0000 +0.0333 0.0000 0.0000 +0.0389 0.0000 0.0000 +0.0444 0.0000 0.0000 +0.0500 0.0000 0.0000 +primID: 1 +startangle_c: 0 +endpose_c: 8 0 0 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 0.0000 +0.0444 0.0000 0.0000 +0.0889 0.0000 0.0000 +0.1333 0.0000 0.0000 +0.1778 0.0000 0.0000 +0.2222 0.0000 0.0000 +0.2667 0.0000 0.0000 +0.3111 0.0000 0.0000 +0.3556 0.0000 0.0000 +0.4000 0.0000 0.0000 +primID: 2 +startangle_c: 0 +endpose_c: -1 0 0 +additionalactioncostmult: 40 +intermediateposes: 10 +0.0000 0.0000 0.0000 +-0.0056 0.0000 0.0000 +-0.0111 0.0000 0.0000 +-0.0167 0.0000 0.0000 +-0.0222 0.0000 0.0000 +-0.0278 0.0000 0.0000 +-0.0333 0.0000 0.0000 +-0.0389 0.0000 0.0000 +-0.0444 0.0000 0.0000 +-0.0500 0.0000 0.0000 +primID: 3 +startangle_c: 0 +endpose_c: 8 1 1 +additionalactioncostmult: 2 +intermediateposes: 10 +0.0000 0.0000 0.0000 +0.0452 -0.0000 0.0000 +0.0904 -0.0000 0.0000 +0.1355 -0.0000 0.0000 +0.1807 0.0008 0.0488 +0.2257 0.0045 0.1176 +0.2703 0.0114 0.1864 +0.3144 0.0213 0.2551 +0.3577 0.0342 0.3239 +0.4000 0.0500 0.3927 +primID: 4 +startangle_c: 0 +endpose_c: 8 -1 -1 +additionalactioncostmult: 2 +intermediateposes: 10 +0.0000 0.0000 0.0000 +0.0452 0.0000 0.0000 +0.0904 0.0000 0.0000 +0.1355 0.0000 0.0000 +0.1807 -0.0008 -0.0488 +0.2257 -0.0045 -0.1176 +0.2703 -0.0114 -0.1864 +0.3144 -0.0213 -0.2551 +0.3577 -0.0342 -0.3239 +0.4000 -0.0500 -0.3927 +primID: 5 +startangle_c: 0 +endpose_c: 0 0 1 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 0.0000 +0.0000 0.0000 0.0436 +0.0000 0.0000 0.0873 +0.0000 0.0000 0.1309 +0.0000 0.0000 0.1745 +0.0000 0.0000 0.2182 +0.0000 0.0000 0.2618 +0.0000 0.0000 0.3054 +0.0000 0.0000 0.3491 +0.0000 0.0000 0.3927 +primID: 6 +startangle_c: 0 +endpose_c: 0 0 -1 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 0.0000 +0.0000 0.0000 -0.0436 +0.0000 0.0000 -0.0873 +0.0000 0.0000 -0.1309 +0.0000 0.0000 -0.1745 +0.0000 0.0000 -0.2182 +0.0000 0.0000 -0.2618 +0.0000 0.0000 -0.3054 +0.0000 0.0000 -0.3491 +0.0000 0.0000 -0.3927 +primID: 0 +startangle_c: 1 +endpose_c: 2 1 1 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 0.3927 +0.0111 0.0056 0.3927 +0.0222 0.0111 0.3927 +0.0333 0.0167 0.3927 +0.0444 0.0222 0.3927 +0.0556 0.0278 0.3927 +0.0667 0.0333 0.3927 +0.0778 0.0389 0.3927 +0.0889 0.0444 0.3927 +0.1000 0.0500 0.3927 +primID: 1 +startangle_c: 1 +endpose_c: 6 3 1 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 0.3927 +0.0333 0.0167 0.3927 +0.0667 0.0333 0.3927 +0.1000 0.0500 0.3927 +0.1333 0.0667 0.3927 +0.1667 0.0833 0.3927 +0.2000 0.1000 0.3927 +0.2333 0.1167 0.3927 +0.2667 0.1333 0.3927 +0.3000 0.1500 0.3927 +primID: 2 +startangle_c: 1 +endpose_c: -2 -1 1 +additionalactioncostmult: 40 +intermediateposes: 10 +0.0000 0.0000 0.3927 +-0.0111 -0.0056 0.3927 +-0.0222 -0.0111 0.3927 +-0.0333 -0.0167 0.3927 +-0.0444 -0.0222 0.3927 +-0.0556 -0.0278 0.3927 +-0.0667 -0.0333 0.3927 +-0.0778 -0.0389 0.3927 +-0.0889 -0.0444 0.3927 +-0.1000 -0.0500 0.3927 +primID: 3 +startangle_c: 1 +endpose_c: 5 4 2 +additionalactioncostmult: 2 +intermediateposes: 10 +0.0000 0.0000 0.3927 +0.0298 0.0184 0.4230 +0.0592 0.0379 0.4533 +0.0881 0.0583 0.4836 +0.1165 0.0796 0.5139 +0.1444 0.1019 0.5442 +0.1717 0.1251 0.5745 +0.1984 0.1492 0.6048 +0.2245 0.1742 0.6351 +0.2500 0.2000 0.6654 +primID: 4 +startangle_c: 1 +endpose_c: 7 2 0 +additionalactioncostmult: 2 +intermediateposes: 10 +0.0000 0.0000 0.3927 +0.0377 0.0156 0.3927 +0.0754 0.0312 0.3927 +0.1131 0.0468 0.3927 +0.1508 0.0623 0.3736 +0.1893 0.0758 0.2989 +0.2287 0.0863 0.2242 +0.2687 0.0939 0.1494 +0.3092 0.0985 0.0747 +0.3500 0.1000 -0.0000 +primID: 5 +startangle_c: 1 +endpose_c: 0 0 2 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 0.3927 +0.0000 0.0000 0.4363 +0.0000 0.0000 0.4800 +0.0000 0.0000 0.5236 +0.0000 0.0000 0.5672 +0.0000 0.0000 0.6109 +0.0000 0.0000 0.6545 +0.0000 0.0000 0.6981 +0.0000 0.0000 0.7418 +0.0000 0.0000 0.7854 +primID: 6 +startangle_c: 1 +endpose_c: 0 0 0 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 0.3927 +0.0000 0.0000 0.3491 +0.0000 0.0000 0.3054 +0.0000 0.0000 0.2618 +0.0000 0.0000 0.2182 +0.0000 0.0000 0.1745 +0.0000 0.0000 0.1309 +0.0000 0.0000 0.0873 +0.0000 0.0000 0.0436 +0.0000 0.0000 0.0000 +primID: 0 +startangle_c: 2 +endpose_c: 1 1 2 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 0.7854 +0.0056 0.0056 0.7854 +0.0111 0.0111 0.7854 +0.0167 0.0167 0.7854 +0.0222 0.0222 0.7854 +0.0278 0.0278 0.7854 +0.0333 0.0333 0.7854 +0.0389 0.0389 0.7854 +0.0444 0.0444 0.7854 +0.0500 0.0500 0.7854 +primID: 1 +startangle_c: 2 +endpose_c: 6 6 2 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 0.7854 +0.0333 0.0333 0.7854 +0.0667 0.0667 0.7854 +0.1000 0.1000 0.7854 +0.1333 0.1333 0.7854 +0.1667 0.1667 0.7854 +0.2000 0.2000 0.7854 +0.2333 0.2333 0.7854 +0.2667 0.2667 0.7854 +0.3000 0.3000 0.7854 +primID: 2 +startangle_c: 2 +endpose_c: -1 -1 2 +additionalactioncostmult: 40 +intermediateposes: 10 +0.0000 0.0000 0.7854 +-0.0056 -0.0056 0.7854 +-0.0111 -0.0111 0.7854 +-0.0167 -0.0167 0.7854 +-0.0222 -0.0222 0.7854 +-0.0278 -0.0278 0.7854 +-0.0333 -0.0333 0.7854 +-0.0389 -0.0389 0.7854 +-0.0444 -0.0444 0.7854 +-0.0500 -0.0500 0.7854 +primID: 3 +startangle_c: 2 +endpose_c: 5 7 3 +additionalactioncostmult: 2 +intermediateposes: 10 +0.0000 0.0000 0.7854 +0.0341 0.0341 0.7854 +0.0678 0.0684 0.8151 +0.1000 0.1043 0.8669 +0.1302 0.1418 0.9188 +0.1584 0.1809 0.9707 +0.1846 0.2213 1.0225 +0.2086 0.2631 1.0744 +0.2304 0.3060 1.1262 +0.2500 0.3500 1.1781 +primID: 4 +startangle_c: 2 +endpose_c: 7 5 1 +additionalactioncostmult: 2 +intermediateposes: 10 +0.0000 0.0000 0.7854 +0.0341 0.0341 0.7854 +0.0684 0.0678 0.7557 +0.1043 0.1000 0.7039 +0.1418 0.1302 0.6520 +0.1809 0.1584 0.6001 +0.2213 0.1846 0.5483 +0.2631 0.2086 0.4964 +0.3060 0.2304 0.4446 +0.3500 0.2500 0.3927 +primID: 5 +startangle_c: 2 +endpose_c: 0 0 3 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 0.7854 +0.0000 0.0000 0.8290 +0.0000 0.0000 0.8727 +0.0000 0.0000 0.9163 +0.0000 0.0000 0.9599 +0.0000 0.0000 1.0036 +0.0000 0.0000 1.0472 +0.0000 0.0000 1.0908 +0.0000 0.0000 1.1345 +0.0000 0.0000 1.1781 +primID: 6 +startangle_c: 2 +endpose_c: 0 0 1 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 0.7854 +0.0000 0.0000 0.7418 +0.0000 0.0000 0.6981 +0.0000 0.0000 0.6545 +0.0000 0.0000 0.6109 +0.0000 0.0000 0.5672 +0.0000 0.0000 0.5236 +0.0000 0.0000 0.4800 +0.0000 0.0000 0.4363 +0.0000 0.0000 0.3927 +primID: 0 +startangle_c: 3 +endpose_c: 1 2 3 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 1.1781 +0.0056 0.0111 1.1781 +0.0111 0.0222 1.1781 +0.0167 0.0333 1.1781 +0.0222 0.0444 1.1781 +0.0278 0.0556 1.1781 +0.0333 0.0667 1.1781 +0.0389 0.0778 1.1781 +0.0444 0.0889 1.1781 +0.0500 0.1000 1.1781 +primID: 1 +startangle_c: 3 +endpose_c: 3 6 3 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 1.1781 +0.0167 0.0333 1.1781 +0.0333 0.0667 1.1781 +0.0500 0.1000 1.1781 +0.0667 0.1333 1.1781 +0.0833 0.1667 1.1781 +0.1000 0.2000 1.1781 +0.1167 0.2333 1.1781 +0.1333 0.2667 1.1781 +0.1500 0.3000 1.1781 +primID: 2 +startangle_c: 3 +endpose_c: -1 -2 3 +additionalactioncostmult: 40 +intermediateposes: 10 +0.0000 0.0000 1.1781 +-0.0056 -0.0111 1.1781 +-0.0111 -0.0222 1.1781 +-0.0167 -0.0333 1.1781 +-0.0222 -0.0444 1.1781 +-0.0278 -0.0556 1.1781 +-0.0333 -0.0667 1.1781 +-0.0389 -0.0778 1.1781 +-0.0444 -0.0889 1.1781 +-0.0500 -0.1000 1.1781 +primID: 3 +startangle_c: 3 +endpose_c: 4 5 2 +additionalactioncostmult: 2 +intermediateposes: 10 +0.0000 0.0000 1.1781 +0.0184 0.0298 1.1478 +0.0379 0.0592 1.1175 +0.0583 0.0881 1.0872 +0.0796 0.1165 1.0569 +0.1019 0.1444 1.0266 +0.1251 0.1717 0.9963 +0.1492 0.1984 0.9660 +0.1742 0.2245 0.9357 +0.2000 0.2500 0.9054 +primID: 4 +startangle_c: 3 +endpose_c: 2 7 4 +additionalactioncostmult: 2 +intermediateposes: 10 +0.0000 0.0000 1.1781 +0.0156 0.0377 1.1781 +0.0312 0.0754 1.1781 +0.0468 0.1131 1.1781 +0.0623 0.1508 1.1972 +0.0758 0.1893 1.2719 +0.0863 0.2287 1.3466 +0.0939 0.2687 1.4214 +0.0985 0.3092 1.4961 +0.1000 0.3500 1.5708 +primID: 5 +startangle_c: 3 +endpose_c: 0 0 2 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 1.1781 +0.0000 0.0000 1.1345 +0.0000 0.0000 1.0908 +0.0000 0.0000 1.0472 +0.0000 0.0000 1.0036 +0.0000 0.0000 0.9599 +0.0000 0.0000 0.9163 +0.0000 0.0000 0.8727 +0.0000 0.0000 0.8290 +0.0000 0.0000 0.7854 +primID: 6 +startangle_c: 3 +endpose_c: 0 0 4 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 1.1781 +0.0000 0.0000 1.2217 +0.0000 0.0000 1.2654 +0.0000 0.0000 1.3090 +0.0000 0.0000 1.3526 +0.0000 0.0000 1.3963 +0.0000 0.0000 1.4399 +0.0000 0.0000 1.4835 +0.0000 0.0000 1.5272 +0.0000 0.0000 1.5708 +primID: 0 +startangle_c: 4 +endpose_c: 0 1 4 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 1.5708 +0.0000 0.0056 1.5708 +0.0000 0.0111 1.5708 +0.0000 0.0167 1.5708 +0.0000 0.0222 1.5708 +0.0000 0.0278 1.5708 +0.0000 0.0333 1.5708 +0.0000 0.0389 1.5708 +0.0000 0.0444 1.5708 +0.0000 0.0500 1.5708 +primID: 1 +startangle_c: 4 +endpose_c: 0 8 4 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 1.5708 +0.0000 0.0444 1.5708 +0.0000 0.0889 1.5708 +0.0000 0.1333 1.5708 +0.0000 0.1778 1.5708 +0.0000 0.2222 1.5708 +0.0000 0.2667 1.5708 +0.0000 0.3111 1.5708 +0.0000 0.3556 1.5708 +0.0000 0.4000 1.5708 +primID: 2 +startangle_c: 4 +endpose_c: 0 -1 4 +additionalactioncostmult: 40 +intermediateposes: 10 +0.0000 0.0000 1.5708 +0.0000 -0.0056 1.5708 +0.0000 -0.0111 1.5708 +0.0000 -0.0167 1.5708 +0.0000 -0.0222 1.5708 +0.0000 -0.0278 1.5708 +0.0000 -0.0333 1.5708 +0.0000 -0.0389 1.5708 +0.0000 -0.0444 1.5708 +0.0000 -0.0500 1.5708 +primID: 3 +startangle_c: 4 +endpose_c: -1 8 5 +additionalactioncostmult: 2 +intermediateposes: 10 +0.0000 0.0000 1.5708 +0.0000 0.0452 1.5708 +0.0000 0.0904 1.5708 +0.0000 0.1355 1.5708 +-0.0008 0.1807 1.6196 +-0.0045 0.2257 1.6884 +-0.0114 0.2703 1.7572 +-0.0213 0.3144 1.8259 +-0.0342 0.3577 1.8947 +-0.0500 0.4000 1.9635 +primID: 4 +startangle_c: 4 +endpose_c: 1 8 3 +additionalactioncostmult: 2 +intermediateposes: 10 +0.0000 0.0000 1.5708 +0.0000 0.0452 1.5708 +0.0000 0.0904 1.5708 +0.0000 0.1355 1.5708 +0.0008 0.1807 1.5220 +0.0045 0.2257 1.4532 +0.0114 0.2703 1.3844 +0.0213 0.3144 1.3156 +0.0342 0.3577 1.2469 +0.0500 0.4000 1.1781 +primID: 5 +startangle_c: 4 +endpose_c: 0 0 5 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 1.5708 +0.0000 0.0000 1.6144 +0.0000 0.0000 1.6581 +0.0000 0.0000 1.7017 +0.0000 0.0000 1.7453 +0.0000 0.0000 1.7890 +0.0000 0.0000 1.8326 +0.0000 0.0000 1.8762 +0.0000 0.0000 1.9199 +0.0000 0.0000 1.9635 +primID: 6 +startangle_c: 4 +endpose_c: 0 0 3 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 1.5708 +0.0000 0.0000 1.5272 +0.0000 0.0000 1.4835 +0.0000 0.0000 1.4399 +0.0000 0.0000 1.3963 +0.0000 0.0000 1.3526 +0.0000 0.0000 1.3090 +0.0000 0.0000 1.2654 +0.0000 0.0000 1.2217 +0.0000 0.0000 1.1781 +primID: 0 +startangle_c: 5 +endpose_c: -1 2 5 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 1.9635 +-0.0056 0.0111 1.9635 +-0.0111 0.0222 1.9635 +-0.0167 0.0333 1.9635 +-0.0222 0.0444 1.9635 +-0.0278 0.0556 1.9635 +-0.0333 0.0667 1.9635 +-0.0389 0.0778 1.9635 +-0.0444 0.0889 1.9635 +-0.0500 0.1000 1.9635 +primID: 1 +startangle_c: 5 +endpose_c: -3 6 5 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 1.9635 +-0.0167 0.0333 1.9635 +-0.0333 0.0667 1.9635 +-0.0500 0.1000 1.9635 +-0.0667 0.1333 1.9635 +-0.0833 0.1667 1.9635 +-0.1000 0.2000 1.9635 +-0.1167 0.2333 1.9635 +-0.1333 0.2667 1.9635 +-0.1500 0.3000 1.9635 +primID: 2 +startangle_c: 5 +endpose_c: 1 -2 5 +additionalactioncostmult: 40 +intermediateposes: 10 +0.0000 0.0000 1.9635 +0.0056 -0.0111 1.9635 +0.0111 -0.0222 1.9635 +0.0167 -0.0333 1.9635 +0.0222 -0.0444 1.9635 +0.0278 -0.0556 1.9635 +0.0333 -0.0667 1.9635 +0.0389 -0.0778 1.9635 +0.0444 -0.0889 1.9635 +0.0500 -0.1000 1.9635 +primID: 3 +startangle_c: 5 +endpose_c: -4 5 6 +additionalactioncostmult: 2 +intermediateposes: 10 +0.0000 0.0000 1.9635 +-0.0184 0.0298 1.9938 +-0.0379 0.0592 2.0241 +-0.0583 0.0881 2.0544 +-0.0796 0.1165 2.0847 +-0.1019 0.1444 2.1150 +-0.1251 0.1717 2.1453 +-0.1492 0.1984 2.1756 +-0.1742 0.2245 2.2059 +-0.2000 0.2500 2.2362 +primID: 4 +startangle_c: 5 +endpose_c: -2 7 4 +additionalactioncostmult: 2 +intermediateposes: 10 +0.0000 0.0000 1.9635 +-0.0156 0.0377 1.9635 +-0.0312 0.0754 1.9635 +-0.0468 0.1131 1.9635 +-0.0623 0.1508 1.9444 +-0.0758 0.1893 1.8697 +-0.0863 0.2287 1.7950 +-0.0939 0.2687 1.7202 +-0.0985 0.3092 1.6455 +-0.1000 0.3500 1.5708 +primID: 5 +startangle_c: 5 +endpose_c: 0 0 6 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 1.9635 +0.0000 0.0000 2.0071 +0.0000 0.0000 2.0508 +0.0000 0.0000 2.0944 +0.0000 0.0000 2.1380 +0.0000 0.0000 2.1817 +0.0000 0.0000 2.2253 +0.0000 0.0000 2.2689 +0.0000 0.0000 2.3126 +0.0000 0.0000 2.3562 +primID: 6 +startangle_c: 5 +endpose_c: 0 0 4 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 1.9635 +0.0000 0.0000 1.9199 +0.0000 0.0000 1.8762 +0.0000 0.0000 1.8326 +0.0000 0.0000 1.7890 +0.0000 0.0000 1.7453 +0.0000 0.0000 1.7017 +0.0000 0.0000 1.6581 +0.0000 0.0000 1.6144 +0.0000 0.0000 1.5708 +primID: 0 +startangle_c: 6 +endpose_c: -1 1 6 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 2.3562 +-0.0056 0.0056 2.3562 +-0.0111 0.0111 2.3562 +-0.0167 0.0167 2.3562 +-0.0222 0.0222 2.3562 +-0.0278 0.0278 2.3562 +-0.0333 0.0333 2.3562 +-0.0389 0.0389 2.3562 +-0.0444 0.0444 2.3562 +-0.0500 0.0500 2.3562 +primID: 1 +startangle_c: 6 +endpose_c: -6 6 6 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 2.3562 +-0.0333 0.0333 2.3562 +-0.0667 0.0667 2.3562 +-0.1000 0.1000 2.3562 +-0.1333 0.1333 2.3562 +-0.1667 0.1667 2.3562 +-0.2000 0.2000 2.3562 +-0.2333 0.2333 2.3562 +-0.2667 0.2667 2.3562 +-0.3000 0.3000 2.3562 +primID: 2 +startangle_c: 6 +endpose_c: 1 -1 6 +additionalactioncostmult: 40 +intermediateposes: 10 +0.0000 0.0000 2.3562 +0.0056 -0.0056 2.3562 +0.0111 -0.0111 2.3562 +0.0167 -0.0167 2.3562 +0.0222 -0.0222 2.3562 +0.0278 -0.0278 2.3562 +0.0333 -0.0333 2.3562 +0.0389 -0.0389 2.3562 +0.0444 -0.0444 2.3562 +0.0500 -0.0500 2.3562 +primID: 3 +startangle_c: 6 +endpose_c: -7 5 7 +additionalactioncostmult: 2 +intermediateposes: 10 +0.0000 0.0000 2.3562 +-0.0341 0.0341 2.3562 +-0.0684 0.0678 2.3859 +-0.1043 0.1000 2.4377 +-0.1418 0.1302 2.4896 +-0.1809 0.1584 2.5415 +-0.2213 0.1846 2.5933 +-0.2631 0.2086 2.6452 +-0.3060 0.2304 2.6970 +-0.3500 0.2500 2.7489 +primID: 4 +startangle_c: 6 +endpose_c: -5 7 5 +additionalactioncostmult: 2 +intermediateposes: 10 +0.0000 0.0000 2.3562 +-0.0341 0.0341 2.3562 +-0.0678 0.0684 2.3265 +-0.1000 0.1043 2.2747 +-0.1302 0.1418 2.2228 +-0.1584 0.1809 2.1709 +-0.1846 0.2213 2.1191 +-0.2086 0.2631 2.0672 +-0.2304 0.3060 2.0154 +-0.2500 0.3500 1.9635 +primID: 5 +startangle_c: 6 +endpose_c: 0 0 7 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 2.3562 +0.0000 0.0000 2.3998 +0.0000 0.0000 2.4435 +0.0000 0.0000 2.4871 +0.0000 0.0000 2.5307 +0.0000 0.0000 2.5744 +0.0000 0.0000 2.6180 +0.0000 0.0000 2.6616 +0.0000 0.0000 2.7053 +0.0000 0.0000 2.7489 +primID: 6 +startangle_c: 6 +endpose_c: 0 0 5 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 2.3562 +0.0000 0.0000 2.3126 +0.0000 0.0000 2.2689 +0.0000 0.0000 2.2253 +0.0000 0.0000 2.1817 +0.0000 0.0000 2.1380 +0.0000 0.0000 2.0944 +0.0000 0.0000 2.0508 +0.0000 0.0000 2.0071 +0.0000 0.0000 1.9635 +primID: 0 +startangle_c: 7 +endpose_c: -2 1 7 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 2.7489 +-0.0111 0.0056 2.7489 +-0.0222 0.0111 2.7489 +-0.0333 0.0167 2.7489 +-0.0444 0.0222 2.7489 +-0.0556 0.0278 2.7489 +-0.0667 0.0333 2.7489 +-0.0778 0.0389 2.7489 +-0.0889 0.0444 2.7489 +-0.1000 0.0500 2.7489 +primID: 1 +startangle_c: 7 +endpose_c: -6 3 7 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 2.7489 +-0.0333 0.0167 2.7489 +-0.0667 0.0333 2.7489 +-0.1000 0.0500 2.7489 +-0.1333 0.0667 2.7489 +-0.1667 0.0833 2.7489 +-0.2000 0.1000 2.7489 +-0.2333 0.1167 2.7489 +-0.2667 0.1333 2.7489 +-0.3000 0.1500 2.7489 +primID: 2 +startangle_c: 7 +endpose_c: 2 -1 7 +additionalactioncostmult: 40 +intermediateposes: 10 +0.0000 0.0000 2.7489 +0.0111 -0.0056 2.7489 +0.0222 -0.0111 2.7489 +0.0333 -0.0167 2.7489 +0.0444 -0.0222 2.7489 +0.0556 -0.0278 2.7489 +0.0667 -0.0333 2.7489 +0.0778 -0.0389 2.7489 +0.0889 -0.0444 2.7489 +0.1000 -0.0500 2.7489 +primID: 3 +startangle_c: 7 +endpose_c: -5 4 6 +additionalactioncostmult: 2 +intermediateposes: 10 +0.0000 0.0000 2.7489 +-0.0298 0.0184 2.7186 +-0.0592 0.0379 2.6883 +-0.0881 0.0583 2.6580 +-0.1165 0.0796 2.6277 +-0.1444 0.1019 2.5974 +-0.1717 0.1251 2.5671 +-0.1984 0.1492 2.5368 +-0.2245 0.1742 2.5065 +-0.2500 0.2000 2.4762 +primID: 4 +startangle_c: 7 +endpose_c: -7 2 8 +additionalactioncostmult: 2 +intermediateposes: 10 +0.0000 0.0000 2.7489 +-0.0377 0.0156 2.7489 +-0.0754 0.0312 2.7489 +-0.1131 0.0468 2.7489 +-0.1508 0.0623 2.7680 +-0.1893 0.0758 2.8427 +-0.2287 0.0863 2.9174 +-0.2687 0.0939 2.9921 +-0.3092 0.0985 3.0669 +-0.3500 0.1000 3.1416 +primID: 5 +startangle_c: 7 +endpose_c: 0 0 6 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 2.7489 +0.0000 0.0000 2.7053 +0.0000 0.0000 2.6616 +0.0000 0.0000 2.6180 +0.0000 0.0000 2.5744 +0.0000 0.0000 2.5307 +0.0000 0.0000 2.4871 +0.0000 0.0000 2.4435 +0.0000 0.0000 2.3998 +0.0000 0.0000 2.3562 +primID: 6 +startangle_c: 7 +endpose_c: 0 0 8 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 2.7489 +0.0000 0.0000 2.7925 +0.0000 0.0000 2.8362 +0.0000 0.0000 2.8798 +0.0000 0.0000 2.9234 +0.0000 0.0000 2.9671 +0.0000 0.0000 3.0107 +0.0000 0.0000 3.0543 +0.0000 0.0000 3.0980 +0.0000 0.0000 3.1416 +primID: 0 +startangle_c: 8 +endpose_c: -1 0 8 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 3.1416 +-0.0056 0.0000 3.1416 +-0.0111 0.0000 3.1416 +-0.0167 0.0000 3.1416 +-0.0222 0.0000 3.1416 +-0.0278 0.0000 3.1416 +-0.0333 0.0000 3.1416 +-0.0389 0.0000 3.1416 +-0.0444 0.0000 3.1416 +-0.0500 0.0000 3.1416 +primID: 1 +startangle_c: 8 +endpose_c: -8 0 8 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 3.1416 +-0.0444 0.0000 3.1416 +-0.0889 0.0000 3.1416 +-0.1333 0.0000 3.1416 +-0.1778 0.0000 3.1416 +-0.2222 0.0000 3.1416 +-0.2667 0.0000 3.1416 +-0.3111 0.0000 3.1416 +-0.3556 0.0000 3.1416 +-0.4000 0.0000 3.1416 +primID: 2 +startangle_c: 8 +endpose_c: 1 0 8 +additionalactioncostmult: 40 +intermediateposes: 10 +0.0000 0.0000 3.1416 +0.0056 0.0000 3.1416 +0.0111 0.0000 3.1416 +0.0167 0.0000 3.1416 +0.0222 0.0000 3.1416 +0.0278 0.0000 3.1416 +0.0333 0.0000 3.1416 +0.0389 0.0000 3.1416 +0.0444 0.0000 3.1416 +0.0500 0.0000 3.1416 +primID: 3 +startangle_c: 8 +endpose_c: -8 -1 9 +additionalactioncostmult: 2 +intermediateposes: 10 +0.0000 0.0000 3.1416 +-0.0452 0.0000 3.1416 +-0.0904 0.0000 3.1416 +-0.1355 0.0000 3.1416 +-0.1807 -0.0008 3.1904 +-0.2257 -0.0045 3.2592 +-0.2703 -0.0114 3.3280 +-0.3144 -0.0213 3.3967 +-0.3577 -0.0342 3.4655 +-0.4000 -0.0500 3.5343 +primID: 4 +startangle_c: 8 +endpose_c: -8 1 7 +additionalactioncostmult: 2 +intermediateposes: 10 +0.0000 0.0000 3.1416 +-0.0452 0.0000 3.1416 +-0.0904 0.0000 3.1416 +-0.1355 0.0000 3.1416 +-0.1807 0.0008 3.0928 +-0.2257 0.0045 3.0240 +-0.2703 0.0114 2.9552 +-0.3144 0.0213 2.8864 +-0.3577 0.0342 2.8177 +-0.4000 0.0500 2.7489 +primID: 5 +startangle_c: 8 +endpose_c: 0 0 9 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 3.1416 +0.0000 0.0000 3.1852 +0.0000 0.0000 3.2289 +0.0000 0.0000 3.2725 +0.0000 0.0000 3.3161 +0.0000 0.0000 3.3598 +0.0000 0.0000 3.4034 +0.0000 0.0000 3.4470 +0.0000 0.0000 3.4907 +0.0000 0.0000 3.5343 +primID: 6 +startangle_c: 8 +endpose_c: 0 0 7 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 3.1416 +0.0000 0.0000 3.0980 +0.0000 0.0000 3.0543 +0.0000 0.0000 3.0107 +0.0000 0.0000 2.9671 +0.0000 0.0000 2.9234 +0.0000 0.0000 2.8798 +0.0000 0.0000 2.8362 +0.0000 0.0000 2.7925 +0.0000 0.0000 2.7489 +primID: 0 +startangle_c: 9 +endpose_c: -2 -1 9 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 3.5343 +-0.0111 -0.0056 3.5343 +-0.0222 -0.0111 3.5343 +-0.0333 -0.0167 3.5343 +-0.0444 -0.0222 3.5343 +-0.0556 -0.0278 3.5343 +-0.0667 -0.0333 3.5343 +-0.0778 -0.0389 3.5343 +-0.0889 -0.0444 3.5343 +-0.1000 -0.0500 3.5343 +primID: 1 +startangle_c: 9 +endpose_c: -6 -3 9 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 3.5343 +-0.0333 -0.0167 3.5343 +-0.0667 -0.0333 3.5343 +-0.1000 -0.0500 3.5343 +-0.1333 -0.0667 3.5343 +-0.1667 -0.0833 3.5343 +-0.2000 -0.1000 3.5343 +-0.2333 -0.1167 3.5343 +-0.2667 -0.1333 3.5343 +-0.3000 -0.1500 3.5343 +primID: 2 +startangle_c: 9 +endpose_c: 2 1 9 +additionalactioncostmult: 40 +intermediateposes: 10 +0.0000 0.0000 3.5343 +0.0111 0.0056 3.5343 +0.0222 0.0111 3.5343 +0.0333 0.0167 3.5343 +0.0444 0.0222 3.5343 +0.0556 0.0278 3.5343 +0.0667 0.0333 3.5343 +0.0778 0.0389 3.5343 +0.0889 0.0444 3.5343 +0.1000 0.0500 3.5343 +primID: 3 +startangle_c: 9 +endpose_c: -5 -4 10 +additionalactioncostmult: 2 +intermediateposes: 10 +0.0000 0.0000 3.5343 +-0.0298 -0.0184 3.5646 +-0.0592 -0.0379 3.5949 +-0.0881 -0.0583 3.6252 +-0.1165 -0.0796 3.6555 +-0.1444 -0.1019 3.6858 +-0.1717 -0.1251 3.7161 +-0.1984 -0.1492 3.7464 +-0.2245 -0.1742 3.7767 +-0.2500 -0.2000 3.8070 +primID: 4 +startangle_c: 9 +endpose_c: -7 -2 8 +additionalactioncostmult: 2 +intermediateposes: 10 +0.0000 0.0000 3.5343 +-0.0377 -0.0156 3.5343 +-0.0754 -0.0312 3.5343 +-0.1131 -0.0468 3.5343 +-0.1508 -0.0623 3.5152 +-0.1893 -0.0758 3.4405 +-0.2287 -0.0863 3.3658 +-0.2687 -0.0939 3.2910 +-0.3092 -0.0985 3.2163 +-0.3500 -0.1000 3.1416 +primID: 5 +startangle_c: 9 +endpose_c: 0 0 10 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 3.5343 +0.0000 0.0000 3.5779 +0.0000 0.0000 3.6216 +0.0000 0.0000 3.6652 +0.0000 0.0000 3.7088 +0.0000 0.0000 3.7525 +0.0000 0.0000 3.7961 +0.0000 0.0000 3.8397 +0.0000 0.0000 3.8834 +0.0000 0.0000 3.9270 +primID: 6 +startangle_c: 9 +endpose_c: 0 0 8 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 3.5343 +0.0000 0.0000 3.4907 +0.0000 0.0000 3.4470 +0.0000 0.0000 3.4034 +0.0000 0.0000 3.3598 +0.0000 0.0000 3.3161 +0.0000 0.0000 3.2725 +0.0000 0.0000 3.2289 +0.0000 0.0000 3.1852 +0.0000 0.0000 3.1416 +primID: 0 +startangle_c: 10 +endpose_c: -1 -1 10 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 3.9270 +-0.0056 -0.0056 3.9270 +-0.0111 -0.0111 3.9270 +-0.0167 -0.0167 3.9270 +-0.0222 -0.0222 3.9270 +-0.0278 -0.0278 3.9270 +-0.0333 -0.0333 3.9270 +-0.0389 -0.0389 3.9270 +-0.0444 -0.0444 3.9270 +-0.0500 -0.0500 3.9270 +primID: 1 +startangle_c: 10 +endpose_c: -6 -6 10 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 3.9270 +-0.0333 -0.0333 3.9270 +-0.0667 -0.0667 3.9270 +-0.1000 -0.1000 3.9270 +-0.1333 -0.1333 3.9270 +-0.1667 -0.1667 3.9270 +-0.2000 -0.2000 3.9270 +-0.2333 -0.2333 3.9270 +-0.2667 -0.2667 3.9270 +-0.3000 -0.3000 3.9270 +primID: 2 +startangle_c: 10 +endpose_c: 1 1 10 +additionalactioncostmult: 40 +intermediateposes: 10 +0.0000 0.0000 3.9270 +0.0056 0.0056 3.9270 +0.0111 0.0111 3.9270 +0.0167 0.0167 3.9270 +0.0222 0.0222 3.9270 +0.0278 0.0278 3.9270 +0.0333 0.0333 3.9270 +0.0389 0.0389 3.9270 +0.0444 0.0444 3.9270 +0.0500 0.0500 3.9270 +primID: 3 +startangle_c: 10 +endpose_c: -5 -7 11 +additionalactioncostmult: 2 +intermediateposes: 10 +0.0000 0.0000 3.9270 +-0.0341 -0.0341 3.9270 +-0.0678 -0.0684 3.9567 +-0.1000 -0.1043 4.0085 +-0.1302 -0.1418 4.0604 +-0.1584 -0.1809 4.1123 +-0.1846 -0.2213 4.1641 +-0.2086 -0.2631 4.2160 +-0.2304 -0.3060 4.2678 +-0.2500 -0.3500 4.3197 +primID: 4 +startangle_c: 10 +endpose_c: -7 -5 9 +additionalactioncostmult: 2 +intermediateposes: 10 +0.0000 0.0000 3.9270 +-0.0341 -0.0341 3.9270 +-0.0684 -0.0678 3.8973 +-0.1043 -0.1000 3.8455 +-0.1418 -0.1302 3.7936 +-0.1809 -0.1584 3.7417 +-0.2213 -0.1846 3.6899 +-0.2631 -0.2086 3.6380 +-0.3060 -0.2304 3.5862 +-0.3500 -0.2500 3.5343 +primID: 5 +startangle_c: 10 +endpose_c: 0 0 11 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 3.9270 +0.0000 0.0000 3.9706 +0.0000 0.0000 4.0143 +0.0000 0.0000 4.0579 +0.0000 0.0000 4.1015 +0.0000 0.0000 4.1452 +0.0000 0.0000 4.1888 +0.0000 0.0000 4.2324 +0.0000 0.0000 4.2761 +0.0000 0.0000 4.3197 +primID: 6 +startangle_c: 10 +endpose_c: 0 0 9 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 3.9270 +0.0000 0.0000 3.8834 +0.0000 0.0000 3.8397 +0.0000 0.0000 3.7961 +0.0000 0.0000 3.7525 +0.0000 0.0000 3.7088 +0.0000 0.0000 3.6652 +0.0000 0.0000 3.6216 +0.0000 0.0000 3.5779 +0.0000 0.0000 3.5343 +primID: 0 +startangle_c: 11 +endpose_c: -1 -2 11 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 4.3197 +-0.0056 -0.0111 4.3197 +-0.0111 -0.0222 4.3197 +-0.0167 -0.0333 4.3197 +-0.0222 -0.0444 4.3197 +-0.0278 -0.0556 4.3197 +-0.0333 -0.0667 4.3197 +-0.0389 -0.0778 4.3197 +-0.0444 -0.0889 4.3197 +-0.0500 -0.1000 4.3197 +primID: 1 +startangle_c: 11 +endpose_c: -3 -6 11 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 4.3197 +-0.0167 -0.0333 4.3197 +-0.0333 -0.0667 4.3197 +-0.0500 -0.1000 4.3197 +-0.0667 -0.1333 4.3197 +-0.0833 -0.1667 4.3197 +-0.1000 -0.2000 4.3197 +-0.1167 -0.2333 4.3197 +-0.1333 -0.2667 4.3197 +-0.1500 -0.3000 4.3197 +primID: 2 +startangle_c: 11 +endpose_c: 1 2 11 +additionalactioncostmult: 40 +intermediateposes: 10 +0.0000 0.0000 4.3197 +0.0056 0.0111 4.3197 +0.0111 0.0222 4.3197 +0.0167 0.0333 4.3197 +0.0222 0.0444 4.3197 +0.0278 0.0556 4.3197 +0.0333 0.0667 4.3197 +0.0389 0.0778 4.3197 +0.0444 0.0889 4.3197 +0.0500 0.1000 4.3197 +primID: 3 +startangle_c: 11 +endpose_c: -4 -5 10 +additionalactioncostmult: 2 +intermediateposes: 10 +0.0000 0.0000 4.3197 +-0.0184 -0.0298 4.2894 +-0.0379 -0.0592 4.2591 +-0.0583 -0.0881 4.2288 +-0.0796 -0.1165 4.1985 +-0.1019 -0.1444 4.1682 +-0.1251 -0.1717 4.1379 +-0.1492 -0.1984 4.1076 +-0.1742 -0.2245 4.0773 +-0.2000 -0.2500 4.0470 +primID: 4 +startangle_c: 11 +endpose_c: -2 -7 12 +additionalactioncostmult: 2 +intermediateposes: 10 +0.0000 0.0000 4.3197 +-0.0156 -0.0377 4.3197 +-0.0312 -0.0754 4.3197 +-0.0468 -0.1131 4.3197 +-0.0623 -0.1508 4.3388 +-0.0758 -0.1893 4.4135 +-0.0863 -0.2287 4.4882 +-0.0939 -0.2687 4.5629 +-0.0985 -0.3092 4.6377 +-0.1000 -0.3500 4.7124 +primID: 5 +startangle_c: 11 +endpose_c: 0 0 10 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 4.3197 +0.0000 0.0000 4.2761 +0.0000 0.0000 4.2324 +0.0000 0.0000 4.1888 +0.0000 0.0000 4.1452 +0.0000 0.0000 4.1015 +0.0000 0.0000 4.0579 +0.0000 0.0000 4.0143 +0.0000 0.0000 3.9706 +0.0000 0.0000 3.9270 +primID: 6 +startangle_c: 11 +endpose_c: 0 0 12 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 4.3197 +0.0000 0.0000 4.3633 +0.0000 0.0000 4.4070 +0.0000 0.0000 4.4506 +0.0000 0.0000 4.4942 +0.0000 0.0000 4.5379 +0.0000 0.0000 4.5815 +0.0000 0.0000 4.6251 +0.0000 0.0000 4.6688 +0.0000 0.0000 4.7124 +primID: 0 +startangle_c: 12 +endpose_c: 0 -1 12 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 4.7124 +0.0000 -0.0056 4.7124 +0.0000 -0.0111 4.7124 +0.0000 -0.0167 4.7124 +0.0000 -0.0222 4.7124 +0.0000 -0.0278 4.7124 +0.0000 -0.0333 4.7124 +0.0000 -0.0389 4.7124 +0.0000 -0.0444 4.7124 +0.0000 -0.0500 4.7124 +primID: 1 +startangle_c: 12 +endpose_c: 0 -8 12 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 4.7124 +0.0000 -0.0444 4.7124 +0.0000 -0.0889 4.7124 +0.0000 -0.1333 4.7124 +0.0000 -0.1778 4.7124 +0.0000 -0.2222 4.7124 +0.0000 -0.2667 4.7124 +0.0000 -0.3111 4.7124 +0.0000 -0.3556 4.7124 +0.0000 -0.4000 4.7124 +primID: 2 +startangle_c: 12 +endpose_c: 0 1 12 +additionalactioncostmult: 40 +intermediateposes: 10 +0.0000 0.0000 4.7124 +0.0000 0.0056 4.7124 +0.0000 0.0111 4.7124 +0.0000 0.0167 4.7124 +0.0000 0.0222 4.7124 +0.0000 0.0278 4.7124 +0.0000 0.0333 4.7124 +0.0000 0.0389 4.7124 +0.0000 0.0444 4.7124 +0.0000 0.0500 4.7124 +primID: 3 +startangle_c: 12 +endpose_c: 1 -8 13 +additionalactioncostmult: 2 +intermediateposes: 10 +0.0000 0.0000 4.7124 +0.0000 -0.0452 4.7124 +0.0000 -0.0904 4.7124 +0.0000 -0.1355 4.7124 +0.0008 -0.1807 4.7612 +0.0045 -0.2257 4.8300 +0.0114 -0.2703 4.8988 +0.0213 -0.3144 4.9675 +0.0342 -0.3577 5.0363 +0.0500 -0.4000 5.1051 +primID: 4 +startangle_c: 12 +endpose_c: -1 -8 11 +additionalactioncostmult: 2 +intermediateposes: 10 +0.0000 0.0000 4.7124 +-0.0000 -0.0452 4.7124 +-0.0000 -0.0904 4.7124 +-0.0000 -0.1355 4.7124 +-0.0008 -0.1807 4.6636 +-0.0045 -0.2257 4.5948 +-0.0114 -0.2703 4.5260 +-0.0213 -0.3144 4.4572 +-0.0342 -0.3577 4.3885 +-0.0500 -0.4000 4.3197 +primID: 5 +startangle_c: 12 +endpose_c: 0 0 13 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 4.7124 +0.0000 0.0000 4.7560 +0.0000 0.0000 4.7997 +0.0000 0.0000 4.8433 +0.0000 0.0000 4.8869 +0.0000 0.0000 4.9306 +0.0000 0.0000 4.9742 +0.0000 0.0000 5.0178 +0.0000 0.0000 5.0615 +0.0000 0.0000 5.1051 +primID: 6 +startangle_c: 12 +endpose_c: 0 0 11 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 4.7124 +0.0000 0.0000 4.6688 +0.0000 0.0000 4.6251 +0.0000 0.0000 4.5815 +0.0000 0.0000 4.5379 +0.0000 0.0000 4.4942 +0.0000 0.0000 4.4506 +0.0000 0.0000 4.4070 +0.0000 0.0000 4.3633 +0.0000 0.0000 4.3197 +primID: 0 +startangle_c: 13 +endpose_c: 1 -2 13 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 5.1051 +0.0056 -0.0111 5.1051 +0.0111 -0.0222 5.1051 +0.0167 -0.0333 5.1051 +0.0222 -0.0444 5.1051 +0.0278 -0.0556 5.1051 +0.0333 -0.0667 5.1051 +0.0389 -0.0778 5.1051 +0.0444 -0.0889 5.1051 +0.0500 -0.1000 5.1051 +primID: 1 +startangle_c: 13 +endpose_c: 3 -6 13 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 5.1051 +0.0167 -0.0333 5.1051 +0.0333 -0.0667 5.1051 +0.0500 -0.1000 5.1051 +0.0667 -0.1333 5.1051 +0.0833 -0.1667 5.1051 +0.1000 -0.2000 5.1051 +0.1167 -0.2333 5.1051 +0.1333 -0.2667 5.1051 +0.1500 -0.3000 5.1051 +primID: 2 +startangle_c: 13 +endpose_c: -1 2 13 +additionalactioncostmult: 40 +intermediateposes: 10 +0.0000 0.0000 5.1051 +-0.0056 0.0111 5.1051 +-0.0111 0.0222 5.1051 +-0.0167 0.0333 5.1051 +-0.0222 0.0444 5.1051 +-0.0278 0.0556 5.1051 +-0.0333 0.0667 5.1051 +-0.0389 0.0778 5.1051 +-0.0444 0.0889 5.1051 +-0.0500 0.1000 5.1051 +primID: 3 +startangle_c: 13 +endpose_c: 4 -5 14 +additionalactioncostmult: 2 +intermediateposes: 10 +0.0000 0.0000 5.1051 +0.0184 -0.0298 5.1354 +0.0379 -0.0592 5.1657 +0.0583 -0.0881 5.1960 +0.0796 -0.1165 5.2263 +0.1019 -0.1444 5.2566 +0.1251 -0.1717 5.2869 +0.1492 -0.1984 5.3172 +0.1742 -0.2245 5.3475 +0.2000 -0.2500 5.3778 +primID: 4 +startangle_c: 13 +endpose_c: 2 -7 12 +additionalactioncostmult: 2 +intermediateposes: 10 +0.0000 0.0000 5.1051 +0.0156 -0.0377 5.1051 +0.0312 -0.0754 5.1051 +0.0468 -0.1131 5.1051 +0.0623 -0.1508 5.0860 +0.0758 -0.1893 5.0113 +0.0863 -0.2287 4.9366 +0.0939 -0.2687 4.8618 +0.0985 -0.3092 4.7871 +0.1000 -0.3500 4.7124 +primID: 5 +startangle_c: 13 +endpose_c: 0 0 14 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 5.1051 +0.0000 0.0000 5.1487 +0.0000 0.0000 5.1924 +0.0000 0.0000 5.2360 +0.0000 0.0000 5.2796 +0.0000 0.0000 5.3233 +0.0000 0.0000 5.3669 +0.0000 0.0000 5.4105 +0.0000 0.0000 5.4542 +0.0000 0.0000 5.4978 +primID: 6 +startangle_c: 13 +endpose_c: 0 0 12 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 5.1051 +0.0000 0.0000 5.0615 +0.0000 0.0000 5.0178 +0.0000 0.0000 4.9742 +0.0000 0.0000 4.9306 +0.0000 0.0000 4.8869 +0.0000 0.0000 4.8433 +0.0000 0.0000 4.7997 +0.0000 0.0000 4.7560 +0.0000 0.0000 4.7124 +primID: 0 +startangle_c: 14 +endpose_c: 1 -1 14 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 5.4978 +0.0056 -0.0056 5.4978 +0.0111 -0.0111 5.4978 +0.0167 -0.0167 5.4978 +0.0222 -0.0222 5.4978 +0.0278 -0.0278 5.4978 +0.0333 -0.0333 5.4978 +0.0389 -0.0389 5.4978 +0.0444 -0.0444 5.4978 +0.0500 -0.0500 5.4978 +primID: 1 +startangle_c: 14 +endpose_c: 6 -6 14 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 5.4978 +0.0333 -0.0333 5.4978 +0.0667 -0.0667 5.4978 +0.1000 -0.1000 5.4978 +0.1333 -0.1333 5.4978 +0.1667 -0.1667 5.4978 +0.2000 -0.2000 5.4978 +0.2333 -0.2333 5.4978 +0.2667 -0.2667 5.4978 +0.3000 -0.3000 5.4978 +primID: 2 +startangle_c: 14 +endpose_c: -1 1 14 +additionalactioncostmult: 40 +intermediateposes: 10 +0.0000 0.0000 5.4978 +-0.0056 0.0056 5.4978 +-0.0111 0.0111 5.4978 +-0.0167 0.0167 5.4978 +-0.0222 0.0222 5.4978 +-0.0278 0.0278 5.4978 +-0.0333 0.0333 5.4978 +-0.0389 0.0389 5.4978 +-0.0444 0.0444 5.4978 +-0.0500 0.0500 5.4978 +primID: 3 +startangle_c: 14 +endpose_c: 7 -5 15 +additionalactioncostmult: 2 +intermediateposes: 10 +0.0000 0.0000 5.4978 +0.0341 -0.0341 5.4978 +0.0684 -0.0678 5.5275 +0.1043 -0.1000 5.5793 +0.1418 -0.1302 5.6312 +0.1809 -0.1584 5.6830 +0.2213 -0.1846 5.7349 +0.2631 -0.2086 5.7868 +0.3060 -0.2304 5.8386 +0.3500 -0.2500 5.8905 +primID: 4 +startangle_c: 14 +endpose_c: 5 -7 13 +additionalactioncostmult: 2 +intermediateposes: 10 +0.0000 0.0000 5.4978 +0.0341 -0.0341 5.4978 +0.0678 -0.0684 5.4681 +0.1000 -0.1043 5.4162 +0.1302 -0.1418 5.3644 +0.1584 -0.1809 5.3125 +0.1846 -0.2213 5.2607 +0.2086 -0.2631 5.2088 +0.2304 -0.3060 5.1569 +0.2500 -0.3500 5.1051 +primID: 5 +startangle_c: 14 +endpose_c: 0 0 15 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 5.4978 +0.0000 0.0000 5.5414 +0.0000 0.0000 5.5851 +0.0000 0.0000 5.6287 +0.0000 0.0000 5.6723 +0.0000 0.0000 5.7160 +0.0000 0.0000 5.7596 +0.0000 0.0000 5.8032 +0.0000 0.0000 5.8469 +0.0000 0.0000 5.8905 +primID: 6 +startangle_c: 14 +endpose_c: 0 0 13 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 5.4978 +0.0000 0.0000 5.4542 +0.0000 0.0000 5.4105 +0.0000 0.0000 5.3669 +0.0000 0.0000 5.3233 +0.0000 0.0000 5.2796 +0.0000 0.0000 5.2360 +0.0000 0.0000 5.1924 +0.0000 0.0000 5.1487 +0.0000 0.0000 5.1051 +primID: 0 +startangle_c: 15 +endpose_c: 2 -1 15 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 5.8905 +0.0111 -0.0056 5.8905 +0.0222 -0.0111 5.8905 +0.0333 -0.0167 5.8905 +0.0444 -0.0222 5.8905 +0.0556 -0.0278 5.8905 +0.0667 -0.0333 5.8905 +0.0778 -0.0389 5.8905 +0.0889 -0.0444 5.8905 +0.1000 -0.0500 5.8905 +primID: 1 +startangle_c: 15 +endpose_c: 6 -3 15 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 5.8905 +0.0333 -0.0167 5.8905 +0.0667 -0.0333 5.8905 +0.1000 -0.0500 5.8905 +0.1333 -0.0667 5.8905 +0.1667 -0.0833 5.8905 +0.2000 -0.1000 5.8905 +0.2333 -0.1167 5.8905 +0.2667 -0.1333 5.8905 +0.3000 -0.1500 5.8905 +primID: 2 +startangle_c: 15 +endpose_c: -2 1 15 +additionalactioncostmult: 40 +intermediateposes: 10 +0.0000 0.0000 5.8905 +-0.0111 0.0056 5.8905 +-0.0222 0.0111 5.8905 +-0.0333 0.0167 5.8905 +-0.0444 0.0222 5.8905 +-0.0556 0.0278 5.8905 +-0.0667 0.0333 5.8905 +-0.0778 0.0389 5.8905 +-0.0889 0.0444 5.8905 +-0.1000 0.0500 5.8905 +primID: 3 +startangle_c: 15 +endpose_c: 5 -4 14 +additionalactioncostmult: 2 +intermediateposes: 10 +0.0000 0.0000 5.8905 +0.0298 -0.0184 5.8602 +0.0592 -0.0379 5.8299 +0.0881 -0.0583 5.7996 +0.1165 -0.0796 5.7693 +0.1444 -0.1019 5.7390 +0.1717 -0.1251 5.7087 +0.1984 -0.1492 5.6784 +0.2245 -0.1742 5.6481 +0.2500 -0.2000 5.6178 +primID: 4 +startangle_c: 15 +endpose_c: 7 -2 0 +additionalactioncostmult: 2 +intermediateposes: 10 +0.0000 0.0000 5.8905 +0.0377 -0.0156 5.8905 +0.0754 -0.0312 5.8905 +0.1131 -0.0468 5.8905 +0.1508 -0.0623 5.9096 +0.1893 -0.0758 5.9843 +0.2287 -0.0863 6.0590 +0.2687 -0.0939 6.1337 +0.3092 -0.0985 6.2085 +0.3500 -0.1000 6.2832 +primID: 5 +startangle_c: 15 +endpose_c: 0 0 14 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 5.8905 +0.0000 0.0000 5.8469 +0.0000 0.0000 5.8032 +0.0000 0.0000 5.7596 +0.0000 0.0000 5.7160 +0.0000 0.0000 5.6723 +0.0000 0.0000 5.6287 +0.0000 0.0000 5.5851 +0.0000 0.0000 5.5414 +0.0000 0.0000 5.4978 +primID: 6 +startangle_c: 15 +endpose_c: 0 0 0 +additionalactioncostmult: 20 +intermediateposes: 10 +0.0000 0.0000 5.8905 +0.0000 0.0000 5.9341 +0.0000 0.0000 5.9778 +0.0000 0.0000 6.0214 +0.0000 0.0000 6.0650 +0.0000 0.0000 6.1087 +0.0000 0.0000 6.1523 +0.0000 0.0000 6.1959 +0.0000 0.0000 6.2396 +0.0000 0.0000 0.0000 diff --git a/Controllers/Packages/amr_startup/config/mqtt_general.yaml b/Controllers/Packages/amr_startup/config/mqtt_general.yaml new file mode 100644 index 0000000..2f172c9 --- /dev/null +++ b/Controllers/Packages/amr_startup/config/mqtt_general.yaml @@ -0,0 +1,8 @@ +MQTT: + Name: T801 + Host: 172.20.235.170 + Port: 1885 + Client_ID: T801 + Username: robotics + Password: robotics + Keep_Alive: 60 \ No newline at end of file diff --git a/Controllers/Packages/amr_startup/config/pnkx_local_planner_params.yaml b/Controllers/Packages/amr_startup/config/pnkx_local_planner_params.yaml new file mode 100644 index 0000000..e0431be --- /dev/null +++ b/Controllers/Packages/amr_startup/config/pnkx_local_planner_params.yaml @@ -0,0 +1,188 @@ + +position_planner_name: PNKXLocalPlanner +docking_planner_name: PNKXDockingLocalPlanner +go_straight_planner_name: PNKXGoStraightLocalPlanner +rotate_planner_name: PNKXRotateLocalPlanner + +base_local_planner: nav_core_adapter::LocalPlannerAdapter +yaw_goal_tolerance: 0.017 +xy_goal_tolerance: 0.015 +stateful: true +publish_topic: true + +LocalPlannerAdapter: + PNKXLocalPlanner: + # Algorithm + trajectory_generator_name: mkt_plugins::LimitedAccelGenerator + algorithm_nav_name: mkt_algorithm::diff::PredictiveTrajectory + algorithm_rotate_name: mkt_algorithm::diff::RotateToGoal + # Goal checking + goal_checker_name: mkt_plugins::GoalChecker + + PNKXDockingLocalPlanner: + # Algorithm + trajectory_generator_name: mkt_plugins::LimitedAccelGenerator + algorithm_nav_name: mkt_algorithm::diff::PredictiveTrajectory + algorithm_rotate_name: mkt_algorithm::diff::RotateToGoal + # Goal checking + goal_checker_name: mkt_plugins::GoalChecker + + PNKXGoStraightLocalPlanner: + # Algorithm + trajectory_generator_name: mkt_plugins::LimitedAccelGenerator + algorithm_nav_name: mkt_algorithm::diff::GoStraight + # Goal checking + goal_checker_name: mkt_plugins::GoalChecker + + PNKXRotateLocalPlanner: + # Algorithm + algorithm_rotate_name: mkt_algorithm::diff::RotateToGoal + trajectory_generator_name: mkt_plugins::LimitedAccelGenerator + # Goal checking + goal_checker_name: mkt_plugins::SimpleGoalChecker + stateful: true + +LimitedAccelGenerator: + max_vel_x: 1.2 + min_vel_x: -1.2 + + max_vel_y: 0.0 # diff drive robot + min_vel_y: 0.0 # diff drive robot + + max_speed_xy: 2.0 # max_trans_vel: 0.8 # choose slightly less than the base's capability + min_speed_xy: 0.1 # min_trans_vel: 0.1 # this is the min trans velocity when there is negligible rotational velocity + + max_vel_theta: 0.3 # max_rot_vel: 1.0 # choose slightly less than the base's capability + min_vel_theta: 0.05 # min_rot_vel: 0.1 default: 0.4 # this is the min angular velocity when there is negligible translational velocity + + acc_lim_x: 1.0 + acc_lim_y: 0.0 # diff drive robot + acc_lim_theta: 0.5 + decel_lim_x: -2.0 + decel_lim_y: -0.0 + decel_lim_theta: -1.0 + + # Whether to split the path into segments or not + split_path: true + sim_time: 1.5 + vx_samples: 15 + vy_samples: 1 + vtheta_samples: 10 + discretize_by_time: true + angular_granularity: 0.05 + linear_granularity: 0.1 + # sim_period + include_last_point: true + +PredictiveTrajectory: + avoid_obstacles: false + xy_local_goal_tolerance: 0.01 + angle_threshold: 0.5 + index_samples: 60 + follow_step_path: true + + # Lookahead + use_velocity_scaled_lookahead_dist: true # Whether to use the velocity scaled lookahead distances or constant lookahead_distance. (default: false) + # only when false: + lookahead_dist: 0.325 # The lookahead distance (m) to use to find the lookahead point. (default: 0.6) + # only when true: + min_lookahead_dist: 0.5 # The minimum lookahead distance (m) threshold. (default: 0.3) + max_lookahead_dist: 1.5 # The maximum lookahead distance (m) threshold. (default: 0.9) + lookahead_time: 1.8 # The time (s) to project the velocity by, a.k.a. lookahead gain. (default: 1.5) + min_journey_squared: 0.3 # Minimum squared journey to consider for goal (default: 0.2) + max_journey_squared: 0.8 # Maximum squared journey to consider for goal (default: 0.2) + + # Rotate to heading param - onle one of use_rotate_to_heading and allow_reversing can be set to true + use_rotate_to_heading: true # Whether to enable rotating to rough heading and goal orientation when using holonomic planners. Recommended on for all robot types that can rotate in place. (default: true) + # only when true: + rotate_to_heading_min_angle: 0.03 # The difference in the path orientation and the starting robot orientation (radians) to trigger a rotate in place. (default: 0.785) + + # stoped + rot_stopped_velocity: 0.03 + trans_stopped_velocity: 0.06 + min_approach_linear_velocity: 0.05 # The minimum velocity (m/s) threshold to apply when approaching the goal to ensure progress. Must be > 0.01. (default: 0.05) + + # Regulated linear velocity scaling + use_regulated_linear_velocity_scaling: true # Whether to use the regulated features for path curvature (e.g. slow on high curvature paths). (default: true) + # only when true: + regulated_linear_scaling_min_radius: 0.6 # The turning radius (m) for which the regulation features are triggered. Remember, sharper turns have smaller radii. (default: 0.9) + regulated_linear_scaling_min_speed: 0.05 # The minimum speed (m/s) for which any of the regulated heuristics can send, to ensure process is still achievable even in high cost spaces with high curvature. Must be > 0.1. (default: 0.25) + + # Inflation cost scaling (Limit velocity by proximity to obstacles) + use_cost_regulated_linear_velocity_scaling: false # Whether to use the regulated features for proximity to obstacles (e.g. slow in close proximity to obstacles). (default: true) + inflation_cost_scaling_factor: 2.0 # (default: 3.0) # must be > 0 + cost_scaling_dist: 0.2 # (default: 0.6) + cost_scaling_gain: 2.0 # (default: 1.0) + +GoStraight: + avoid_obstacles: false + xy_local_goal_tolerance: 0.01 + angle_threshold: 0.6 + index_samples: 60 + follow_step_path: true + + # Lookahead + use_velocity_scaled_lookahead_dist: true # Whether to use the velocity scaled lookahead distances or constant lookahead_distance. (default: false) + # only when false: + lookahead_dist: 0.325 # The lookahead distance (m) to use to find the lookahead point. (default: 0.6) + # only when true: + min_lookahead_dist: 0.5 # The minimum lookahead distance (m) threshold. (default: 0.3) + max_lookahead_dist: 1.5 # The maximum lookahead distance (m) threshold. (default: 0.9) + lookahead_time: 1.8 # The time (s) to project the velocity by, a.k.a. lookahead gain. (default: 1.5) + min_journey_squared: 0.3 # Minimum squared journey to consider for goal (default: 0.2) + max_journey_squared: 0.6 # Minimum squared journey to consider for goal (default: 0.2) + + # Rotate to heading param - onle one of use_rotate_to_heading and allow_reversing can be set to true + use_rotate_to_heading: true # Whether to enable rotating to rough heading and goal orientation when using holonomic planners. Recommended on for all robot types that can rotate in place. (default: true) + # only when true: + rotate_to_heading_min_angle: 0.35 # The difference in the path orientation and the starting robot orientation (radians) to trigger a rotate in place. (default: 0.785) + # Speed + min_approach_linear_velocity: 0.05 # The minimum velocity (m/s) threshold to apply when approaching the goal to ensure progress. Must be > 0.01. (default: 0.05) + + # stoped + rot_stopped_velocity: 0.03 + trans_stopped_velocity: 0.06 + min_approach_linear_velocity: 0.05 # The minimum velocity (m/s) threshold to apply when approaching the goal to ensure progress. Must be > 0.01. (default: 0.05) + + use_regulated_linear_velocity_scaling: false + use_cost_regulated_linear_velocity_scaling: false + +RotateToGoal: + avoid_obstacles: false + xy_local_goal_tolerance: 0.01 + angle_threshold: 0.6 + index_samples: 60 + follow_step_path: true + + # Lookahead + use_velocity_scaled_lookahead_dist: true # Whether to use the velocity scaled lookahead distances or constant lookahead_distance. (default: false) + # only when false: + lookahead_dist: 0.325 # The lookahead distance (m) to use to find the lookahead point. (default: 0.6) + # only when true: + min_lookahead_dist: 0.5 # The minimum lookahead distance (m) threshold. (default: 0.3) + max_lookahead_dist: 1.5 # The maximum lookahead distance (m) threshold. (default: 0.9) + lookahead_time: 1.8 # The time (s) to project the velocity by, a.k.a. lookahead gain. (default: 1.5) + min_journey_squared: 0.3 # Minimum squared journey to consider for goal (default: 0.2) + max_journey_squared: 0.6 # Maximum squared journey to consider for goal (default: 0.2) + + # Rotate to heading param - onle one of use_rotate_to_heading and allow_reversing can be set to true + use_rotate_to_heading: true # Whether to enable rotating to rough heading and goal orientation when using holonomic planners. Recommended on for all robot types that can rotate in place. (default: true) + # only when true: + rotate_to_heading_min_angle: 0.03 # The difference in the path orientation and the starting robot orientation (radians) to trigger a rotate in place. (default: 0.785) + + # stoped + rot_stopped_velocity: 0.03 + trans_stopped_velocity: 0.06 + min_approach_linear_velocity: 0.05 # The minimum velocity (m/s) threshold to apply when approaching the goal to ensure progress. Must be > 0.01. (default: 0.05) + + # Regulated linear velocity scaling + use_regulated_linear_velocity_scaling: true # Whether to use the regulated features for path curvature (e.g. slow on high curvature paths). (default: true) + # only when true: + regulated_linear_scaling_min_radius: 0.6 # The turning radius (m) for which the regulation features are triggered. Remember, sharper turns have smaller radii. (default: 0.9) + regulated_linear_scaling_min_speed: 0.05 # The minimum speed (m/s) for which any of the regulated heuristics can send, to ensure process is still achievable even in high cost spaces with high curvature. Must be > 0.1. (default: 0.25) + + # Inflation cost scaling (Limit velocity by proximity to obstacles) + use_cost_regulated_linear_velocity_scaling: false # Whether to use the regulated features for proximity to obstacles (e.g. slow in close proximity to obstacles). (default: true) + inflation_cost_scaling_factor: 2.0 # (default: 3.0) # must be > 0 + cost_scaling_dist: 0.2 # (default: 0.6) + cost_scaling_gain: 2.0 # (default: 1.0) \ No newline at end of file diff --git a/Controllers/Packages/amr_startup/config/ros_diff_drive_controller.yaml b/Controllers/Packages/amr_startup/config/ros_diff_drive_controller.yaml new file mode 100755 index 0000000..cf1a0da --- /dev/null +++ b/Controllers/Packages/amr_startup/config/ros_diff_drive_controller.yaml @@ -0,0 +1,71 @@ +# ----------------------------------- +left_wheel : 'left_wheel_joint' +right_wheel : 'right_wheel_joint' +publish_rate: 50 # this is what the real MiR platform publishes (default: 50) +# These covariances are exactly what the real MiR platform publishes +pose_covariance_diagonal : [0.00001, 0.00001, 1000000.0, 1000000.0, 1000000.0, 1000.0] +twist_covariance_diagonal: [0.1, 0.1, 1000000.0, 1000000.0, 1000000.0, 1000.0] +enable_odom_tf: true +enable_wheel_tf: true +allow_multiple_cmd_vel_publishers: true +# open_loop: false +# Wheel separation and diameter. These are both optional. +# diff_drive_controller will attempt to read either one or both from the +# URDF if not specified as a parameter. +# We don't set the value here because it's different for each MiR type (100, 250, ...), and +# the plugin figures out the correct values. +wheel_separation : 0.5106 +wheel_radius : 0.1 + +# Wheel separation and radius multipliers +wheel_separation_multiplier: 1.0 # default: 1.0 +wheel_radius_multiplier : 1.0 # default: 1.0 + +# Velocity commands timeout [s], default 0.5 +cmd_vel_timeout: 1.0 + +# frame_ids (same as real MiR platform) +base_frame_id: base_footprint # default: base_link base_footprint +odom_frame_id: odom # default: odom + +# Velocity and acceleration limits +# Whenever a min_* is unspecified, default to -max_* +linear: + x: + has_velocity_limits : true + max_velocity : 1.5 # m/s; move_base max_vel_x: 0.8 + min_velocity : -1.5 # m/s + has_acceleration_limits: true + max_acceleration : 3.0 # m/s^2; move_base acc_lim_x: 1.5 + min_acceleration : -3.0 # m/s^2 + has_jerk_limits : true + max_jerk : 5.0 # m/s^3 +angular: + z: + has_velocity_limits : true + max_velocity : 0.6 # rad/s; move_base max_rot_vel: 1.0 + min_velocity : -0.6 + has_acceleration_limits: true + max_acceleration : 3.0 # rad/s^2; move_base acc_lim_th: 2.0 + has_jerk_limits : true + max_jerk : 2.5 # rad/s^3 + +left_wheel_joint: + lookup_name: WheelPlugin + max_publish_rate: 50 + mesurement_topic: left_encoder + frame_id: left_wheel_link + wheel_topic: /left_wheel + subscribe_queue_size: 1 + command_timeout: 5.0 + origin : [0.0, 0.255, 0.075, 0.0, 0.0, 0.0] # origin from base_frame_id + +right_wheel_joint: + lookup_name: WheelPlugin + max_publish_rate: 50 + mesurement_topic: right_encoder + frame_id: right_wheel_link + wheel_topic: /right_wheel + subscribe_queue_size: 1 + command_timeout: 5.0 + origin : [0.0, -0.255, 0.075, 0.0, 0.0, 0.0] # origin from base_frame_id diff --git a/Controllers/Packages/amr_startup/config/sbpl_global_params.yaml b/Controllers/Packages/amr_startup/config/sbpl_global_params.yaml new file mode 100755 index 0000000..ec2ac75 --- /dev/null +++ b/Controllers/Packages/amr_startup/config/sbpl_global_params.yaml @@ -0,0 +1,10 @@ +base_global_planner: SBPLLatticePlanner +SBPLLatticePlanner: + environment_type: XYThetaLattice + planner_type: ARAPlanner + allocated_time: 10.0 + initial_epsilon: 1.0 + force_scratch_limit: 10000 + forward_search: true + nominalvel_mpersecs: 0.2 + timetoturn45degsinplace_secs: 0.6 # = 0.6 rad/s diff --git a/Controllers/Packages/amr_startup/config/sick_line_guidance_mls.yaml b/Controllers/Packages/amr_startup/config/sick_line_guidance_mls.yaml new file mode 100644 index 0000000..959a137 --- /dev/null +++ b/Controllers/Packages/amr_startup/config/sick_line_guidance_mls.yaml @@ -0,0 +1,61 @@ +bus: + device: can0 + driver_plugin: can::SocketCANInterface + master_allocator: canopen::SimpleMaster::Allocator +sync: + interval_ms: 10 + overflow: 0 +# +# heartbeat: # simple heartbeat producer (optional, not supported by OLS or MLS, do not enable heartbeats) +# rate: 100 # heartbeat rate (1/rate in seconds) +# msg: "77f#05" # message to send, cansend format: heartbeat of node 127 with status 5=Started +nodes: + f_mlse: + id: 0x01 # CAN-Node-ID of can device, default: Node-ID 10=0x0A for OLS and MLS + eds_pkg: sick_line_guidance # package name for relative path + eds_file: mls/SICK-MLS.eds # path to EDS/DCF file + publish: ["1001","1018sub1","1018sub4","2021sub1!","2021sub2!","2021sub3!","2021sub4!","2022!"] + # MLS: 1001 = error register, 1018sub1 = VendorID, 1018sub4 = SerialNumber, TPDO1 = 0x2021sub1 to 0x2021sub4 and 0x2022 + + # sick_line_guidance configuration of this node: + sick_device_family: "MLS" # can devices of OLS10, OLS20 or MLS family currently supported + sick_topic: "f_mlse" # MLS_Measurement messages are published in topic "/mls" + subscribe_queue_size: 1 + sick_frame_id: "f_mlse" # MLS_Measurement messages are published frame_id "mls_measurement_frame" + + # device configuration of writable parameter by dcf_overlay: "objectindex": "parametervalue" + # example: "2027": "0x01" # Object 2027 (sensorFlipped, defaultvalue 0x00) will be configured with value 0x01 + # dcf_overlay: + # "2028sub1": "0x01" # UseMarkers (0 = disable marker detection, 1 = enable marker detection), UINT8, DataType=0x0005, defaultvalue=0 + # "2028sub2": "0x02" # MarkerStyle (0 = disable marker detection, 1 = standard mode, 2 = extended mode), UINT8, DataType=0x0005, defaultvalue=0 + # "2028sub3": "0x01" # FailSafeMode (0 = disabled, 1 = enabled), UINT8, DataType=0x0005, defaultvalue=0 + # "2025": "1000" # Min.Level, UINT16, DataType=0x0006, defaultvalue=600 + # "2026": "1" # Offset [mm] Nullpunkt, INT16, DataType=0x0003, defaultvalue=0 + # "2027": "0x01" # sensorFlipped, UINT8, DataType=0x0005, defaultvalue=0 + # "2029": "0x01" # LockTeach, UINT8, DataType=0x0005, defaultvalue=0 + # + + b_mlse: + id: 0x02 # CAN-Node-ID of can device, default: Node-ID 10=0x0A for OLS and MLS + eds_pkg: sick_line_guidance # package name for relative path + eds_file: mls/SICK-MLS.eds # path to EDS/DCF file + publish: ["1001","1018sub1","1018sub4","2021sub1!","2021sub2!","2021sub3!","2021sub4!","2022!"] + # MLS: 1001 = error register, 1018sub1 = VendorID, 1018sub4 = SerialNumber, TPDO1 = 0x2021sub1 to 0x2021sub4 and 0x2022 + + # sick_line_guidance configuration of this node: + sick_device_family: "MLS" # can devices of OLS10, OLS20 or MLS family currently supported + sick_topic: "b_mlse" # MLS_Measurement messages are published in topic "/mls" + subscribe_queue_size: 1 + sick_frame_id: "b_mlse" # MLS_Measurement messages are published frame_id "mls_measurement_frame" + + # device configuration of writable parameter by dcf_overlay: "objectindex": "parametervalue" + # example: "2027": "0x01" # Object 2027 (sensorFlipped, defaultvalue 0x00) will be configured with value 0x01 + # dcf_overlay: + # "2028sub1": "0x01" # UseMarkers (0 = disable marker detection, 1 = enable marker detection), UINT8, DataType=0x0005, defaultvalue=0 + # "2028sub2": "0x02" # MarkerStyle (0 = disable marker detection, 1 = standard mode, 2 = extended mode), UINT8, DataType=0x0005, defaultvalue=0 + # "2028sub3": "0x01" # FailSafeMode (0 = disabled, 1 = enabled), UINT8, DataType=0x0005, defaultvalue=0 + # "2025": "1000" # Min.Level, UINT16, DataType=0x0006, defaultvalue=600 + # "2026": "1" # Offset [mm] Nullpunkt, INT16, DataType=0x0003, defaultvalue=0 + # "2027": "0x01" # sensorFlipped, UINT8, DataType=0x0005, defaultvalue=0 + # "2029": "0x01" # LockTeach, UINT8, DataType=0x0005, defaultvalue=0 + # \ No newline at end of file diff --git a/Controllers/Packages/amr_startup/config/two_points_global_params.yaml b/Controllers/Packages/amr_startup/config/two_points_global_params.yaml new file mode 100644 index 0000000..ca0b60f --- /dev/null +++ b/Controllers/Packages/amr_startup/config/two_points_global_params.yaml @@ -0,0 +1,3 @@ +base_global_planner: TwoPointsPlanner +TwoPointsPlanner: + lethal_obstacle: 20 \ No newline at end of file diff --git a/Controllers/Packages/amr_startup/launch/amr_control.launch b/Controllers/Packages/amr_startup/launch/amr_control.launch new file mode 100644 index 0000000..269a45a --- /dev/null +++ b/Controllers/Packages/amr_startup/launch/amr_control.launch @@ -0,0 +1,50 @@ + + + + + + + + + + + + + + + [[0.412, -0.304], [0.412, 0.304], [-0.412, 0.304], [-0.412, -0.304]] + + + + [[0.511,-0.1955],[0.511,0.1955],[-0.511,0.1955],[-0.511,-0.1955]] + + + + [[0.583,-0.48],[0.583,0.48],[-0.583,0.48],[-0.583,-0.48]] + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Controllers/Packages/amr_startup/launch/amr_startup.launch b/Controllers/Packages/amr_startup/launch/amr_startup.launch new file mode 100644 index 0000000..5a53679 --- /dev/null +++ b/Controllers/Packages/amr_startup/launch/amr_startup.launch @@ -0,0 +1,102 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/Controllers/Packages/amr_startup/launch/fake_detected_shelf.launch b/Controllers/Packages/amr_startup/launch/fake_detected_shelf.launch new file mode 100755 index 0000000..51daad5 --- /dev/null +++ b/Controllers/Packages/amr_startup/launch/fake_detected_shelf.launch @@ -0,0 +1,5 @@ + + + + + diff --git a/Controllers/Packages/amr_startup/launch/hector_mapping.launch b/Controllers/Packages/amr_startup/launch/hector_mapping.launch new file mode 100644 index 0000000..d74fa3e --- /dev/null +++ b/Controllers/Packages/amr_startup/launch/hector_mapping.launch @@ -0,0 +1,65 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/Controllers/Packages/amr_startup/launch/includes/amcl.launch b/Controllers/Packages/amr_startup/launch/includes/amcl.launch new file mode 100644 index 0000000..3569a2c --- /dev/null +++ b/Controllers/Packages/amr_startup/launch/includes/amcl.launch @@ -0,0 +1,105 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Controllers/Packages/amr_startup/launch/includes/ekf.launch.xml b/Controllers/Packages/amr_startup/launch/includes/ekf.launch.xml new file mode 100644 index 0000000..1b77cca --- /dev/null +++ b/Controllers/Packages/amr_startup/launch/includes/ekf.launch.xml @@ -0,0 +1,7 @@ + + + + + + + diff --git a/Controllers/Packages/amr_startup/launch/includes/rviz.launch b/Controllers/Packages/amr_startup/launch/includes/rviz.launch new file mode 100644 index 0000000..b415953 --- /dev/null +++ b/Controllers/Packages/amr_startup/launch/includes/rviz.launch @@ -0,0 +1,3 @@ + + + \ No newline at end of file diff --git a/Controllers/Packages/amr_startup/launch/includes/spawn_maze.launch.xml b/Controllers/Packages/amr_startup/launch/includes/spawn_maze.launch.xml new file mode 100644 index 0000000..0c0063a --- /dev/null +++ b/Controllers/Packages/amr_startup/launch/includes/spawn_maze.launch.xml @@ -0,0 +1,5 @@ + + + + diff --git a/Controllers/Packages/amr_startup/launch/includes/spawn_trolley.launch b/Controllers/Packages/amr_startup/launch/includes/spawn_trolley.launch new file mode 100644 index 0000000..b223b41 --- /dev/null +++ b/Controllers/Packages/amr_startup/launch/includes/spawn_trolley.launch @@ -0,0 +1,11 @@ + + + + + + + + + diff --git a/Controllers/Packages/amr_startup/launch/robot_empty_world.launch b/Controllers/Packages/amr_startup/launch/robot_empty_world.launch new file mode 100644 index 0000000..af3ae07 --- /dev/null +++ b/Controllers/Packages/amr_startup/launch/robot_empty_world.launch @@ -0,0 +1,72 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Controllers/Packages/amr_startup/launch/robot_gazebo_common.launch b/Controllers/Packages/amr_startup/launch/robot_gazebo_common.launch new file mode 100644 index 0000000..3efd0c4 --- /dev/null +++ b/Controllers/Packages/amr_startup/launch/robot_gazebo_common.launch @@ -0,0 +1,59 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + [robot/joint_states] + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/Controllers/Packages/amr_startup/launch/robot_maze_world.launch b/Controllers/Packages/amr_startup/launch/robot_maze_world.launch new file mode 100644 index 0000000..7569152 --- /dev/null +++ b/Controllers/Packages/amr_startup/launch/robot_maze_world.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/Controllers/Packages/amr_startup/launch/sehc_world.launch b/Controllers/Packages/amr_startup/launch/sehc_world.launch new file mode 100644 index 0000000..1e35cc4 --- /dev/null +++ b/Controllers/Packages/amr_startup/launch/sehc_world.launch @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/Controllers/Packages/amr_startup/launch/start_maps.launch b/Controllers/Packages/amr_startup/launch/start_maps.launch new file mode 100644 index 0000000..aacc94c --- /dev/null +++ b/Controllers/Packages/amr_startup/launch/start_maps.launch @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + diff --git a/Controllers/Packages/amr_startup/launch/ware_houses_world.launch b/Controllers/Packages/amr_startup/launch/ware_houses_world.launch new file mode 100644 index 0000000..b05446f --- /dev/null +++ b/Controllers/Packages/amr_startup/launch/ware_houses_world.launch @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/Controllers/Packages/amr_startup/package.xml b/Controllers/Packages/amr_startup/package.xml new file mode 100644 index 0000000..0413dec --- /dev/null +++ b/Controllers/Packages/amr_startup/package.xml @@ -0,0 +1,59 @@ + + + amr_startup + 0.0.0 + The amr_startup package + + + + + robotics + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + + + + + + + + diff --git a/Controllers/Packages/amr_startup/rosconsole.config b/Controllers/Packages/amr_startup/rosconsole.config new file mode 100644 index 0000000..7c0221b --- /dev/null +++ b/Controllers/Packages/amr_startup/rosconsole.config @@ -0,0 +1,2 @@ +log4j.logger.ros.tf=ERROR +log4j.logger.ros.tf2=ERROR \ No newline at end of file diff --git a/Controllers/Packages/amr_startup/rviz/navigation.rviz b/Controllers/Packages/amr_startup/rviz/navigation.rviz new file mode 100644 index 0000000..1f16975 --- /dev/null +++ b/Controllers/Packages/amr_startup/rviz/navigation.rviz @@ -0,0 +1,646 @@ +Panels: + - Class: rviz/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Grid1/Offset1 + - /TF1/Frames1 + - /Global Map1/Straight Path1 + - /Local Map1 + - /Local Map1/Trajectory1 + - /Local Map1/Trajectory1/transform plan1 + - /Pose1 + Splitter Ratio: 0.37295082211494446 + Tree Height: 422 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Name: Time + SyncMode: 0 + SyncSource: Back LaserScan +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 5 + Y: 5 + Z: 0 + Plane: XY + Plane Cell Count: 100 + Reference Frame: map + Value: true + - Alpha: 0.4000000059604645 + Class: rviz/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: Map + Topic: /map + Unreliable: false + Use Timestamp: false + Value: true + - Alpha: 0.5 + Class: rviz/RobotModel + Collision Enabled: true + Enabled: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + Name: RobotModel + Robot Description: robot_description + TF Prefix: "" + Update Interval: 0 + Value: false + Visual Enabled: true + - Class: rviz/TF + Enabled: true + Filter (blacklist): "" + Filter (whitelist): "" + Frame Timeout: 15 + Frames: + All Enabled: false + back_nanoscan3_base_link: + Value: false + back_nanoscan3_sensor_link: + Value: false + base_footprint: + Value: true + base_link: + Value: false + bl_caster_rotation_link: + Value: false + bl_caster_wheel_link: + Value: false + br_caster_rotation_link: + Value: false + br_caster_wheel_link: + Value: false + camera_link: + Value: false + fl_caster_rotation_link: + Value: false + fl_caster_wheel_link: + Value: false + fr_caster_rotation_link: + Value: false + fr_caster_wheel_link: + Value: false + front_nanoscan3_base_link: + Value: false + front_nanoscan3_sensor_link: + Value: false + imu_frame: + Value: false + imu_link: + Value: false + left_wheel_link: + Value: false + lifting_link: + Value: false + map: + Value: true + odom: + Value: true + right_wheel_link: + Value: false + surface: + Value: false + Marker Alpha: 1 + Marker Scale: 0.5 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + map: + odom: + base_footprint: + base_link: + back_nanoscan3_base_link: + back_nanoscan3_sensor_link: + {} + bl_caster_rotation_link: + bl_caster_wheel_link: + {} + br_caster_rotation_link: + br_caster_wheel_link: + {} + camera_link: + {} + fl_caster_rotation_link: + fl_caster_wheel_link: + {} + fr_caster_rotation_link: + fr_caster_wheel_link: + {} + front_nanoscan3_base_link: + front_nanoscan3_sensor_link: + {} + imu_link: + imu_frame: + {} + left_wheel_link: + {} + lifting_link: + surface: + {} + right_wheel_link: + {} + Update Interval: 0 + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/LaserScan + Color: 204; 0; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: Front LaserScan + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 6 + Size (m): 0.09000000357627869 + Style: Points + Topic: f_scan + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/LaserScan + Color: 0; 255; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: Back LaserScan + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Points + Topic: /b_scan + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Arrow Length: 0.20000000298023224 + Axes Length: 0.30000001192092896 + Axes Radius: 0.009999999776482582 + Class: rviz/PoseArray + Color: 0; 192; 0 + Enabled: false + Head Length: 0.07000000029802322 + Head Radius: 0.029999999329447746 + Name: Amcl Particle Swarm + Queue Size: 10 + Shaft Length: 0.23000000417232513 + Shaft Radius: 0.009999999776482582 + Shape: Arrow (Flat) + Topic: particlecloud + Unreliable: false + Value: false + - Class: rviz/Group + Displays: + - Alpha: 0.5 + Class: rviz/Map + Color Scheme: costmap + Draw Behind: true + Enabled: true + Name: Costmap + Topic: /amr_node/global_costmap/costmap + Unreliable: false + Use Timestamp: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 25; 255; 0 + Enabled: false + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.20000000298023224 + Line Style: Lines + Line Width: 0.05000000074505806 + Name: Full Plan + Offset: + X: 0 + Y: 0 + Z: 0.8999999761581421 + Pose Color: 255; 85; 255 + Pose Style: Axes + Queue Size: 10 + Radius: 0.019999999552965164 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /amr_node/SBPLLatticePlanner/plan + Unreliable: false + Value: false + - Alpha: 1 + Class: rviz/Polygon + Color: 255; 0; 0 + Enabled: true + Name: Footprint + Queue Size: 10 + Topic: move_base_node/global_costmap/footprint + Unreliable: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 25; 255; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.00800000037997961 + Line Style: Billboards + Line Width: 0.009999999776482582 + Name: CustomPath + Offset: + X: 0 + Y: 0 + Z: 0.8999999761581421 + Pose Color: 255; 85; 255 + Pose Style: None + Queue Size: 10 + Radius: 0.004999999888241291 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /amr_node/CustomPlanner/plan + Unreliable: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 0; 0; 0 + Enabled: false + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Straight Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: Axes + Queue Size: 10 + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /amr_node/LocalPlannerAdapter/MKTGoStraightLocalPlanner/global_plan + Unreliable: false + Value: false + Enabled: true + Name: Global Map + - Class: rviz/Group + Displays: + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 252; 175; 62 + Enabled: false + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.03999999910593033 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Global Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: Axes + Queue Size: 10 + Radius: 0.009999999776482582 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /amr_node/LocalPlannerAdapter/global_plan + Unreliable: false + Value: false + - Alpha: 0.699999988079071 + Class: rviz/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: Costmap + Topic: /amr_node/local_costmap/costmap + Unreliable: false + Use Timestamp: false + Value: true + - Alpha: 1 + Class: rviz/Polygon + Color: 25; 255; 0 + Enabled: true + Name: Footprint + Queue Size: 10 + Topic: /amr_node/local_costmap/footprint + Unreliable: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 138; 226; 52 + Enabled: false + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.029999999329447746 + Line Style: Billboards + Line Width: 0.029999999329447746 + Name: Local Plan + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: Axes + Queue Size: 10 + Radius: 0.009999999776482582 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /amr_node/LocalPlannerAdapter/transformed_global_plan + Unreliable: false + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: /amr_node/PredictiveTrajectory/cost_cloud + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Class: rviz/Group + Displays: + - Alpha: 1 + Axes Length: 0.10000000149011612 + Axes Radius: 0.029999999329447746 + Class: rviz/Pose + Color: 0; 0; 0 + Enabled: true + Head Length: 0.05000000074505806 + Head Radius: 0.05000000074505806 + Name: SubGoalPose + Queue Size: 10 + Shaft Length: 0.029999999329447746 + Shaft Radius: 0.029999999329447746 + Shape: Arrow + Topic: /amr_node/sub_goal + Unreliable: false + Value: true + - Alpha: 1 + Axes Length: 0.30000001192092896 + Axes Radius: 0.10000000149011612 + Class: rviz/Pose + Color: 252; 233; 79 + Enabled: false + Head Length: 0.05000000074505806 + Head Radius: 0.05000000074505806 + Name: ClosetPose + Queue Size: 10 + Shaft Length: 0.029999999329447746 + Shaft Radius: 0.029999999329447746 + Shape: Arrow + Topic: /amr_node/closet_robot_goal + Unreliable: false + Value: false + - Alpha: 1 + Axes Length: 0.05000000074505806 + Axes Radius: 0.019999999552965164 + Class: rviz/Pose + Color: 173; 127; 168 + Enabled: false + Head Length: 0.05000000074505806 + Head Radius: 0.05000000074505806 + Name: Look A Head Pose + Queue Size: 10 + Shaft Length: 0.05000000074505806 + Shaft Radius: 0.029999999329447746 + Shape: Axes + Topic: /amr_node/lookahead_point + Unreliable: false + Value: false + - Alpha: 1 + Arrow Length: 0.30000001192092896 + Axes Length: 0.30000001192092896 + Axes Radius: 0.029999999329447746 + Class: rviz/PoseArray + Color: 0; 0; 0 + Enabled: true + Head Length: 0.07000000029802322 + Head Radius: 0.05000000074505806 + Name: reached intermediated goals + Queue Size: 10 + Shaft Length: 0.23000000417232513 + Shaft Radius: 0.029999999329447746 + Shape: Axes + Topic: /amr_node/reached_intermediate_goals + Unreliable: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 245; 121; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.019999999552965164 + Length: 0.029999999329447746 + Line Style: Billboards + Line Width: 0.009999999776482582 + Name: transform plan + Offset: + X: 0 + Y: 0 + Z: 0.5 + Pose Color: 138; 226; 52 + Pose Style: Axes + Queue Size: 10 + Radius: 0.019999999552965164 + Shaft Diameter: 0.009999999776482582 + Shaft Length: 0.009999999776482582 + Topic: /amr_node/transformed_plan + Unreliable: false + Value: true + - Class: rviz/Marker + Enabled: false + Marker Topic: /amr_node/PredictiveTrajectory/cost_left_goals + Name: L + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /amr_node/PredictiveTrajectory/cost_right_goals + Name: R + Namespaces: + {} + Queue Size: 100 + Value: false + Enabled: true + Name: Trajectory + Enabled: true + Name: Local Map + - Alpha: 1 + Axes Length: 0.05000000074505806 + Axes Radius: 0.014999999664723873 + Class: rviz/Pose + Color: 46; 52; 54 + Enabled: true + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Name: Pose + Queue Size: 10 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Shape: Axes + Topic: /amr_node/current_goal + Unreliable: false + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/MoveCamera + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/Select + - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 + Topic: initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: move_base_simple/goal + - Class: rviz/Measure + - Class: rviz/PublishPoint + Single click: true + Topic: clicked_point + Value: true + Views: + Current: + Angle: -3.135005474090576 + Class: rviz/TopDownOrtho + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Scale: -359.3964538574219 + Target Frame: base_link + X: -0.49439820647239685 + Y: 0.196189746260643 + Saved: + - Angle: -34.55989074707031 + Class: rviz/TopDownOrtho + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Invert Z Axis: false + Name: TopDownOrtho + Near Clip Distance: 0.009999999776482582 + Scale: -132.97349548339844 + Target Frame: base_link + X: 34.338645935058594 + Y: 35.28913879394531 +Window Geometry: + Displays: + collapsed: false + Height: 573 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd000000040000000000000156000001e3fc0200000008fb000000100044006900730070006c006100790073000000003d000001e3000000c900fffffffb0000001200530065006c0065006300740069006f006e000000003d000002590000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001b500000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb0000000a0049006d006100670065000000003d0000039e0000000000000000fb0000000c00430061006d006500720061020000007c0000013600000280000001e0fb0000000c00430061006d0065007200610200000644000000b7000002c1000002f9000000010000010f00000259fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000259000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b20000000000000000000000020000041800000168fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007520000003efc0100000002fb0000000800540069006d0065000000000000000752000003bc00fffffffb0000000800540069006d0065010000000000000450000000000000000000000418000001e300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1048 + X: 0 + Y: 21 diff --git a/Controllers/Packages/amr_startup/sdf/maze/model.config b/Controllers/Packages/amr_startup/sdf/maze/model.config new file mode 100644 index 0000000..9a00866 --- /dev/null +++ b/Controllers/Packages/amr_startup/sdf/maze/model.config @@ -0,0 +1,11 @@ + + + maze + 1.0 + model.sdf + + Martin Günther + martin.guenther@dfki.de + + + diff --git a/Controllers/Packages/amr_startup/sdf/maze/model.sdf b/Controllers/Packages/amr_startup/sdf/maze/model.sdf new file mode 100644 index 0000000..b31cc0b --- /dev/null +++ b/Controllers/Packages/amr_startup/sdf/maze/model.sdf @@ -0,0 +1,345 @@ + + + + -0.078283 0.098984 0 0 -0 0 + + + + + 20 0.15 2.5 + + + 0 0 1.25 0 -0 0 + + + 0 0 1.25 0 -0 0 + + + 20 0.15 2.5 + + + + + 1 1 1 1 + + + 0.030536 9.925 0 0 -0 0 + + + + + + 20 0.15 2.5 + + + 0 0 1.25 0 -0 0 + + + 0 0 1.25 0 -0 0 + + + 20 0.15 2.5 + + + + + 1 1 1 1 + + + 9.95554 0 0 0 0 -1.5708 + + + + + + 20 0.15 2.5 + + + 0 0 1.25 0 -0 0 + + + 0 0 1.25 0 -0 0 + + + 20 0.15 2.5 + + + + + 1 1 1 1 + + + 0.030536 -9.925 0 0 -0 3.14159 + + + + + + 1.5 0.15 2.5 + + + 0 0 1.25 0 -0 0 + + + 0 0 1.25 0 -0 0 + + + 1.5 0.15 2.5 + + + + + 1 1 1 1 + + + 5.35089 3.21906 0 0 -0 3.14159 + + + + + + 5.25 0.15 2.5 + + + 0 0 1.25 0 -0 0 + + + 0 0 1.25 0 -0 0 + + + 5.25 0.15 2.5 + + + + + 1 1 1 1 + + + 4.67589 5.76906 0 0 -0 1.5708 + + + + + + 5.5 0.15 2.5 + + + 0 0 1.25 0 -0 0 + + + 0 0 1.25 0 -0 0 + + + 5.5 0.15 2.5 + + + + + 1 1 1 1 + + + 7.10914 4.73454 0 0 0 -1.5708 + + + + + + 3 0.15 2.5 + + + 0 0 1.25 0 -0 0 + + + 0 0 1.25 0 -0 0 + + + 3 0.15 2.5 + + + + + 1 1 1 1 + + + 8.53414 2.05954 0 0 -0 0 + + + + + + 20 0.15 2.5 + + + 0 0 1.25 0 -0 0 + + + 0 0 1.25 0 -0 0 + + + 20 0.15 2.5 + + + + + 1 1 1 1 + + + -9.89446 0 0 0 -0 1.5708 + + + + + + 5.5 0.15 2.5 + + + 0 0 1.25 0 -0 0 + + + 0 0 1.25 0 -0 0 + + + 5.5 0.15 2.5 + + + + + 1 1 1 1 + + + -4.35914 -2.82889 0 0 0 -1.5708 + + + + + + 5.75 0.15 2.5 + + + 0 0 1.25 0 -0 0 + + + 0 0 1.25 0 -0 0 + + + 5.75 0.15 2.5 + + + + + 1 1 1 1 + + + -7.15914 -5.50389 0 0 -0 3.14159 + + + + + + 16 0.15 2.5 + + + 0 0 1.25 0 -0 0 + + + 0 0 1.25 0 -0 0 + + + 16 0.15 2.5 + + + + + 1 1 1 1 + + + -1.89911 1.86906 0 0 -0 0 + + + + + + 1.5 0.15 2.5 + + + 0 0 1.25 0 -0 0 + + + 0 0 1.25 0 -0 0 + + + 1.5 0.15 2.5 + + + + + 1 1 1 1 + + + 6.02589 2.54406 0 0 -0 1.5708 + + + + + + 0.15 0.15 2.5 + + + 0 0 1.25 0 -0 0 + + + 0 0 1.25 0 -0 0 + + + 0.15 0.15 2.5 + + + + + 1 1 1 1 + + + 6.02589 3.21906 0 0 -0 0 + + 1 + + diff --git a/Controllers/Packages/nova5_control/CMakeLists.txt b/Controllers/Packages/nova5_control/CMakeLists.txt new file mode 100644 index 0000000..5e9aad5 --- /dev/null +++ b/Controllers/Packages/nova5_control/CMakeLists.txt @@ -0,0 +1,145 @@ +cmake_minimum_required(VERSION 3.0.2) +project(nova5_control) + +## Compile as C++11, supported in ROS Kinetic and newer +add_compile_options(-std=c++17) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages + +find_package(catkin REQUIRED COMPONENTS + roscpp + std_msgs + modbus_tcp +) + +find_package(Boost REQUIRED COMPONENTS system) + + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( + INCLUDE_DIRS include + LIBRARIES nova5_control + CATKIN_DEPENDS roscpp std_msgs modbus_tcp + DEPENDS Boost +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations ${GSTREAMER_LIBRARIES} +include_directories( + include + ${catkin_INCLUDE_DIRS} +) + +add_library(${PROJECT_NAME} src/nova5_modbus.cpp src/imr_nova_control.cpp) +add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${GSTREAMER_LIBRARIES}) + + + +add_executable(test_nova example/simple.cpp) +add_dependencies(test_nova ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(test_nova ${catkin_LIBRARIES} nova5_control) + +add_executable(test_thread_nova example/test_thread.cpp) +add_dependencies(test_thread_nova ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(test_thread_nova ${catkin_LIBRARIES} nova5_control) + +## Declare a C++ library +# add_library(${PROJECT_NAME} +# src/${PROJECT_NAME}/nova5_control.cpp +# ) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +## With catkin_make all packages are built within a single CMake context +## The recommended prefix ensures that target names across packages don't collide +# add_executable(${PROJECT_NAME}_node src/nova5_control_node.cpp) + +## Rename C++ executable without prefix +## The above recommended prefix causes long target names, the following renames the +## target back to the shorter version for ease of user use +## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" +# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +# target_link_libraries(${PROJECT_NAME}_node +# ${catkin_LIBRARIES} +# ) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# catkin_install_python(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html +# install(TARGETS ${PROJECT_NAME}_node +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark libraries for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) + +## Mark cpp header files for installation +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE +) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_nova5_control.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/Controllers/Packages/nova5_control/README.md b/Controllers/Packages/nova5_control/README.md new file mode 100644 index 0000000..78cde28 --- /dev/null +++ b/Controllers/Packages/nova5_control/README.md @@ -0,0 +1,42 @@ +# Reference +https://docs.google.com/spreadsheets/d/1LkLH5IGXDXprNB6_w0-2-jNupRHEH-4PBC6LIrh3G1I/edit?gid=0#gid=0 +https://www.dobot-robots.com/service/download-center?keyword=&products%5B%5D=535 +https://tranduc-my.sharepoint.com/:w:/g/personal/tranduong1692_tranduc_onmicrosoft_com/EVSb3HeS-HZPlmAMflUsuLMBLrq5_Dnm5ku2Kj9Bu8C8Rw?e=FEROOU +# Require +Clone: http://git.pnkx/HiepLM/modbus.git + http://git.pnkx/HiepLM/libserial.git +# Guide + // Nên chạy trong một thread// + // Khởi tạo cổng kết nối robot thông qua modbus tcp // + cnt_nova5 *nova5 = nullptr; + nova5 = new cnt_nova5(IP,PORT); + //==================================================// + + // Quy trình kết nối robot // + if((*nova5).nova_connect()) + { + // Kiểm tra xem robot có đang trong trạng thái EMC không + if((*nova5).nova_robotMode() == 3) (*nova5).nova_powerOn(); + + // Sau khi kết nối thành công cần enable robot + if((*nova5).nova_robotMode() == 4) (*nova5).nova_enable(); + + // Sau khi enable robot thành công cần bắt đầu chương trình của ccbox + if((*nova5).nova_robotMode() == 5) (*nova5).nova_startProgram(); + } + else (*nova5).nova_close(); + //==========================// + + // Sử dụng hàm nova_movePoint() cần đưa vào vòng lặp chờ đến khi robot di chuyển xong và trả về phản hồi, sau đó mới được đưa ra lệnh điều khiển tiếp theo + + // Ví dụ hàm nova_movePoint() // + if(count == 0) + { + // Set tốc độ global tay máy + nova5->nova_speedGlobal(50.0); + + if(nova5->nova_movePoint(ORIGIN_COOR)==1 && count == 0) count ++; + } + //============================// + + diff --git a/Controllers/Packages/nova5_control/example/simple.cpp b/Controllers/Packages/nova5_control/example/simple.cpp new file mode 100644 index 0000000..a76db13 --- /dev/null +++ b/Controllers/Packages/nova5_control/example/simple.cpp @@ -0,0 +1,500 @@ +#include"nova5_control/nova5_modbus.h" +#include +#include + + + +char getKey() +{ + struct termios oldt, newt; + char ch; + tcgetattr(STDIN_FILENO, &oldt); + newt = oldt; + newt.c_lflag &= ~(ICANON | ECHO); + tcsetattr(STDIN_FILENO, TCSANOW, &newt); + ch = getchar(); + tcsetattr(STDIN_FILENO, TCSANOW, &oldt); + return ch; +} + + +int main(int argc,char **argv) +{ + ros::init(argc,argv,"EXAMPLE"); + ros::NodeHandle nh("~"); + + std::string IP = "192.168.2.6"; + int PORT = 502; + + + // Coordinates + std::vector HOME = {-141.91,301.01,524.27,-179.4,-0.04,0.0}; + std::vector CENTER_P = {309.11,63.29,524.27,-179.4,-0.04,0.00}; + std::vector ORIGIN_COOR = {150.71,-435.9,524.27,-179.4,-0.04,-90.00}; + std::vector OK_COOR = {-141.91,301.01,150,-179.4,-0.04,0.0}; + std::vector NG_COOR = {-141.91,657.01,150,-179.4,-0.04,0.0}; + + std::vector TEST_COOR = {250.71,-480.9,424.27,-179.4,-0.04,-90.00}; + + std::vector STORE_COOR(6); + std::vector Point(6); + std::vector Point_current(6); + std::vector Center_temp(6); + std::vector Point_temp1(6); + std::vector Point_temp2(6); + std::vector Point_temp3(6); + + uint16_t isCheck = 0; + int timeStamp_cam = 0; + static int count = 0, count_1 = 0; + static int center_count = 0; + static int get_point_current =0; + static int ob = 0; + static uint16_t speed = 0; + static int count_ok = 0; + static int count_ng = 0; + bool connect = false; + bool drag = false; + bool flag_home = false; + bool poweroff = false; + + // Khởi tạo + std::shared_ptr nova5_ptr_ = nullptr; + nova5_ptr_ = std::make_shared(IP,PORT); + + // Kết nối đến robot nova + connect = nova5_ptr_->nova_connect(); + if(connect) + { + if(nova5_ptr_->nova_robotMode() == 3) + { + if(nova5_ptr_->nova_powerOn() == -1) + { + ROS_ERROR("Please check robot arm"); + return -1;// biến tạm để cho thấy robot bị lỗi, thay thành các biến phản hồi lỗi + } + } + // Sau khi kết nối thành công cần enable robot + nova5_ptr_->nova_enable(); + + // Sau khi enable robot thành công cần bắt đầu chương trình của ccbox + nova5_ptr_->nova_startProgram(); + } + else + { + // alarm = nova5_ptr_->nova_statusAlarm(); + ROS_WARN("Robot is reconnecting with maximum time 15 minutes..."); + for(int i=0; i<180U; ++i) + { + connect = nova5_ptr_->nova_connect(); + if(connect || !ros::ok()) + { + while(ros::ok()) + { + double mode = nova5_ptr_->nova_robotMode(); + if(nova5_ptr_->nova_statusAlarm() == 1 && mode == 9) ROS_ERROR("Kinematic ERROR! Please check robot!"); + if(mode == 3) + { + if(nova5_ptr_->nova_powerOn() == -1) + { + ROS_ERROR("Please check robot arm"); + return -1;// biến tạm để cho thấy robot bị lỗi, thay thành các biến phản hồi lỗi + } + } + if(mode == 4) break; + ros::Duration(5).sleep(); + } + + // Sau khi kết nối thành công cần enable robot + nova5_ptr_->nova_enable(); + + // Sau khi enable robot thành công cần bắt đầu chương trình của ccbox + nova5_ptr_->nova_startProgram(); + + break; + } + ros::Duration(5).sleep(); + } + + } + + ros::Rate loop_rate(100); + + + //===========================Vòng lặp chính========================// + //=================================================================// + //=================================================================// + while(ros::ok() && connect) + { + static int temp = 0; + static bool checkconnect = false; + checkconnect = nova5_ptr_->nova_checkConnected(); + if(checkconnect) + { + double robotmode = nova5_ptr_->nova_robotMode(); + + //===Kiểm tra trạng thái robot===// + //===============================// + if( robotmode == 3) + { + poweroff = true; + // Power on robot + if(nova5_ptr_->nova_powerOn() == 1) + { + // Sau khi power on cần enable robot + nova5_ptr_->nova_enable(); + nova5_ptr_->nova_startProgram(); + } + } + + else if(robotmode == 6) + { + ROS_WARN("Robot is in drag mode"); + drag = true; + } + + if(drag == true && robotmode == 5) + { + ROS_WARN("PRESS: n to continute"); + ROS_WARN("PRESS: h to go home"); + char key = getKey(); + ROS_WARN("%d", key); + /*Kiểm tra nếu key = n hoặc N thì thực hiện lệnh tiếp tục*/ + if(key == 110 || key == 78) + { + nova5_ptr_->nova_startProgram(); + drag = false; + get_point_current = 0; + count = 0; + center_count = 0; + flag_home = false; + } + /*Kiểm tra nếu key = h hoặc H thì thực hiện lệnh về home */ + else if(key == 104) + { + nova5_ptr_->nova_startProgram(); + drag = false; + get_point_current = 0; + center_count = 0; + ROS_INFO("Robot is comming home..."); + flag_home = true; + } + } + //===============================// + //===============================// + + + //===Chu trình gắp phôi và lưu trữ phôi===// + //========================================// + /* Bước 0 ve home */ + if(center_count == 0) + { + if(get_point_current == 0) + { + //Đọc tọa độ robot hiện tại trong vòng 6s + + nova5_ptr_->nova_getCoorcurrent(Point_current); + + ROS_ERROR("x current: %f", Point_current[0]); + ROS_ERROR("y current: %f", Point_current[1]); + ROS_ERROR("z current: %f", Point_current[2]); + + if(CENTER_P[2] > (Point_current[2] + 10)) + { + Center_temp[0] = Point_current[0]; + Center_temp[1] = Point_current[1]; + Center_temp[2] = nova5_ptr_->nova_check_z(Point_current[0],Point_current[1],CENTER_P[2],0); + Center_temp[3] = Point_current[3]; + Center_temp[4] = Point_current[4]; + Center_temp[5] = Point_current[5]; + } + else if(CENTER_P[2] < (Point_current[2] - 10)) + { + Center_temp[0] = CENTER_P[0]; + Center_temp[1] = CENTER_P[1]; + Center_temp[2] = nova5_ptr_->nova_check_z(CENTER_P[0],CENTER_P[1],Point_current[2], 0); + Center_temp[3] = CENTER_P[3]; + Center_temp[4] = CENTER_P[4]; + Center_temp[5] = CENTER_P[5]; + } + else if(CENTER_P[2] >= (Point_current[2] - 10) && CENTER_P[2] < (Point_current[2] + 10)) center_count = 1; + + ROS_WARN("x : %f", Center_temp[0]); + ROS_WARN("y : %f", Center_temp[1]); + ROS_WARN("z : %f", Center_temp[2]); + get_point_current = 1; + } + + if(center_count == 0) + { + if(nova5_ptr_->nova_movePoint(Center_temp,100,1)==1 && center_count == 0) center_count++; + } + } + + /* Bước 1 */ + else if(center_count == 1) + { + if(nova5_ptr_->nova_movePoint(CENTER_P,100,1)==1 && center_count == 1) + { + center_count = 2; + } + } + + if(flag_home && center_count == 2) + { + if(nova5_ptr_->nova_movePoint(HOME,100,1)==1 && center_count == 2) + { + ROS_INFO("Complete"); + // nova5_ptr_->nova_stopProgram(); + // nova5_ptr_->nova_close(); + return 0; + } + } + + /* Bước 2 di chuyển cánh tay đến vị trí chụp ảnh */ + if(!flag_home && center_count == 2 && count == 0) + { + if(nova5_ptr_->nova_movePoint(ORIGIN_COOR,100,1)==1 && count == 0) + { + count++ ; + } + } + + /* Bước 3 xác định vị trí vật thể và di chuyển tới vị trí trên vật thể */ + else if(!flag_home && center_count == 2 && count == 1) + { + static bool continue_flag = false; + if(temp == 0) + { + + // Đọc tọa độ vật trong thời gian max là 10s + while( ros::ok() && !nova5_ptr_->nova_statusAlarm()) + { + if(!nova5_ptr_->nova_checkConnected() || !ros::ok() || nova5_ptr_->nova_statusAlarm() || nova5_ptr_->nova_robotMode() != 7) + { + continue_flag = true; + break; + } + ros::Duration(1).sleep(); + ROS_INFO("Read camera"); + + if(nova5_ptr_->nova_readCoordinates(Point,ORIGIN_COOR,isCheck,timeStamp_cam) == 1) + { + // Lấy thời gian của máy tính hiện tại + ros::Time current_time = ros::Time::now(); + // Chỉ lấy phần giây và ép kiểu sang int + int seconds = static_cast(current_time.sec); + + // Dữ liệu cũ không được cập nhật, chờ cho đến khi nhận được dữ liệu mới nhất + while ((seconds - timeStamp_cam) >= TIME_ERR && ros::ok()) + { + ROS_WARN("Data is out of date!"); + ROS_WARN("Wait for data update or press ctrl + c to cancel!"); + + // Lấy thời gian của máy tính hiện tại + ros::Time current_time = ros::Time::now(); + // Chỉ lấy phần giây và ép kiểu sang int + int seconds = static_cast(current_time.sec); + + nova5_ptr_->nova_readCoordinates(Point,ORIGIN_COOR,isCheck,timeStamp_cam); + // Nếu khoảng thời giữa 2 lần gửi dữ liệu < 1s thì được coi là hợp lệ + if((seconds - timeStamp_cam) < TIME_ERR){ + nova5_ptr_->nova_readCoordinates(Point,ORIGIN_COOR,isCheck,timeStamp_cam); + break; + } + if(!nova5_ptr_->nova_checkConnected() || nova5_ptr_->nova_robotMode() == 6) + { + ROS_WARN("Robot is in drag mode"); + continue_flag = true; + break; + } + ros::Duration(1).sleep(); + } + + // Set tọa độ cho điểm trung gian 1 + Point_temp1[0] = Point[0]; + Point_temp1[1] = Point[1]; + Point_temp1[2] = nova5_ptr_->nova_check_z(Point[0],Point[1],ORIGIN_COOR[2], 0); + Point_temp1[3] = Point[3]; + Point_temp1[4] = Point[4]; + Point_temp1[5] = Point[5]; + temp = 1; + break; + } + + } + + if(continue_flag) + { + continue_flag = false; + continue; + } + + // Point_temp1[0] = TEST_COOR[0]; + // Point_temp1[1] = TEST_COOR[1]; + // Point_temp1[2] = nova5_ptr_->nova_check_z(TEST_COOR[0],TEST_COOR[1],ORIGIN_COOR[2], 0); + // Point_temp1[3] = TEST_COOR[3]; + // Point_temp1[4] = TEST_COOR[4]; + // Point_temp1[5] = TEST_COOR[5]; + temp =1; + } + + if(count == 1 && continue_flag == false) + { + if(nova5_ptr_->nova_movePoint(Point_temp1,80,1) == 1) count++; + } + } + + /* Bước 4 di chuyển tới vị trí vật thể */ + else if(!flag_home && center_count == 2 && count == 2) + { + if(nova5_ptr_->nova_movePoint(Point,20,0)== 1 && count == 2) + { + nova5_ptr_->nova_onTool(); + count++; + } + } + + /* Bước 5 di chuyển tới vị trí trên vật thể */ + else if(!flag_home && center_count == 2 && count == 3) + { + if(nova5_ptr_->nova_movePoint(Point_temp1,40,0)== 1 && count == 3) + { + count++; + } + } + + /* Bước 6 di chuyển tới vị trí điểm home */ + else if(!flag_home && center_count == 2 &&count == 4) + { + if(nova5_ptr_->nova_movePoint(CENTER_P,100,1)==1 && count == 4) + { + + // Nếu vật thể là NG thì set điểm lưu trữ NG_COOR + if(isCheck == 0) + { + NG_COOR[0]+=55; + + STORE_COOR[0] = NG_COOR[0]; + STORE_COOR[1] = NG_COOR[1]; + STORE_COOR[2] = NG_COOR[2]; + STORE_COOR[3] = NG_COOR[3]; + STORE_COOR[4] = NG_COOR[4]; + STORE_COOR[5] = NG_COOR[5]; + + Point_temp3[0] = NG_COOR[0]; + Point_temp3[1] = NG_COOR[1]; + Point_temp3[2] = nova5_ptr_->nova_check_z(NG_COOR[0],NG_COOR[1],CENTER_P[2], 0); + Point_temp3[3] = NG_COOR[3]; + Point_temp3[4] = NG_COOR[4]; + Point_temp3[5] = NG_COOR[5]; + + speed = 30; + count_ng++; + } + // Nếu vật thể là OK thì set điểm lưu trữ OK_COOR + else if(isCheck == 1) + { + OK_COOR[0]+=55; + + STORE_COOR[0] = OK_COOR[0]; + STORE_COOR[1] = OK_COOR[1]; + STORE_COOR[2] = OK_COOR[2]; + STORE_COOR[3] = OK_COOR[3]; + STORE_COOR[4] = OK_COOR[4]; + STORE_COOR[5] = OK_COOR[5]; + + Point_temp3[0] = OK_COOR[0]; + Point_temp3[1] = OK_COOR[1]; + Point_temp3[2] = nova5_ptr_->nova_check_z(OK_COOR[0],OK_COOR[1],CENTER_P[2], 0); + Point_temp3[3] = OK_COOR[3]; + Point_temp3[4] = OK_COOR[4]; + Point_temp3[5] = OK_COOR[5]; + + speed = 20; + count_ok++; + } + count++; + } + } + + /* Bước 7 di chuyển đến điểm trung gian trên điểm lưu trữ */ + else if(!flag_home && center_count == 2 &&count == 5) + { + if(nova5_ptr_->nova_movePoint(Point_temp3,100,1) == 1 && count == 5) + { + count++ ; + } + } + + /* Bước 8 di chuyển đến điểm lưu trữ */ + else if(!flag_home && center_count == 2 &&count == 6) + { + if(nova5_ptr_->nova_movePoint(STORE_COOR,speed,0) == 1 && count == 6) + { + nova5_ptr_->nova_offTool(0.5); + count++ ; + } + } + + /* Bước 9 di chuyển đến điểm trung gian trên điểm lưu trữ */ + else if(!flag_home && center_count == 2 &&count == 7) + { + if(nova5_ptr_->nova_movePoint(Point_temp3,speed,0)==1 && count == 7) + { + count++; + } + } + + /* Bước 10 reset các biến */ + else if(!flag_home && center_count == 2 && count >= 8) + { + // timeStamp_cam_1 = timeStamp_cam; + temp = 0; + count = 0; + center_count = 1; + // if(isCheck > 1) isCheck =0; + + if(count_ok > 4 && count_ng > 4) break; + // break; + } + //========================================// + //========================================// + + // Hiển thị bước robot đang thực hiện + if((count + center_count) != count_1) ROS_INFO("STEP: %d", count + center_count); + count_1 = count + center_count; + } + else + { + // alarm = nova5_ptr_->nova_statusAlarm(); + ROS_WARN("Robot is reconnecting with maximum time 15 minutes..."); + for(int i=0; i<180U; ++i) + { + + if(!ros::ok()) break; + if(nova5_ptr_->nova_connect() ) + { + nova5_ptr_->nova_stopProgram(); + // Sau khi enable robot thành công cần bắt đầu chương trình của ccbox + nova5_ptr_->nova_startProgram(); + temp = 0; + count = 0; + center_count = 0; + get_point_current = 0; + flag_home = false; + + break; + } + ros::Duration(5).sleep(); + } + } + loop_rate.sleep(); + ros::spinOnce(); + } + + // nova5_ptr_->nova_stopProgram(); + // nova5_ptr_->nova_close(); +} + + diff --git a/Controllers/Packages/nova5_control/example/test_thread.cpp b/Controllers/Packages/nova5_control/example/test_thread.cpp new file mode 100644 index 0000000..d8cc740 --- /dev/null +++ b/Controllers/Packages/nova5_control/example/test_thread.cpp @@ -0,0 +1,101 @@ +#include "nova5_control/imr_nova_control.h" +#include +#include +#include +#include + +std::shared_ptr controller = nullptr; + +unsigned int ok_count_max = 0; +unsigned int ng_count_max = 1; +bool enable = true; +bool continue_flag = false; +bool go_home_flag = false; +bool power_on_flag = false; + +void callBack(const std_msgs::UInt16::ConstPtr& msg) +{ + if(controller != nullptr) + { + switch (msg->data) + { + case 1: + controller->startHomeThread(); + // continue_flag = true; + // go_home_flag = false; + // power_on_flag = false; + break; + case 2: + // continue_flag = false; + // go_home_flag = true; + // power_on_flag = false; + break; + case 3: + // continue_flag = false; + // go_home_flag = false; + // power_on_flag = true; + break; + default: + // continue_flag = false; + // go_home_flag = false; + // power_on_flag = false; + break; + } + } +} + +int main(int argc, char** argv) { + // Initialize the ROS node + ros::init(argc, argv, "test_imr_nova_control"); + ros::NodeHandle nh = ros::NodeHandle("~"); + ros::Subscriber sub = nh.subscribe("mode", 1, callBack); + ros::Publisher pub = nh.advertise("status", 1); + + try { + // Initialize ROS time + // ros::Time::init(); + + // Create an instance of imr_nova_control + controller = std::make_shared(); + + + + controller->ok_count_max_ = &ok_count_max; + controller->ng_count_max_ = &ng_count_max; + controller->enable_ = &enable; + + controller->continue_flag_ = &continue_flag; + controller->go_home_flag_ = &go_home_flag; + controller->power_on_flag_ = &power_on_flag; + + + #if 0 + controller->startHomeThread(); + #endif + + #if 0 + controller->startModeThread(); + #endif + + std::cout << "imr_nova_control instance created successfully." << std::endl; + + // Run ROS spin to process callbacks (if applicable) + // ros::AsyncSpinner spinner(2); // Adjust the number of threads as needed + // spinner.start(); + ros::Rate rate(5); + + while (ros::ok()) + { + std_msgs::UInt16 msg; + msg.data = controller->statusCode_; + pub.publish(msg); + ros::spinOnce(); + rate.sleep(); + } + + } catch (const std::exception& e) { + std::cerr << "An error occurred: " << e.what() << std::endl; + } + ros::spin(); + return 0; +} diff --git a/Controllers/Packages/nova5_control/include/nova5_control/imr_nova_control.h b/Controllers/Packages/nova5_control/include/nova5_control/imr_nova_control.h new file mode 100644 index 0000000..600f2f9 --- /dev/null +++ b/Controllers/Packages/nova5_control/include/nova5_control/imr_nova_control.h @@ -0,0 +1,95 @@ +#ifndef __NOVA5_CONTROL_IMR_NOVA_CONTROL_H__ +#define __NOVA5_CONTROL_IMR_NOVA_CONTROL_H__ + +// #include +#include +#include +#include +#include"nova5_control/nova5_modbus.h" + +class imr_nova_control +{ +public: + imr_nova_control(); + virtual ~imr_nova_control(); + + void startModeThread(); + void stopModeThread(); + + void startHomeThread(); + void stopHomeThread(); + + unsigned int *ok_count_max_; + unsigned int *ng_count_max_; + + bool *enable_; + double mode_; + + bool *continue_flag_; + bool *go_home_flag_; + bool *power_on_flag_; + + enum statusCode + { + ROBOT_WAITING = 98, + ROBOT_RECONNECTING = 99, + ROBOT_RUNNING = 100, + ROBOT_DISCONNECTED = 101, + ROBOT_READ_DATA_ZERO = 102, + ROBOT_DATA_NOT_UPDATED = 103, + ROBOT_TARGET_COORDINATES_OUT_OF_RANGE = 104, + ROBOT_COMING_HOME = 105, + ROBOT_CHECK_EMERGENCY_STOP_BUTTON = 106, + ROBOT_IN_ALARM_STATE = 107, + ROBOT_POWER_ON_WAITTING = 108, + ROBOT_POWER_ON = 109, + ROBOT_DRAG = 110, + ROBOT_WAITING_SELECT_CONTINUE_OR_GO_HOME = 111, + ROBOT_OK_FULL = 112, + ROBOT_NG_FULL = 113, + ROBOT_ERROR = 999 + }; + + /** + * @var statusCode_ + * 98: Robot is waiting + * 99: Robot is reconnecting + * 100: Robot is running + * 101: Robot is disconnected + * 102: Read data is 0 (Camera can not detect object or lost connection) + * 103: Data is not updated (camera maybe disconnected) + * 104: Target coordinates are not within the working range + * 105: Robot is comming home + * 106: Check the robot's emergency stop button + * 107: Robot is in alarm state + * 108: Robot is power on + * 109: Robot is in drag mode + * 110: OK object is over the limit + * 111: NG object is over the limit + * 999: Robot is error + */ + statusCode statusCode_; + +private: + char getKey(); + void imr_dobot_control_thread(); + void imr_dobot_go_home_thread(); + + // Coordinates + std::vector HOME = {-141.91,301.01,524.27,-179.4,-0.04,0.0}; + std::vector CENTER_P = {309.11,63.29,524.27,-179.4,-0.04,0.00}; + std::vector ORIGIN_COOR = {150.71,-435.9,524.27,-179.4,-0.04,-90.00}; + std::vector OK_COOR = {-141.91,301.01,150,-179.4,-0.04,0.0}; + std::vector NG_COOR = {-141.91,657.01,150,-179.4,-0.04,0.0}; + + std::string IP = "192.168.2.6"; + int PORT = 502; + + bool modeThreadRunning_; // Variable to check if the thread is running + bool homeThreadRunning_; // Variable to check if the thread is running + std::shared_ptr thr_main_; + std::shared_ptr thr_go_home_; + +}; + +#endif // __NOVA5_CONTROL_IMR_NOVA_CONTROL_H__ \ No newline at end of file diff --git a/Controllers/Packages/nova5_control/include/nova5_control/nova5_modbus.h b/Controllers/Packages/nova5_control/include/nova5_control/nova5_modbus.h new file mode 100644 index 0000000..53a14d8 --- /dev/null +++ b/Controllers/Packages/nova5_control/include/nova5_control/nova5_modbus.h @@ -0,0 +1,300 @@ +// nova5_modbus.h +#ifndef NOVA5_CONTROL_H +#define NOVA5_CONTROL_H +// #pragma once + +#include +#include +#include +#include +#include +#include +#include + +#include"modbus_tcp/modbus.h" + + +#define TIME_ERR 1 +#define ERR -1 +#define TIME_OUT 1.5 + +// Coil Registers +#define COIL_START 0 +#define COIL_PAUSE 1 +#define COIL_CONTINUE 2 +#define COIL_STOP 3 +#define COIL_EMERGENCY_STOP 4 +#define COIL_CLEAR_ALARM 5 +#define COIL_POWER_ON 6 +#define COIL_ENABLE 100 +#define COIL_DISABLE 101 +// #define USER_DEFINED_COIL_START 3095 +#define COILD_MOVE 3095 +#define COILD_MOVING_DONE 3096 +#define COILD_TOOL 3097 +#define COILD_MOVE_MODE 3098 // 0 moveJ - 1 moveL +// #define USER_DEFINED_COIL_END 9999 + +// Discrete Input Registers +#define DISCRETE_STOP_STATE 1 +#define DISCRETE_PAUSE_STATE 2 +#define DISCRETE_RUNNING_STATE 3 +#define DISCRETE_ALARM_STATE 4 +#define DISCRETE_RESERVED_1 5 +#define DISCRETE_REMOTE_CONTROL 7 + + +// Input Registers +#define INPUT_ROBOT_MODE_START 1012 +#define INPUT_ROBOT_MODE_END 1015 +#define INPUT_TIMESTAMP_START 1016 +#define INPUT_TIMESTAMP_END 1019 +#define INPUT_TARGET_JOINT_POSITION_START 1096 +#define INPUT_TARGET_JOINT_POSITION_END 1119 +#define INPUT_TARGET_JOINT_CURRENT_START 1168 +#define INPUT_TARGET_JOINT_CURRENT_END 1191 +#define INPUT_ACTUAL_JOINT_POSITION_START 1216 +#define INPUT_ACTUAL_JOINT_POSITION_END 1239 +#define INPUT_ACTUAL_JOINT_CURRENT_START 1264 +#define INPUT_ACTUAL_JOINT_CURRENT_END 1287 +#define INPUT_ROBOT_BRAKE_STATUS_H 1512 //HIGH BYTE +#define INPUT_ROBOT_ENABLING_STATUS_L 1513 //LOW BYTE +#define INPUT_ROBOT_DRAG_STATUS_H 1513 //HIGH BYTE +#define INPUT_ROBOT_DRAG_STATUS_L 1514 //HIGH BYTE +#define INPUT_ROBOT_ALARM_STATUS_H 1514 //HIGH BYTE +#define INPUT_LOAD_WEIGHT_START 1584 +#define INPUT_LOAD_WEIGHT_END 1587 +#define INPUT_ECCENTRIC_DISTANCE_X_START 1588 +#define INPUT_ECCENTRIC_DISTANCE_X_END 1591 +#define INPUT_ECCENTRIC_DISTANCE_Y_START 1592 +#define INPUT_ECCENTRIC_DISTANCE_Y_END 1595 +#define INPUT_ECCENTRIC_DISTANCE_Z_START 1596 +#define INPUT_ECCENTRIC_DISTANCE_Z_END 1599 +#define INPUT_USER_COORDINATES_START 1600 +#define INPUT_USER_COORDINATES_END 1623 +#define INPUT_TOOL_COORDINATES_START 1624 +#define INPUT_TOOL_COORDINATES_END 1647 + +// Holding Registers +#define HOLDING_GLOBAL_SPEED 100 + +// #define HOLDING_USER_DEFINED_START 3095 +#define HOLDING_TARGET_POINT_START 3095 +#define HOLDING_TARGET_POINT_END 3118 +#define HOLDING_CAMERA_POINT_START 3119 +#define HOLDING_CAMERA_POINT_END 3134 +#define HOLDING_CAMERA_OK 3135 +#define HOLDING_CAMERA_TIMESTAMP_START 3136 +#define HOLDING_CAMERA_TIMESTAMP_END 3139 +#define HOLDING_POINT_CURRENT_START 3140 +#define HOLDING_POINT_CURRENT_END 3163 +#define HOLDING_Z_MIN_USER_SET_START 3164 +#define HOLDING_Z_MIN_USER_SET_END 3167 +// #define HOLDING_USER_DEFINED_END 8999 + +// #define HOLDING_CURRENT_SCREW_INDEX 9000 +#define HOLDING_ALARM_CODE 9054 + + + + +class cnt_nova5 +{ +public: + + cnt_nova5(std::string ip_address, int port); + virtual ~cnt_nova5(); + /** + * @brief Connect to nova + */ + int nova_connect(void); + /** + * @brief Check connection + */ + bool nova_checkConnected(void); + + void nova_disable(void); + /** + * @brief Close to nova + */ + void nova_close(void); + /** + * @brief Alarm notification + */ + bool nova_statusAlarm(void); + /** + * @brief Clear alarm + */ + void nova_clearAlarm(void); + /** + * @brief Power on robot + */ + int nova_powerOn(void); + /** + * @brief enable robot + */ + bool nova_enable(void); + /** + * @brief start program + */ + bool nova_startProgram(void); + /** + * @brief stop program + */ + void nova_stopProgram(void); + /** + * @brief Read robot mode and return robot mode + 3: Emergency stop mode, not powered on + 4: Powered on mode, not enabled + 5: Enabled mode, program not started + 6: Drag mode + 7: Program started mode + 9: Kinematic error of robot nova + */ + double nova_robotMode(void); + /** + * @brief Select moveJ or moveL + */ + void nova_robotMove(void); + /** + * @brief Set speed global for nova + * @param speed Speed setting for nova + */ + void nova_speedGlobal(uint16_t speed); + /** + * @brief Check nova status (nova has 7 status registers form 1-7) + * @param add Address of status register + * @param value Register status + */ + int nova_status(int add, bool &value); + /** + * @brief Check nova multistate (nova has 7 status registers form 1-7) + * @param add First status register address + * @param length Number of rgisters to read + * @param buff Register states + */ + int nova_multistate(int add, int length, bool *buff); + /** + * @brief Get 6 values ​​x, y, z, Rx, Ry, Rz + * @param Point Condinates after conversion + * @param Point_initial Scan coordinates + * @param isCheck True or false check variable + * @param Timestamp Time stamp + */ + + int nova_readCoordinates(std::vector& Point,std::vector& Point_initial, uint16_t& isCheck, int& Timestamp); + /** + * @brief Get coordinates from camera + * @param Point Variable store coordinates current + */ + int nova_getCoorcurrent(std::vector& Point); + /** + * @brief Read time stamp nova + * @param time Time stamp of nova + */ + int nova_timeStamp(uint64_t& time); + /** + * @brief + */ + int nova_createArea(void); + /** + * @brief Turn on tool + */ + void nova_onTool(void); + /** + * @brief Turn off tool + */ + void nova_offTool(const double delay); + /** + * @brief Move robot nova + * @param Point Target coordinates + * @param speed cai toc do cho tay may + * @param mode 0 - moveL, const int mode = 1 + * 1 - moveJ + */ + int nova_movePoint(std::vector& Point,const int speed, const int mode = 1); + /** + * @brief Check z + * + * @param x + * @param y + * @param z + * @param choose 0 - z min theo mac dinh 200 mm + * 1 - z min theo tham so duoc set tu CCBOX + */ + double nova_check_z(double& x, double& y, double& z, int choose = 0); + + int movePointRunning_; + bool *run_ptr_; + + +private: + /** + * @brief Robot move done notification + */ + bool nova_movingDone(void); + /** + * @brief Allow the robot to start moving + */ + void nova_allowMove(void); + /** + * @brief Dismiss the robot moving + */ + void nova_dismissMove(void); + /** + * @brief Send 6 values ​​x, y, z, Rx, Ry, Rz + * @param mode 0 - moveJ, const int mode = 0 + * 1 - moveL + * @param Point Coordinates to sent + */ + int nova_sendCoordinates(std::vector& Point); + /** + * @brief Check point coordinates + * return 1: Point within range + * return 0: Point is not range + * return -1: Error + * @param x + * @param y + * @param z + */ + int workArealimit(double x, double y, double z); + /** + * @brief Convert object coordinates to robot tool condinates + * @param Point Condinates after conversion + * @param Point_initial Scan coordinates + * @param Coor Object coordinates + */ + int convert(std::vector& Point,std::vector& Point_initial, std::vector& Coor); + /** + * @brief Get z min and return z min + */ + double getZmin(void); + + /* data */ + enum WorkAreaStatus { + WITHIN_LIMIT = 1, + OUT_OF_LIMIT = 0, + ERROR = -1 + }; + + boost::shared_ptr nova_; + bool inter_var; + + bool send; + double a1,b1,c1; + double a2,b2; + double r_max; + double r_min; + double z_min_static; + + // double safety_factor; + + double x_deviation; + double y_deviation; + double z_deviation; + + bool flag_move; + uint64_t mode_store; +}; + +#endif // NOVA5_CONTROL_H \ No newline at end of file diff --git a/Controllers/Packages/nova5_control/package.xml b/Controllers/Packages/nova5_control/package.xml new file mode 100644 index 0000000..7cf1182 --- /dev/null +++ b/Controllers/Packages/nova5_control/package.xml @@ -0,0 +1,68 @@ + + + nova5_control + 0.0.0 + The nova5_control package + + + + + robotics + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + roscpp + std_msgs + roscpp + std_msgs + roscpp + std_msgs + + modbus_tcp + modbus_tcp + modbus_tcp + + + + + + + diff --git a/Controllers/Packages/nova5_control/src/imr_nova_control.cpp b/Controllers/Packages/nova5_control/src/imr_nova_control.cpp new file mode 100644 index 0000000..c350f9b --- /dev/null +++ b/Controllers/Packages/nova5_control/src/imr_nova_control.cpp @@ -0,0 +1,1331 @@ +#include "nova5_control/imr_nova_control.h" + +imr_nova_control::imr_nova_control() + : enable_(nullptr), + ok_count_max_(nullptr), + ng_count_max_(nullptr), + continue_flag_(nullptr), + go_home_flag_(nullptr), + power_on_flag_(nullptr) +{ + modeThreadRunning_ = false; // Variable to check if the thread is running + homeThreadRunning_ = false; // Variable to check if the thread is running + mode_ = 0; +} + + +imr_nova_control::~imr_nova_control() +{ + // ROS_WARN("Destructor"); + if (thr_main_ && thr_main_->joinable()) + { + thr_main_->join(); + } + if (thr_go_home_ && thr_go_home_->joinable()) + { + thr_go_home_->join(); + } + + // delete ok_count_max_; // Giải phóng bộ nhớ + // delete ng_count_max_; +} + + +char imr_nova_control::getKey() +{ + struct termios oldt, newt; + char ch; + tcgetattr(STDIN_FILENO, &oldt); + newt = oldt; + newt.c_lflag &= ~(ICANON | ECHO); + tcsetattr(STDIN_FILENO, TCSANOW, &newt); + ch = getchar(); + tcsetattr(STDIN_FILENO, TCSANOW, &oldt); + return ch; +} + + +void imr_nova_control::startModeThread() +{ + if(enable_ != NULL && ok_count_max_ != NULL && ng_count_max_ != NULL && continue_flag_ != NULL && go_home_flag_ != NULL && power_on_flag_ != NULL) + { + if (!modeThreadRunning_ && !homeThreadRunning_) + { + modeThreadRunning_ = true; // Set the flag to start the thread + thr_main_ = std::make_shared(&imr_nova_control::imr_dobot_control_thread, this); + ROS_WARN("modeThreadRunning_: %x", modeThreadRunning_); + } + else + { + ROS_WARN("Please stop go home before starting robot"); + } + } + else + { + statusCode_ = imr_nova_control::ROBOT_ERROR; + } +} + + +void imr_nova_control::startHomeThread() +{ + if(enable_ != NULL) + { + if (!homeThreadRunning_ && !modeThreadRunning_) + { + homeThreadRunning_ = true; // Set the flag to start the thread + thr_go_home_ = std::make_shared(&imr_nova_control::imr_dobot_go_home_thread, this); + ROS_WARN("homeThreadRunning_: %x", homeThreadRunning_); + } + else + { + ROS_WARN("Please stop robot before going home"); + } + } + else + { + statusCode_ = imr_nova_control::ROBOT_ERROR; + } +} + + +void imr_nova_control::stopModeThread() +{ + if (modeThreadRunning_ && !homeThreadRunning_) + { + ROS_WARN("cancel thread"); + modeThreadRunning_ = false; // Set the flag to stop the thread + // Join threads to ensure proper cleanup + if (thr_main_ && thr_main_->joinable()) + { + thr_main_->join(); + } + } + // delete ok_count_max_; // Giải phóng bộ nhớ + // delete ng_count_max_; +} + + +void imr_nova_control::stopHomeThread() +{ + if (homeThreadRunning_ && !modeThreadRunning_) + { + ROS_WARN("cancel thread"); + homeThreadRunning_ = false; // Set the flag to stop the thread + // Join threads to ensure proper cleanup + if (thr_go_home_ && thr_go_home_->joinable()) + { + thr_go_home_->join(); + } + } +} + + +void imr_nova_control::imr_dobot_control_thread() +{ + statusCode_ = imr_nova_control::ROBOT_WAITING; + + std::vector STORE_COOR(6); + std::vector Point(6); + std::vector Point_current(6); + std::vector Center_temp(6); + std::vector Point_temp1(6); + std::vector Point_temp2(6); + std::vector Point_temp3(6); + + + int timeStamp_cam = 0; + int count = 0, count_1 = 0xffff; + int center_count = 0; + int get_point_current =0; + + uint16_t isCheck = 0; + uint16_t speed = 0; + + unsigned int count_ok = 0; + unsigned int count_ng = 0; + + bool connect = false; + bool drag = false; + bool flag_home = false; + bool temp = false; + + // Khởi tạo + std::shared_ptr nova5_ptr_ = nullptr; + nova5_ptr_ = std::make_shared(IP,PORT); + + nova5_ptr_->run_ptr_ = enable_; + + + // Kết nối đến robot nova + connect = nova5_ptr_->nova_connect(); + + if(connect) + { + double mode = nova5_ptr_->nova_robotMode(); + mode_ = mode; + if(mode == 3) + { + statusCode_ = imr_nova_control::ROBOT_POWER_ON; + + if(nova5_ptr_->nova_powerOn() == -1) + { + statusCode_ = imr_nova_control::ROBOT_ERROR; + + ROS_ERROR("Please check robot arm"); + modeThreadRunning_ = false;// biến tạm để cho thấy robot bị lỗi, thay thành các biến phản hồi lỗi + } + } + if(mode == 9) + { + nova5_ptr_->nova_clearAlarm(); + + if(nova5_ptr_->nova_statusAlarm()) + { + statusCode_ = imr_nova_control::ROBOT_ERROR; + + ROS_ERROR("Please check robot arm or try again!"); + modeThreadRunning_ = false;// biến tạm để cho thấy robot bị lỗi, thay thành các biến phản hồi lỗi + } + } + if(modeThreadRunning_) + { + // Sau khi kết nối thành công cần enable robot + if(nova5_ptr_->nova_enable()) + { + + // Sau khi enable robot thành công cần bắt đầu chương trình của ccbox + bool status_start_program = nova5_ptr_->nova_startProgram(); + if(status_start_program) statusCode_ = imr_nova_control::ROBOT_RUNNING; + else + { + statusCode_ = imr_nova_control::ROBOT_ERROR; + + ROS_ERROR("Please check robot arm"); + modeThreadRunning_ = false; + } + } + else + { + statusCode_ = imr_nova_control::ROBOT_ERROR; + + ROS_ERROR("Please check robot arm"); + modeThreadRunning_ = false; + } + } + } + + else + { + // alarm = nova5_ptr_->nova_statusAlarm(); + ROS_WARN("Robot is reconnecting with maximum time 15 minutes..."); + for(int i=0; i<180U; ++i) + { + statusCode_ = imr_nova_control::ROBOT_RECONNECTING; + + if(enable_ == NULL || !(*enable_)) + { + statusCode_ = imr_nova_control::ROBOT_ERROR; + + modeThreadRunning_ = false; + break; + } + connect = nova5_ptr_->nova_connect(); + if(connect || !ros::ok()) + { + while(ros::ok() + && (enable_ != NULL && (*enable_)) + ) + { + double mode = nova5_ptr_->nova_robotMode(); + double alarm = nova5_ptr_->nova_statusAlarm(); + mode_ = mode; + if(alarm && mode == 9) + { + statusCode_ = imr_nova_control::ROBOT_ERROR; + + ROS_ERROR("Kinematic ERROR! Please check robot!"); + modeThreadRunning_ = false;// biến tạm để cho thấy robot bị lỗi, thay thành các biến phản hồi lỗi + break; + } + if(mode == 3) + { + statusCode_ = imr_nova_control::ROBOT_POWER_ON; + + if(nova5_ptr_->nova_powerOn() == -1) + { + statusCode_ = imr_nova_control::ROBOT_ERROR; + + ROS_ERROR("Please check robot arm"); + modeThreadRunning_ = false;// biến tạm để cho thấy robot bị lỗi, thay thành các biến phản hồi lỗi + break; + } + } + if(mode == 4) break; + ros::Duration(5).sleep(); + } + + if(modeThreadRunning_) + { + // Sau khi kết nối thành công cần enable robot + if(nova5_ptr_->nova_enable()) + { + // Sau khi enable robot thành công cần bắt đầu chương trình của ccbox + bool status_start_program = nova5_ptr_->nova_startProgram(); + if(status_start_program) statusCode_ = imr_nova_control::ROBOT_RUNNING; + else + { + statusCode_ = imr_nova_control::ROBOT_ERROR; + + ROS_ERROR("Please check robot arm"); + modeThreadRunning_ = false; + } + } + else + { + statusCode_ = imr_nova_control::ROBOT_ERROR; + + ROS_ERROR("Please check robot arm"); + modeThreadRunning_ = false; + } + } + + break; + } + ros::Duration(5).sleep(); + } + + } + + ros::Rate loop_rate(100); + + + //===========================Vòng lặp chính========================// + //=================================================================// + //=================================================================// + gotoTop: + + while(ros::ok() && connect && modeThreadRunning_ + && (enable_ != NULL && (*enable_) && (*ok_count_max_ > 0 || *ng_count_max_ > 0)) + ) + { + // ROS_WARN("statusCode_: %f",statusCode_); + static bool checkconnect = false; + static bool alarm = false; + + //====Kiểm tra kết nối và trạng thái alarm====// + checkconnect = nova5_ptr_->nova_checkConnected(); + alarm = nova5_ptr_->nova_statusAlarm(); + //============================================// + + if(checkconnect && !alarm) + { + + // Lấy trạng thái robot + double robotmode = nova5_ptr_->nova_robotMode(); + mode_ = robotmode; + //===Kiểm tra trạng thái robot===// + //===============================// + if( robotmode == 3) + { + ros::Duration(0.5).sleep(); + + bool alarm = nova5_ptr_->nova_statusAlarm(); + if(alarm) + { + statusCode_ = imr_nova_control::ROBOT_IN_ALARM_STATE; + + for(int i=0; i<3U; ++i) + { + nova5_ptr_->nova_clearAlarm(); + if(nova5_ptr_->nova_statusAlarm() == 0 || !(*enable_) || !ros::ok()) + { + statusCode_ = imr_nova_control::ROBOT_RUNNING; + break; + } + if(i == 2) + { + double mode = nova5_ptr_->nova_robotMode(); + mode_ = mode; + if(mode == 3) + { + statusCode_ = imr_nova_control::ROBOT_CHECK_EMERGENCY_STOP_BUTTON; + + ROS_ERROR("Please check button E-Stop on robot IMR!"); + goto gotoTop; + } + else + { + ROS_ERROR("Robot is in alarm state!"); + } + } + ros::Duration(0.2).sleep(); + } + } + + statusCode_ = imr_nova_control::ROBOT_POWER_ON_WAITTING; + + ROS_WARN("OPTION: power on"); + while(ros::ok() && modeThreadRunning_ + && (enable_ != NULL && (*enable_)) + ) + { + double mode = nova5_ptr_->nova_robotMode(); + mode_ = mode; + if(mode != 3) goto gotoTop; + + if(*power_on_flag_ == true) + { + break; + } + } + + // Power on robot + if(*power_on_flag_ == true) + { + statusCode_ = imr_nova_control::ROBOT_POWER_ON; + + if(nova5_ptr_->nova_powerOn() == 1) + { + // Sau khi kết nối thành công cần enable robot + if(nova5_ptr_->nova_enable()) + { + // Sau khi enable robot thành công cần bắt đầu chương trình của ccbox + bool status_start_program = nova5_ptr_->nova_startProgram(); + if(status_start_program) + { + if(count_ok >= *ok_count_max_ && count_ng >= *ng_count_max_) + { + statusCode_ = imr_nova_control::ROBOT_ERROR; + modeThreadRunning_ = false; + break; + } + statusCode_ = imr_nova_control::ROBOT_RUNNING; + } + else + { + statusCode_ = imr_nova_control::ROBOT_ERROR; + + ROS_ERROR("Please check robot arm"); + modeThreadRunning_ = false; + } + + temp = false; + get_point_current = 0; + count = 0; + center_count = 0; + flag_home = false; + } + else + { + statusCode_ = imr_nova_control::ROBOT_ERROR; + + ROS_ERROR("Please check robot arm"); + modeThreadRunning_ = false; + continue; + } + } + else + { + statusCode_ = imr_nova_control::ROBOT_ERROR; + + ROS_ERROR("Please check robot arm"); + modeThreadRunning_ = false; + continue; + } + } + } + else if(robotmode == 4) + { + ROS_WARN("Robot is in powered on mode, not enabled"); + modeThreadRunning_ = false; + continue; + } + else if(robotmode == 6) + { + statusCode_ = imr_nova_control::ROBOT_DRAG; + + ROS_WARN("Robot is in drag mode"); + drag = true; + } + else if(robotmode == 9) + { + ros::Duration(2).sleep(); + double mode = nova5_ptr_->nova_robotMode(); + mode_ = mode; + if(mode == 9) + { + statusCode_ = imr_nova_control::ROBOT_ERROR; + + ROS_ERROR("Kinematic error of robot nova"); + // modeThreadRunning_ = false; + continue; + } + else drag = true; + } + + if(drag == true && robotmode == 5) + { + statusCode_ = imr_nova_control::ROBOT_WAITING_SELECT_CONTINUE_OR_GO_HOME; + + ROS_WARN("OPTION: 1 to continute"); + ROS_WARN("OPTION: 2 to go home"); + while(ros::ok() && modeThreadRunning_ + && (enable_ != NULL && (*enable_)) + ) + { + double mode = nova5_ptr_->nova_robotMode(); + mode_ = mode; + if(mode != 5) goto gotoTop; + if(*continue_flag_ == true || *go_home_flag_ == true) + { + break; + } + } + /*Kiểm tra nếu key = n hoặc N thì thực hiện lệnh tiếp tục*/ + if(*continue_flag_ == true) + { + // Sau khi enable robot thành công cần bắt đầu chương trình của ccbox + bool status_start_program = nova5_ptr_->nova_startProgram(); + if(status_start_program) + { + if(count_ok >= *ok_count_max_ && count_ng >= *ng_count_max_) + { + statusCode_ = imr_nova_control::ROBOT_COMING_HOME; + + drag = false; + get_point_current = 0; + center_count = 0; + ROS_INFO("Robot is comming home..."); + flag_home = true; + } + else + { + statusCode_ = imr_nova_control::ROBOT_RUNNING; + + temp = false; + drag = false; + get_point_current = 0; + count = 0; + center_count = 0; + flag_home = false; + } + } + else + { + statusCode_ = imr_nova_control::ROBOT_ERROR; + + temp = false; + drag = false; + get_point_current = 0; + count = 0; + center_count = 0; + flag_home = false; + + ROS_ERROR("Please check robot arm"); + modeThreadRunning_ = false; + } + } + /*Kiểm tra nếu key = h hoặc H thì thực hiện lệnh về home */ + else if(*go_home_flag_ == true) + { + // Sau khi enable robot thành công cần bắt đầu chương trình của ccbox + bool status_start_program = nova5_ptr_->nova_startProgram(); + if(status_start_program) statusCode_ = imr_nova_control::ROBOT_COMING_HOME; + else + { + statusCode_ = imr_nova_control::ROBOT_ERROR; + + ROS_ERROR("Please check robot arm"); + modeThreadRunning_ = false; + } + drag = false; + get_point_current = 0; + center_count = 0; + ROS_INFO("Robot is comming home..."); + flag_home = true; + } + else + { + ROS_WARN("Please press n or h!"); + } + } + //===============================// + //===============================// + + if(robotmode == 7 && !flag_home) + { + statusCode_ = imr_nova_control::ROBOT_RUNNING; + } + else if(robotmode == 7 && flag_home) + { + statusCode_ = imr_nova_control::ROBOT_COMING_HOME; + } + + //===Chu trình gắp phôi và lưu trữ phôi===// + //========================================// + if(robotmode == 7 && nova5_ptr_->movePointRunning_ != -1) + { + + /* Ve home */ + if(flag_home && center_count == 2) + { + if(nova5_ptr_->nova_movePoint(HOME,100,1)==1 && center_count == 2) + { + flag_home = false; + ROS_INFO("Complete"); + modeThreadRunning_ = false; + continue; + } + } + + /* Bước 0 */ + if(center_count == 0) + { + if(get_point_current == 0) + { + //Đọc tọa độ robot hiện tại trong vòng 6s + static uint16_t y_value = 0; + + nova5_ptr_->nova_getCoorcurrent(Point_current); + + y_value = abs(Point_current[1]) < 300 ? 300 : static_cast(abs(Point_current[1])); + + speed = (y_value / 300.0) * 20; + ROS_WARN("Speed: %d", speed); + + + + if(CENTER_P[2] > (Point_current[2] + 10)) + { + Center_temp[0] = Point_current[0]; + Center_temp[1] = Point_current[1]; + Center_temp[2] = nova5_ptr_->nova_check_z(Point_current[0],Point_current[1],CENTER_P[2],0); + Center_temp[3] = Point_current[3]; + Center_temp[4] = Point_current[4]; + Center_temp[5] = Point_current[5]; + } + else if(CENTER_P[2] < (Point_current[2] - 10)) + { + Center_temp[0] = CENTER_P[0]; + Center_temp[1] = CENTER_P[1]; + Center_temp[2] = nova5_ptr_->nova_check_z(CENTER_P[0],CENTER_P[1],Point_current[2], 0); + Center_temp[3] = CENTER_P[3]; + Center_temp[4] = CENTER_P[4]; + Center_temp[5] = CENTER_P[5]; + } + else if(CENTER_P[2] >= (Point_current[2] - 10) && CENTER_P[2] < (Point_current[2] + 10)) center_count = 1; + + // ROS_WARN("x : %f", Center_temp[0]); + // ROS_WARN("y : %f", Center_temp[1]); + // ROS_WARN("z : %f", Center_temp[2]); + get_point_current = 1; + } + + if(center_count == 0) + { + if(nova5_ptr_->nova_movePoint(Center_temp,speed,0)==1 && center_count == 0) center_count++; + } + } + + /* Bước 1 */ + else if(center_count == 1) + { + if(nova5_ptr_->nova_movePoint(CENTER_P,100,1)==1 && center_count == 1) + { + center_count = 2; + } + } + + /* Bước 2 di chuyển cánh tay đến vị trí chụp ảnh */ + if(!flag_home && center_count == 2 && count == 0) + { + if(nova5_ptr_->nova_movePoint(ORIGIN_COOR,100,1)==1 && count == 0) + { + count++ ; + } + } + + /* Bước 3 xác định vị trí vật thể và di chuyển tới vị trí trên vật thể */ + else if(!flag_home && center_count == 2 && count == 1) + { + static bool continue_flag = false; + if(!temp) + { + Retry: + // Đọc tọa độ vật + while(ros::ok() + && (enable_ != NULL && (*enable_)) + ) + { + // Kiểm tra kết nối và trạng thái robot + bool connect = nova5_ptr_->nova_checkConnected(); + bool alarm = nova5_ptr_->nova_statusAlarm(); + double mode = nova5_ptr_->nova_robotMode(); + mode_ = mode; + + // Kiểm tra nếu robot mất kết nối, báo lỗi hoặc robot đang ở chế độ drag thì thoát khỏi vòng lặp + if(!connect || alarm || mode != 7 || !modeThreadRunning_) + { + // Set continue_flag = true và thoát khỏi vòng lặp + continue_flag = true; + break; + } + ros::Duration(1).sleep(); + ROS_INFO("Read camera..."); + + int status_readCoor = nova5_ptr_->nova_readCoordinates(Point,ORIGIN_COOR,isCheck,timeStamp_cam); + if(status_readCoor == 1) + { + // // Lấy thời gian của máy tính hiện tại + // ros::Time current_time = ros::Time::now(); + + // // Chỉ lấy phần giây và ép kiểu sang int + // int seconds = static_cast(current_time.sec); + double seconds = ros::Time::now().toSec(); + + ROS_ERROR("x : %d", timeStamp_cam); + ROS_ERROR("y : %f", seconds); + + // Nếu khoảng thời giữa 2 lần gửi dữ liệu < 1s thì được coi là hợp lệ và thoát khỏi vòng lặp + if((seconds - static_cast(timeStamp_cam)) < TIME_OUT) + { + if(isCheck == 1) + { + if(count_ok >= *ok_count_max_) + { + statusCode_ = imr_nova_control::ROBOT_OK_FULL; + + ROS_WARN("OK object is over the limit!"); + goto Retry; + } + } + if(isCheck == 0) + { + if(count_ng >= *ng_count_max_) + { + statusCode_ = imr_nova_control::ROBOT_NG_FULL; + + ROS_WARN("NG object is over the limit!"); + goto Retry; + } + } + } + + + if ((seconds - static_cast(timeStamp_cam)) >= TIME_OUT) + { + // Dữ liệu cũ không được cập nhật, chờ cho đến khi nhận được dữ liệu mới nhất + while ( ros::ok() + && (enable_ != NULL && (*enable_)) + ) + { + statusCode_ = imr_nova_control::ROBOT_DATA_NOT_UPDATED; + + ROS_WARN("Data is out of date!"); + ROS_WARN("Wait for data update or press ctrl + c to cancel!"); + + // // Lấy thời gian của máy tính hiện tại + // ros::Time current_time = ros::Time::now(); + + // // Chỉ lấy phần giây và ép kiểu sang int + // int seconds = static_cast(current_time.sec); + + double seconds = ros::Time::now().toSec(); + + // Đọc lại tọa độ vật + int status_readCoor = nova5_ptr_->nova_readCoordinates(Point,ORIGIN_COOR,isCheck,timeStamp_cam); + ROS_ERROR("x : %d", timeStamp_cam); + ROS_ERROR("y : %f", seconds); + // Nếu khoảng thời giữa 2 lần gửi dữ liệu < 1s thì được coi là hợp lệ và thoát khỏi vòng lặp + if(((seconds - static_cast(timeStamp_cam)) < TIME_OUT) && status_readCoor) + { + if(isCheck == 1) + { + if(count_ok >= *ok_count_max_) + { + statusCode_ = imr_nova_control::ROBOT_OK_FULL; + + ROS_WARN("OK object is over the limit!"); + goto Retry; + } + else + { + break; + } + } + if(isCheck == 0) + { + if(count_ng >= *ng_count_max_) + { + statusCode_ = imr_nova_control::ROBOT_NG_FULL; + + ROS_WARN("NG object is over the limit!"); + goto Retry; + } + else + { + break; + } + } + } + + // Kiểm tra kết nối và trạng thái robot + bool connect = nova5_ptr_->nova_checkConnected(); + bool alarm = nova5_ptr_->nova_statusAlarm(); + double mode = nova5_ptr_->nova_robotMode(); + mode_ = mode; + + if(!connect || mode != 7 || alarm || !modeThreadRunning_) + { + // ROS_WARN("Robot is in drag mode"); + continue_flag = true; + break; + } + ros::Duration(1).sleep(); + } + } + + // Set tọa độ cho điểm trung gian 1 + Point_temp1[0] = Point[0]; + Point_temp1[1] = Point[1]; + Point_temp1[2] = nova5_ptr_->nova_check_z(Point[0],Point[1],ORIGIN_COOR[2], 0); + Point_temp1[3] = Point[3]; + Point_temp1[4] = Point[4]; + Point_temp1[5] = Point[5]; + temp = true; + break; + } + else + { + statusCode_ = imr_nova_control::ROBOT_READ_DATA_ZERO; + } + + } + + // Nếu continue_flag = true thì thực hiện vòng vặp tiếp theo + if(continue_flag) + { + continue_flag = false; + continue; + } + + temp = true; + } + + if(count == 1 && continue_flag == false) + { + if(nova5_ptr_->nova_movePoint(Point_temp1,80,1) == 1) count++; + } + } + + /* Bước 4 di chuyển tới vị trí vật thể */ + else if(!flag_home && center_count == 2 && count == 2) + { + if(nova5_ptr_->nova_movePoint(Point,20,0)== 1 && count == 2) + { + nova5_ptr_->nova_onTool(); + count++; + } + } + + /* Bước 5 di chuyển tới vị trí trên vật thể */ + else if(!flag_home && center_count == 2 && count == 3) + { + if(nova5_ptr_->nova_movePoint(Point_temp1,40,0)== 1 && count == 3) + { + count++; + } + } + + /* Bước 6 di chuyển tới vị trí điểm home */ + else if(!flag_home && center_count == 2 &&count == 4) + { + if(nova5_ptr_->nova_movePoint(CENTER_P,100,1)==1 && count == 4) + { + + // Nếu vật thể là NG thì set điểm lưu trữ NG_COOR + if(isCheck == 0) + { + NG_COOR[0]+=55; + + STORE_COOR[0] = NG_COOR[0]; + STORE_COOR[1] = NG_COOR[1]; + STORE_COOR[2] = NG_COOR[2]; + STORE_COOR[3] = NG_COOR[3]; + STORE_COOR[4] = NG_COOR[4]; + STORE_COOR[5] = NG_COOR[5]; + + Point_temp3[0] = NG_COOR[0]; + Point_temp3[1] = NG_COOR[1]; + Point_temp3[2] = nova5_ptr_->nova_check_z(NG_COOR[0],NG_COOR[1],CENTER_P[2], 0); + Point_temp3[3] = NG_COOR[3]; + Point_temp3[4] = NG_COOR[4]; + Point_temp3[5] = NG_COOR[5]; + + speed = 30; + count_ng++; + } + // Nếu vật thể là OK thì set điểm lưu trữ OK_COOR + else if(isCheck == 1) + { + OK_COOR[0]+=55; + + STORE_COOR[0] = OK_COOR[0]; + STORE_COOR[1] = OK_COOR[1]; + STORE_COOR[2] = OK_COOR[2]; + STORE_COOR[3] = OK_COOR[3]; + STORE_COOR[4] = OK_COOR[4]; + STORE_COOR[5] = OK_COOR[5]; + + Point_temp3[0] = OK_COOR[0]; + Point_temp3[1] = OK_COOR[1]; + Point_temp3[2] = nova5_ptr_->nova_check_z(OK_COOR[0],OK_COOR[1],CENTER_P[2], 0); + Point_temp3[3] = OK_COOR[3]; + Point_temp3[4] = OK_COOR[4]; + Point_temp3[5] = OK_COOR[5]; + + speed = 20; + count_ok++; + } + count++; + } + } + + /* Bước 7 di chuyển đến điểm trung gian trên điểm lưu trữ */ + else if(!flag_home && center_count == 2 &&count == 5) + { + if(nova5_ptr_->nova_movePoint(Point_temp3,100,1) == 1 && count == 5) + { + count++ ; + } + } + + /* Bước 8 di chuyển đến điểm lưu trữ */ + else if(!flag_home && center_count == 2 &&count == 6) + { + if(nova5_ptr_->nova_movePoint(STORE_COOR,speed,0) == 1 && count == 6) + { + nova5_ptr_->nova_offTool(0.5); + count++ ; + } + } + + /* Bước 9 di chuyển đến điểm trung gian trên điểm lưu trữ */ + else if(!flag_home && center_count == 2 &&count == 7) + { + if(nova5_ptr_->nova_movePoint(Point_temp3,speed,0)==1 && count == 7) + { + count++; + } + } + + /* Bước 10 reset các biến */ + else if(!flag_home && center_count == 2 && count >= 8) + { + temp = false; + count = 0; + center_count = 1; + if(ok_count_max_ != NULL && ng_count_max_ != NULL) + { + if(count_ok >= *ok_count_max_ && count_ng >= *ng_count_max_) + { + modeThreadRunning_ = false; + break; + } + } + } + //========================================// + //========================================// + + // Hiển thị bước robot đang thực hiện + if((count + center_count) != count_1) ROS_INFO("STEP: %d", count + center_count); + count_1 = count + center_count; + } + if(nova5_ptr_->movePointRunning_ == -1) + { + statusCode_ = imr_nova_control::ROBOT_TARGET_COORDINATES_OUT_OF_RANGE; + + ROS_ERROR("Please check target coordinates!"); + modeThreadRunning_ = false; + continue; + } + } + + //===Kết nối lại robot khi mất kết nối===// + //=======================================// + if(!checkconnect) + { + ROS_WARN("Robot is reconnecting with maximum time 15 minutes..."); + for(int i=0; i<180U; ++i) + { + statusCode_ = imr_nova_control::ROBOT_RECONNECTING; + + if(!ros::ok() || !modeThreadRunning_ || enable_ == NULL || !(*enable_)) break; + if(nova5_ptr_->nova_connect()) + { + nova5_ptr_->nova_stopProgram(); + // Sau khi enable robot thành công cần bắt đầu chương trình của ccbox + bool status_start_program = nova5_ptr_->nova_startProgram(); + if(status_start_program) statusCode_ = imr_nova_control::ROBOT_RUNNING; + else + { + statusCode_ = imr_nova_control::ROBOT_ERROR; + + ROS_ERROR("Please check robot arm"); + modeThreadRunning_ = false; + } + temp = false; + count = 0; + center_count = 0; + get_point_current = 0; + flag_home = false; + + break; + } + ros::Duration(5).sleep(); + + // Nếu kết nối quá 15 phút thì thoát khỏi vòng lặp + if(i == 179) + { + statusCode_ = imr_nova_control::ROBOT_DISCONNECTED; + + ROS_ERROR("Robot is not connected!"); + modeThreadRunning_ = false; + break; + } + } + } + //========================================// + //========================================// + + + //==============Xóa alarm================// + //=======================================// + if(alarm) + { + + for(int i=0; i<3U; ++i) + { + nova5_ptr_->nova_clearAlarm(); + ros::Duration(0.2).sleep(); + if(nova5_ptr_->nova_statusAlarm() == 0 || !(*enable_)) + { + ROS_WARN("Alarm is cleared!"); + break; + } + if(i == 2) + { + double mode = nova5_ptr_->nova_robotMode(); + mode_ = mode; + if(mode == 3) + { + statusCode_ = imr_nova_control::ROBOT_CHECK_EMERGENCY_STOP_BUTTON; + + ROS_ERROR("Please check button E-Stop on robot IMR!"); + } + else + { + statusCode_ = imr_nova_control::ROBOT_IN_ALARM_STATE; + + ROS_ERROR("Robot is in alarm state!"); + } + } + } + } + //========================================// + //========================================// + + loop_rate.sleep(); + ros::spinOnce(); + } + //=======================Kết thúc vòng lặp chính===================// + //=================================================================// + //=================================================================// +} + + +void imr_nova_control::imr_dobot_go_home_thread() +{ + statusCode_ = imr_nova_control::ROBOT_WAITING; + + std::vector Point_current(6); + std::vector Center_temp(6); + + int timeStamp_cam = 0; + int count = 0, count_1 = 0xffff; + int center_count = 0; + int get_point_current =0; + + uint16_t speed = 0; + + bool connect = false; + bool drag = false; + + // Khởi tạo + std::shared_ptr nova5_ptr_ = nullptr; + nova5_ptr_ = std::make_shared(IP,PORT); + + nova5_ptr_->run_ptr_ = enable_; + + + // Kết nối đến robot nova + connect = nova5_ptr_->nova_connect(); + if(connect) + { + double mode = nova5_ptr_->nova_robotMode(); + mode_ = mode; + if(mode == 3) + { + statusCode_ = imr_nova_control::ROBOT_POWER_ON; + + if(nova5_ptr_->nova_powerOn() == -1) + { + statusCode_ = imr_nova_control::ROBOT_ERROR; + + ROS_ERROR("Please check robot arm"); + homeThreadRunning_ = false;// biến tạm để cho thấy robot bị lỗi, thay thành các biến phản hồi lỗi + } + } + if(homeThreadRunning_) + { + // Sau khi kết nối thành công cần enable robot + if(nova5_ptr_->nova_enable()) + { + // Sau khi enable robot thành công cần bắt đầu chương trình của ccbox + bool status_start_program = nova5_ptr_->nova_startProgram(); + if(status_start_program) statusCode_ = imr_nova_control::ROBOT_RUNNING; + else + { + statusCode_ = imr_nova_control::ROBOT_ERROR; + + ROS_ERROR("Please check robot arm"); + modeThreadRunning_ = false; + } + } + else + { + statusCode_ = imr_nova_control::ROBOT_ERROR; + + ROS_ERROR("Please check robot arm"); + homeThreadRunning_ = false; + } + } + + } + else + { + + // alarm = nova5_ptr_->nova_statusAlarm(); + ROS_WARN("Robot is reconnecting with maximum time 15 minutes..."); + for(int i=0; i<180U; ++i) + { + statusCode_ = imr_nova_control::ROBOT_RECONNECTING; + + if(enable_ == NULL || !(*enable_)) + { + homeThreadRunning_ = false; + break; + } + connect = nova5_ptr_->nova_connect(); + if(connect || !ros::ok()) + { + while(ros::ok() + && (enable_ != NULL && (*enable_)) + ) + { + double mode = nova5_ptr_->nova_robotMode(); + double alarm = nova5_ptr_->nova_statusAlarm(); + mode_ = mode; + if(mode == 9) + { + statusCode_ = imr_nova_control::ROBOT_ERROR; + + ROS_ERROR("Kinematic ERROR! Please check robot!"); + homeThreadRunning_ = false;// biến tạm để cho thấy robot bị lỗi, thay thành các biến phản hồi lỗi + break; + } + if(mode == 3) + { + if(nova5_ptr_->nova_powerOn() == -1) + { + statusCode_ = imr_nova_control::ROBOT_ERROR; + + ROS_ERROR("Please check robot arm"); + homeThreadRunning_ = false;// biến tạm để cho thấy robot bị lỗi, thay thành các biến phản hồi lỗi + break; + } + } + if(mode == 4) break; + ros::Duration(5).sleep(); + } + + if(homeThreadRunning_) + { + // Sau khi kết nối thành công cần enable robot + if(nova5_ptr_->nova_enable()) + { + // Sau khi enable robot thành công cần bắt đầu chương trình của ccbox + bool status_start_program = nova5_ptr_->nova_startProgram(); + if(status_start_program) statusCode_ = imr_nova_control::ROBOT_RUNNING; + else + { + statusCode_ = imr_nova_control::ROBOT_ERROR; + + ROS_ERROR("Please check robot arm"); + modeThreadRunning_ = false; + } + } + else + { + statusCode_ = imr_nova_control::ROBOT_ERROR; + + ROS_ERROR("Please check robot arm"); + homeThreadRunning_ = false; + } + } + + break; + } + ros::Duration(5).sleep(); + } + + } + + ros::Rate loop_rate(100); + + + //===========================Vòng lặp chính========================// + //=================================================================// + //=================================================================// + while(ros::ok() && connect && homeThreadRunning_ + && (enable_ != NULL && (*enable_)) + ) + { + static bool temp = false; + static bool checkconnect = false; + static bool alarm = false; + + //====Kiểm tra kết nối và trạng thái alarm====// + checkconnect = nova5_ptr_->nova_checkConnected(); + alarm = nova5_ptr_->nova_statusAlarm(); + //============================================// + + if(checkconnect && !alarm) + { + statusCode_ = imr_nova_control::ROBOT_COMING_HOME; + // Lấy trạng thái robot + double robotmode = nova5_ptr_->nova_robotMode(); + mode_ = robotmode; + + //===Kiểm tra trạng thái robot===// + //===============================// + if( robotmode == 3) + { + statusCode_ = imr_nova_control::ROBOT_ERROR; + + ROS_ERROR("Go home failed!"); + ROS_ERROR("Please try again!"); + homeThreadRunning_ = false; + continue; + } + + else if(robotmode == 6) + { + ROS_WARN("Robot is in drag mode"); + drag = true; + } + + if(drag == true && robotmode == 5) + { + statusCode_ = imr_nova_control::ROBOT_ERROR; + + ROS_ERROR("Go home failed!"); + ROS_ERROR("Please try again!"); + homeThreadRunning_ = false; + continue; + } + //===============================// + //===============================// + + + //===Chu trình gắp phôi và lưu trữ phôi===// + //========================================// + if(robotmode ==7) + { + if(get_point_current == 0) + { + static uint16_t y_value = 0; + ROS_INFO("Robot is comming home..."); + //Đọc tọa độ robot hiện tại trong vòng 6s + + nova5_ptr_->nova_getCoorcurrent(Point_current); + + y_value = abs(Point_current[1]) < 300 ? 300 : static_cast(abs(Point_current[1])); + + speed = (y_value / 300.0) * 20; + ROS_WARN("Speed: %d", speed); + + + if(CENTER_P[2] > (Point_current[2] + 10)) + { + Center_temp[0] = Point_current[0]; + Center_temp[1] = Point_current[1]; + Center_temp[2] = nova5_ptr_->nova_check_z(Point_current[0],Point_current[1],CENTER_P[2],0); + Center_temp[3] = Point_current[3]; + Center_temp[4] = Point_current[4]; + Center_temp[5] = Point_current[5]; + } + else if(CENTER_P[2] < (Point_current[2] - 10)) + { + Center_temp[0] = CENTER_P[0]; + Center_temp[1] = CENTER_P[1]; + Center_temp[2] = nova5_ptr_->nova_check_z(CENTER_P[0],CENTER_P[1],Point_current[2], 0); + Center_temp[3] = CENTER_P[3]; + Center_temp[4] = CENTER_P[4]; + Center_temp[5] = CENTER_P[5]; + } + else if(CENTER_P[2] >= (Point_current[2] - 10) && CENTER_P[2] < (Point_current[2] + 10)) center_count = 1; + + get_point_current = 1; + } + /* Bước 0 ve home */ + if(center_count == 0) + { + if(nova5_ptr_->nova_movePoint(Center_temp,speed,0)==1 && center_count == 0) + { + center_count++ ; + } + } + + /* Bước 1 */ + else if(center_count == 1) + { + if(nova5_ptr_->nova_movePoint(CENTER_P,100,1)==1 && center_count == 1) + { + center_count++ ; + } + } + + else if(center_count == 2) + { + if(nova5_ptr_->nova_movePoint(HOME,100,1)==1 && center_count == 2) + { + ROS_INFO("Complete"); + homeThreadRunning_ = false; + continue; + } + } + } + } + + else + { + statusCode_ = imr_nova_control::ROBOT_DISCONNECTED; + ROS_ERROR("Go home failed!"); + ROS_ERROR("Please try again!"); + homeThreadRunning_ = false; + continue; + } + } +} \ No newline at end of file diff --git a/Controllers/Packages/nova5_control/src/nova5_modbus.cpp b/Controllers/Packages/nova5_control/src/nova5_modbus.cpp new file mode 100644 index 0000000..4d88233 --- /dev/null +++ b/Controllers/Packages/nova5_control/src/nova5_modbus.cpp @@ -0,0 +1,670 @@ +#include"nova5_control/nova5_modbus.h" + + + +cnt_nova5::cnt_nova5(std::string ip_address, int port) + : nova_(nullptr), + movePointRunning_(0), + run_ptr_(nullptr) +{ + nova_ = boost::make_shared(ip_address, port); + inter_var = false; + + + // Declare the values of the sphere O + a1 = 0.0; + b1 = 0.0; + c1 = 0.0; + r_max = 750.0; + + // Declare the values of cylinder I + a2 = 0.0; + b2 = 0.0; + r_min = 250.0; + + // Deviation of camera + x_deviation = 8 + (350/2.0); + y_deviation = 100.0 + (234.5/2.0); + z_deviation = 140.5 + 5 - 28.0 + 3.5; // Chiều cao tool + chiều cao camera + dung sai + + z_min_static = 170; + mode_store = 0; + flag_move = 0; +} + +cnt_nova5::~cnt_nova5() +{ + nova_stopProgram(); + nova_close(); +} + +int cnt_nova5::nova_connect(void) +{ + ROS_INFO("Nova is connecting......."); + try + { + + if(!nova_->modbus_connect()) + { + ROS_ERROR("Nova connection is error"); + return 0; + } + else + { + ROS_INFO("Nova is Connected"); + return 1; + } + + } + + catch (...) + { + ROS_ERROR_STREAM("Caught unknown exception"); + return -1; + } + +} + +bool cnt_nova5::nova_checkConnected(void) +{ + try + { + return nova_->_connected; + } + catch (const std::exception& e) + { + ROS_ERROR_STREAM(e.what()); + } + catch (...) + { + ROS_ERROR_STREAM("Caught unknown exception"); + } +} + +void cnt_nova5::nova_disable(void) +{ + nova_dismissMove(); + nova_stopProgram(); + nova_->modbus_write_coil(COIL_DISABLE, true); + ros::Duration(0.1).sleep(); + nova_->modbus_write_coil(COIL_DISABLE, false); + ros::Duration(0.6).sleep(); +} + +void cnt_nova5::nova_close(void) +{ + ROS_INFO("Close robot"); + nova_disable(); + nova_->modbus_close(); +} + +int cnt_nova5::workArealimit(double x, double y, double z) +{ + double dO_instant = z + (1/(2*r_max)) * (pow((x + a1), 2) + pow((y + b1), 2)); + + // INNER BOUNDING CYLINDER + double dI_instant = sqrt(pow((x + a2),2) + pow((y + b2),2)); + + if(dO_instant < r_max) + { + return((dI_instant > r_min) ? WITHIN_LIMIT : OUT_OF_LIMIT); + } + else return ERROR; + +} + +int cnt_nova5::nova_sendCoordinates(std::vector& Point) +{ + try + { + if(Point.size() != 6U) throw std::invalid_argument("A point has 6 parameters"); + if(workArealimit(Point[0],Point[1],Point[2]) == 0) throw std::out_of_range("Point beyond work area"); + if(workArealimit(Point[0],Point[1],Point[2]) == ERROR) throw std::runtime_error("Unknown error"); + + std::array part; + std::vector Point_send(6); + + if(Point[2] < z_min_static) + { + Point_send[0] = Point[0]; + Point_send[1] = Point[1]; + Point_send[2] = z_min_static; + Point_send[3] = Point[3]; + Point_send[4] = Point[4]; + Point_send[5] = Point[5]; + } + else + { + Point_send[0] = Point[0]; + Point_send[1] = Point[1]; + Point_send[2] = Point[2]; + Point_send[3] = Point[3]; + Point_send[4] = Point[4]; + Point_send[5] = Point[5]; + } + + for (int i = 0; i < 6U; ++i) + { + std::memcpy(&part[i*4], reinterpret_cast(&Point_send[i]), sizeof(double)); + } + + if(nova_->modbus_write_registers(HOLDING_TARGET_POINT_START,24,part.data()) == 0) return 1; + else return 0; + } + catch(const std::invalid_argument& e) + { + ROS_ERROR_STREAM("ERROR: " << e.what()); + ros::Duration(TIME_ERR).sleep(); + return ERR; + } + catch(const std::out_of_range& e) + { + ROS_ERROR_STREAM("ERROR: " << e.what()); + ros::Duration(TIME_ERR).sleep(); + return ERR; + } + catch(const std::runtime_error& e) + { + ROS_ERROR_STREAM("ERROR send: " << e.what()); + ros::Duration(TIME_ERR).sleep(); + return ERR; + } + +} + +int cnt_nova5::convert(std::vector& Point, std::vector& Point_initial, std::vector& Coor) +{ + if(Point.size() == 6U) + { + for(int i = 0; i < Point.size(); ++i) + { + Point[i] = Point_initial[i]; + } + + Point[0] = Point[0] + x_deviation - Coor[1] - (x_deviation - Coor[1])*0.15; // x + Point[1] = Point[1] - y_deviation + Coor[2]; //y + Point[2] = Point[2] + z_deviation - Coor[0]; // z + Point[3] = Point[3]; // Rx + Point[4] = Point[4]; // Ry + Point[5] = Point[5] + 90.0 - Coor[3]; // Rz + + return 1; + } + else return ERR; + + +} + +int cnt_nova5::nova_readCoordinates(std::vector& Point,std::vector& Point_initial, uint16_t& isCheck, int& Timestamp) +{ + try + { + std::array register_buffer; + std::vector Point_receive(4); + + if(Point.size() != 6U) throw std::invalid_argument("A point has 6 parameters"); + if (register_buffer.size() < 21) throw std::runtime_error("Buffer size is too small"); + + int status = nova_->modbus_read_holding_registers(HOLDING_CAMERA_POINT_START,21,register_buffer.data()); + + if (status == 0 ) + { + // ROS_INFO("Successfully read registers coordinates"); + + for (int i = 0; i < 4U; ++i) + { + memcpy(&Point_receive[i], ®ister_buffer[i * 4U], sizeof(double)); + // ROS_INFO("Double value %d: %f", i, Point_receive[i]); + } + isCheck = register_buffer[16]; + Timestamp = (static_cast(register_buffer[17]) << 16) | register_buffer[18]; + + // convert(Point,Point_initial,Point_receive); + if(Point_receive[1] == 0 && Point_receive[2] == 0 && isCheck == 0) throw std::runtime_error("Camera error"); + if(convert(Point,Point_initial,Point_receive) == -1) throw std::invalid_argument("A point has 6 parameters"); + if(workArealimit(Point[0],Point[1],Point[2]) == 0) throw std::out_of_range("Point beyond work area"); + if(workArealimit(Point[0],Point[1],Point[2]) == -1) throw std::runtime_error("Unknown error"); + + return 1; + + } + else + { + // ROS_ERROR("Failed to read registers coordinates"); + return 0; + } + } + catch(const std::invalid_argument& e) + { + ROS_ERROR_STREAM("ERROR: " << e.what()); + ros::Duration(TIME_ERR).sleep(); + return ERR; + } + catch(const std::out_of_range& e) + { + ROS_ERROR_STREAM("ERROR: " << e.what()); + ros::Duration(TIME_ERR).sleep(); + return ERR; + } + catch(const std::runtime_error& e) + { + ROS_ERROR_STREAM("ERROR: " << e.what()); + ros::Duration(TIME_ERR).sleep(); + return ERR; + } +} + +int cnt_nova5::nova_status(int add, bool &value) +{ + try + { + if(add < DISCRETE_STOP_STATE || add > DISCRETE_REMOTE_CONTROL) throw std::out_of_range("Register does not exist"); + int status = nova_->modbus_read_coil(add, value); + if(status == 0) + { + // ROS_INFO("Successfully read register status"); + return 1; + } + else + { + // ROS_ERROR("Failed to read register status %d", status); + return 0; + } + } + catch(const std::out_of_range& e) + { + ROS_ERROR_STREAM("ERROR: " << e.what()); + ros::Duration(TIME_ERR).sleep(); + return ERR; + } + catch(...) + { + ROS_ERROR_STREAM("Caught unknown exception"); + return ERR; + } + +} + +int cnt_nova5::nova_multistate(int add, int length, bool *buff) +{ + try + { + if(add < DISCRETE_STOP_STATE || add > DISCRETE_REMOTE_CONTROL) throw std::out_of_range("Register does not exist"); + if(length > (DISCRETE_REMOTE_CONTROL - add) || length <= 0) throw std::out_of_range("Invalid length value"); + + int status = nova_->modbus_read_coils(add, length, buff); + if(status == 0) + { + // ROS_INFO("Successfully read register multistate"); + return 1; + } + else + { + // ROS_ERROR("Failed to read register multistate %d", status); + return 0; + } + } + catch(const std::out_of_range& e) + { + ROS_ERROR_STREAM("ERROR: " << e.what()); + ros::Duration(TIME_ERR).sleep(); + return ERR; + } + catch(...) + { + ROS_ERROR_STREAM("Caught unknown exception"); + return ERR; + } +} + +int cnt_nova5::nova_timeStamp(uint64_t& time) +{ + try + { + std::array register_buffer; + int status = nova_->modbus_read_holding_registers(INPUT_TIMESTAMP_START,4, register_buffer.data()); + if(status == 0) + { + // ROS_INFO("Successfully read register time stamp"); + + memcpy(&time,®ister_buffer,sizeof(uint64_t)); + + return 1; + } + else + { + // ROS_ERROR("Failed to read register time stamp %d", status); + return 0; + } + } + catch(const std::exception& e) + { + ROS_ERROR_STREAM("Caught unknown exception"); + return ERR; + } + +} + +double cnt_nova5::nova_robotMode(void) +{ + uint64_t mode_uint64; + std::array register_buffer; + int status = nova_->modbus_read_input_registers(INPUT_ROBOT_MODE_START,4,register_buffer.data()); + + if(status == 0) + { + // ROS_INFO("Successfully read register robot mode"); + + memcpy(&mode_uint64, ®ister_buffer, sizeof(uint64_t)); + if(mode_uint64 != mode_store) ROS_INFO("Mode: %ld",mode_uint64); + mode_store = mode_uint64; + double mode_double = (double)mode_uint64; + return mode_uint64; + } + else + { + // ROS_ERROR("Failed to read register robot mode %d", status); + return ERR; + } + +} + +int cnt_nova5::nova_powerOn(void) +{ + static int count = 0; + if(run_ptr_ == nullptr) run_ptr_ = &inter_var; + while(ros::ok() && nova_robotMode() == 3 && *run_ptr_) + { + ROS_INFO("Wait 25s for robot to power on..."); + if(nova_statusAlarm() == 1) + { + nova_->modbus_write_coil(COIL_CLEAR_ALARM, true); + ros::Duration(0.01).sleep(); + } + + nova_->modbus_write_coil(COIL_POWER_ON, true); + ros::Duration(20).sleep(); + + if(nova_robotMode() == 3) + { + nova_->modbus_write_coil(COIL_CLEAR_ALARM, false); + ros::Duration(0.01).sleep(); + nova_->modbus_write_coil(COIL_POWER_ON, false); + ros::Duration(0.01).sleep(); + count++; + if(count>1) + { + ROS_ERROR("Power on failed..."); + count = 0; + return -1; + break; + } + } + else + { + ROS_INFO("Power on successfully"); + nova_->modbus_write_coil(COIL_CLEAR_ALARM, false); + ros::Duration(0.01).sleep(); + nova_->modbus_write_coil(COIL_POWER_ON, false); + ros::Duration(0.01).sleep(); + return 1; + break; + } + ros::spinOnce(); + } + + + return 0; +} + +bool cnt_nova5::nova_enable(void) +{ + int status_clear_alarm = nova_->modbus_write_coil(COIL_CLEAR_ALARM, true); + ros::Duration(0.01).sleep(); + int status_enable = nova_->modbus_write_coil(COIL_ENABLE, true); + ros::Duration(0.6).sleep(); + + nova_->modbus_write_coil(COIL_CLEAR_ALARM, false); + ros::Duration(0.01).sleep(); + nova_->modbus_write_coil(COIL_ENABLE, false); + ros::Duration(0.01).sleep(); + + double mode = nova_robotMode(); + + if(mode >= 5 && status_clear_alarm == 0 && status_enable == 0 ) + { + ROS_INFO("Enable done"); + return true; + } + else + { + ROS_ERROR("Enable failed"); + return false; + } +} + +void cnt_nova5::nova_allowMove(void) +{ + nova_->modbus_write_coil(COILD_MOVE,true); + ros::Duration(0.3).sleep(); +} + +void cnt_nova5::nova_dismissMove(void) +{ + nova_->modbus_write_coil(COILD_MOVE,false); + ros::Duration(0.01).sleep(); +} + +void cnt_nova5::nova_stopProgram(void) +{ + nova_->modbus_write_coil(COIL_STOP, true); + ros::Duration(0.01).sleep(); + nova_->modbus_write_coil(COIL_STOP, false); + ros::Duration(0.5).sleep(); +} + +bool cnt_nova5::nova_startProgram(void) +{ + flag_move = 0; + nova_dismissMove(); + nova_offTool(0.01); + if(run_ptr_ == nullptr) run_ptr_ = &inter_var; + int i = 0; + while(i < 4 && ros::ok() && *run_ptr_) + { + nova_->modbus_write_coil(COIL_START, true); + ros::Duration(0.3).sleep(); + nova_->modbus_write_coil(COIL_START, false); + ros::Duration(0.02).sleep(); + uint64_t mode = nova_robotMode(); + if(mode == 7) break; + + i++; + } + uint64_t mode = nova_robotMode(); + if(mode == 7) return true; + else return false; +} + +bool cnt_nova5::nova_movingDone(void) +{ + + bool status; + nova_->modbus_read_coil(COILD_MOVING_DONE, status); + // ROS_INFO("STATUS movingDone %d",status); + return status; + +} + +bool cnt_nova5::nova_statusAlarm(void) +{ + bool alarm = false; + nova_->modbus_read_input_bit(DISCRETE_ALARM_STATE,alarm); + // ROS_INFO("ALARM ST: %x",alarm); + + return alarm; +} + +void cnt_nova5::nova_clearAlarm() +{ + nova_->modbus_write_coil(COIL_CLEAR_ALARM, true); + ros::Duration(0.1).sleep(); + nova_->modbus_write_coil(COIL_CLEAR_ALARM, false); + ros::Duration(0.01).sleep(); +} + +void cnt_nova5::nova_onTool(void) +{ + ROS_INFO("ON TOOL"); + nova_->modbus_write_coil(COILD_TOOL, true); + ros::Duration(2).sleep(); +} + +void cnt_nova5::nova_offTool(const double delay) +{ + ROS_INFO("OFF TOOL"); + nova_->modbus_write_coil(COILD_TOOL, false); + ros::Duration(delay).sleep(); +} + +void cnt_nova5::nova_speedGlobal(uint16_t speed) +{ + nova_->modbus_write_register(HOLDING_GLOBAL_SPEED,speed); + // ros::Duration(0.01).sleep(); +} + +int cnt_nova5::nova_getCoorcurrent(std::vector& Point) +{ + try + { + if(Point.size() != 6U) throw std::invalid_argument("Coordinates have 6 parameters"); + std::array register_buffer; + for(int i = 0; i <= 5; i++) + { + ros::Duration(1).sleep(); + int status = nova_->modbus_read_holding_registers(HOLDING_POINT_CURRENT_START,24,register_buffer.data()); + if (status == 0) + { + for(int i = 0; i < 6U; ++i) + { + memcpy(&Point[i], ®ister_buffer[i * 4U], sizeof(double)); + } + if((Point[0] != 0 && Point[1] != 0 && Point[2] !=0) || + (!ros::ok()) || nova_robotMode() != 7 || !nova_checkConnected() && *run_ptr_ == false) break; + if(i == 5) return 0; + } + else return ERR; + } + return 0; + } + catch(const std::invalid_argument& e) + { + ROS_ERROR_STREAM("ERROR: " << e.what()); + ros::Duration(TIME_ERR).sleep(); + return ERR; + } + catch(const std::exception& e) + { + std::cerr << e.what() << '\n'; + return ERR; + } +} + +double cnt_nova5::getZmin(void) +{ + std::array register_buffer; + double Z_min; + + nova_->modbus_read_holding_registers(HOLDING_Z_MIN_USER_SET_START,4,register_buffer.data()); + memcpy(&Z_min, ®ister_buffer, sizeof(double)); + ROS_INFO("Z min : %f",Z_min); + return Z_min; +} + +double cnt_nova5::nova_check_z(double& x, double& y, double& z, int choose) +{ + static double z_min_store = getZmin(); + static double z_min; + + double z_max = r_max - 10 - (1/(2*r_max)) * (pow((x + a1), 2) + pow((y + b1), 2)); + + if(choose == 1) z_min = z_min_static; + else if(choose == 0) z_min = z_min_store; + + if(z_max > z_min) + { + if(z > z_min && z < z_max) return z; + else if(z <= z_min) return z_min; + else if(z >= z_max) return z_max; + return ERR; + } + else return z_min; + + +} + +int cnt_nova5::nova_movePoint(std::vector& Point,const int speed, const int mode) +{ + + try + { + /* code */ + if(Point.size() != 6U) throw std::invalid_argument("A point has 6 parameters - move Point"); + if(workArealimit(Point[0],Point[1],Point[2]) == 0) throw std::out_of_range("Point beyond work area - move Point"); + if(workArealimit(Point[0],Point[1],Point[2]) == -1) throw std::runtime_error("Unknown error - move Point"); + + movePointRunning_ = 1; + + if(flag_move == 0) + { + // Chọn chế độ di chuyển moveJ move L + nova_->modbus_write_coil(COILD_MOVE_MODE, mode); + + // Cài đặt tốc độ toàn cục cho tay máy + nova_speedGlobal(speed); + + // Gửi tọa độ mục tiêu và xác nhận cho robot di chuyển đến tọa độ mục tiêu cho robot tay máy + if(nova_sendCoordinates(Point) == 1) nova_allowMove(); + else return 0; + flag_move = 1; + } + + // Chờ cho robot di chuyển xong + if(nova_movingDone() == 1) + { + nova_dismissMove(); + flag_move = 0; + return 1; + } + // If no movement completed, return an error + return 0; + } + catch(const std::invalid_argument& e) + { + ROS_ERROR_STREAM("ERROR: " << e.what()); + ros::Duration(TIME_ERR).sleep(); + movePointRunning_ = ERR; + return ERR; + } + catch(const std::out_of_range& e) + { + ROS_ERROR_STREAM("ERROR: " << e.what()); + ros::Duration(TIME_ERR).sleep(); + movePointRunning_ = ERR; + return ERR; + } + catch(const std::runtime_error& e) + { + ROS_ERROR_STREAM("ERROR send: " << e.what()); + ros::Duration(TIME_ERR).sleep(); + movePointRunning_ = ERR; + return ERR; + } + catch(const std::exception& e) + { + ROS_ERROR_STREAM("ERROR: " << e.what()); + ros::Duration(TIME_ERR).sleep(); + movePointRunning_ = ERR; + return ERR; + } +} diff --git a/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/CHANGELOG.rst b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/CHANGELOG.rst new file mode 100755 index 0000000..35f3563 --- /dev/null +++ b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/CHANGELOG.rst @@ -0,0 +1,237 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package diff_drive_controller +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.21.2 (2023-09-03) +------------------- + +0.21.1 (2023-01-31) +------------------- +* Don't hardcode plugin library path +* Contributors: Jochen Sprickerhof + +0.21.0 (2022-10-21) +------------------- +* std::bind and placeholders instead of boost +* use boost::placeholders::_1/_2 in remaining instances, include boost/bind/bind.hpp instead of boost/bind.hpp, eliminated unnecessary bind.hpp include +* Contributors: Lucas Walter + +0.20.0 (2022-05-06) +------------------- +* Drop old C++ standard +* Use new boost bind placeholders +* Add to every .launch and .test file +* Contributors: Jochen Sprickerhof, Lucas Walter + +0.19.0 (2021-06-13) +------------------- +* fix NaN bug +* fix test to expose NaN bug +* Wait long enough for accumulator to be cleared +* Add test for `#532 `_ + Close `#540 `_ +* Contributors: Caio Amaral, Matt Reynolds, Melvin Wang + +0.18.1 (2020-12-03) +------------------- +* Fix null pointer checks in diff_drive_controller +* Use version-agnostic FindBoost for headers +* Contributors: David V. Lu, Matt Reynolds + +0.18.0 (2020-10-11) +------------------- +* Fix dependency on Boost +* Apply consistent format to CMakeLists.txt +* Update package.xml to format 3 +* Clean dependencies of diff_drive_controller package +* Apply waitForController method to all diff_drive_controller tests +* Move odom_pub setup to the end to allow consistent isControllerAlive check +* Contributors: Mateus Amarante + +0.17.0 (2020-05-12) +------------------- + +0.16.1 (2020-04-27) +------------------- + +0.16.0 (2020-04-16) +------------------- +* Fix warning dynamic_reconfigure +* Bump CMake version to prevent CMP0048 +* Add missing header guards +* Replace header guard with #pragma once +* Prefix every xacro tag with 'xacro:' +* Modernize xacro + - Remove '--inorder' + - Use 'xacro' over 'xacro.py' +* switch implementation of read and write methods of Diffbot class +* Refactor nan test + EXPECT_NE(x, bool) -> EXPECT_TRUE/FALSE(x) + EXPECT_EQ(x, double) -> EXPECT_DOUBLE_EQ(x, double) + + clang default reformat +* Check for nan cmd_vel +* Contributors: Anas Abou Allaban, Bence Magyar, Franz, Matt Reynolds, Raffaello Bonghi + +0.15.1 (2020-03-09) +------------------- +* Use nullptr +* add missing pluginlib deps. +* Update null link pointer error message +* Revert cmake include catkin_INCLUDE_DIRS as system +* Use C++11 `std::this_thread::sleep_for`. +* Contributors: Bence Magyar, Enrique Fernandez Perdomo, Matt Reynolds, Sean Yen + +0.15.0 (2019-03-26) +------------------- +* Default all controller builds to C++14 +* boost::chrono -> std::chrono +* boost::assign -> C++ initializer list +* boost::shared_ptr -> std::shared_ptr +* Using left/right multiplies for desired vel +* diff-drive publish joint trajectory controller state +* fix install destination for libraries (`#403 `_) +* Contributors: Bence Magyar, Gennaro Raiola, James Xu, Jeremie Deray, Jordan Palacios + +0.14.3 (2019-02-09) +------------------- +* use operators instead of aliases +* Fix typo descripion -> description +* Contributors: Daniel Ingram, James Xu + +0.14.2 (2018-10-23) +------------------- + +0.14.1 (2018-06-26) +------------------- +* Added 'multiplier' in DynamicParams ostream and changed boolean printing to 'enabled/disabled' +* isPublishngCmdVelOut to check getNumPublisheres until timeout +* Contributors: Kei Okada, Martin Ganeff + +0.14.0 (2018-04-27) +------------------- +* add dynamic_reconf to diff_drive_controller +* migrate to new pluginlib headers +* per wheel radius multiplier +* fix xacro macro warning +* [DiffDrive] Fix time-sensitive tests of diff_drive_controller +* separate include_directories as SYSTEM to avoid unrelated compilation warnings +* Contributors: Jeremie Deray, Mathias Lüdtke + +0.13.2 (2017-12-23) +------------------- + +0.13.1 (2017-11-06) +------------------- + +0.13.0 (2017-08-10) +------------------- +* Add test for allow_multiple_cmd_vel_publishers param +* add check for multiple publishers on cmd_vel +* Added tests for the odom_frame_id parameter. +* Parameterized diff_drive_controller's odom_frame_id +* Publish executed velocity if publish_cmd +* refactor to remove code duplication +* fixup pointer type for new convention +* Allow diff_drive_controller to use spheres as well as cylinders for wheel collision geometry. Cylinders are not well behaved on Gazebo/ODE heightfields, using spheres works around the issue. +* Contributors: Bence Magyar, Eric Tappan, Jeremie Deray, Karsten Knese, Tully Foote, mallanmba, tappan-at-git + +0.12.3 (2017-04-23) +------------------- + +0.12.2 (2017-04-21) +------------------- + +0.12.1 (2017-03-08) +------------------- +* Add exporting include dirs +* Contributors: Bence Magyar + +0.12.0 (2017-02-15) +------------------- +* Fix most catkin lint issues +* Change for format2 +* Add Enrique and Bence to maintainers +* Add urdf compatibility header +* Add --inorder to xacro calls +* Add missing xacro tags +* Use xacro instead of xacro.py +* Disable angular jerk limit test +* Replace boost::shared_ptr with urdf::XYConstSharedPtr when exists +* Contributors: Bence Magyar + +0.11.2 (2016-08-16) +------------------- + +0.11.1 (2016-05-23) +------------------- + +0.11.0 (2016-05-03) +------------------- + +0.10.0 (2015-11-20) +------------------- +* Address -Wunused-parameter warnings +* Limit jerk +* Add param velocity_rolling_window_size +* Minor fixes + 1. Coding style + 2. Tolerance to fall-back to Runge-Kutta 2 integration + 3. Remove unused variables +* Fix the following bugs in the testForward test: + 1. Check traveled distance in XY plane + 2. Use expected speed variable on test check +* Add test for NaN +* Add test for bad URDF +* Contributors: Adolfo Rodriguez Tsouroukdissian, Enrique Fernandez, Paul Mathieu + +0.9.2 (2015-05-04) +------------------ +* Allow the wheel separation and radius to be set from different sources + i.e. one can be set from the URDF, the other from the parameter server. + If wheel separation and wheel diameter is specified in the parameter server, don't look them up from urdf +* Contributors: Bence Magyar, Nils Berg + +0.9.1 (2014-11-03) +------------------ + +0.9.0 (2014-10-31) +------------------ +* Add support for multiple wheels per side +* Odometry computation: + - New option to compute in open loop fashion + - New option to skip publishing odom frame to tf +* Remove dependency on angles package +* Buildsystem fixes +* Contributors: Bence Magyar, Lukas Bulwahn, efernandez + +0.8.1 (2014-07-11) +------------------ + +0.8.0 (2014-05-12) +------------------ +* Add base_frame_id param (defaults to base_link) + The nav_msgs/Odometry message specifies the child_frame_id field, + which was previously not set. + This commit creates a parameter to replace the previously hard-coded + value of the child_frame_id of the published tf frame, and uses it + in the odom message as well. +* Contributors: enriquefernandez + +0.7.2 (2014-04-01) +------------------ + +0.7.1 (2014-03-31) +------------------ +* Changed test-depend to build-depend for release jobs. +* Contributors: Bence Magyar + +0.7.0 (2014-03-28) +------------------ +* diff_drive_controller: New controller for differential drive wheel systems. +* Control is in the form of a velocity command, that is split then sent on the two wheels of a differential drive +wheel base. +* Odometry is published to tf and to a dedicated nav__msgs/Odometry topic. +* Realtime-safe implementation. +* Implements task-space velocity and acceleration limits. +* Automatic stop after command time-out. +* Contributors: Bence Magyar, Paul Mathieu, Enrique Fernandez. diff --git a/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/CMakeLists.txt b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/CMakeLists.txt new file mode 100755 index 0000000..93a7d95 --- /dev/null +++ b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/CMakeLists.txt @@ -0,0 +1,216 @@ +cmake_minimum_required(VERSION 3.0.2) +project(diff_drive_controller) + +# Load catkin and all dependencies required for this package +find_package(catkin REQUIRED COMPONENTS + controller_interface + control_msgs + dynamic_reconfigure + geometry_msgs + hardware_interface + nav_msgs + pluginlib + realtime_tools + tf + urdf +) + +find_package(Boost REQUIRED) + +# Declare a catkin package +generate_dynamic_reconfigure_options(cfg/DiffDriveController.cfg) + +catkin_package( + INCLUDE_DIRS include + CATKIN_DEPENDS + controller_interface + control_msgs + dynamic_reconfigure + geometry_msgs + hardware_interface + nav_msgs + realtime_tools + tf + LIBRARIES ${PROJECT_NAME} + DEPENDS Boost +) + + +########### +## Build ## +########### + +# Specify header include paths +include_directories( + include ${catkin_INCLUDE_DIRS} + include ${Boost_INCLUDE_DIRS} +) + +add_library(${PROJECT_NAME} + src/diff_drive_controller.cpp + src/odometry.cpp + src/speed_limiter.cpp +) +target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) +add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${PROJECT_NAME}_gencfg) + + +############# +## Testing ## +############# + +if (CATKIN_ENABLE_TESTING) + find_package(controller_manager REQUIRED) + find_package(rosgraph_msgs REQUIRED) + find_package(std_srvs REQUIRED) + find_package(rostest REQUIRED) + + include_directories( + ${controller_manager_INCLUDE_DIRS} + ${rosgraph_msgs_INCLUDE_DIRS} + ${std_srvs_INCLUDE_DIRS} + ) + + set(test_LIBRARIES + ${controller_manager_LIBRARIES} + ${rosgraph_msgs_LIBRARIES} + ${std_srvs_LIBRARIES} + ) + + add_executable(diffbot test/diffbot.cpp) + target_link_libraries(diffbot ${catkin_LIBRARIES} ${test_LIBRARIES}) + + add_executable(skidsteerbot test/skidsteerbot.cpp) + target_link_libraries(skidsteerbot ${catkin_LIBRARIES} ${test_LIBRARIES}) + + add_dependencies(tests diffbot skidsteerbot) + + add_rostest_gtest(diff_drive_test + test/diff_drive_controller.test + test/diff_drive_test.cpp + ) + target_link_libraries(diff_drive_test ${catkin_LIBRARIES} ${test_LIBRARIES}) + + add_rostest_gtest(diff_drive_nan_test + test/diff_drive_controller_nan.test + test/diff_drive_nan_test.cpp + ) + target_link_libraries(diff_drive_nan_test ${catkin_LIBRARIES} ${test_LIBRARIES}) + + add_rostest_gtest(diff_drive_limits_test + test/diff_drive_controller_limits.test + test/diff_drive_limits_test.cpp + ) + target_link_libraries(diff_drive_limits_test ${catkin_LIBRARIES} ${test_LIBRARIES}) + + add_rostest_gtest(diff_drive_timeout_test + test/diff_drive_timeout.test + test/diff_drive_timeout_test.cpp + ) + target_link_libraries(diff_drive_timeout_test ${catkin_LIBRARIES} ${test_LIBRARIES}) + + add_rostest(test/diff_drive_multipliers.test) + + add_rostest(test/diff_drive_left_right_multipliers.test) + + add_rostest_gtest(diff_drive_fail_test + test/diff_drive_wrong.test + test/diff_drive_fail_test.cpp + ) + target_link_libraries(diff_drive_fail_test ${catkin_LIBRARIES} ${test_LIBRARIES}) + + add_rostest_gtest(diff_drive_odom_tf_test + test/diff_drive_odom_tf.test + test/diff_drive_odom_tf_test.cpp + ) + target_link_libraries(diff_drive_odom_tf_test ${catkin_LIBRARIES} ${test_LIBRARIES}) + + add_rostest_gtest(diff_drive_default_cmd_vel_out_test + test/diff_drive_default_cmd_vel_out.test + test/diff_drive_default_cmd_vel_out_test.cpp + ) + target_link_libraries(diff_drive_default_cmd_vel_out_test ${catkin_LIBRARIES} ${test_LIBRARIES}) + + add_rostest_gtest(diff_drive_pub_cmd_vel_out_test + test/diff_drive_pub_cmd_vel_out.test + test/diff_drive_pub_cmd_vel_out_test.cpp + ) + target_link_libraries(diff_drive_pub_cmd_vel_out_test ${catkin_LIBRARIES} ${test_LIBRARIES}) + + add_rostest_gtest(diff_drive_default_wheel_joint_controller_state_test + test/diff_drive_default_wheel_joint_controller_state.test + test/diff_drive_default_wheel_joint_controller_state_test.cpp + ) + target_link_libraries(diff_drive_default_wheel_joint_controller_state_test ${catkin_LIBRARIES} ${test_LIBRARIES}) + + add_rostest_gtest(diff_drive_publish_wheel_joint_controller_state_test + test/diff_drive_publish_wheel_joint_controller_state.test + test/diff_drive_publish_wheel_joint_controller_state_test.cpp + ) + target_link_libraries(diff_drive_publish_wheel_joint_controller_state_test ${catkin_LIBRARIES} ${test_LIBRARIES}) + + add_rostest_gtest(diff_drive_default_odom_frame_test + test/diff_drive_default_odom_frame.test + test/diff_drive_default_odom_frame_test.cpp + ) + target_link_libraries(diff_drive_default_odom_frame_test ${catkin_LIBRARIES} ${test_LIBRARIES}) + + add_rostest_gtest(diff_drive_odom_frame_test + test/diff_drive_odom_frame.test + test/diff_drive_odom_frame_test.cpp + ) + target_link_libraries(diff_drive_odom_frame_test ${catkin_LIBRARIES} ${test_LIBRARIES}) + + add_rostest_gtest(diff_drive_multiple_cmd_vel_publishers_test + test/diff_drive_multiple_cmd_vel_publishers.test + test/diff_drive_multiple_cmd_vel_publishers_test.cpp + ) + target_link_libraries(diff_drive_multiple_cmd_vel_publishers_test ${catkin_LIBRARIES} ${test_LIBRARIES}) + + add_rostest(test/diff_drive_open_loop.test) + + add_rostest(test/skid_steer_controller.test) + + add_rostest(test/skid_steer_no_wheels.test) + + add_rostest(test/diff_drive_radius_sphere.test) + + add_rostest(test/diff_drive_radius_param.test) + + add_rostest(test/diff_drive_radius_param_fail.test) + + add_rostest(test/diff_drive_separation_param.test) + + add_rostest(test/diff_drive_bad_urdf.test) + + add_rostest(test/diff_drive_get_wheel_radius_fail.test) + + add_rostest_gtest(diff_drive_dyn_reconf_test + test/diff_drive_dyn_reconf.test + test/diff_drive_dyn_reconf_test.cpp + ) + target_link_libraries(diff_drive_dyn_reconf_test ${catkin_LIBRARIES} ${test_LIBRARIES}) + +endif() + + +############# +## Install ## +############# + +# Install headers +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +) + +# Install library +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) + +# Install plugins +install(FILES diff_drive_controller_plugins.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) diff --git a/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/README.md b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/README.md new file mode 100755 index 0000000..371baed --- /dev/null +++ b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/README.md @@ -0,0 +1,5 @@ +## Diff Drive Controller ## + +Controller for a differential drive mobile base. + +Detailed user documentation can be found in the controller's [ROS wiki page](http://wiki.ros.org/diff_drive_controller). diff --git a/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/cfg/DiffDriveController.cfg b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/cfg/DiffDriveController.cfg new file mode 100755 index 0000000..31a1668 --- /dev/null +++ b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/cfg/DiffDriveController.cfg @@ -0,0 +1,18 @@ +#!/usr/bin/env python + +PACKAGE = 'diff_drive_controller' + +from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, bool_t, double_t + +gen = ParameterGenerator() + +# Kinematic parameters related +gen.add("left_wheel_radius_multiplier", double_t, 0, "Left wheel radius multiplier.", 1.0, 0.5, 1.5) +gen.add("right_wheel_radius_multiplier", double_t, 0, "Right wheel radius multiplier.", 1.0, 0.5, 1.5) +gen.add("wheel_separation_multiplier", double_t, 0, "Wheel separation multiplier.", 1.0, 0.5, 1.5) + +# Publication related +gen.add("publish_rate", double_t, 0, "Publish rate of odom.", 50.0, 0.0, 2000.0) +gen.add("enable_odom_tf", bool_t, 0, "Publish odom frame to tf.", True) + +exit(gen.generate(PACKAGE, "diff_drive_controller", "DiffDriveController")) diff --git a/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/diff_drive_controller_plugins.xml b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/diff_drive_controller_plugins.xml new file mode 100755 index 0000000..a1c1d05 --- /dev/null +++ b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/diff_drive_controller_plugins.xml @@ -0,0 +1,9 @@ + + + + + The DiffDriveController tracks velocity commands. It expects 2 VelocityJointInterface type of hardware interfaces. + + + + diff --git a/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.h b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.h new file mode 100755 index 0000000..6299bb0 --- /dev/null +++ b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.h @@ -0,0 +1,310 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, PAL Robotics, S.L. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the PAL Robotics nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* + * Author: Bence Magyar, Enrique Fernández + */ + +#pragma once +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace diff_drive_controller{ + + /** + * This class makes some assumptions on the model of the robot: + * - the rotation axes of wheels are collinear + * - the wheels are identical in radius + * Additional assumptions to not duplicate information readily available in the URDF: + * - the wheels have the same parent frame + * - a wheel collision geometry is a cylinder or sphere in the urdf + * - a wheel joint frame center's vertical projection on the floor must lie within the contact patch + */ + class DiffDriveController + : public controller_interface::Controller + { + public: + DiffDriveController(); + + /** + * \brief Initialize controller + * \param hw Velocity joint interface for the wheels + * \param root_nh Node handle at root namespace + * \param controller_nh Node handle inside the controller namespace + */ + bool init(hardware_interface::VelocityJointInterface* hw, + ros::NodeHandle& root_nh, + ros::NodeHandle &controller_nh); + + /** + * \brief Updates controller, i.e. computes the odometry and sets the new velocity commands + * \param time Current time + * \param period Time since the last called to update + */ + void update(const ros::Time& time, const ros::Duration& period); + + /** + * \brief Starts controller + * \param time Current time + */ + void starting(const ros::Time& time); + + /** + * \brief Stops controller + * \param time Current time + */ + void stopping(const ros::Time& /*time*/); + + private: + std::string name_; + + /// Odometry related: + ros::Duration publish_period_; + ros::Time last_state_publish_time_; + bool open_loop_; + + /// Hardware handles: + std::vector left_wheel_joints_; + std::vector right_wheel_joints_; + + // Previous time + ros::Time time_previous_; + + /// Previous velocities from the encoders: + std::vector vel_left_previous_; + std::vector vel_right_previous_; + + /// Previous velocities from the encoders: + double vel_left_desired_previous_; + double vel_right_desired_previous_; + + /// Velocity command related: + struct Commands + { + double lin; + double ang; + ros::Time stamp; + + Commands() : lin(0.0), ang(0.0), stamp(0.0) {} + }; + realtime_tools::RealtimeBuffer command_; + Commands command_struct_; + ros::Subscriber sub_command_; + + /// Publish executed commands + std::shared_ptr > cmd_vel_pub_; + + /// Odometry related: + std::shared_ptr > odom_pub_; + std::shared_ptr > tf_odom_pub_; + Odometry odometry_; + + /// Controller state publisher + std::shared_ptr > controller_state_pub_; + + /// Wheel separation, wrt the midpoint of the wheel width: + double wheel_separation_; + + /// Wheel radius (assuming it's the same for the left and right wheels): + double wheel_radius_; + + /// Wheel separation and radius calibration multipliers: + double wheel_separation_multiplier_; + double left_wheel_radius_multiplier_; + double right_wheel_radius_multiplier_; + + /// Timeout to consider cmd_vel commands old: + double cmd_vel_timeout_; + + /// Whether to allow multiple publishers on cmd_vel topic or not: + bool allow_multiple_cmd_vel_publishers_; + + /// Frame to use for the robot base: + std::string base_frame_id_; + + /// Frame to use for odometry and odom tf: + std::string odom_frame_id_; + + /// Whether to publish odometry to tf or not: + bool enable_odom_tf_; + + /// Number of wheel joints: + size_t wheel_joints_size_; + + /// Speed limiters: + Commands last1_cmd_; + Commands last0_cmd_; + SpeedLimiter limiter_lin_; + SpeedLimiter limiter_ang_; + + /// Publish limited velocity: + bool publish_cmd_; + + /// Publish wheel data: + bool publish_wheel_joint_controller_state_; + + // A struct to hold dynamic parameters + // set from dynamic_reconfigure server + struct DynamicParams + { + bool update; + + double left_wheel_radius_multiplier; + double right_wheel_radius_multiplier; + double wheel_separation_multiplier; + + bool publish_cmd; + + double publish_rate; + bool enable_odom_tf; + + DynamicParams() + : left_wheel_radius_multiplier(1.0) + , right_wheel_radius_multiplier(1.0) + , wheel_separation_multiplier(1.0) + , publish_cmd(false) + , publish_rate(50) + , enable_odom_tf(true) + {} + + friend std::ostream& operator<<(std::ostream& os, const DynamicParams& params) + { + os << "DynamicParams:\n" + // + << "\tOdometry parameters:\n" + << "\t\tleft wheel radius multiplier: " << params.left_wheel_radius_multiplier << "\n" + << "\t\tright wheel radius multiplier: " << params.right_wheel_radius_multiplier << "\n" + << "\t\twheel separation multiplier: " << params.wheel_separation_multiplier << "\n" + // + << "\tPublication parameters:\n" + << "\t\tPublish executed velocity command: " << (params.publish_cmd?"enabled":"disabled") << "\n" + << "\t\tPublication rate: " << params.publish_rate << "\n" + << "\t\tPublish frame odom on tf: " << (params.enable_odom_tf?"enabled":"disabled"); + + return os; + } + }; + + realtime_tools::RealtimeBuffer dynamic_params_; + + /// Dynamic Reconfigure server + typedef dynamic_reconfigure::Server ReconfigureServer; + std::shared_ptr dyn_reconf_server_; + boost::recursive_mutex dyn_reconf_server_mutex_; + + private: + /** + * \brief Brakes the wheels, i.e. sets the velocity to 0 + */ + void brake(); + + /** + * \brief Velocity command callback + * \param command Velocity command message (twist) + */ + void cmdVelCallback(const geometry_msgs::Twist& command); + + /** + * \brief Get the wheel names from a wheel param + * \param [in] controller_nh Controller node handler + * \param [in] wheel_param Param name + * \param [out] wheel_names Vector with the whel names + * \return true if the wheel_param is available and the wheel_names are + * retrieved successfully from the param server; false otherwise + */ + bool getWheelNames(ros::NodeHandle& controller_nh, + const std::string& wheel_param, + std::vector& wheel_names); + + /** + * \brief Sets odometry parameters from the URDF, i.e. the wheel radius and separation + * \param root_nh Root node handle + * \param left_wheel_name Name of the left wheel joint + * \param right_wheel_name Name of the right wheel joint + */ + bool setOdomParamsFromUrdf(ros::NodeHandle& root_nh, + const std::string& left_wheel_name, + const std::string& right_wheel_name, + bool lookup_wheel_separation, + bool lookup_wheel_radius); + + /** + * \brief Sets the odometry publishing fields + * \param root_nh Root node handle + * \param controller_nh Node handle inside the controller namespace + */ + void setOdomPubFields(ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh); + + /** + * \brief Callback for dynamic_reconfigure server + * \param config The config set from dynamic_reconfigure server + * \param level not used at this time. + * \see dyn_reconf_server_ + */ + void reconfCallback(DiffDriveControllerConfig& config, uint32_t /*level*/); + + /** + * \brief Update the dynamic parameters in the RT loop + */ + void updateDynamicParams(); + + /** + * \brief + * \param time Current time + * \param period Time since the last called to update + * \param curr_cmd Current velocity command + * \param wheel_separation wheel separation with multiplier + * \param left_wheel_radius left wheel radius with multiplier + * \param right_wheel_radius right wheel radius with multiplier + */ + void publishWheelData(const ros::Time& time, + const ros::Duration& period, + Commands& curr_cmd, + double wheel_separation, + double left_wheel_radius, + double right_wheel_radius); + }; + +} // namespace diff_drive_controller diff --git a/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/include/diff_drive_controller/odometry.h b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/include/diff_drive_controller/odometry.h new file mode 100755 index 0000000..39a5eb8 --- /dev/null +++ b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/include/diff_drive_controller/odometry.h @@ -0,0 +1,210 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, PAL Robotics, S.L. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the PAL Robotics nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* + * Author: Luca Marchionni + * Author: Bence Magyar + * Author: Enrique Fernández + * Author: Paul Mathieu + */ + +#pragma once + + +#include +#include +#include +#include +#include + +namespace diff_drive_controller +{ + namespace bacc = boost::accumulators; + + /** + * \brief The Odometry class handles odometry readings + * (2D pose and velocity with related timestamp) + */ + class Odometry + { + public: + + /// Integration function, used to integrate the odometry: + typedef boost::function IntegrationFunction; + + /** + * \brief Constructor + * Timestamp will get the current time value + * Value will be set to zero + * \param velocity_rolling_window_size Rolling window size used to compute the velocity mean + */ + Odometry(size_t velocity_rolling_window_size = 10); + + /** + * \brief Initialize the odometry + * \param time Current time + */ + void init(const ros::Time &time); + + /** + * \brief Updates the odometry class with latest wheels position + * \param left_pos Left wheel position [rad] + * \param right_pos Right wheel position [rad] + * \param time Current time + * \return true if the odometry is actually updated + */ + bool update(double left_pos, double right_pos, const ros::Time &time); + + /** + * \brief Updates the odometry class with latest velocity command + * \param linear Linear velocity [m/s] + * \param angular Angular velocity [rad/s] + * \param time Current time + */ + void updateOpenLoop(double linear, double angular, const ros::Time &time); + + /** + * \brief heading getter + * \return heading [rad] + */ + double getHeading() const + { + return heading_; + } + + /** + * \brief x position getter + * \return x position [m] + */ + double getX() const + { + return x_; + } + + /** + * \brief y position getter + * \return y position [m] + */ + double getY() const + { + return y_; + } + + /** + * \brief linear velocity getter + * \return linear velocity [m/s] + */ + double getLinear() const + { + return linear_; + } + + /** + * \brief angular velocity getter + * \return angular velocity [rad/s] + */ + double getAngular() const + { + return angular_; + } + + /** + * \brief Sets the wheel parameters: radius and separation + * \param wheel_separation Separation between left and right wheels [m] + * \param left_wheel_radius Left wheel radius [m] + * \param right_wheel_radius Right wheel radius [m] + */ + void setWheelParams(double wheel_separation, double left_wheel_radius, double right_wheel_radius); + + /** + * \brief Velocity rolling window size setter + * \param velocity_rolling_window_size Velocity rolling window size + */ + void setVelocityRollingWindowSize(size_t velocity_rolling_window_size); + + private: + + /// Rolling mean accumulator and window: + typedef bacc::accumulator_set > RollingMeanAcc; + typedef bacc::tag::rolling_window RollingWindow; + + /** + * \brief Integrates the velocities (linear and angular) using 2nd order Runge-Kutta + * \param linear Linear velocity [m] (linear displacement, i.e. m/s * dt) computed by encoders + * \param angular Angular velocity [rad] (angular displacement, i.e. m/s * dt) computed by encoders + */ + void integrateRungeKutta2(double linear, double angular); + + /** + * \brief Integrates the velocities (linear and angular) using exact method + * \param linear Linear velocity [m] (linear displacement, i.e. m/s * dt) computed by encoders + * \param angular Angular velocity [rad] (angular displacement, i.e. m/s * dt) computed by encoders + */ + void integrateExact(double linear, double angular); + + /** + * \brief Reset linear and angular accumulators + */ + void resetAccumulators(); + + /// Current timestamp: + ros::Time timestamp_; + + /// Current pose: + double x_; // [m] + double y_; // [m] + double heading_; // [rad] + + /// Current velocity: + double linear_; // [m/s] + double angular_; // [rad/s] + + /// Wheel kinematic parameters [m]: + double wheel_separation_; + double left_wheel_radius_; + double right_wheel_radius_; + + /// Previou wheel position/state [rad]: + double left_wheel_old_pos_; + double right_wheel_old_pos_; + + /// Rolling mean accumulators for the linar and angular velocities: + size_t velocity_rolling_window_size_; + RollingMeanAcc linear_acc_; + RollingMeanAcc angular_acc_; + + /// Integration funcion, used to integrate the odometry: + IntegrationFunction integrate_fun_; + }; +} diff --git a/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/include/diff_drive_controller/speed_limiter.h b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/include/diff_drive_controller/speed_limiter.h new file mode 100755 index 0000000..32f6c4b --- /dev/null +++ b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/include/diff_drive_controller/speed_limiter.h @@ -0,0 +1,129 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, PAL Robotics, S.L. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the PAL Robotics nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* + * Author: Enrique Fernández + */ + +#pragma once + + +namespace diff_drive_controller +{ + + class SpeedLimiter + { + public: + + /** + * \brief Constructor + * \param [in] has_velocity_limits if true, applies velocity limits + * \param [in] has_acceleration_limits if true, applies acceleration limits + * \param [in] has_jerk_limits if true, applies jerk limits + * \param [in] min_velocity Minimum velocity [m/s], usually <= 0 + * \param [in] max_velocity Maximum velocity [m/s], usually >= 0 + * \param [in] min_acceleration Minimum acceleration [m/s^2], usually <= 0 + * \param [in] max_acceleration Maximum acceleration [m/s^2], usually >= 0 + * \param [in] min_jerk Minimum jerk [m/s^3], usually <= 0 + * \param [in] max_jerk Maximum jerk [m/s^3], usually >= 0 + */ + SpeedLimiter( + bool has_velocity_limits = false, + bool has_acceleration_limits = false, + bool has_jerk_limits = false, + double min_velocity = 0.0, + double max_velocity = 0.0, + double min_acceleration = 0.0, + double max_acceleration = 0.0, + double min_jerk = 0.0, + double max_jerk = 0.0 + ); + + /** + * \brief Limit the velocity and acceleration + * \param [in, out] v Velocity [m/s] + * \param [in] v0 Previous velocity to v [m/s] + * \param [in] v1 Previous velocity to v0 [m/s] + * \param [in] dt Time step [s] + * \return Limiting factor (1.0 if none) + */ + double limit(double& v, double v0, double v1, double dt); + + /** + * \brief Limit the velocity + * \param [in, out] v Velocity [m/s] + * \return Limiting factor (1.0 if none) + */ + double limit_velocity(double& v); + + /** + * \brief Limit the acceleration + * \param [in, out] v Velocity [m/s] + * \param [in] v0 Previous velocity [m/s] + * \param [in] dt Time step [s] + * \return Limiting factor (1.0 if none) + */ + double limit_acceleration(double& v, double v0, double dt); + + /** + * \brief Limit the jerk + * \param [in, out] v Velocity [m/s] + * \param [in] v0 Previous velocity to v [m/s] + * \param [in] v1 Previous velocity to v0 [m/s] + * \param [in] dt Time step [s] + * \return Limiting factor (1.0 if none) + * \see http://en.wikipedia.org/wiki/Jerk_%28physics%29#Motion_control + */ + double limit_jerk(double& v, double v0, double v1, double dt); + + public: + // Enable/Disable velocity/acceleration/jerk limits: + bool has_velocity_limits; + bool has_acceleration_limits; + bool has_jerk_limits; + + // Velocity limits: + double min_velocity; + double max_velocity; + + // Acceleration limits: + double min_acceleration; + double max_acceleration; + + // Jerk limits: + double min_jerk; + double max_jerk; + }; + +} // namespace diff_drive_controller diff --git a/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/package.xml b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/package.xml new file mode 100755 index 0000000..1dc2794 --- /dev/null +++ b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/package.xml @@ -0,0 +1,51 @@ + + + + diff_drive_controller + 0.21.2 + Controller for a differential drive mobile base. + + Bence Magyar + Enrique Fernandez + + BSD + + https://github.com/ros-controls/ros_controllers/wiki + https://github.com/ros-controls/ros_controllers/issues + https://github.com/ros-controls/ros_controllers + + Bence Magyar + + catkin + + controller_interface + control_msgs + dynamic_reconfigure + geometry_msgs + hardware_interface + nav_msgs + realtime_tools + tf + + boost + pluginlib + urdf + + boost + + pluginlib + urdf + + controller_manager + rosgraph_msgs + rostest + rostopic + std_srvs + xacro + + + + + diff --git a/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/src/diff_drive_controller.cpp b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/src/diff_drive_controller.cpp new file mode 100755 index 0000000..68c1348 --- /dev/null +++ b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/src/diff_drive_controller.cpp @@ -0,0 +1,840 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, PAL Robotics, S.L. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the PAL Robotics nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* + * Author: Bence Magyar, Enrique Fernández + */ + +#include +#include +#include +#include +#include +#include + +static double euclideanOfVectors(const urdf::Vector3& vec1, const urdf::Vector3& vec2) +{ + return std::sqrt(std::pow(vec1.x-vec2.x,2) + + std::pow(vec1.y-vec2.y,2) + + std::pow(vec1.z-vec2.z,2)); +} + +/* +* \brief Check that a link exists and has a geometry collision. +* \param link The link +* \return true if the link has a collision element with geometry +*/ +static bool hasCollisionGeometry(const urdf::LinkConstSharedPtr& link) +{ + if (!link) + { + ROS_ERROR("Link pointer is null."); + return false; + } + + if (!link->collision) + { + ROS_ERROR_STREAM("Link " << link->name << " does not have collision description. Add collision description for link to urdf."); + return false; + } + + if (!link->collision->geometry) + { + ROS_ERROR_STREAM("Link " << link->name << " does not have collision geometry description. Add collision geometry description for link to urdf."); + return false; + } + return true; +} + +/* + * \brief Check if the link is modeled as a cylinder + * \param link Link + * \return true if the link is modeled as a Cylinder; false otherwise + */ +static bool isCylinder(const urdf::LinkConstSharedPtr& link) +{ + if (!hasCollisionGeometry(link)) + { + return false; + } + + if (link->collision->geometry->type != urdf::Geometry::CYLINDER) + { + ROS_DEBUG_STREAM("Link " << link->name << " does not have cylinder geometry"); + return false; + } + + return true; +} + +/* + * \brief Check if the link is modeled as a sphere + * \param link Link + * \return true if the link is modeled as a Sphere; false otherwise + */ +static bool isSphere(const urdf::LinkConstSharedPtr& link) +{ + if (!hasCollisionGeometry(link)) + { + return false; + } + + if (link->collision->geometry->type != urdf::Geometry::SPHERE) + { + ROS_DEBUG_STREAM("Link " << link->name << " does not have sphere geometry"); + return false; + } + + return true; +} + +/* + * \brief Get the wheel radius + * \param [in] wheel_link Wheel link + * \param [out] wheel_radius Wheel radius [m] + * \return true if the wheel radius was found; false otherwise + */ +static bool getWheelRadius(const urdf::LinkConstSharedPtr& wheel_link, double& wheel_radius) +{ + if (isCylinder(wheel_link)) + { + wheel_radius = (static_cast(wheel_link->collision->geometry.get()))->radius; + return true; + } + else if (isSphere(wheel_link)) + { + wheel_radius = (static_cast(wheel_link->collision->geometry.get()))->radius; + return true; + } + + ROS_ERROR_STREAM("Wheel link " << wheel_link->name << " is NOT modeled as a cylinder or sphere!"); + return false; +} + +namespace diff_drive_controller{ + + DiffDriveController::DiffDriveController() + : open_loop_(false) + , command_struct_() + , wheel_separation_(0.0) + , wheel_radius_(0.0) + , wheel_separation_multiplier_(1.0) + , left_wheel_radius_multiplier_(1.0) + , right_wheel_radius_multiplier_(1.0) + , cmd_vel_timeout_(0.5) + , allow_multiple_cmd_vel_publishers_(true) + , base_frame_id_("base_link") + , odom_frame_id_("odom") + , enable_odom_tf_(true) + , wheel_joints_size_(0) + , publish_cmd_(false) + , publish_wheel_joint_controller_state_(false) + { + } + + bool DiffDriveController::init(hardware_interface::VelocityJointInterface* hw, + ros::NodeHandle& root_nh, + ros::NodeHandle &controller_nh) + { + const std::string complete_ns = controller_nh.getNamespace(); + std::size_t id = complete_ns.find_last_of("/"); + name_ = complete_ns.substr(id + 1); + + // Get joint names from the parameter server + std::vector left_wheel_names, right_wheel_names; + if (!getWheelNames(controller_nh, "left_wheel", left_wheel_names) || + !getWheelNames(controller_nh, "right_wheel", right_wheel_names)) + { + return false; + } + + if (left_wheel_names.size() != right_wheel_names.size()) + { + ROS_ERROR_STREAM_NAMED(name_, + "#left wheels (" << left_wheel_names.size() << ") != " << + "#right wheels (" << right_wheel_names.size() << ")."); + return false; + } + else + { + wheel_joints_size_ = left_wheel_names.size(); + + left_wheel_joints_.resize(wheel_joints_size_); + right_wheel_joints_.resize(wheel_joints_size_); + } + + // Odometry related: + double publish_rate; + controller_nh.param("publish_rate", publish_rate, 50.0); + ROS_INFO_STREAM_NAMED(name_, "Controller state will be published at " + << publish_rate << "Hz."); + publish_period_ = ros::Duration(1.0 / publish_rate); + + controller_nh.param("open_loop", open_loop_, open_loop_); + + controller_nh.param("wheel_separation_multiplier", wheel_separation_multiplier_, wheel_separation_multiplier_); + ROS_INFO_STREAM_NAMED(name_, "Wheel separation will be multiplied by " + << wheel_separation_multiplier_ << "."); + + if (controller_nh.hasParam("wheel_radius_multiplier")) + { + double wheel_radius_multiplier; + controller_nh.getParam("wheel_radius_multiplier", wheel_radius_multiplier); + + left_wheel_radius_multiplier_ = wheel_radius_multiplier; + right_wheel_radius_multiplier_ = wheel_radius_multiplier; + } + else + { + controller_nh.param("left_wheel_radius_multiplier", left_wheel_radius_multiplier_, left_wheel_radius_multiplier_); + controller_nh.param("right_wheel_radius_multiplier", right_wheel_radius_multiplier_, right_wheel_radius_multiplier_); + } + + ROS_INFO_STREAM_NAMED(name_, "Left wheel radius will be multiplied by " + << left_wheel_radius_multiplier_ << "."); + ROS_INFO_STREAM_NAMED(name_, "Right wheel radius will be multiplied by " + << right_wheel_radius_multiplier_ << "."); + + int velocity_rolling_window_size = 10; + controller_nh.param("velocity_rolling_window_size", velocity_rolling_window_size, velocity_rolling_window_size); + ROS_INFO_STREAM_NAMED(name_, "Velocity rolling window size of " + << velocity_rolling_window_size << "."); + + odometry_.setVelocityRollingWindowSize(velocity_rolling_window_size); + + // Twist command related: + controller_nh.param("cmd_vel_timeout", cmd_vel_timeout_, cmd_vel_timeout_); + ROS_INFO_STREAM_NAMED(name_, "Velocity commands will be considered old if they are older than " + << cmd_vel_timeout_ << "s."); + + controller_nh.param("allow_multiple_cmd_vel_publishers", allow_multiple_cmd_vel_publishers_, allow_multiple_cmd_vel_publishers_); + ROS_INFO_STREAM_NAMED(name_, "Allow mutiple cmd_vel publishers is " + << (allow_multiple_cmd_vel_publishers_?"enabled":"disabled")); + + controller_nh.param("base_frame_id", base_frame_id_, base_frame_id_); + ROS_INFO_STREAM_NAMED(name_, "Base frame_id set to " << base_frame_id_); + + controller_nh.param("odom_frame_id", odom_frame_id_, odom_frame_id_); + ROS_INFO_STREAM_NAMED(name_, "Odometry frame_id set to " << odom_frame_id_); + + controller_nh.param("enable_odom_tf", enable_odom_tf_, enable_odom_tf_); + ROS_INFO_STREAM_NAMED(name_, "Publishing to tf is " << (enable_odom_tf_?"enabled":"disabled")); + + // Velocity and acceleration limits: + controller_nh.param("linear/x/has_velocity_limits" , limiter_lin_.has_velocity_limits , limiter_lin_.has_velocity_limits ); + controller_nh.param("linear/x/has_acceleration_limits", limiter_lin_.has_acceleration_limits, limiter_lin_.has_acceleration_limits); + controller_nh.param("linear/x/has_jerk_limits" , limiter_lin_.has_jerk_limits , limiter_lin_.has_jerk_limits ); + controller_nh.param("linear/x/max_velocity" , limiter_lin_.max_velocity , limiter_lin_.max_velocity ); + controller_nh.param("linear/x/min_velocity" , limiter_lin_.min_velocity , -limiter_lin_.max_velocity ); + controller_nh.param("linear/x/max_acceleration" , limiter_lin_.max_acceleration , limiter_lin_.max_acceleration ); + controller_nh.param("linear/x/min_acceleration" , limiter_lin_.min_acceleration , -limiter_lin_.max_acceleration ); + controller_nh.param("linear/x/max_jerk" , limiter_lin_.max_jerk , limiter_lin_.max_jerk ); + controller_nh.param("linear/x/min_jerk" , limiter_lin_.min_jerk , -limiter_lin_.max_jerk ); + + controller_nh.param("angular/z/has_velocity_limits" , limiter_ang_.has_velocity_limits , limiter_ang_.has_velocity_limits ); + controller_nh.param("angular/z/has_acceleration_limits", limiter_ang_.has_acceleration_limits, limiter_ang_.has_acceleration_limits); + controller_nh.param("angular/z/has_jerk_limits" , limiter_ang_.has_jerk_limits , limiter_ang_.has_jerk_limits ); + controller_nh.param("angular/z/max_velocity" , limiter_ang_.max_velocity , limiter_ang_.max_velocity ); + controller_nh.param("angular/z/min_velocity" , limiter_ang_.min_velocity , -limiter_ang_.max_velocity ); + controller_nh.param("angular/z/max_acceleration" , limiter_ang_.max_acceleration , limiter_ang_.max_acceleration ); + controller_nh.param("angular/z/min_acceleration" , limiter_ang_.min_acceleration , -limiter_ang_.max_acceleration ); + controller_nh.param("angular/z/max_jerk" , limiter_ang_.max_jerk , limiter_ang_.max_jerk ); + controller_nh.param("angular/z/min_jerk" , limiter_ang_.min_jerk , -limiter_ang_.max_jerk ); + + // Publish limited velocity: + controller_nh.param("publish_cmd", publish_cmd_, publish_cmd_); + + // Publish wheel data: + controller_nh.param("publish_wheel_joint_controller_state", publish_wheel_joint_controller_state_, publish_wheel_joint_controller_state_); + + // If either parameter is not available, we need to look up the value in the URDF + bool lookup_wheel_separation = !controller_nh.getParam("wheel_separation", wheel_separation_); + bool lookup_wheel_radius = !controller_nh.getParam("wheel_radius", wheel_radius_); + + if (!setOdomParamsFromUrdf(root_nh, + left_wheel_names[0], + right_wheel_names[0], + lookup_wheel_separation, + lookup_wheel_radius)) + { + return false; + } + + // Regardless of how we got the separation and radius, use them + // to set the odometry parameters + const double ws = wheel_separation_multiplier_ * wheel_separation_; + const double lwr = left_wheel_radius_multiplier_ * wheel_radius_; + const double rwr = right_wheel_radius_multiplier_ * wheel_radius_; + odometry_.setWheelParams(ws, lwr, rwr); + ROS_INFO_STREAM_NAMED(name_, + "Odometry params : wheel separation " << ws + << ", left wheel radius " << lwr + << ", right wheel radius " << rwr); + + if (publish_cmd_) + { + cmd_vel_pub_.reset(new realtime_tools::RealtimePublisher(controller_nh, "cmd_vel_out", 100)); + } + + // Wheel joint controller state: + if (publish_wheel_joint_controller_state_) + { + controller_state_pub_.reset(new realtime_tools::RealtimePublisher(controller_nh, "wheel_joint_controller_state", 100)); + + const size_t num_wheels = wheel_joints_size_ * 2; + + controller_state_pub_->msg_.joint_names.resize(num_wheels); + + controller_state_pub_->msg_.desired.positions.resize(num_wheels); + controller_state_pub_->msg_.desired.velocities.resize(num_wheels); + controller_state_pub_->msg_.desired.accelerations.resize(num_wheels); + controller_state_pub_->msg_.desired.effort.resize(num_wheels); + + controller_state_pub_->msg_.actual.positions.resize(num_wheels); + controller_state_pub_->msg_.actual.velocities.resize(num_wheels); + controller_state_pub_->msg_.actual.accelerations.resize(num_wheels); + controller_state_pub_->msg_.actual.effort.resize(num_wheels); + + controller_state_pub_->msg_.error.positions.resize(num_wheels); + controller_state_pub_->msg_.error.velocities.resize(num_wheels); + controller_state_pub_->msg_.error.accelerations.resize(num_wheels); + controller_state_pub_->msg_.error.effort.resize(num_wheels); + + for (size_t i = 0; i < wheel_joints_size_; ++i) + { + controller_state_pub_->msg_.joint_names[i] = left_wheel_names[i]; + controller_state_pub_->msg_.joint_names[i + wheel_joints_size_] = right_wheel_names[i]; + } + + vel_left_previous_.resize(wheel_joints_size_, 0.0); + vel_right_previous_.resize(wheel_joints_size_, 0.0); + } + + setOdomPubFields(root_nh, controller_nh); + + // Get the joint object to use in the realtime loop + for (size_t i = 0; i < wheel_joints_size_; ++i) + { + ROS_INFO_STREAM_NAMED(name_, + "Adding left wheel with joint name: " << left_wheel_names[i] + << " and right wheel with joint name: " << right_wheel_names[i]); + left_wheel_joints_[i] = hw->getHandle(left_wheel_names[i]); // throws on failure + right_wheel_joints_[i] = hw->getHandle(right_wheel_names[i]); // throws on failure + } + + sub_command_ = controller_nh.subscribe("cmd_vel", 1, &DiffDriveController::cmdVelCallback, this); + + // Initialize dynamic parameters + DynamicParams dynamic_params; + dynamic_params.left_wheel_radius_multiplier = left_wheel_radius_multiplier_; + dynamic_params.right_wheel_radius_multiplier = right_wheel_radius_multiplier_; + dynamic_params.wheel_separation_multiplier = wheel_separation_multiplier_; + + dynamic_params.publish_rate = publish_rate; + dynamic_params.enable_odom_tf = enable_odom_tf_; + + dynamic_params_.writeFromNonRT(dynamic_params); + + // Initialize dynamic_reconfigure server + DiffDriveControllerConfig config; + config.left_wheel_radius_multiplier = left_wheel_radius_multiplier_; + config.right_wheel_radius_multiplier = right_wheel_radius_multiplier_; + config.wheel_separation_multiplier = wheel_separation_multiplier_; + + config.publish_rate = publish_rate; + config.enable_odom_tf = enable_odom_tf_; + + dyn_reconf_server_ = std::make_shared(dyn_reconf_server_mutex_, controller_nh); + + // Update parameters + dyn_reconf_server_mutex_.lock(); + dyn_reconf_server_->updateConfig(config); + dyn_reconf_server_mutex_.unlock(); + + dyn_reconf_server_->setCallback( + std::bind(&DiffDriveController::reconfCallback, this, std::placeholders::_1, std::placeholders::_2)); + return true; + } + + void DiffDriveController::update(const ros::Time& time, const ros::Duration& period) + { + // update parameter from dynamic reconf + updateDynamicParams(); + + // Apply (possibly new) multipliers: + const double ws = wheel_separation_multiplier_ * wheel_separation_; + const double lwr = left_wheel_radius_multiplier_ * wheel_radius_; + const double rwr = right_wheel_radius_multiplier_ * wheel_radius_; + + odometry_.setWheelParams(ws, lwr, rwr); + + // COMPUTE AND PUBLISH ODOMETRY + if (open_loop_) + { + odometry_.updateOpenLoop(last0_cmd_.lin, last0_cmd_.ang, time); + } + else + { + double left_pos = 0.0; + double right_pos = 0.0; + for (size_t i = 0; i < wheel_joints_size_; ++i) + { + const double lp = left_wheel_joints_[i].getPosition(); + const double rp = right_wheel_joints_[i].getPosition(); + if (std::isnan(lp) || std::isnan(rp)) + return; + + left_pos += lp; + right_pos += rp; + } + left_pos /= wheel_joints_size_; + right_pos /= wheel_joints_size_; + + // Estimate linear and angular velocity using joint information + odometry_.update(left_pos, right_pos, time); + } + + // Publish odometry message + if (last_state_publish_time_ + publish_period_ < time) + { + last_state_publish_time_ += publish_period_; + // Compute and store orientation info + const geometry_msgs::Quaternion orientation( + tf::createQuaternionMsgFromYaw(odometry_.getHeading())); + + // Populate odom message and publish + if (odom_pub_->trylock()) + { + odom_pub_->msg_.header.stamp = time; + odom_pub_->msg_.pose.pose.position.x = odometry_.getX(); + odom_pub_->msg_.pose.pose.position.y = odometry_.getY(); + odom_pub_->msg_.pose.pose.orientation = orientation; + odom_pub_->msg_.twist.twist.linear.x = odometry_.getLinear(); + odom_pub_->msg_.twist.twist.angular.z = odometry_.getAngular(); + odom_pub_->unlockAndPublish(); + } + + // Publish tf /odom frame + if (enable_odom_tf_ && tf_odom_pub_->trylock()) + { + geometry_msgs::TransformStamped& odom_frame = tf_odom_pub_->msg_.transforms[0]; + odom_frame.header.stamp = time; + odom_frame.transform.translation.x = odometry_.getX(); + odom_frame.transform.translation.y = odometry_.getY(); + odom_frame.transform.rotation = orientation; + tf_odom_pub_->unlockAndPublish(); + } + } + + // MOVE ROBOT + // Retreive current velocity command and time step: + Commands curr_cmd = *(command_.readFromRT()); + const double dt = (time - curr_cmd.stamp).toSec(); + + // Brake if cmd_vel has timeout: + if (dt > cmd_vel_timeout_) + { + curr_cmd.lin = 0.0; + curr_cmd.ang = 0.0; + } + + // Limit velocities and accelerations: + const double cmd_dt(period.toSec()); + + limiter_lin_.limit(curr_cmd.lin, last0_cmd_.lin, last1_cmd_.lin, cmd_dt); + limiter_ang_.limit(curr_cmd.ang, last0_cmd_.ang, last1_cmd_.ang, cmd_dt); + + last1_cmd_ = last0_cmd_; + last0_cmd_ = curr_cmd; + + // Publish limited velocity: + if (publish_cmd_ && cmd_vel_pub_ && cmd_vel_pub_->trylock()) + { + cmd_vel_pub_->msg_.header.stamp = time; + cmd_vel_pub_->msg_.twist.linear.x = curr_cmd.lin; + cmd_vel_pub_->msg_.twist.angular.z = curr_cmd.ang; + cmd_vel_pub_->unlockAndPublish(); + } + + // Compute wheels velocities: + const double vel_left = (curr_cmd.lin - curr_cmd.ang * ws / 2.0)/lwr; + const double vel_right = (curr_cmd.lin + curr_cmd.ang * ws / 2.0)/rwr; + + // Set wheels velocities: + for (size_t i = 0; i < wheel_joints_size_; ++i) + { + left_wheel_joints_[i].setCommand(vel_left); + right_wheel_joints_[i].setCommand(vel_right); + } + + publishWheelData(time, period, curr_cmd, ws, lwr, rwr); + time_previous_ = time; + } + + void DiffDriveController::starting(const ros::Time& time) + { + brake(); + + // Register starting time used to keep fixed rate + last_state_publish_time_ = time; + time_previous_ = time; + + odometry_.init(time); + } + + void DiffDriveController::stopping(const ros::Time& /*time*/) + { + brake(); + } + + void DiffDriveController::brake() + { + const double vel = 0.0; + for (size_t i = 0; i < wheel_joints_size_; ++i) + { + left_wheel_joints_[i].setCommand(vel); + right_wheel_joints_[i].setCommand(vel); + } + } + + void DiffDriveController::cmdVelCallback(const geometry_msgs::Twist& command) + { + if (isRunning()) + { + // check that we don't have multiple publishers on the command topic + if (!allow_multiple_cmd_vel_publishers_ && sub_command_.getNumPublishers() > 1) + { + ROS_ERROR_STREAM_THROTTLE_NAMED(1.0, name_, "Detected " << sub_command_.getNumPublishers() + << " publishers. Only 1 publisher is allowed. Going to brake."); + brake(); + return; + } + + if(!std::isfinite(command.angular.z) || !std::isfinite(command.linear.x)) + { + ROS_WARN_THROTTLE(1.0, "Received NaN in velocity command. Ignoring."); + return; + } + + command_struct_.ang = command.angular.z; + command_struct_.lin = command.linear.x; + command_struct_.stamp = ros::Time::now(); + command_.writeFromNonRT (command_struct_); + ROS_DEBUG_STREAM_NAMED(name_, + "Added values to command. " + << "Ang: " << command_struct_.ang << ", " + << "Lin: " << command_struct_.lin << ", " + << "Stamp: " << command_struct_.stamp); + } + else + { + ROS_ERROR_NAMED(name_, "Can't accept new commands. Controller is not running."); + } + } + + bool DiffDriveController::getWheelNames(ros::NodeHandle& controller_nh, + const std::string& wheel_param, + std::vector& wheel_names) + { + XmlRpc::XmlRpcValue wheel_list; + if (!controller_nh.getParam(wheel_param, wheel_list)) + { + ROS_ERROR_STREAM_NAMED(name_, + "Couldn't retrieve wheel param '" << wheel_param << "'."); + return false; + } + + if (wheel_list.getType() == XmlRpc::XmlRpcValue::TypeArray) + { + if (wheel_list.size() == 0) + { + ROS_ERROR_STREAM_NAMED(name_, + "Wheel param '" << wheel_param << "' is an empty list"); + return false; + } + + for (int i = 0; i < wheel_list.size(); ++i) + { + if (wheel_list[i].getType() != XmlRpc::XmlRpcValue::TypeString) + { + ROS_ERROR_STREAM_NAMED(name_, + "Wheel param '" << wheel_param << "' #" << i << + " isn't a string."); + return false; + } + } + + wheel_names.resize(wheel_list.size()); + for (int i = 0; i < wheel_list.size(); ++i) + { + wheel_names[i] = static_cast(wheel_list[i]); + } + } + else if (wheel_list.getType() == XmlRpc::XmlRpcValue::TypeString) + { + wheel_names.push_back(wheel_list); + } + else + { + ROS_ERROR_STREAM_NAMED(name_, + "Wheel param '" << wheel_param << + "' is neither a list of strings nor a string."); + return false; + } + + return true; + } + + bool DiffDriveController::setOdomParamsFromUrdf(ros::NodeHandle& root_nh, + const std::string& left_wheel_name, + const std::string& right_wheel_name, + bool lookup_wheel_separation, + bool lookup_wheel_radius) + { + if (!(lookup_wheel_separation || lookup_wheel_radius)) + { + // Short-circuit in case we don't need to look up anything, so we don't have to parse the URDF + return true; + } + + // Parse robot description + const std::string model_param_name = "robot_description"; + bool res = root_nh.hasParam(model_param_name); + std::string robot_model_str=""; + if (!res || !root_nh.getParam(model_param_name,robot_model_str)) + { + ROS_ERROR_NAMED(name_, "Robot description couldn't be retrieved from param server."); + return false; + } + + urdf::ModelInterfaceSharedPtr model(urdf::parseURDF(robot_model_str)); + + urdf::JointConstSharedPtr left_wheel_joint(model->getJoint(left_wheel_name)); + urdf::JointConstSharedPtr right_wheel_joint(model->getJoint(right_wheel_name)); + + if (!left_wheel_joint) + { + ROS_ERROR_STREAM_NAMED(name_, left_wheel_name + << " couldn't be retrieved from model description"); + return false; + } + + if (!right_wheel_joint) + { + ROS_ERROR_STREAM_NAMED(name_, right_wheel_name + << " couldn't be retrieved from model description"); + return false; + } + + if (lookup_wheel_separation) + { + // Get wheel separation + ROS_INFO_STREAM("left wheel to origin: " << left_wheel_joint->parent_to_joint_origin_transform.position.x << "," + << left_wheel_joint->parent_to_joint_origin_transform.position.y << ", " + << left_wheel_joint->parent_to_joint_origin_transform.position.z); + ROS_INFO_STREAM("right wheel to origin: " << right_wheel_joint->parent_to_joint_origin_transform.position.x << "," + << right_wheel_joint->parent_to_joint_origin_transform.position.y << ", " + << right_wheel_joint->parent_to_joint_origin_transform.position.z); + + wheel_separation_ = euclideanOfVectors(left_wheel_joint->parent_to_joint_origin_transform.position, + right_wheel_joint->parent_to_joint_origin_transform.position); + + ROS_INFO_STREAM("wheel_separation : " << wheel_separation_); + + } + + if (lookup_wheel_radius) + { + // Get wheel radius + if (!getWheelRadius(model->getLink(left_wheel_joint->child_link_name), wheel_radius_)) + { + ROS_ERROR_STREAM_NAMED(name_, "Couldn't retrieve " << left_wheel_name << " wheel radius"); + return false; + } + else ROS_INFO("wheel radius : %f", wheel_radius_); + } + + return true; + } + + void DiffDriveController::setOdomPubFields(ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh) + { + // Get and check params for covariances + XmlRpc::XmlRpcValue pose_cov_list; + controller_nh.getParam("pose_covariance_diagonal", pose_cov_list); + ROS_ASSERT(pose_cov_list.getType() == XmlRpc::XmlRpcValue::TypeArray); + ROS_ASSERT(pose_cov_list.size() == 6); + for (int i = 0; i < pose_cov_list.size(); ++i) + ROS_ASSERT(pose_cov_list[i].getType() == XmlRpc::XmlRpcValue::TypeDouble); + + XmlRpc::XmlRpcValue twist_cov_list; + controller_nh.getParam("twist_covariance_diagonal", twist_cov_list); + ROS_ASSERT(twist_cov_list.getType() == XmlRpc::XmlRpcValue::TypeArray); + ROS_ASSERT(twist_cov_list.size() == 6); + for (int i = 0; i < twist_cov_list.size(); ++i) + ROS_ASSERT(twist_cov_list[i].getType() == XmlRpc::XmlRpcValue::TypeDouble); + + // Setup odometry realtime publisher + odom message constant fields + odom_pub_.reset(new realtime_tools::RealtimePublisher(controller_nh, "odom", 100)); + odom_pub_->msg_.header.frame_id = odom_frame_id_; + odom_pub_->msg_.child_frame_id = base_frame_id_; + odom_pub_->msg_.pose.pose.position.z = 0; + odom_pub_->msg_.pose.covariance = { + static_cast(pose_cov_list[0]), 0., 0., 0., 0., 0., + 0., static_cast(pose_cov_list[1]), 0., 0., 0., 0., + 0., 0., static_cast(pose_cov_list[2]), 0., 0., 0., + 0., 0., 0., static_cast(pose_cov_list[3]), 0., 0., + 0., 0., 0., 0., static_cast(pose_cov_list[4]), 0., + 0., 0., 0., 0., 0., static_cast(pose_cov_list[5]) }; + odom_pub_->msg_.twist.twist.linear.y = 0; + odom_pub_->msg_.twist.twist.linear.z = 0; + odom_pub_->msg_.twist.twist.angular.x = 0; + odom_pub_->msg_.twist.twist.angular.y = 0; + odom_pub_->msg_.twist.covariance = { + static_cast(twist_cov_list[0]), 0., 0., 0., 0., 0., + 0., static_cast(twist_cov_list[1]), 0., 0., 0., 0., + 0., 0., static_cast(twist_cov_list[2]), 0., 0., 0., + 0., 0., 0., static_cast(twist_cov_list[3]), 0., 0., + 0., 0., 0., 0., static_cast(twist_cov_list[4]), 0., + 0., 0., 0., 0., 0., static_cast(twist_cov_list[5]) }; + tf_odom_pub_.reset(new realtime_tools::RealtimePublisher(root_nh, "/tf", 100)); + tf_odom_pub_->msg_.transforms.resize(1); + tf_odom_pub_->msg_.transforms[0].transform.translation.z = 0.0; + tf_odom_pub_->msg_.transforms[0].child_frame_id = base_frame_id_; + tf_odom_pub_->msg_.transforms[0].header.frame_id = odom_frame_id_; + } + + void DiffDriveController::reconfCallback(DiffDriveControllerConfig& config, uint32_t /*level*/) + { + DynamicParams dynamic_params; + dynamic_params.left_wheel_radius_multiplier = config.left_wheel_radius_multiplier; + dynamic_params.right_wheel_radius_multiplier = config.right_wheel_radius_multiplier; + dynamic_params.wheel_separation_multiplier = config.wheel_separation_multiplier; + + dynamic_params.publish_rate = config.publish_rate; + + dynamic_params.enable_odom_tf = config.enable_odom_tf; + + dynamic_params_.writeFromNonRT(dynamic_params); + + ROS_INFO_STREAM_NAMED(name_, "Dynamic Reconfigure:\n" << dynamic_params); + } + + void DiffDriveController::updateDynamicParams() + { + // Retreive dynamic params: + const DynamicParams dynamic_params = *(dynamic_params_.readFromRT()); + + left_wheel_radius_multiplier_ = dynamic_params.left_wheel_radius_multiplier; + right_wheel_radius_multiplier_ = dynamic_params.right_wheel_radius_multiplier; + wheel_separation_multiplier_ = dynamic_params.wheel_separation_multiplier; + + publish_period_ = ros::Duration(1.0 / dynamic_params.publish_rate); + enable_odom_tf_ = dynamic_params.enable_odom_tf; + } + + void DiffDriveController::publishWheelData(const ros::Time& time, const ros::Duration& period, Commands& curr_cmd, + double wheel_separation, double left_wheel_radius, double right_wheel_radius) + { + if (publish_wheel_joint_controller_state_ && controller_state_pub_->trylock()) + { + const double cmd_dt(period.toSec()); + + // Compute desired wheels velocities, that is before applying limits: + const double vel_left_desired = (curr_cmd.lin - curr_cmd.ang * wheel_separation / 2.0) / left_wheel_radius; + const double vel_right_desired = (curr_cmd.lin + curr_cmd.ang * wheel_separation / 2.0) / right_wheel_radius; + controller_state_pub_->msg_.header.stamp = time; + + for (size_t i = 0; i < wheel_joints_size_; ++i) + { + const double control_duration = (time - time_previous_).toSec(); + + const double left_wheel_acc = (left_wheel_joints_[i].getVelocity() - vel_left_previous_[i]) / control_duration; + const double right_wheel_acc = (right_wheel_joints_[i].getVelocity() - vel_right_previous_[i]) / control_duration; + + // Actual + controller_state_pub_->msg_.actual.positions[i] = left_wheel_joints_[i].getPosition(); + controller_state_pub_->msg_.actual.velocities[i] = left_wheel_joints_[i].getVelocity(); + controller_state_pub_->msg_.actual.accelerations[i] = left_wheel_acc; + controller_state_pub_->msg_.actual.effort[i] = left_wheel_joints_[i].getEffort(); + + controller_state_pub_->msg_.actual.positions[i + wheel_joints_size_] = right_wheel_joints_[i].getPosition(); + controller_state_pub_->msg_.actual.velocities[i + wheel_joints_size_] = right_wheel_joints_[i].getVelocity(); + controller_state_pub_->msg_.actual.accelerations[i + wheel_joints_size_] = right_wheel_acc; + controller_state_pub_->msg_.actual.effort[i+ wheel_joints_size_] = right_wheel_joints_[i].getEffort(); + + // Desired + controller_state_pub_->msg_.desired.positions[i] += vel_left_desired * cmd_dt; + controller_state_pub_->msg_.desired.velocities[i] = vel_left_desired; + controller_state_pub_->msg_.desired.accelerations[i] = (vel_left_desired - vel_left_desired_previous_) * cmd_dt; + controller_state_pub_->msg_.desired.effort[i] = std::numeric_limits::quiet_NaN(); + + controller_state_pub_->msg_.desired.positions[i + wheel_joints_size_] += vel_right_desired * cmd_dt; + controller_state_pub_->msg_.desired.velocities[i + wheel_joints_size_] = vel_right_desired; + controller_state_pub_->msg_.desired.accelerations[i + wheel_joints_size_] = (vel_right_desired - vel_right_desired_previous_) * cmd_dt; + controller_state_pub_->msg_.desired.effort[i+ wheel_joints_size_] = std::numeric_limits::quiet_NaN(); + + // Error + controller_state_pub_->msg_.error.positions[i] = controller_state_pub_->msg_.desired.positions[i] - + controller_state_pub_->msg_.actual.positions[i]; + controller_state_pub_->msg_.error.velocities[i] = controller_state_pub_->msg_.desired.velocities[i] - + controller_state_pub_->msg_.actual.velocities[i]; + controller_state_pub_->msg_.error.accelerations[i] = controller_state_pub_->msg_.desired.accelerations[i] - + controller_state_pub_->msg_.actual.accelerations[i]; + controller_state_pub_->msg_.error.effort[i] = controller_state_pub_->msg_.desired.effort[i] - + controller_state_pub_->msg_.actual.effort[i]; + + controller_state_pub_->msg_.error.positions[i + wheel_joints_size_] = controller_state_pub_->msg_.desired.positions[i + wheel_joints_size_] - + controller_state_pub_->msg_.actual.positions[i + wheel_joints_size_]; + controller_state_pub_->msg_.error.velocities[i + wheel_joints_size_] = controller_state_pub_->msg_.desired.velocities[i + wheel_joints_size_] - + controller_state_pub_->msg_.actual.velocities[i + wheel_joints_size_]; + controller_state_pub_->msg_.error.accelerations[i + wheel_joints_size_] = controller_state_pub_->msg_.desired.accelerations[i + wheel_joints_size_] - + controller_state_pub_->msg_.actual.accelerations[i + wheel_joints_size_]; + controller_state_pub_->msg_.error.effort[i+ wheel_joints_size_] = controller_state_pub_->msg_.desired.effort[i + wheel_joints_size_] - + controller_state_pub_->msg_.actual.effort[i + wheel_joints_size_]; + + // Save previous velocities to compute acceleration + vel_left_previous_[i] = left_wheel_joints_[i].getVelocity(); + vel_right_previous_[i] = right_wheel_joints_[i].getVelocity(); + vel_left_desired_previous_ = vel_left_desired; + vel_right_desired_previous_ = vel_right_desired; + } + + controller_state_pub_->unlockAndPublish(); + } + } + +} // namespace diff_drive_controller + +PLUGINLIB_EXPORT_CLASS(diff_drive_controller::DiffDriveController, controller_interface::ControllerBase); diff --git a/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/src/odometry.cpp b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/src/odometry.cpp new file mode 100755 index 0000000..fafbb74 --- /dev/null +++ b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/src/odometry.cpp @@ -0,0 +1,174 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, PAL Robotics, S.L. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the PAL Robotics nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* + * Author: Luca Marchionni + * Author: Bence Magyar + * Author: Enrique Fernández + * Author: Paul Mathieu + */ + +#include + +namespace diff_drive_controller +{ + namespace bacc = boost::accumulators; + + Odometry::Odometry(size_t velocity_rolling_window_size) + : timestamp_(0.0) + , x_(0.0) + , y_(0.0) + , heading_(0.0) + , linear_(0.0) + , angular_(0.0) + , wheel_separation_(0.0) + , left_wheel_radius_(0.0) + , right_wheel_radius_(0.0) + , left_wheel_old_pos_(0.0) + , right_wheel_old_pos_(0.0) + , velocity_rolling_window_size_(velocity_rolling_window_size) + , linear_acc_(RollingWindow::window_size = velocity_rolling_window_size) + , angular_acc_(RollingWindow::window_size = velocity_rolling_window_size) + , integrate_fun_(std::bind(&Odometry::integrateExact, this, std::placeholders::_1, std::placeholders::_2)) + { + } + + void Odometry::init(const ros::Time& time) + { + // Reset accumulators and timestamp: + resetAccumulators(); + timestamp_ = time; + } + + bool Odometry::update(double left_pos, double right_pos, const ros::Time &time) + { + /// Get current wheel joint positions: + const double left_wheel_cur_pos = left_pos * left_wheel_radius_; + const double right_wheel_cur_pos = right_pos * right_wheel_radius_; + + /// Estimate velocity of wheels using old and current position: + const double left_wheel_est_vel = left_wheel_cur_pos - left_wheel_old_pos_; + const double right_wheel_est_vel = right_wheel_cur_pos - right_wheel_old_pos_; + + /// Update old position with current: + left_wheel_old_pos_ = left_wheel_cur_pos; + right_wheel_old_pos_ = right_wheel_cur_pos; + + /// Compute linear and angular diff: + const double linear = (right_wheel_est_vel + left_wheel_est_vel) * 0.5 ; + const double angular = (right_wheel_est_vel - left_wheel_est_vel) / wheel_separation_; + + /// Integrate odometry: + integrate_fun_(linear, angular); + + /// We cannot estimate the speed with very small time intervals: + const double dt = (time - timestamp_).toSec(); + if (dt < 0.0001) + return false; // Interval too small to integrate with + + timestamp_ = time; + + /// Estimate speeds using a rolling mean to filter them out: + linear_acc_(linear/dt); + angular_acc_(angular/dt); + + linear_ = bacc::rolling_mean(linear_acc_); + angular_ = bacc::rolling_mean(angular_acc_); + + return true; + } + + void Odometry::updateOpenLoop(double linear, double angular, const ros::Time &time) + { + /// Save last linear and angular velocity: + linear_ = linear; + angular_ = angular; + + /// Integrate odometry: + const double dt = (time - timestamp_).toSec(); + timestamp_ = time; + integrate_fun_(linear * dt, angular * dt); + } + + void Odometry::setWheelParams(double wheel_separation, double left_wheel_radius, double right_wheel_radius) + { + wheel_separation_ = wheel_separation; + left_wheel_radius_ = left_wheel_radius; + right_wheel_radius_ = right_wheel_radius; + } + + void Odometry::setVelocityRollingWindowSize(size_t velocity_rolling_window_size) + { + velocity_rolling_window_size_ = velocity_rolling_window_size; + + resetAccumulators(); + } + + void Odometry::integrateRungeKutta2(double linear, double angular) + { + const double direction = heading_ + angular * 0.5; + + /// Runge-Kutta 2nd order integration: + x_ += linear * cos(direction); + y_ += linear * sin(direction); + heading_ += angular; + } + + /** + * \brief Other possible integration method provided by the class + * \param linear + * \param angular + */ + void Odometry::integrateExact(double linear, double angular) + { + if (fabs(angular) < 1e-6) + integrateRungeKutta2(linear, angular); + else + { + /// Exact integration (should solve problems when angular is zero): + const double heading_old = heading_; + const double r = linear/angular; + heading_ += angular; + x_ += r * (sin(heading_) - sin(heading_old)); + y_ += -r * (cos(heading_) - cos(heading_old)); + } + } + + void Odometry::resetAccumulators() + { + linear_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_); + angular_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_); + } + +} // namespace diff_drive_controller diff --git a/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/src/speed_limiter.cpp b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/src/speed_limiter.cpp new file mode 100755 index 0000000..630ec82 --- /dev/null +++ b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/src/speed_limiter.cpp @@ -0,0 +1,137 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, PAL Robotics, S.L. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the PAL Robotics nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* + * Author: Enrique Fernández + */ + +#include + +#include + +template +T clamp(T x, T min, T max) +{ + return std::min(std::max(min, x), max); +} + +namespace diff_drive_controller +{ + + SpeedLimiter::SpeedLimiter( + bool has_velocity_limits, + bool has_acceleration_limits, + bool has_jerk_limits, + double min_velocity, + double max_velocity, + double min_acceleration, + double max_acceleration, + double min_jerk, + double max_jerk + ) + : has_velocity_limits(has_velocity_limits) + , has_acceleration_limits(has_acceleration_limits) + , has_jerk_limits(has_jerk_limits) + , min_velocity(min_velocity) + , max_velocity(max_velocity) + , min_acceleration(min_acceleration) + , max_acceleration(max_acceleration) + , min_jerk(min_jerk) + , max_jerk(max_jerk) + { + } + + double SpeedLimiter::limit(double& v, double v0, double v1, double dt) + { + const double tmp = v; + + limit_jerk(v, v0, v1, dt); + limit_acceleration(v, v0, dt); + limit_velocity(v); + + return tmp != 0.0 ? v / tmp : 1.0; + } + + double SpeedLimiter::limit_velocity(double& v) + { + const double tmp = v; + + if (has_velocity_limits) + { + v = clamp(v, min_velocity, max_velocity); + } + + return tmp != 0.0 ? v / tmp : 1.0; + } + + double SpeedLimiter::limit_acceleration(double& v, double v0, double dt) + { + const double tmp = v; + + if (has_acceleration_limits) + { + const double dv_min = min_acceleration * dt; + const double dv_max = max_acceleration * dt; + + const double dv = clamp(v - v0, dv_min, dv_max); + + v = v0 + dv; + } + + return tmp != 0.0 ? v / tmp : 1.0; + } + + double SpeedLimiter::limit_jerk(double& v, double v0, double v1, double dt) + { + const double tmp = v; + + if (has_jerk_limits) + { + const double dv = v - v0; + const double dv0 = v0 - v1; + + const double dt2 = 2. * dt * dt; + + const double da_min = min_jerk * dt2; + const double da_max = max_jerk * dt2; + + const double da = clamp(dv - dv0, da_min, da_max); + + v = v0 + dv0 + da; + } + + return tmp != 0.0 ? v / tmp : 1.0; + } + +} // namespace diff_drive_controller diff --git a/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_bad_urdf.test b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_bad_urdf.test new file mode 100755 index 0000000..a5f44f2 --- /dev/null +++ b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_bad_urdf.test @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + diff --git a/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_common.launch b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_common.launch new file mode 100755 index 0000000..94bbabc --- /dev/null +++ b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_common.launch @@ -0,0 +1,35 @@ + + + + True + + + + + + + + + + + + + + + + diff --git a/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_controller.test b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_controller.test new file mode 100755 index 0000000..3542517 --- /dev/null +++ b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_controller.test @@ -0,0 +1,14 @@ + + + + + + + + + + + diff --git a/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_controller_limits.test b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_controller_limits.test new file mode 100755 index 0000000..949ad82 --- /dev/null +++ b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_controller_limits.test @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + diff --git a/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_controller_nan.test b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_controller_nan.test new file mode 100755 index 0000000..fb6f9dd --- /dev/null +++ b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_controller_nan.test @@ -0,0 +1,20 @@ + + + + + + + diffbot_controller: + publish_cmd: True + + + + + + + + + diff --git a/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_default_cmd_vel_out.test b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_default_cmd_vel_out.test new file mode 100755 index 0000000..0f9c5ef --- /dev/null +++ b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_default_cmd_vel_out.test @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + diff --git a/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_default_cmd_vel_out_test.cpp b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_default_cmd_vel_out_test.cpp new file mode 100755 index 0000000..907187b --- /dev/null +++ b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_default_cmd_vel_out_test.cpp @@ -0,0 +1,69 @@ +/////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2017, PAL Robotics. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of PAL Robotics, Inc. nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +////////////////////////////////////////////////////////////////////////////// + +/// \author Jeremie Deray + +#include "test_common.h" + +// TEST CASES +TEST_F(DiffDriveControllerTest, testDefaultCmdVelOutTopic) +{ + // wait for ROS + waitForController(); + // msgs are published in the same loop + // thus if odom is published cmd_vel_out + // should be as well (if enabled) + waitForOdomMsgs(); + + EXPECT_FALSE(isPublishingCmdVelOut()); + + // zero everything before test + geometry_msgs::Twist cmd_vel; + cmd_vel.linear.x = 0.0; + cmd_vel.angular.z = 0.0; + publish(cmd_vel); + ros::Duration(0.1).sleep(); + + cmd_vel.linear.x = 0.1; + publish(cmd_vel); + ros::Duration(0.1).sleep(); + + EXPECT_FALSE(isPublishingCmdVelOut()); +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + ros::init(argc, argv, "diff_drive_default_cmd_vel_out_topic_test"); + + ros::AsyncSpinner spinner(1); + spinner.start(); + int ret = RUN_ALL_TESTS(); + spinner.stop(); + ros::shutdown(); + return ret; +} diff --git a/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_default_odom_frame.test b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_default_odom_frame.test new file mode 100755 index 0000000..e6c1738 --- /dev/null +++ b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_default_odom_frame.test @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + diff --git a/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_default_odom_frame_test.cpp b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_default_odom_frame_test.cpp new file mode 100755 index 0000000..c06fcc0 --- /dev/null +++ b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_default_odom_frame_test.cpp @@ -0,0 +1,69 @@ +/////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2015, Locus Robotics Corp. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of PAL Robotics, Inc. nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +////////////////////////////////////////////////////////////////////////////// + +/// \author Eric Tappan + +#include "test_common.h" +#include + +// TEST CASES +TEST_F(DiffDriveControllerTest, testOdomFrame) +{ + // wait for ROS + waitForController(); + + // set up tf listener + tf::TransformListener listener; + ros::Duration(2.0).sleep(); + // check the original odom frame doesn't exist + EXPECT_TRUE(listener.frameExists("odom")); +} + +TEST_F(DiffDriveControllerTest, testOdomTopic) +{ + // wait for ROS + waitForController(); + waitForOdomMsgs(); + + // get an odom message + nav_msgs::Odometry odom_msg = getLastOdom(); + // check its frame_id + ASSERT_STREQ(odom_msg.header.frame_id.c_str(), "odom"); +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + ros::init(argc, argv, "diff_drive_default_odom_frame_test"); + + ros::AsyncSpinner spinner(1); + spinner.start(); + int ret = RUN_ALL_TESTS(); + spinner.stop(); + ros::shutdown(); + return ret; +} diff --git a/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_default_wheel_joint_controller_state.test b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_default_wheel_joint_controller_state.test new file mode 100755 index 0000000..4b17188 --- /dev/null +++ b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_default_wheel_joint_controller_state.test @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + diff --git a/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_default_wheel_joint_controller_state_test.cpp b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_default_wheel_joint_controller_state_test.cpp new file mode 100755 index 0000000..cb29465 --- /dev/null +++ b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_default_wheel_joint_controller_state_test.cpp @@ -0,0 +1,65 @@ +/////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2018, PAL Robotics. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of PAL Robotics, Inc. nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +////////////////////////////////////////////////////////////////////////////// + +/// \author Jeremie Deray + +#include "test_common.h" + +// TEST CASES +TEST_F(DiffDriveControllerTest, testDefaultJointTrajectoryControllerStateTopic) +{ + // wait for ROS + waitForController(); + + EXPECT_FALSE(isPublishingJointTrajectoryControllerState()); + + // zero everything before test + geometry_msgs::Twist cmd_vel; + cmd_vel.linear.x = 0.0; + cmd_vel.angular.z = 0.0; + publish(cmd_vel); + ros::Duration(0.1).sleep(); + + cmd_vel.linear.x = 0.1; + publish(cmd_vel); + ros::Duration(0.1).sleep(); + + EXPECT_FALSE(isPublishingJointTrajectoryControllerState()); +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + ros::init(argc, argv, "diff_drive_default_wheel_joint_controller_state_topic_test"); + + ros::AsyncSpinner spinner(1); + spinner.start(); + int ret = RUN_ALL_TESTS(); + spinner.stop(); + ros::shutdown(); + return ret; +} diff --git a/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_dyn_reconf.test b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_dyn_reconf.test new file mode 100755 index 0000000..bb670ed --- /dev/null +++ b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_dyn_reconf.test @@ -0,0 +1,20 @@ + + + + + + + + diffbot_controller: + enable_odom_tf: False + + + + + + + + diff --git a/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_dyn_reconf_test.cpp b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_dyn_reconf_test.cpp new file mode 100755 index 0000000..94842cd --- /dev/null +++ b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_dyn_reconf_test.cpp @@ -0,0 +1,176 @@ +/////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2018, PAL Robotics S.L. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of PAL Robotics, Inc. nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +////////////////////////////////////////////////////////////////////////////// + +/// \author Jeremie Deray + +#include "test_common.h" +#include + +#include + +// TEST CASES +TEST_F(DiffDriveControllerTest, testDynReconfServerAlive) +{ + // wait for ROS + waitForController(); + + // Expect server is alive + EXPECT_TRUE(ros::service::exists("diffbot_controller/set_parameters", true)); + + dynamic_reconfigure::ReconfigureRequest srv_req; + dynamic_reconfigure::ReconfigureResponse srv_resp; + + // Expect server is callable (get-fashion) + EXPECT_TRUE(ros::service::call("diffbot_controller/set_parameters", srv_req, srv_resp)); + + EXPECT_EQ(1, srv_resp.config.bools.size()); + + if (!srv_resp.config.bools.empty()) + { + EXPECT_EQ("enable_odom_tf", srv_resp.config.bools[0].name); + // expect false since it is set to false in the .test + EXPECT_EQ(false, srv_resp.config.bools[0].value); + } + + EXPECT_EQ(4, srv_resp.config.doubles.size()); + + if (srv_resp.config.doubles.size() >= 4) + { + EXPECT_EQ("left_wheel_radius_multiplier", srv_resp.config.doubles[0].name); + EXPECT_EQ(1, srv_resp.config.doubles[0].value); + + EXPECT_EQ("right_wheel_radius_multiplier", srv_resp.config.doubles[1].name); + EXPECT_EQ(1, srv_resp.config.doubles[1].value); + + EXPECT_EQ("wheel_separation_multiplier", srv_resp.config.doubles[2].name); + EXPECT_EQ(1, srv_resp.config.doubles[2].value); + + EXPECT_EQ("publish_rate", srv_resp.config.doubles[3].name); + EXPECT_EQ(50, srv_resp.config.doubles[3].value); + } + + dynamic_reconfigure::DoubleParameter double_param; + double_param.name = "left_wheel_radius_multiplier"; + double_param.value = 0.95; + + srv_req.config.doubles.push_back(double_param); + + double_param.name = "right_wheel_radius_multiplier"; + double_param.value = 0.95; + + srv_req.config.doubles.push_back(double_param); + + double_param.name = "wheel_separation_multiplier"; + double_param.value = 0.95; + + srv_req.config.doubles.push_back(double_param); + + double_param.name = "publish_rate"; + double_param.value = 150; + + srv_req.config.doubles.push_back(double_param); + + dynamic_reconfigure::BoolParameter bool_param; + bool_param.name = "enable_odom_tf"; + bool_param.value = false; + + srv_req.config.bools.push_back(bool_param); + + // Expect server is callable (set-fashion) + EXPECT_TRUE(ros::service::call("diffbot_controller/set_parameters", srv_req, srv_resp)); + + EXPECT_EQ(1, srv_resp.config.bools.size()); + + if (!srv_resp.config.bools.empty()) + { + EXPECT_EQ("enable_odom_tf", srv_resp.config.bools[0].name); + EXPECT_EQ(false, srv_resp.config.bools[0].value); + } + + EXPECT_EQ(4, srv_resp.config.doubles.size()); + + if (srv_resp.config.doubles.size() >= 4) + { + EXPECT_EQ("left_wheel_radius_multiplier", srv_resp.config.doubles[0].name); + EXPECT_EQ(0.95, srv_resp.config.doubles[0].value); + + EXPECT_EQ("right_wheel_radius_multiplier", srv_resp.config.doubles[1].name); + EXPECT_EQ(0.95, srv_resp.config.doubles[1].value); + + EXPECT_EQ("wheel_separation_multiplier", srv_resp.config.doubles[2].name); + EXPECT_EQ(0.95, srv_resp.config.doubles[2].value); + + EXPECT_EQ("publish_rate", srv_resp.config.doubles[3].name); + EXPECT_EQ(150, srv_resp.config.doubles[3].value); + } +} + +// TEST CASES +TEST_F(DiffDriveControllerTest, testDynReconfEnableTf) +{ + // wait for ROS + while(!isControllerAlive() && ros::ok()) + { + ros::Duration(0.1).sleep(); + } + if (!ros::ok()) + FAIL() << "Something went wrong while executing test"; + + // set up tf listener + tf::TransformListener listener; + ros::Duration(2.0).sleep(); + // check the odom frame doesn't exist + EXPECT_FALSE(listener.frameExists("odom")); + + dynamic_reconfigure::ReconfigureRequest srv_req; + dynamic_reconfigure::ReconfigureResponse srv_resp; + + dynamic_reconfigure::BoolParameter bool_param; + bool_param.name = "enable_odom_tf"; + bool_param.value = true; + + srv_req.config.bools.push_back(bool_param); + + EXPECT_TRUE(ros::service::call("diffbot_controller/set_parameters", srv_req, srv_resp)); + + ros::Duration(2.0).sleep(); + // check the odom frame doesn't exist + EXPECT_TRUE(listener.frameExists("odom")); +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + ros::init(argc, argv, "diff_drive_dyn_reconf_test"); + + ros::AsyncSpinner spinner(1); + spinner.start(); + int ret = RUN_ALL_TESTS(); + spinner.stop(); + ros::shutdown(); + return ret; +} diff --git a/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_fail_test.cpp b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_fail_test.cpp new file mode 100755 index 0000000..bb1ee6c --- /dev/null +++ b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_fail_test.cpp @@ -0,0 +1,60 @@ +/////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2013, PAL Robotics S.L. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of PAL Robotics, Inc. nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +////////////////////////////////////////////////////////////////////////////// + +/// \author Paul Mathieu + +#include "test_common.h" + +// TEST CASES +TEST_F(DiffDriveControllerTest, testWrongJointName) +{ + // the controller should never be alive + int secs = 0; + while(!isControllerAlive() && ros::ok() && secs < 5) + { + ros::Duration(1.0).sleep(); + secs++; + } + if (!ros::ok()) + FAIL() << "Something went wrong while executing test."; + + // give up and assume controller load failure after 5 seconds + EXPECT_GE(secs, 5); +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + ros::init(argc, argv, "diff_drive_fail_test"); + + ros::AsyncSpinner spinner(1); + spinner.start(); + int ret = RUN_ALL_TESTS(); + spinner.stop(); + ros::shutdown(); + return ret; +} diff --git a/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_get_wheel_radius_fail.test b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_get_wheel_radius_fail.test new file mode 100755 index 0000000..fae7a83 --- /dev/null +++ b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_get_wheel_radius_fail.test @@ -0,0 +1,19 @@ + + + + + + + + + + + + + + + + diff --git a/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_left_right_multipliers.test b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_left_right_multipliers.test new file mode 100755 index 0000000..f1432fb --- /dev/null +++ b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_left_right_multipliers.test @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + diff --git a/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_limits_test.cpp b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_limits_test.cpp new file mode 100755 index 0000000..5215075 --- /dev/null +++ b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_limits_test.cpp @@ -0,0 +1,231 @@ +/////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2013, PAL Robotics S.L. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of PAL Robotics, Inc. nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +////////////////////////////////////////////////////////////////////////////// + +/// \author Paul Mathieu + +#include "test_common.h" + +// TEST CASES +TEST_F(DiffDriveControllerTest, testLinearJerkLimits) +{ + // wait for ROS + waitForController(); + + // zero everything before test + geometry_msgs::Twist cmd_vel; + cmd_vel.linear.x = 0.0; + cmd_vel.angular.z = 0.0; + publish(cmd_vel); + ros::Duration(2.0).sleep(); + // get initial odom + nav_msgs::Odometry old_odom = getLastOdom(); + // send a big command + cmd_vel.linear.x = 10.0; + publish(cmd_vel); + // wait for a while + ros::Duration(0.5).sleep(); + + nav_msgs::Odometry new_odom = getLastOdom(); + + // check if the robot speed is now 0.37m.s-1 + EXPECT_NEAR(new_odom.twist.twist.linear.x, 0.37, JERK_LINEAR_VELOCITY_TOLERANCE); + EXPECT_LT(fabs(new_odom.twist.twist.angular.z - old_odom.twist.twist.angular.z), EPS); + + cmd_vel.linear.x = 0.0; + publish(cmd_vel); +} + +TEST_F(DiffDriveControllerTest, testLinearAccelerationLimits) +{ + // wait for ROS + while(!isControllerAlive()) + { + ros::Duration(0.1).sleep(); + } + // zero everything before test + geometry_msgs::Twist cmd_vel; + cmd_vel.linear.x = 0.0; + cmd_vel.angular.z = 0.0; + publish(cmd_vel); + ros::Duration(2.0).sleep(); + // get initial odom + nav_msgs::Odometry old_odom = getLastOdom(); + // send a big command + cmd_vel.linear.x = 10.0; + publish(cmd_vel); + // wait for a while + ros::Duration(0.5).sleep(); + + nav_msgs::Odometry new_odom = getLastOdom(); + + // check if the robot speed is now 0.5 m.s-1, which is 1.0m.s-2 * 0.5s + EXPECT_LT(fabs(new_odom.twist.twist.linear.x - old_odom.twist.twist.linear.x), 0.5 + VELOCITY_TOLERANCE); + EXPECT_LT(fabs(new_odom.twist.twist.angular.z - old_odom.twist.twist.angular.z), EPS); + + cmd_vel.linear.x = 0.0; + publish(cmd_vel); +} + +TEST_F(DiffDriveControllerTest, testLinearVelocityLimits) +{ + // wait for ROS + while(!isControllerAlive()) + { + ros::Duration(0.1).sleep(); + } + // zero everything before test + geometry_msgs::Twist cmd_vel; + cmd_vel.linear.x = 0.0; + cmd_vel.angular.z = 0.0; + publish(cmd_vel); + ros::Duration(2.0).sleep(); + // get initial odom + nav_msgs::Odometry old_odom = getLastOdom(); + // send a big command + cmd_vel.linear.x = 10.0; + publish(cmd_vel); + // wait for a while + ros::Duration(5.0).sleep(); + + nav_msgs::Odometry new_odom = getLastOdom(); + + // check if the robot speed is now 1.0 m.s-1, the limit + EXPECT_LT(fabs(new_odom.twist.twist.linear.x - old_odom.twist.twist.linear.x), 1.0 + VELOCITY_TOLERANCE); + EXPECT_LT(fabs(new_odom.twist.twist.angular.z - old_odom.twist.twist.angular.z), EPS); + + cmd_vel.linear.x = 0.0; + publish(cmd_vel); +} + +/* This test has been failing on Travis for a long time due to timing issues however it works well when ran manually +TEST_F(DiffDriveControllerTest, testAngularJerkLimits) +{ + // wait for ROS + while(!isControllerAlive()) + { + ros::Duration(0.1).sleep(); + } + // zero everything before test + geometry_msgs::Twist cmd_vel; + cmd_vel.linear.x = 0.0; + cmd_vel.angular.z = 0.0; + publish(cmd_vel); + ros::Duration(2.0).sleep(); + // get initial odom + nav_msgs::Odometry old_odom = getLastOdom(); + // send a big command + cmd_vel.angular.z = 10.0; + publish(cmd_vel); + // wait for a while + ros::Duration(0.5).sleep(); + + nav_msgs::Odometry new_odom = getLastOdom(); + + // check if the robot speed is now 0.7rad.s-1 + EXPECT_NEAR(new_odom.twist.twist.angular.z, 0.7, JERK_ANGULAR_VELOCITY_TOLERANCE); + EXPECT_LT(fabs(new_odom.twist.twist.linear.x - old_odom.twist.twist.linear.x), EPS); + + cmd_vel.angular.z = 0.0; + publish(cmd_vel); +} +*/ + +TEST_F(DiffDriveControllerTest, testAngularAccelerationLimits) +{ + // wait for ROS + while(!isControllerAlive()) + { + ros::Duration(0.1).sleep(); + } + // zero everything before test + geometry_msgs::Twist cmd_vel; + cmd_vel.linear.x = 0.0; + cmd_vel.angular.z = 0.0; + publish(cmd_vel); + ros::Duration(2.0).sleep(); + // get initial odom + nav_msgs::Odometry old_odom = getLastOdom(); + // send a big command + cmd_vel.angular.z = 10.0; + publish(cmd_vel); + // wait for a while + ros::Duration(0.5).sleep(); + + nav_msgs::Odometry new_odom = getLastOdom(); + + // check if the robot speed is now 1.0rad.s-1, which is 2.0rad.s-2 * 0.5s + EXPECT_LT(fabs(new_odom.twist.twist.angular.z - old_odom.twist.twist.angular.z), 1.0 + VELOCITY_TOLERANCE); + EXPECT_LT(fabs(new_odom.twist.twist.linear.x - old_odom.twist.twist.linear.x), EPS); + + cmd_vel.angular.z = 0.0; + publish(cmd_vel); +} + +TEST_F(DiffDriveControllerTest, testAngularVelocityLimits) +{ + // wait for ROS + while(!isControllerAlive()) + { + ros::Duration(0.1).sleep(); + } + // zero everything before test + geometry_msgs::Twist cmd_vel; + cmd_vel.linear.x = 0.0; + cmd_vel.angular.z = 0.0; + publish(cmd_vel); + ros::Duration(2.0).sleep(); + // get initial odom + nav_msgs::Odometry old_odom = getLastOdom(); + // send a big command + cmd_vel.angular.z = 10.0; + publish(cmd_vel); + // wait for a while + ros::Duration(5.0).sleep(); + + nav_msgs::Odometry new_odom = getLastOdom(); + + // check if the robot speed is now 2.0rad.s-1, the limit + EXPECT_LT(fabs(new_odom.twist.twist.angular.z - old_odom.twist.twist.angular.z), 2.0 + VELOCITY_TOLERANCE); + EXPECT_LT(fabs(new_odom.twist.twist.linear.x - old_odom.twist.twist.linear.x), EPS); + + cmd_vel.angular.z = 0.0; + publish(cmd_vel); +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + ros::init(argc, argv, "diff_drive_limits_test"); + + ros::AsyncSpinner spinner(1); + spinner.start(); + //ros::Duration(0.5).sleep(); + int ret = RUN_ALL_TESTS(); + spinner.stop(); + ros::shutdown(); + return ret; +} diff --git a/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_multiple_cmd_vel_publishers.test b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_multiple_cmd_vel_publishers.test new file mode 100755 index 0000000..6681cce --- /dev/null +++ b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_multiple_cmd_vel_publishers.test @@ -0,0 +1,24 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_multiple_cmd_vel_publishers_test.cpp b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_multiple_cmd_vel_publishers_test.cpp new file mode 100755 index 0000000..929abd1 --- /dev/null +++ b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_multiple_cmd_vel_publishers_test.cpp @@ -0,0 +1,72 @@ +/////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2014, PAL Robotics S.L. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of PAL Robotics, Inc. nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +////////////////////////////////////////////////////////////////////////////// + +/// \author Bence Magyar + +#include "test_common.h" + +// TEST CASES +TEST_F(DiffDriveControllerTest, breakWithMultiplePublishers) +{ + // wait for ROS + waitForController(); + waitForOdomMsgs(); + + nav_msgs::Odometry old_odom = getLastOdom(); + //TODO: we should be programatically publish from 2 different nodes + // not the current hacky solution with the launch files + ros::Duration(1.0).sleep(); + nav_msgs::Odometry new_odom = getLastOdom(); + + const double dx = new_odom.pose.pose.position.x - old_odom.pose.pose.position.x; + const double dy = new_odom.pose.pose.position.y - old_odom.pose.pose.position.y; + const double dz = new_odom.pose.pose.position.z - old_odom.pose.pose.position.z; + EXPECT_NEAR(sqrt(dx*dx + dy*dy), 0.0, POSITION_TOLERANCE); + EXPECT_LT(fabs(dz), EPS); + + // convert to rpy and test that way + double roll_old, pitch_old, yaw_old; + double roll_new, pitch_new, yaw_new; + tf::Matrix3x3(tfQuatFromGeomQuat(old_odom.pose.pose.orientation)).getRPY(roll_old, pitch_old, yaw_old); + tf::Matrix3x3(tfQuatFromGeomQuat(new_odom.pose.pose.orientation)).getRPY(roll_new, pitch_new, yaw_new); + EXPECT_LT(fabs(roll_new - roll_old), EPS); + EXPECT_LT(fabs(pitch_new - pitch_old), EPS); + EXPECT_LT(fabs(yaw_new - yaw_old), EPS); +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + ros::init(argc, argv, "diff_drive_multiple_publishers_test"); + + ros::AsyncSpinner spinner(1); + spinner.start(); + int ret = RUN_ALL_TESTS(); + spinner.stop(); + ros::shutdown(); + return ret; +} diff --git a/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_multipliers.test b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_multipliers.test new file mode 100755 index 0000000..e6d9f9e --- /dev/null +++ b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_multipliers.test @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + diff --git a/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_nan_test.cpp b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_nan_test.cpp new file mode 100755 index 0000000..769b42d --- /dev/null +++ b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_nan_test.cpp @@ -0,0 +1,131 @@ +/////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2014, PAL Robotics S.L. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of PAL Robotics, Inc. nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +////////////////////////////////////////////////////////////////////////////// + +/// \author Enrique Fernandez + +#include "test_common.h" + +// NaN +#include + +// TEST CASES +TEST_F(DiffDriveControllerTest, testNaN) { + // wait for ROS + waitForController(); + + // zero everything before test + geometry_msgs::Twist cmd_vel; + cmd_vel.linear.x = 0.0; + cmd_vel.angular.z = 0.0; + publish(cmd_vel); + ros::Duration(2.0).sleep(); + + // send a command + cmd_vel.linear.x = 0.1; + ros::Duration(2.0).sleep(); + + // stop robot (will generate NaN) + stop(); + ros::Duration(2.0).sleep(); + + nav_msgs::Odometry odom = getLastOdom(); + + EXPECT_FALSE(std::isnan(odom.twist.twist.linear.x)); + EXPECT_FALSE(std::isnan(odom.twist.twist.angular.z)); + EXPECT_FALSE(std::isnan(odom.pose.pose.position.x)); + EXPECT_FALSE(std::isnan(odom.pose.pose.position.y)); + EXPECT_FALSE(std::isnan(odom.pose.pose.orientation.z)); + EXPECT_FALSE(std::isnan(odom.pose.pose.orientation.w)); + + // start robot + start(); + ros::Duration(2.0).sleep(); + + odom = getLastOdom(); + + EXPECT_FALSE(std::isnan(odom.twist.twist.linear.x)); + EXPECT_FALSE(std::isnan(odom.twist.twist.angular.z)); + EXPECT_FALSE(std::isnan(odom.pose.pose.position.x)); + EXPECT_FALSE(std::isnan(odom.pose.pose.position.y)); + EXPECT_FALSE(std::isnan(odom.pose.pose.orientation.z)); + EXPECT_FALSE(std::isnan(odom.pose.pose.orientation.w)); +} + +TEST_F(DiffDriveControllerTest, testNaNCmd) { + // wait for ROS + while (!isControllerAlive()) { + ros::Duration(0.1).sleep(); + } + // zero everything before test + geometry_msgs::Twist cmd_vel; + cmd_vel.linear.x = 0.0; + cmd_vel.angular.z = 0.0; + publish(cmd_vel); + ros::Duration(0.1).sleep(); + + // send NaN + for (int i = 0; i < 10; ++i) { + geometry_msgs::Twist cmd_vel; + cmd_vel.linear.x = NAN; + cmd_vel.angular.z = 0.0; + publish(cmd_vel); + geometry_msgs::TwistStamped odom_msg = getLastCmdVelOut(); + EXPECT_FALSE(std::isnan(odom_msg.twist.linear.x)); + EXPECT_FALSE(std::isnan(odom_msg.twist.angular.z)); + ros::Duration(0.1).sleep(); + } + for (int i = 0; i < 10; ++i) { + geometry_msgs::Twist cmd_vel; + cmd_vel.linear.x = 0.0; + cmd_vel.angular.z = NAN; + publish(cmd_vel); + geometry_msgs::TwistStamped odom_msg = getLastCmdVelOut(); + EXPECT_FALSE(std::isnan(odom_msg.twist.linear.x)); + EXPECT_FALSE(std::isnan(odom_msg.twist.angular.z)); + ros::Duration(0.1).sleep(); + } + + nav_msgs::Odometry odom = getLastOdom(); + EXPECT_DOUBLE_EQ(odom.twist.twist.linear.x, 0.0); + EXPECT_DOUBLE_EQ(odom.pose.pose.position.x, 0.0); + EXPECT_DOUBLE_EQ(odom.pose.pose.position.y, 0.0); + + geometry_msgs::TwistStamped odom_msg = getLastCmdVelOut(); + EXPECT_DOUBLE_EQ(odom_msg.twist.linear.x, 0.0); +} + +int main(int argc, char **argv) { + testing::InitGoogleTest(&argc, argv); + ros::init(argc, argv, "diff_drive_nan_test"); + + ros::AsyncSpinner spinner(1); + spinner.start(); + int ret = RUN_ALL_TESTS(); + spinner.stop(); + ros::shutdown(); + return ret; +} diff --git a/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_odom_frame.test b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_odom_frame.test new file mode 100755 index 0000000..7ac4072 --- /dev/null +++ b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_odom_frame.test @@ -0,0 +1,20 @@ + + + + + + + + diffbot_controller: + odom_frame_id: new_odom + + + + + + + + diff --git a/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_odom_frame_test.cpp b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_odom_frame_test.cpp new file mode 100755 index 0000000..ea86e40 --- /dev/null +++ b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_odom_frame_test.cpp @@ -0,0 +1,83 @@ +/////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2015, Locus Robotics Corp. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of PAL Robotics, Inc. nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +////////////////////////////////////////////////////////////////////////////// + +/// \author Eric Tappan + +#include "test_common.h" +#include + +// TEST CASES +TEST_F(DiffDriveControllerTest, testNoOdomFrame) +{ + // wait for ROS + waitForController(); + + // set up tf listener + tf::TransformListener listener; + ros::Duration(2.0).sleep(); + // check the original odom frame doesn't exist + EXPECT_FALSE(listener.frameExists("odom")); +} + +TEST_F(DiffDriveControllerTest, testNewOdomFrame) +{ + // wait for ROS + waitForController(); + + // set up tf listener + tf::TransformListener listener; + ros::Duration(2.0).sleep(); + // check the new_odom frame does exist + EXPECT_TRUE(listener.frameExists("new_odom")); +} + +TEST_F(DiffDriveControllerTest, testOdomTopic) +{ + // wait for ROS + waitForController(); + + // wait until we received first odom msg + waitForOdomMsgs(); + + // get an odom message + nav_msgs::Odometry odom_msg = getLastOdom(); + // check its frame_id + ASSERT_STREQ(odom_msg.header.frame_id.c_str(), "new_odom"); +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + ros::init(argc, argv, "diff_drive_odom_frame_test"); + + ros::AsyncSpinner spinner(1); + spinner.start(); + int ret = RUN_ALL_TESTS(); + spinner.stop(); + ros::shutdown(); + return ret; +} diff --git a/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_odom_tf.test b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_odom_tf.test new file mode 100755 index 0000000..117a630 --- /dev/null +++ b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_odom_tf.test @@ -0,0 +1,20 @@ + + + + + + + + diffbot_controller: + enable_odom_tf: False + + + + + + + + diff --git a/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_odom_tf_test.cpp b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_odom_tf_test.cpp new file mode 100755 index 0000000..fef3096 --- /dev/null +++ b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_odom_tf_test.cpp @@ -0,0 +1,57 @@ +/////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2014, PAL Robotics S.L. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of PAL Robotics, Inc. nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +////////////////////////////////////////////////////////////////////////////// + +/// \author Bence Magyar + +#include "test_common.h" +#include + +// TEST CASES +TEST_F(DiffDriveControllerTest, testNoOdomFrame) +{ + // wait for ROS + waitForController(); + + // set up tf listener + tf::TransformListener listener; + ros::Duration(2.0).sleep(); + // check the odom frame doesn't exist + EXPECT_FALSE(listener.frameExists("odom")); +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + ros::init(argc, argv, "diff_drive_odom_tf_test"); + + ros::AsyncSpinner spinner(1); + spinner.start(); + int ret = RUN_ALL_TESTS(); + spinner.stop(); + ros::shutdown(); + return ret; +} diff --git a/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_open_loop.test b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_open_loop.test new file mode 100755 index 0000000..c6484b3 --- /dev/null +++ b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_open_loop.test @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + diff --git a/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_pub_cmd_vel_out.test b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_pub_cmd_vel_out.test new file mode 100755 index 0000000..3734c37 --- /dev/null +++ b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_pub_cmd_vel_out.test @@ -0,0 +1,20 @@ + + + + + + + diffbot_controller: + publish_cmd: True + + + + + + + + + diff --git a/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_pub_cmd_vel_out_test.cpp b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_pub_cmd_vel_out_test.cpp new file mode 100755 index 0000000..88c4e11 --- /dev/null +++ b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_pub_cmd_vel_out_test.cpp @@ -0,0 +1,74 @@ +/////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2017, PAL Robotics. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of PAL Robotics, Inc. nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +////////////////////////////////////////////////////////////////////////////// + +/// \author Jeremie Deray + +#include "test_common.h" + +// TEST CASES +TEST_F(DiffDriveControllerTest, testCmdVelOutTopic) +{ + // wait for ROS + waitForController(); + // msgs are published in the same loop + // thus if odom is published cmd_vel_out + // should be as well (if enabled) + waitForOdomMsgs(); + + EXPECT_TRUE(isPublishingCmdVelOut()); + + // zero everything before test + geometry_msgs::Twist cmd_vel; + cmd_vel.linear.x = 0.0; + cmd_vel.angular.z = 0.0; + publish(cmd_vel); + ros::Duration(0.1).sleep(); + + cmd_vel.linear.x = 0.1; + publish(cmd_vel); + ros::Duration(0.1).sleep(); + + EXPECT_TRUE(isPublishingCmdVelOut()); + + // get a twist message + geometry_msgs::TwistStamped odom_msg = getLastCmdVelOut(); + + EXPECT_GT(fabs(odom_msg.twist.linear.x), 0); +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + ros::init(argc, argv, "diff_drive_pub_cmd_vel_out_topic_test"); + + ros::AsyncSpinner spinner(1); + spinner.start(); + int ret = RUN_ALL_TESTS(); + spinner.stop(); + ros::shutdown(); + return ret; +} diff --git a/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_publish_wheel_joint_controller_state.test b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_publish_wheel_joint_controller_state.test new file mode 100755 index 0000000..d4f9fff --- /dev/null +++ b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_publish_wheel_joint_controller_state.test @@ -0,0 +1,21 @@ + + + + + + + + diffbot_controller: + publish_wheel_joint_controller_state: True + + + + + + + + + diff --git a/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_publish_wheel_joint_controller_state_test.cpp b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_publish_wheel_joint_controller_state_test.cpp new file mode 100755 index 0000000..300353d --- /dev/null +++ b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_publish_wheel_joint_controller_state_test.cpp @@ -0,0 +1,65 @@ +/////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2018, PAL Robotics. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of PAL Robotics, Inc. nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +////////////////////////////////////////////////////////////////////////////// + +/// \author Jeremie Deray + +#include "test_common.h" + +// TEST CASES +TEST_F(DiffDriveControllerTest, testPublishJointTrajectoryControllerStateTopic) +{ + // wait for ROS + waitForController(); + + EXPECT_TRUE(isPublishingJointTrajectoryControllerState()); + + // zero everything before test + geometry_msgs::Twist cmd_vel; + cmd_vel.linear.x = 0.0; + cmd_vel.angular.z = 0.0; + publish(cmd_vel); + ros::Duration(0.1).sleep(); + + cmd_vel.linear.x = 0.1; + publish(cmd_vel); + ros::Duration(0.1).sleep(); + + EXPECT_TRUE(isPublishingJointTrajectoryControllerState()); +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + ros::init(argc, argv, "diff_drive_publish_wheel_joint_controller_state_topic_test"); + + ros::AsyncSpinner spinner(1); + spinner.start(); + int ret = RUN_ALL_TESTS(); + spinner.stop(); + ros::shutdown(); + return ret; +} diff --git a/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_radius_param.test b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_radius_param.test new file mode 100755 index 0000000..3baa691 --- /dev/null +++ b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_radius_param.test @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + diff --git a/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_radius_param_fail.test b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_radius_param_fail.test new file mode 100755 index 0000000..9fb9ac7 --- /dev/null +++ b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_radius_param_fail.test @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + diff --git a/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_radius_sphere.test b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_radius_sphere.test new file mode 100755 index 0000000..24e1b75 --- /dev/null +++ b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_radius_sphere.test @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + diff --git a/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_separation_param.test b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_separation_param.test new file mode 100755 index 0000000..07c7afb --- /dev/null +++ b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_separation_param.test @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + diff --git a/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_test.cpp b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_test.cpp new file mode 100755 index 0000000..d89fde9 --- /dev/null +++ b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_test.cpp @@ -0,0 +1,154 @@ +/////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2013, PAL Robotics S.L. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of PAL Robotics, Inc. nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +////////////////////////////////////////////////////////////////////////////// + +/// \author Bence Magyar + +#include "test_common.h" +#include + +// TEST CASES +TEST_F(DiffDriveControllerTest, testForward) +{ + // wait for ROS + waitForController(); + + // zero everything before test + geometry_msgs::Twist cmd_vel; + cmd_vel.linear.x = 0.0; + cmd_vel.angular.z = 0.0; + publish(cmd_vel); + ros::Duration(2.0).sleep(); + // get initial odom + nav_msgs::Odometry old_odom = getLastOdom(); + // send a velocity command of 0.1 m/s + cmd_vel.linear.x = 0.1; + publish(cmd_vel); + // wait for 10s + ros::Duration(10.0).sleep(); + + nav_msgs::Odometry new_odom = getLastOdom(); + + const ros::Duration actual_elapsed_time = new_odom.header.stamp - old_odom.header.stamp; + + const double expected_distance = cmd_vel.linear.x * actual_elapsed_time.toSec(); + + // check if the robot traveled 1 meter in XY plane, changes in z should be ~~0 + const double dx = new_odom.pose.pose.position.x - old_odom.pose.pose.position.x; + const double dy = new_odom.pose.pose.position.y - old_odom.pose.pose.position.y; + const double dz = new_odom.pose.pose.position.z - old_odom.pose.pose.position.z; + EXPECT_NEAR(sqrt(dx*dx + dy*dy), expected_distance, POSITION_TOLERANCE); + EXPECT_LT(fabs(dz), EPS); + + // convert to rpy and test that way + double roll_old, pitch_old, yaw_old; + double roll_new, pitch_new, yaw_new; + tf::Matrix3x3(tfQuatFromGeomQuat(old_odom.pose.pose.orientation)).getRPY(roll_old, pitch_old, yaw_old); + tf::Matrix3x3(tfQuatFromGeomQuat(new_odom.pose.pose.orientation)).getRPY(roll_new, pitch_new, yaw_new); + EXPECT_LT(fabs(roll_new - roll_old), EPS); + EXPECT_LT(fabs(pitch_new - pitch_old), EPS); + EXPECT_LT(fabs(yaw_new - yaw_old), EPS); + EXPECT_NEAR(fabs(new_odom.twist.twist.linear.x), cmd_vel.linear.x, EPS); + EXPECT_LT(fabs(new_odom.twist.twist.linear.y), EPS); + EXPECT_LT(fabs(new_odom.twist.twist.linear.z), EPS); + + EXPECT_LT(fabs(new_odom.twist.twist.angular.x), EPS); + EXPECT_LT(fabs(new_odom.twist.twist.angular.y), EPS); + EXPECT_LT(fabs(new_odom.twist.twist.angular.z), EPS); +} + +TEST_F(DiffDriveControllerTest, testTurn) +{ + // wait for ROS + waitForController(); + + // zero everything before test + geometry_msgs::Twist cmd_vel; + cmd_vel.linear.x = 0.0; + cmd_vel.angular.z = 0.0; + publish(cmd_vel); + ros::Duration(2.0).sleep(); + // get initial odom + nav_msgs::Odometry old_odom = getLastOdom(); + // send a velocity command + cmd_vel.angular.z = M_PI/10.0; + publish(cmd_vel); + // wait for 10s + ros::Duration(10.0).sleep(); + + nav_msgs::Odometry new_odom = getLastOdom(); + + // check if the robot rotated PI around z, changes in the other fields should be ~~0 + EXPECT_LT(fabs(new_odom.pose.pose.position.x - old_odom.pose.pose.position.x), EPS); + EXPECT_LT(fabs(new_odom.pose.pose.position.y - old_odom.pose.pose.position.y), EPS); + EXPECT_LT(fabs(new_odom.pose.pose.position.z - old_odom.pose.pose.position.z), EPS); + + const ros::Duration actual_elapsed_time = new_odom.header.stamp - old_odom.header.stamp; + const double expected_rotation = cmd_vel.angular.z * actual_elapsed_time.toSec(); + + // convert to rpy and test that way + double roll_old, pitch_old, yaw_old; + double roll_new, pitch_new, yaw_new; + tf::Matrix3x3(tfQuatFromGeomQuat(old_odom.pose.pose.orientation)).getRPY(roll_old, pitch_old, yaw_old); + tf::Matrix3x3(tfQuatFromGeomQuat(new_odom.pose.pose.orientation)).getRPY(roll_new, pitch_new, yaw_new); + EXPECT_LT(fabs(roll_new - roll_old), EPS); + EXPECT_LT(fabs(pitch_new - pitch_old), EPS); + EXPECT_NEAR(fabs(yaw_new - yaw_old), expected_rotation, ORIENTATION_TOLERANCE); + + EXPECT_LT(fabs(new_odom.twist.twist.linear.x), EPS); + EXPECT_LT(fabs(new_odom.twist.twist.linear.y), EPS); + EXPECT_LT(fabs(new_odom.twist.twist.linear.z), EPS); + + EXPECT_LT(fabs(new_odom.twist.twist.angular.x), EPS); + EXPECT_LT(fabs(new_odom.twist.twist.angular.y), EPS); + EXPECT_NEAR(fabs(new_odom.twist.twist.angular.z), expected_rotation/actual_elapsed_time.toSec(), EPS); +} + +TEST_F(DiffDriveControllerTest, testOdomFrame) +{ + // wait for ROS + waitForController(); + + // set up tf listener + tf::TransformListener listener; + ros::Duration(2.0).sleep(); + // check the odom frame exist + EXPECT_TRUE(listener.frameExists("odom")); +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + ros::init(argc, argv, "diff_drive_test"); + + ros::AsyncSpinner spinner(1); + spinner.start(); + //ros::Duration(0.5).sleep(); + int ret = RUN_ALL_TESTS(); + spinner.stop(); + ros::shutdown(); + return ret; +} diff --git a/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_timeout.test b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_timeout.test new file mode 100755 index 0000000..b55a16f --- /dev/null +++ b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_timeout.test @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + diff --git a/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_timeout_test.cpp b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_timeout_test.cpp new file mode 100755 index 0000000..1f822ce --- /dev/null +++ b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_timeout_test.cpp @@ -0,0 +1,70 @@ +/////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2013, PAL Robotics S.L. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of PAL Robotics, Inc. nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +////////////////////////////////////////////////////////////////////////////// + +/// \author Paul Mathieu + +#include "test_common.h" + +// TEST CASES +TEST_F(DiffDriveControllerTest, testTimeout) +{ + // wait for ROS + waitForController(); + + // zero everything before test + geometry_msgs::Twist cmd_vel; + cmd_vel.linear.x = 0.0; + cmd_vel.angular.z = 0.0; + publish(cmd_vel); + // give some time to the controller to react to the command + ros::Duration(0.1).sleep(); + // get initial odom + nav_msgs::Odometry old_odom = getLastOdom(); + // send a velocity command of 1 m/s + cmd_vel.linear.x = 1.0; + publish(cmd_vel); + // wait a bit + ros::Duration(3.0).sleep(); + + nav_msgs::Odometry new_odom = getLastOdom(); + + // check if the robot has stopped after 0.5s, thus covering less than 0.5s*1.0m.s-1 + some (big) tolerance + EXPECT_LT(fabs(new_odom.pose.pose.position.x - old_odom.pose.pose.position.x), 0.8); +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + ros::init(argc, argv, "diff_drive_test"); + + ros::AsyncSpinner spinner(1); + spinner.start(); + int ret = RUN_ALL_TESTS(); + spinner.stop(); + ros::shutdown(); + return ret; +} diff --git a/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_wrong.test b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_wrong.test new file mode 100755 index 0000000..6efbf73 --- /dev/null +++ b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diff_drive_wrong.test @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + diff --git a/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diffbot.cpp b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diffbot.cpp new file mode 100755 index 0000000..0b69273 --- /dev/null +++ b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diffbot.cpp @@ -0,0 +1,93 @@ +/////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2013, PAL Robotics S.L. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of PAL Robotics, Inc. nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +////////////////////////////////////////////////////////////////////////////// + +// NOTE: The contents of this file have been taken largely from the ros_control wiki tutorials + +#include "diffbot.h" +#include +#include +#include +#include +#include + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "diffbot"); + ros::NodeHandle nh; + + // This should be set in launch files as well + nh.setParam("/use_sim_time", true); + + Diffbot<> robot; + ROS_WARN_STREAM("period: " << robot.getPeriod().toSec()); + controller_manager::ControllerManager cm(&robot, nh); + + ros::Publisher clock_publisher = nh.advertise("/clock", 1); + + ros::AsyncSpinner spinner(1); + spinner.start(); + + std::chrono::system_clock::time_point begin = std::chrono::system_clock::now(); + std::chrono::system_clock::time_point end = std::chrono::system_clock::now(); + + ros::Time internal_time(0); + const ros::Duration dt = robot.getPeriod(); + double elapsed_secs = 0; + + while(ros::ok()) + { + begin = std::chrono::system_clock::now(); + + robot.read(); + cm.update(internal_time, dt); + + robot.write(); + + end = std::chrono::system_clock::now(); + + elapsed_secs = std::chrono::duration_cast >((end - begin)).count(); + + if (dt.toSec() - elapsed_secs < 0.0) + { + ROS_WARN_STREAM_THROTTLE( + 0.1, "Control cycle is taking to much time, elapsed: " << elapsed_secs); + } + else + { + ROS_DEBUG_STREAM_THROTTLE(1.0, "Control cycle is, elapsed: " << elapsed_secs); + std::this_thread::sleep_for(std::chrono::duration(dt.toSec() - elapsed_secs)); + } + + rosgraph_msgs::Clock clock; + clock.clock = ros::Time(internal_time); + clock_publisher.publish(clock); + internal_time += dt; + } + spinner.stop(); + + return 0; +} diff --git a/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diffbot.h b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diffbot.h new file mode 100755 index 0000000..6c8b9cd --- /dev/null +++ b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diffbot.h @@ -0,0 +1,143 @@ +/////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2013, PAL Robotics S.L. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of PAL Robotics, Inc. nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +////////////////////////////////////////////////////////////////////////////// + +// NOTE: The contents of this file have been taken largely from the ros_control wiki tutorials + +#pragma once + + +// ROS +#include +#include + +// ros_control +#include +#include +#include +#include +#include + +// NaN +#include + +// ostringstream +#include + +template +class Diffbot : public hardware_interface::RobotHW +{ +public: + Diffbot() + : running_(true) + , start_srv_(nh_.advertiseService("start", &Diffbot::start_callback, this)) + , stop_srv_(nh_.advertiseService("stop", &Diffbot::stop_callback, this)) + { + // Intialize raw data + std::fill_n(pos_, NUM_JOINTS, 0.0); + std::fill_n(vel_, NUM_JOINTS, 0.0); + std::fill_n(eff_, NUM_JOINTS, 0.0); + std::fill_n(cmd_, NUM_JOINTS, 0.0); + + // Connect and register the joint state and velocity interface + for (unsigned int i = 0; i < NUM_JOINTS; ++i) + { + std::ostringstream os; + os << "wheel_" << i << "_joint"; + + hardware_interface::JointStateHandle state_handle(os.str(), &pos_[i], &vel_[i], &eff_[i]); + jnt_state_interface_.registerHandle(state_handle); + + hardware_interface::JointHandle vel_handle(jnt_state_interface_.getHandle(os.str()), &cmd_[i]); + jnt_vel_interface_.registerHandle(vel_handle); + } + + registerInterface(&jnt_state_interface_); + registerInterface(&jnt_vel_interface_); + } + + ros::Time getTime() const {return ros::Time::now();} + ros::Duration getPeriod() const {return ros::Duration(0.01);} + + void read() + { + // Read the joint state of the robot into the hardware interface + if (running_) + { + for (unsigned int i = 0; i < NUM_JOINTS; ++i) + { + // Note that pos_[i] will be NaN for one more cycle after we start(), + // but that is consistent with the knowledge we have about the state + // of the robot. + pos_[i] += vel_[i]*getPeriod().toSec(); // update position + vel_[i] = cmd_[i]; // might add smoothing here later + } + } + else + { + std::fill_n(pos_, NUM_JOINTS, std::numeric_limits::quiet_NaN()); + std::fill_n(vel_, NUM_JOINTS, std::numeric_limits::quiet_NaN()); + } + } + + void write() + { + // Write the commands to the joints + std::ostringstream os; + for (unsigned int i = 0; i < NUM_JOINTS - 1; ++i) + { + os << cmd_[i] << ", "; + } + os << cmd_[NUM_JOINTS - 1]; + + ROS_INFO_STREAM("Commands for joints: " << os.str()); + } + + bool start_callback(std_srvs::Empty::Request& /*req*/, std_srvs::Empty::Response& /*res*/) + { + running_ = true; + return true; + } + + bool stop_callback(std_srvs::Empty::Request& /*req*/, std_srvs::Empty::Response& /*res*/) + { + running_ = false; + return true; + } + +private: + hardware_interface::JointStateInterface jnt_state_interface_; + hardware_interface::VelocityJointInterface jnt_vel_interface_; + double cmd_[NUM_JOINTS]; + double pos_[NUM_JOINTS]; + double vel_[NUM_JOINTS]; + double eff_[NUM_JOINTS]; + bool running_; + + ros::NodeHandle nh_; + ros::ServiceServer start_srv_; + ros::ServiceServer stop_srv_; +}; diff --git a/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diffbot.xacro b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diffbot.xacro new file mode 100755 index 0000000..c9b96c2 --- /dev/null +++ b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diffbot.xacro @@ -0,0 +1,77 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Green + + + + Gazebo/Purple + + + diff --git a/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diffbot_bad.xacro b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diffbot_bad.xacro new file mode 100755 index 0000000..ec88c38 --- /dev/null +++ b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diffbot_bad.xacro @@ -0,0 +1,142 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1 + 1 + + + + + + 1 + 1 + + + + + Gazebo/Green + + + + Gazebo/Red + + + + Gazebo/Blue + + + + Gazebo/Purple + + + diff --git a/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diffbot_controllers.yaml b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diffbot_controllers.yaml new file mode 100755 index 0000000..f89c38c --- /dev/null +++ b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diffbot_controllers.yaml @@ -0,0 +1,8 @@ +diffbot_controller: + type: "diff_drive_controller/DiffDriveController" + left_wheel: 'wheel_0_joint' + right_wheel: 'wheel_1_joint' + publish_rate: 50.0 # defaults to 50 + pose_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0] + twist_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0] + cmd_vel_timeout: 20.0 # we test this separately, give plenty for the other tests diff --git a/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diffbot_left_right_multipliers.yaml b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diffbot_left_right_multipliers.yaml new file mode 100755 index 0000000..26b21eb --- /dev/null +++ b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diffbot_left_right_multipliers.yaml @@ -0,0 +1,4 @@ +diffbot_controller: + wheel_separation_multiplier: 2.3 + left_wheel_radius_multiplier: 1.4 + right_wheel_radius_multiplier: 1.4 diff --git a/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diffbot_limits.yaml b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diffbot_limits.yaml new file mode 100755 index 0000000..78f0bf9 --- /dev/null +++ b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diffbot_limits.yaml @@ -0,0 +1,19 @@ +diffbot_controller: + linear: + x: + has_velocity_limits: true + min_velocity: -0.5 + max_velocity: 1.0 + has_acceleration_limits: true + min_acceleration: -0.5 + max_acceleration: 1.0 + has_jerk_limits: true + max_jerk: 5.0 + angular: + z: + has_velocity_limits: true + max_velocity: 2.0 + has_acceleration_limits: true + max_acceleration: 2.0 + has_jerk_limits: true + max_jerk: 10.0 diff --git a/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diffbot_multipliers.yaml b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diffbot_multipliers.yaml new file mode 100755 index 0000000..d2ef5d5 --- /dev/null +++ b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diffbot_multipliers.yaml @@ -0,0 +1,3 @@ +diffbot_controller: + wheel_separation_multiplier: 2.3 + wheel_radius_multiplier: 1.4 diff --git a/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diffbot_open_loop.yaml b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diffbot_open_loop.yaml new file mode 100755 index 0000000..19224f3 --- /dev/null +++ b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diffbot_open_loop.yaml @@ -0,0 +1,2 @@ +diffbot_controller: + open_loop: true diff --git a/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diffbot_sphere_wheels.xacro b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diffbot_sphere_wheels.xacro new file mode 100755 index 0000000..90b2962 --- /dev/null +++ b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diffbot_sphere_wheels.xacro @@ -0,0 +1,75 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Green + + + + Gazebo/Purple + + + diff --git a/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diffbot_square_wheels.xacro b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diffbot_square_wheels.xacro new file mode 100755 index 0000000..a87ef57 --- /dev/null +++ b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diffbot_square_wheels.xacro @@ -0,0 +1,75 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Green + + + + Gazebo/Purple + + + diff --git a/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diffbot_timeout.yaml b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diffbot_timeout.yaml new file mode 100755 index 0000000..4ecfa18 --- /dev/null +++ b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diffbot_timeout.yaml @@ -0,0 +1,2 @@ +diffbot_controller: + cmd_vel_timeout: 0.5 diff --git a/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diffbot_wrong.yaml b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diffbot_wrong.yaml new file mode 100755 index 0000000..516fd75 --- /dev/null +++ b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/diffbot_wrong.yaml @@ -0,0 +1,8 @@ +diffbot_controller: + type: "diff_drive_controller/DiffDriveController" + left_wheel: 'this_joint_does_not_exist' + right_wheel: 'right_wheel_joint' + publish_rate: 50.0 # defaults to 50 + pose_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0] + twist_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0] + cmd_vel_timeout: 20.0 # we test this separately, give plenty for the other tests diff --git a/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/skid_steer_common.launch b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/skid_steer_common.launch new file mode 100755 index 0000000..378ccdb --- /dev/null +++ b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/skid_steer_common.launch @@ -0,0 +1,35 @@ + + + + True + + + + + + + + + + + + + + + + diff --git a/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/skid_steer_controller.test b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/skid_steer_controller.test new file mode 100755 index 0000000..1c47937 --- /dev/null +++ b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/skid_steer_controller.test @@ -0,0 +1,14 @@ + + + + + + + + + + + diff --git a/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/skid_steer_no_wheels.test b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/skid_steer_no_wheels.test new file mode 100755 index 0000000..bb0d9bc --- /dev/null +++ b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/skid_steer_no_wheels.test @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + diff --git a/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/skidsteerbot.cpp b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/skidsteerbot.cpp new file mode 100755 index 0000000..05852f3 --- /dev/null +++ b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/skidsteerbot.cpp @@ -0,0 +1,93 @@ +/////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2013, PAL Robotics S.L. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of PAL Robotics, Inc. nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +////////////////////////////////////////////////////////////////////////////// + +// NOTE: The contents of this file have been taken largely from the ros_control wiki tutorials + +#include "diffbot.h" +#include +#include +#include +#include +#include + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "skidsteerbot"); + ros::NodeHandle nh; + + // This should be set in launch files as well + nh.setParam("/use_sim_time", true); + + Diffbot<6> robot; + ROS_WARN_STREAM("period: " << robot.getPeriod().toSec()); + controller_manager::ControllerManager cm(&robot, nh); + + ros::Publisher clock_publisher = nh.advertise("/clock", 1); + + //ros::Rate rate(1.0 / robot.getPeriod().toSec()); + ros::AsyncSpinner spinner(1); + spinner.start(); + + std::chrono::system_clock::time_point begin = std::chrono::system_clock::now(); + std::chrono::system_clock::time_point end = std::chrono::system_clock::now(); + + ros::Time internal_time(0); + const ros::Duration dt = robot.getPeriod(); + double elapsed_secs = 0; + + while(ros::ok()) + { + begin = std::chrono::system_clock::now(); + + robot.read(); + cm.update(internal_time, dt); + robot.write(); + + end = std::chrono::system_clock::now(); + + elapsed_secs = std::chrono::duration_cast >((end - begin)).count(); + + if (dt.toSec() - elapsed_secs < 0.0) + { + ROS_WARN_STREAM_THROTTLE( + 0.1, "Control cycle is taking to much time, elapsed: " << elapsed_secs); + } + else + { + ROS_DEBUG_STREAM_THROTTLE(1.0, "Control cycle is, elapsed: " << elapsed_secs); + std::this_thread::sleep_for(std::chrono::duration(dt.toSec() - elapsed_secs)); + } + + rosgraph_msgs::Clock clock; + clock.clock = ros::Time(internal_time); + clock_publisher.publish(clock); + internal_time += dt; + } + spinner.stop(); + + return 0; +} diff --git a/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/skidsteerbot.xacro b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/skidsteerbot.xacro new file mode 100755 index 0000000..f2b36ef --- /dev/null +++ b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/skidsteerbot.xacro @@ -0,0 +1,90 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Green + + + + Gazebo/Purple + + + diff --git a/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/skidsteerbot_controllers.yaml b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/skidsteerbot_controllers.yaml new file mode 100755 index 0000000..ab49424 --- /dev/null +++ b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/skidsteerbot_controllers.yaml @@ -0,0 +1,8 @@ +skidsteerbot_controller: + type: "diff_drive_controller/DiffDriveController" + left_wheel: ['wheel_0_joint', 'wheel_1_joint', 'wheel_2_joint'] + right_wheel: ['wheel_3_joint', 'wheel_4_joint', 'wheel_5_joint'] + publish_rate: 50.0 # defaults to 50 + pose_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0] + twist_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0] + cmd_vel_timeout: 20.0 # we test this separately, give plenty for the other tests diff --git a/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/skidsteerbot_no_wheels.yaml b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/skidsteerbot_no_wheels.yaml new file mode 100755 index 0000000..37f0d99 --- /dev/null +++ b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/skidsteerbot_no_wheels.yaml @@ -0,0 +1,8 @@ +skidsteerbot_controller: + type: "diff_drive_controller/DiffDriveController" + left_wheel: [] + right_wheel: ['wheel_3_joint', 'wheel_4_joint', 'wheel_5_joint'] + publish_rate: 50.0 # defaults to 50 + pose_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0] + twist_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0] + cmd_vel_timeout: 20.0 # we test this separately, give plenty for the other tests diff --git a/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/sphere_wheel.xacro b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/sphere_wheel.xacro new file mode 100755 index 0000000..bd8dd47 --- /dev/null +++ b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/sphere_wheel.xacro @@ -0,0 +1,43 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1 + 1 + + + + Gazebo/Red + + + diff --git a/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/square_wheel.xacro b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/square_wheel.xacro new file mode 100755 index 0000000..56680a0 --- /dev/null +++ b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/square_wheel.xacro @@ -0,0 +1,44 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1 + 1 + + + + Gazebo/Red + + + diff --git a/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/test_common.h b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/test_common.h new file mode 100755 index 0000000..a726d75 --- /dev/null +++ b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/test_common.h @@ -0,0 +1,162 @@ +/////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2013, PAL Robotics S.L. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of PAL Robotics, Inc. nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +////////////////////////////////////////////////////////////////////////////// + +/// \author Bence Magyar + +#pragma once + + +#include + +#include + +#include + +#include +#include +#include +#include + +#include + +// Floating-point value comparison threshold +const double EPS = 0.01; +const double POSITION_TOLERANCE = 0.01; // 1 cm-s precision +const double VELOCITY_TOLERANCE = 0.02; // 2 cm-s-1 precision +const double JERK_LINEAR_VELOCITY_TOLERANCE = 0.10; // 10 cm-s-1 precision +const double JERK_ANGULAR_VELOCITY_TOLERANCE = 0.05; // 3 deg-s-1 precision +const double ORIENTATION_TOLERANCE = 0.03; // 0.57 degree precision + +class DiffDriveControllerTest : public ::testing::Test +{ +public: + + DiffDriveControllerTest() + : received_first_odom(false) + , cmd_pub(nh.advertise("cmd_vel", 100)) + , odom_sub(nh.subscribe("odom", 100, &DiffDriveControllerTest::odomCallback, this)) + , vel_out_sub(nh.subscribe("cmd_vel_out", 100, &DiffDriveControllerTest::cmdVelOutCallback, this)) + , joint_traj_controller_state_sub(nh.subscribe("wheel_joint_controller_state", 100, &DiffDriveControllerTest::jointTrajectoryControllerStateCallback, this)) + , start_srv(nh.serviceClient("start")) + , stop_srv(nh.serviceClient("stop")) + { + } + + ~DiffDriveControllerTest() + { + odom_sub.shutdown(); + joint_traj_controller_state_sub.shutdown(); + } + + nav_msgs::Odometry getLastOdom(){ return last_odom; } + geometry_msgs::TwistStamped getLastCmdVelOut(){ return last_cmd_vel_out; } + control_msgs::JointTrajectoryControllerState getLastJointTrajectoryControllerState(){ return last_joint_traj_controller_state; } + void publish(geometry_msgs::Twist cmd_vel){ cmd_pub.publish(cmd_vel); } + bool isControllerAlive()const{ return (odom_sub.getNumPublishers() > 0); } + bool isPublishingCmdVelOut(const ros::Duration &timeout=ros::Duration(1)) const + { + ros::Time start = ros::Time::now(); + int get_num_publishers = vel_out_sub.getNumPublishers(); + while ( (get_num_publishers == 0) && (ros::Time::now() < start + timeout) ) { + ros::Duration(0.1).sleep(); + get_num_publishers = vel_out_sub.getNumPublishers(); + } + return (get_num_publishers > 0); + } + bool isPublishingJointTrajectoryControllerState(){ return (joint_traj_controller_state_sub.getNumPublishers() > 0); } + bool hasReceivedFirstOdom()const{ return received_first_odom; } + + void start(){ std_srvs::Empty srv; start_srv.call(srv); } + void stop(){ std_srvs::Empty srv; stop_srv.call(srv); } + + void waitForController() const + { + while(!isControllerAlive() && ros::ok()) + { + ROS_DEBUG_STREAM_THROTTLE(0.5, "Waiting for controller."); + ros::Duration(0.1).sleep(); + } + if (!ros::ok()) + FAIL() << "Something went wrong while executing test."; + } + + void waitForOdomMsgs() const + { + while(!hasReceivedFirstOdom() && ros::ok()) + { + ROS_DEBUG_STREAM_THROTTLE(0.5, "Waiting for odom messages to be published."); + ros::Duration(0.01).sleep(); + } + if (!ros::ok()) + FAIL() << "Something went wrong while executing test."; + } + +private: + bool received_first_odom; + ros::NodeHandle nh; + ros::Publisher cmd_pub; + ros::Subscriber odom_sub; + ros::Subscriber vel_out_sub; + nav_msgs::Odometry last_odom; + geometry_msgs::TwistStamped last_cmd_vel_out; + ros::Subscriber joint_traj_controller_state_sub; + control_msgs::JointTrajectoryControllerState last_joint_traj_controller_state; + + ros::ServiceClient start_srv; + ros::ServiceClient stop_srv; + + void odomCallback(const nav_msgs::Odometry& odom) + { + ROS_INFO_STREAM("Callback received: pos.x: " << odom.pose.pose.position.x + << ", orient.z: " << odom.pose.pose.orientation.z + << ", lin_est: " << odom.twist.twist.linear.x + << ", ang_est: " << odom.twist.twist.angular.z); + last_odom = odom; + received_first_odom = true; + } + + void jointTrajectoryControllerStateCallback(const control_msgs::JointTrajectoryControllerState& joint_traj_controller_state) + { + ROS_INFO_STREAM("Joint trajectory controller state callback."); + ROS_DEBUG_STREAM("Joint trajectory controller state callback received:\n" << + joint_traj_controller_state); + + last_joint_traj_controller_state = joint_traj_controller_state; + } + + void cmdVelOutCallback(const geometry_msgs::TwistStamped& cmd_vel_out) + { + ROS_INFO_STREAM("Callback received: lin: " << cmd_vel_out.twist.linear.x + << ", ang: " << cmd_vel_out.twist.angular.z); + last_cmd_vel_out = cmd_vel_out; + } +}; + +inline tf::Quaternion tfQuatFromGeomQuat(const geometry_msgs::Quaternion& quat) +{ + return tf::Quaternion(quat.x, quat.y, quat.z, quat.w); +} diff --git a/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/view_diffbot.launch b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/view_diffbot.launch new file mode 100755 index 0000000..212993f --- /dev/null +++ b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/view_diffbot.launch @@ -0,0 +1,26 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/view_skidsteerbot.launch b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/view_skidsteerbot.launch new file mode 100755 index 0000000..4460cee --- /dev/null +++ b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/view_skidsteerbot.launch @@ -0,0 +1,26 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/wheel.xacro b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/wheel.xacro new file mode 100755 index 0000000..1e8dd89 --- /dev/null +++ b/Controllers/Packages/robot_gazebo_plugins/diff_drive_controller/test/wheel.xacro @@ -0,0 +1,43 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1 + 1 + + + + Gazebo/Red + + + diff --git a/Controllers/Packages/robot_gazebo_plugins/ros_lift_controller/CMakeLists.txt b/Controllers/Packages/robot_gazebo_plugins/ros_lift_controller/CMakeLists.txt new file mode 100644 index 0000000..0248c8d --- /dev/null +++ b/Controllers/Packages/robot_gazebo_plugins/ros_lift_controller/CMakeLists.txt @@ -0,0 +1,146 @@ +cmake_minimum_required(VERSION 3.0.2) +project(ros_lift_controller) + +## Compile as C++11, supported in ROS Kinetic and newer +# add_compile_options(-std=c++11) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + gazebo_msgs + roscpp + rospy + gazebo_plugins + message_generation + std_msgs +) + +## System dependencies are found with CMake's conventions +find_package(gazebo REQUIRED) +find_package(Boost REQUIRED COMPONENTS thread) +find_package(Eigen3) +if(NOT EIGEN3_FOUND) + # Fallback to cmake_modules + find_package(cmake_modules REQUIRED) + find_package(Eigen REQUIRED) + set(EIGEN3_INCLUDE_DIRS ${EIGEN_INCLUDE_DIRS}) +else() + set(EIGEN3_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR}) +endif() + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ +## Generate messages in the 'msg' folder +add_message_files( + FILES + Lift_Measurement.msg +) + +## Generate added messages and services with any dependencies listed here +generate_messages( + DEPENDENCIES + std_msgs +) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( + INCLUDE_DIRS include + CATKIN_DEPENDS roscpp rospy std_msgs message_runtime + DEPENDS Boost +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} + ${GAZEBO_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIRS} +) + +## Declare a C++ library +add_library(${PROJECT_NAME} + src/ros_jack_lifter.cpp +) +add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +target_link_libraries(${PROJECT_NAME} + ${catkin_LIBRARIES} + ${Boost_LIBRARIES} + ${GAZEBO_LIBRARIES} +) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# catkin_install_python(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html +# install(TARGETS ${PROJECT_NAME}_node +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark libraries for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) + +## Mark cpp header files for installation +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE +) + +## Mark other files for installation (e.g. launch and bag files, etc.) +install(FILES + ros_lift_controller_plugins.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_ros_lift_controller.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/Controllers/Packages/robot_gazebo_plugins/ros_lift_controller/include/ros_lift_controller/ros_jack_lifter.h b/Controllers/Packages/robot_gazebo_plugins/ros_lift_controller/include/ros_lift_controller/ros_jack_lifter.h new file mode 100755 index 0000000..e37c244 --- /dev/null +++ b/Controllers/Packages/robot_gazebo_plugins/ros_lift_controller/include/ros_lift_controller/ros_jack_lifter.h @@ -0,0 +1,104 @@ +#ifndef GAZEBO_JACKING_LIFTER_PLUGIN_H_INCLUDED_ +#define GAZEBO_JACKING_LIFTER_PLUGIN_H_INCLUDED_ + +#include +// Gazebo +#include + +// ROS +#include +#include +#include +#include +#include +#include +#include +#include + +// Custom Callback Queue +#include +#include + +// Boost +#include +#include + +namespace gazebo +{ + // class Joint; + // class Entity; + + + class RosLiftLifter : public ModelPlugin + { + public: + RosLiftLifter(); + ~RosLiftLifter(); + void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf); + + protected: + virtual void UpdateChild(); + virtual void FiniChild(); + + private: + GazeboRosPtr gazebo_ros_; + physics::ModelPtr parent; + //void publishOdometry(double step_time); + void publishLiftSensorStates(); + void publishTF(physics::JointPtr joint); /// publishes the wheel tf's + void publishJointState(physics::JointPtr joint); + void effortController(physics::JointPtr joint, const double &error, const double &dt); + void positionController(physics::JointPtr joint, const double &target, double ¤t , const double &dt); + + event::ConnectionPtr update_connection_; + physics::JointPtr base_lifting_joint_, base_rotating_joint_; + double lift_torque_, rotate_torque_; + + std::string robot_namespace_; + std::string command_topic_; + std::string robot_base_frame_; + + // ROS STUFF + ros::Subscriber cmd_lift_subscriber_; + boost::shared_ptr transform_broadcaster_; + ros::Publisher joint_state_publisher_; + ros::Publisher state_lift_publisher_; + + boost::mutex lock; + + // Custom Callback Queue + ros::CallbackQueue queue_; + boost::thread callback_queue_thread_; + void QueueThread(); + + // DiffDrive stuff + void cmdCallback(const geometry_msgs::Pose::ConstPtr& cmd_msg); + /// updates the relative robot pose based on the wheel encoders + void UpdateForkEncoder(); + + double lift_height_cmd_, current_lift_position_, lift_step_; + double lift_rotate_cmd_, current_lift_angle_, rotate_step_; + double lift_height_maximum_; + + bool alive_; + double lift_height_encoder_; + common::Time last_encoder_update_; + double lift_height_origin_; + common::PID joint_lift_pid_, joint_rotate_pid_; + + // Update Rate + double update_rate_; + double update_period_; + common::Time last_actuator_update_; + + // Flags + bool publishTF_; + bool publishJointState_; + + // bool use_velocity_control_; + // double max_velocity_; + }; +} + +#endif + diff --git a/Controllers/Packages/robot_gazebo_plugins/ros_lift_controller/msg/Lift_Measurement.msg b/Controllers/Packages/robot_gazebo_plugins/ros_lift_controller/msg/Lift_Measurement.msg new file mode 100644 index 0000000..3e8fe1f --- /dev/null +++ b/Controllers/Packages/robot_gazebo_plugins/ros_lift_controller/msg/Lift_Measurement.msg @@ -0,0 +1,5 @@ +Header header + +float32 angle_position # Angle to the X axis's of base_link and X axis's of surface [rad] +bool top_sensor +bool bottom_sensor \ No newline at end of file diff --git a/Controllers/Packages/robot_gazebo_plugins/ros_lift_controller/package.xml b/Controllers/Packages/robot_gazebo_plugins/ros_lift_controller/package.xml new file mode 100644 index 0000000..8c5871a --- /dev/null +++ b/Controllers/Packages/robot_gazebo_plugins/ros_lift_controller/package.xml @@ -0,0 +1,90 @@ + + + ros_lift_controller + 0.0.0 + The ros_lift_controller package + + + + + robotics + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + message_generation + + + + + + message_runtime + + + + + catkin + + roscpp + rospy + gazebo8 + gazebo8_msgs + gazebo8_plugins + gazebo8_ros + orunav_generic + eigen + boost + std_msgs + + roscpp + rospy + gazebo8 + gazebo8_msgs + gazebo8_plugins + gazebo8_ros + orunav_generic + eigen + boost + std_msgs + + roscpp + rospy + gazebo8 + gazebo8_msgs + gazebo8_plugins + gazebo8_ros + orunav_generic + eigen + boost + std_msgs + + + + + + diff --git a/Controllers/Packages/robot_gazebo_plugins/ros_lift_controller/src/ros_jack_lifter.cpp b/Controllers/Packages/robot_gazebo_plugins/ros_lift_controller/src/ros_jack_lifter.cpp new file mode 100755 index 0000000..e79e3ae --- /dev/null +++ b/Controllers/Packages/robot_gazebo_plugins/ros_lift_controller/src/ros_jack_lifter.cpp @@ -0,0 +1,335 @@ +/** + * \file gazebo_ros_steer_drive.cpp + * \brief A fork lifter plugin for gazebo - taken from the old SAUNA repo. + * \author Henrik Andreasson + */ + + +#include +#include + +#include + + //#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace gazebo +{ + RosLiftLifter::RosLiftLifter() {} + + // Destructor + RosLiftLifter::~RosLiftLifter() {} + + // Load the controller + void RosLiftLifter::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf) + { + parent = _parent; + gazebo_ros_ = GazeboRosPtr(new GazeboRos(_parent, _sdf, "ForkLifter")); + // Make sure the ROS node for Gazebo has already been initialized + gazebo_ros_->isInitialized(); + + gazebo_ros_->getParameter(update_rate_, "updateRate", 100.0); + + gazebo_ros_->getParameter(lift_torque_, "liftTorque", 1000.0); + gazebo_ros_->getParameter(rotate_torque_, "rotateTorque", 1000.0); + gazebo_ros_->getParameter(lift_height_maximum_, "liftHeightMaximum", 0.1); + + gazebo_ros_->getParameter(robot_base_frame_, "robotBaseFrame", "base_link"); + + // Gazebo specific height to align the forks to the origin. + gazebo_ros_->getParameter(lift_height_origin_, "liftHeightOrigin", 0.08); + + gazebo_ros_->getParameterBoolean(publishTF_, "publishTF", false); + gazebo_ros_->getParameterBoolean(publishJointState_, "publishJointState", false); + + gazebo_ros_->getParameter(command_topic_, "commandTopic", "cmd_jacking"); + // gazebo_ros_->getParameterBoolean(use_velocity_control_, "useVelocityControl", true); + // gazebo_ros_->getParameter(max_velocity_, "maxVelocity", 0.08); + + gazebo_ros_->getParameter(lift_step_, "liftStep", 0.002); + gazebo_ros_->getParameter(rotate_step_, "rotateStep", 0.05); + + double pid_p, pid_i, pid_d; + gazebo_ros_->getParameter(pid_p, "pidP", 2000); + gazebo_ros_->getParameter(pid_i, "pidI", 0); + gazebo_ros_->getParameter(pid_d, "pidD", 10); + + base_lifting_joint_ = gazebo_ros_->getJoint(parent, "liftJoint", "base_lifting_joint"); + base_rotating_joint_ = gazebo_ros_->getJoint(parent, "rotateJoint", "base_rotating_joint"); +#if GAZEBO_MAJOR_VERSION > 2 + base_lifting_joint_->SetParam("fmax", 0, lift_torque_); + base_rotating_joint_->SetParam("fmax", 0, rotate_torque_); +#else + base_lifting_joint_->SetMaxForce(0, lift_torque_); + base_rotating_joint_->SetMaxForce(0, rotate_torque_); +#endif + + // Initialize update rate stuff + if (this->update_rate_ > 0.0) this->update_period_ = 1.0 / this->update_rate_; + else this->update_period_ = 0.0; + +#if GAZEBO_MAJOR_VERSION >= 8 + last_actuator_update_ = parent->GetWorld()->SimTime(); +#else + last_actuator_update_ = parent->GetWorld()->GetSimTime(); +#endif + + // Initialize velocity stuff + alive_ = true; + this->joint_lift_pid_.Init(pid_p, pid_i, pid_d, 100, -100, lift_torque_, -lift_torque_); + this->joint_rotate_pid_.Init(pid_p, pid_i, pid_d, 100, -100, rotate_torque_, -rotate_torque_); + + if (this->publishJointState_) { + joint_state_publisher_ = gazebo_ros_->node()->advertise("joint_states", 1000); + ROS_INFO("%s: Advertise joint_states!", gazebo_ros_->info()); + } + + transform_broadcaster_ = boost::shared_ptr(new tf::TransformBroadcaster()); + + // ROS: Subscribe to the velocity command topic (cmd_fork) + ROS_INFO("%s: Try to subscribe to %s!", gazebo_ros_->info(), command_topic_.c_str()); + + ros::SubscribeOptions so = + ros::SubscribeOptions::create(command_topic_, 1, + boost::bind(&RosLiftLifter::cmdCallback, this, _1), + ros::VoidPtr(), &queue_); + + cmd_lift_subscriber_ = gazebo_ros_->node()->subscribe(so); + ROS_INFO("%s: Subscribe to %s!", gazebo_ros_->info(), command_topic_.c_str()); + state_lift_publisher_ = gazebo_ros_->node()->advertise("lift_sensor_states", 100); + + // start custom queue for diff drive + this->callback_queue_thread_ = boost::thread(boost::bind(&RosLiftLifter::QueueThread, this)); + + // listen to the update event (broadcast every simulation iteration) + this->update_connection_ = event::Events::ConnectWorldUpdateBegin(boost::bind(&RosLiftLifter::UpdateChild, this)); + + lift_height_cmd_ = 0.0; + current_lift_position_ = 0.0; + lift_rotate_cmd_= 0.0; + current_lift_angle_ = 0.0; + } + + void RosLiftLifter::publishLiftSensorStates() + { + ros_lift_controller::Lift_Measurement lm; + lm.header.stamp = ros::Time::now(); + lm.header.frame_id = robot_base_frame_; + +#if GAZEBO_MAJOR_VERSION >= 8 + ignition::math::Pose3d pose_rotate = base_rotating_joint_->GetChild()->RelativePose(); +#else + ignition::math::Pose3d pose_rotate = base_rotating_joint_->GetChild()->GetRelativePose().Ign(); +#endif + + lm.angle_position = pose_rotate.Rot().Euler().Z(); + + lm.top_sensor = fabs(current_lift_position_ - lift_height_maximum_) < 0.01? true : false; + lm.bottom_sensor = fabs(current_lift_position_ - 0.0) < 0.01? true : false; + state_lift_publisher_.publish(lm); + } + + void RosLiftLifter::publishJointState(physics::JointPtr joint) + { + std::vector joints; + joints.push_back(joint); + + ros::Time current_time = ros::Time::now(); + sensor_msgs::JointState joint_state; + joint_state.header.stamp = current_time; + joint_state.name.resize(joints.size()); + joint_state.position.resize(joints.size()); + joint_state.velocity.resize(joints.size()); + joint_state.effort.resize(joints.size()); + for (std::size_t i = 0; i < joints.size(); i++) { + joint_state.name[i] = joints[i]->GetName(); +#if GAZEBO_MAJOR_VERSION >= 8 + joint_state.position[i] = joints[i]->Position(0); +#else + joint_state.position[i] = joints[i]->GetAngle(0).Radian(); +#endif + joint_state.velocity[i] = joints[i]->GetVelocity(0); + joint_state.effort[i] = joints[i]->GetForce(0); + } + joint_state_publisher_.publish(joint_state); + } + + void RosLiftLifter::publishTF(physics::JointPtr joint) + { + ros::Time current_time = ros::Time::now(); + std::vector joints; + joints.push_back(joint); + + for (std::size_t i = 0; i < joints.size(); i++) { + std::string frame = gazebo_ros_->resolveTF(joints[i]->GetName()); + std::string parent_frame = gazebo_ros_->resolveTF(joints[i]->GetParent()->GetName()); + +#if GAZEBO_MAJOR_VERSION >= 8 + ignition::math::Pose3d pose = joints[i]->GetChild()->RelativePose(); +#else + ignition::math::Pose3d pose = joints[i]->GetChild()->GetRelativePose().Ign(); +#endif + + tf::Quaternion qt(pose.Rot().X(), pose.Rot().Y(), pose.Rot().Z(), pose.Rot().W()); + tf::Vector3 vt(pose.Pos().X(), pose.Pos().Y(), pose.Pos().Z()); + + tf::Transform transform(qt, vt); + transform_broadcaster_->sendTransform(tf::StampedTransform(transform, current_time, parent_frame, frame)); + } + + } + // Update the controller + void RosLiftLifter::UpdateChild() + { + UpdateForkEncoder(); +#if GAZEBO_MAJOR_VERSION >= 8 + common::Time current_time = parent->GetWorld()->SimTime(); +#else + common::Time current_time = parent->GetWorld()->GetSimTime(); +#endif + + double seconds_since_last_update = (current_time - last_actuator_update_).Double(); + if (seconds_since_last_update > update_period_) { + + if (publishJointState_) + { + publishJointState(base_lifting_joint_); + publishJointState(base_rotating_joint_); + } + publishLiftSensorStates(); + +// Use the PID class... +#if GAZEBO_MAJOR_VERSION >= 8 + ignition::math::Pose3d pose_lift = base_lifting_joint_->GetChild()->RelativePose(); +#else + ignition::math::Pose3d pose_lift = base_lifting_joint_->GetChild()->GetRelativePose().Ign(); +#endif + double target_lift_height = lift_height_cmd_ + this->lift_height_origin_; + double current_lift_height = pose_lift.Pos().Z(); + double error_lift_height = current_lift_height - target_lift_height; + this->effortController(base_lifting_joint_, error_lift_height, seconds_since_last_update); + this->positionController(base_lifting_joint_, lift_height_cmd_, current_lift_position_, lift_step_); + +#if GAZEBO_MAJOR_VERSION >= 8 + ignition::math::Pose3d pose_rotate = base_rotating_joint_->GetChild()->RelativePose(); +#else + ignition::math::Pose3d pose_rotate = base_rotating_joint_->GetChild()->GetRelativePose().Ign(); +#endif + + double target_lift_yaw = lift_rotate_cmd_; + double current_lift_yaw = pose_rotate.Rot().Euler().Z(); + double error_lift_yaw = current_lift_yaw - target_lift_yaw; + this->effortController(base_rotating_joint_, error_lift_yaw, seconds_since_last_update); + this->positionController(base_rotating_joint_, lift_rotate_cmd_, current_lift_angle_, rotate_step_); + + last_actuator_update_ += common::Time(update_period_); + } + +#if 0 + if (odom_source_ == ENCODER) UpdateOdometryEncoder(); + common::Time current_time = parent->GetWorld()->GetSimTime(); + double seconds_since_last_update = (current_time - last_actuator_update_).Double(); + if (seconds_since_last_update > update_period_) { + + publishOdometry(seconds_since_last_update); + if (publishWheelTF_) publishWheelTF(); + if (publishWheelJointState_) publishWheelJointState(); + + double target_wheel_roation_speed = cmd_.speed / (wheel_diameter_ / 2.0); + double target_steering_angle_speed = cmd_.angle; + + motorController(target_wheel_roation_speed, target_steering_angle_speed, seconds_since_last_update); + + //ROS_INFO("v = %f, w = %f !", target_wheel_roation_speed, target_steering_angle); + + last_actuator_update_ += common::Time(update_period_); + } +#endif + } + + void RosLiftLifter::effortController(physics::JointPtr joint, const double &error, const double &dt) + { + double control_value = this->joint_lift_pid_.Update(error, dt); +#if GAZEBO_MAJOR_VERSION > 2 + joint->SetForce(0, control_value); +#else + joint->SetForce(0, control_value); +#endif + } + + void RosLiftLifter::positionController(physics::JointPtr joint, const double &target, double ¤t , const double &dt) + { + double error = target - current; + double step = dt; + // Nếu sai số lớn, tăng/giảm dần + if (std::abs(error) > step) + { + current += step * (error > 0 ? 1 : -1); + } + else + { + current = target; // gần bằng rồi, gán luôn + } + +#if GAZEBO_MAJOR_VERSION > 2 + joint->SetPosition(0, current); +#else + joint->SetPosition(0, current); +#endif + } + + // Finalize the controller + void RosLiftLifter::FiniChild() + { + alive_ = false; + queue_.clear(); + queue_.disable(); + gazebo_ros_->node()->shutdown(); + callback_queue_thread_.join(); + } + + void RosLiftLifter::cmdCallback(const geometry_msgs::Pose::ConstPtr& cmd_msg) + { + boost::mutex::scoped_lock scoped_lock(lock); + lift_height_cmd_ = std::min(lift_height_maximum_, cmd_msg->position.z); + lift_rotate_cmd_ = tf::getYaw(cmd_msg->orientation); + } + + void RosLiftLifter::QueueThread() + { + static const double timeout = 0.01; + + while (alive_ && gazebo_ros_->node()->ok()) { + queue_.callAvailable(ros::WallDuration(timeout)); + } + } + + void RosLiftLifter::UpdateForkEncoder() + { +#if GAZEBO_MAJOR_VERSION >= 8 + common::Time current_time = parent->GetWorld()->SimTime(); +#else + common::Time current_time = parent->GetWorld()->GetSimTime(); +#endif + double step_time = (current_time - last_encoder_update_).Double(); + last_encoder_update_ = current_time; +#if GAZEBO_MAJOR_VERSION >= 8 + ignition::math::Pose3d pose = base_lifting_joint_->GetChild()->RelativePose(); +#else + ignition::math::Pose3d pose = base_lifting_joint_->GetChild()->GetRelativePose().Ign(); +#endif + lift_height_encoder_ = pose.Pos().Z(); + } + GZ_REGISTER_MODEL_PLUGIN(RosLiftLifter) +} + diff --git a/Controllers/auto_setup.sh b/Controllers/auto_setup.sh new file mode 100755 index 0000000..5e9cadb --- /dev/null +++ b/Controllers/auto_setup.sh @@ -0,0 +1,39 @@ +sudo apt-get update +sudo apt-get install libmosquitto-dev +sudo apt-get install libeigen3-dev + +#1.1 Build OPCUA +open62541_zip_file="open62541-pack-master" +unzip Libraries/Systems/$open62541_zip_file.zip -d Libraries/Systems/ +mv Libraries/Systems/$open62541_zip_file Libraries/Systems/open62541 +mkdir -p Libraries/Systems/open62541/build +cd Libraries/Systems/open62541/build +cmake .. +make +sudo make install +cd ../../../.. +rm -fr Libraries/Systems/open62541 + +#1.2 Build JSON +json_zip_file="json-3.12.0" +unzip Libraries/Systems/$json_zip_file.zip -d Libraries/Systems/ +mv Libraries/Systems/$json_zip_file Libraries/Systems/json +mkdir -p Libraries/Systems/json/build +cd Libraries/Systems/json/build +cmake .. +make +sudo make install +cd ../../../.. +rm -fr Libraries/Systems/json + +#1.2 Build Yaml-cpp +yaml_cpp_zip_file="yaml-cpp-yaml-cpp-0.6.0" +unzip Libraries/Systems/$yaml_cpp_zip_file.zip -d Libraries/Systems/ +mv Libraries/Systems/$yaml_cpp_zip_file Libraries/Systems/yaml-cpp +mkdir -p Libraries/Systems/yaml-cpp/build +cd Libraries/Systems/yaml-cpp/build +cmake .. +make +sudo make install +cd ../../../.. +rm -fr Libraries/Systems/yaml-cpp \ No newline at end of file diff --git a/Devices/Cores/models/.gitignore b/Devices/Cores/models/.gitignore new file mode 100755 index 0000000..8c2b884 --- /dev/null +++ b/Devices/Cores/models/.gitignore @@ -0,0 +1,14 @@ +# ---> VisualStudioCode +.vscode/* +!.vscode/settings.json +!.vscode/tasks.json +!.vscode/launch.json +!.vscode/extensions.json +!.vscode/*.code-snippets + +# Local History for Visual Studio Code +.history/ + +# Built Visual Studio Code Extensions +*.vsix + diff --git a/Devices/Cores/models/CMakeLists.txt b/Devices/Cores/models/CMakeLists.txt new file mode 100755 index 0000000..d2889ea --- /dev/null +++ b/Devices/Cores/models/CMakeLists.txt @@ -0,0 +1,198 @@ +cmake_minimum_required(VERSION 3.0.2) +project(models) + +## Compile as C++11, supported in ROS Kinetic and newer +add_compile_options(-std=c++17) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + geometry_msgs + roscpp +) + +## System dependencies are found with CMake's conventions +find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a exec_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# geometry_msgs +# ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within th +# )s +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( + INCLUDE_DIRS include +# LIBRARIES models + CATKIN_DEPENDS geometry_msgs roscpp + DEPENDS Boost +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( + include + ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +# add_library(${PROJECT_NAME} +# src/${PROJECT_NAME}/models.cpp +# ) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +## With catkin_make all packages are built within a single CMake context +## The recommended prefix ensures that target names across p +# ) +## same as for the library above +# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +# target_link_libraries(${PROJECT_NAME}_node +# ${catkin_LIBRARIES} +# ) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# catkin_install_python(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html +# install(TARGETS ${PROJECT_NAME}_node +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark libraries for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html +# install(TARGETS ${PROJECT_NAME} +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE +) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_models.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/Devices/Cores/models/README.md b/Devices/Cores/models/README.md new file mode 100755 index 0000000..355ba4b --- /dev/null +++ b/Devices/Cores/models/README.md @@ -0,0 +1,3 @@ +# models + +Chứa các base interface để tải vào các objects \ No newline at end of file diff --git a/Devices/Cores/models/include/models/BaseAbsoluteEncoder.h b/Devices/Cores/models/include/models/BaseAbsoluteEncoder.h new file mode 100755 index 0000000..925f91d --- /dev/null +++ b/Devices/Cores/models/include/models/BaseAbsoluteEncoder.h @@ -0,0 +1,43 @@ +#ifndef __ABSOLUTE_ENCODER_H_INCLUDED +#define __ABSOLUTE_ENCODER_H_INCLUDED + +#include + +namespace models +{ +class BaseAbsoluteEncoder +{ +public: + + /** + * @brief Constructs + * @param name The name to give this instance of the model base + */ + virtual void initialize(ros::NodeHandle & nh, const std::string &name) = 0; + + /** + * @brief Get the angle of rotation of an axis() rad + */ + virtual double Position() = 0; + + /** + * @brief Get the velocity rotation of an axis() rad + */ + virtual double Velocity() = 0; + + /** + * @brief Decontructor + */ + virtual ~BaseAbsoluteEncoder() {} + +protected: + /** + * @brief Contructor + */ + BaseAbsoluteEncoder() {} + +}; // BaseAbsoluteEncoder + +} // namespace models + +#endif //__ABSOLUTE_ENCODER_H_INCLUDED \ No newline at end of file diff --git a/Devices/Cores/models/include/models/BaseSensorGroups.h b/Devices/Cores/models/include/models/BaseSensorGroups.h new file mode 100755 index 0000000..e32c387 --- /dev/null +++ b/Devices/Cores/models/include/models/BaseSensorGroups.h @@ -0,0 +1,48 @@ +#ifndef __BASE_SENSOR_GROUPS_H_INCLUDED_ +#define __BASE_SENSOR_GROUPS_H_INCLUDED_ + +namespace models +{ + enum SensorState + { + Normal, + Warn, + Error, + NotActive, + }; + +class BaseSensorGroups +{ +public: + /** + * @brief Constructs + * @param name The name to give this instance of the model base + */ + virtual void initialize(ros::NodeHandle & nh, const std::string &name) = 0; + + /** + * @brief Get state from sensors + */ + virtual models::SensorState State() = 0; + + /** + * @brief Get bit logic get from sensor + */ + virtual bool Get() = 0; + + /** + * @brief Decontructor + */ + virtual ~BaseSensorGroups() {} + +protected: + /** + * @brief Contructor + */ + BaseSensorGroups() {} + +}; // class BaseSensorGroups + +} // namespace models + +#endif \ No newline at end of file diff --git a/Devices/Cores/models/include/models/BaseSteerDrive.h b/Devices/Cores/models/include/models/BaseSteerDrive.h new file mode 100755 index 0000000..039f6a0 --- /dev/null +++ b/Devices/Cores/models/include/models/BaseSteerDrive.h @@ -0,0 +1,105 @@ +#ifndef __STEER_DRIVE_H_INCLUDED +#define __STEER_DRIVE_H_INCLUDED + +#include + +namespace models +{ +class BaseSteerDrive +{ +public: + + /** + * @brief Constructs + * @param name The name to give this instance of the model base + */ + virtual void initialize(ros::NodeHandle & nh, const std::string &name) = 0; + + /** + * @brief Constructs + * @param name Check model is deady + * @return true, if model is ready, false otherwise + */ + virtual bool Ready() {return true;} + + + /** + * @brief Get the velocity limit on axis(index). + * @return Velocity limit specified in SDF + */ + virtual double GetVelocityLimit(unsigned int _index) = 0; + + /** + * @brief Set the velocity limit on a joint axis. + * @param[in] _velocity Velocity limit for the axis. + */ + virtual void SetVelocityLimit(double _velocity) = 0; + + /** + * @brief Get the rotation rate of an axis(index) + * @return The rotaional velocity of the joint axis. + */ + virtual double GetVelocity() = 0; + + /** + * @brief Set the velocity of an axis(index). + * In ODE and Bullet, the SetVelocityMaximal function is used to + * set the velocity of the child link relative to the parent. + * In Simbody and DART, this function updates the velocity state, + * which has a recursive effect on the rest of the chain. + * @param[in] _vel Velocity. + */ + virtual void SetVelocity(double _vel) = 0; + + /** + * @brief Get the position of an axis according to its index. + * + * For rotational axes, the value is in radians. For prismatic axes, + * it is in meters. + * + * For static models, it returns the static joint position. + * + * It returns ignition::math::NAN_D in case the position can't be + * obtained. For instance, if the index is invalid, if the joint is + * fixed, etc. + * + * Subclasses can't override this method. See PositionImpl instead. + * + * @return Current position of the axis. + * @sa PositionImpl + */ + virtual double Position() = 0; + + /** + * @brief The child links of this joint are updated based on desired + * position. And all the links connected to the child link of this joint + * except through the parent link of this joint moves with the child + * link. + * @param[in] _position Position to set the joint to. + * unspecified, pure kinematic teleportation. + * link with respect to the world frame will remain the same after + * setting the position. By default this is false, which means there are + * no guarantees about what the child link's world velocity will be after + * the position is changed (the behavior is determined by the underlying + * physics engine). + * + * @note{Only ODE and Bullet support _preserveWorldVelocity being true.} + */ + virtual void SetPosition(const double _position) = 0; + + /** + * @brief Decontructor + */ + virtual ~BaseSteerDrive() {} + +protected: + /** + * @brief Contructor + */ + BaseSteerDrive() {} + +}; // class BaseSteerDrive + +} // namespace models + +#endif // __STEER_DRIVE_H_INCLUDED \ No newline at end of file diff --git a/Devices/Cores/models/include/models/ModelsTypes.h b/Devices/Cores/models/include/models/ModelsTypes.h new file mode 100755 index 0000000..e144b0c --- /dev/null +++ b/Devices/Cores/models/include/models/ModelsTypes.h @@ -0,0 +1,53 @@ +#ifndef __MODELS_TYPES_H_INCLUDED +#define __MODELS_TYPES_H_INCLUDED + +#include +#include +/** + * @brief models forward declarations and type defines +*/ +namespace models +{ + class BaseSteerDrive; + class BaseAbsoluteEncoder; + class BaseSensorGroups; + + /** + * @def BaseSteerDrivePtr + * @brief Boost shared pointer to a BaseSteerDrive object + */ + typedef boost::shared_ptr BaseSteerDrivePtr; + + /** + * @def BaseAbsoluteEncoderPtr + * @brief Boost shared pointer to a BaseAbsoluteEncoder object + */ + typedef boost::shared_ptr BaseAbsoluteEncoderPtr; + + /** + * @def BaseSensorGroupsPtr + * @brief Boost shared pointer to a BaseSensorGroups object + */ + typedef boost::shared_ptr BaseSensorGroupsPtr; + + /** + * @def BaseSteerDrive_V + * @brief Vector of BaseSteerDrivePtr + */ + typedef std::vector BaseSteerDrive_V; + + /** + * @def BaseAbsoluteEncoder_V + * @brief Vector of BaseAbsoluteEncoderPtr + */ + typedef std::vector BaseAbsoluteEncoder_V; + + /** + * @def BaseSensorGroups_V + * @brief Vector of BaseSensorGroupsPtr + */ + typedef std::vector BaseSensorGroups_V; + +} // namespace models + +#endif // MODELS_TYPES_H_INCLUDED \ No newline at end of file diff --git a/Devices/Cores/models/package.xml b/Devices/Cores/models/package.xml new file mode 100755 index 0000000..5071cf4 --- /dev/null +++ b/Devices/Cores/models/package.xml @@ -0,0 +1,65 @@ + + + models + 0.0.0 + The models package + + + + + robotics + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + geometry_msgs + roscpp + geometry_msgs + roscpp + geometry_msgs + roscpp + + + + + + + + diff --git a/Devices/Libraries/Ros/delta_modbus/CMakeLists.txt b/Devices/Libraries/Ros/delta_modbus/CMakeLists.txt new file mode 100755 index 0000000..20ee94a --- /dev/null +++ b/Devices/Libraries/Ros/delta_modbus/CMakeLists.txt @@ -0,0 +1,85 @@ +cmake_minimum_required(VERSION 3.0.2) +project(delta_modbus) + +find_package(catkin REQUIRED COMPONENTS + roscpp + std_msgs + modbus_tcp + diagnostic_msgs +) + +find_package(Boost REQUIRED COMPONENTS system) + +catkin_package( + INCLUDE_DIRS include + LIBRARIES ${PROJECT_NAME} + CATKIN_DEPENDS roscpp std_msgs modbus_tcp diagnostic_msgs + DEPENDS Boost +) + +include_directories( + include + ${catkin_INCLUDE_DIRS} +) + +add_library(${PROJECT_NAME} src/delta_modbus_tcp.cpp) +add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${GSTREAMER_LIBRARIES}) + +add_executable(test_delta test/test_delta.cpp) +add_dependencies(test_delta ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(test_delta ${PROJECT_NAME} ${catkin_LIBRARIES}) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# catkin_install_python(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html +# install(TARGETS test_delta +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark libraries for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) + +## Mark cpp header files for installation +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE +) + +## Mark other files for installation (e.g. launch and bag files, etc.) +install(FILES + launch/test_delta.launch + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_delta_modbus_tcp.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/Devices/Libraries/Ros/delta_modbus/include/delta_modbus/delta_exception.h b/Devices/Libraries/Ros/delta_modbus/include/delta_modbus/delta_exception.h new file mode 100755 index 0000000..e981de1 --- /dev/null +++ b/Devices/Libraries/Ros/delta_modbus/include/delta_modbus/delta_exception.h @@ -0,0 +1,53 @@ +#ifndef __DELTA_EXCEPTION_H_INCLUDE +#define __DELTA_EXCEPTION_H_INCLUDE +#include +#include +using namespace std; + +class CompareException : public std::exception { +public: + const char * what () const throw () { + return "The first Address value must be less than the second Address value"; + } +}; + +class AddressIsNotExistException : public std::exception { + int Address; +public: + AddressIsNotExistException(int Address) { + this->Address = Address; + } + + const char * what () const throw () { + std::stringstream msg; + msg << "The Addtess 0x" << Address << " is not Exist"; + return msg.str().c_str(); + } +}; + +class OutRangeException : public std::exception { + int Maximum; +public: + OutRangeException(int Maximum) { + this->Maximum = Maximum; + } + + const char * what () const throw () { + std::stringstream msg; + msg << "Starting address must be 0 - 65535; quantity must be 0 - " << Maximum; + return msg.str().c_str(); + } +}; + +class Exception : public std::exception { + int Maximum; +public: + std::string msg; + Exception(std::string msg) { + this->msg = msg; + } + const char * what () const throw () { + return msg.c_str(); + } +}; +#endif diff --git a/Devices/Libraries/Ros/delta_modbus/include/delta_modbus/delta_infomation.h b/Devices/Libraries/Ros/delta_modbus/include/delta_modbus/delta_infomation.h new file mode 100755 index 0000000..f32bc94 --- /dev/null +++ b/Devices/Libraries/Ros/delta_modbus/include/delta_modbus/delta_infomation.h @@ -0,0 +1,89 @@ +#ifndef __DELTA_INFOMATION_H_INCLUDE_ +#define __DELTA_INFOMATION_H_INCLUDE_ + +#define PLC_RUN_OK 0X00 +#define NOT_CONNECT_TO_PLC 0X01 +#define PLC_NOT_RUN 0X02 + +#define DELTA_NAMESPACE_BEGIN namespace delta { +#define DELTA_NAMESPACE delta +#define DELTA_NAMESPACE_END } + +#define ON true +#define OFF false +#define TIME_ERROR 5 + +#define S0_HEX 0x0001 +#define S1023_HEX 0x03FF +#define S0_NUM 0 +#define S1023 1023 + +#define X0_HEX 0x0400 +#define X377_HEX 0x04FF +#define X0_NUM 0 +#define X377_NUM 377 + +#define Y0_HEX 0x0500 +#define Y377_HEX 0x05FF +#define Y0_NUM 0 +#define Y377_NUM 377 + +#define M0_HEX 0x0800 +#define M1535_HEX 0x0DFF +#define M0_NUM 0 +#define M1535_NUM 1535 + +#define M1536_HEX 0xB000 +#define M4095_HEX 0xB9FF +#define M1536_NUM 1536 +#define M4095_NUM 4095 + +#define C0_HEX 0x0E00 +#define C199_HEX 0x0EC7 +#define C0_NUM 0 +#define C199_NUM 199 + +#define D0_HEX 0x1000 +#define D4095_HEX 0x1FFF +#define D0_NUM 0 +#define D4095_NUM 4095 + +#define READ_M_BITS 0x01 +#define READ_D_REGS 0x02 +#define READ_Y_BITS 0x03 +#define READ_X_BITS 0x04 +#define WRITE_M_BITS 0x05 +#define WRITE_D_REGS 0x06 +#define WRITE_Y_BITS 0x0F + +enum DO_ { + Y0 = 0 ,Y1 = 1 ,Y2 = 2 ,Y3 = 3 ,Y4 = 4 ,Y5 = 5 ,Y6 = 6 ,Y7 = 7 , + Y10 = 8 ,Y11 = 9 ,Y12 = 10 ,Y13 = 11 ,Y14 = 12 ,Y15 = 13 ,Y16 = 14 ,Y17 = 15 , + Y20 = 16 ,Y21 = 17 ,Y22 = 18 ,Y23 = 19 ,Y24 = 20 ,Y25 = 21 ,Y26 = 22 ,Y27 = 23 , + Y30 = 24 ,Y31 = 25 ,Y32 = 26 ,Y33 = 27 ,Y34 = 28 ,Y35 = 29 ,Y36 = 30 ,Y37 = 31 , + Y40 = 32 ,Y41 = 33 ,Y42 = 34 ,Y43 = 35 ,Y44 = 36 ,Y45 = 37 ,Y46 = 38 ,Y47 = 39 , + Y50 = 40 ,Y51 = 41 ,Y52 = 42 ,Y53 = 43 ,Y54 = 44 ,Y55 = 45 ,Y56 = 46 ,Y57 = 47 , + Y60 = 48 ,Y61 = 49 ,Y62 = 50 ,Y63 = 51 ,Y64 = 52 ,Y65 = 53 ,Y66 = 54 ,Y67 = 55 , + Y70 = 56 ,Y71 = 57 ,Y72 = 58 ,Y73 = 59 ,Y74 = 60 ,Y75 = 61 ,Y76 = 62 ,Y77 = 63 , + Y100 = 64 ,Y101 = 65 ,Y102 = 66 ,Y103 = 67 ,Y104 = 68 ,Y105 = 69 ,Y106 = 70 ,Y107 = 71 , + Y110 = 72 ,Y111 = 73 ,Y112 = 74 ,Y113 = 75 ,Y114 = 76 ,Y115 = 77 ,Y116 = 78 ,Y117 = 79 , + Y120 = 80 ,Y121 = 81 ,Y122 = 82 ,Y123 = 83 ,Y124 = 84 ,Y125 = 85 ,Y126 = 86 ,Y127 = 87 , + Y130 = 88 ,Y131 = 89 ,Y132 = 90 ,Y133 = 91 ,Y134 = 92 ,Y135 = 93 ,Y136 = 94 ,Y137 = 95 +}; + +enum DI_ { + X0 = 0 ,X1 = 1 ,X2 = 2 ,X3 = 3 ,X4 = 4 ,X5 = 5 ,X6 = 6 ,X7 = 7 , + X10 = 8 ,X11 = 9 ,X12 = 10 ,X13 = 11 ,X14 = 12 ,X15 = 13 ,X16 = 14 ,X17 = 15 , + X20 = 16 ,X21 = 17 ,X22 = 18 ,X23 = 19 ,X24 = 20 ,X25 = 21 ,X26 = 22 ,X27 = 23 , + X30 = 24 ,X31 = 25 ,X32 = 26 ,X33 = 27 ,X34 = 28 ,X35 = 29 ,X36 = 30 ,X37 = 31 , + X40 = 32 ,X41 = 33 ,X42 = 34 ,X43 = 35 ,X44 = 36 ,X45 = 37 ,X46 = 38 ,X47 = 39 , + X50 = 40 ,X51 = 41 ,X52 = 42 ,X53 = 43 ,X54 = 44 ,X55 = 45 ,X56 = 46 ,X57 = 47 , + X60 = 48 ,X61 = 49 ,X62 = 50 ,X63 = 51 ,X64 = 52 ,X65 = 53 ,X66 = 54 ,X67 = 55 , + X70 = 56 ,X71 = 57 ,X72 = 58 ,X73 = 59 ,X74 = 60 ,X75 = 61 ,X76 = 62 ,X77 = 63 , + X100 = 64 ,X101 = 65 ,X102 = 66 ,X103 = 67 ,X104 = 68 ,X105 = 69 ,X106 = 70 ,X107 = 71 , + X110 = 72 ,X111 = 73 ,X112 = 74 ,X113 = 75 ,X114 = 76 ,X115 = 77 ,X116 = 78 ,X117 = 79 , + X120 = 80 ,X121 = 81 ,X122 = 82 ,X123 = 83 ,X124 = 84 ,X125 = 85 ,X126 = 86 ,X127 = 87 , + X130 = 88 ,X131 = 89 ,X132 = 90 ,X133 = 91 ,X134 = 92 ,X135 = 93 ,X136 = 94 ,X137 = 95 +}; + +#endif diff --git a/Devices/Libraries/Ros/delta_modbus/include/delta_modbus/delta_modbus_tcp.h b/Devices/Libraries/Ros/delta_modbus/include/delta_modbus/delta_modbus_tcp.h new file mode 100755 index 0000000..8b09487 --- /dev/null +++ b/Devices/Libraries/Ros/delta_modbus/include/delta_modbus/delta_modbus_tcp.h @@ -0,0 +1,216 @@ +#ifndef __DELTA_LIBRARY_H_INCLUDED +#define __DELTA_LIBRARY_H_INCLUDED +#include +#include +#include "modbus_tcp/modbus.h" +#include +#include "delta_modbus/delta_infomation.h" +#include "delta_modbus/delta_exception.h" + + +DELTA_NAMESPACE_BEGIN +class PLC +{ +public: + PLC(std::string ip_address, int port, int id = 1); + + virtual ~PLC(); + + /** + * @brief Connect to PLC + */ + void connect(void); + + /** + * @brief Close to PLC + */ + void close(void); + + /** + * @brief Checking PLC is connected + */ + bool checkConnected(void); + + /** + * @brief Write ON/OFF M Bit + * @param M_number M bit + * @return 0/-1 + */ + int writeM(int M_number, const bool status); + + + /** + * @brief Set ON/OFF multiple M bit + * @param start_number begin M bit + * @param end_number end M bit + * @return 0/-1 + */ + int mulWriteM(int start_number, int end_number, bool *status); + + + /** + * @brief SET ON M Bit + * @param M_number M bit + * @return 0/-1 + */ + int setM(int M_number); + + /** + * @brief SET OFF M Bit + * @param M_number Node handle to get param for PLC + * @return 0/-1 + */ + int resetM(int M_number); + + /** + * @brief Set ON multiple M bit + * @param start_number begin M bit + * @param end_number end M bit + * @return 0/-1 + */ + int mulSetM(int start_number, int end_number); + + /** + * @brief Set OFF multiple M bit + * @param start_number begin M bit + * @param end_number end M bit + * @return 0/-1 + */ + int mulResetM(int start_number, int end_number); + + /** + * @brief Get load M Bit + * @param M_number M bit + * @param buffer Status of M bit + * @return 0/-1 + */ + int getM(int M_number, bool &buffer); + + /** + * @brief Get load mul M bit + * @param start_number Begin M bit + * @param end_number End M bit + * @param buffer Status of M bit + * @return 0/-1 + */ + int mulGetM(int start_number, int end_number, bool* buffer); + + /** + * @brief Set On output Y + * @param Y_number + * @return 0/-1 + */ + int setY(int Y_number); + + /** + * @brief Set Off output Y + * @param Y_number + * @return 0/-1 + */ + int resetY(int Y_number); + + /** + * @brief Set multiple output Y is ON/OFF + * @param start_number begin M bit + * @param end_number end M bit + * @return 0/-1 + */ + int mulWriteY(int start_number, int end_number, bool *buffer); + + /** + * @brief Set multiple output Y is ON + * @param start_number begin M bit + * @param end_number end M bit + * @return 0/-1 + */ + int mulSetY(int start_number, int end_number); + + /** + * Set multiple output Y is OFF + * @param start_number begin M bit + * @param end_number end M bit + * @return 0/-1 + */ + int mulResetY(int start_number, int end_number); + + /** + * @brief Get status Y coils from start_number to end_number. + * @param start_number begin M bit + * @param end_number end M bit + * @param buffer The status bit coils add to buffer + */ + int mulGetY(int start_number, int end_number, bool *buffer); + + /** + * @brief Read D register + * @param D_number Number D + * @param buffer Value of D_number + */ + int getD(int D_number, uint16_t &buffer); + + /** + * @brief Send data to D register + * @param D_number Number D + * @param buffer Value of D_number + */ + int mulGetD(int start_number, int end_number, uint16_t *buffer); + + /** + * @brief Send data to D register () + * @param start_number Begin D register + * @param end_number End D register + * @param buffer Buffer to store Data Read from Input Bits + */ + int setD(int D_number, int16_t &buffer); + + /** + * @brief Send data to D register () + * @param start_number Begin D register + * @param end_number End D register + * @param buffer Buffer to store Data Read from Input Bits + */ + int mulSetD(int start_number, int end_number, uint16_t *buffer); + + /** + * @brief Read X bit + * @param X_number Number X + * @param buffer Status of X + */ + int getX(int X_number, bool &buffer); + + /** + * @brief Read X bit + * @param start_number Begin number X + * @param end_number End number X + * @param buffer Status of X + */ + int mulGetX(int start_number, int end_number, bool* buffer); + + /** + * @brief Read C register + * @param C_number Number C + * @param buffer Value of C_number + */ + int getC(int C_number, uint16_t &buffer); + + /** + * @brief Send data to C register () + * @param start_number Begin C register + * @param end_number End C register + * @param buffer Buffer to store Data Read from Input Bits + */ + int mulGetC(int start_number, int end_number, uint16_t *buffer); + + /** + * @brief get error information + */ + inline uint8_t getError(); + +private: + /* Properties */ + uint8_t error_num; + boost::shared_ptr mb_; +}; +DELTA_NAMESPACE_END + +#endif diff --git a/Devices/Libraries/Ros/delta_modbus/launch/test_delta.launch b/Devices/Libraries/Ros/delta_modbus/launch/test_delta.launch new file mode 100755 index 0000000..75a71a2 --- /dev/null +++ b/Devices/Libraries/Ros/delta_modbus/launch/test_delta.launch @@ -0,0 +1,9 @@ + + + + + + + + + \ No newline at end of file diff --git a/Devices/Libraries/Ros/delta_modbus/package.xml b/Devices/Libraries/Ros/delta_modbus/package.xml new file mode 100755 index 0000000..6938f80 --- /dev/null +++ b/Devices/Libraries/Ros/delta_modbus/package.xml @@ -0,0 +1,73 @@ + + + delta_modbus + 0.0.0 + The delta_modbus_tcp package + + + + + robotics + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + roscpp + roscpp + roscpp + + std_msgs + modbus_tcp + diagnostic_msgs + + std_msgs + modbus_tcp + diagnostic_msgs + + std_msgs + modbus_tcp + diagnostic_msgs + + + + + + + diff --git a/Devices/Libraries/Ros/delta_modbus/plc_config.yaml b/Devices/Libraries/Ros/delta_modbus/plc_config.yaml new file mode 100755 index 0000000..1b5c755 --- /dev/null +++ b/Devices/Libraries/Ros/delta_modbus/plc_config.yaml @@ -0,0 +1,3 @@ +IP: 192.168.2.100 # Địa chỉ HOST của thiết bị cua PLC +PORT: 502 # Cổng PORT kết nối vớt PLC +ID: 1 # Số ID của thiết bị mặc định là 1 \ No newline at end of file diff --git a/Devices/Libraries/Ros/delta_modbus/src/delta_modbus_tcp.cpp b/Devices/Libraries/Ros/delta_modbus/src/delta_modbus_tcp.cpp new file mode 100755 index 0000000..5bed71f --- /dev/null +++ b/Devices/Libraries/Ros/delta_modbus/src/delta_modbus_tcp.cpp @@ -0,0 +1,866 @@ +#include "delta_modbus/delta_modbus_tcp.h" +#include + +DELTA_NAMESPACE::PLC::PLC(std::string ip_address, int port, int id) + : mb_(nullptr) +{ + mb_ = boost::make_shared(ip_address, port); + mb_->modbus_set_slave_id(id); +} + +DELTA_NAMESPACE::PLC::~PLC() +{ + mb_ = nullptr; +} + +void DELTA_NAMESPACE::PLC::connect(void) { + ROS_INFO("Delta PLC is connecting......."); + try + { + if(!mb_->modbus_connect()) { + ROS_ERROR("Delta PLC connection is error"); + error_num = NOT_CONNECT_TO_PLC; + } else { + ros::Duration(0.5).sleep(); + error_num = PLC_RUN_OK; + ROS_INFO("Delta PLC is Connected"); + } + } + catch(const boost::exception& e) + { + ROS_ERROR_STREAM(boost::diagnostic_information(e)); + } + catch (const std::exception& e) + { + ROS_ERROR_STREAM(e.what()); + } + catch (...) + { + ROS_ERROR_STREAM("Caught unknown exception"); + } +} + +void DELTA_NAMESPACE::PLC::close() +{ + try + { + mb_->modbus_close(); + } + catch(const boost::exception& e) + { + ROS_ERROR_STREAM(boost::diagnostic_information(e)); + } + catch (const std::exception& e) + { + ROS_ERROR_STREAM(e.what()); + } + catch (...) + { + ROS_ERROR_STREAM("Caught unknown exception"); + } +} + +bool DELTA_NAMESPACE::PLC::checkConnected() +{ + try + { + return mb_->_connected; + } + catch (const std::exception& e) + { + ROS_ERROR_STREAM(e.what()); + } + catch(const boost::exception& e) + { + ROS_ERROR_STREAM(boost::diagnostic_information(e)); + } + catch (...) + { + ROS_ERROR_STREAM("Caught unknown exception"); + } +} + +int DELTA_NAMESPACE::PLC::writeM(int M_number, const bool status) { + try { + if(M_number < M0_NUM || M_number > M4095_NUM) { throw AddressIsNotExistException(M_number); } + if(M_number >= M0_NUM && M_number <= M1535_NUM) { + return mb_->modbus_write_coil(M0_HEX + M_number, status); + } + else if(M_number >= M1536_NUM && M_number <= M4095_NUM) { + return mb_->modbus_write_coil(M1536_HEX + M_number, status); + } + } + catch(AddressIsNotExistException & e) { + ROS_ERROR_STREAM("delta::writeM: " << e.what()); + ros::Duration(TIME_ERROR).sleep(); + return BAD_CON; + } + catch(std::exception& e) { + ROS_ERROR_STREAM("delta::writeM: " << e.what()); + ros::Duration(TIME_ERROR).sleep(); + return BAD_CON; + } + catch(const boost::exception& e) + { + ROS_ERROR_STREAM(boost::diagnostic_information(e)); + } + catch (...) + { + ROS_ERROR_STREAM("Caught unknown exception"); + } + return 0; +} + +int DELTA_NAMESPACE::PLC::mulWriteM(int start_number, int end_number, bool *status) { + try { + if(start_number >= end_number) { throw CompareException(); } + if(start_number < M0_NUM || start_number > M4095_NUM) { throw AddressIsNotExistException(start_number); } + if(end_number < M0_NUM || end_number > M4095_NUM) { throw AddressIsNotExistException(end_number); } + + if(start_number >= M0_NUM && end_number <= M1535_NUM) { + int amount = end_number - start_number + 1; + bool value[amount]; + for(int i = 0; i < amount; i++) value[i] = status[i]; + return mb_->modbus_write_coils(M0_HEX + start_number, amount,value); + } + else if(start_number >= M1536_NUM && end_number <= M4095_NUM) { + int amount = end_number - start_number + 1; + bool value[amount]; + for(int i = 0; i < amount; i++) value[i] = status[start_number + i]; + return mb_->modbus_write_coils(M1536_HEX + start_number - M1535_NUM, amount, value); + } + else { + int amount1,amount2; + amount1 = M1535_NUM - start_number + 1; + amount2 = end_number - M1536_NUM + 1; + bool value1[amount1], value2[amount2]; + for(int i = 0; i < amount1; i++) value1[i] = status[i]; + for(int i = 0; i < amount2; i++) value2[i] = status[amount1 + i]; + int n1,n2; + n1 = mb_->modbus_write_coils(M0_HEX + start_number,amount1,value1); + n2 = mb_->modbus_write_coils(M1536_HEX, amount2,value2); + return n1*n2; + } + } + catch(CompareException & e) { + ROS_ERROR_STREAM("delta::mulWriteM: " << e.what()); + ros::Duration(TIME_ERROR).sleep(); + return BAD_CON; + } + catch(AddressIsNotExistException & e) { + ROS_ERROR_STREAM("delta::mulWriteM: " << e.what()); + ros::Duration(TIME_ERROR).sleep(); + return BAD_CON; + } + catch(std::exception & e) { + ROS_ERROR_STREAM("delta::mulWriteM: " << e.what()); + ros::Duration(TIME_ERROR).sleep(); + return BAD_CON; + } + catch(const boost::exception& e) + { + ROS_ERROR_STREAM(boost::diagnostic_information(e)); + } + catch (...) + { + ROS_ERROR_STREAM("Caught unknown exception"); + } + return 0; +} + +int DELTA_NAMESPACE::PLC::setM(int M_number) { + try { + if(M_number < M0_NUM || M_number > M4095_NUM) { throw AddressIsNotExistException(M_number); } + if(M_number >= M0_NUM && M_number <= M1535_NUM){ + return mb_->modbus_write_coil(M0_HEX + M_number, ON); + } + else if(M_number >= M1536_NUM && M_number <= M4095_NUM) { + return mb_->modbus_write_coil(M1536_HEX + M_number, ON); + } + } + catch(AddressIsNotExistException & e) { + ROS_ERROR_STREAM("PLC::setM: " << e.what()); + ros::Duration(TIME_ERROR).sleep(); + return BAD_CON; + } + catch(std::exception& e) { + ROS_ERROR_STREAM("PLC::setM: " << e.what()); + ros::Duration(TIME_ERROR).sleep(); + return BAD_CON; + } + catch(const boost::exception& e) + { + ROS_ERROR_STREAM(boost::diagnostic_information(e)); + } + catch (...) + { + ROS_ERROR_STREAM("Caught unknown exception"); + } + return 0; +} + +int DELTA_NAMESPACE::PLC::resetM(int M_number) { + try { + if(M_number < M0_NUM || M_number > M4095_NUM) { throw AddressIsNotExistException(M_number); } + if(M_number >= M0_NUM && M_number <= M1535_NUM){ + return mb_->modbus_write_coil(M0_HEX + M_number, OFF); + } + else if(M_number >= M1536_NUM && M_number <= M4095_NUM) { + return mb_->modbus_write_coil(M1536_HEX + M_number, OFF); + } + } + catch(AddressIsNotExistException & e) { + ROS_ERROR_STREAM("PLC::setM: " << e.what()); + ros::Duration(TIME_ERROR).sleep(); + return BAD_CON; + } + catch(std::exception& e) { + ROS_ERROR_STREAM("PLC::setM: " << e.what()); + ros::Duration(TIME_ERROR).sleep(); + return BAD_CON; + } + catch(const boost::exception& e) + { + ROS_ERROR_STREAM(boost::diagnostic_information(e)); + } + catch (...) + { + ROS_ERROR_STREAM("Caught unknown exception"); + } + return 0; +} + +int DELTA_NAMESPACE::PLC::mulSetM(int start_number, int end_number) { + try { + if(start_number >= end_number) { throw CompareException(); } + if(start_number < M0_NUM || start_number > M4095_NUM) { throw AddressIsNotExistException(start_number); } + if(end_number < M0_NUM || end_number > M4095_NUM) { throw AddressIsNotExistException(end_number); } + + if(start_number >= M0_NUM && end_number <= M1535_NUM) { + int amount = end_number - start_number + 1; + bool value[amount]; + for(int i = 0; i < amount; i++) value[i] = ON; + return mb_->modbus_write_coils(M0_HEX + start_number, amount,value); + } + else if(start_number >= M1536_NUM && end_number <= M4095_NUM) { + int amount = end_number - start_number + 1; + bool value[amount]; + for(int i = 0; i < amount; i++) value[i] = ON; + return mb_->modbus_write_coils(M1536_HEX + start_number - M1535_NUM, amount, value); + } + else { + int amount1,amount2; + amount1 = M1535_NUM - start_number + 1; + amount2 = end_number - M1536_NUM + 1; + bool value1[amount1], value2[amount2]; + for(int i = 0; i < amount1; i++) value1[i] = ON; + for(int i = 0; i < amount2; i++) value2[i] = ON; + int n1,n2; + n1 = mb_->modbus_write_coils(M0_HEX + start_number,amount1,value1); + n2 = mb_->modbus_write_coils(M1536_HEX, amount2,value2); + return n1*n2; + } + } + catch(CompareException & e) { + ROS_ERROR_STREAM("PLC::mulSetM: " << e.what()); + ros::Duration(TIME_ERROR).sleep(); + return BAD_CON; + } + catch(AddressIsNotExistException & e) { + ROS_ERROR_STREAM("PLC::mulSetM: " << e.what()); + ros::Duration(TIME_ERROR).sleep(); + return BAD_CON; + } + catch(std::exception& e) { + ROS_ERROR_STREAM("PLC::mulSetM: " << e.what()); + ros::Duration(TIME_ERROR).sleep(); + return BAD_CON; + } + catch(const boost::exception& e) + { + ROS_ERROR_STREAM(boost::diagnostic_information(e)); + } + catch (...) + { + ROS_ERROR_STREAM("Caught unknown exception"); + } + return 0; +} + +int DELTA_NAMESPACE::PLC::mulResetM(int start_number, int end_number) { + try { + if(start_number >= end_number) { throw CompareException(); } + if(start_number < M0_NUM || start_number > M4095_NUM) { throw AddressIsNotExistException(start_number); } + if(end_number < M0_NUM || end_number > M4095_NUM) { throw AddressIsNotExistException(end_number); } + + if(start_number >= M0_NUM && end_number <= M1535_NUM) { + int amount = end_number - start_number + 1; + bool value[amount]; + for(int i = 0; i < amount; i++) value[i] = OFF; + return mb_->modbus_write_coils(M0_HEX + start_number, amount,value); + } + else if(start_number >= M1536_NUM && end_number <= M4095_NUM) { + int amount = end_number - start_number + 1; + bool value[amount]; + for(int i = 0; i < amount; i++) value[i] = OFF; + return mb_->modbus_write_coils(M1536_HEX + start_number - M1535_NUM, amount, value); + } + else { + int amount1,amount2; + amount1 = M1535_NUM - start_number + 1; + amount2 = end_number - M1536_NUM + 1; + bool value1[amount1], value2[amount2]; + for(int i = 0; i < amount1; i++) value1[i] = OFF; + for(int i = 0; i < amount2; i++) value2[i] = OFF; + int n1,n2; + n1 = mb_->modbus_write_coils(M0_HEX + start_number,amount1,value1); + n2 = mb_->modbus_write_coils(M1536_HEX, amount2,value2); + return n1*n2; + } + } + catch(CompareException & e) { + ROS_ERROR_STREAM("PLC::mulSetM: " << e.what()); + ros::Duration(TIME_ERROR).sleep(); + return BAD_CON; + } + catch(AddressIsNotExistException & e) { + ROS_ERROR_STREAM("PLC::mulSetM: " << e.what()); + ros::Duration(TIME_ERROR).sleep(); + return BAD_CON; + } + catch(std::exception& e) { + ROS_ERROR_STREAM("PLC::mulSetM: " << e.what()); + ros::Duration(TIME_ERROR).sleep(); + return BAD_CON; + } + catch(const boost::exception& e) + { + ROS_ERROR_STREAM(boost::diagnostic_information(e)); + } + catch (...) + { + ROS_ERROR_STREAM("Caught unknown exception"); + } + return 0; +} + +int DELTA_NAMESPACE::PLC::getM(int M_number, bool &buffer) { + try { + if(M_number < M0_NUM || M_number > M4095_NUM) { throw AddressIsNotExistException(M_number); } + + if(M_number >= M0_NUM && M_number <= M1535_NUM) + { + return mb_->modbus_read_coil(M0_HEX + M_number, buffer); + } + else if(M_number >= M1536_NUM && M_number <= M4095_NUM){ + return mb_->modbus_read_coil(M1536_HEX + M_number, buffer); + } + } + catch(AddressIsNotExistException & e) { + ROS_ERROR_STREAM("PLC::getM: " << e.what()); + ros::Duration(TIME_ERROR).sleep(); + return BAD_CON; + } + catch(std::exception& e) { + ROS_ERROR_STREAM("PLC::getM: " << e.what()); + ros::Duration(TIME_ERROR).sleep(); + return BAD_CON; + } + catch(const boost::exception& e) + { + ROS_ERROR_STREAM(boost::diagnostic_information(e)); + } + catch (...) + { + ROS_ERROR_STREAM("Caught unknown exception"); + } + return 0; +} + +int DELTA_NAMESPACE::PLC::mulGetM(int start_number, int end_number, bool* buffer) { + try { + if(start_number >= end_number) { throw CompareException(); } + if(start_number < M0_NUM || start_number > M4095_NUM) { throw AddressIsNotExistException(start_number); } + if(end_number < M0_NUM || end_number > M4095_NUM) { throw AddressIsNotExistException(end_number); } + + if(start_number >= M0_NUM && end_number <= M1535_NUM) { + int amount = end_number - start_number + 1; + return mb_->modbus_read_coils(M0_HEX + start_number, amount,buffer); + } + else if(start_number >= M1536_NUM && end_number <= M4095_NUM) { + int amount = end_number - start_number + 1; + return mb_->modbus_read_coils(M1536_HEX + start_number - M1535_NUM, amount,buffer); + } + else { + int amount1,amount2; + amount1 = M1535_NUM - start_number + 1; + amount2 = end_number - M1536_NUM + 1; + bool value1[amount1], value2[amount2]; + int n1,n2; + n1 = mb_->modbus_read_coils(M0_HEX + start_number,amount1,value1); + n2 = mb_->modbus_read_coils(M1536_HEX, amount2,value2); + for(int i = 0; i < amount1; i++) buffer[i] = value1[i]; + for(int i = 0; i < amount2; i++) buffer[amount1 + i] = value2[i]; + return n1*n2; + } + } + catch(CompareException & e) { + ROS_ERROR_STREAM("PLC::mulGetM: " << e.what()); + ros::Duration(TIME_ERROR).sleep(); + return BAD_CON; + } + catch(AddressIsNotExistException & e) { + ROS_ERROR_STREAM("PLC::mulGetM: " << e.what()); + ros::Duration(TIME_ERROR).sleep(); + return BAD_CON; + } + catch(std::exception& e) { + ROS_ERROR_STREAM("PLC::mulGetM: " << e.what()); + ros::Duration(TIME_ERROR).sleep(); + return BAD_CON; + } + catch(const boost::exception& e) + { + ROS_ERROR_STREAM(boost::diagnostic_information(e)); + } + catch (...) + { + ROS_ERROR_STREAM("Caught unknown exception"); + } + return 0; +} + +int DELTA_NAMESPACE::PLC::setY(int Y_number) { + try { + if(Y_number < Y0_NUM || Y_number > Y377_NUM) { throw AddressIsNotExistException(Y_number); } + return mb_->modbus_write_coil(Y0_HEX + Y_number, ON); + } + catch(AddressIsNotExistException & e) { + ROS_ERROR_STREAM("PLC::setY: " << e.what()); + ros::Duration(TIME_ERROR).sleep(); + return BAD_CON; + } + catch(std::exception& e) { + ROS_ERROR_STREAM("PLC::setY: " << e.what()); + ros::Duration(TIME_ERROR).sleep(); + return BAD_CON; + } + catch(const boost::exception& e) + { + ROS_ERROR_STREAM(boost::diagnostic_information(e)); + } + catch (...) + { + ROS_ERROR_STREAM("Caught unknown exception"); + } + return 0; +} + +int DELTA_NAMESPACE::PLC::resetY(int Y_number) { + try { + if(Y_number < Y0_NUM || Y_number > Y377_NUM) { throw AddressIsNotExistException(Y_number); } + return mb_->modbus_write_coil(Y0_HEX + Y_number, OFF); + } + catch(AddressIsNotExistException & e) { + ROS_ERROR_STREAM("PLC::setY: " << e.what()); + ros::Duration(TIME_ERROR).sleep(); + return BAD_CON; + } + catch(std::exception& e) { + ROS_ERROR_STREAM("PLC::setY: " << e.what()); + ros::Duration(TIME_ERROR).sleep(); + return BAD_CON; + } + catch(const boost::exception& e) + { + ROS_ERROR_STREAM(boost::diagnostic_information(e)); + } + catch (...) + { + ROS_ERROR_STREAM("Caught unknown exception"); + } + return 0; +} + +int DELTA_NAMESPACE::PLC::mulWriteY(int start_number, int end_number, bool *buffer) { + + try { + if(start_number >= end_number) { throw CompareException(); } + if(start_number < Y0_NUM || start_number > Y377_NUM) { throw AddressIsNotExistException(start_number); } + if(end_number < Y0_NUM || end_number > Y377_NUM) { throw AddressIsNotExistException(end_number); } + int amount = end_number - start_number + 1; + return mb_->modbus_write_coils(Y0_HEX + start_number, amount, buffer); + } + catch(CompareException & e) { + ROS_ERROR_STREAM("PLC::mulSetY: " << e.what()); + ros::Duration(TIME_ERROR).sleep(); + return BAD_CON; + } + catch(AddressIsNotExistException & e) { + ROS_ERROR_STREAM("PLC::mulSetY: " << e.what()); + ros::Duration(TIME_ERROR).sleep(); + return BAD_CON; + } + catch(std::exception& e) { + ROS_ERROR_STREAM("PLC::mulSetY: " << e.what()); + ros::Duration(TIME_ERROR).sleep(); + return BAD_CON; + } + catch(const boost::exception& e) + { + ROS_ERROR_STREAM(boost::diagnostic_information(e)); + } + catch (...) + { + ROS_ERROR_STREAM("Caught unknown exception"); + } + return 0; +} + +int DELTA_NAMESPACE::PLC::mulSetY(int start_number, int end_number) { + try { + if(start_number >= end_number) { throw CompareException(); } + if(start_number < Y0_NUM || start_number > Y377_NUM) { throw AddressIsNotExistException(start_number); } + if(end_number < Y0_NUM || end_number > Y377_NUM) { throw AddressIsNotExistException(end_number); } + int amount = end_number - start_number + 1; + bool value[amount]; + for(int i = 0; i < amount; i++) value[i] = ON; + return mb_->modbus_write_coils(Y0_HEX + start_number, amount, value); + } + catch(CompareException & e) { + ROS_ERROR_STREAM("PLC::mulSetY: " << e.what()); + ros::Duration(TIME_ERROR).sleep(); + return BAD_CON; + } + catch(AddressIsNotExistException & e) { + ROS_ERROR_STREAM("PLC::mulSetY: " << e.what()); + ros::Duration(TIME_ERROR).sleep(); + return BAD_CON; + } + catch(std::exception& e) { + ROS_ERROR_STREAM("PLC::mulSetY: " << e.what()); + ros::Duration(TIME_ERROR).sleep(); + return BAD_CON; + } + catch(const boost::exception& e) + { + ROS_ERROR_STREAM(boost::diagnostic_information(e)); + } + catch (...) + { + ROS_ERROR_STREAM("Caught unknown exception"); + } + return 0; +} + +int DELTA_NAMESPACE::PLC::mulResetY(int start_number, int end_number) { + try { + if(start_number >= end_number) { throw CompareException(); } + if(start_number < Y0_NUM || start_number > Y377_NUM) { throw AddressIsNotExistException(start_number); } + if(end_number < Y0_NUM || end_number > Y377_NUM) { throw AddressIsNotExistException(end_number); } + int amount = end_number - start_number + 1; + bool value[amount]; + for(int i = 0; i < amount; i++) value[i] = OFF; + return mb_->modbus_write_coils(Y0_HEX + start_number, amount, value); + } + catch(CompareException & e) { + ROS_ERROR_STREAM("PLC::mulSetY: " << e.what()); + ros::Duration(TIME_ERROR).sleep(); + return BAD_CON; + } + catch(AddressIsNotExistException & e) { + ROS_ERROR_STREAM("PLC::mulSetY: " << e.what()); + ros::Duration(TIME_ERROR).sleep(); + return BAD_CON; + } + catch(std::exception& e) { + ROS_ERROR_STREAM("PLC::mulSetY: " << e.what()); + ros::Duration(TIME_ERROR).sleep(); + return BAD_CON; + } + catch(const boost::exception& e) + { + ROS_ERROR_STREAM(boost::diagnostic_information(e)); + } + catch (...) + { + ROS_ERROR_STREAM("Caught unknown exception"); + } + return 0; +} + +int DELTA_NAMESPACE::PLC::mulGetY(int start_number, int end_number, bool *buffer) { + try { + if(start_number >= end_number) { throw CompareException(); } + if(start_number < Y0_NUM || start_number > Y377_NUM) { throw AddressIsNotExistException(start_number); } + if(end_number < Y0_NUM || end_number > Y377_NUM) { throw AddressIsNotExistException(end_number); } + int amount = end_number - start_number + 1; + return mb_->modbus_read_coils(Y0_HEX + start_number, amount, buffer); + } + catch(CompareException & e) { + ROS_ERROR_STREAM("PLC::mulGetY: " << e.what()); + ros::Duration(TIME_ERROR).sleep(); + return BAD_CON; + } + catch(AddressIsNotExistException & e) { + ROS_ERROR_STREAM("PLC::mulGetY: " << e.what()); + ros::Duration(TIME_ERROR).sleep(); + return BAD_CON; + } + catch(std::exception& e) { + ROS_ERROR_STREAM("PLC::mulGetY: " << e.what()); + ros::Duration(TIME_ERROR).sleep(); + return BAD_CON; + } + catch(const boost::exception& e) + { + ROS_ERROR_STREAM(boost::diagnostic_information(e)); + } + catch (...) + { + ROS_ERROR_STREAM("Caught unknown exception"); + } + return 0; +} + +int DELTA_NAMESPACE::PLC::getD(int D_number, uint16_t &buffer) { + try + { + if(D_number < D0_NUM || D_number > D4095_NUM) + throw AddressIsNotExistException(D_number); + return mb_->modbus_read_holding_register(D0_HEX + D_number, buffer); + } + catch(AddressIsNotExistException & e) { + ROS_ERROR_STREAM("PLC::getD: " << e.what()); + ros::Duration(TIME_ERROR).sleep(); + return BAD_CON; + } + catch(std::exception& e) { + ROS_ERROR_STREAM("PLC::getD: " << e.what()); + ros::Duration(TIME_ERROR).sleep(); + return BAD_CON; + } + catch(const boost::exception& e) + { + ROS_ERROR_STREAM(boost::diagnostic_information(e)); + } + catch (...) + { + ROS_ERROR_STREAM("Caught unknown exception"); + } + return 0; +} + +int DELTA_NAMESPACE::PLC::mulGetD(int start_number, int end_number, uint16_t *buffer) { + try { + if(start_number >= end_number) { throw CompareException(); } + if(start_number < D0_NUM || start_number > D4095_NUM) { throw AddressIsNotExistException(start_number); } + if(end_number < D0_NUM || end_number > D4095_NUM) { throw AddressIsNotExistException(end_number); } + int amount = end_number - start_number + 1; + return mb_->modbus_read_holding_registers(D0_HEX + start_number, amount, buffer); + } + catch(CompareException & e) { + ROS_ERROR_STREAM("PLC::mulGetD: " << e.what()); + ros::Duration(TIME_ERROR).sleep(); + return BAD_CON; + } + catch(AddressIsNotExistException & e) { + ROS_ERROR_STREAM("PLC::mulGetD: " << e.what()); + ros::Duration(TIME_ERROR).sleep(); + return BAD_CON; + } + catch(std::exception& e) { + ROS_ERROR_STREAM("PLC::mulGetD: " << e.what()); + ros::Duration(TIME_ERROR).sleep(); + return BAD_CON; + } + catch(const boost::exception& e) + { + ROS_ERROR_STREAM(boost::diagnostic_information(e)); + } + catch (...) + { + ROS_ERROR_STREAM("Caught unknown exception"); + } + return 0; +} + +int DELTA_NAMESPACE::PLC::setD(int D_number, int16_t &buffer) { + try { + if(D_number < D0_NUM || D_number > D4095_NUM) { throw AddressIsNotExistException(D_number); } + return mb_->modbus_write_register(D0_HEX + D_number, buffer); + } + catch(AddressIsNotExistException & e) { + ROS_ERROR_STREAM("PLC::setD: " << e.what()); + ros::Duration(TIME_ERROR).sleep(); + return BAD_CON; + } + catch(std::exception& e) { + ROS_ERROR_STREAM("PLC::setD: " << e.what()); + ros::Duration(TIME_ERROR).sleep(); + return BAD_CON; + } + catch(const boost::exception& e) + { + ROS_ERROR_STREAM(boost::diagnostic_information(e)); + } + catch (...) + { + ROS_ERROR_STREAM("Caught unknown exception"); + } + return 0; +} + +int DELTA_NAMESPACE::PLC::mulSetD(int start_number, int end_number, uint16_t *buffer) { + try { + if(start_number >= end_number) { throw CompareException(); } + if(start_number < D0_NUM || start_number > D4095_NUM) { throw AddressIsNotExistException(start_number); } + if(end_number < D0_NUM || end_number > D4095_NUM) { throw AddressIsNotExistException(end_number); } + int amount = end_number - start_number + 1; + return mb_->modbus_write_registers(D0_HEX + start_number, amount, buffer); + } + catch(CompareException & e) { + ROS_ERROR_STREAM("PLC::mulSetD: " << e.what()); + ros::Duration(TIME_ERROR).sleep(); + return BAD_CON; + } + catch(AddressIsNotExistException & e) { + ROS_ERROR_STREAM("PLC::mulSetD: " << e.what()); + ros::Duration(TIME_ERROR).sleep(); + return BAD_CON; + } + catch(std::exception& e) { + ROS_ERROR_STREAM("PLC::mulSetD: " << e.what()); + ros::Duration(TIME_ERROR).sleep(); + return BAD_CON; + } + catch(const boost::exception& e) + { + ROS_ERROR_STREAM(boost::diagnostic_information(e)); + } + catch (...) + { + ROS_ERROR_STREAM("Caught unknown exception"); + } + return 0; +} + +int DELTA_NAMESPACE::PLC::getX(int X_number, bool &buffer) { + try { + if(X_number < X0_NUM || X_number > X377_NUM) { throw AddressIsNotExistException(X_number); } + mb_->modbus_read_input_bit(X0_HEX + X_number, buffer); + } + catch(AddressIsNotExistException & e) { + ROS_ERROR_STREAM("PLC::getX: " << e.what()); + ros::Duration(TIME_ERROR).sleep(); + return BAD_CON; + } + catch(std::exception& e) { + ROS_ERROR_STREAM("PLC::getX: " << e.what()); + ros::Duration(TIME_ERROR).sleep(); + return BAD_CON; + } + return 0; +} + +/* + * Read X bit + * @param start_number Begin number X + * @param end_number End number X + * @param buffer Status of X + */ +int DELTA_NAMESPACE::PLC::mulGetX(int start_number, int end_number, bool* buffer) { + try { + if(start_number >= end_number) { throw CompareException(); } + if(start_number < X0_NUM || start_number > X377_NUM) { throw AddressIsNotExistException(start_number); } + if(end_number < X0_NUM || end_number > X377_NUM) { throw AddressIsNotExistException(end_number); } + int amount = end_number - start_number + 1; + mb_->modbus_read_input_bits(X0_HEX + start_number, amount, buffer); + } + catch(CompareException & e) { + ROS_ERROR_STREAM("PLC::mulGetX: " << e.what()); + ros::Duration(TIME_ERROR).sleep(); + return BAD_CON; + } + catch(AddressIsNotExistException & e) { + ROS_ERROR_STREAM("PLC::mulGetX: " << e.what()); + ros::Duration(TIME_ERROR).sleep(); + return BAD_CON; + } + catch(std::exception& e) { + ROS_ERROR_STREAM("PLC::mulGetX: " << e.what()); + ros::Duration(TIME_ERROR).sleep(); + return BAD_CON; + } + catch(const boost::exception& e) + { + ROS_ERROR_STREAM(boost::diagnostic_information(e)); + } + catch (...) + { + ROS_ERROR_STREAM("Caught unknown exception"); + } + return 0; +} + +int DELTA_NAMESPACE::PLC::getC(int C_number, uint16_t &buffer) { + try { + if(C_number < C0_NUM || C_number > C199_NUM) { throw AddressIsNotExistException(C_number); } + mb_->modbus_read_holding_register(C0_HEX + C_number, buffer); + } + catch(AddressIsNotExistException & e) { + ROS_ERROR_STREAM("PLC::getC: " << e.what()); + ros::Duration(TIME_ERROR).sleep(); + return BAD_CON; + } + catch(std::exception& e) { + ROS_ERROR_STREAM("PLC::getC: " << e.what()); + ros::Duration(TIME_ERROR).sleep(); + return BAD_CON; + } + catch(const boost::exception& e) + { + ROS_ERROR_STREAM(boost::diagnostic_information(e)); + } + catch (...) + { + ROS_ERROR_STREAM("Caught unknown exception"); + } + return 0; +} + +int DELTA_NAMESPACE::PLC::mulGetC(int start_number, int end_number, uint16_t *buffer) { + try { + if(start_number >= end_number) { throw CompareException(); } + if(start_number < C0_NUM || start_number > C199_NUM) { throw AddressIsNotExistException(start_number); } + if(end_number < C0_NUM || end_number > C199_NUM) { throw AddressIsNotExistException(end_number); } + int amount = end_number - start_number + 1; + mb_->modbus_read_holding_registers(C0_HEX + start_number, amount, buffer); + } + catch(CompareException & e) { + ROS_ERROR_STREAM("PLC::mulGetC: " << e.what()); + ros::Duration(TIME_ERROR).sleep(); + return BAD_CON; + } + catch(AddressIsNotExistException & e) { + ROS_ERROR_STREAM("PLC::mulGetC: " << e.what()); + ros::Duration(TIME_ERROR).sleep(); + return BAD_CON; + } + catch(std::exception& e) { + ROS_ERROR_STREAM("PLC::mulGetC: " << e.what()); + ros::Duration(TIME_ERROR).sleep(); + return BAD_CON; + } + catch(const boost::exception& e) + { + ROS_ERROR_STREAM(boost::diagnostic_information(e)); + } + catch (...) + { + ROS_ERROR_STREAM("Caught unknown exception"); + } + return 0; +} + + diff --git a/Devices/Libraries/Ros/delta_modbus/test/test_delta.cpp b/Devices/Libraries/Ros/delta_modbus/test/test_delta.cpp new file mode 100755 index 0000000..ea26a53 --- /dev/null +++ b/Devices/Libraries/Ros/delta_modbus/test/test_delta.cpp @@ -0,0 +1,34 @@ +#include +#include +#include "delta_modbus/delta_modbus_tcp.h" +#include + + +/********************************************************************** + * MAIN +***********************************************************************/ +int main(int argc, char** argv) { + ros::init(argc, argv, "test_delta_modbus_tcp"); + ros::NodeHandle nh("~"); + + std::string node_name = ros::this_node::getName(); + ROS_INFO("node_name: %s", node_name.c_str()); + + DELTA_NAMESPACE::PLC *plc = NULL; + + std::string IP = "192.168.1.5"; + int PORT = 502; + + plc = new DELTA_NAMESPACE::PLC(IP, PORT); + plc->connect(); + + uint16_t D101; + bool M[2]; + plc->getD(1, D101); plc->mulGetM(1, 2,M); + ROS_INFO("D1 %d M1 %x %x", D101, M[0], M[1]); + + ros::spin(); + plc = NULL; + delete(plc); + return 0; +} diff --git a/Devices/Libraries/Ros/libserial/.gitignore b/Devices/Libraries/Ros/libserial/.gitignore new file mode 100755 index 0000000..8c2b884 --- /dev/null +++ b/Devices/Libraries/Ros/libserial/.gitignore @@ -0,0 +1,14 @@ +# ---> VisualStudioCode +.vscode/* +!.vscode/settings.json +!.vscode/tasks.json +!.vscode/launch.json +!.vscode/extensions.json +!.vscode/*.code-snippets + +# Local History for Visual Studio Code +.history/ + +# Built Visual Studio Code Extensions +*.vsix + diff --git a/Devices/Libraries/Ros/libserial/CMakeLists.txt b/Devices/Libraries/Ros/libserial/CMakeLists.txt new file mode 100755 index 0000000..4e29c4b --- /dev/null +++ b/Devices/Libraries/Ros/libserial/CMakeLists.txt @@ -0,0 +1,45 @@ +cmake_minimum_required(VERSION 3.0.2) +project(libserial) + +find_package(catkin REQUIRED COMPONENTS + roscpp +) + +catkin_package( + INCLUDE_DIRS include + LIBRARIES ${PROJECT_NAME} + CATKIN_DEPENDS roscpp +# DEPENDS other non-ROS libs +) +include_directories( + include + ${Boost_INCLUDE_DIRS} + ${catkin_INCLUDE_DIRS} + ${console_bridge_INCLUDE_DIRS} +) + +#modbustcp +set(YOUR_LIB_SOURCES + src/serial.cpp + src/rs485.cpp +) +add_library(${PROJECT_NAME} ${YOUR_LIB_SOURCES}) +SET(-ludev udev) +target_link_libraries(${PROJECT_NAME} + ${catkin_LIBRARIES} + ${Boost_LIBRARIES} + ${GSTREAMER_LIBRARIES} + ${-ludev} +) + +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE +) \ No newline at end of file diff --git a/Devices/Libraries/Ros/libserial/README.md b/Devices/Libraries/Ros/libserial/README.md new file mode 100755 index 0000000..f6f293a --- /dev/null +++ b/Devices/Libraries/Ros/libserial/README.md @@ -0,0 +1,2 @@ +# libserial + diff --git a/Devices/Libraries/Ros/libserial/include/libserial/define.h b/Devices/Libraries/Ros/libserial/include/libserial/define.h new file mode 100755 index 0000000..c03e248 --- /dev/null +++ b/Devices/Libraries/Ros/libserial/include/libserial/define.h @@ -0,0 +1,43 @@ +#pragma once + +#define OFF 0 +#define ON 1 +#define MAX_MSG_LENGTH 2048 + +typedef enum parity { + PARITY_NONE, + PARITY_EVEN, + PARITY_ODD +} parity_t; + +typedef enum stop_bits { + STOPBIT_1, + STOPBIT_2 +} stop_bits_t; + +typedef enum data_bits { + DATABIT_5, + DATABIT_6, + DATABIT_7, + DATABIT_8 +} data_bits_t; + +struct _rs485 { + /* Socket or file descriptor */ + int s; + /* Device: "/dev/ttyS0", "/dev/ttyUSB0" or "/dev/tty.USA19*" on Mac OS X. */ + const char* _port; + /* Bauds: 9600, 19200, 57600, 115200, etc */ + unsigned int _baud; + /* Data bit */ + data_bits_t _data_bit; + /* Stop bit */ + stop_bits_t _stop_bit; + /* Parity:*/ + parity_t _parity; + /* Save old termios settings */ + struct termios _old_tios; +}; + + + diff --git a/Devices/Libraries/Ros/libserial/include/libserial/rs485.h b/Devices/Libraries/Ros/libserial/include/libserial/rs485.h new file mode 100755 index 0000000..c3532f8 --- /dev/null +++ b/Devices/Libraries/Ros/libserial/include/libserial/rs485.h @@ -0,0 +1,88 @@ +// +// Created by Hiep on 5/12/2020. +// +#pragma once +#include +#include "serial.h" + +typedef struct _rs485 rs485_t; + +class rs485: public Serial +{ +private: + /** + * Configure + * @param devfile Path name of dev + * @param baud Baudrate + * @param parity Parity + * @param data_bit + * @param stop_bit + */ + virtual int _rs485_connect(rs485_t &ctx); + + /** + * create new port + * @param devfile Path name of dev + * @param baud Baudrate + * @param parity Parity + * @param data_bit + * @param stop_bit + */ + virtual void new_port(const char* devfile, unsigned int baud, parity_t parity, + data_bits_t data_bit,stop_bits_t stop_bit); + +public: + /** + * Data Sender + * @param to_send Request to Be Sent to Server + * @param length Length of the Request + * @return Size of the request + */ + virtual ssize_t sendMsgs(uint8_t *to_send, uint16_t length); + + /** + * Data Receiver + * @param buffer Buffer to Store the Data` Retrieved + * @return Size of Incoming Data + */ + virtual ssize_t receiveMsgs(uint8_t *buffer) const; + + /* + * Reconnect to device if rs485 modules was disconected. + */ + virtual void reconnect(void); + + /* + * Set read character miximum + * @param[in] num The character minximum to receive. + */ + virtual int setCharacterNumToRead(int num); + + /** + * Close serial port + */ + virtual void close_port(); + + /** + * @brief Constructor + * @param[in] devfile Example /dev/tty* + * @param[in] baud Number of transmitted bits per a second + * @param[in] parity The parity check bit {Even, None , Old } + * @param[in] data_bit Number of bits in a transmission frame + * @param[in] stop_bit End bit + */ + rs485(const char* devfile, unsigned int baud, parity_t parity, + data_bits_t data_bit,stop_bits_t stop_bit); + /** + * Destructor. + */ + virtual ~rs485(); +/* Properties */ +protected: + rs485_t ctx; + uint8_t size_pkg; + float param_safety; +public: + bool _connected{}; /* Write status connection of devices */ + std::string error_msg; /* To saved error message */ +}; \ No newline at end of file diff --git a/Devices/Libraries/Ros/libserial/include/libserial/serial.h b/Devices/Libraries/Ros/libserial/include/libserial/serial.h new file mode 100755 index 0000000..002bef3 --- /dev/null +++ b/Devices/Libraries/Ros/libserial/include/libserial/serial.h @@ -0,0 +1,44 @@ +// +// Created by Hiep on 4/12/2020. +// +#pragma once +// #include +#include /* std::cout */ +#include /* Standard input/output definitions */ +#include /* String function definitions */ +#include /* UNIX standard function definitions */ +#include /* File control definitions */ +#include /* Error number definitions */ +#include /* POSIX terminal control definitions */ +#include /* uin8_t */ +#include +#include +#include "define.h" + +class Serial +{ +public: + /* + * @brief Setting baudrate + * @param[in] b baud speed input + */ + virtual speed_t setBaudRate(unsigned int b); + + /* + * @brief Setting parity bits type + * @param[in] b baud speed input + */ + virtual void setParity(termios &tios, parity p); + + /* + * @brief Setting stop bits type + * @param[in] b baud speed input + */ + virtual void setStopBits(termios &tios, stop_bits s); + + /* + * @brief Setting data bits type + * @param[in] b baud speed input + */ + virtual void setDataBits(termios &tios, data_bits d); +}; diff --git a/Devices/Libraries/Ros/libserial/include/libserial/udevadm_info.h b/Devices/Libraries/Ros/libserial/include/libserial/udevadm_info.h new file mode 100755 index 0000000..64a2e71 --- /dev/null +++ b/Devices/Libraries/Ros/libserial/include/libserial/udevadm_info.h @@ -0,0 +1,119 @@ +#ifndef __UDEVADM_INFO_H_INCLUDE__ +#define __UDEVADM_INFO_H_INCLUDE__ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +class udevadmInfo { +public: + udevadmInfo(const char *product_name); + virtual ~udevadmInfo(void); + virtual int init(); + virtual bool scanDevices(void); + const char *port; +protected: + struct udev *udev; + char *product_name_; +}; + +udevadmInfo::udevadmInfo(const char *product_name) + : product_name_((char *)product_name) +{ + +} + +udevadmInfo::~udevadmInfo(void){ + udev_unref(udev); +} + +int udevadmInfo::init() { + udev = udev_new(); + if (!udev) { + printf("Error while initialization!\n"); + return EXIT_FAILURE; + } + sigset_t mask; + + // Set signals we want to catch + sigemptyset(&mask); + sigaddset(&mask, SIGTERM); + sigaddset(&mask, SIGINT); + + // Change the signal mask and check + if (sigprocmask(SIG_BLOCK, &mask, nullptr) < 0) { + ROS_ERROR("UDEV-Error while sigprocmask(): %s\n", std::strerror(errno)); + return EXIT_FAILURE; + } + // Get a signal file descriptor + int signal_fd = signalfd(-1, &mask, 0); + // Check the signal file descriptor + if (signal_fd < 0) { + ROS_ERROR("UDEV-Error while signalfd(): %s\n", std::strerror(errno)); + return EXIT_FAILURE; + } + return EXIT_SUCCESS; +} + +bool udevadmInfo::scanDevices(void) { + struct udev_device *device; + struct udev_enumerate *enumerate; + struct udev_list_entry *devices, *dev_list_entry; + + // Create enumerate object + enumerate = udev_enumerate_new(udev); + if (!enumerate) { + ROS_ERROR("Error while creating udev enumerate\n"); + return false; + } + + // Scan devices + udev_enumerate_scan_devices(enumerate); + + // Fill up device list + devices = udev_enumerate_get_list_entry(enumerate); + if (!devices) { + ROS_ERROR("Error while getting device list\n"); + return false; + } + + udev_list_entry_foreach(dev_list_entry, devices) { + const char* path = udev_list_entry_get_name(dev_list_entry); + // Get the device + device = udev_device_new_from_syspath(udev, udev_list_entry_get_name(dev_list_entry)); + // Print device information + if(udev_device_get_devnode(device) != NULL && strstr(udev_device_get_sysname(device),"ttyUSB") != NULL) { + // ROS_INFO("DEVNODE=%s", udev_device_get_devnode(device)); + struct udev_device* usb = udev_device_get_parent_with_subsystem_devtype(device,"usb","usb_device"); + // ROS_INFO("usb = %s:%s, manufacturer = %s, product = %s, serial = %s, speed = %s, bMaxPower = %s", + // udev_device_get_sysattr_value(usb, "idVendor"), + // udev_device_get_sysattr_value(usb, "idProduct"), + // udev_device_get_sysattr_value(usb, "manufacturer"), + // udev_device_get_sysattr_value(usb, "product"), + // udev_device_get_sysattr_value(usb, "serial"), + // udev_device_get_sysattr_value(usb, "speed"), + // udev_device_get_sysattr_value(usb, "bMaxPower")); + if(strstr(udev_device_get_sysattr_value(usb, "product"), (const char *)product_name_) != NULL) { + port = udev_device_get_devnode(device); + return true; + } + } + else { + continue; + } + // Free the device + udev_device_unref(device); + } + // Free enumerate + udev_enumerate_unref(enumerate); + return false; +} +#endif \ No newline at end of file diff --git a/Devices/Libraries/Ros/libserial/package.xml b/Devices/Libraries/Ros/libserial/package.xml new file mode 100755 index 0000000..6b98837 --- /dev/null +++ b/Devices/Libraries/Ros/libserial/package.xml @@ -0,0 +1,62 @@ + + + libserial + 0.0.0 + The libserial package + + + + + robotics + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + roscpp + roscpp + roscpp + + + + + + + + diff --git a/Devices/Libraries/Ros/libserial/src/rs485.cpp b/Devices/Libraries/Ros/libserial/src/rs485.cpp new file mode 100755 index 0000000..70e177e --- /dev/null +++ b/Devices/Libraries/Ros/libserial/src/rs485.cpp @@ -0,0 +1,309 @@ +#include "libserial/rs485.h" + +/* + * Configure + * @param devfile Path name of dev + * @param baud Baudrate + * @param parity Parity + * @param data_bit + * @param stop_bit + */ +int rs485::_rs485_connect(rs485_t &ctx) +{ + struct termios tios; + int flags; + /* The O_NOCTTY flag tells UNIX that this program doesn't want + to be the "controlling terminal" for that port. If you + don't specify this then any input (such as keyboard abort + signals and so forth) will affect your process + Timeouts are ignored in canonical input mode or when the + NDELAY option is set on the file via open or fcntl + */ + flags = O_RDWR | O_NOCTTY | O_NONBLOCK | O_NDELAY; + ctx.s = open(ctx._port,flags); + if (ctx.s < 0) + { + std::stringstream e; + e << ctx._port <<" is failed: " << strerror(errno); + error_msg = e.str(); + _connected = false; + return -1; + } + else + { + fcntl(ctx.s, F_SETFL, 0); + } + /* Save */ + if(tcgetattr(ctx.s, &ctx._old_tios) <0) + { + ROS_ERROR("Can't get terminal parameters"); + } + + /* + * Enable the receiver and set local mode... + */ + bzero(&tios, sizeof(tios)); + + /* C_ISPEED Input baud (new interface) + * C_OSPEED Output baud (new interface) + */ + speed_t speed = setBaudRate(ctx._baud); + /* Set the baud rate */ + if ((cfsetispeed(&tios, speed) < 0) || + (cfsetospeed(&tios, speed) < 0)) + { + close(ctx.s); + ctx.s = -1; + _connected = false; + return -1; + } + setDataBits(tios,ctx._data_bit); + setStopBits(tios,ctx._stop_bit); + setParity(tios, ctx._parity); + + /* Read the man page of termios if you need more information. + * This field isn't used on POSIX systems + * tios.c_line = 0; + */ + + /* C_LFLAG Line options + ISIG Enable SIGINTR, SIGSUSP, SIGDSUSP, and SIGQUIT signals + ICANON Enable canonical input (else raw) + XCASE Map uppercase \lowercase (obsolete) + ECHO Enable echoing of input characters + ECHOE Echo erase character as BS-SP-BS + ECHOK Echo NL after kill character + ECHONL Echo NL + NOFLSH Disable flushing of input buffers after + interrupt or quit characters + IEXTEN Enable extended functions + ECHOCTL Echo control characters as ^char and delete as ~? + ECHOPRT Echo erased character as character erased + ECHOKE BS-SP-BS entire line on line kill + FLUSHO Output being flushed + PENDIN Retype pending input at next read or input char + TOSTOP Send SIGTTOU for background output + Canonical input is line-oriented. Input characters are put + into a buffer which can be edited interactively by the user + until a CR (carriage return) or LF (line feed) character is + received. + Raw input is unprocessed. Input characters are passed + through exactly as they are received, when they are + received. Generally you'll deselect the ICANON, ECHO, + ECHOE, and ISIG options when using raw input + */ + + /* Raw input */ + tios.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);; + + /* Software flow control is disabled */ + tios.c_iflag &= ~(IXON | IXOFF | IXANY); + + /* C_OFLAG Output options + OPOST Postprocess output (not set = raw output) + ONLCR Map NL to CR-NL + ONCLR ant others needs OPOST to be enabled + */ + + /* Raw ouput */ + tios.c_oflag = 0; + + /* C_CC Control characters + VMIN Minimum number of characters to read + VTIME Time to wait for data (tenths of seconds) + UNIX serial interface drivers provide the ability to + specify character and packet timeouts. Two elements of the + c_cc array are used for timeouts: VMIN and VTIME. Timeouts + are ignored in canonical input mode or when the NDELAY + option is set on the file via open or fcntl. + VMIN specifies the minimum number of characters to read. If + it is set to 0, then the VTIME value specifies the time to + wait for every character read. Note that this does not mean + that a read call for N bytes will wait for N characters to + come in. Rather, the timeout will apply to the first + character and the read call will return the number of + characters immediately available (up to the number you + request). + If VMIN is non-zero, VTIME specifies the time to wait for + the first character read. If a character is read within the + time given, any read will block (wait) until all VMIN + characters are read. That is, once the first character is + read, the serial interface driver expects to receive an + entire packet of characters (VMIN bytes total). If no + character is read within the time allowed, then the call to + read returns 0. This method allows you to tell the serial + driver you need exactly N bytes and any read call will + return 0 or N bytes. However, the timeout only applies to + the first character read, so if for some reason the driver + misses one character inside the N byte packet then the read + call could block forever waiting for additional input + characters. + VTIME specifies the amount of time to wait for incoming + characters in tenths of seconds. If VTIME is set to 0 (the + default), reads will block (wait) indefinitely unless the + NDELAY option is set on the port with open or fcntl. + */ + /* Unused because we use open with the NDELAY option */ + tios.c_cc[VMIN] = 5; /* The character minximum to read*/ + tios.c_cc[VTIME] = 20; /* The wait time maximum to read*/ + /* clean port */ + tcflush(ctx.s, TCIFLUSH); + /* activate the settings port */ + if (tcsetattr(ctx.s, TCSANOW, &tios) < 0) { + ROS_ERROR("Can't get terminal parameters"); + close(ctx.s); + ctx.s = -1; + _connected = false; + return -1; + } + ctx._old_tios = tios; + _connected = true; + return 0; +} + +/* + * Reconnect to device if rs485 modules was disconected. + */ +void rs485::reconnect(void) +{ + _rs485_connect(ctx); +} + +/* + * create new port + * @param devfile Path name of dev + * @param baud Baudrate + * @param parity Parity + * @param data_bit + * @param stop_bit + */ +void rs485::new_port(const char* devfile,unsigned int baud, parity_t parity, + data_bits_t data_bit,stop_bits_t stop_bit) +{ + ctx._port = devfile; + ctx._baud = baud; + ctx._parity = parity; + ctx._data_bit = data_bit; + ctx._stop_bit = stop_bit; + _connected = false; + if(parity == PARITY_EVEN || parity == PARITY_ODD) + size_pkg++; + switch(stop_bit) + { + case STOPBIT_1: + size_pkg++; + break; + case STOPBIT_2: + size_pkg +=2; + break; + default: + break; + } + + switch(data_bit) + { + case DATABIT_5: + size_pkg += 5; + break; + case DATABIT_6: + size_pkg += 6; + break; + case DATABIT_7: + size_pkg += 7; + break; + case DATABIT_8: + size_pkg += 8; + break; + default: + break; + } + size_pkg +=2; // bit start and stop + + switch(baud) + { + case 19200: + param_safety = 1.5; + break; + case 115200: + param_safety = 3.5; + break; + default: + param_safety = 1.5; + break; + } +} + +/* + * Close serial port + */ +void rs485::close_port() +{ + close(this->ctx.s); +} + +/* + * Data Sender + * @param to_send Request to Be Sent to Server + * @param length Length of the Request + * @return Size of the request + */ +ssize_t rs485::sendMsgs(uint8_t *to_send, uint16_t length) +{ + float time = param_safety*1000000/(ctx._baud / (size_pkg * length)); + usleep(time); + memset((to_send + length),'\0', 1); + ssize_t num = write(ctx.s, to_send, (size_t)length); + usleep(time); + return num; +} + +/* + * Data Receiver + * @param buffer Buffer to Store the Data` Retrieved + * @return Size of Incoming Data + */ +ssize_t rs485::receiveMsgs(uint8_t *buffer) const +{ + memset(buffer,'\0',sizeof(buffer)); + return read(ctx.s, (char *) buffer,MAX_MSG_LENGTH); +} + +/* + * @brief Constructor + * @param[in] devfile Example /dev/tty* + * @param[in] baud Number of transmitted bits per a second + * @param[in] parity The parity check bit {Even, None , Old } + * @param[in] data_bit Number of bits in a transmission frame + * @param[in] stop_bit End bit + */ +rs485::rs485(const char* devfile, unsigned int baud, parity_t parity, data_bits_t data_bit,stop_bits_t stop_bit) +{ + new_port(devfile, baud, parity, data_bit, stop_bit); + _rs485_connect(ctx); +} + +/* + * Destructor. + */ +rs485::~rs485() +{ + +} + +/* + * Set read character miximum + * @param[in] num The character minximum to receive. + */ +int rs485::setCharacterNumToRead(int num) +{ + ctx._old_tios.c_cc[VMIN] = num; + /* clean port */ + tcflush(ctx.s, TCIFLUSH); + /* activate the settings port */ + if (tcsetattr(ctx.s, TCSANOW, &ctx._old_tios) < 0) { + ROS_ERROR("Can't get terminal parameters"); + close(ctx.s); + ctx.s = -1; + } + return 0; +} \ No newline at end of file diff --git a/Devices/Libraries/Ros/libserial/src/serial.cpp b/Devices/Libraries/Ros/libserial/src/serial.cpp new file mode 100755 index 0000000..8c015a3 --- /dev/null +++ b/Devices/Libraries/Ros/libserial/src/serial.cpp @@ -0,0 +1,181 @@ +#include "libserial/serial.h" + +/* + * @brief Setting baudrate + * @param[in] b baud speed input + */ +speed_t Serial::setBaudRate(unsigned int b) { + speed_t speed; + switch (b) { + case 110: + speed = B110; + break; + case 300: + speed = B300; + break; + case 600: + speed = B600; + break; + case 1200: + speed = B1200; + break; + case 2400: + speed = B2400; + break; + case 4800: + speed = B4800; + break; + case 9600: + speed = B9600; + break; + case 19200: + speed = B19200; + break; + case 8400: + speed = B38400; + break; + case 57600: + speed = B57600; + break; + case 115200: + speed = B115200; + break; + case 230400: + speed = B230400; + break; + case 460800: + speed = B460800; + break; + case 500000: + speed = B500000; + break; + case 576000: + speed = B576000; + break; + case 921600: + speed = B921600; + break; + case 1000000: + speed = B1000000; + break; + case 1152000: + speed = B1152000; + break; + case 1500000: + speed = B1500000; + break; + case 2500000: + speed = B2500000; + break; + case 3000000: + speed = B3000000; + break; + case 3500000: + speed = B3500000; + break; + case 4000000: + speed = B4000000; + break; + default: + speed = B9600; + break; + } + return speed; +} + +/* + * @brief Setting parity bits type + * @param[in] b baud speed input + */ +void Serial::setParity(termios& tios, parity p) { + /* PARENB Enable parity bit + PARODD Use odd parity instead of even */ + switch (p) + { + /* None */ + case PARITY_NONE: + tios.c_cflag &= ~PARENB; + tios.c_iflag &= ~INPCK; + break; + /* Even */ + case PARITY_EVEN: + tios.c_cflag |= PARENB; + tios.c_cflag &= ~PARODD; + tios.c_iflag |= INPCK; + break; + /* Odd */ + case PARITY_ODD: + tios.c_cflag |= PARENB; + tios.c_cflag |= PARODD; + break; + /* Default None */ + default: + tios.c_cflag &= ~PARENB; + tios.c_iflag &= ~INPCK; + break; + } +} + +/* + * @brief Setting stop bits type + * @param[in] b baud speed input + */ +void Serial::setStopBits(termios& tios, stop_bits s) { + /* Stop bit (1 or 2) */ + switch (s) + { + case STOPBIT_1: + tios.c_cflag &= ~CSTOPB; + break; + + case STOPBIT_2: + tios.c_cflag |= CSTOPB; + break; + + default: + tios.c_cflag &= ~CSTOPB; + break; + } +} + +/* + * @brief Setting data bits type + * @param[in] b baud speed input + */ +void Serial::setDataBits(termios& tios, data_bits d) +{ + /* C_CFLAG Control options + * CLOCAL Local line - do not change "owner" of port + * CREAD Enable receiver + */ + tios.c_cflag |= (CREAD | CLOCAL); + /* CSIZE, HUPCL, CRTSCTS (hardware flow control) */ + + /* Set data bits (5, 6, 7, 8 bits) + * CSIZE Bit mask for data bits + */ + tios.c_cflag &= ~CSIZE; + switch (d) + { + case DATABIT_8: + tios.c_cflag |= CS8; + break; + + case DATABIT_7: + tios.c_cflag |= CS7; + break; + + case DATABIT_6: + tios.c_cflag |= CS6; + break; + + case DATABIT_5: + tios.c_cflag |= CS5; + break; + + default: + tios.c_cflag |= CS8; + break; + } +} + diff --git a/Devices/Libraries/Ros/md_controller/CMakeLists.txt b/Devices/Libraries/Ros/md_controller/CMakeLists.txt new file mode 100755 index 0000000..ad59743 --- /dev/null +++ b/Devices/Libraries/Ros/md_controller/CMakeLists.txt @@ -0,0 +1,229 @@ +cmake_minimum_required(VERSION 3.0.2) +project(md_controller) + +## Compile as C++17, supported in ROS Kinetic and newer +add_compile_options(-std=c++17) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + roscpp + rospy + std_msgs + libserial +) + +## System dependencies are found with CMake's conventions +find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a exec_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# std_msgs +# ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( + INCLUDE_DIRS include + LIBRARIES ${PROJECT_NAME}_lib + CATKIN_DEPENDS roscpp rospy std_msgs libserial + DEPENDS Boost +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} +) + +## Declare a C++ library +add_library(${PROJECT_NAME}_lib + src/md_controller.cpp + src/diagnostic.cpp + src/md_controller_subscriber.cpp +) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +## With catkin_make all packages are built within a single CMake context +## The recommended prefix ensures that target names across packages don't collide +add_executable(${PROJECT_NAME}_node src/md_controller_node.cpp) + +## Rename C++ executable without prefix +## The above recommended prefix causes long target names, the following renames the +## target back to the shorter version for ease of user use +## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" +# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") + +## Add cmake target dependencies of the executable +## same as for the library above +add_dependencies(${PROJECT_NAME}_node + ${PROJECT_NAME}_lib + ${${PROJECT_NAME}_EXPORTED_TARGETS} + ${catkin_EXPORTED_TARGETS} +) + +add_dependencies(${PROJECT_NAME}_lib + ${${PROJECT_NAME}_EXPORTED_TARGETS} + ${catkin_EXPORTED_TARGETS} +) + +## Specify libraries to link a library or executable target against +target_link_libraries(${PROJECT_NAME}_node + ${PROJECT_NAME}_lib + ${Boost_LIBRARIES} + ${catkin_LIBRARIES} +) + +target_link_libraries(${PROJECT_NAME}_lib + ${Boost_LIBRARIES} + ${catkin_LIBRARIES} +) +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# catkin_install_python(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html +install(TARGETS ${PROJECT_NAME}_node + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +## Mark libraries for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html +install(TARGETS ${PROJECT_NAME}_lib + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} + ) + +## Mark cpp header files for installation +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE + ) + +install(DIRECTORY + launch + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +## Mark other files for installation (e.g. launch and bag files, etc.) +install(FILES + motorInfomation.yaml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_md_controller.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/Devices/Libraries/Ros/md_controller/README.md b/Devices/Libraries/Ros/md_controller/README.md new file mode 100755 index 0000000..68d10d8 --- /dev/null +++ b/Devices/Libraries/Ros/md_controller/README.md @@ -0,0 +1,15 @@ +#KHởi tạo USB->rs485: + +Bước 1: Cấu hình USB->rs485: + - Mở cổng terminal, Gõ lệnh "sudo touch /etc/udev/rules.d/99-usb-serial.rules" + - Mở file rules "sudo nano /etc/udev/rules.d/99-usb-serial.rules" + - Copy 2 dòng lệnh sau: + - KERNEL=="ttyUSB[0-9]*", OWNER="robotics" + - SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6001", ATTRS{manufacturer}=="PRATI", ATTRS{product}=="USB_MD200", SYMLINK+="USB_MD200" + - Save và exit + - kiểm tra thuộc tính "udevadm info --name=/dev/ttyUSB0 --attribute-walk" +- Nếu đã cấu hình rồi thì bỏ qua bước 1, IPC sẽ tự động thực hiện và cấp quyền cho cổng serial mỗi khi khởi động. Đồng thời, Không cần dùng lệnh "sudo chmod 666 /dev/ttyUSB*". + +Bước 2: + - Kiểm tra USB->rs485 đã kết nối ok chưa. Gõ lệnh "ls /dev". Bạn sẽ thấy thiết bị tên "AGV-BLDV20KM". Nếu không có thì chưa kết nối được + - KHởi chạy chương trình "roslaunch driver_blvd_controller agvrun.launch" diff --git a/Devices/Libraries/Ros/md_controller/include/md_controller/define.h b/Devices/Libraries/Ros/md_controller/include/md_controller/define.h new file mode 100755 index 0000000..766bc23 --- /dev/null +++ b/Devices/Libraries/Ros/md_controller/include/md_controller/define.h @@ -0,0 +1,112 @@ +#ifndef __MD_CTRL_DEFINE_H_INCLUDED +#define __MD_CTRL_DEFINE_H_INCLUDED + +#define HEADER_RMID_SEND 0xB7 +#define HEADER_TMID_SEND 0XAC + +#define MAX_BUFFER_LENGHT 2084 +#define PACKAGE_MSG_MAIN_DATA 22 +#define MAX_ID_LENGHT 253 +#define TIME_PUBLISH_TO_SEND 7500 //us + + +/* Frame of messgae package*/ +typedef struct _Header +{ + uint8_t RMID = HEADER_RMID_SEND; + uint8_t TMID; +}header_t; + +typedef struct _ID_Number +{ + uint8_t adr[MAX_ID_LENGHT]; + uint8_t size; +}id_number_t; + +typedef struct _Parameter_ID +{ + /* data */ + uint8_t PID; +}param_id_t; + +typedef struct Data_number +{ + uint8_t NUM; +}dt_num_t; + +/***********************************************************************************************************/ + +typedef struct _filter +{ + uint8_t _buffer[MAX_BUFFER_LENGHT]; + uint8_t result[PACKAGE_MSG_MAIN_DATA]; + unsigned long r_index; + unsigned long w_index; + unsigned long c_flag; +}filter_t; + +enum PID_COMMAND +{ + CMD_TQ_OFF = 2, // Tq-off, motor free state + CMD_BRAKE = 4, // Erectric brake + CMD_MAIN_BC_ON = 5, // PID_MAIN_DATA broadcasting ON + CMD_MAIN_BC_OFF = 6, // broadcasing OFF + CMD_ALARM_RESET = 8, // Reset alarm + CMD_POSI_RESET = 10, // Position reset(set position to zero) + CMD_MONITOR_BC_ON = 11, // PID_MONITOR broadcasting ON + CMD_MONITOR_BC_OFF = 12, // Broadcasting off + CMD_IO_MONITOR_ON = 13, // PID_IO_MONITOR BC ON + CMD_IO_MONITOR_OFF = 14, // PID_IO_MONITOR BC OFF + CMD_FAN_ON = 15, // Fan ON(motor cooling fan) + CMD_FAN_OFF = 16, // Fan OFF + CMD_CLUTCH_ON = 17, // Mechanical brake(clutch) ON + CMD_CLUTCH_OFF = 18, // Mechanical breka OFF + CMD_TAR_VEL_OFF = 20, // Erase target vel, set by PID_TAR_VEL + CMD_SLOW_START_OFF = 21, // Erase target slow/start value + CMD_SLOW_DOWN_OFF = 22, // Erase target slow/down vaule + CMD_CAN_RESEND_ON = 23, // Send CAN data to RS485 serial port. + CMD_CAN_RESEND_OFF = 24, // Turn off resending of CAN data + CMD_MAX_TQ_OFF = 25, // Erase target limit load(max. current) + CMD_ENC_OFF = 26, // Cancel the use of encoder sensor. + CMD_LOW_SPEED_LIMIT_OF = 27, // Cancel the set of low speed limit + F_HIGH = 28, // Cancel the set of high speed limit. + CMD_SPEED_LI_OFF =29, // Cancel the set of low/high speed limits + CMD_CURVE_OFF = 31, // Cancel set of curve fitting func. + CMD_STEP_OFF = 32, // Cancel step input mode + CMD_UICOM_OFF = 44, // I/O control(ctrl 11pin cnt) available + CMD_UICOM_ON = 45, // I/O control disable(when comm. is used) + CMD_MAX_RP_OFF = 46, // Cancel max. speed set by DIP SW + CMD_HALL_TY_OFF = 47, // Cancel set of motor hall type + CMD_LOW_POT_OFF = 48, // Cancel set of low limit of POT input + CMD_HIGH_POT_OFF = 49, // Cancel set of high limit of POT input + CMD_MAIN_BC_ON2 = 50, // PID_MAIN_DATA, BC ON for 2nd motor + CMD_MAIN_BC_OFF2 = 51, // PID_MAIN_DATA, BC OFF for 2nd motor + CMD_MONIT_BC_ON2 = 52, // PID_MONITOR, BC ON for 2nd motor + CMD_MONIT_BC_OFF2 = 53, // PID_MONITOR, BC OFF for 2nd motor + CMD_IO_MONIT_BC_ON2 = 54, // PID_IO_MONITOR, BC ON for 2nd motor + CMD_IO_MONIT_BC_OFF2 = 55 // PID_IO_MONITOR, BC OFF for 2nd motor +}; + +enum _BIT +{ + _OFF = 0x00u, + _ON = 0x01u +}; + +enum _BAUD_OF_MD200 +{ + _9600bps = 1, + _19200bps = 2, + _38400bps = 3, + _57600bps = 4, + _115200bps = 5 +}; + +typedef enum EXIT_CODES_ENUM +{ + EXIT_OK = 0, + EXIT_ERROR = 1, + EXIT_CONFIG_ERROR = -1 +} EXIT_CODES; + +#endif // __MD_CTRL_DEFINE_H_INCLUDED \ No newline at end of file diff --git a/Devices/Libraries/Ros/md_controller/include/md_controller/diagnostic.h b/Devices/Libraries/Ros/md_controller/include/md_controller/diagnostic.h new file mode 100755 index 0000000..df728fd --- /dev/null +++ b/Devices/Libraries/Ros/md_controller/include/md_controller/diagnostic.h @@ -0,0 +1,94 @@ +#ifndef __MD_CTRL_DIAGNOTICS_H_INCLUDED +#define __MD_CTRL_DIAGNOTICS_H_INCLUDED + +#include + +namespace MD +{ + typedef enum DIAGNOSTIC_STATUS_ENUM + { + OK, // status okay, no errors + EXIT, // delta exiting + ERROR_STATUS, // device signaled an error + CONFIGURATION_ERROR, // invalid configuration, check configuration files + INITIALIZATION_ERROR, // initialization of driver failed + INTERNAL_ERROR // internal error, should never happen + + } DIAGNOSTIC_STATUS; + /* + * class Diagnostic publishes diagnostic messages for sick_line_guidance + */ + class Diagnostic + { + public: + /* + * Initializes the global diagnostic handler. + * + * @param[in] nh ros::NodeHandle + * @param[in] publish_topic ros topic to publish diagnostic messages + * @param[in] component description of the component reporting + */ + static void init(ros::NodeHandle &nh, const std::string & publish_topic, const std::string & component) + { + g_diagnostic_handler.init(nh, publish_topic, component); + } + + /* + * Updates and reports the current status. + * + * @param[in] status current status (OK, ERROR, ...) + * @param[in] message optional diagnostic message + */ + static void update(DIAGNOSTIC_STATUS status, const std::string & message = "") + { + g_diagnostic_handler.update(status, message); + } + + protected: + /* + * class DiagnosticImpl implements diagnostic for sick_line_guidance + */ + class DiagnosticImpl + { + public: + + /* + * Constructor. + */ + DiagnosticImpl(); + + /* + * Initialization. + * + * @param[in] nh ros::NodeHandle + * @param[in] publish_topic ros topic to publish diagnostic messages + * @param[in] component description of the component reporting + */ + void init(ros::NodeHandle & nh, const std::string & publish_topic, const std::string & component); + + /* + * Updates and reports the current status. + * + * @param[in] status current status (OK, ERROR, ...) + * @param[in] message optional diagnostic message + */ + void update(DIAGNOSTIC_STATUS status, const std::string & message = ""); + + protected: + + /* + * member data. + */ + + bool m_diagnostic_initialized; // flag indicating proper initialization of diagnostic + ros::Publisher m_diagnostic_publisher; // publishes diagnostic messages + std::string m_diagnostic_component; // name of the component publishing diagnostic messages + + }; // class DiagnosticImpl + + + static DiagnosticImpl g_diagnostic_handler; // singleton, implements the diagnostic for sick_line_guidance + + }; // class Diagnostic +} +#endif // __MD_CTRL_DIAGNOTICS_H_INCLUDED \ No newline at end of file diff --git a/Devices/Libraries/Ros/md_controller/include/md_controller/md_controller.h b/Devices/Libraries/Ros/md_controller/include/md_controller/md_controller.h new file mode 100755 index 0000000..a629cfa --- /dev/null +++ b/Devices/Libraries/Ros/md_controller/include/md_controller/md_controller.h @@ -0,0 +1,78 @@ +#ifndef __MD_CTRL_H_INCLUDED +#define __MD_CTRL_H_INCLUDED + +#include +#include +#include "libserial/rs485.h" +#include "md_controller/define.h" +#include "md_controller/diagnostic.h" +#include + +namespace MD +{ + +// shortcut to read a member from a XmlRpcValue, or to return a defaultvalue, it the member does not exist +template static T readMember(XmlRpc::XmlRpcValue & value, const std::string & member, const T & defaultvalue) +{ + if(value.hasMember(member)) + return value[member]; + return defaultvalue; +} + +class MdController: public rs485 +{ +public: + MdController(ros::NodeHandlePtr nh, const char *devfile, unsigned int baud, parity_t parity, data_bits_t data_bit,stop_bits_t stop_bit); + virtual ~MdController(); + + // virtual void set_slave_id(int *ids, uint8_t lenght); + // virtual void publish_speed(int16_t *speed); + // virtual void main_BC_ON(); + // virtual void main_BC_OFF(); + + /* COMMAND */ + virtual void pid_defaut_set(uint8_t address); + virtual void pid_tq_off(uint8_t address); + virtual void pid_break(uint8_t address); + virtual void pid_stop(uint8_t address); + virtual void pid_command(uint8_t address, PID_COMMAND cmd); + virtual void pid_alarm_reset(uint8_t address); + virtual void pid_posi_reset(uint8_t address); + virtual void pid_main_bc_state(uint8_t address, _BIT data); + virtual void pid_monitor_bc_state(uint8_t address, _BIT data); + virtual void pid_preset_save(uint8_t address, uint8_t data); + virtual void pid_preset_recall(uint8_t address, uint8_t data); + virtual void pid_vel_cmd(uint8_t address, int16_t speed); + virtual void pid_vel_cmd2(uint8_t address, int16_t high_speed); + virtual void pid_open_vel_cmd(uint8_t address, int16_t data); + + bool err{}; + int err_no{}; + +private: + id_number_t slave_id; + uint8_t rate = 100; + boost::thread ThreadOne,ThreadTwo; + bool theadEnable; + filter_t cs; + ros::NodeHandlePtr nh_; + XmlRpc::XmlRpcValue drivers; + + virtual uint8_t GetcheckSum(uint length, uint8_t *to_send); + virtual void buidRequest(uint8_t *to_send,header_t hd ,uint8_t add, param_id_t pid, dt_num_t num) const; + virtual int Read(uint8_t *read_buf, ssize_t &num); + virtual int Write(header_t hd ,uint8_t address, param_id_t pid, dt_num_t num, uint8_t *value); + virtual bool check_baudrate(unsigned int baud); + virtual void set_bad_con(void); + // virtual void loadParam(void); + + /*Write*/ + virtual void pid_id_setting(uint8_t id_setting); + virtual void pid_baudrate_setting(uint8_t address, _BAUD_OF_MD200 baudrate_setting); + /* READ */ + // static void cmdMainBCdoStuff(void *payload, uint8_t *publish_rate); +}; + +} // namespace MD + +#endif // __MD_CTRL_H_INCLUDED diff --git a/Devices/Libraries/Ros/md_controller/include/md_controller/md_controller_subscriber.h b/Devices/Libraries/Ros/md_controller/include/md_controller/md_controller_subscriber.h new file mode 100755 index 0000000..1d39a09 --- /dev/null +++ b/Devices/Libraries/Ros/md_controller/include/md_controller/md_controller_subscriber.h @@ -0,0 +1,70 @@ +#ifndef __MD_CTRL_SUBSCRIBER_H_INCLUDED +#define __MD_CTRL_SUBSCRIBER_H_INCLUDED + +#include +#include +#include + +namespace MD +{ + class Subscriber + { + public: + /* + * @brief Constructor. + * @param[in] nh ros::NodeHandle + * @param[in] ros_topic topic for ros messages + * @param[in] subscribe_queue_size buffer size for ros messages + * @param[in] wait_time Time for checking to Wheel stop (1 second) + */ + Subscriber(ros::NodeHandle & nh, const std::string & ros_topic, int subscribe_queue_size = 1, double schedule_delay = 0.05, double schedule_delay_lastest = 0.5); + + Subscriber(ros::NodeHandle & nh, XmlRpc::XmlRpcValue::iterator object); + + /** + * @brief returns true, if publishing of a wheel topic is publishing + * + */ + bool isWheelTriggered(void); + + /** + * @brief returns true, if not subcribe wheel + */ + bool isWheelLastestTriggered(void); + + /** + * @brief schedules WheelController function + * @param[in] schedule if true, publishing is scheduled, otherwise a possibly pending schedule is removed. + */ + void scheduleWheelController(bool schedule); + + /* + * @brief Destructor. + */ + virtual ~Subscriber(); + + boost::mutex m_measurement_mutex; + int16_t speed; + uint8_t slave_id; + double ratio; + + protected: + /* + * @brief Callbacks for ros messages from wheel topic. + */ + virtual void wheelCallback(const std_msgs::Float32::Ptr & msg); + + ros::NodeHandle nh_; + std::string topic_name_; + int m_subscribe_queue_size_; + ros::Subscriber control_wheel_sub_; + + ros::Time publish_time_; + ros::Time publish_lastest_time_; + ros::Duration schedule_delay_; + ros::Duration schedule_lastest_delay_; + boost::mutex publish_mutex_; + }; +} + +#endif // __MD_CTRL_SUBSCRIBER_H_INCLUDED \ No newline at end of file diff --git a/Devices/Libraries/Ros/md_controller/launch/run.launch b/Devices/Libraries/Ros/md_controller/launch/run.launch new file mode 100755 index 0000000..fdd7bee --- /dev/null +++ b/Devices/Libraries/Ros/md_controller/launch/run.launch @@ -0,0 +1,13 @@ + + + + + + + + + + + + + \ No newline at end of file diff --git a/Devices/Libraries/Ros/md_controller/motorInfomation.yaml b/Devices/Libraries/Ros/md_controller/motorInfomation.yaml new file mode 100755 index 0000000..e763369 --- /dev/null +++ b/Devices/Libraries/Ros/md_controller/motorInfomation.yaml @@ -0,0 +1,15 @@ + +port_name: /dev/ttyTHS0 +baudrate: 19200 +producte : no_name +drivers: + lelf_driver: + id: 0x02 + ratio: -30.0 + toppic: "/left_wheel" + subscribe_queue_size: 1 + right_driver: + id: 0x01 + ratio: 30.0 + toppic: "/right_wheel" + subscribe_queue_size: 1 \ No newline at end of file diff --git a/Devices/Libraries/Ros/md_controller/package.xml b/Devices/Libraries/Ros/md_controller/package.xml new file mode 100755 index 0000000..bdc9196 --- /dev/null +++ b/Devices/Libraries/Ros/md_controller/package.xml @@ -0,0 +1,74 @@ + + + md_controller + 0.0.0 + The md_controller package + + + + + robotics + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + roscpp + rospy + std_msgs + libserial + + roscpp + rospy + std_msgs + libserial + + roscpp + rospy + std_msgs + libserial + + + + + + + + + diff --git a/Devices/Libraries/Ros/md_controller/src/diagnostic.cpp b/Devices/Libraries/Ros/md_controller/src/diagnostic.cpp new file mode 100755 index 0000000..f0ccd6a --- /dev/null +++ b/Devices/Libraries/Ros/md_controller/src/diagnostic.cpp @@ -0,0 +1,76 @@ +#include +#include +#include "md_controller/diagnostic.h" + + +/** + * g_diagnostic_handler: singleton, implements the diagnostic for sick_line_guidance + */ +MD::Diagnostic::DiagnosticImpl MD::Diagnostic::g_diagnostic_handler; + + +/* + * Constructor. + */ +MD::Diagnostic::DiagnosticImpl::DiagnosticImpl() : m_diagnostic_initialized(false), m_diagnostic_component("") +{ +} + +/* + * Initialization. + * + * @param[in] nh ros::NodeHandle + * @param[in] publish_topic ros topic to publish diagnostic messages + * @param[in] component description of the component reporting + */ +void MD::Diagnostic::DiagnosticImpl::init(ros::NodeHandle &nh, const std::string & publish_topic, const std::string & component) +{ + m_diagnostic_publisher = nh.advertise(publish_topic, 1); + m_diagnostic_component = component; + m_diagnostic_initialized = true; +} + +/* + * Updates and reports the current status. + * + * @param[in] status current status (OK, ERROR, ...) + * @param[in] message optional diagnostic message + */ +void MD::Diagnostic::DiagnosticImpl::update(DIAGNOSTIC_STATUS status, const std::string &message) +{ + if (m_diagnostic_initialized) + { + static std::map status_description = { + {DIAGNOSTIC_STATUS::OK, "OK"}, + {DIAGNOSTIC_STATUS::EXIT, "EXIT"}, + {DIAGNOSTIC_STATUS::ERROR_STATUS, "ERROR_STATUS"}, + {DIAGNOSTIC_STATUS::CONFIGURATION_ERROR, "CONFIGURATION_ERROR"}, + {DIAGNOSTIC_STATUS::INITIALIZATION_ERROR, "INITIALIZATION_ERROR"}, + {DIAGNOSTIC_STATUS::INTERNAL_ERROR, "INTERNAL_ERROR"} + }; + + // create DiagnosticStatus + diagnostic_msgs::DiagnosticStatus msg; + msg.level = (status == DIAGNOSTIC_STATUS::OK) ? (diagnostic_msgs::DiagnosticStatus::OK) : (diagnostic_msgs::DiagnosticStatus::ERROR); // Level of operation + msg.name = m_diagnostic_component; // description of the test/component reporting + msg.hardware_id = ""; // hardware unique string (tbd) + msg.values.clear(); // array of values associated with the status + // description of the status + msg.message = status_description[status]; + if(msg.message.empty()) + { + msg.message = "ERROR"; + } + if (!message.empty()) + { + msg.message = msg.message + ": " + message; + } + // publish DiagnosticStatus in DiagnosticArray + diagnostic_msgs::DiagnosticArray msg_array; + msg_array.header.stamp = ros::Time::now(); + msg_array.header.frame_id = ""; + msg_array.status.clear(); + msg_array.status.push_back(msg); + m_diagnostic_publisher.publish(msg_array); + } +} \ No newline at end of file diff --git a/Devices/Libraries/Ros/md_controller/src/md_controller.cpp b/Devices/Libraries/Ros/md_controller/src/md_controller.cpp new file mode 100755 index 0000000..eda6c0c --- /dev/null +++ b/Devices/Libraries/Ros/md_controller/src/md_controller.cpp @@ -0,0 +1,666 @@ +#include "md_controller/md_controller.h" + + +MD::MdController::~MdController(){} +/* + * Object + * @param devfile Path name of dev + * @param baud Baudrate + * @param parity Parity + * @param data_bit Data bit + * @param stop_bit Stop bit + */ +MD::MdController::MdController(ros::NodeHandlePtr nh, const char *devfile, unsigned int baud, parity_t parity, data_bits_t data_bit,stop_bits_t stop_bit) + : nh_(nh), rs485(devfile,baud, parity, data_bit, stop_bit) +{ + if(this->_connected == true) + { + if(!this->check_baudrate(baud)) + { + std::stringstream msg; + msg << "DRIVER not support baudrate" << baud << "bps"; + MD::Diagnostic::update(MD::DIAGNOSTIC_STATUS::CONFIGURATION_ERROR, msg.str()); + ROS_ERROR_STREAM("md_controller_node: " << msg.str()); + this->close_port(); + exit(-1); + } + else + { + std::stringstream msg; + msg << " Connected "<err = false; + this->err_no = 0; + ROS_INFO_STREAM("md_controller_node: " << msg.str()); + ros::Duration(0.7).sleep(); // sleep for 0.5 second + MD::Diagnostic::update(MD::DIAGNOSTIC_STATUS::OK, msg.str()); + } + } + else + { + ros::Duration(2).sleep(); + std::stringstream err; + err << error_msg; + ROS_ERROR_STREAM(error_msg); + MD::Diagnostic::update(MD::DIAGNOSTIC_STATUS::INITIALIZATION_ERROR, error_msg); + } +} + +// /** +// * Get parameter from yaml file +// */ +// void MD::MdController::loadParam(void) +// { +// int node_id[this->drivers.size()]; +// int count = 0; + +// for(XmlRpc::XmlRpcValue::iterator _iter = this->drivers.begin(); _iter != this->drivers.end(); ++_iter) +// { +// std::string driver_name = readMember(_iter->second, "name", _iter->first); +// if(_iter->second.hasMember("id")) +// { +// node_id[count] = _iter->second["id"]; +// ROS_INFO("md_controller: initializing driver_name \"%s\", id \"0x0%d\"...", driver_name.c_str(),node_id[count]); +// count++; +// } else +// { +// ROS_ERROR_STREAM("Node '" << _iter->first << "' has no id"); +// MD::Diagnostic::update(MD::DIAGNOSTIC_STATUS::CONFIGURATION_ERROR, " Node name has no id"); +// } +// } +// this->set_slave_id(node_id,count); +// } + +/* + * Check baudrate has supported for device + * @param baud Package Array size byte + * @param return true/false + */ +bool MD::MdController::check_baudrate(unsigned int baud) +{ + bool status = false; + unsigned int data_defaut[] = {9600, 19200, 38400, 57600, 115200}; + for(uint8_t i = 0; i < (uint8_t)sizeof(data_defaut)/sizeof(unsigned int); i++) + { + if(baud == data_defaut[i]) + { + status = true; + break; + } + } + return status; +} + +/* + * Checksum + * @param length Package Array size byte + * @param to_send Package Array + * @param return byte + */ +uint8_t MD::MdController::GetcheckSum(uint length, uint8_t *to_send) +{ + uint8_t byTmp = 0; + for(short i = 0; i < length; i++ ) byTmp += *(to_send + i); + return (~byTmp +1); + +} //GetcheckSum + +/** + * Set connection is bad + */ +void MD::MdController::set_bad_con(void) +{ + this->err = true; + ROS_ERROR("BAD CONNECTION"); +} + +/* + * Modbus Request Builder + * @param to_send Message Buffer input + * @param hd Header + * @param add ID Number + * @param pid Parameter ID + * @param num Data number + */ +void MD::MdController::buidRequest(uint8_t *to_send,header_t hd , + uint8_t address, param_id_t pid, dt_num_t num) const +{ + to_send[0] = hd.RMID; + to_send[1] = hd.TMID; + to_send[2] = address; + to_send[3] = pid.PID; + to_send[4] = num.NUM; +} // buidRequest + +/* + * Write Request Builder and Sender + * @param address Reference Address + * @param lenght Amount of data to be Written + * @param value Data to Be Written + */ +int MD::MdController::Write(header_t hd ,uint8_t address, param_id_t pid, + dt_num_t num, uint8_t *value) +{ + int status = 0; + uint length = num.NUM + 5; + uint8_t to_send[length+1]; + this->buidRequest(to_send,hd,address,pid,num); + for(uint8_t i = 0; i < num.NUM; i++) to_send[length -num.NUM +i] = value[i]; + to_send[length] = this->GetcheckSum(length,to_send); + struct stat sb; + if(stat(this->ctx._port, &sb) < 0) this->reconnect(); + if(this->_connected) + { + status = this->sendMsgs(to_send,length+1); + } + return status; +} + +/* + * Read Request Builder and Sender + * @param address Reference Address + * @param lenght Amount of Data to Read + * @param return EXIT_CODES Type + */ +int MD::MdController::Read(uint8_t *read_buf, ssize_t &num) +{ + ssize_t k = this->receiveMsgs(read_buf); + num = k; + if(k < 0) + { + this->set_bad_con(); + return EXIT_CONFIG_ERROR; + } + else{ + return EXIT_OK; + } + +} + +/* + * Default setting + * @param address ID + * Data : 0x55(CHECK) + * 183, 172, ID, 3, 1, 0x55, CHK + */ +void MD::MdController::pid_defaut_set(uint8_t address) +{ + header_t hd; hd.TMID = 184; + param_id_t pid; pid.PID = 3; + dt_num_t num; num.NUM = 1; + uint8_t value[num.NUM]; + value[num.NUM - 1] = 0x55u; //Data : 0x55(CHECK) + this->Write(hd,address,pid,num,value); +} + +/* + * Stop naturally + * @param address ID + * Stop motor naturally, data don’t care(x). + * 183, 172, ID, 5, 1, x, CHK + */ +void MD::MdController::pid_tq_off(uint8_t address) +{ + header_t hd; hd.TMID = 184; + param_id_t pid; pid.PID = 5; + dt_num_t num; num.NUM = 1; + uint8_t value[num.NUM]; + value[num.NUM - 1] = 0X01u; //data don’t care(x). + this->Write(hd,address,pid,num,value); +} + +/* + * Erectric brake + * @param address ID + * Stop motor urgently(electric braking mode) + * 183, 172, ID, 6, 1, x, CHK + */ +void MD::MdController::pid_break(uint8_t address) +{ + header_t hd; hd.TMID = 184; + param_id_t pid; pid.PID = 6; + dt_num_t num; num.NUM = 1; + uint8_t value[num.NUM]; + value[num.NUM - 1] = 0x01u; //data don’t care(x). + this->Write(hd,address,pid,num,value); +} + +/* + * stop + * @param address ID + * Stop motor urgently(electric braking mode) + * 183, 172, ID, 6, 1, x, CHK + */ +void MD::MdController::pid_stop(uint8_t address) +{ + header_t hd; hd.TMID = 184; + param_id_t pid; pid.PID = 5; + dt_num_t num; num.NUM = 1; + uint8_t value[num.NUM]; + value[num.NUM - 1] = 0x01u; //data don’t care(x). + this->Write(hd,address,pid,num,value); +} + +/* + * Pid command + * @param address ID + * @param cmd Contents on CMD number (forllow page 6-CommunicationProtocolOnRS485_V44) + * 183, 172, ID, 10, 1, CMD, CHK + */ +void MD::MdController::pid_command(uint8_t address, PID_COMMAND cmd) +{ + header_t hd; hd.TMID = 184; + param_id_t pid; pid.PID = 10; + dt_num_t num; num.NUM = 1; + uint8_t value[num.NUM]; + value[num.NUM - 1] = cmd; + this->Write(hd,address,pid,num,value); +} + +/* + * Reset alarm + * @param address ID + * Data don’t care(x) + * 183, 172, ID, 12, 1, x, CHK + */ +void MD::MdController::pid_alarm_reset(uint8_t address) +{ + header_t hd; hd.TMID = 184; + param_id_t pid; pid.PID = 12; + dt_num_t num; num.NUM = 1; + uint8_t value[num.NUM]; + value[num.NUM - 1] = 0X01u; //data don’t care(x). + this->Write(hd,address,pid,num,value); +} + +/* + * Reset position, Motor position to zero + * @param address ID + * Data don’t care(x) + * 183, 172, ID, 13, 1, x, CHK + */ +void MD::MdController::pid_posi_reset(uint8_t address) +{ + header_t hd; hd.TMID = 184; + param_id_t pid; pid.PID = 13; + dt_num_t num; num.NUM = 1; + uint8_t value[num.NUM]; + value[num.NUM - 1] = 0X01u; //data don’t care(x). + this->Write(hd,address,pid,num,value); +} + +/* + * Request broadcasting of PID_MAIN_DATA + * @param address ID + * @param data DATA 1 : PID 193 broadcasting on OR 0 : broasdcasting off + * 183, 172, ID, 14, 1, DATA, CHK + */ +void MD::MdController::pid_main_bc_state(uint8_t address, _BIT data) +{ + header_t hd; hd.TMID = 184; + param_id_t pid; pid.PID = 14; + dt_num_t num; num.NUM = 1; + uint8_t value[num.NUM]; + value[num.NUM - 1] = data; + this->Write(hd,address,pid,num,value); +} + +/* + * Request BC on/off of PID_MONITOR + * @param address ID + * @param data DATA 1 : PID 196 broadcasting on OR 0 : broasdcasting off + * 183, 172, ID, 15, 1, DATA, CHK + */ +void MD::MdController::pid_monitor_bc_state(uint8_t address, _BIT data) +{ + header_t hd; hd.TMID = 184; + param_id_t pid; pid.PID = 15; + dt_num_t num; num.NUM = 1; + uint8_t value[num.NUM]; + value[num.NUM - 1] = data; + this->Write(hd,address,pid,num,value); +} + +/* + * Save preset position + * @param address ID + * @param data DATA Preset number(address, 1~20) + * Set current position to the preset address + * 183, 172, ID, 31, 1, DATA, CHK + */ +void MD::MdController::pid_preset_save(uint8_t address, uint8_t data) +{ + header_t hd; hd.TMID = 184; + param_id_t pid; pid.PID = 31; + dt_num_t num; num.NUM = 1; + uint8_t value[num.NUM]; + if(data > 0 && data <=20) + { + value[num.NUM - 1] = data; + this->Write(hd,address,pid,num,value); + } + else + { + ROS_ERROR("Command 'PID_PRESET_SAVE' element 'data' number must in ranges (1~20). Prefer page 10 CommunicationProtocolOnRS485_V44"); + + } +} + +/* + * Go to the recalled preset position + * @param address ID + * @param data DATA Preset number(address, 1~20) + * Recall the saved preset data and move to thatposition.(position control) + * 183, 172, ID, 32, 1, DATA, CHK + */ +void MD::MdController::pid_preset_recall(uint8_t address, uint8_t data) +{ + header_t hd; hd.TMID = 184; + param_id_t pid; pid.PID = 32; + dt_num_t num; num.NUM = 1; + uint8_t value[num.NUM]; + if(data > 0 && data <=20) + { + value[num.NUM - 1] = data; + this->Write(hd,address,pid,num,value); + } + else + { + ROS_ERROR("Command 'PID_PRESET_RECALL' element 'data' number must in ranges (1~20). Prefer page 10 CommunicationProtocolOnRS485_V44"); + } + +} + +/* + * Velocity command + * @param address ID + * @param speed Speed(rpm) = (D1 | D2<<8) + * Speed>0, CCW direction + * Speed<0, CW direction + * 183, 172, ID, 130, 2, D1, D2, CHK + */ +void MD::MdController::pid_vel_cmd(uint8_t address, int16_t speed) +{ + header_t hd; hd.TMID = 184; + param_id_t pid; pid.PID = 130; + dt_num_t num; num.NUM = 2; + uint8_t value[num.NUM]; + value[num.NUM - 2] = speed & 0x00FFu; + value[num.NUM - 1] = speed >> 8u; + this->Write(hd,address,pid,num,value); +} + +/* + * Velocity command used more than 25,000rpm + * @param address ID + * @param high_speed Speed(rpm) = (D1 | D2<<8) x 10 + * Speed>0, CCW direction + * Speed<0, CW direction + * 183, 172, ID, 131, 2, D1, D2, CHK + */ +void MD::MdController::pid_vel_cmd2(uint8_t address, int16_t high_speed) +{ + header_t hd; hd.TMID = 184; + param_id_t pid; pid.PID = 131; + dt_num_t num; num.NUM = 2; + uint8_t value[num.NUM]; + value[num.NUM - 2] = high_speed*10 & 0x00FFu; + value[num.NUM - 1] = high_speed*10 >> 8u; + this->Write(hd,address,pid,num,value); +} + +/* + * Velocity command used more than 25,000rpm + * @param address ID + * @param data D1, D2: Open-loop velocity + * Range : -1023~1023 + * 183, 172, ID, 134, 2, D1, D2, CHK + */ +void MD::MdController::pid_open_vel_cmd(uint8_t address, int16_t data) +{ + header_t hd; hd.TMID = 184; + param_id_t pid; pid.PID = 134; + dt_num_t num; num.NUM = 2; + uint8_t value[num.NUM]; + if(data >= -1023 && data <= 1023) + { + value[num.NUM - 2] = data & 0x00FFu; + value[num.NUM - 1] = data >> 8u; + this->Write(hd,address,pid,num,value); + } + else + { + ROS_ERROR("Command 'PID_PRESET_RECALL' element 'data' number must in ranges (-1023 ~ 1023). Prefer page 10 CommunicationProtocolOnRS485_V44"); + } +} + +/* + * ID setting + * @param id_setting ID : 1~253 : setting ID + * Write command(0xaa) + * 183, 172, 254, 133, 2, 0xaa, ID, CHK + */ +void MD::MdController::pid_id_setting(uint8_t id_setting) +{ + header_t hd; hd.TMID = 184; + param_id_t pid; pid.PID = 133; + dt_num_t num; num.NUM = 2; + uint8_t value[num.NUM]; + if(id_setting > 0 && id_setting <=253) + { + value[num.NUM - 2] = 0xAAu; + value[num.NUM - 1] = id_setting; + this->Write(hd,254,pid,num,value); + } + else + { + ROS_ERROR("Command 'PID_ID' element 'id_setting' number must in ranges (1 ~ 253). Prefer page 12 CommunicationProtocolOnRS485_V44"); + } + +} + +/* + * Set the baudrate, BAUD + * @param address ID + * @param baudrate_setting Baudrate setting(RS485) + * Write command(0xaa) + * 183, 172, ID, 135, 2, 0xaa, BAUD, CHK + */ +void MD::MdController::pid_baudrate_setting(uint8_t address, _BAUD_OF_MD200 baudrate_setting) +{ + header_t hd; hd.TMID = 184; + param_id_t pid; pid.PID = 133; + dt_num_t num; num.NUM = 2; + uint8_t value[num.NUM]; + value[num.NUM - 2] = 0xAAu; + value[num.NUM - 1] = (uint8_t)baudrate_setting; + this->Write(hd,address,pid,num,value); +} + +// /* +// * Set slave id +// * @param ids Pointer save array slave id +// * @param lenght Lenght of ids array (sizeof(ids)/sizeof(uint8_t)) +// */ +// void MD::MdController::set_slave_id(int *ids, uint8_t lenght) +// { +// uint8_t n = lenght; +// for(uint8_t i = 0; i < n; i++) +// { +// for(int j = 0; j < n; j++) +// { +// if(ids[j] == ids[i] && j != i) +// { +// ids[j] = ids[j+1]; +// n--; +// } +// } +// } +// if(n != lenght) ROS_WARN("There are %d ID input to be same!!", lenght - n); +// this->slave_id.size = n; +// for(uint8_t k = 0; k < n; k++) this->slave_id.adr[k] = ids[k]; +// } + +// /* +// * Set slave id +// * @param speed Speed list input +// */ +// void MD::MdController::publish_speed(int16_t *speed) +// { +// if(this->slave_id.size > 0) +// { +// for(uint8_t i = 0; i < this->slave_id.size; i++) +// { +// if(abs(speed[i]) > 0) +// this->pid_vel_cmd(this->slave_id.adr[i],speed[i]); +// else +// this->pid_break(this->slave_id.adr[i]); +// } +// }else +// { +// ros::Duration(3).sleep(); // sleep for 3 second +// ROS_ERROR("There are not slave ID of devices MD Motor"); +// } + +// } + +// /* +// * PID_MAIN_DATA broadcasting ON +// * @param CMD_MAIN_BC_ON 5 +// */ +// void MD::MdController::main_BC_ON() +// { +// if(this->slave_id.size > 0) +// { +// for(uint8_t i = 0; i < this->slave_id.size; i++) +// { +// this->pid_command(this->slave_id.adr[i],CMD_MAIN_BC_ON); +// usleep(30000); +// } +// this->theadEnable = true; +// if(this->ThreadOne.joinable() == false) +// { +// this->ThreadOne = boost::thread(this->cmdMainBCdoStuff,this,&this->rate); +// } +// }else +// { +// ros::Duration(3).sleep(); // sleep for 3 second +// ROS_ERROR("There are not slave ID of devices MD Motor"); +// } +// } + +// /* +// * PID_MAIN_DATA broadcasting OFF +// * @param CMD_MAIN_BC_OFF 6 +// */ +// void MD::MdController::main_BC_OFF() +// { +// if(this->slave_id.size > 0) +// { +// for(uint8_t i = 0; i < this->slave_id.size; i++) +// { +// this->pid_command(this->slave_id.adr[i],CMD_MAIN_BC_OFF); +// usleep(30000); +// } +// this->theadEnable = false; +// if(this->ThreadOne.joinable() == true) +// this->ThreadOne.join(); +// }else +// { +// ros::Duration(3).sleep(); // sleep for 3 second +// ROS_ERROR("There are not slave ID of devices MD Motor"); +// } +// } + +// /* +// * Read data +// * @param payload This +// * @param publish_rate Loop rate +// */ +// void MD::MdController::cmdMainBCdoStuff(void *payload, uint8_t *publish_rate) +// { +// MD::MdController *as = (MD::MdController *)payload; +// ros::Rate loop_rate(*publish_rate); +// ROS_INFO("TheadOne to read msg devices motor is ready !!"); +// while(ros::ok() && as->theadEnable) +// { +// ssize_t num; +// uint8_t msg[256]; +// as->Read(msg,num); + +// as->cs.w_index += num; +// if(as->cs.w_index >= MAX_BUFFER_LENGHT) { +// uint8_t l_part = MAX_BUFFER_LENGHT - (as->cs.w_index - num); +// for(uint8_t i = 0; i < l_part; i++) as->cs._buffer[as->cs.w_index - num + i] = msg[i]; +// as->cs.w_index = num - l_part; +// for(uint8_t i = l_part; i < num; i++) as->cs._buffer[as->cs.w_index - num + l_part + i] = msg[i]; +// as->cs.c_flag++; +// } +// else { +// for(uint8_t i = 0; i < num; i++) as->cs._buffer[as->cs.w_index - num + i] = msg[i]; +// } + +// if(as->cs.w_index - as->cs.r_index > PACKAGE_MSG_MAIN_DATA || as->cs.c_flag > 0) +// { +// if(as->cs._buffer[as->cs.r_index] == 172 && as->cs._buffer[as->cs.r_index + 1] == 183) +// { +// int num_data = as->cs._buffer[as->cs.r_index + 4]; +// if(as->cs.w_index - as->cs.r_index >= 6 + num_data) +// { +// as->cs.r_index += 6 + num_data; + +// if(as->cs.r_index >= MAX_BUFFER_LENGHT) +// { +// as->cs.c_flag--; +// uint8_t l_part = MAX_BUFFER_LENGHT - (as->cs.r_index - PACKAGE_MSG_MAIN_DATA); +// for(uint8_t i = 0; i < l_part; i++) +// as->cs.result[i] = as->cs._buffer[as->cs.r_index - PACKAGE_MSG_MAIN_DATA + i]; +// as->cs.r_index = PACKAGE_MSG_MAIN_DATA - l_part; +// for(uint8_t i = l_part; i < PACKAGE_MSG_MAIN_DATA; i++) +// as->cs.result[i] = as->cs._buffer[as->cs.r_index - PACKAGE_MSG_MAIN_DATA + l_part + i ]; +// } +// else +// { +// for(uint8_t i = 0; i < PACKAGE_MSG_MAIN_DATA; i++) +// as->cs.result[i] = as->cs._buffer[as->cs.r_index - PACKAGE_MSG_MAIN_DATA + i]; +// } +// std::cout << "num_data " << std::dec << num_data << " "; +// // std::cout << "w_index " << std::dec << (int)(as->cs.w_index) << " r_index = " << std::dec << (int)(as->cs.r_index) << " "; +// // std::cout << " c_flags = " << std::dec << (int)(as->cs.c_flag); +// //std::cout << " result = "; +// // for(int i = 0; i < 22; i++) +// // { +// // std::cout << std::hex << (int)as->cs.result[i] << " "; +// // } +// std::cout << std:: endl; +// } +// } +// else +// { +// std::cout << " buffer = "; +// for(int i = as->cs.r_index; i < as->cs.r_index + 22; i++) +// { +// std::cout << std::hex << (int)as->cs._buffer[i] << " "; +// } +// std::cout << std:: endl; +// as->cs.r_index ++; +// } +// if(as->cs.c_flag > 1) +// { +// ROS_WARN("ERROR DATA"); +// exit(-1); +// } +// } +// std::cout << "w_index = " << std::dec << (int)(as->cs.w_index) << " r_index = " << std::dec << (int)(as->cs.r_index) << " "; +// std::cout << " c_flags = " << std::dec << (int)(as->cs.c_flag); +// // std::cout << " buffer = "; +// // for(int i = 0; i < 300; i++) +// // { +// // std::cout << std::hex << (int)as->cs._buffer[i] << " "; +// // } +// std::cout << std:: endl; + +// loop_rate.sleep(); +// ros::spinOnce(); +// } + +// } diff --git a/Devices/Libraries/Ros/md_controller/src/md_controller_node.cpp b/Devices/Libraries/Ros/md_controller/src/md_controller_node.cpp new file mode 100755 index 0000000..9ef6ff1 --- /dev/null +++ b/Devices/Libraries/Ros/md_controller/src/md_controller_node.cpp @@ -0,0 +1,146 @@ +#include +#include +#include +#include "md_controller/md_controller.h" +#include "md_controller/diagnostic.h" +#include "md_controller/md_controller_subscriber.h" +#include "libserial/udevadm_info.h" +#include + +/* define struct */ +typedef struct device{ + std::string _port; + int _baud; +}device_t; + + +bool findDevice(ros::NodeHandlePtr nh, std::string node_name, device_t *dv) { + ROS_INFO("loadParam() - node_name: %s", node_name.c_str()); + if(!nh->param(node_name + "/baudrate", dv->_baud, dv->_baud)) + { + if(!nh->getParam("baudrate", dv->_baud)) + return false; + } + + ROS_INFO("product"); + std::string product; //port name + if(!nh->param(node_name + "/product", product, product)) {} + else if(!nh->getParam("product", product)) {} + else + { + udevadmInfo *ludev = new udevadmInfo(product.c_str()); + if(ludev->init() == EXIT_FAILURE) {} + if(ludev->scanDevices()) + { + strcpy(dv->_port.data(), ludev->port); + return true; + } + } + + ROS_INFO("port_name"); + if(!nh->param(node_name + "/port_name", dv->_port, dv->_port)) + { + if(!nh->getParam("port_name", dv->_port)) + return false; + } + + ROS_WARN("Can not scan device from /product. So try to use /port_name is %s", dv->_port.c_str()); + return true; + +} + +/** + * @brief shortcut to read a member from a XmlRpcValue, or to return a defaultvalue, it the member does not exist + */ +template static T readMember(XmlRpc::XmlRpcValue & value, const std::string & member, const T & defaultvalue) +{ + try + { + if(value.hasMember(member)) + return value[member]; + return defaultvalue; + } + catch(const XmlRpc::XmlRpcException& e) + { + ROS_WARN_STREAM( "'"<(); + std::string node_name = ros::this_node::getName(); + ROS_INFO("%s.cpp-node_name: %s", node_name.c_str(), node_name.c_str()); + + /* connect with the server */ + std::string diagnostic_topic = "MD_diagnostic"; + MD::Diagnostic::init(*nh, diagnostic_topic, "MD md_controller"); + + + XmlRpc::XmlRpcValue drivers; + if(!nh->getParam("drivers", drivers) || drivers.size() < 1) + { + ROS_ERROR("%s: no driver found in yaml-file, please check configuration. Aborting...", node_name.c_str()); + MD::Diagnostic::update(MD::DIAGNOSTIC_STATUS::CONFIGURATION_ERROR, " No driver found in yaml-file"); + return 2; + } + + std::vector> p_md_controller_subscriber; + for(XmlRpc::XmlRpcValue::iterator object_iter = drivers.begin(); object_iter != drivers.end(); ++object_iter) + { + std::string topic_name = readMember(object_iter->second, "toppic", ""); + int subscribe_queue_size = readMember(object_iter->second, "subscribe_queue_size", 1); + ROS_INFO("%s: initializing toppic \"%s\" queue_size \"%d\"...", node_name.c_str(), topic_name.c_str(), subscribe_queue_size); + p_md_controller_subscriber.push_back(boost::make_shared(*nh, object_iter)); + } + + device_t dv; + if(findDevice(nh, node_name, &dv)) + { + ROS_INFO("%s: found device successfull", node_name.c_str()); + } + else + { + ROS_ERROR("%s: Error when find device", node_name.c_str()); + return 2; + } + + uint8_t rate = 20; // Hz + ros::Rate loop_rate(rate); + while (ros::ok()) + { + ROS_INFO("%s: Conecting port name %s with baudrate %d ...", node_name.c_str(), dv._port.c_str(), dv._baud); + boost::shared_ptr md_controller = boost::make_shared(nh, dv._port.c_str(), dv._baud, PARITY_NONE, DATABIT_8, STOPBIT_1); + while(ros::ok() && md_controller->_connected) + { + for(auto &p_sub : p_md_controller_subscriber) + { + if(p_sub->isWheelTriggered()) + { + double rotate_speed_wheel; + { + boost::lock_guard publish_lockguard(p_sub->m_measurement_mutex); + rotate_speed_wheel = p_sub->speed; + } + if(abs(rotate_speed_wheel) > 0) + md_controller->pid_vel_cmd(p_sub->slave_id, rotate_speed_wheel); + else + md_controller->pid_break(p_sub->slave_id); + p_sub->scheduleWheelController(false); + } + } + loop_rate.sleep(); + ros::spinOnce(); + } + ros::spinOnce(); + md_controller->close_port(); + } + ros::spin(); + return 0; +} + diff --git a/Devices/Libraries/Ros/md_controller/src/md_controller_subscriber.cpp b/Devices/Libraries/Ros/md_controller/src/md_controller_subscriber.cpp new file mode 100755 index 0000000..e05c930 --- /dev/null +++ b/Devices/Libraries/Ros/md_controller/src/md_controller_subscriber.cpp @@ -0,0 +1,102 @@ +#include "md_controller/md_controller_subscriber.h" +#include + +/** + * @brief shortcut to read a member from a XmlRpcValue, or to return a defaultvalue, it the member does not exist + */ +template static T readMember(XmlRpc::XmlRpcValue & value, const std::string & member, const T & defaultvalue) +{ + try + { + if(value.hasMember(member)) + return value[member]; + return defaultvalue; + } + catch(const XmlRpc::XmlRpcException& e) + { + return defaultvalue; + } + +} + +MD::Subscriber::Subscriber(ros::NodeHandle & nh, const std::string & ros_topic, int subscribe_queue_size, double schedule_delay, double schedule_delay_lastest) + : nh_(nh), slave_id(0), + topic_name_(ros_topic), + m_subscribe_queue_size_(subscribe_queue_size), + schedule_delay_(ros::Duration(schedule_delay)), + schedule_lastest_delay_(ros::Duration(schedule_delay_lastest)), + publish_time_(ros::Time(0)), + publish_lastest_time_(ros::Time(0)) +{ + control_wheel_sub_ = nh_.subscribe(topic_name_, m_subscribe_queue_size_, &MD::Subscriber::wheelCallback, this); +} + +MD::Subscriber::Subscriber(ros::NodeHandle & nh, XmlRpc::XmlRpcValue::iterator object) + : nh_(nh), slave_id(0), + publish_time_(ros::Time(0)), + publish_lastest_time_(ros::Time(0)) +{ + topic_name_ = readMember(object->second, "toppic", ""); + m_subscribe_queue_size_ = readMember(object->second, "subscribe_queue_size", 1); + slave_id = (uint8_t)readMember(object->second, "id", 1); + ratio = (double)readMember(object->second, "ratio", 0.0); + ratio = readMember(object->second, "ratio", ratio); + double schedule_delay = readMember(object->second, "schedule_delay", 0.05); + double schedule_delay_lastest = readMember(object->second, "schedule_delay_lastest", 0.5); + + this->schedule_delay_ = ros::Duration(schedule_delay); + this->schedule_lastest_delay_ = ros::Duration(schedule_delay_lastest); + control_wheel_sub_ = nh_.subscribe(topic_name_, m_subscribe_queue_size_, &MD::Subscriber::wheelCallback, this); +} + +MD::Subscriber::~Subscriber() = default; + + +void MD::Subscriber::wheelCallback(const std_msgs::Float32::Ptr & msg) +{ + boost::lock_guard schedule_lockguard(this->m_measurement_mutex); + // check that we don't have multiple publishers on the command topic + if (control_wheel_sub_.getNumPublishers() > 1) + { + ROS_ERROR_STREAM_THROTTLE(1.0, "Detected " << control_wheel_sub_.getNumPublishers() + << " publishers " << topic_name_ << ". Only 1 publisher is allowed. Going to brake."); + speed = 0; + return; + } + + if (!std::isfinite(msg->data)) + { + ROS_WARN_THROTTLE(1.0, "Received NaN in %s command. Ignoring.", topic_name_.c_str()); + speed = 0; + return; + } + speed = (int16_t)(msg->data * ratio * 60 / 2 / M_PI); + this->scheduleWheelController(true); +} + +bool MD::Subscriber::isWheelTriggered(void) +{ + boost::lock_guard schedule_lockguard(publish_mutex_); + return !publish_time_.isZero() && ros::Time::now() > publish_time_; +} + +bool MD::Subscriber::isWheelLastestTriggered(void) +{ + boost::lock_guard schedule_lockguard(publish_mutex_); + return !publish_lastest_time_.isZero() && ros::Time::now() > publish_lastest_time_; +} + +void MD::Subscriber::scheduleWheelController(bool schedule) +{ + boost::lock_guard schedule_lockguard(publish_mutex_); + if(schedule && publish_time_.isZero()) + { + publish_time_ = ros::Time::now() + schedule_delay_; + publish_lastest_time_ = ros::Time::now() + schedule_lastest_delay_; + } + if(!schedule && !publish_time_.isZero()) + { + publish_time_ = ros::Time(0); + publish_lastest_time_ = ros::Time(0); + } +} \ No newline at end of file diff --git a/Devices/Libraries/Ros/modbus/README.md b/Devices/Libraries/Ros/modbus/README.md new file mode 100755 index 0000000..962453b --- /dev/null +++ b/Devices/Libraries/Ros/modbus/README.md @@ -0,0 +1 @@ +# modbus \ No newline at end of file diff --git a/Devices/Libraries/Ros/modbus/modbus_rtu/CMakeLists.txt b/Devices/Libraries/Ros/modbus/modbus_rtu/CMakeLists.txt new file mode 100755 index 0000000..e280ed4 --- /dev/null +++ b/Devices/Libraries/Ros/modbus/modbus_rtu/CMakeLists.txt @@ -0,0 +1,38 @@ +cmake_minimum_required(VERSION 3.0.2) +project(modbus_rtu) + +find_package(catkin REQUIRED COMPONENTS + roscpp + std_msgs + libserial +) + +catkin_package( + INCLUDE_DIRS include + LIBRARIES modbus_rtu + CATKIN_DEPENDS roscpp std_msgs libserial +# DEPENDS system_lib +) + +include_directories( + include + ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +add_library(${PROJECT_NAME} src/modbus_rtu.cpp) +add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(${PROJECT_NAME} ${Boost_INCLUDE_DIRS} ${catkin_LIBRARIES}) + +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) + +## Mark cpp header files for installation +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE +) \ No newline at end of file diff --git a/Devices/Libraries/Ros/modbus/modbus_rtu/include/modbus_rtu/define.h b/Devices/Libraries/Ros/modbus/modbus_rtu/include/modbus_rtu/define.h new file mode 100755 index 0000000..d9a3855 --- /dev/null +++ b/Devices/Libraries/Ros/modbus/modbus_rtu/include/modbus_rtu/define.h @@ -0,0 +1,40 @@ +#pragma once + +/* + * Exception Codes + */ +#define EX_PARAM_NOT_EXIT 0x00 /* The parameter number does not exits */ +#define EX_ACCESS_TO_PARAM 0x01 /* There is no write access to the parameter */ +#define EX_PARAM_LIMITS 0x02 /* The data value exceeds the parameter limits */ +#define EX_SUB_INDEX_NO_EXIT 0x03 /* The sub-index in use does not exits */ +#define EX_PARAM_NOT_TYPE 0x04 /* The parameter is not of the array type */ +#define EX_TYPE_NO_MATCH 0x05 /* The data type does not match the parameter called */ +#define EX_ONLY_RESET 0x06 /* Only reset */ +#define EX_NOT_CHANGEABLE 0x07 /* Not changeable */ +#define EX_NO_WRITE_ACCESS 0x0B /* No write address */ +#define EX_NO_POSSIBLE_PRE_MODE 0x11 /* Data change in the parameter called is not possible in the present mode */ +#define EX_OTHER_ERROR 0x12 /* Other error */ +#define EX_INVALID_DATA_ADDR 0x40 /* Invalid data address */ +#define EX_INVALID_MSGS_LENGTH 0x41 /* Invalid message length */ +#define EX_INVALID_LENGTH_VALUE 0x42 /* Invalid data length or value */ +#define EX_INVALID_FUNC_CODE 0x43 /* Invalid function code */ +#define EX_NO_BUS_ACCESS 0x82 /* There is no bus access */ +#define EX_DATA_NOT_POSSIBLE 0x83 /* Data change is not possible because factory set-up is selected */ +#define EX_BAD_DATA 0XFF /* Bad Data lenght or Address */ + +#define BAD_CON -1 +#define BAD_CONFIG -2 +#define WORD_BITS 65535 +#define DATA_BITS 16 +/* + * Commonly use function codes + */ + +#define READ_COIL_BITS 0x01 /* Read coils status */ +#define READ_INPUT_BITS 0x02 /* Read input status */ +#define READ_HOLDING_REGS 0x03 /* Read holding registers */ +#define READ_INPUT_REGS 0x04 /* Read input register */ +#define WRITE_SINGLE_COIL 0x05 /* Write single coil status */ +#define WRITE_SINGLE_HOLDING_REG 0x06 /* Write single register */ +#define WRITE_MULTIPLE_COILS 0X0F /* Multiple coil write */ +#define WRITE_MULTIPLE_HOLDING_REGS 0X10 /* Multiple register write*/ \ No newline at end of file diff --git a/Devices/Libraries/Ros/modbus/modbus_rtu/include/modbus_rtu/modbus_rtu.h b/Devices/Libraries/Ros/modbus/modbus_rtu/include/modbus_rtu/modbus_rtu.h new file mode 100755 index 0000000..99c6a4f --- /dev/null +++ b/Devices/Libraries/Ros/modbus/modbus_rtu/include/modbus_rtu/modbus_rtu.h @@ -0,0 +1,152 @@ +#ifndef __MOSBUS_RTU_H_INCLUDE_ +#define __MOSBUS_RTU_H_INCLUDE_ +#include +#include +#include /* uin8_t */ +#include "modbus_rtu/define.h" +#include "libserial/rs485.h" + +class modbus_rtu : public rs485{ + public: + + /* + * @brief Constructor + * @param[in] poly The poly number to caculartion CRC16 + * @param[in] devfile Example /dev/tty* + * @param[in] baud Number of transmitted bits per a second + * @param[in] parity The parity check bit {Even, None , Old } + * @param[in] data_bit Number of bits in a transmission frame + * @param[in] stop_bit End bit + */ + modbus_rtu(uint16_t poly, const char* devfile, unsigned int baud, parity_t parity, data_bits_t data_bit,stop_bits_t stop_bit); + /* + * Destructor. + */ + virtual ~modbus_rtu(); + + /* + * Read Holding Registers + * @brief MODBUS FUNCTION 0x03 + * @param[in] slave_id The id on modbus device + * @param[in] address Reference Address + * @param[in] amount Amount of Registers to Read + * @param[in] buffer Buffer to Store Data Read from Registers + */ + virtual int modbus_read_holding_registers(uint8_t slave_id, int address, int amount, uint16_t *buffer); + + /* + * Read Input Registers + * @brief MODBUS FUNCTION 0x04 + * @param[in] slave_id The id on modbus device + * @param[in] address Reference Address + * @param[in] amount Amount of Registers to Read + * @param[in] buffer Buffer to Store Data Read from Registers + */ + virtual int modbus_read_input_registers(uint8_t slave_id, int address, int amount, uint16_t *buffer); + + /* + * Write Single Register + * @brief FUCTION 0x06 + * @param[in] slave_id The id on modbus device + * @param[in] address Reference Address + * @param[in] value Value to Be Written to Register + */ + virtual int modbus_write_register(uint8_t slave_id, int address, const uint16_t& value); + + /* + * Write Multiple Registers + * @brief MODBUS FUNCION 0x10 + * @param[in] slave_id The id on modbus device + * @param[in] address Reference Address + * @param[in] amount Amount of Value to Write + * @param[in] value Values to Be Written to the Registers + */ + int modbus_write_registers(uint8_t slave_id, int address, int amount, const uint16_t *value); + + /* + * @brief Mb_calcul_crc : compute the crc of a packet and put it at the end + * @param[in] msg Message to send + * @param[in] length The length of message to send + * @param[in] poly The poly number to caculartion CRC16 + * @return CRC Result is CRC value 16 bit + */ + virtual uint16_t getCRC16(uint8_t *to_send, uint16_t length); + /* + * @brief Mb_calcul_crc : compute the crc of a packet and put it at the end + * @param[in] msg Message to send + * @param[in] length The length of message to send + * @param[in] poly The poly number to caculartion CRC16 + * @return bool Result is CRC value true/false + */ + virtual bool checkCRC16(uint8_t *to_send, uint16_t length); + /* + * Modbus Request Builder + * @param[in] slave_id The id on modbus device + * @param[in] to_send Message Buffer to Be Sent + * @param[in] address Reference Address + * @param[im] func Modbus Functional Code + */ + inline void modbus_build_request(uint8_t slave_id, uint8_t *to_send, int address, uint8_t func) const; + + /* + * Read Request Builder and Sender + * @param[in] slave_id The id on modbus device + * @param[in] address Reference Address + * @param[in] amount Amount of Data to Read + * @param[in] func Modbus Functional Code + */ + ssize_t modbus_read(uint8_t slave_id, int address, uint amount, int func); + + /* + * Write Request Builder and Sender + * @param[in] slave_id The id on modbus device + * @param[in] address Reference Address + * @param[in] amount Amount of data to be Written + * @param[in] func Modbus Functional Code + * @param[in] value Data to Be Written + */ + ssize_t modbus_write(uint8_t slave_id, int address, uint amount, int func, const uint16_t *value); + + /* + * Data Sender + * @param[in] to_send Request to Be Sent to Server + * @param[in] length Length of the Request + * @param[in] Size of the request + */ + inline ssize_t modbus_send(uint8_t *to_send, uint16_t length); + + /* + * Data Receiver + * @param[in] buffer Buffer to Store the Data Retrieved + * @return Size of Incoming Data + */ + inline ssize_t modbus_receive(uint8_t *buffer); + + /* + * Error Code Handler + * @param[in] msg Message Received from the Server + * @param[in] func Modbus Functional Code + */ + virtual void modbuserror_handle(const uint8_t *msg, int func); + + /* + * Set Bad Data lenght or Address + */ + inline void set_bad_input(void); + + /* + * Set Bad connection + */ + virtual void set_bad_con(void); + /** + * Set config faild + */ + virtual void setConfigFaild(void); + + private: + /* Properties */ + bool err{}; + int err_no{}; + uint16_t _poly; /* POLY is const number to cacurla CRC16 */ + }; +#endif \ No newline at end of file diff --git a/Devices/Libraries/Ros/modbus/modbus_rtu/package.xml b/Devices/Libraries/Ros/modbus/modbus_rtu/package.xml new file mode 100755 index 0000000..edad76a --- /dev/null +++ b/Devices/Libraries/Ros/modbus/modbus_rtu/package.xml @@ -0,0 +1,67 @@ + + + modbus_rtu + 0.0.0 + The modbus_rtu package + + + + + robotics + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + roscpp + std_msgs + roscpp + std_msgs + roscpp + std_msgs + + libserial + libserial + + + + + + + diff --git a/Devices/Libraries/Ros/modbus/modbus_rtu/src/modbus_rtu.cpp b/Devices/Libraries/Ros/modbus/modbus_rtu/src/modbus_rtu.cpp new file mode 100755 index 0000000..f87e62c --- /dev/null +++ b/Devices/Libraries/Ros/modbus/modbus_rtu/src/modbus_rtu.cpp @@ -0,0 +1,450 @@ +#include "modbus_rtu/modbus_rtu.h" + +/** + * @brief Constructor + * @param[in] poly The poly number to caculartion CRC16 + * @param[in] devfile Example /dev/tty* + * @param[in] baud Number of transmitted bits per a second + * @param[in] parity The parity check bit {Even, None , Old } + * @param[in] data_bit Number of bits in a transmission frame + * @param[in] stop_bit End bit + */ +modbus_rtu::modbus_rtu(uint16_t poly, const char* devfile, unsigned int baud, parity_t parity, data_bits_t data_bit,stop_bits_t stop_bit) : _poly(poly), rs485(devfile, baud, parity, data_bit, stop_bit){ + err = false; + err_no = 0; + error_msg = ""; + ROS_INFO("Set POLY value is 0x%x", _poly); + if(setCharacterNumToRead(5) < 0) {setConfigFaild();} +} + +/** + * Destructor. + */ +modbus_rtu::~modbus_rtu(){} + +/** + * Read Holding Registers + * MODBUS FUNCTION 0x03 + * @param[in] slave_id The id on modbus device + * @param[in] address Reference Address + * @param[in] amount Amount of Registers to Read + * @param[in] buffer Buffer to Store Data Read from Registers + */ +int modbus_rtu::modbus_read_holding_registers(uint8_t slave_id, int address, int amount, uint16_t *buffer){ + if(_connected){ + if(address > WORD_BITS || amount > DATA_BITS){ + set_bad_input(); + return EX_BAD_DATA; + } + modbus_read(slave_id, address, amount, READ_HOLDING_REGS); + //if(setCharacterNumToRead(5 + amount * 2) < 0) {setConfigFaild(); return BAD_CONFIG;} + uint8_t to_rec[MAX_MSG_LENGTH]; + memset(to_rec,'\0',MAX_MSG_LENGTH); + ssize_t k = 0; + k = modbus_receive(to_rec); + if (k == -1 || k == 65535) {return BAD_CON;} + if(k == 5 + amount * 2) { + if(!checkCRC16(to_rec,k)){ + ROS_ERROR("modbus_read_holding_registers"); + set_bad_input(); + return EX_BAD_DATA; + } + modbuserror_handle(to_rec, READ_HOLDING_REGS); + if(err) return err_no; + for(int i = 0; i < to_rec[2u]/2; i++){ + buffer[i] = ((uint16_t)to_rec[3u + 2u * i]) << 8u; + buffer[i] |= ((uint16_t)to_rec[4u + 2u * i] & 0x00FFu); + } + return 0; + } else { + ROS_ERROR("receive message is not correct %d ", (int)k); + for(int i = 0; i < k; i++) printf("0x%x ",to_rec[i]); + printf("\n"); + set_bad_input(); + return EX_BAD_DATA; + } + } else { + return BAD_CON; + } +} + +/** + * Read Input Registers + * MODBUS FUNCTION 0x04 + * @param[in] slave_id The id on modbus device + * @param[in] address Reference Address + * @param[in] amount Amount of Registers to Read + * @param[in] buffer Buffer to Store Data Read from Registers + */ +int modbus_rtu::modbus_read_input_registers(uint8_t slave_id, int address, int amount, uint16_t *buffer){ + if(_connected){ + if(amount > DATA_BITS || address > WORD_BITS){ + set_bad_input(); + return EX_BAD_DATA; + } + modbus_read(slave_id, address, amount, READ_INPUT_REGS); + //if(setCharacterNumToRead(5 + amount * 2) < 0) {setConfigFaild(); return BAD_CONFIG;} + uint8_t to_rec[MAX_MSG_LENGTH]; + memset(to_rec,'\0',MAX_MSG_LENGTH); + ssize_t k = 0; + k = modbus_receive(to_rec); + if (k == -1 || k == 65535) {return BAD_CON;} + if(k > 0){ + if(!checkCRC16(to_rec,k)) { + ROS_ERROR("modbus_read_input_registers"); + set_bad_input(); + return EX_BAD_DATA; + } + modbuserror_handle(to_rec, READ_INPUT_REGS); + if(err) return err_no; + for(int i = 0; i < to_rec[2u]/2; i++){ + buffer[i] = ((uint16_t)to_rec[3u + 2u * i]) << 8u; + buffer[i] |= ((uint16_t)to_rec[4u + 2u * i] & 0x00FFu); + } + return 0; + } else { + ROS_ERROR("Slave no response !!"); + set_bad_input(); + return EX_BAD_DATA; + } + } else { + return BAD_CON; + } +} + +/** + * Write Single Register + * @brief FUCTION 0x06 + * @param[in] slave_id The id on modbus device + * @param[in] address Reference Address + * @param[in] value Value to Be Written to Register + */ +int modbus_rtu::modbus_write_register(uint8_t slave_id, int address, const uint16_t& value) { + if(_connected){ + if(address > WORD_BITS){ + set_bad_input(); + return EX_BAD_DATA; + } + modbus_write(slave_id, address, 1, WRITE_SINGLE_HOLDING_REG, &value); + //if(setCharacterNumToRead(8) < 0 ) {setConfigFaild(); return BAD_CONFIG;} + uint8_t to_rec[MAX_MSG_LENGTH]; + memset(to_rec,'\0',MAX_MSG_LENGTH); + ssize_t k = 0; + k = modbus_receive(to_rec); + if (k == -1 || k == 65535) {return BAD_CON;} + if(k == 8){ + // for(int i = 0; i < k; i++) ROS_INFO("0x%x",to_rec[i]); + if(!checkCRC16(to_rec, k)) { + ROS_ERROR("modbus_write_register"); + set_bad_input(); + return EX_BAD_DATA; + } + modbuserror_handle(to_rec, WRITE_SINGLE_HOLDING_REG); + if(err) return err_no; + return 0; + } else { + ROS_ERROR("Slave no response !!"); + set_bad_input(); + return EX_BAD_DATA; + } + } else { + return BAD_CON; + } +} + +/** + * Write Multiple Registers + * @brief MODBUS FUNCION 0x10 + * @param[in] slave_id The id on modbus device + * @param[in] address Reference Address + * @param[in] amount Amount of Value to Write + * @param[in] value Values to Be Written to the Registers + */ +int modbus_rtu::modbus_write_registers(uint8_t slave_id, int address, int amount, const uint16_t *value){ + if(_connected){ + if(amount > DATA_BITS || address > WORD_BITS){ + set_bad_input(); + return EX_BAD_DATA; + } + modbus_write(slave_id, address, amount, WRITE_MULTIPLE_HOLDING_REGS, value); + //if(setCharacterNumToRead(8) < 0) {setConfigFaild(); return BAD_CONFIG;} + uint8_t to_rec[MAX_MSG_LENGTH]; + memset(to_rec,'\0',MAX_MSG_LENGTH); + ssize_t k = 0; + k = modbus_receive(to_rec); + if (k == -1 || k == 65535) {return BAD_CON;} + if(k == 8) { + if(!checkCRC16(to_rec, k)){ + ROS_ERROR("modbus_write_registers"); + set_bad_input(); + return EX_BAD_DATA; + } + modbuserror_handle(to_rec, WRITE_MULTIPLE_HOLDING_REGS); + if(err) return err_no; + return 0; + } else { + ROS_ERROR("Slave no response !!"); + set_bad_input(); + return EX_BAD_DATA; + } + } else { + return BAD_CON; + } +} + +/** + * @brief Mb_calcul_crc : compute the crc of a packet and put it at the end + * @param[in] msg Message to send + * @param[in] length The length of message to send + * @param[in] poly The poly number to caculartion CRC16 + * @return CRC Result is CRC value 16 bit + */ +uint16_t modbus_rtu::getCRC16(uint8_t *to_send, uint16_t length){ + /* + EX: Ban đầu CRC = 1111 1111 1111 1111 chuyển sang Hex là FFFF + Chọn data_p là 54 hay 0101 0100(1 byte) là số cần tính. + Chọn số poly = A001h hay 1010 000 000 0001 + (Poly là một số mà bạn sử dụng làm phép tính số CRC cơ sở của mình.) + + + Bước 1: Dịch CRC và data_p sang phải 1 bit + data_p = 54, là 0101 0100 trở thành 0010 1010 + CRC = 1111 1111 1111 1111 trở thành 0111 1111 1111 1111 + + + Bước 2: Kiểm tra BIT ngoài cùng bên phải của Dữ liệu và so sánh nó với một trong các CRC + NẾU chúng bằng nhau, dịch chuyển CRC sang phải 1 bit + NẾU chúng không phải, dịch chuyển CRC sang phải 1 bit VÀ cộng thêm số Poly một lần nữa. + Thực hiện bước 2 đúng 8 lần vì 1 byte có 8 bit. + + +Bước 3: Bước 1 và 2 sẽ được lăp lại theo số lượng data_p. + */ + unsigned char i; + unsigned int data; + unsigned int crc = 0xffff; + do { + for (i=0, data=(unsigned int)0xff & *to_send++; i < 8; i++, data >>= 1) { + if ((crc & 0x0001) ^ (data & 0x0001)) + crc = (crc >> 1) ^ _poly; + else crc >>= 1; + } + } while (--length); + return (crc); +} + +/** + * @brief Mb_calcul_crc : compute the crc of a packet and put it at the end + * @param[in] msg Message to send + * @param[in] length The length of message to send + * @param[in] poly The poly number to caculartion CRC16 + * @return bool Result is CRC value true/false + */ +bool modbus_rtu::checkCRC16(uint8_t *to_send, uint16_t length){ + uint16_t to_check; + to_check = getCRC16(to_send,length - 2); + for(int i = 0; i < length; i++) printf("0x%x ",to_send[i]); + printf("length(%d) crc16(0x%x 0x%x) 0x%x 0x%x ", (int)length, to_check & 0x00FFu, to_check >> 8u, to_send[length - 2], to_send[length - 1]); + printf("\n"); + return to_send[length - 2] == (uint8_t)(to_check & 0x00FFu) && to_send[length - 1] == (uint8_t)(to_check >> 8u); +} + +/* + * Modbus Request Builder + * @param[in] slave_id The id on modbus device + * @param[in] to_send Message Buffer to Be Sent + * @param[in] address Reference Address + * @param[in] func Modbus Functional Code + */ +void modbus_rtu::modbus_build_request(uint8_t slave_id, uint8_t *to_send, int address, uint8_t func) const { + to_send[0] = slave_id; + to_send[1] = func; + to_send[2] = (uint8_t) (address >> 8u); + to_send[3] = (uint8_t) (address & 0x00FFu); +} + +/* + * Write Request Builder and Sender + * @param[in] slave_id The id on modbus device + * @param[in] address Reference Address + * @param[in] amount Amount of data to be Written + * @param[in] func Modbus Functional Code + * @param[in] value Data to Be Written + */ +ssize_t modbus_rtu::modbus_write(uint8_t slave_id, int address, uint amount, int func, const uint16_t *value) { + ssize_t result = 0; + if(func == WRITE_SINGLE_COIL || func == WRITE_SINGLE_HOLDING_REG){ + uint8_t to_send[8]; + modbus_build_request(slave_id, to_send, address, func); + to_send[4] = (uint8_t) (value[0] >> 8u); + to_send[5] = (uint8_t) (value[0] & 0x00FFu); + uint16_t crc = getCRC16(to_send,6); + to_send[6] = (uint8_t) (crc & 0x00FFu); + to_send[7] = (uint8_t) (crc >> 8u); + // for(int i = 0; i < 8; i++) ROS_INFO("0x%x", to_send[i]); + result = modbus_send(to_send,8); + } else if(func == WRITE_MULTIPLE_HOLDING_REGS){ + uint8_t to_send[9 + 2 * amount]; + modbus_build_request(slave_id,to_send, address, func); + to_send[4] = (uint8_t)(amount >> 8u); + to_send[5] = (uint8_t)(amount & 0x00FFu); + to_send[6] = (uint8_t)(amount * 2); + for(int i = 0; i < amount; i++){ + to_send[7 + 2 * i] = (uint8_t)(value[i] >> 8u); + to_send[8 + 2 * i] = (uint8_t)(value[i] & 0x00FFu); + } + uint16_t crc = getCRC16(to_send, 9 + 2 * amount - 2); + to_send[9 + 2 * amount - 2] = (uint8_t)(crc & 0x00FFu); + to_send[9 + 2 * amount - 1] = (uint8_t)(crc >> 8u); + //for(int i = 0; i < 9 + 2 * amount; i++) ROS_INFO("0x%x", to_send[i]); + result = modbus_send(to_send, 9 + 2 * amount); + } + return result; +} +/* + * Read Request Builder and Sender + * @param[in] slave_id The id on modbus device + * @param[in] address Reference Address + * @param[in] amount Amount of Data to Read + * @param[in] func Modbus Functional Code + */ +ssize_t modbus_rtu::modbus_read(uint8_t slave_id, int address, uint amount, int func){ + uint8_t to_send[8]; + modbus_build_request(slave_id, to_send, address, func); + to_send[4] = (uint8_t) (amount >> 8u); + to_send[5] = (uint8_t) (amount & 0x00FFu); + uint16_t crc = getCRC16(to_send,6); + to_send[6] = (uint8_t) (crc & 0x00FFu); + to_send[7] = (uint8_t) (crc >> 8u); + return modbus_send(to_send, 8); +} + +/* + * Data Sender + * @param[in] to_send Request to Be Sent to Server + * @param[in] length Length of the Request + * @return Size of the request + */ +ssize_t modbus_rtu::modbus_send(uint8_t *to_send, uint16_t length){ + struct stat sb; + if(stat(ctx._port, &sb) < 0) reconnect(); + std::cout << std::endl; + return sendMsgs(to_send, length); +} + +/* + * Data Receiver + * @param[in] buffer Buffer to Store the Data Retrieved + * @return Size of Incoming Data + */ +ssize_t modbus_rtu::modbus_receive(uint8_t *buffer) { + struct timeval timeout; + timeout.tv_sec = 2; + timeout.tv_usec = 0; + fd_set set; + FD_ZERO(&set); /* clear the set */ + FD_SET(ctx.s, &set); /* add our file descriptor to the set */ + int rv = select(ctx.s + 1, &set, NULL, NULL, &timeout); + if(rv == -1){ + err = true; + _connected = false; + error_msg = "Bad connection select"; + return BAD_CON; + } else if(rv == 0) { + err = true; + _connected = false; + error_msg = "Bad connection timeout"; /* an timeout occured */ + return BAD_CON; + } else + return receiveMsgs(buffer); +} + +/* + * Error Code Handler + * @param[in] msg Message Received from the Server + * @param[in] func Modbus Functional Code + */ +void modbus_rtu::modbuserror_handle(const uint8_t *msg, int func){ + if(msg[1] == func + 0x80){ + err = true; + err_no = 1; + switch(msg[2]){ + case EX_PARAM_NOT_EXIT: + error_msg = "The parameter number does not exits"; + break; + case EX_ACCESS_TO_PARAM: + error_msg = "There is no write access to the parameter"; + break; + case EX_PARAM_LIMITS: + error_msg = "The data value exceeds the parameter limits"; + break; + case EX_SUB_INDEX_NO_EXIT: + error_msg = "The sub-index in use does not exits"; + break; + case EX_PARAM_NOT_TYPE: + error_msg = "The parameter is not of the array type"; + break; + case EX_TYPE_NO_MATCH: + error_msg = "The data type does not match the parameter called"; + break; + case EX_ONLY_RESET: + error_msg = "Only reset"; + break; + case EX_NOT_CHANGEABLE: + error_msg = "Not changeable"; + break; + case EX_NO_WRITE_ACCESS: + error_msg = "No write address"; + break; + case EX_NO_POSSIBLE_PRE_MODE: + error_msg = "Data change in the parameter called is not possible in the present mode"; + break; + case EX_OTHER_ERROR: + error_msg = "Other error"; + break; + case EX_INVALID_DATA_ADDR: + error_msg = "Invalid data address"; + break; + case EX_INVALID_MSGS_LENGTH: + error_msg = "Invalid message length"; + break; + case EX_INVALID_LENGTH_VALUE: + error_msg = "Invalid data length or value"; + break; + case EX_INVALID_FUNC_CODE: + error_msg = "Invalid function code"; + break; + case EX_NO_BUS_ACCESS: + error_msg = "There is no bus access"; + break; + case EX_DATA_NOT_POSSIBLE: + error_msg = "Data change is not possible because factory set-up is selected"; + break; + } + } + err = false; + error_msg = "NO ERR"; +} + +/* + * Set Bad Data lenght or Address + */ +void modbus_rtu::set_bad_input(void) { + err = true; + error_msg = "Bad Data lenght or Address"; +} + +/* + * Set Bad connection + */ +void modbus_rtu::set_bad_con(void) { + err = true; + _connected = false; + error_msg = "Bad connection"; +} + +/** + * Set config faild + */ +void modbus_rtu::setConfigFaild(void){ + err = true; + _connected = false; + error_msg = "Driver config failded"; +} diff --git a/Devices/Libraries/Ros/modbus/modbus_tcp/CMakeLists.txt b/Devices/Libraries/Ros/modbus/modbus_tcp/CMakeLists.txt new file mode 100755 index 0000000..96b03fe --- /dev/null +++ b/Devices/Libraries/Ros/modbus/modbus_tcp/CMakeLists.txt @@ -0,0 +1,78 @@ +cmake_minimum_required(VERSION 3.0.2) +project(modbus_tcp) + +find_package(catkin REQUIRED COMPONENTS + roscpp + std_msgs +) + +catkin_package( + INCLUDE_DIRS include + LIBRARIES modbus_tcp + CATKIN_DEPENDS roscpp std_msgs +# DEPENDS system_lib +) + +include_directories( + include + ${catkin_INCLUDE_DIRS} +) + +add_library(${PROJECT_NAME} src/modbus.cpp) +add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${GSTREAMER_LIBRARIES}) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# catkin_install_python(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html +# install(TARGETS ${PROJECT_NAME}_node +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark libraries for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) + +## Mark cpp header files for installation +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE +) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_modbus_tcp.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/Devices/Libraries/Ros/modbus/modbus_tcp/include/modbus_tcp/modbus.h b/Devices/Libraries/Ros/modbus/modbus_tcp/include/modbus_tcp/modbus.h new file mode 100755 index 0000000..a7cadd8 --- /dev/null +++ b/Devices/Libraries/Ros/modbus/modbus_tcp/include/modbus_tcp/modbus.h @@ -0,0 +1,240 @@ +#ifndef MODBUS_H +#define MODBUS_H +#pragma once +#include +#include +#include +#include +#include +#include +#include +#include + + +#define MAX_MSG_LENGTH 260 + +///Function Code +#define READ_COILS 0x01 +#define READ_INPUT_BITS 0x02 +#define READ_REGS 0x03 +#define READ_INPUT_REGS 0x04 +#define WRITE_COIL 0x05 +#define WRITE_REG 0x06 +#define WRITE_COILS 0x0F +#define WRITE_REGS 0x10 + +///Exception Codes + +#define EX_ILLEGAL_FUNCTION 0x01 // Function Code not Supported +#define EX_ILLEGAL_ADDRESS 0x02 // Output Address not exists +#define EX_ILLEGAL_VALUE 0x03 // Output Value not in Range +#define EX_SERVER_FAILURE 0x04 // Slave Deive Fails to process request +#define EX_ACKNOWLEDGE 0x05 // Service Need Long Time to Execute +#define EX_SERVER_BUSY 0x06 // Server Was Unable to Accept MB Request PDU +#define EX_NEGATIVE_ACK 0x07 +#define EX_MEM_PARITY_PROB 0x08 +#define EX_GATEWAY_PROBLEMP 0x0A // Gateway Path not Available +#define EX_GATEWYA_PROBLEMF 0x0B // Target Device Failed to Response +#define EX_BAD_DATA 0XFF // Bad Data lenght or Address + +#define BAD_CON -1 + +/** + * @class Modbus Operator Class + * @brief Providing networking support and mobus operation support. + */ +class modbus { +public: + /** + * Main Constructor of Modbus Connector Object + * @param host IP Address of Host + * @param port Port for the TCP Connection + * @return A Modbus Connector Object + */ + modbus(std::string ip_address, int port); + + /** + * Destructor of Modbus Connector Object + */ + virtual ~modbus(); + + /** + * Modbus Slave ID Setter + * @param id ID of the Modbus Server Slave + */ + void modbus_set_slave_id(int id); + + /** + * @brief Build up a Modbus/TCP Connection + * @return If A Connection Is Successfully Built + */ + bool modbus_connect(); + + /** + * @brief Close the Modbus/TCP Connection + */ + void modbus_close() const; + + /** + * @brief Read Coil + * MODBUS FUNCTION 0x01 + * @param address Reference Address + * @param buffer Buffer to Store Data Read from Coils + */ + int modbus_read_coil(int address, bool &buffer); + + /** + * @brief Read Coils + * MODBUS FUNCTION 0x01 + * @param address Reference Address + * @param amount Amount of Coils to Read + * @param buffer Buffer to Store Data Read from Coils + */ + int modbus_read_coils(int address, int amount, bool* buffer); + + /** + * @brief Read Input Bits(Discrete Data) + * MODBUS FUNCITON 0x02 + * @param address Reference Address + * @param buffer Buffer to store Data Read from Input Bits + */ + int modbus_read_input_bit(int address, bool &buffer); + + /** + * @brief Read Input Bits(Discrete Data) + * MODBUS FUNCITON 0x02 + * @param address Reference Address + * @param amount Amount of Bits to Read + * @param buffer Buffer to store Data Read from Input Bits + */ + int modbus_read_input_bits(int address, int amount, bool* buffer); + + /** + * @brief Read Holding Register + * MODBUS FUNCTION 0x03 + * @param address Reference Address + * @param buffer Buffer to Store Data Read from Registers + */ + int modbus_read_holding_register(int address, uint16_t &buffer); + + /** + * @brief Read Holding Registers + * MODBUS FUNCTION 0x03 + * @param address Reference Address + * @param amount Amount of Registers to Read + * @param buffer Buffer to Store Data Read from Registers + */ + int modbus_read_holding_registers(int address, int amount, uint16_t *buffer); + + /** + * @brief Read Input Registers + * MODBUS FUNCTION 0x04 + * @param address Reference Address + * @param amount Amount of Registers to Read + * @param buffer Buffer to Store Data Read from Registers + */ + int modbus_read_input_registers(int address, int amount, uint16_t *buffer); + + + /** + * @brief Write Single Coils + * MODBUS FUNCTION 0x05 + * @param address Reference Address + * @param to_write Value to be Written to Coil + */ + int modbus_write_coil(int address, const bool& to_write); + + /** + * @brief Write Single Register + * FUCTION 0x06 + * @param address Reference Address + * @param value Value to Be Written to Register + */ + int modbus_write_register(int address, const uint16_t& value); + + /** + * @brief Write Multiple Coils + * MODBUS FUNCTION 0x0F + * @param address Reference Address + * @param amount Amount of Coils to Write + * @param value Values to Be Written to Coils + */ + int modbus_write_coils(int address, int amount, const bool *value); + + /** + * @brief Write Multiple Registers + * MODBUS FUNCION 0x10 + * @param address Reference Address + * @param amount Amount of Value to Write + * @param value Values to Be Written to the Registers + */ + int modbus_write_registers(int address, int amount, const uint16_t *value); + + bool _connected{}; + bool err{}; + int err_no{}; + std::string error_msg; + struct timeval timeout{}; + +private: + + /** + * @brief Modbus Request Builder + * @param to_send Message Buffer to Be Sent + * @param address Reference Address + * @param func Modbus Functional Code + */ + void modbus_build_request(uint8_t *to_send, uint address, int func) const; + + /** + * @brief Read Request Builder and Sender + * @param address Reference Address + * @param amount Amount of Data to Read + * @param func Modbus Functional Code + */ + int modbus_read(int address, uint amount, int func); + + /** + * @brief Write Request Builder and Sender + * @param address Reference Address + * @param amount Amount of data to be Written + * @param func Modbus Functional Code + * @param value Data to Be Written + */ + int modbus_write(int address, uint amount, int func, const uint16_t *value); + + /** + * @brief Data Sender + * @param to_send Request to Be Sent to Server + * @param length Length of the Request + * @return Size of the request + */ + ssize_t modbus_send(uint8_t *to_send, int length); + + /** + * @brief Data Receiver + * @param buffer Buffer to Store the Data Retrieved + * @return Size of Incoming Data + */ + ssize_t modbus_receive(uint8_t *buffer) const; + + /** + * @brief Error Code Handler + * @param msg Message Received from the Server + * @param func Modbus Functional Code + */ + void modbuserror_handle(const uint8_t *msg, int func); + + void set_bad_con(); + void set_bad_input(); + +// Properties + int _socket{}; + uint _msg_id{}; + int _slaveid{}; + uint16_t PORT{}; + std::string HOST; + struct sockaddr_in _server{}; +}; + +#endif //MODBUS_H diff --git a/Devices/Libraries/Ros/modbus/modbus_tcp/package.xml b/Devices/Libraries/Ros/modbus/modbus_tcp/package.xml new file mode 100755 index 0000000..9b4e317 --- /dev/null +++ b/Devices/Libraries/Ros/modbus/modbus_tcp/package.xml @@ -0,0 +1,65 @@ + + + modbus_tcp + 0.0.0 + The modbus_tcp package + + + + + robotics + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + roscpp + std_msgs + roscpp + std_msgs + roscpp + std_msgs + + + + + + + + diff --git a/Devices/Libraries/Ros/modbus/modbus_tcp/src/modbus.cpp b/Devices/Libraries/Ros/modbus/modbus_tcp/src/modbus.cpp new file mode 100755 index 0000000..25f4f39 --- /dev/null +++ b/Devices/Libraries/Ros/modbus/modbus_tcp/src/modbus.cpp @@ -0,0 +1,501 @@ +#include "modbus_tcp/modbus.h" + + + +modbus::modbus(std::string ip_address, int port) +{ + this->HOST = ip_address; + ROS_INFO("modbus - HOST = %s", this->HOST.c_str()); + this->PORT = (uint16_t)port; + ROS_INFO("modbus - PORT = %d", this->PORT); + this->_slaveid = 1; + this->_msg_id = 1; + this->_connected = false; + this->err = false; + this->err_no = 0; + this->error_msg = ""; + this->timeout.tv_sec = 10; // after 10 seconds connect() will timeout + this->timeout.tv_usec = 0; +} + +modbus::~modbus(void) +{ + modbus_close(); +} + +void modbus::modbus_set_slave_id(int id) +{ + _slaveid = id; +} + +bool modbus::modbus_connect() +{ + if(this->HOST.empty() || this->PORT == 0) { + ROS_ERROR_STREAM("Modbus TCP: Missing Host and Port"); + return false; + } else { + ROS_INFO_STREAM("Modbus TCP: Found Proper Host "<< this->HOST << " and Port " <PORT); + } + + this->_socket = socket(AF_INET, SOCK_STREAM, 0); + if(this->_socket == -1) { + ROS_ERROR_STREAM("Modbus TCP: Error Opening Socket"); + return false; + } else { + ROS_INFO_STREAM("Modbus TCP: Socket Opened Successfully"); + //ROS_INFO_STREAM("Device is connecting......"); + } + + setsockopt(_socket, SOL_SOCKET, SO_SNDTIMEO, &timeout, sizeof(timeout)); + setsockopt(_socket, SOL_SOCKET, SO_RCVTIMEO, &timeout, sizeof(timeout)); + + _server.sin_family = AF_INET; + _server.sin_addr.s_addr = inet_addr(HOST.c_str()); + _server.sin_port = htons(PORT); + + if (connect(_socket, (struct sockaddr*)&_server, sizeof(_server)) < 0) { + ROS_ERROR_STREAM("Modbus TCP: Host "<< this->HOST << " and Port " <PORT << " Connection Error"); + return false; + } + +// ROS_INFO_STREAM("Modbus TCP: Host "<< this->HOST << " and Port " <PORT << "is Connected"); + _connected = true; + err = false; + return true; +} + +void modbus::modbus_close() const { + close(_socket); + ROS_INFO_STREAM("Modbus TCP: Socket Closed"); +} + +void modbus::modbus_build_request(uint8_t *to_send, uint address, int func) const { + to_send[0] = (uint8_t) _msg_id >> 8u; + to_send[1] = (uint8_t) (_msg_id & 0x00FFu); + to_send[2] = 0; + to_send[3] = 0; + to_send[4] = 0; + to_send[6] = (uint8_t) _slaveid; + to_send[7] = (uint8_t) func; + to_send[8] = (uint8_t) (address >> 8u); + to_send[9] = (uint8_t) (address & 0x00FFu); +} + +int modbus::modbus_write(int address, uint amount, int func, const uint16_t *value) { + int status = 0; + if(func == WRITE_COIL || func == WRITE_REG) { + uint8_t to_send[12]; + modbus_build_request(to_send, address, func); + to_send[5] = 6; + to_send[10] = (uint8_t) (value[0] >> 8u); + to_send[11] = (uint8_t) (value[0] & 0x00FFu); + status = modbus_send(to_send, 12); + } else if(func == WRITE_REGS){ + uint8_t to_send[13 + 2 * amount]; + modbus_build_request(to_send, address, func); + to_send[5] = (uint8_t) (7 + 2 * amount); + to_send[10] = (uint8_t) (amount >> 8u); + to_send[11] = (uint8_t) (amount & 0x00FFu); + to_send[12] = (uint8_t) (2 * amount); + for(int i = 0; i < amount; i++) { + to_send[13 + 2 * i] = (uint8_t) (value[i] >> 8u); + to_send[14 + 2 * i] = (uint8_t) (value[i] & 0x00FFu); + } + status = modbus_send(to_send, 13 + 2 * amount); + } else if(func == WRITE_COILS) { + uint8_t to_send[14 + (amount -1) / 8 ]; + modbus_build_request(to_send, address, func); + to_send[5] = (uint8_t) (7 + (amount + 7) / 8); + to_send[10] = (uint8_t) (amount >> 8u); + to_send[11] = (uint8_t) (amount & 0x00FFu); + to_send[12] = (uint8_t) ((amount + 7) / 8); + for(int i = 0; i < (amount+7)/8; i++) + to_send[13 + i] = 0; // init needed before summing! + for(int i = 0; i < amount; i++) { + to_send[13 + i/8] += (uint8_t) (value[i] << (i % 8u)); + } + status = modbus_send(to_send, 14 + (amount - 1) / 8); + } + return status; + +} + +int modbus::modbus_read(int address, uint amount, int func){ + uint8_t to_send[12]; + modbus_build_request(to_send, address, func); + to_send[5] = 6; + to_send[10] = (uint8_t) (amount >> 8u); + to_send[11] = (uint8_t) (amount & 0x00FFu); + return modbus_send(to_send, 12); +} + +int modbus::modbus_read_holding_register(int address, uint16_t &buffer) { + if(_connected) { + if(address > 65535) { + set_bad_input(); + return EX_BAD_DATA; + } + modbus_read(address, 1, READ_REGS); + uint8_t to_rec[MAX_MSG_LENGTH]; + ssize_t k = modbus_receive(to_rec); + if (k == -1) { + set_bad_con(); + return BAD_CON; + } + if(k < 11) return EX_BAD_DATA; + modbuserror_handle(to_rec, READ_REGS); + if(err) return err_no; + buffer = ((uint16_t)to_rec[9u]) << 8u; + buffer += (uint16_t) to_rec[10u]; + return 0; + } else { + set_bad_con(); + return BAD_CON; + } +} + +int modbus::modbus_read_holding_registers(int address, int amount, uint16_t *buffer) { + if(_connected) { + if(amount > 65535 || address > 65535) { + set_bad_input(); + return EX_BAD_DATA; + } + modbus_read(address, amount, READ_REGS); + uint8_t to_rec[MAX_MSG_LENGTH]; + ssize_t k = modbus_receive(to_rec); + if (k == -1) { + set_bad_con(); + return BAD_CON; + } + if(k < 2 * amount + 9) return EX_BAD_DATA; + modbuserror_handle(to_rec, READ_REGS); + if(err) return err_no; + for(uint i = 0; i < amount; i++) { + buffer[i] = ((uint16_t)to_rec[9u + 2u * i]) << 8u; + buffer[i] += (uint16_t) to_rec[10u + 2u * i]; + } + return 0; + } else { + set_bad_con(); + return BAD_CON; + } +} + +int modbus::modbus_read_input_registers(int address, int amount, uint16_t *buffer) { + if(_connected){ + if(amount > 65535 || address > 65535) { + set_bad_input(); + return EX_BAD_DATA; + } + modbus_read(address, amount, READ_INPUT_REGS); + uint8_t to_rec[MAX_MSG_LENGTH]; + ssize_t k = modbus_receive(to_rec); + // ROS_INFO("modbus_read_input_registers: k = %d, amount = %d", (int)k , amount); + if (k == -1) { + set_bad_con(); + return BAD_CON; + } + modbuserror_handle(to_rec, READ_INPUT_REGS); + if(err) return err_no; + for(uint i = 0; i < amount; i++) { + buffer[i] = ((uint16_t)to_rec[9u + 2u * i]) << 8u; + buffer[i] += (uint16_t) to_rec[10u + 2u * i]; + } + return 0; + } else { + set_bad_con(); + return BAD_CON; + } +} + +int modbus::modbus_read_coil(int address, bool &buffer) { + if(_connected) { + if(address > 65535) { + set_bad_input(); + return EX_BAD_DATA; + } + modbus_read(address, 1, READ_COILS); + uint8_t to_rec[MAX_MSG_LENGTH]; + ssize_t k = modbus_receive(to_rec); + // ROS_INFO("modbus_read_coil: k = %d, amount = %d", (int)k , 1); + if (k == -1) { + set_bad_con(); + return BAD_CON; + } + modbuserror_handle(to_rec, READ_COILS); + if(err) return err_no; + buffer = (bool) ((to_rec[9u]) & 1u); + return 0; + } else { + set_bad_con(); + return BAD_CON; + } +} + +int modbus::modbus_read_coils(int address, int amount, bool *buffer) { + if(_connected) { + if(amount > 2040 || address > 65535) { + set_bad_input(); + return EX_BAD_DATA; + } + modbus_read(address, amount, READ_COILS); + uint8_t to_rec[MAX_MSG_LENGTH]; + ssize_t k = modbus_receive(to_rec); + if (k == -1) { + set_bad_con(); + return BAD_CON; + } + modbuserror_handle(to_rec, READ_COILS); + // ROS_INFO("modbus_read_coils: k = %d, amount = %d", (int)k , amount); + if(err) return err_no; + for(uint i = 0; i < amount; i++) { + buffer[i] = (bool) ((to_rec[9u + i / 8u] >> (i % 8u)) & 1u); + } + return 0; + } else { + set_bad_con(); + return BAD_CON; + } +} + +int modbus::modbus_read_input_bit(int address, bool &buffer) { + if(_connected) { + if(address > 65535) { + set_bad_input(); + return EX_BAD_DATA; + } + modbus_read(address, 1, READ_INPUT_BITS); + uint8_t to_rec[MAX_MSG_LENGTH]; + ssize_t k = modbus_receive(to_rec); + // ROS_INFO("modbus_read_input_bit: k = %d, amount = %d", (int)k , 1); + if (k == -1) { + set_bad_con(); + return BAD_CON; + } + if(err) return err_no; + buffer = (bool)(to_rec[9u] & 1u); + modbuserror_handle(to_rec, READ_INPUT_BITS); + return 0; + } else { + return BAD_CON; + } +} + +int modbus::modbus_read_input_bits(int address, int amount, bool* buffer) { + if(_connected) { + if(amount > 2040 || address > 65535) { + set_bad_input(); + return EX_BAD_DATA; + } + modbus_read(address, amount, READ_INPUT_BITS); + uint8_t to_rec[MAX_MSG_LENGTH]; + ssize_t k = modbus_receive(to_rec); + // ROS_INFO("modbus_read_input_bits: k = %d, amount = %d", (int)k , amount); + if (k == -1) { + set_bad_con(); + return BAD_CON; + } + if(err) return err_no; + for(uint i = 0; i < amount; i++) { + buffer[i] = (bool) ((to_rec[9u + i / 8u] >> (i % 8u)) & 1u); + } + modbuserror_handle(to_rec, READ_INPUT_BITS); + return 0; + } else { + return BAD_CON; + } +} + +int modbus::modbus_write_coil(int address, const bool& to_write) { + if(_connected) { + if(address > 65535) { + set_bad_input(); + return EX_BAD_DATA; + } + int value = to_write * 0xFF00; + modbus_write(address, 1, WRITE_COIL, (uint16_t *)&value); + uint8_t to_rec[MAX_MSG_LENGTH]; + ssize_t k = modbus_receive(to_rec); + if (k == -1) { + set_bad_con(); + return BAD_CON; + } + modbuserror_handle(to_rec, WRITE_COIL); + if(err) return err_no; + return 0; + } else { + set_bad_con(); + return BAD_CON; + } +} + +int modbus::modbus_write_register(int address, const uint16_t& value) { + if(_connected) { + if(address > 65535) { + set_bad_input(); + return EX_BAD_DATA; + } + modbus_write(address, 1, WRITE_REG, &value); + uint8_t to_rec[MAX_MSG_LENGTH]; + ssize_t k = modbus_receive(to_rec); + if (k == -1) { + set_bad_con(); + return BAD_CON; + } + modbuserror_handle(to_rec, WRITE_REG); + if(err) return err_no; + return 0; + } else { + set_bad_con(); + return BAD_CON; + } +} + +int modbus::modbus_write_coils(int address, int amount, const bool *value) { + if(_connected) { + if(address > 65535 || amount > 65535) { + set_bad_input(); + return EX_BAD_DATA; + } + uint16_t temp[amount]; + for(int i = 0; i < amount; i++) { + temp[i] = (uint16_t)value[i]; + } + modbus_write(address, amount, WRITE_COILS, temp); + uint8_t to_rec[MAX_MSG_LENGTH]; + ssize_t k = modbus_receive(to_rec); + // ROS_INFO("modbus_write_coils: k = %d, amount = %d", (int)k , amount); + if (k == -1) { + set_bad_con(); + return BAD_CON; + } + modbuserror_handle(to_rec, WRITE_COILS); + if(err) return err_no; + return 0; + } else { + set_bad_con(); + return BAD_CON; + } +} + +int modbus::modbus_write_registers(int address, int amount, const uint16_t *value) { + if(_connected) { + if(address > 65535 || amount > 65535) { + set_bad_input(); + return EX_BAD_DATA; + } + modbus_write(address, amount, WRITE_REGS, value); + uint8_t to_rec[MAX_MSG_LENGTH]; + ssize_t k = modbus_receive(to_rec); + if (k == -1) { + set_bad_con(); + return BAD_CON; + } + modbuserror_handle(to_rec, WRITE_REGS); + if(err) return err_no; + return 0; + } else { + set_bad_con(); + return BAD_CON; + } +} + +ssize_t modbus::modbus_send(uint8_t *to_send, int length) { + _msg_id++; + return send(_socket, to_send, (size_t)length, 0); +} + + +ssize_t modbus::modbus_receive(uint8_t *buffer) const { + + timeval t_out; + fd_set set; + t_out.tv_sec = 1; + t_out.tv_usec = 0; + FD_ZERO(&set); /* clear the set */ + FD_SET(_socket, &set); /* add our file descriptor to the set */ + int rv = select(_socket +1, &set, NULL, NULL, &t_out); + if(rv == -1) + { + ROS_ERROR_STREAM("socket error accured"); + return -1; + } + else if(rv == 0) + { + ROS_ERROR_STREAM("socket timeout occured"); + return -1; + } + else + return recv(_socket, (char *) buffer, 1024, 0); +} + +void modbus::set_bad_con() { + err = true; + _connected = false; + error_msg = "BAD CONNECTION"; +} + + +void modbus::set_bad_input() { + err = true; + error_msg = "BAD FUNCTION INPUT"; +} + +/** + * Error Code Handler + * @param msg Message Received from the Server + * @param func Modbus Functional Code + */ +void modbus::modbuserror_handle(const uint8_t *msg, int func) { + if(msg[7] == func + 0x80) { + err = true; + switch(msg[8]){ + case EX_ILLEGAL_FUNCTION: + error_msg = "1 Illegal Function"; + err_no = EX_ILLEGAL_FUNCTION; + break; + case EX_ILLEGAL_ADDRESS: + error_msg = "2 Illegal Address"; + err_no = EX_ILLEGAL_ADDRESS; + break; + case EX_ILLEGAL_VALUE: + error_msg = "3 Illegal Value"; + err_no = EX_ILLEGAL_VALUE; + break; + case EX_SERVER_FAILURE: + error_msg = "4 Server Failure"; + err_no = EX_SERVER_FAILURE; + break; + case EX_ACKNOWLEDGE: + error_msg = "5 Acknowledge"; + err_no = EX_ACKNOWLEDGE; + break; + case EX_SERVER_BUSY: + error_msg = "6 Server Busy"; + err_no = EX_SERVER_BUSY; + break; + case EX_NEGATIVE_ACK: + error_msg = "7 Negative Acknowledge"; + err_no = EX_NEGATIVE_ACK; + break; + case EX_MEM_PARITY_PROB: + error_msg = "8 Memory Parity Problem"; + err_no = EX_MEM_PARITY_PROB; + break; + case EX_GATEWAY_PROBLEMP: + error_msg = "10 Gateway Path Unavailable"; + err_no = EX_GATEWAY_PROBLEMP; + break; + case EX_GATEWYA_PROBLEMF: + error_msg = "11 Gateway Target Device Failed to Respond"; + err_no = EX_GATEWYA_PROBLEMF; + break; + default: + error_msg = "UNK"; + err_no = 0; + break; + } + } + err = false; + error_msg = "NO ERR"; +} \ No newline at end of file diff --git a/Devices/Libraries/Ros/random_numbers/.clang-format b/Devices/Libraries/Ros/random_numbers/.clang-format new file mode 100755 index 0000000..28dedb8 --- /dev/null +++ b/Devices/Libraries/Ros/random_numbers/.clang-format @@ -0,0 +1,64 @@ +--- +BasedOnStyle: Google +AccessModifierOffset: -2 +ConstructorInitializerIndentWidth: 2 +AlignEscapedNewlinesLeft: false +AlignTrailingComments: true +AllowAllParametersOfDeclarationOnNextLine: false +AllowShortIfStatementsOnASingleLine: false +AllowShortLoopsOnASingleLine: false +AllowShortFunctionsOnASingleLine: None +AlwaysBreakTemplateDeclarations: true +AlwaysBreakBeforeMultilineStrings: false +BreakBeforeBinaryOperators: false +BreakBeforeTernaryOperators: false +BreakConstructorInitializersBeforeComma: true +BinPackParameters: true +ColumnLimit: 120 +ConstructorInitializerAllOnOneLineOrOnePerLine: true +DerivePointerBinding: false +PointerBindsToType: true +ExperimentalAutoDetectBinPacking: false +IndentCaseLabels: true +MaxEmptyLinesToKeep: 1 +NamespaceIndentation: None +ObjCSpaceBeforeProtocolList: true +PenaltyBreakBeforeFirstCallParameter: 19 +PenaltyBreakComment: 60 +PenaltyBreakString: 100 +PenaltyBreakFirstLessLess: 1000 +PenaltyExcessCharacter: 1000 +PenaltyReturnTypeOnItsOwnLine: 70 +SpacesBeforeTrailingComments: 2 +Cpp11BracedListStyle: false +Standard: Auto +IndentWidth: 2 +TabWidth: 2 +UseTab: Never +IndentFunctionDeclarationAfterType: false +SpacesInParentheses: false +SpacesInAngles: false +SpaceInEmptyParentheses: false +SpacesInCStyleCastParentheses: false +SpaceAfterControlStatementKeyword: true +SpaceBeforeAssignmentOperators: true +ContinuationIndentWidth: 4 +SortIncludes: false +SpaceAfterCStyleCast: false + +# Configure each individual brace in BraceWrapping +BreakBeforeBraces: Custom + +# Control of individual brace wrapping cases +BraceWrapping: + AfterClass: 'true' + AfterControlStatement: 'true' + AfterEnum : 'true' + AfterFunction : 'true' + AfterNamespace : 'true' + AfterStruct : 'true' + AfterUnion : 'true' + BeforeCatch : 'true' + BeforeElse : 'true' + IndentBraces : 'false' +... diff --git a/Devices/Libraries/Ros/random_numbers/.github/workflows/format.yaml b/Devices/Libraries/Ros/random_numbers/.github/workflows/format.yaml new file mode 100755 index 0000000..f4b8811 --- /dev/null +++ b/Devices/Libraries/Ros/random_numbers/.github/workflows/format.yaml @@ -0,0 +1,17 @@ +# This is a format job. Pre-commit has a first-party GitHub action, so we use +# that: https://github.com/pre-commit/action + +name: Format + +on: [push, pull_request] + +jobs: + pre-commit: + name: Format + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v2 + - uses: actions/setup-python@v2 + - name: Install clang-format-10 + run: sudo apt-get install clang-format-10 + - uses: pre-commit/action@v2.0.0 diff --git a/Devices/Libraries/Ros/random_numbers/.github/workflows/industrial_ci_action.yaml b/Devices/Libraries/Ros/random_numbers/.github/workflows/industrial_ci_action.yaml new file mode 100755 index 0000000..d7dfb1b --- /dev/null +++ b/Devices/Libraries/Ros/random_numbers/.github/workflows/industrial_ci_action.yaml @@ -0,0 +1,68 @@ +# This config uses industrial_ci (https://github.com/ros-industrial/industrial_ci.git). +# For troubleshooting, see readme (https://github.com/ros-industrial/industrial_ci/blob/master/README.rst) + +name: BuildAndTest + +on: + workflow_dispatch: + pull_request: + push: + branches: + - master + +jobs: + industrial_ci: + strategy: + matrix: + env: + - ROS_DISTRO: noetic + ROS_REPO: main + CATKIN_LINT: true + - ROS_DISTRO: melodic + ROS_REPO: main + CATKIN_LINT: true + - ROS_DISTRO: kinetic + ROS_REPO: main + env: + CCACHE_DIR: "${{ github.workspace }}/.ccache" + BASEDIR: ${{ github.workspace }}/.work + CACHE_PREFIX: "${{ matrix.env.ROS_DISTRO }}-${{ matrix.env.ROS_REPO }}" + + name: "${{ matrix.env.ROS_DISTRO }}-${{ matrix.env.ROS_REPO }}${{ matrix.env.CATKIN_LINT && ' + catkin_lint' || ''}}" + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v2 + # The target directory cache doesn't include the source directory because + # that comes from the checkout. See "prepare target_ws for cache" task below + - name: cache target_ws + if: ${{ ! matrix.env.CCOV }} + uses: pat-s/always-upload-cache@v2.1.5 + with: + path: ${{ env.BASEDIR }}/target_ws + key: target_ws-${{ env.CACHE_PREFIX }}-${{ hashFiles('**/CMakeLists.txt', '**/package.xml') }}-${{ github.run_id }} + restore-keys: | + target_ws-${{ env.CACHE_PREFIX }}-${{ hashFiles('**/CMakeLists.txt', '**/package.xml') }} + - name: cache ccache + uses: pat-s/always-upload-cache@v2.1.5 + with: + path: ${{ env.CCACHE_DIR }} + key: ccache-${{ env.CACHE_PREFIX }}-${{ github.sha }}-${{ github.run_id }} + restore-keys: | + ccache-${{ env.CACHE_PREFIX }}-${{ github.sha }} + ccache-${{ env.CACHE_PREFIX }} + - name: industrial_ci + uses: 'ros-industrial/industrial_ci@master' + env: ${{ matrix.env }} + - name: upload test artifacts (on failure) + uses: actions/upload-artifact@v2 + if: failure() + with: + name: test-results + path: ${{ env.BASEDIR }}/target_ws/**/test_results/**/*.xml + - name: prepare target_ws for cache + if: ${{ always() && ! matrix.env.CCOV }} + run: | + du -sh ${{ env.BASEDIR }}/target_ws + sudo find ${{ env.BASEDIR }}/target_ws -wholename '*/test_results/*' -delete + sudo rm -rf ${{ env.BASEDIR }}/target_ws/src + du -sh ${{ env.BASEDIR }}/target_ws diff --git a/Devices/Libraries/Ros/random_numbers/.github/workflows/prerelease.yaml b/Devices/Libraries/Ros/random_numbers/.github/workflows/prerelease.yaml new file mode 100755 index 0000000..d2b0ba4 --- /dev/null +++ b/Devices/Libraries/Ros/random_numbers/.github/workflows/prerelease.yaml @@ -0,0 +1,26 @@ +# This config uses industrial_ci (https://github.com/ros-industrial/industrial_ci.git). +# For troubleshooting, see readme (https://github.com/ros-industrial/industrial_ci/blob/master/README.rst) + +name: pre-release + +on: + workflow_dispatch: + +jobs: + default: + strategy: + fail-fast: false + matrix: + distro: [melodic, noetic] + + env: + ROS_DISTRO: ${{ matrix.distro }} + PRERELEASE: true + BASEDIR: ${{ github.workspace }}/.work + + name: "${{ matrix.distro }}" + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v2 + - name: industrial_ci + uses: ros-industrial/industrial_ci@master diff --git a/Devices/Libraries/Ros/random_numbers/.gitignore b/Devices/Libraries/Ros/random_numbers/.gitignore new file mode 100755 index 0000000..b25c15b --- /dev/null +++ b/Devices/Libraries/Ros/random_numbers/.gitignore @@ -0,0 +1 @@ +*~ diff --git a/Devices/Libraries/Ros/random_numbers/.pre-commit-config.yaml b/Devices/Libraries/Ros/random_numbers/.pre-commit-config.yaml new file mode 100755 index 0000000..0a5267b --- /dev/null +++ b/Devices/Libraries/Ros/random_numbers/.pre-commit-config.yaml @@ -0,0 +1,43 @@ +# To use: +# +# pre-commit run -a +# +# Or: +# +# pre-commit install # (runs every time you commit in git) +# +# To update this file: +# +# pre-commit autoupdate +# +# See https://github.com/pre-commit/pre-commit + +repos: + # Standard hooks + - repo: https://github.com/pre-commit/pre-commit-hooks + rev: v3.4.0 + hooks: + - id: check-added-large-files + - id: check-case-conflict + - id: check-merge-conflict + - id: check-symlinks + - id: check-yaml + - id: debug-statements + - id: end-of-file-fixer + - id: mixed-line-ending + - id: trailing-whitespace + + - repo: https://github.com/psf/black + rev: 22.3.0 + hooks: + - id: black + + - repo: local + hooks: + - id: clang-format + name: clang-format + description: Format files with ClangFormat. + entry: clang-format-10 + language: system + files: \.(c|cc|cxx|cpp|frag|glsl|h|hpp|hxx|ih|ispc|ipp|java|js|m|proto|vert)$ + args: ['-fallback-style=none', '-i'] diff --git a/Devices/Libraries/Ros/random_numbers/CHANGELOG.rst b/Devices/Libraries/Ros/random_numbers/CHANGELOG.rst new file mode 100755 index 0000000..6cac465 --- /dev/null +++ b/Devices/Libraries/Ros/random_numbers/CHANGELOG.rst @@ -0,0 +1,130 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package random_numbers +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.3.2 (2018-02-27) +------------------ +* Update maintainership. (`#11 `_) +* Contributors: Steven! Ragnarök + +0.3.1 (2016-04-04) +------------------ +* Merge pull request `#10 `_ from jspricke/cmake_lib + Use catkin variables for install dirs +* Contributors: Dave Coleman, Jochen Sprickerhof + +0.3.0 (2014-09-05) +------------------ +* Update README.md with Documentation +* Allow the randomly generated seed to be saved so that experiments / benc... +* Initialize static int to 0 +* Save the first_seed even when passed in manually. +* Allow the randomly generated seed to be saved so that experiments / benchmarks can be recreated in the future +* Added ability to specify random number generator seed for stochastic behavior +* Added travis build status indicator in README.md +* Contributors: Dave Coleman, Dave Hershberger, Ioan A Sucan + +0.2.0 (2013-07-16) +------------------ +* Merge pull request `#2 `_ from wjwwood/patch-1 + Fix linkedit error on OS X with newer versions of Boost +* Fix linkedit error on OS X with newer versions of Boost + When building `random_numbers` on OS X with Boost 1.53.0 I get: + ``` + ==> Processing catkin package: 'random_numbers' + ==> Creating build directory: 'build_isolated/random_numbers' + ==> Building with env: '/Users/william/moveit_ws/install_isolated/env.sh' + ==> cmake /Users/william/moveit_ws/src/random_numbers -DCATKIN_DEVEL_PREFIX=/Users/william/moveit_ws/devel_isolated/random_numbers -DCMAKE_INSTALL_PREFIX=/Users/william/moveit_ws/install_isolated + -- The C compiler identification is Clang 4.2.0 + -- The CXX compiler identification is Clang 4.2.0 + -- Check for working C compiler: /usr/bin/cc + -- Check for working C compiler: /usr/bin/cc -- works + -- Detecting C compiler ABI info + -- Detecting C compiler ABI info - done + -- Check for working CXX compiler: /usr/bin/c++ + -- Check for working CXX compiler: /usr/bin/c++ -- works + -- Detecting CXX compiler ABI info + -- Detecting CXX compiler ABI info - done + -- Using CATKIN_DEVEL_PREFIX: /Users/william/moveit_ws/devel_isolated/random_numbers + -- Using CMAKE_PREFIX_PATH: /Users/william/moveit_ws/install_isolated + -- This workspace overlays: /Users/william/moveit_ws/install_isolated + -- Found PythonInterp: /usr/bin/python (found version "2.7.2") + -- Found PY_em: /Library/Python/2.7/site-packages/em.pyc + -- Found gtest: gtests will be built + -- Using CATKIN_TEST_RESULTS_DIR: /Users/william/moveit_ws/build_isolated/random_numbers/test_results + -- catkin 0.5.65 + WARNING: 'catkin' should be listed as a buildtool dependency in the package.xml (instead of build dependency) + -- Boost version: 1.53.0 + -- Found the following Boost libraries: + -- date_time + -- thread + -- Configuring done + -- Generating done + -- Build files have been written to: /Users/william/moveit_ws/build_isolated/random_numbers + ==> make -j4 -l4 in '/Users/william/moveit_ws/build_isolated/random_numbers' + Scanning dependencies of target random_numbers + [100%] Building CXX object CMakeFiles/random_numbers.dir/src/random_numbers.cpp.o + Linking CXX shared library /Users/william/moveit_ws/devel_isolated/random_numbers/lib/librandom_numbers.dylib + Undefined symbols for architecture x86_64: + "boost::system::system_category()", referenced from: + ___cxx_global_var_init3 in random_numbers.cpp.o + boost::thread_exception::thread_exception(int, char const*) in random_numbers.cpp.o + "boost::system::generic_category()", referenced from: + ___cxx_global_var_init1 in random_numbers.cpp.o + ___cxx_global_var_init2 in random_numbers.cpp.o + ld: symbol(s) not found for architecture x86_64 + clang: error: linker command failed with exit code 1 (use -v to see invocation) + make[2]: *** [/Users/william/moveit_ws/devel_isolated/random_numbers/lib/librandom_numbers.dylib] Error 1 + make[1]: *** [CMakeFiles/random_numbers.dir/all] Error 2 + make: *** [all] Error 2 + Traceback (most recent call last): + File "./src/catkin/bin/../python/catkin/builder.py", line 658, in build_workspace_isolated + number=index + 1, of=len(ordered_packages) + File "./src/catkin/bin/../python/catkin/builder.py", line 443, in build_package + install, jobs, force_cmake, quiet, last_env, cmake_args, make_args + File "./src/catkin/bin/../python/catkin/builder.py", line 297, in build_catkin_package + run_command(make_cmd, build_dir, quiet) + File "./src/catkin/bin/../python/catkin/builder.py", line 186, in run_command + raise subprocess.CalledProcessError(proc.returncode, ' '.join(cmd)) + CalledProcessError: Command '/Users/william/moveit_ws/install_isolated/env.sh make -j4 -l4' returned non-zero exit status 2 + <== Failed to process package 'random_numbers': + Command '/Users/william/moveit_ws/install_isolated/env.sh make -j4 -l4' returned non-zero exit status 2 + Reproduce this error by running: + ==> /Users/william/moveit_ws/install_isolated/env.sh make -j4 -l4 + Command failed, exiting. + ``` + Adding the `system` element to the `Boost` components being found fixes this. +* fix typo +* Create README.md +* update description +* Merge pull request `#1 `_ from ablasdel/patch-1 + Update package.xml to buildtool_depend +* Update package.xml to buildtool_depend +* Added tag 0.1.3 for changeset 78f37b23c724 +* Contributors: Aaron Blasdel, Tully Foote, William Woodall, isucan + +0.1.3 (2012-10-12 20:13) +------------------------ +* removing outdated install rule +* fixing install rule +* Added tag 0.1.2 for changeset 42db44939f5e +* Contributors: Tully Foote + +0.1.2 (2012-10-12 19:50) +------------------------ +* forgot rename +* Added tag 0.1.2 for changeset 79869d337273 +* updating catkinization and 0.1.2 +* Added tag 0.1.1 for changeset 2e564507c3d1 +* Contributors: Ioan Sucan, Tully Foote + +0.1.1 (2012-06-18 13:21) +------------------------ +* fix manifest +* Added tag 0.1.0 for changeset a1286e23910e +* Contributors: Ioan Sucan + +0.1.0 (2012-06-18 13:17) +------------------------ +* add initial version +* Contributors: Ioan Sucan diff --git a/Devices/Libraries/Ros/random_numbers/CMakeLists.txt b/Devices/Libraries/Ros/random_numbers/CMakeLists.txt new file mode 100755 index 0000000..f877822 --- /dev/null +++ b/Devices/Libraries/Ros/random_numbers/CMakeLists.txt @@ -0,0 +1,29 @@ +cmake_minimum_required(VERSION 3.0.2) +project(random_numbers) + +find_package(catkin REQUIRED) + +catkin_package( + LIBRARIES ${PROJECT_NAME} + INCLUDE_DIRS include + ) + +find_package(Boost REQUIRED date_time system thread) +include_directories(${Boost_INCLUDE_DIR}) + +include_directories(include) + +add_library(${PROJECT_NAME} + src/random_numbers.cpp) + +target_link_libraries(${PROJECT_NAME} ${Boost_LIBRARIES}) + +set_target_properties(${PROJECT_NAME} PROPERTIES WINDOWS_EXPORT_ALL_SYMBOLS TRUE) + +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) diff --git a/Devices/Libraries/Ros/random_numbers/README.md b/Devices/Libraries/Ros/random_numbers/README.md new file mode 100755 index 0000000..0b66470 --- /dev/null +++ b/Devices/Libraries/Ros/random_numbers/README.md @@ -0,0 +1,12 @@ +# random_numbers + +This library contains wrappers for generating floating point values, integers, quaternions using boost libraries. + +## Build Status + +[![BuildAndTest](https://github.com/ros-planning/random_numbers/actions/workflows/industrial_ci_action.yaml/badge.svg?branch=master)](https://github.com/ros-planning/random_numbers/actions/workflows/industrial_ci_action.yaml?branch=master) +[![Format](https://github.com/ros-planning/random_numbers/actions/workflows/format.yaml/badge.svg?branch=master)](https://github.com/ros-planning/random_numbers/actions/workflows/format.yaml?branch=master) + +## Features + +New: you can pass in a custom random number generator seed to allow optional deterministic behavior during debugging, testing, etc. using the secondary constructor. diff --git a/Devices/Libraries/Ros/random_numbers/include/random_numbers/random_numbers.h b/Devices/Libraries/Ros/random_numbers/include/random_numbers/random_numbers.h new file mode 100755 index 0000000..2ff6a94 --- /dev/null +++ b/Devices/Libraries/Ros/random_numbers/include/random_numbers/random_numbers.h @@ -0,0 +1,118 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Willow Garage nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Ioan Sucan */ + +#ifndef RANDOM_NUMBERS_RANDOM_NUMBERS_ +#define RANDOM_NUMBERS_RANDOM_NUMBERS_ + +#include +#include +#include +#include +#include + +namespace random_numbers +{ +/** \brief Random number generation (wrapper for boost). An instance of this class + cannot be used by multiple threads at once (member functions + are not const). However, the constructor is thread safe and + different instances can be used safely in any number of + threads. It is also guaranteed that all created instances will + have a "random" random seed. */ +class RandomNumberGenerator +{ +public: + /** \brief Constructor. Always sets a "random" random seed */ + RandomNumberGenerator(void); + + /** \brief Constructor. Allow a seed to be specified for deterministic behaviour */ + RandomNumberGenerator(boost::uint32_t seed); + + /** \brief Generate a random real between 0 and 1 */ + double uniform01(void) + { + return uni_(); + } + + /** + * @brief Generate a random real within given bounds: [\e lower_bound, \e upper_bound) + * @param lower_bound The lower bound + * @param upper_bound The upper bound + */ + double uniformReal(double lower_bound, double upper_bound) + { + return (upper_bound - lower_bound) * uni_() + lower_bound; + } + + /** \brief Generate a random real using a normal distribution with mean 0 and variance 1 */ + double gaussian01(void) + { + return normal_(); + } + + /** \brief Generate a random real using a normal distribution with given mean and variance */ + double gaussian(double mean, double stddev) + { + return normal_() * stddev + mean; + } + + /** \brief Uniform random unit quaternion sampling. The computed value has the order (x,y,z,w) + * @param value[4] A four dimensional array in which the computed quaternion will be returned + */ + void quaternion(double value[4]); + + /** \brief Generate an integer uniformly at random within a specified range (inclusive) */ + int uniformInteger(int min, int max) + { + boost::uniform_int<> dis(min, max); + return dis(generator_); + } + + /** + * \brief Allow the randomly generated seed to be saved so that experiments / benchmarks can be recreated in the + * future + */ + boost::uint32_t getFirstSeed(); + +private: + boost::mt19937 generator_; + boost::uniform_real<> uniDist_; + boost::normal_distribution<> normalDist_; + boost::variate_generator > uni_; + boost::variate_generator > normal_; +}; +} // namespace random_numbers + +#endif diff --git a/Devices/Libraries/Ros/random_numbers/package.xml b/Devices/Libraries/Ros/random_numbers/package.xml new file mode 100755 index 0000000..49be262 --- /dev/null +++ b/Devices/Libraries/Ros/random_numbers/package.xml @@ -0,0 +1,20 @@ + + random_numbers + 0.3.2 + + This library contains wrappers for generating floating point values, integers, quaternions using boost libraries. + + The constructor of the wrapper is guaranteed to be thread safe and initialize its random number generator to a random seed. + Seeds are obtained using a separate and different random number generator. + + Ioan Sucan + Steven! Ragnarök + BSD + http://ros.org/wiki/random_numbers + + catkin + + boost + + boost + diff --git a/Devices/Libraries/Ros/random_numbers/src/random_numbers.cpp b/Devices/Libraries/Ros/random_numbers/src/random_numbers.cpp new file mode 100755 index 0000000..0880aa0 --- /dev/null +++ b/Devices/Libraries/Ros/random_numbers/src/random_numbers.cpp @@ -0,0 +1,108 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Willow Garage nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Ioan Sucan */ + +#include "random_numbers/random_numbers.h" + +#include +#include +#include +#include +#include +#include + +static boost::uint32_t first_seed_ = 0; + +/// Compute the first seed to be used; this function should be called only once +static boost::uint32_t firstSeed(void) +{ + boost::scoped_ptr mem(new int()); + first_seed_ = (boost::uint32_t)( + (boost::posix_time::microsec_clock::universal_time() - boost::posix_time::ptime(boost::date_time::min_date_time)) + .total_microseconds() + + (unsigned long long)(mem.get())); + return first_seed_; +} + +/// We use a different random number generator for the seeds of the +/// Other random generators. The root seed is from the number of +/// nano-seconds in the current time. +static boost::uint32_t nextSeed(void) +{ + static boost::mutex rngMutex; + boost::mutex::scoped_lock slock(rngMutex); + static boost::lagged_fibonacci607 sGen(firstSeed()); + static boost::uniform_int<> sDist(1, 1000000000); + static boost::variate_generator > s(sGen, sDist); + boost::uint32_t v = s(); + return v; +} + +random_numbers::RandomNumberGenerator::RandomNumberGenerator(void) + : generator_(nextSeed()) + , uniDist_(0, 1) + , normalDist_(0, 1) + , uni_(generator_, uniDist_) + , normal_(generator_, normalDist_) +{ +} + +random_numbers::RandomNumberGenerator::RandomNumberGenerator(boost::uint32_t seed) + : generator_(seed), uniDist_(0, 1), normalDist_(0, 1), uni_(generator_, uniDist_), normal_(generator_, normalDist_) +{ + // Because we manually specified a seed, we need to save it ourselves + first_seed_ = seed; +} + +// From: "Uniform Random Rotations", Ken Shoemake, Graphics Gems III, +// pg. 124-132 +void random_numbers::RandomNumberGenerator::quaternion(double value[4]) +{ + double x0 = uni_(); + double r1 = sqrt(1.0 - x0), r2 = sqrt(x0); + double t1 = 2.0 * boost::math::constants::pi() * uni_(), + t2 = 2.0 * boost::math::constants::pi() * uni_(); + double c1 = cos(t1), s1 = sin(t1); + double c2 = cos(t2), s2 = sin(t2); + value[0] = s1 * r1; + value[1] = c1 * r1; + value[2] = s2 * r2; + value[3] = c2 * r2; +} + +boost::uint32_t random_numbers::RandomNumberGenerator::getFirstSeed() +{ + return first_seed_; +} diff --git a/Devices/Libraries/Ros/ros_canopen/.github/workflows/main.yml b/Devices/Libraries/Ros/ros_canopen/.github/workflows/main.yml new file mode 100755 index 0000000..2c4f8c5 --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/.github/workflows/main.yml @@ -0,0 +1,34 @@ +# This config uses industrial_ci (https://github.com/ros-industrial/industrial_ci.git). +# For troubleshooting, see readme (https://github.com/ros-industrial/industrial_ci/blob/master/README.rst) + +name: CI + +on: + push: + pull_request: + schedule: + - cron: '0 4 * * *' # every day at 4 AM (UTC) + workflow_dispatch: # allow manually starting this workflow + +jobs: + industrial_ci: + name: ROS ${{ matrix.ROS_DISTRO }} (${{ matrix.ROS_REPO }}) + runs-on: ubuntu-latest + strategy: + matrix: # matrix is the product of entries + ROS_DISTRO: [melodic, noetic] + ROS_REPO: [testing, main] + env: + CCACHE_DIR: "${{ github.workspace }}/.ccache" # directory for ccache (and how we enable ccache in industrial_ci) + steps: + - uses: actions/checkout@v2 # clone target repository + - uses: actions/cache@v2 # fetch/store the directory used by ccache before/after the ci run + with: + path: ${{ env.CCACHE_DIR }} + key: ccache-${{ matrix.ROS_DISTRO }}-${{ matrix.ROS_REPO }}-${{github.run_id}} + restore-keys: | + ccache-${{ matrix.ROS_DISTRO }}-${{ matrix.ROS_REPO }}- + - uses: 'ros-industrial/industrial_ci@master' # run industrial_ci + env: + ROS_DISTRO: ${{ matrix.ROS_DISTRO }} + ROS_REPO: ${{ matrix.ROS_REPO }} diff --git a/Devices/Libraries/Ros/ros_canopen/.github/workflows/prerelease.yml b/Devices/Libraries/Ros/ros_canopen/.github/workflows/prerelease.yml new file mode 100755 index 0000000..36384d1 --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/.github/workflows/prerelease.yml @@ -0,0 +1,20 @@ +name: Pre-release + +on: [workflow_dispatch] + +jobs: + default: + strategy: + matrix: + distro: [melodic, noetic] + + env: + BUILDER: catkin_make_isolated + ROS_DISTRO: ${{ matrix.distro }} + PRERELEASE: true + + name: "${{ matrix.distro }}" + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v2 + - uses: ros-industrial/industrial_ci@master diff --git a/Devices/Libraries/Ros/ros_canopen/.gitignore b/Devices/Libraries/Ros/ros_canopen/.gitignore new file mode 100755 index 0000000..d450f31 --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/.gitignore @@ -0,0 +1,440 @@ +# ---> VisualStudio +## Ignore Visual Studio temporary files, build results, and +## files generated by popular Visual Studio add-ons. +## +## Get latest from https://github.com/github/gitignore/blob/main/VisualStudio.gitignore + +# User-specific files +*.rsuser +*.suo +*.user +*.userosscache +*.sln.docstates + +# User-specific files (MonoDevelop/Xamarin Studio) +*.userprefs + +# Mono auto generated files +mono_crash.* + +# Build results +[Dd]ebug/ +[Dd]ebugPublic/ +[Rr]elease/ +[Rr]eleases/ +x64/ +x86/ +[Ww][Ii][Nn]32/ +[Aa][Rr][Mm]/ +[Aa][Rr][Mm]64/ +bld/ +[Bb]in/ +[Oo]bj/ +[Ll]og/ +[Ll]ogs/ + +# Visual Studio 2015/2017 cache/options directory +.vs/ +# Uncomment if you have tasks that create the project's static files in wwwroot +#wwwroot/ + +# Visual Studio 2017 auto generated files +Generated\ Files/ + +# MSTest test Results +[Tt]est[Rr]esult*/ +[Bb]uild[Ll]og.* + +# NUnit +*.VisualState.xml +TestResult.xml +nunit-*.xml + +# Build Results of an ATL Project +[Dd]ebugPS/ +[Rr]eleasePS/ +dlldata.c + +# Benchmark Results +BenchmarkDotNet.Artifacts/ + +# .NET Core +project.lock.json +project.fragment.lock.json +artifacts/ + +# ASP.NET Scaffolding +ScaffoldingReadMe.txt + +# StyleCop +StyleCopReport.xml + +# Files built by Visual Studio +*_i.c +*_p.c +*_h.h +*.ilk +*.meta +*.obj +*.iobj +*.pch +*.pdb +*.ipdb +*.pgc +*.pgd +*.rsp +*.sbr +*.tlb +*.tli +*.tlh +*.tmp +*.tmp_proj +*_wpftmp.csproj +*.log +*.tlog +*.vspscc +*.vssscc +.builds +*.pidb +*.svclog +*.scc + +# Chutzpah Test files +_Chutzpah* + +# Visual C++ cache files +ipch/ +*.aps +*.ncb +*.opendb +*.opensdf +*.sdf +*.cachefile +*.VC.db +*.VC.VC.opendb + +# Visual Studio profiler +*.psess +*.vsp +*.vspx +*.sap + +# Visual Studio Trace Files +*.e2e + +# TFS 2012 Local Workspace +$tf/ + +# Guidance Automation Toolkit +*.gpState + +# ReSharper is a .NET coding add-in +_ReSharper*/ +*.[Rr]e[Ss]harper +*.DotSettings.user + +# TeamCity is a build add-in +_TeamCity* + +# DotCover is a Code Coverage Tool +*.dotCover + +# AxoCover is a Code Coverage Tool +.axoCover/* +!.axoCover/settings.json + +# Coverlet is a free, cross platform Code Coverage Tool +coverage*.json +coverage*.xml +coverage*.info + +# Visual Studio code coverage results +*.coverage +*.coveragexml + +# NCrunch +_NCrunch_* +.*crunch*.local.xml +nCrunchTemp_* + +# MightyMoose +*.mm.* +AutoTest.Net/ + +# Web workbench (sass) +.sass-cache/ + +# Installshield output folder +[Ee]xpress/ + +# DocProject is a documentation generator add-in +DocProject/buildhelp/ +DocProject/Help/*.HxT +DocProject/Help/*.HxC +DocProject/Help/*.hhc +DocProject/Help/*.hhk +DocProject/Help/*.hhp +DocProject/Help/Html2 +DocProject/Help/html + +# Click-Once directory +publish/ + +# Publish Web Output +*.[Pp]ublish.xml +*.azurePubxml +# Note: Comment the next line if you want to checkin your web deploy settings, +# but database connection strings (with potential passwords) will be unencrypted +*.pubxml +*.publishproj + +# Microsoft Azure Web App publish settings. Comment the next line if you want to +# checkin your Azure Web App publish settings, but sensitive information contained +# in these scripts will be unencrypted +PublishScripts/ + +# NuGet Packages +*.nupkg +# NuGet Symbol Packages +*.snupkg +# The packages folder can be ignored because of Package Restore +**/[Pp]ackages/* +# except build/, which is used as an MSBuild target. +!**/[Pp]ackages/build/ +# Uncomment if necessary however generally it will be regenerated when needed +#!**/[Pp]ackages/repositories.config +# NuGet v3's project.json files produces more ignorable files +*.nuget.props +*.nuget.targets + +# Microsoft Azure Build Output +csx/ +*.build.csdef + +# Microsoft Azure Emulator +ecf/ +rcf/ + +# Windows Store app package directories and files +AppPackages/ +BundleArtifacts/ +Package.StoreAssociation.xml +_pkginfo.txt +*.appx +*.appxbundle +*.appxupload + +# Visual Studio cache files +# files ending in .cache can be ignored +*.[Cc]ache +# but keep track of directories ending in .cache +!?*.[Cc]ache/ + +# Others +ClientBin/ +~$* +*~ +*.dbmdl +*.dbproj.schemaview +*.jfm +*.pfx +*.publishsettings +orleans.codegen.cs + +# Including strong name files can present a security risk +# (https://github.com/github/gitignore/pull/2483#issue-259490424) +#*.snk + +# Since there are multiple workflows, uncomment next line to ignore bower_components +# (https://github.com/github/gitignore/pull/1529#issuecomment-104372622) +#bower_components/ + +# RIA/Silverlight projects +Generated_Code/ + +# Backup & report files from converting an old project file +# to a newer Visual Studio version. Backup files are not needed, +# because we have git ;-) +_UpgradeReport_Files/ +Backup*/ +UpgradeLog*.XML +UpgradeLog*.htm +ServiceFabricBackup/ +*.rptproj.bak + +# SQL Server files +*.mdf +*.ldf +*.ndf + +# Business Intelligence projects +*.rdl.data +*.bim.layout +*.bim_*.settings +*.rptproj.rsuser +*- [Bb]ackup.rdl +*- [Bb]ackup ([0-9]).rdl +*- [Bb]ackup ([0-9][0-9]).rdl + +# Microsoft Fakes +FakesAssemblies/ + +# GhostDoc plugin setting file +*.GhostDoc.xml + +# Node.js Tools for Visual Studio +.ntvs_analysis.dat +node_modules/ + +# Visual Studio 6 build log +*.plg + +# Visual Studio 6 workspace options file +*.opt + +# Visual Studio 6 auto-generated workspace file (contains which files were open etc.) +*.vbw + +# Visual Studio 6 auto-generated project file (contains which files were open etc.) +*.vbp + +# Visual Studio 6 workspace and project file (working project files containing files to include in project) +*.dsw +*.dsp + +# Visual Studio 6 technical files +*.ncb +*.aps + +# Visual Studio LightSwitch build output +**/*.HTMLClient/GeneratedArtifacts +**/*.DesktopClient/GeneratedArtifacts +**/*.DesktopClient/ModelManifest.xml +**/*.Server/GeneratedArtifacts +**/*.Server/ModelManifest.xml +_Pvt_Extensions + +# Paket dependency manager +.paket/paket.exe +paket-files/ + +# FAKE - F# Make +.fake/ + +# CodeRush personal settings +.cr/personal + +# Python Tools for Visual Studio (PTVS) +__pycache__/ +*.pyc + +# Cake - Uncomment if you are using it +# tools/** +# !tools/packages.config + +# Tabs Studio +*.tss + +# Telerik's JustMock configuration file +*.jmconfig + +# BizTalk build output +*.btp.cs +*.btm.cs +*.odx.cs +*.xsd.cs + +# OpenCover UI analysis results +OpenCover/ + +# Azure Stream Analytics local run output +ASALocalRun/ + +# MSBuild Binary and Structured Log +*.binlog + +# NVidia Nsight GPU debugger configuration file +*.nvuser + +# MFractors (Xamarin productivity tool) working folder +.mfractor/ + +# Local History for Visual Studio +.localhistory/ + +# Visual Studio History (VSHistory) files +.vshistory/ + +# BeatPulse healthcheck temp database +healthchecksdb + +# Backup folder for Package Reference Convert tool in Visual Studio 2017 +MigrationBackup/ + +# Ionide (cross platform F# VS Code tools) working folder +.ionide/ + +# Fody - auto-generated XML schema +FodyWeavers.xsd + +# VS Code files for those working on multiple tools +.vscode/* +!.vscode/settings.json +!.vscode/tasks.json +!.vscode/launch.json +!.vscode/extensions.json +*.code-workspace + +# Local History for Visual Studio Code +.history/ + +# Windows Installer files from build outputs +*.cab +*.msi +*.msix +*.msm +*.msp + +# JetBrains Rider +*.sln.iml + +# ---> VisualStudioCode +.vscode/* +!.vscode/settings.json +!.vscode/tasks.json +!.vscode/launch.json +!.vscode/extensions.json +!.vscode/*.code-snippets + +# Local History for Visual Studio Code +.history/ + +# Built Visual Studio Code Extensions +*.vsix + +# temporary files +*~ + +# autogenerated folders +bin +build +lib +__pycache__ +msg_gen +srv_gen + +# autogenerated msgs +*Action.msg +*Action.py +*ActionFeedback.msg +*ActionFeedback.py +*ActionGoal.msg +*ActionGoal.py +*Feedback.msg +*Feedback.py +*Goal.msg +*Goal.py +*Result.msg +*Result.py +*.pyc + diff --git a/Devices/Libraries/Ros/ros_canopen/.travis.yml b/Devices/Libraries/Ros/ros_canopen/.travis.yml new file mode 100755 index 0000000..bd95aac --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/.travis.yml @@ -0,0 +1,27 @@ +language: generic +services: + - docker + +cache: + directories: + - $HOME/.ccache + +env: + global: + - CCACHE_DIR=$HOME/.ccache + matrix: + - ROS_DISTRO="melodic" ROS_REPO=ros-shadow-fixed + - ROS_DISTRO="melodic" ROS_REPO=ros CATKIN_LINT=pedantic + - ROS_DISTRO="melodic" ROSDEP_SKIP_KEYS=muparser EXPECT_EXIT_CODE=1 + - ROS_DISTRO="melodic" ABICHECK_URL='github:ros-industrial/ros_canopen#melodic' + - ROS_DISTRO="noetic" + - ROS_DISTRO="noetic" ROS_REPO=ros OS_NAME=debian OS_CODE_NAME=buster + +matrix: + allow_failures: + - env: ROS_DISTRO="melodic" ABICHECK_URL='github:ros-industrial/ros_canopen#melodic' + +install: + - git clone --quiet --depth=1 https://github.com/ros-industrial/industrial_ci.git .industrial_ci -b master +script: + - .industrial_ci/travis.sh diff --git a/Devices/Libraries/Ros/ros_canopen/CONTRIBUTING.md b/Devices/Libraries/Ros/ros_canopen/CONTRIBUTING.md new file mode 100755 index 0000000..1acf82d --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/CONTRIBUTING.md @@ -0,0 +1,21 @@ +ROS-Industrial is a community project. We welcome contributions from any source, from those who are extremely active to casual users. The following sections outline the steps on how to contribute to ROS-Industrial. It assumes there is an existing repository to which one would like to contribute (item 1 in the figure above) and one is familiar with the Git "Fork and Branch" workflow, detailed [here](http://blog.scottlowe.org/2015/01/27/using-fork-branch-git-workflow/). + +1. Before any development is undertaken, a contributor would communicate a need and/or issue to the ROS-Industrial community. This can be done by submitting an issue on the appropriate GitHub repo, the [issues repo](https://github.com/ros-industrial/ros_industrial_issues), or by posting a message in the [ROS-Industrial category on ROS Discourse](//swri-ros-pkg-dev@googlegroups.com). . Doing so may save you time if similar development is underway and ensure that whatever approach you take is acceptable to the community of reviewers once it is submitted. +2. The second step (item 2) is to implement your change. If you are working on a code contribution, we highly recommend you utilize the [ROS Qt-Creator Plug-in](http://rosindustrial.org/news/2016/6/9/ros-qt-ide-plugin). Verify that your change successfully builds and passes all tests. +3. Next, push your changes to a "feature" branch in your personal fork of the repo and issue a pull request (PR)(item 3). The PR allows maintainers to review the submitted code. Before the PR can be accepted, the maintainer and contributor must agree that the contribution is implemented appropriately. This process can take several back-and-forth steps (see [example](https://github.com/ros-industrial/motoman/pull/89)). Contributors should expect to spend as much time reviewing/changing the code as on the initial implementation. This time can be minimized by communicating with the ROS-Industrial community before any contribution is made. +4. Issuing a Pull Request (PR) triggers the [Travis Continuous Integrations (CI)](https://github.com/ros-industrial/industrial_ci) step (item 4) which happens automatically in the background. The Travis CI performs several operations, and if any of the steps below fail, then the PR is marked accordingly for the maintainer. + * Travis Workflow: + * Installs a barebones ROS distribution on a fresh Ubuntu virtual machine. + * Creates a catkin workspace and puts the repository in it. + * Uses wstool to check out any from-source dependencies (i.e. other repositories). + * Resolves package dependencies using rosdep (i.e. install packages using apt-get). + * Compiles the catkin workspace. + * Runs all available unit tests. +5. If the PR passes Travis CI and one of the maintainers is satisfied with the changes, they post a +1 as a comment on the PR (item 5). The +1 signifies that the PR is ready to be merged. All PRs require at least one +1 and pass Travis CI before it can be merged. +6. The next step (item 6) is for the PR to be merged into the main branch. This is done through the GitHub web interface by selecting the “Merge pull request” button. After the PR is merged, all status badges are updated automatically. +7. Periodically, the maintainer will release the package (item 7), which then gets sent to the [ROS Build Farm](http://wiki.ros.org/build.ros.org) for Debian creation. +8. The publishing of the released packages (item 8) is managed by OSRF and is not on a set schedule. This usually happens when all packages for a given distro are built successfully and stable. The current status for the distro kinetic can be found [here](http://repositories.ros.org/status_page/ros_kinetic_default.html) . Navigating to other distros can be done by changing the distro name in the link. +9. Once the package has been published, it is available to be installed by the developer (item 9). +10. After the install of a new version, the developer may have questions, experience issues or it may not have the necessary functionality which should all be reported on the packages GitHub repository as an issue (item 10). If an issue is identified or there is missing functionality that the developer requires, the cycle starts back at (item 2). + +For more details, please refer to the [ROS-I wiki](http://wiki.ros.org/Industrial/DevProcess). diff --git a/Devices/Libraries/Ros/ros_canopen/LICENSE b/Devices/Libraries/Ros/ros_canopen/LICENSE new file mode 100755 index 0000000..bde60ce --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/LICENSE @@ -0,0 +1,165 @@ +GNU LESSER GENERAL PUBLIC LICENSE + Version 3, 29 June 2007 + + Copyright (C) 2007 Free Software Foundation, Inc. + Everyone is permitted to copy and distribute verbatim copies + of this license document, but changing it is not allowed. + + + This version of the GNU Lesser General Public License incorporates +the terms and conditions of version 3 of the GNU General Public +License, supplemented by the additional permissions listed below. + + 0. Additional Definitions. + + As used herein, "this License" refers to version 3 of the GNU Lesser +General Public License, and the "GNU GPL" refers to version 3 of the GNU +General Public License. + + "The Library" refers to a covered work governed by this License, +other than an Application or a Combined Work as defined below. + + An "Application" is any work that makes use of an interface provided +by the Library, but which is not otherwise based on the Library. +Defining a subclass of a class defined by the Library is deemed a mode +of using an interface provided by the Library. + + A "Combined Work" is a work produced by combining or linking an +Application with the Library. The particular version of the Library +with which the Combined Work was made is also called the "Linked +Version". + + The "Minimal Corresponding Source" for a Combined Work means the +Corresponding Source for the Combined Work, excluding any source code +for portions of the Combined Work that, considered in isolation, are +based on the Application, and not on the Linked Version. + + The "Corresponding Application Code" for a Combined Work means the +object code and/or source code for the Application, including any data +and utility programs needed for reproducing the Combined Work from the +Application, but excluding the System Libraries of the Combined Work. + + 1. Exception to Section 3 of the GNU GPL. + + You may convey a covered work under sections 3 and 4 of this License +without being bound by section 3 of the GNU GPL. + + 2. Conveying Modified Versions. + + If you modify a copy of the Library, and, in your modifications, a +facility refers to a function or data to be supplied by an Application +that uses the facility (other than as an argument passed when the +facility is invoked), then you may convey a copy of the modified +version: + + a) under this License, provided that you make a good faith effort to + ensure that, in the event an Application does not supply the + function or data, the facility still operates, and performs + whatever part of its purpose remains meaningful, or + + b) under the GNU GPL, with none of the additional permissions of + this License applicable to that copy. + + 3. Object Code Incorporating Material from Library Header Files. + + The object code form of an Application may incorporate material from +a header file that is part of the Library. You may convey such object +code under terms of your choice, provided that, if the incorporated +material is not limited to numerical parameters, data structure +layouts and accessors, or small macros, inline functions and templates +(ten or fewer lines in length), you do both of the following: + + a) Give prominent notice with each copy of the object code that the + Library is used in it and that the Library and its use are + covered by this License. + + b) Accompany the object code with a copy of the GNU GPL and this license + document. + + 4. Combined Works. + + You may convey a Combined Work under terms of your choice that, +taken together, effectively do not restrict modification of the +portions of the Library contained in the Combined Work and reverse +engineering for debugging such modifications, if you also do each of +the following: + + a) Give prominent notice with each copy of the Combined Work that + the Library is used in it and that the Library and its use are + covered by this License. + + b) Accompany the Combined Work with a copy of the GNU GPL and this license + document. + + c) For a Combined Work that displays copyright notices during + execution, include the copyright notice for the Library among + these notices, as well as a reference directing the user to the + copies of the GNU GPL and this license document. + + d) Do one of the following: + + 0) Convey the Minimal Corresponding Source under the terms of this + License, and the Corresponding Application Code in a form + suitable for, and under terms that permit, the user to + recombine or relink the Application with a modified version of + the Linked Version to produce a modified Combined Work, in the + manner specified by section 6 of the GNU GPL for conveying + Corresponding Source. + + 1) Use a suitable shared library mechanism for linking with the + Library. A suitable mechanism is one that (a) uses at run time + a copy of the Library already present on the user's computer + system, and (b) will operate properly with a modified version + of the Library that is interface-compatible with the Linked + Version. + + e) Provide Installation Information, but only if you would otherwise + be required to provide such information under section 6 of the + GNU GPL, and only to the extent that such information is + necessary to install and execute a modified version of the + Combined Work produced by recombining or relinking the + Application with a modified version of the Linked Version. (If + you use option 4d0, the Installation Information must accompany + the Minimal Corresponding Source and Corresponding Application + Code. If you use option 4d1, you must provide the Installation + Information in the manner specified by section 6 of the GNU GPL + for conveying Corresponding Source.) + + 5. Combined Libraries. + + You may place library facilities that are a work based on the +Library side by side in a single library together with other library +facilities that are not Applications and are not covered by this +License, and convey such a combined library under terms of your +choice, if you do both of the following: + + a) Accompany the combined library with a copy of the same work based + on the Library, uncombined with any other library facilities, + conveyed under the terms of this License. + + b) Give prominent notice with the combined library that part of it + is a work based on the Library, and explaining where to find the + accompanying uncombined form of the same work. + + 6. Revised Versions of the GNU Lesser General Public License. + + The Free Software Foundation may publish revised and/or new versions +of the GNU Lesser General Public License from time to time. Such new +versions will be similar in spirit to the present version, but may +differ in detail to address new problems or concerns. + + Each version is given a distinguishing version number. If the +Library as you received it specifies that a certain numbered version +of the GNU Lesser General Public License "or any later version" +applies to it, you have the option of following the terms and +conditions either of that published version or of any later version +published by the Free Software Foundation. If the Library as you +received it does not specify a version number of the GNU Lesser +General Public License, you may choose any version of the GNU Lesser +General Public License ever published by the Free Software Foundation. + + If the Library as you received it specifies that a proxy can decide +whether future versions of the GNU Lesser General Public License shall +apply, that proxy's public statement of acceptance of any version is +permanent authorization for you to choose that version for the +Library. \ No newline at end of file diff --git a/Devices/Libraries/Ros/ros_canopen/README.md b/Devices/Libraries/Ros/ros_canopen/README.md new file mode 100755 index 0000000..df78114 --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/README.md @@ -0,0 +1,11 @@ +ros_canopen +=========== + +Canopen implementation for ROS. + +[![Build Status](https://travis-ci.com/ros-industrial/ros_canopen.svg?branch=melodic-devel)](https://travis-ci.com/ros-industrial/ros_canopen) +[![License: LGPL v3](https://img.shields.io/badge/License-LGPL%20v3-blue.svg)](https://www.gnu.org/licenses/lgpl-3.0) +([![License: BSD 3-Clause](https://img.shields.io/badge/License-BSD%203--Clause-blue.svg)](https://opensource.org/licenses/BSD-3-Clause) for `can_msgs` and `socketcan_bridge`) + +The current develop branch is `melodic-devel`, it targets ROS `melodic`. Needs C++14 compiler. +The released version gets synced over to the distro branch for each release. diff --git a/Devices/Libraries/Ros/ros_canopen/can_msgs/CHANGELOG.rst b/Devices/Libraries/Ros/ros_canopen/can_msgs/CHANGELOG.rst new file mode 100755 index 0000000..aaa841c --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/can_msgs/CHANGELOG.rst @@ -0,0 +1,72 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package can_msgs +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.8.5 (2020-09-22) +------------------ + +0.8.4 (2020-08-22) +------------------ + +0.8.3 (2020-05-07) +------------------ +* Bump CMake version to avoid CMP0048 warning + Signed-off-by: ahcorde +* Contributors: ahcorde + +0.8.2 (2019-11-04) +------------------ + +0.8.1 (2019-07-14) +------------------ +* Set C++ standard to c++14 +* Contributors: Harsh Deshpande + +0.8.0 (2018-07-11) +------------------ + +0.7.8 (2018-05-04) +------------------ + +0.7.7 (2018-05-04) +------------------ + +0.7.6 (2017-08-30) +------------------ + +0.7.5 (2017-05-29) +------------------ + +0.7.4 (2017-04-25) +------------------ + +0.7.3 (2017-04-25) +------------------ + +0.7.2 (2017-03-28) +------------------ + +0.7.1 (2017-03-20) +------------------ + +0.7.0 (2016-12-13) +------------------ + +0.6.5 (2016-12-10) +------------------ +* hamonized versions +* styled and sorted CMakeLists.txt + * removed boilerplate comments + * indention + * reviewed exported dependencies +* styled and sorted package.xml +* Adds message_runtime to can_msgs dependencies. + Added the missing dependency, also changes message_generation to a build_depend. +* Finalizes work on the socketcan_bridge and can_msgs. + Readies the packages socketcan_bridge and can_msgs for the merge with ros_canopen. + Bumps the version for both packages to 0.1.0. Final cleanup in CMakeLists, added + comments to the shell script and launchfile used for testing. +* Introduces the can_msgs package for message types. + Package to hold CAN message types, the Frame message type can contain the data + as returned by SocketCAN. +* Contributors: Ivor Wanders, Mathias Lüdtke diff --git a/Devices/Libraries/Ros/ros_canopen/can_msgs/CMakeLists.txt b/Devices/Libraries/Ros/ros_canopen/can_msgs/CMakeLists.txt new file mode 100755 index 0000000..c3cc14a --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/can_msgs/CMakeLists.txt @@ -0,0 +1,29 @@ +cmake_minimum_required(VERSION 3.0.2) +project(can_msgs) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_STANDARD 14) +endif() + +find_package(catkin REQUIRED + COMPONENTS + message_generation + std_msgs +) + +add_message_files(DIRECTORY msg + FILES + Frame.msg +) + +generate_messages( + DEPENDENCIES + std_msgs +) + +catkin_package( + CATKIN_DEPENDS + message_runtime + std_msgs +) diff --git a/Devices/Libraries/Ros/ros_canopen/can_msgs/msg/Frame.msg b/Devices/Libraries/Ros/ros_canopen/can_msgs/msg/Frame.msg new file mode 100755 index 0000000..9a878be --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/can_msgs/msg/Frame.msg @@ -0,0 +1,7 @@ +Header header +uint32 id +bool is_rtr +bool is_extended +bool is_error +uint8 dlc +uint8[8] data \ No newline at end of file diff --git a/Devices/Libraries/Ros/ros_canopen/can_msgs/package.xml b/Devices/Libraries/Ros/ros_canopen/can_msgs/package.xml new file mode 100755 index 0000000..0fc808e --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/can_msgs/package.xml @@ -0,0 +1,24 @@ + +. + can_msgs + 0.8.5 + CAN related message types. + + Mathias Lüdtke + Ivor Wanders + + BSD + + http://wiki.ros.org/can_msgs + https://github.com/ros-industrial/ros_canopen + https://github.com/ros-industrial/ros_canopen/issues + + catkin + + std_msgs + + message_generation + message_runtime + message_runtime + + diff --git a/Devices/Libraries/Ros/ros_canopen/canopen_402/CHANGELOG.rst b/Devices/Libraries/Ros/ros_canopen/canopen_402/CHANGELOG.rst new file mode 100755 index 0000000..8636030 --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/canopen_402/CHANGELOG.rst @@ -0,0 +1,233 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package canopen_402 +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.8.5 (2020-09-22) +------------------ + +0.8.4 (2020-08-22) +------------------ +* handle illegal states in nextStateForEnabling +* tranistion -> transition +* Contributors: Mathias Lüdtke, Mikael Arguedas + +0.8.3 (2020-05-07) +------------------ +* Bump CMake version to avoid CMP0048 warning + Signed-off-by: ahcorde +* Contributors: ahcorde + +0.8.2 (2019-11-04) +------------------ +* enable rosconsole_bridge bindings +* switch to new logging macros +* Contributors: Mathias Lüdtke + +0.8.1 (2019-07-14) +------------------ +* Set C++ standard to c++14 +* Contributors: Harsh Deshpande + +0.8.0 (2018-07-11) +------------------ +* handle invalid supported drive modes object +* made Mode402::registerMode a variadic template +* use std::isnan +* migrated to std::atomic +* migrated to std::unordered_map and std::unordered_set +* migrated to std pointers +* fix initialization bug in ProfiledPositionMode +* Contributors: Mathias Lüdtke + +0.7.8 (2018-05-04) +------------------ +* Revert "pull make_shared into namespaces" + This reverts commit 9b2cd05df76d223647ca81917d289ca6330cdee6. +* Contributors: Mathias Lüdtke + +0.7.7 (2018-05-04) +------------------ +* Added state_switch_timeout parameter to motor. +* added types for all function objects +* pull make_shared into namespaces +* added types for all shared_ptrs +* migrate to new classloader headers +* address catkin_lint errors/warnings +* Contributors: Alexander Gutenkunst, Mathias Lüdtke + +0.7.6 (2017-08-30) +------------------ + +0.7.5 (2017-05-29) +------------------ + +0.7.4 (2017-04-25) +------------------ +* use portable boost::math::isnan +* Contributors: Mathias Lüdtke + +0.7.3 (2017-04-25) +------------------ + +0.7.2 (2017-03-28) +------------------ + +0.7.1 (2017-03-20) +------------------ +* do quickstop for halt only if operation is enabled +* Contributors: Mathias Lüdtke + +0.7.0 (2016-12-13) +------------------ + +0.6.5 (2016-12-10) +------------------ +* stop on internal limit only if limit was not reached before +* hardened code with the help of cppcheck +* Do not send control if it was not changed + Otherwise invalid state machine transitions might get commanded +* styled and sorted CMakeLists.txt + * removed boilerplate comments + * indention + * reviewed exported dependencies +* styled and sorted package.xml +* update package URLs +* added option to turn off mode monitor +* reset Fault_Reset bit in output only +* enforce rising edge on fault reset bit on init and recover +* Revert "Enforce rising edge on fault reset bit on init and recover" +* enforce rising edge on fault reset bit on init and recover +* Merge pull request `#117 `_ from ipa-mdl/condvars + Reviewed condition variables +* use boost::numeric_cast for limit checks since it handles 64bit values right +* add clamping test +* enforce target type limits +* ModeTargetHelper is now template +* readability fix +* simplified State402::waitForNewState and Motor402::switchState +* Set Halt bit unless current mode releases it +* Contributors: Mathias Lüdtke + +0.6.4 (2015-07-03) +------------------ + +0.6.3 (2015-06-30) +------------------ +* improved PP mode +* do not quickstop in fault states +* do not diable selected mode on recover, just restart it +* initialize to No_Mode +* removed some empty lines +* implemented support for mode switching in a certain 402 state, defaults to Operation_Enable +* pass LayerStatus to switchMode +* remove enableMotor, introduced switchState +* added motor_layer settings +* fixed PP mode +* explicit find for class_loader +* fail-safe setting of op_mode to No_Mode +* Improved behaviour of concurrent commands +* added alternative Transitions 7 and 10 via Quickstop +* added alternative success condition to waitForNewState +* make motor init/recover interruptable +* changed maintainer +* removed SM-based 402 implementation +* added Motor402 plugin +* added new 402 implementation +* added MotorBase +* Added validity checks +* Removed overloaded functions and makes the handle functions protected +* Removes test executable +* Removes unnecessary configure_drive and clears the set Targets +* Some position fixes +* Removed timeout + Reduced recover trials +* Removes some logs +* Homing integrated +* handleRead, handleWrite fixes +* Merge remote-tracking branch 'mdl/indigo_dev' into refactor_sm + Conflicts: + canopen_402/include/canopen_402/canopen_402.h + canopen_402/src/canopen_402/canopen_402.cpp + canopen_motor_node/src/control_node.cpp +* Moved supported_drive_modes to ModeSpecificEntries +* * Init, Recover, Halt for SCHUNK + * Removed sleeps from the state machine + * Now works as reentrant states +* refactored Layer mechanisms +* Recover failure +* Merge remote-tracking branch 'mdl/indigo_dev' into refactor_sm + Conflicts: + canopen_402/include/canopen_402/canopen_402.h + canopen_402/src/canopen_402/canopen_402.cpp +* Removing some unnecessary couts +* First version with Recover + * Tested on SCHUNK LWA4D +* Initializing all modules at once +* Moving SCHUNK using the IP mode sub-state machine +* Schunk does not set operation mode via synchronized RPDO +* initialize homing_needed to false +* Working with the guard handling and some scoped_locks to prevent unwanted access +* Passing ``state_`` to motor machine +* Fixes crash for unitialized boost pointer for ``target_vel_`` and ``target_pos_`` +* Thread running +* Separates the hw with the SM test + Advance on the Mode Switching Machine +* Organizing IPMode State Machine +* Adds mode switch and a pre-version of the turnOn sequence +* Event argument passed to the Motor state machine +* Adds the internal actions +* Control_word is set from motor state machine +* Motor abstraction on higher level machine + Some pointers organization +* * Begins with the Higher Level Machine + * Separates the status and control from the 402 node +* Ip mode sub-machine +* Organizing the status and control machine +* do not read homing method if homing mode is not supported +* inti ``enter_mode_failure_`` to false +* require message strings for error indicators, added missing strings, added ROS logging in sync loop +* Merge pull request `#75 `_ from mistoll/indigo_release_candidate + Move ip_mode_sub_mode to configureModeSpecificEntries +* Fixed tabs/spaces +* bind IP sub mode only if IP is supported +* Move ip_mode_sub_mode to configureModeSpecificEntries +* Update state_machine.h +* Ongoing changes for the state machine +* * Eliminates Internal State conflict + * Treats exceptions inside the state machine +* Cleaning the 402.cpp file +* Test file +* Adds state machine definition +* Adds state machine simple test +* Some cleaning necessary to proceed +* Header files for isolating the 402 state machine +* Effort value +* Merge pull request `#6 `_ from ipa-mdl/indigo_dev + Work-around for https://github.com/ipa320/ros_canopen/issues/62 +* Merge branch 'indigo_dev' of https://github.com/ipa-mdl/ros_canopen into indigo_dev +* fixed unintialized members +* Mode Error priority +* Order issue +* Merge branch 'indigo_dev' of https://github.com/ipa-mdl/ros_canopen into indigo_dev + Conflicts: + canopen_motor_node/CMakeLists.txt +* Error status +* Merge branch 'indigo_dev' into merge + Conflicts: + canopen_chain_node/include/canopen_chain_node/chain_ros.h + canopen_master/include/canopen_master/canopen.h + canopen_master/include/canopen_master/layer.h + canopen_master/src/node.cpp + canopen_motor_node/CMakeLists.txt + canopen_motor_node/src/control_node.cpp +* Contributors: Florian Weisshardt, Mathias Lüdtke, Michael Stoll, Thiago de Freitas Oliveira Araujo, thiagodefreitas + +0.6.2 (2014-12-18) +------------------ + +0.6.1 (2014-12-15) +------------------ +* remove ipa_* and IPA_* prefixes +* added descriptions and authors +* renamed ipa_canopen_402 to canopen_402 +* Contributors: Florian Weisshardt, Mathias Lüdtke diff --git a/Devices/Libraries/Ros/ros_canopen/canopen_402/CMakeLists.txt b/Devices/Libraries/Ros/ros_canopen/canopen_402/CMakeLists.txt new file mode 100755 index 0000000..345fbba --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/canopen_402/CMakeLists.txt @@ -0,0 +1,78 @@ +cmake_minimum_required(VERSION 3.0.2) +project(canopen_402) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_STANDARD 14) +endif() + +find_package(catkin REQUIRED + COMPONENTS + canopen_master + class_loader +) + +find_package(Boost REQUIRED +) + +catkin_package( + INCLUDE_DIRS + include + LIBRARIES + ${PROJECT_NAME} + CATKIN_DEPENDS + canopen_master + DEPENDS + Boost +) + +include_directories( + include + ${Boost_INCLUDE_DIRS} + ${catkin_INCLUDE_DIRS} +) + +# canopen_402 +add_library(${PROJECT_NAME} + src/motor.cpp +) +target_link_libraries(${PROJECT_NAME} + ${catkin_LIBRARIES} +) + +# canopen_402_plugin +add_library(${PROJECT_NAME}_plugin + src/plugin.cpp +) +target_link_libraries(${PROJECT_NAME}_plugin + ${catkin_LIBRARIES} + ${PROJECT_NAME} +) + +install( + TARGETS + ${PROJECT_NAME} + ${PROJECT_NAME}_plugin + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" +) +install( + FILES + ${PROJECT_NAME}_plugin.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +if(CATKIN_ENABLE_TESTING) + catkin_add_gtest(${PROJECT_NAME}-test_clamping + test/clamping.cpp + ) + target_link_libraries(${PROJECT_NAME}-test_clamping + ${catkin_LIBRARIES} + ) +endif() diff --git a/Devices/Libraries/Ros/ros_canopen/canopen_402/canopen_402_plugin.xml b/Devices/Libraries/Ros/ros_canopen/canopen_402/canopen_402_plugin.xml new file mode 100755 index 0000000..e734224 --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/canopen_402/canopen_402_plugin.xml @@ -0,0 +1,5 @@ + + + Allocator for Motor402 instances + + \ No newline at end of file diff --git a/Devices/Libraries/Ros/ros_canopen/canopen_402/include/canopen_402/base.h b/Devices/Libraries/Ros/ros_canopen/canopen_402/include/canopen_402/base.h new file mode 100755 index 0000000..a704c76 --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/canopen_402/include/canopen_402/base.h @@ -0,0 +1,45 @@ +#ifndef CANOPEN_402_BASE_H +#define CANOPEN_402_BASE_H + +#include + +namespace canopen +{ + +class MotorBase : public canopen::Layer { +protected: + MotorBase(const std::string &name) : Layer(name) {} +public: + enum OperationMode + { + No_Mode = 0, + Profiled_Position = 1, + Velocity = 2, + Profiled_Velocity = 3, + Profiled_Torque = 4, + Reserved = 5, + Homing = 6, + Interpolated_Position = 7, + Cyclic_Synchronous_Position = 8, + Cyclic_Synchronous_Velocity = 9, + Cyclic_Synchronous_Torque = 10, + }; + virtual bool setTarget(double val) = 0; + virtual bool enterModeAndWait(uint16_t mode) = 0; + virtual bool isModeSupported(uint16_t mode) = 0; + virtual uint16_t getMode() = 0; + virtual void registerDefaultModes(ObjectStorageSharedPtr storage) {} + + typedef std::shared_ptr MotorBaseSharedPtr; + + class Allocator { + public: + virtual MotorBaseSharedPtr allocate(const std::string &name, ObjectStorageSharedPtr storage, const canopen::Settings &settings) = 0; + virtual ~Allocator() {} + }; +}; +typedef MotorBase::MotorBaseSharedPtr MotorBaseSharedPtr; + +} + +#endif diff --git a/Devices/Libraries/Ros/ros_canopen/canopen_402/include/canopen_402/motor.h b/Devices/Libraries/Ros/ros_canopen/canopen_402/include/canopen_402/motor.h new file mode 100755 index 0000000..6404a71 --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/canopen_402/include/canopen_402/motor.h @@ -0,0 +1,390 @@ +#ifndef CANOPEN_402_MOTOR_H +#define CANOPEN_402_MOTOR_H + +#include +#include +#include +#include + +#include +#include +#include + +namespace canopen +{ + +class State402{ +public: + enum StatusWord + { + SW_Ready_To_Switch_On=0, + SW_Switched_On=1, + SW_Operation_enabled=2, + SW_Fault=3, + SW_Voltage_enabled=4, + SW_Quick_stop=5, + SW_Switch_on_disabled=6, + SW_Warning=7, + SW_Manufacturer_specific0=8, + SW_Remote=9, + SW_Target_reached=10, + SW_Internal_limit=11, + SW_Operation_mode_specific0=12, + SW_Operation_mode_specific1=13, + SW_Manufacturer_specific1=14, + SW_Manufacturer_specific2=15 + }; + enum InternalState + { + Unknown = 0, + Start = 0, + Not_Ready_To_Switch_On = 1, + Switch_On_Disabled = 2, + Ready_To_Switch_On = 3, + Switched_On = 4, + Operation_Enable = 5, + Quick_Stop_Active = 6, + Fault_Reaction_Active = 7, + Fault = 8, + }; + InternalState getState(); + InternalState read(uint16_t sw); + bool waitForNewState(const time_point &abstime, InternalState &state); + State402() : state_(Unknown) {} +private: + boost::condition_variable cond_; + boost::mutex mutex_; + InternalState state_; +}; + +class Command402 { + struct Op { + uint16_t to_set_; + uint16_t to_reset_; + Op(uint16_t to_set ,uint16_t to_reset) : to_set_(to_set), to_reset_(to_reset) {} + void operator()(uint16_t &val) const { + val = (val & ~to_reset_) | to_set_; + } + }; + class TransitionTable { + boost::container::flat_map< std::pair, Op > transitions_; + void add(const State402::InternalState &from, const State402::InternalState &to, Op op){ + transitions_.insert(std::make_pair(std::make_pair(from, to), op)); + } + public: + TransitionTable(); + const Op& get(const State402::InternalState &from, const State402::InternalState &to) const { + return transitions_.at(std::make_pair(from, to)); + } + }; + static const TransitionTable transitions_; + static State402::InternalState nextStateForEnabling(State402::InternalState state); + Command402(); +public: + enum ControlWord + { + CW_Switch_On=0, + CW_Enable_Voltage=1, + CW_Quick_Stop=2, + CW_Enable_Operation=3, + CW_Operation_mode_specific0=4, + CW_Operation_mode_specific1=5, + CW_Operation_mode_specific2=6, + CW_Fault_Reset=7, + CW_Halt=8, + CW_Operation_mode_specific3=9, + // CW_Reserved1=10, + CW_Manufacturer_specific0=11, + CW_Manufacturer_specific1=12, + CW_Manufacturer_specific2=13, + CW_Manufacturer_specific3=14, + CW_Manufacturer_specific4=15, + }; + static bool setTransition(uint16_t &cw, const State402::InternalState &from, const State402::InternalState &to, State402::InternalState *next); +}; + +template class WordAccessor{ + uint16_t &word_; +public: + WordAccessor(uint16_t &word) : word_(word) {} + bool set(uint8_t bit){ + uint16_t val = MASK & (1< OpModeAccesser; + virtual bool start() = 0; + virtual bool read(const uint16_t &sw) = 0; + virtual bool write(OpModeAccesser& cw) = 0; + virtual bool setTarget(const double &val) { ROSCANOPEN_ERROR("canopen_402", "Mode::setTarget not implemented"); return false; } + virtual ~Mode() {} +}; +typedef std::shared_ptr ModeSharedPtr; + +template class ModeTargetHelper : public Mode { + T target_; + std::atomic has_target_; + +public: + ModeTargetHelper(uint16_t mode) : Mode (mode) {} + bool hasTarget() { return has_target_; } + T getTarget() { return target_; } + virtual bool setTarget(const double &val) { + if(std::isnan(val)){ + ROSCANOPEN_ERROR("canopen_402", "target command is not a number"); + return false; + } + + using boost::numeric_cast; + using boost::numeric::positive_overflow; + using boost::numeric::negative_overflow; + + try + { + target_= numeric_cast(val); + } + catch(negative_overflow&) { + ROSCANOPEN_WARN("canopen_402", "Command " << val << " does not fit into target, clamping to min limit"); + target_= std::numeric_limits::min(); + } + catch(positive_overflow&) { + ROSCANOPEN_WARN("canopen_402", "Command " << val << " does not fit into target, clamping to max limit"); + target_= std::numeric_limits::max(); + } + catch(...){ + ROSCANOPEN_ERROR("canopen_402", "Was not able to cast command " << val); + return false; + } + + has_target_ = true; + return true; + } + virtual bool start() { has_target_ = false; return true; } +}; + +template class ModeForwardHelper : public ModeTargetHelper { + canopen::ObjectStorage::Entry target_entry_; +public: + ModeForwardHelper(ObjectStorageSharedPtr storage) : ModeTargetHelper(ID) { + if(SUB) storage->entry(target_entry_, OBJ, SUB); + else storage->entry(target_entry_, OBJ); + } + virtual bool read(const uint16_t &sw) { return true;} + virtual bool write(Mode::OpModeAccesser& cw) { + if(this->hasTarget()){ + cw = cw.get() | CW_MASK; + target_entry_.set(this->getTarget()); + return true; + }else{ + cw = cw.get() & ~CW_MASK; + return false; + } + } +}; + +typedef ModeForwardHelper ProfiledVelocityMode; +typedef ModeForwardHelper ProfiledTorqueMode; +typedef ModeForwardHelper CyclicSynchronousPositionMode; +typedef ModeForwardHelper CyclicSynchronousVelocityMode; +typedef ModeForwardHelper CyclicSynchronousTorqueMode; +typedef ModeForwardHelper VelocityMode; +typedef ModeForwardHelper InterpolatedPositionMode; + +class ProfiledPositionMode : public ModeTargetHelper { + canopen::ObjectStorage::Entry target_position_; + double last_target_; + uint16_t sw_; +public: + enum SW_masks { + MASK_Reached = (1<entry(target_position_, 0x607A); + } + virtual bool start() { sw_ = 0; last_target_= std::numeric_limits::quiet_NaN(); return ModeTargetHelper::start(); } + virtual bool read(const uint16_t &sw) { sw_ = sw; return (sw & MASK_Error) == 0; } + virtual bool write(OpModeAccesser& cw) { + cw.set(CW_Immediate); + if(hasTarget()){ + int32_t target = getTarget(); + if((sw_ & MASK_Acknowledged) == 0 && target != last_target_){ + if(cw.get(CW_NewPoint)){ + cw.reset(CW_NewPoint); // reset if needed + }else{ + target_position_.set(target); + cw.set(CW_NewPoint); + last_target_ = target; + } + } else if (sw_ & MASK_Acknowledged){ + cw.reset(CW_NewPoint); + } + return true; + } + return false; + } +}; + +class HomingMode: public Mode{ +protected: + enum SW_bits { + SW_Attained = State402::SW_Operation_mode_specific0, + SW_Error = State402::SW_Operation_mode_specific1, + }; + enum CW_bits { + CW_StartHoming = Command402::CW_Operation_mode_specific0, + }; +public: + HomingMode() : Mode(MotorBase::Homing) {} + virtual bool executeHoming(canopen::LayerStatus &status) = 0; +}; + +class DefaultHomingMode: public HomingMode{ + canopen::ObjectStorage::Entry homing_method_; + std::atomic execute_; + + boost::mutex mutex_; + boost::condition_variable cond_; + uint16_t status_; + + enum SW_masks { + MASK_Reached = (1<entry(homing_method_, 0x6098); + } + virtual bool start(); + virtual bool read(const uint16_t &sw); + virtual bool write(OpModeAccesser& cw); + + virtual bool executeHoming(canopen::LayerStatus &status); +}; + +class Motor402 : public MotorBase +{ +public: + + Motor402(const std::string &name, ObjectStorageSharedPtr storage, const canopen::Settings &settings) + : MotorBase(name), status_word_(0),control_word_(0), + switching_state_(State402::InternalState(settings.get_optional("switching_state", static_cast(State402::Operation_Enable)))), + monitor_mode_(settings.get_optional("monitor_mode", true)), + state_switch_timeout_(settings.get_optional("state_switch_timeout", 5)) + { + storage->entry(status_word_entry_, 0x6041); + storage->entry(control_word_entry_, 0x6040); + storage->entry(op_mode_display_, 0x6061); + storage->entry(op_mode_, 0x6060); + try{ + storage->entry(supported_drive_modes_, 0x6502); + } + catch(...){ + } + } + + virtual bool setTarget(double val); + virtual bool enterModeAndWait(uint16_t mode); + virtual bool isModeSupported(uint16_t mode); + virtual uint16_t getMode(); + + template + bool registerMode(uint16_t mode, Args&&... args) { + return mode_allocators_.insert(std::make_pair(mode, [args..., mode, this](){ + if(isModeSupportedByDevice(mode)) registerMode(mode, ModeSharedPtr(new T(args...))); + })).second; + } + + virtual void registerDefaultModes(ObjectStorageSharedPtr storage){ + registerMode (MotorBase::Profiled_Position, storage); + registerMode (MotorBase::Velocity, storage); + registerMode (MotorBase::Profiled_Velocity, storage); + registerMode (MotorBase::Profiled_Torque, storage); + registerMode (MotorBase::Homing, storage); + registerMode (MotorBase::Interpolated_Position, storage); + registerMode (MotorBase::Cyclic_Synchronous_Position, storage); + registerMode (MotorBase::Cyclic_Synchronous_Velocity, storage); + registerMode (MotorBase::Cyclic_Synchronous_Torque, storage); + } + + class Allocator : public MotorBase::Allocator{ + public: + virtual MotorBaseSharedPtr allocate(const std::string &name, ObjectStorageSharedPtr storage, const canopen::Settings &settings); + }; +protected: + virtual void handleRead(LayerStatus &status, const LayerState ¤t_state); + virtual void handleWrite(LayerStatus &status, const LayerState ¤t_state); + virtual void handleDiag(LayerReport &report); + virtual void handleInit(LayerStatus &status); + virtual void handleShutdown(LayerStatus &status); + virtual void handleHalt(LayerStatus &status); + virtual void handleRecover(LayerStatus &status); + +private: + virtual bool isModeSupportedByDevice(uint16_t mode); + void registerMode(uint16_t id, const ModeSharedPtr &m); + + ModeSharedPtr allocMode(uint16_t mode); + + bool readState(LayerStatus &status, const LayerState ¤t_state); + bool switchMode(LayerStatus &status, uint16_t mode); + bool switchState(LayerStatus &status, const State402::InternalState &target); + + std::atomic status_word_; + uint16_t control_word_; + boost::mutex cw_mutex_; + std::atomic start_fault_reset_; + std::atomic target_state_; + + + State402 state_handler_; + + boost::mutex map_mutex_; + std::unordered_map modes_; + typedef std::function AllocFuncType; + std::unordered_map mode_allocators_; + + ModeSharedPtr selected_mode_; + uint16_t mode_id_; + boost::condition_variable mode_cond_; + boost::mutex mode_mutex_; + const State402::InternalState switching_state_; + const bool monitor_mode_; + const boost::chrono::seconds state_switch_timeout_; + + canopen::ObjectStorage::Entry status_word_entry_; + canopen::ObjectStorage::Entry control_word_entry_; + canopen::ObjectStorage::Entry op_mode_display_; + canopen::ObjectStorage::Entry op_mode_; + canopen::ObjectStorage::Entry supported_drive_modes_; +}; + +} + +#endif diff --git a/Devices/Libraries/Ros/ros_canopen/canopen_402/package.xml b/Devices/Libraries/Ros/ros_canopen/canopen_402/package.xml new file mode 100755 index 0000000..0398cc3 --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/canopen_402/package.xml @@ -0,0 +1,28 @@ + + + canopen_402 + 0.8.5 + This implements the CANopen device profile for drives and motion control. CiA(r) 402 + + Mathias Lüdtke + Thiago de Freitas + Mathias Lüdtke + + LGPLv3 + + http://wiki.ros.org/canopen_402 + https://github.com/ros-industrial/ros_canopen + https://github.com/ros-industrial/ros_canopen/issues + + catkin + + libboost-dev + canopen_master + class_loader + + rosunit + + + + + diff --git a/Devices/Libraries/Ros/ros_canopen/canopen_402/src/motor.cpp b/Devices/Libraries/Ros/ros_canopen/canopen_402/src/motor.cpp new file mode 100755 index 0000000..b6cc29d --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/canopen_402/src/motor.cpp @@ -0,0 +1,549 @@ +#include +#include + +namespace canopen +{ + +State402::InternalState State402::getState(){ + boost::mutex::scoped_lock lock(mutex_); + return state_; +} + +State402::InternalState State402::read(uint16_t sw) { + static const uint16_t r = (1 << SW_Ready_To_Switch_On); + static const uint16_t s = (1 << SW_Switched_On); + static const uint16_t o = (1 << SW_Operation_enabled); + static const uint16_t f = (1 << SW_Fault); + static const uint16_t q = (1 << SW_Quick_stop); + static const uint16_t d = (1 << SW_Switch_on_disabled); + + InternalState new_state = Unknown; + + uint16_t state = sw & ( d | q | f | o | s | r ); + switch ( state ) + { + // ( d | q | f | o | s | r ): + case ( 0 | 0 | 0 | 0 | 0 | 0 ): + case ( 0 | q | 0 | 0 | 0 | 0 ): + new_state = Not_Ready_To_Switch_On; + break; + + case ( d | 0 | 0 | 0 | 0 | 0 ): + case ( d | q | 0 | 0 | 0 | 0 ): + new_state = Switch_On_Disabled; + break; + + case ( 0 | q | 0 | 0 | 0 | r ): + new_state = Ready_To_Switch_On; + break; + + case ( 0 | q | 0 | 0 | s | r ): + new_state = Switched_On; + break; + + case ( 0 | q | 0 | o | s | r ): + new_state = Operation_Enable; + break; + + case ( 0 | 0 | 0 | o | s | r ): + new_state = Quick_Stop_Active; + break; + + case ( 0 | 0 | f | o | s | r ): + case ( 0 | q | f | o | s | r ): + new_state = Fault_Reaction_Active; + break; + + case ( 0 | 0 | f | 0 | 0 | 0 ): + case ( 0 | q | f | 0 | 0 | 0 ): + new_state = Fault; + break; + + default: + ROSCANOPEN_WARN("canopen_402", "Motor is currently in an unknown state: " << std::hex << state << std::dec); + } + boost::mutex::scoped_lock lock(mutex_); + if(new_state != state_){ + state_ = new_state; + cond_.notify_all(); + } + return state_; +} +bool State402::waitForNewState(const time_point &abstime, State402::InternalState &state){ + boost::mutex::scoped_lock lock(mutex_); + while(state_ == state && cond_.wait_until(lock, abstime) == boost::cv_status::no_timeout) {} + bool res = state != state_; + state = state_; + return res; +} + +const Command402::TransitionTable Command402::transitions_; + +Command402::TransitionTable::TransitionTable(){ + typedef State402 s; + + transitions_.reserve(32); + + Op disable_voltage(0,(1< " << to); + } + return false; +} + +template struct masked_status_not_equal { + uint16_t &status_; + masked_status_not_equal(uint16_t &status) : status_(status) {} + bool operator()() const { return (status_ & mask) != not_equal; } +}; +bool DefaultHomingMode::start() { + execute_ = false; + return read(0); +} +bool DefaultHomingMode::read(const uint16_t &sw) { + boost::mutex::scoped_lock lock(mutex_); + uint16_t old = status_; + status_ = sw & (MASK_Reached | MASK_Attained | MASK_Error); + if(old != status_){ + cond_.notify_all(); + } + return true; +} +bool DefaultHomingMode::write(Mode::OpModeAccesser& cw) { + cw = 0; + if(execute_){ + cw.set(CW_StartHoming); + return true; + } + return false; +} + +bool DefaultHomingMode::executeHoming(canopen::LayerStatus &status) { + if(!homing_method_.valid()){ + return error(status, "homing method entry is not valid"); + } + + if(homing_method_.get_cached() == 0){ + return true; + } + + time_point prepare_time = get_abs_time(boost::chrono::seconds(1)); + // ensure homing is not running + boost::mutex::scoped_lock lock(mutex_); + if(!cond_.wait_until(lock, prepare_time, masked_status_not_equal (status_))){ + return error(status, "could not prepare homing"); + } + if(status_ & MASK_Error){ + return error(status, "homing error before start"); + } + + execute_ = true; + + // ensure start + if(!cond_.wait_until(lock, prepare_time, masked_status_not_equal (status_))){ + return error(status, "homing did not start"); + } + if(status_ & MASK_Error){ + return error(status, "homing error at start"); + } + + time_point finish_time = get_abs_time(boost::chrono::seconds(10)); // + + // wait for attained + if(!cond_.wait_until(lock, finish_time, masked_status_not_equal (status_))){ + return error(status, "homing not attained"); + } + if(status_ & MASK_Error){ + return error(status, "homing error during process"); + } + + // wait for motion stop + if(!cond_.wait_until(lock, finish_time, masked_status_not_equal (status_))){ + return error(status, "homing did not stop"); + } + if(status_ & MASK_Error){ + return error(status, "homing error during stop"); + } + + if((status_ & MASK_Reached) && (status_ & MASK_Attained)){ + execute_ = false; + return true; + } + + return error(status, "something went wrong while homing"); +} + +bool Motor402::setTarget(double val){ + if(state_handler_.getState() == State402::Operation_Enable){ + boost::mutex::scoped_lock lock(mode_mutex_); + return selected_mode_ && selected_mode_->setTarget(val); + } + return false; +} +bool Motor402::isModeSupported(uint16_t mode) { return mode != MotorBase::Homing && allocMode(mode); } + +bool Motor402::enterModeAndWait(uint16_t mode) { + LayerStatus s; + bool okay = mode != MotorBase::Homing && switchMode(s, mode); + if(!s.bounded()){ + ROSCANOPEN_ERROR("canopen_402", "Could not switch to mode " << mode << ", reason: " << s.reason()); + } + return okay; +} + +uint16_t Motor402::getMode() { + boost::mutex::scoped_lock lock(mode_mutex_); + return selected_mode_ ? selected_mode_->mode_id_ : MotorBase::No_Mode; +} + +bool Motor402::isModeSupportedByDevice(uint16_t mode){ + if(!supported_drive_modes_.valid()) { + BOOST_THROW_EXCEPTION( std::runtime_error("Supported drive modes (object 6502) is not valid")); + } + return mode > 0 && mode <= 32 && (supported_drive_modes_.get_cached() & (1<<(mode-1))); +} +void Motor402::registerMode(uint16_t id, const ModeSharedPtr &m){ + boost::mutex::scoped_lock map_lock(map_mutex_); + if(m && m->mode_id_ == id) modes_.insert(std::make_pair(id, m)); +} + +ModeSharedPtr Motor402::allocMode(uint16_t mode){ + ModeSharedPtr res; + if(isModeSupportedByDevice(mode)){ + boost::mutex::scoped_lock map_lock(map_mutex_); + std::unordered_map::iterator it = modes_.find(mode); + if(it != modes_.end()){ + res = it->second; + } + } + return res; +} + +bool Motor402::switchMode(LayerStatus &status, uint16_t mode) { + + if(mode == MotorBase::No_Mode){ + boost::mutex::scoped_lock lock(mode_mutex_); + selected_mode_.reset(); + try{ // try to set mode + op_mode_.set(mode); + }catch(...){} + return true; + } + + ModeSharedPtr next_mode = allocMode(mode); + if(!next_mode){ + status.error("Mode is not supported."); + return false; + } + + if(!next_mode->start()){ + status.error("Could not start mode."); + return false; + } + + { // disable mode handler + boost::mutex::scoped_lock lock(mode_mutex_); + + if(mode_id_ == mode && selected_mode_ && selected_mode_->mode_id_ == mode){ + // nothing to do + return true; + } + + selected_mode_.reset(); + } + + if(!switchState(status, switching_state_)) return false; + + op_mode_.set(mode); + + bool okay = false; + + { // wait for switch + boost::mutex::scoped_lock lock(mode_mutex_); + + time_point abstime = get_abs_time(boost::chrono::seconds(5)); + if(monitor_mode_){ + while(mode_id_ != mode && mode_cond_.wait_until(lock, abstime) == boost::cv_status::no_timeout) {} + }else{ + while(mode_id_ != mode && get_abs_time() < abstime){ + boost::reverse_lock reverse(lock); // unlock inside loop + op_mode_display_.get(); // poll + boost::this_thread::sleep_for(boost::chrono::milliseconds(20)); // wait some time + } + } + + if(mode_id_ == mode){ + selected_mode_ = next_mode; + okay = true; + }else{ + status.error("Mode switch timed out."); + op_mode_.set(mode_id_); + } + } + + if(!switchState(status, State402::Operation_Enable)) return false; + + return okay; + +} + +bool Motor402::switchState(LayerStatus &status, const State402::InternalState &target){ + time_point abstime = get_abs_time(state_switch_timeout_); + State402::InternalState state = state_handler_.getState(); + target_state_ = target; + while(state != target_state_){ + boost::mutex::scoped_lock lock(cw_mutex_); + State402::InternalState next = State402::Unknown; + if(!Command402::setTransition(control_word_ ,state, target_state_ , &next)){ + status.error("Could not set transition"); + return false; + } + lock.unlock(); + if(state != next && !state_handler_.waitForNewState(abstime, state)){ + status.error("Transition timeout"); + return false; + } + } + return state == target; +} + +bool Motor402::readState(LayerStatus &status, const LayerState ¤t_state){ + uint16_t old_sw, sw = status_word_entry_.get(); // TODO: added error handling + old_sw = status_word_.exchange(sw); + + state_handler_.read(sw); + + boost::mutex::scoped_lock lock(mode_mutex_); + uint16_t new_mode = monitor_mode_ ? op_mode_display_.get() : op_mode_display_.get_cached(); + if(selected_mode_ && selected_mode_->mode_id_ == new_mode){ + if(!selected_mode_->read(sw)){ + status.error("Mode handler has error"); + } + } + if(new_mode != mode_id_){ + mode_id_ = new_mode; + mode_cond_.notify_all(); + } + if(selected_mode_ && selected_mode_->mode_id_ != new_mode){ + status.warn("mode does not match"); + } + if(sw & (1< Off){ + readState(status, current_state); + } +} +void Motor402::handleWrite(LayerStatus &status, const LayerState ¤t_state){ + if(current_state > Off){ + boost::mutex::scoped_lock lock(cw_mutex_); + control_word_ |= (1<mode_id_ == mode_id_){ + okay = selected_mode_->write(cwa); + } else { + cwa = 0; + } + if(okay) { + control_word_ &= ~(1<::iterator it = mode_allocators_.begin(); it != mode_allocators_.end(); ++it){ + (it->second)(); + } + + if(!readState(status, Init)){ + status.error("Could not read motor state"); + return; + } + { + boost::mutex::scoped_lock lock(cw_mutex_); + control_word_ = 0; + start_fault_reset_ = true; + } + if(!switchState(status, State402::Operation_Enable)){ + status.error("Could not enable motor"); + return; + } + + ModeSharedPtr m = allocMode(MotorBase::Homing); + if(!m){ + return; // homing not supported + } + + HomingMode *homing = dynamic_cast(m.get()); + + if(!homing){ + status.error("Homing mode has incorrect handler"); + return; + } + + if(!switchMode(status, MotorBase::Homing)){ + status.error("Could not enter homing mode"); + return; + } + + if(!homing->executeHoming(status)){ + status.error("Homing failed"); + return; + } + + switchMode(status, No_Mode); +} +void Motor402::handleShutdown(LayerStatus &status){ + switchMode(status, MotorBase::No_Mode); + switchState(status, State402::Switch_On_Disabled); +} +void Motor402::handleHalt(LayerStatus &status){ + State402::InternalState state = state_handler_.getState(); + boost::mutex::scoped_lock lock(cw_mutex_); + + // do not demand quickstop in case of fault + if(state == State402::Fault_Reaction_Active || state == State402::Fault) return; + + if(state != State402::Operation_Enable){ + target_state_ = state; + }else{ + target_state_ = State402::Quick_Stop_Active; + if(!Command402::setTransition(control_word_ ,state, State402::Quick_Stop_Active, 0)){ + status.warn("Could not quick stop"); + } + } +} +void Motor402::handleRecover(LayerStatus &status){ + start_fault_reset_ = true; + { + boost::mutex::scoped_lock lock(mode_mutex_); + if(selected_mode_ && !selected_mode_->start()){ + status.error("Could not restart mode."); + return; + } + } + if(!switchState(status, State402::Operation_Enable)){ + status.error("Could not enable motor"); + return; + } +} + +} // namespace diff --git a/Devices/Libraries/Ros/ros_canopen/canopen_402/src/plugin.cpp b/Devices/Libraries/Ros/ros_canopen/canopen_402/src/plugin.cpp new file mode 100755 index 0000000..ac63a96 --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/canopen_402/src/plugin.cpp @@ -0,0 +1,8 @@ +#include +#include + +canopen::MotorBaseSharedPtr canopen::Motor402::Allocator::allocate(const std::string &name, canopen::ObjectStorageSharedPtr storage, const canopen::Settings &settings) { + return std::make_shared(name, storage, settings); +} + +CLASS_LOADER_REGISTER_CLASS(canopen::Motor402::Allocator, canopen::MotorBase::Allocator); diff --git a/Devices/Libraries/Ros/ros_canopen/canopen_402/test/clamping.cpp b/Devices/Libraries/Ros/ros_canopen/canopen_402/test/clamping.cpp new file mode 100755 index 0000000..9630c32 --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/canopen_402/test/clamping.cpp @@ -0,0 +1,57 @@ +#include +#include + + +template class ModeTargetHelperTest : public canopen::ModeTargetHelper, public ::testing::Test{ +public: + ModeTargetHelperTest() : canopen::ModeTargetHelper(0) {} + virtual bool read(const uint16_t &sw) { return false; } + virtual bool write(canopen::Mode::OpModeAccesser& cw) { return false; } +}; + +typedef ::testing::Types MyTypes; + +TYPED_TEST_CASE(ModeTargetHelperTest, MyTypes); + +TYPED_TEST(ModeTargetHelperTest, CheckNaN){ + ASSERT_FALSE(this->setTarget(std::numeric_limits::quiet_NaN())); +} + +TYPED_TEST(ModeTargetHelperTest, CheckZero){ + ASSERT_TRUE(this->setTarget(0.0)); +} + +TYPED_TEST(ModeTargetHelperTest, CheckOne){ + ASSERT_TRUE(this->setTarget(1.0)); +} + +TYPED_TEST(ModeTargetHelperTest, CheckMax){ + double max = static_cast(std::numeric_limits::max()); + + ASSERT_TRUE(this->setTarget(max)); + ASSERT_EQ(max, this->getTarget()); + + ASSERT_TRUE(this->setTarget(max-1)); + ASSERT_EQ(max-1,this->getTarget()); + + ASSERT_TRUE(this->setTarget(max+1)); + ASSERT_EQ(max, this->getTarget()); +} + +TYPED_TEST(ModeTargetHelperTest, CheckMin){ + double min = static_cast(std::numeric_limits::min()); + + ASSERT_TRUE(this->setTarget(min)); + ASSERT_EQ(min, this->getTarget()); + + ASSERT_TRUE(this->setTarget(min-1)); + ASSERT_EQ(min, this->getTarget()); + + ASSERT_TRUE(this->setTarget(min+1)); + ASSERT_EQ(min+1,this->getTarget()); +} + +int main(int argc, char **argv){ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/Devices/Libraries/Ros/ros_canopen/canopen_chain_node/CHANGELOG.rst b/Devices/Libraries/Ros/ros_canopen/canopen_chain_node/CHANGELOG.rst new file mode 100755 index 0000000..bf72805 --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/canopen_chain_node/CHANGELOG.rst @@ -0,0 +1,199 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package canopen_chain_node +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.8.5 (2020-09-22) +------------------ + +0.8.4 (2020-08-22) +------------------ +* pass settings from ROS node to SocketCANInterface +* Contributors: Mathias Lüdtke + +0.8.3 (2020-05-07) +------------------ +* Bump CMake version to avoid CMP0048 warning + Signed-off-by: ahcorde +* Contributors: ahcorde + +0.8.2 (2019-11-04) +------------------ +* rename to logWarning to fix build on Debian stretch +* log the result of all services in RosChain +* enable rosconsole_bridge bindings +* switch to new logging macros +* Contributors: Mathias Lüdtke + +0.8.1 (2019-07-14) +------------------ +* Set C++ standard to c++14 +* implemented create\*ListenerM helpers +* Replacing FastDelegate with std::function and std::bind. +* Contributors: Alexander Gutenkunst, Harsh Deshpande, Joshua Whitley, Mathias Lüdtke + +0.8.0 (2018-07-11) +------------------ +* migrated to std::function and std::bind +* make sync_node return proper error codes +* refactored PublishFunc +* migrated to std::atomic +* migrated to std pointers +* removed deprecated types +* Contributors: Mathias Lüdtke + +0.7.8 (2018-05-04) +------------------ +* Revert "pull make_shared into namespaces" + This reverts commit 9b2cd05df76d223647ca81917d289ca6330cdee6. +* Contributors: Mathias Lüdtke + +0.7.7 (2018-05-04) +------------------ +* added types for all function objects +* pull make_shared into namespaces +* added types for all shared_ptrs +* migrate to new classloader headers +* address catkin_lint errors/warnings +* removed IPC/SHM based sync masters +* Contributors: Mathias Lüdtke + +0.7.6 (2017-08-30) +------------------ + +0.7.5 (2017-05-29) +------------------ +* added reset_errors_before_recover option +* Contributors: Mathias Lüdtke + +0.7.4 (2017-04-25) +------------------ + +0.7.3 (2017-04-25) +------------------ + +0.7.2 (2017-03-28) +------------------ + +0.7.1 (2017-03-20) +------------------ +* refactored EMCY handling into separate layer +* do not reset thread for recover +* properly stop run thread if init failed +* deprecation warning for SHM-based master implementations +* implemented canopen_sync_node +* wait only if sync is disabled +* added object access services +* implement level-based object logging +* added node name lookup +* Contributors: Mathias Lüdtke + +0.7.0 (2016-12-13) +------------------ + +0.6.5 (2016-12-10) +------------------ +* protect MotorChain setup with RosChain lock +* added include to ; solving `#177 `_ +* default to canopen::SimpleMaster::Allocator (`#71 `_) +* exit code for generic error should be 1, not -1 +* styled and sorted CMakeLists.txt + * removed boilerplate comments + * indention + * reviewed exported dependencies +* styled and sorted package.xml +* update package URLs +* moved roslib include into source file +* renamed chain_ros.h to ros_chain.h, fixes `#126 `_ +* Use of catkin_EXPORTED_TARGETS + Install target for canopen_ros_chain +* Splitted charn_ros.h into chain_ros.h and ros_chain.cpp +* Revert "stop heartbeat after stack was shutdown" + This reverts commit de985b5e9664edbbcc4f743fff3e2a2391e1bf8f. +* improve failure handling in init service callback +* improved excetion handling in init and recover callback +* Merge pull request `#109 `_ from ipa-mdl/shutdown-crashes + Fix for pluginlib-related crashes on shutdown +* catch std::exception instead of canopen::Exception (`#110 `_) +* call to detroy is not needed anymore +* added GuardedClassLoader implementation +* minor shutdown improvements +* Contributors: Mathias Lüdtke, Michael Stoll, xaedes + +0.6.4 (2015-07-03) +------------------ + +0.6.3 (2015-06-30) +------------------ +* added motor_layer settings +* remove boost::posix_time::milliseconds from SyncProperties +* removed support for silence_us since bus timing cannot be guaranteed +* implemented plugin-based Master allocators, defaults to LocalMaster +* set initialized to false explicitly if init failed +* include for std_msgs::String was missing +* Merge remote-tracking branch 'origin/std_trigger' into new_402 + Conflicts: + canopen_chain_node/CMakeLists.txt + canopen_chain_node/include/canopen_chain_node/chain_ros.h +* halt explicitly on shutdown +* stop heartbeat after stack was shutdown +* migrated to Timer instead of ros::Timer to send heartbeat even after ros was shutdown +* run loop even if ros is shutdown +* improved chain shutdown behaviour +* fix for g++: proper message generation +* Merge branch 'publisher' into muparser + Conflicts: + canopen_motor_node/src/control_node.cpp +* added generic object publishers +* migrated to std_srvs/Trigger +* use atomic flag instead of thread pointer for synchronization +* do not run diagnostics if chain was not initalized, output warning instead +* Changes Layer Status to Warning during the service calls +* refactored Layer mechanisms +* heartbeat works now +* check XmlRpcValue types in dcf_overlay +* removed IPCLayer sync listener, loopback is disabled per default +* added simple heartbeat timer +* added sync silence feature +* parse sync properties only if sync_ms is valid +* require message strings for error indicators, added missing strings, added ROS logging in sync loop +* skip "eds_pkg" if not provided +* clear layer before plugin loader is deleted +* implemented node list as struct +* 'modules' was renamed to 'nodes' +* removed chain name +* added driver_plugin parameter for pluginlib look-up +* implemented threading in CANLayer +* removed bitrate, added loopback to DriverInterface::init +* allow dcf_overlay in defaults as well +* recursive merge of MergedXmlRpcStruct +* added dcf_overlay parameter +* Merge branch 'auto_scale' into indigo_dev + Conflicts: + canopen_chain_node/include/canopen_chain_node/chain_ros.h +* Merge remote-tracking branch 'ipa320/indigo_dev' into indigo_dev + Conflicts: + canopen_chain_node/include/canopen_chain_node/chain_ros.h + canopen_motor_node/src/control_node.cpp +* catch exceptions during master creation +* removed MasterType form template +* added master_type parameter +* Merge branch 'indigo_dev' into merge + Conflicts: + canopen_chain_node/include/canopen_chain_node/chain_ros.h + canopen_master/include/canopen_master/canopen.h + canopen_master/include/canopen_master/layer.h + canopen_master/src/node.cpp + canopen_motor_node/CMakeLists.txt + canopen_motor_node/src/control_node.cpp +* added MergedXmlRpcStruct as replacement for read_xmlrpc_or_praram +* Contributors: Mathias Lüdtke, thiagodefreitas + +0.6.2 (2014-12-18) +------------------ + +0.6.1 (2014-12-15) +------------------ +* remove ipa_* and IPA_* prefixes +* added descriptions and authors +* renamed ipa_canopen_chain_ros to canopen_chain_node +* Contributors: Florian Weisshardt, Mathias Lüdtke diff --git a/Devices/Libraries/Ros/ros_canopen/canopen_chain_node/CMakeLists.txt b/Devices/Libraries/Ros/ros_canopen/canopen_chain_node/CMakeLists.txt new file mode 100755 index 0000000..45c6f9d --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/canopen_chain_node/CMakeLists.txt @@ -0,0 +1,114 @@ +cmake_minimum_required(VERSION 3.0.2) +project(canopen_chain_node) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_STANDARD 14) +endif() + +find_package(catkin REQUIRED + COMPONENTS + canopen_master + diagnostic_updater + message_generation + pluginlib + rosconsole_bridge + roscpp + roslib + socketcan_interface + std_msgs + std_srvs +) + +find_package(Boost REQUIRED + COMPONENTS + filesystem +) + +add_service_files(DIRECTORY srv + FILES + GetObject.srv + SetObject.srv) + +generate_messages(DEPENDENCIES) + +catkin_package( + INCLUDE_DIRS + include + LIBRARIES + canopen_ros_chain + CATKIN_DEPENDS + canopen_master + diagnostic_updater + message_runtime + pluginlib + rosconsole_bridge + roscpp + socketcan_interface + std_srvs + DEPENDS + Boost +) + +include_directories( + include + ${Boost_INCLUDE_DIRS} + ${catkin_INCLUDE_DIRS} +) + +# canopen_ros_chain +add_library(canopen_ros_chain + src/ros_chain.cpp + src/rosconsole_bridge.cpp +) +target_link_libraries(canopen_ros_chain + ${Boost_LIBRARIES} + ${catkin_LIBRARIES} +) +add_dependencies(canopen_ros_chain + ${catkin_EXPORTED_TARGETS} + ${${PROJECT_NAME}_EXPORTED_TARGETS} +) + +# canopen_chain_node +add_executable(${PROJECT_NAME} + src/chain_node.cpp +) +target_link_libraries(${PROJECT_NAME} + canopen_ros_chain + ${Boost_LIBRARIES} + ${catkin_LIBRARIES} +) + +# canopen_sync_node +add_executable(canopen_sync_node + src/rosconsole_bridge.cpp + src/sync_node.cpp +) +target_link_libraries(canopen_sync_node + ${Boost_LIBRARIES} + ${catkin_LIBRARIES} +) + +install( + TARGETS + ${PROJECT_NAME} + canopen_ros_chain + canopen_sync_node + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install( + DIRECTORY + include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" +) + +install( + DIRECTORY + launch + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) diff --git a/Devices/Libraries/Ros/ros_canopen/canopen_chain_node/include/canopen_chain_node/ros_chain.h b/Devices/Libraries/Ros/ros_canopen/canopen_chain_node/include/canopen_chain_node/ros_chain.h new file mode 100755 index 0000000..e66444b --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/canopen_chain_node/include/canopen_chain_node/ros_chain.h @@ -0,0 +1,215 @@ +#ifndef H_CANOPEN_ROS_CHAIN +#define H_CANOPEN_ROS_CHAIN + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace canopen{ + +typedef std::function PublishFuncType; +PublishFuncType createPublishFunc(ros::NodeHandle &nh, const std::string &name, canopen::NodeSharedPtr node, const std::string &key, bool force); + +class MergedXmlRpcStruct : public XmlRpc::XmlRpcValue{ + MergedXmlRpcStruct(const XmlRpc::XmlRpcValue& a) :XmlRpc::XmlRpcValue(a){ assertStruct(); } +public: + MergedXmlRpcStruct(){ assertStruct(); } + MergedXmlRpcStruct(const XmlRpc::XmlRpcValue& a, const MergedXmlRpcStruct &b, bool recursive= true) :XmlRpc::XmlRpcValue(a){ + assertStruct(); + + for(ValueStruct::const_iterator it = b._value.asStruct->begin(); it != b._value.asStruct->end(); ++it){ + std::pair res = _value.asStruct->insert(*it); + + if(recursive && !res.second && res.first->second.getType() == XmlRpc::XmlRpcValue::TypeStruct && it->second.getType() == XmlRpc::XmlRpcValue::TypeStruct){ + res.first->second = MergedXmlRpcStruct(res.first->second, it->second); // recursive struct merge with implicit cast + } + } + + + } +}; + +class Logger: public DiagGroup{ + const canopen::NodeSharedPtr node_; + + std::vector > entries_; + + static void log_entry(diagnostic_updater::DiagnosticStatusWrapper &stat, uint8_t level, const std::string &name, std::function getter){ + if(stat.level >= level){ + try{ + stat.add(name, getter()); + }catch(...){ + stat.add(name, ""); + } + } + } + +public: + Logger(canopen::NodeSharedPtr node): node_(node) { add(node_); } + + bool add(uint8_t level, const std::string &key, bool forced){ + try{ + ObjectDict::Key k(key); + const ObjectDict::EntryConstSharedPtr entry = node_->getStorage()->dict_->get(k); + std::string name = entry->desc.empty() ? key : entry->desc; + entries_.push_back(std::bind(log_entry, std::placeholders::_1, level, name, node_->getStorage()->getStringReader(k, !forced))); + return true; + } + catch(std::exception& e){ + ROS_ERROR_STREAM(boost::diagnostic_information(e)); + return false; + } + } + + template void add(const std::shared_ptr &n){ + DiagGroup::add(std::static_pointer_cast(n)); + } + + virtual void log(diagnostic_updater::DiagnosticStatusWrapper &stat){ + if(node_->getState() == canopen::Node::Unknown){ + stat.summary(stat.WARN,"Not initialized"); + }else{ + LayerReport r; + diag(r); + if(r.bounded()){ // valid + stat.summary(r.get(), r.reason()); + for(std::vector >::const_iterator it = r.values().begin(); it != r.values().end(); ++it){ + stat.add(it->first, it->second); + } + for(size_t i=0; i < entries_.size(); ++i) entries_[i](stat); + } + } + } + virtual ~Logger() {} +}; +typedef std::shared_ptr LoggerSharedPtr; + +class GuardedClassLoaderList { +public: + typedef std::shared_ptr ClassLoaderBaseSharedPtr; + static void addLoader(ClassLoaderBaseSharedPtr b){ + guarded_loaders().push_back(b); + } + ~GuardedClassLoaderList(){ + guarded_loaders().clear(); + } +private: + static std::vector< ClassLoaderBaseSharedPtr>& guarded_loaders(){ + static std::vector loaders; + return loaders; + } +}; + +template class GuardedClassLoader { + typedef pluginlib::ClassLoader Loader; + std::shared_ptr loader_; +public: + typedef std::shared_ptr ClassSharedPtr; + GuardedClassLoader(const std::string& package, const std::string& allocator_base_class) + : loader_(new Loader(package, allocator_base_class)) { + GuardedClassLoaderList::addLoader(loader_); + } + ClassSharedPtr createInstance(const std::string& lookup_name){ + return loader_->createUniqueInstance(lookup_name); + } +}; + +template class ClassAllocator : public GuardedClassLoader { +public: + typedef std::shared_ptr ClassSharedPtr; + ClassAllocator (const std::string& package, const std::string& allocator_base_class) : GuardedClassLoader(package, allocator_base_class) {} + template ClassSharedPtr allocateInstance(const std::string& lookup_name, const T1 & t1){ + return this->createInstance(lookup_name)->allocate(t1); + } + template ClassSharedPtr allocateInstance(const std::string& lookup_name, const T1 & t1, const T2 & t2){ + return this->createInstance(lookup_name)->allocate(t1, t2); + } + template ClassSharedPtr allocateInstance(const std::string& lookup_name, const T1 & t1, const T2 & t2, const T3 & t3){ + return this->createInstance(lookup_name)->allocate(t1, t2, t3); + } +}; +class RosChain : GuardedClassLoaderList, public canopen::LayerStack { +private: + GuardedClassLoader driver_loader_; + ClassAllocator master_allocator_; + bool setup_node(const XmlRpc::XmlRpcValue ¶ms, const std::string& name, const MergedXmlRpcStruct &defaults); +protected: + can::DriverInterfaceSharedPtr interface_; + MasterSharedPtr master_; + std::shared_ptr > nodes_; + std::shared_ptr > emcy_handlers_; + std::map nodes_lookup_; + canopen::SyncLayerSharedPtr sync_; + std::vector loggers_; + std::vector publishers_; + + can::StateListenerConstSharedPtr state_listener_; + + std::unique_ptr thread_; + + ros::NodeHandle nh_; + ros::NodeHandle nh_priv_; + + diagnostic_updater::Updater diag_updater_; + ros::Timer diag_timer_; + + boost::mutex mutex_; + ros::ServiceServer srv_init_; + ros::ServiceServer srv_recover_; + ros::ServiceServer srv_halt_; + ros::ServiceServer srv_shutdown_; + ros::ServiceServer srv_get_object_; + ros::ServiceServer srv_set_object_; + + time_duration update_duration_; + + struct HeartbeatSender{ + can::Frame frame; + can::DriverInterfaceSharedPtr interface; + bool send(){ + return interface && interface->send(frame); + } + } hb_sender_; + Timer heartbeat_timer_; + + std::atomic running_; + boost::mutex diag_mutex_; + + bool reset_errors_before_recover_; + + void logState(const can::State &s); + void run(); + virtual bool handle_init(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res); + virtual bool handle_recover(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res); + virtual void handleWrite(LayerStatus &status, const LayerState ¤t_state); + virtual void handleShutdown(LayerStatus &status); + virtual bool handle_shutdown(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res); + virtual bool handle_halt(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res); + + bool handle_get_object(canopen_chain_node::GetObject::Request &req, canopen_chain_node::GetObject::Response &res); + bool handle_set_object(canopen_chain_node::SetObject::Request &req, canopen_chain_node::SetObject::Response &res); + + bool setup_bus(); + bool setup_sync(); + bool setup_heartbeat(); + bool setup_nodes(); + virtual bool nodeAdded(XmlRpc::XmlRpcValue ¶ms, const canopen::NodeSharedPtr &node, const LoggerSharedPtr &logger); + void report_diagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat); + virtual bool setup_chain(); +public: + RosChain(const ros::NodeHandle &nh, const ros::NodeHandle &nh_priv); + bool setup(); + virtual ~RosChain(); +}; + +} //namespace canopen + +#endif diff --git a/Devices/Libraries/Ros/ros_canopen/canopen_chain_node/launch/chain.launch b/Devices/Libraries/Ros/ros_canopen/canopen_chain_node/launch/chain.launch new file mode 100755 index 0000000..78f8ced --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/canopen_chain_node/launch/chain.launch @@ -0,0 +1,6 @@ + + + + + + diff --git a/Devices/Libraries/Ros/ros_canopen/canopen_chain_node/package.xml b/Devices/Libraries/Ros/ros_canopen/canopen_chain_node/package.xml new file mode 100755 index 0000000..9eddcfc --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/canopen_chain_node/package.xml @@ -0,0 +1,36 @@ + + + canopen_chain_node + 0.8.5 + Base implementation for CANopen chains node with support for management services and diagnostics + + Mathias Lüdtke + Mathias Lüdtke + + LGPLv3 + + http://wiki.ros.org/canopen_chain_node + https://github.com/ros-industrial/ros_canopen + https://github.com/ros-industrial/ros_canopen/issues + + catkin + + message_generation + message_runtime + message_runtime + + libboost-dev + libboost-filesystem-dev + canopen_master + diagnostic_updater + pluginlib + rosconsole_bridge + roscpp + roslib + socketcan_interface + std_srvs + + std_msgs + std_msgs + + diff --git a/Devices/Libraries/Ros/ros_canopen/canopen_chain_node/src/chain_node.cpp b/Devices/Libraries/Ros/ros_canopen/canopen_chain_node/src/chain_node.cpp new file mode 100755 index 0000000..181633e --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/canopen_chain_node/src/chain_node.cpp @@ -0,0 +1,21 @@ +#include +#include +#include + +using namespace can; +using namespace canopen; + +int main(int argc, char** argv){ + ros::init(argc, argv, "canopen_chain_node_node"); + ros::NodeHandle nh; + ros::NodeHandle nh_priv("~"); + + RosChain chain(nh, nh_priv); + + if(!chain.setup()){ + return 1; + } + + ros::spin(); + return 0; +} diff --git a/Devices/Libraries/Ros/ros_canopen/canopen_chain_node/src/ros_chain.cpp b/Devices/Libraries/Ros/ros_canopen/canopen_chain_node/src/ros_chain.cpp new file mode 100755 index 0000000..5a4b990 --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/canopen_chain_node/src/ros_chain.cpp @@ -0,0 +1,644 @@ +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace canopen { + +template +static PublishFuncType create(ros::NodeHandle &nh, const std::string &name, ObjectStorageSharedPtr storage, const std::string &key, const bool force){ + using data_type = typename ObjectStorage::DataType
::type; + using entry_type = ObjectStorage::Entry; + + entry_type entry = storage->entry(key); + if(!entry.valid()) return 0; + + const ros::Publisher pub = nh.advertise(name, 1); + + typedef const data_type(entry_type::*getter_type)(void); + const getter_type getter = force ? static_cast(&entry_type::get) : static_cast(&entry_type::get_cached); + + return [force, pub, entry, getter] () mutable { + Tpub msg; + msg.data = (const typename Tpub::_data_type &) (entry.*getter)(); + pub.publish(msg); + }; +} + +PublishFuncType createPublishFunc(ros::NodeHandle &nh, const std::string &name, canopen::NodeSharedPtr node, const std::string &key, bool force){ + ObjectStorageSharedPtr s = node->getStorage(); + + switch(ObjectDict::DataTypes(s->dict_->get(key)->data_type)){ + case ObjectDict::DEFTYPE_INTEGER8: return create< std_msgs::Int8, ObjectDict::DEFTYPE_INTEGER8 >(nh, name, s, key, force); + case ObjectDict::DEFTYPE_INTEGER16: return create< std_msgs::Int16, ObjectDict::DEFTYPE_INTEGER16 >(nh, name, s, key, force); + case ObjectDict::DEFTYPE_INTEGER32: return create< std_msgs::Int32, ObjectDict::DEFTYPE_INTEGER32 >(nh, name, s, key, force); + case ObjectDict::DEFTYPE_INTEGER64: return create< std_msgs::Int64, ObjectDict::DEFTYPE_INTEGER64 >(nh, name, s, key, force); + + case ObjectDict::DEFTYPE_UNSIGNED8: return create< std_msgs::UInt8, ObjectDict::DEFTYPE_UNSIGNED8 >(nh, name, s, key, force); + case ObjectDict::DEFTYPE_UNSIGNED16: return create< std_msgs::UInt16, ObjectDict::DEFTYPE_UNSIGNED16 >(nh, name, s, key, force); + case ObjectDict::DEFTYPE_UNSIGNED32: return create< std_msgs::UInt32, ObjectDict::DEFTYPE_UNSIGNED32 >(nh, name, s, key, force); + case ObjectDict::DEFTYPE_UNSIGNED64: return create< std_msgs::UInt64, ObjectDict::DEFTYPE_UNSIGNED64 >(nh, name, s, key, force); + + case ObjectDict::DEFTYPE_REAL32: return create< std_msgs::Float32, ObjectDict::DEFTYPE_REAL32 >(nh, name, s, key, force); + case ObjectDict::DEFTYPE_REAL64: return create< std_msgs::Float64, ObjectDict::DEFTYPE_REAL64 >(nh, name, s, key, force); + + case ObjectDict::DEFTYPE_VISIBLE_STRING: return create< std_msgs::String, ObjectDict::DEFTYPE_VISIBLE_STRING >(nh, name, s, key, force); + case ObjectDict::DEFTYPE_OCTET_STRING: return create< std_msgs::String, ObjectDict::DEFTYPE_DOMAIN >(nh, name, s, key, force); + case ObjectDict::DEFTYPE_UNICODE_STRING: return create< std_msgs::String, ObjectDict::DEFTYPE_UNICODE_STRING >(nh, name, s, key, force); + case ObjectDict::DEFTYPE_DOMAIN: return create< std_msgs::String, ObjectDict::DEFTYPE_DOMAIN >(nh, name, s, key, force); + + default: return 0; + } +} + +void RosChain::logState(const can::State &s){ + can::DriverInterfaceSharedPtr interface = interface_; + std::string msg; + if(interface && !interface->translateError(s.internal_error, msg)) msg = "Undefined"; ; + ROS_INFO_STREAM("Current state: " << s.driver_state << " device error: " << s.error_code << " internal_error: " << s.internal_error << " (" << msg << ")"); +} + +void RosChain::run(){ + running_ = true; + time_point abs_time = boost::chrono::high_resolution_clock::now(); + while(running_){ + LayerStatus s; + try{ + read(s); + write(s); + if(!s.bounded()) ROS_ERROR_STREAM_THROTTLE(10, s.reason()); + else if(!s.bounded()) ROS_WARN_STREAM_THROTTLE(10, s.reason()); + } + catch(const canopen::Exception& e){ + ROS_ERROR_STREAM_THROTTLE(1, boost::diagnostic_information(e)); + } + if(!sync_){ + abs_time += update_duration_; + boost::this_thread::sleep_until(abs_time); + } + } +} + +template class ResponseLogger { +protected: + bool logged; + const T& res; + const std::string command; +public: + ResponseLogger(const T& res, const std::string &command) : logged(false), res(res), command(command){} + ~ResponseLogger() { + if(!logged && !res.success){ + if (res.message.empty()){ + ROS_ERROR_STREAM(command << " failed"); + }else{ + ROS_ERROR_STREAM(command << " failed: " << res.message); + } + logged = true; + } + } +}; + +class TriggerResponseLogger: public ResponseLogger { +public: + TriggerResponseLogger(const std_srvs::Trigger::Response& res, const std::string &command) : ResponseLogger(res, command){ + ROS_INFO_STREAM(command << "..."); + } + void logWarning() { + ROS_WARN_STREAM(command << " successful with warning(s): " << res.message); + logged = true; + } + ~TriggerResponseLogger() { + if(!logged && res.success){ + if (res.message.empty()){ + ROS_INFO_STREAM(command << " successful"); + }else{ + ROS_INFO_STREAM(command << " successful: " << res.message); + } + logged = true; + } + } +}; + +bool RosChain::handle_init(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res){ + TriggerResponseLogger rl(res, "Initializing"); + boost::mutex::scoped_lock lock(mutex_); + if(getLayerState() > Off){ + res.success = true; + res.message = "already initialized"; + return true; + } + thread_.reset(new boost::thread(&RosChain::run, this)); + LayerReport status; + try{ + init(status); + res.success = status.bounded(); + res.message = status.reason(); + if(!status.bounded()){ + diag(status); + res.message = status.reason(); + }else{ + heartbeat_timer_.restart(); + return true; + } + } + catch( const std::exception &e){ + std::string info = boost::diagnostic_information(e); + ROS_ERROR_STREAM(info); + res.message = info; + status.error(res.message); + } + catch(...){ + res.message = "Unknown exception"; + status.error(res.message); + } + + res.success = false; + shutdown(status); + return true; +} +bool RosChain::handle_recover(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res){ + TriggerResponseLogger rl(res, "Recovering"); + boost::mutex::scoped_lock lock(mutex_); + res.success = false; + + if(getLayerState() > Init){ + LayerReport status; + try{ + if(!reset_errors_before_recover_ || emcy_handlers_->callFunc(&EMCYHandler::resetErrors, status)){ + recover(status); + } + if(!status.bounded()){ + diag(status); + } + res.success = status.bounded(); + res.message = status.reason(); + if(status.equals()) rl.logWarning(); + } + catch( const std::exception &e){ + std::string info = boost::diagnostic_information(e); + ROS_ERROR_STREAM(info); + res.message = info; + } + catch(...){ + res.message = "Unknown exception"; + } + }else{ + res.message = "not running"; + } + return true; +} + +void RosChain::handleWrite(LayerStatus &status, const LayerState ¤t_state) { + LayerStack::handleWrite(status, current_state); + if(current_state > Shutdown){ + for(const PublishFuncType& func: publishers_) func(); + } +} + +void RosChain::handleShutdown(LayerStatus &status){ + boost::mutex::scoped_lock lock(diag_mutex_); + heartbeat_timer_.stop(); + LayerStack::handleShutdown(status); + if(running_){ + running_ = false; + thread_->interrupt(); + thread_->join(); + thread_.reset(); + } +} + +bool RosChain::handle_shutdown(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res){ + TriggerResponseLogger rl(res, "Shutting down"); + boost::mutex::scoped_lock lock(mutex_); + res.success = true; + if(getLayerState() > Init){ + LayerStatus s; + halt(s); + shutdown(s); + }else{ + res.message = "not running"; + } + return true; +} + +bool RosChain::handle_halt(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res){ + TriggerResponseLogger rl(res, "Halting down"); + boost::mutex::scoped_lock lock(mutex_); + res.success = true; + if(getLayerState() > Init){ + LayerStatus s; + halt(s); + }else{ + res.message = "not running"; + } + return true; +} + +bool RosChain::handle_get_object(canopen_chain_node::GetObject::Request &req, canopen_chain_node::GetObject::Response &res){ + ResponseLogger rl(res, "Getting object " + req.node); + std::map::iterator it = nodes_lookup_.find(req.node); + if(it == nodes_lookup_.end()){ + res.message = "node not found"; + }else{ + try { + res.value = it->second->getStorage()->getStringReader(canopen::ObjectDict::Key(req.object), req.cached)(); + res.success = true; + } catch(std::exception& e) { + res.message = boost::diagnostic_information(e); + } + } + return true; +} + +bool RosChain::handle_set_object(canopen_chain_node::SetObject::Request &req, canopen_chain_node::SetObject::Response &res){ + ResponseLogger rl(res, "Setting object " + req.node); + std::map::iterator it = nodes_lookup_.find(req.node); + if(it == nodes_lookup_.end()){ + res.message = "node not found"; + }else{ + try { + it->second->getStorage()->getStringWriter(canopen::ObjectDict::Key(req.object), req.cached)(req.value); + res.success = true; + } catch(std::exception& e) { + res.message = boost::diagnostic_information(e); + } + } + return true; +} + +bool RosChain::setup_bus(){ + ros::NodeHandle bus_nh(nh_priv_,"bus"); + std::string can_device; + std::string driver_plugin; + std::string master_alloc; + bool loopback; + + if(!bus_nh.getParam("device",can_device)){ + ROS_ERROR("Device not set"); + return false; + } + + bus_nh.param("loopback",loopback, false); + + bus_nh.param("driver_plugin",driver_plugin, std::string("can::SocketCANInterface")); + + try{ + interface_ = driver_loader_.createInstance(driver_plugin); + } + + catch(pluginlib::PluginlibException& ex){ + ROS_ERROR_STREAM(ex.what()); + return false; + } + + state_listener_ = interface_->createStateListenerM(this, &RosChain::logState); + + if(bus_nh.getParam("master_type",master_alloc)){ + ROS_ERROR("please migrate to master allocators"); + return false; + } + + bus_nh.param("master_allocator",master_alloc, std::string("canopen::SimpleMaster::Allocator")); + + try{ + master_= master_allocator_.allocateInstance(master_alloc, can_device, interface_); + } + catch( const std::exception &e){ + std::string info = boost::diagnostic_information(e); + ROS_ERROR_STREAM(info); + return false; + } + + if(!master_){ + ROS_ERROR_STREAM("Could not allocate master."); + return false; + } + + add(std::make_shared(interface_, can_device, loopback, XmlRpcSettings::create(bus_nh))); + + return true; +} + +bool RosChain::setup_sync(){ + ros::NodeHandle sync_nh(nh_priv_,"sync"); + + int sync_ms = 0; + int sync_overflow = 0; + + if(!sync_nh.getParam("interval_ms", sync_ms)){ + ROS_WARN("Sync interval was not specified, so sync is disabled per default"); + } + + if(sync_ms < 0){ + ROS_ERROR_STREAM("Sync interval "<< sync_ms << " is invalid"); + return false; + } + + int update_ms = sync_ms; + if(sync_ms == 0) nh_priv_.getParam("update_ms", update_ms); + if(update_ms == 0){ + ROS_ERROR_STREAM("Update interval "<< sync_ms << " is invalid"); + return false; + }else{ + update_duration_ = boost::chrono::milliseconds(update_ms); + } + + if(sync_ms){ + if(!sync_nh.getParam("overflow", sync_overflow)){ + ROS_WARN("Sync overflow was not specified, so overflow is disabled per default"); + } + if(sync_overflow == 1 || sync_overflow > 240){ + ROS_ERROR_STREAM("Sync overflow "<< sync_overflow << " is invalid"); + return false; + } + if(sync_nh.param("silence_us", 0) != 0){ + ROS_WARN("silence_us is not supported anymore"); + } + + // TODO: parse header + sync_ = master_->getSync(SyncProperties(can::MsgHeader(0x80), sync_ms, sync_overflow)); + + if(!sync_ && sync_ms){ + ROS_ERROR_STREAM("Initializing sync master failed"); + return false; + } + add(sync_); + } + return true; +} + +bool RosChain::setup_heartbeat(){ + ros::NodeHandle hb_nh(nh_priv_,"heartbeat"); + std::string msg; + double rate = 0; + + bool got_any = hb_nh.getParam("msg", msg); + got_any = hb_nh.getParam("rate", rate) || got_any; + + if( !got_any) return true; // nothing todo + + if(rate <=0 ){ + ROS_ERROR_STREAM("Rate '"<< rate << "' is invalid"); + return false; + } + + hb_sender_.frame = can::toframe(msg); + + + if(!hb_sender_.frame.isValid()){ + ROS_ERROR_STREAM("Message '"<< msg << "' is invalid"); + return false; + } + + hb_sender_.interface = interface_; + + heartbeat_timer_.start(std::bind(&HeartbeatSender::send, &hb_sender_), boost::chrono::duration(1.0/rate), false); + + return true; + + +} +std::pair parseObjectName(std::string obj_name){ + size_t pos = obj_name.find('!'); + bool force = pos != std::string::npos; + if(force) obj_name.erase(pos); + return std::make_pair(obj_name, force); +} + +bool addLoggerEntries(XmlRpc::XmlRpcValue merged, const std::string param, uint8_t level, Logger &logger){ + if(merged.hasMember(param)){ + try{ + XmlRpc::XmlRpcValue objs = merged[param]; + for(int i = 0; i < objs.size(); ++i){ + std::pair obj_name = parseObjectName(objs[i]); + + if(!logger.add(level, obj_name.first, obj_name.second)){ + ROS_ERROR_STREAM("Could not create logger for '" << obj_name.first << "'"); + return false; + } + } + } + catch(...){ + ROS_ERROR_STREAM("Could not parse " << param << " parameter"); + return false; + } + } + return true; +} + +bool RosChain::setup_nodes(){ + nodes_.reset(new canopen::LayerGroupNoDiag("301 layer")); + add(nodes_); + + emcy_handlers_.reset(new canopen::LayerGroupNoDiag("EMCY layer")); + + XmlRpc::XmlRpcValue nodes; + if(!nh_priv_.getParam("nodes", nodes)){ + ROS_WARN("falling back to 'modules', please switch to 'nodes'"); + nh_priv_.getParam("modules", nodes); + } + MergedXmlRpcStruct defaults; + nh_priv_.getParam("defaults", defaults); + + if(nodes.getType() == XmlRpc::XmlRpcValue::TypeArray){ + for(size_t i = 0; i < nodes.size(); ++i){ + if(nodes[i].hasMember("name")){ + if(!setup_node(nodes[i], nodes[i]["name"], defaults)) return false; + }else{ + ROS_ERROR_STREAM("Node at list index " << i << " has no name"); + return false; + } + } + }else{ + for(XmlRpc::XmlRpcValue::iterator it = nodes.begin(); it != nodes.end(); ++it){ + if(!setup_node(it->second, it->first, defaults)) return false; + } + } + return true; +} + +bool RosChain::setup_node(const XmlRpc::XmlRpcValue& params, const std::string &name, const MergedXmlRpcStruct &defaults){ + int node_id; + try{ + node_id = params["id"]; + } + catch(...){ + ROS_ERROR_STREAM("Node '" << name << "' has no id"); + return false; + } + MergedXmlRpcStruct merged(params, defaults); + + if(!merged.hasMember("name")){ + merged["name"]=name; + } + + ObjectDict::Overlay overlay; + if(merged.hasMember("dcf_overlay")){ + XmlRpc::XmlRpcValue dcf_overlay = merged["dcf_overlay"]; + if(dcf_overlay.getType() != XmlRpc::XmlRpcValue::TypeStruct){ + ROS_ERROR_STREAM("dcf_overlay is no struct"); + return false; + } + for(XmlRpc::XmlRpcValue::iterator ito = dcf_overlay.begin(); ito!= dcf_overlay.end(); ++ito){ + if(ito->second.getType() != XmlRpc::XmlRpcValue::TypeString){ + ROS_ERROR_STREAM("dcf_overlay '" << ito->first << "' must be string"); + return false; + } + overlay.push_back(ObjectDict::Overlay::value_type(ito->first, ito->second)); + } + } + + std::string eds; + + try{ + eds = (std::string) merged["eds_file"]; + } + catch(...){ + ROS_ERROR_STREAM("EDS path '" << eds << "' invalid"); + return false; + } + + try{ + if(merged.hasMember("eds_pkg")){ + std::string pkg = merged["eds_pkg"]; + std::string p = ros::package::getPath(pkg); + if(p.empty()){ + ROS_WARN_STREAM("Package '" << pkg << "' was not found"); + }else{ + eds = (boost::filesystem::path(p)/eds).make_preferred().native();; + } + } + } + catch(...){ + } + + ObjectDictSharedPtr dict = ObjectDict::fromFile(eds, overlay); + if(!dict){ + ROS_ERROR_STREAM("EDS '" << eds << "' could not be parsed"); + return false; + } + canopen::NodeSharedPtr node = std::make_shared(interface_, dict, node_id, sync_); + + LoggerSharedPtr logger = std::make_shared(node); + + if(!nodeAdded(merged, node, logger)) return false; + + if(!addLoggerEntries(merged, "log", diagnostic_updater::DiagnosticStatusWrapper::OK, *logger)) return false; + if(!addLoggerEntries(merged, "log_warn", diagnostic_updater::DiagnosticStatusWrapper::WARN, *logger)) return false; + if(!addLoggerEntries(merged, "log_error", diagnostic_updater::DiagnosticStatusWrapper::ERROR, *logger)) return false; + + loggers_.push_back(logger); + diag_updater_.add(name, std::bind(&Logger::log, logger, std::placeholders::_1)); + + std::string node_name = std::string(merged["name"]); + + if(merged.hasMember("publish")){ + try{ + XmlRpc::XmlRpcValue objs = merged["publish"]; + for(int i = 0; i < objs.size(); ++i){ + std::pair obj_name = parseObjectName(objs[i]); + + PublishFuncType pub = createPublishFunc(nh_, node_name +"_"+obj_name.first, node, obj_name.first, obj_name.second); + if(!pub){ + ROS_ERROR_STREAM("Could not create publisher for '" << obj_name.first << "'"); + return false; + } + publishers_.push_back(pub); + } + } + catch(...){ + ROS_ERROR("Could not parse publish parameter"); + return false; + } + } + nodes_->add(node); + nodes_lookup_.insert(std::make_pair(node_name, node)); + + std::shared_ptr emcy = std::make_shared(interface_, node->getStorage()); + emcy_handlers_->add(emcy); + logger->add(emcy); + + return true; +} + +bool RosChain::nodeAdded(XmlRpc::XmlRpcValue ¶ms, const canopen::NodeSharedPtr &node, const LoggerSharedPtr &logger){ + return true; +} + +void RosChain::report_diagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat){ + boost::mutex::scoped_lock lock(diag_mutex_); + LayerReport r; + if(getLayerState() == Off){ + stat.summary(stat.WARN,"Not initialized"); + }else if(!running_){ + stat.summary(stat.ERROR,"Thread is not running"); + }else{ + diag(r); + if(r.bounded()){ // valid + stat.summary(r.get(), r.reason()); + for(std::vector >::const_iterator it = r.values().begin(); it != r.values().end(); ++it){ + stat.add(it->first, it->second); + } + } + } +} + +RosChain::RosChain(const ros::NodeHandle &nh, const ros::NodeHandle &nh_priv) +: LayerStack("ROS stack"),driver_loader_("socketcan_interface", "can::DriverInterface"), + master_allocator_("canopen_master", "canopen::Master::Allocator"), + nh_(nh), nh_priv_(nh_priv), + diag_updater_(nh_,nh_priv_), + running_(false), + reset_errors_before_recover_(false){} + +bool RosChain::setup(){ + boost::mutex::scoped_lock lock(mutex_); + bool okay = setup_chain(); + if(okay) add(emcy_handlers_); + return okay; +} + +bool RosChain::setup_chain(){ + std::string hw_id; + nh_priv_.param("hardware_id", hw_id, std::string("none")); + nh_priv_.param("reset_errors_before_recover", reset_errors_before_recover_, false); + + diag_updater_.setHardwareID(hw_id); + diag_updater_.add("chain", this, &RosChain::report_diagnostics); + + diag_timer_ = nh_.createTimer(ros::Duration(diag_updater_.getPeriod()/2.0),std::bind(&diagnostic_updater::Updater::update, &diag_updater_)); + + ros::NodeHandle nh_driver(nh_, "driver"); + + srv_init_ = nh_driver.advertiseService("init",&RosChain::handle_init, this); + srv_recover_ = nh_driver.advertiseService("recover",&RosChain::handle_recover, this); + srv_halt_ = nh_driver.advertiseService("halt",&RosChain::handle_halt, this); + srv_shutdown_ = nh_driver.advertiseService("shutdown",&RosChain::handle_shutdown, this); + + srv_get_object_ = nh_driver.advertiseService("get_object",&RosChain::handle_get_object, this); + srv_set_object_ = nh_driver.advertiseService("set_object",&RosChain::handle_set_object, this); + + return setup_bus() && setup_sync() && setup_heartbeat() && setup_nodes(); +} + +RosChain::~RosChain(){ + try{ + LayerStatus s; + halt(s); + shutdown(s); + }catch(...){ ROS_ERROR("CATCH"); } +} + +} diff --git a/Devices/Libraries/Ros/ros_canopen/canopen_chain_node/src/rosconsole_bridge.cpp b/Devices/Libraries/Ros/ros_canopen/canopen_chain_node/src/rosconsole_bridge.cpp new file mode 100755 index 0000000..92027ba --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/canopen_chain_node/src/rosconsole_bridge.cpp @@ -0,0 +1,2 @@ +#include +REGISTER_ROSCONSOLE_BRIDGE; diff --git a/Devices/Libraries/Ros/ros_canopen/canopen_chain_node/src/sync_node.cpp b/Devices/Libraries/Ros/ros_canopen/canopen_chain_node/src/sync_node.cpp new file mode 100755 index 0000000..4aec701 --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/canopen_chain_node/src/sync_node.cpp @@ -0,0 +1,101 @@ +#include +#include +#include +#include +#include +#include + +template std::string join(const T &container, const std::string &delim){ + if(container.empty()) return std::string(); + std::stringstream sstr; + typename T::const_iterator it = container.begin(); + sstr << *it; + for(++it; it != container.end(); ++it){ + sstr << delim << *it; + } + return sstr.str(); +} +void report_diagnostics(canopen::LayerStack &sync, diagnostic_updater::DiagnosticStatusWrapper &stat){ + canopen::LayerReport r; + sync.read(r); + sync.diag(r); + if(sync.getLayerState() !=canopen::Layer::Off && r.bounded()){ // valid + stat.summary(r.get(), r.reason()); + for(std::vector >::const_iterator it = r.values().begin(); it != r.values().end(); ++it){ + stat.add(it->first, it->second); + } + if(!r.bounded()){ + canopen::LayerStatus s; + sync.recover(s); + } + }else{ + stat.summary(stat.WARN, "sync not initilized"); + canopen::LayerStatus s; + sync.init(s); + } +} + +int main(int argc, char** argv){ + ros::init(argc, argv, "canopen_sync_node"); + + ros::NodeHandle nh; + ros::NodeHandle nh_priv("~"); + + ros::NodeHandle sync_nh(nh_priv, "sync"); + int sync_ms; + if(!sync_nh.getParam("interval_ms", sync_ms) || sync_ms <=0){ + ROS_ERROR_STREAM("Sync interval "<< sync_ms << " is invalid"); + return 1; + } + + int sync_overflow = 0; + if(!sync_nh.getParam("overflow", sync_overflow)){ + ROS_WARN("Sync overflow was not specified, so overflow is disabled per default"); + } + if(sync_overflow == 1 || sync_overflow > 240){ + ROS_ERROR_STREAM("Sync overflow "<< sync_overflow << " is invalid"); + return 1; + } + + + std::string can_device; + if(!nh_priv.getParam("bus/device",can_device)){ + ROS_ERROR("Device not set"); + return 1; + } + + can::SocketCANDriverSharedPtr driver = std::make_shared(); + canopen::SyncProperties sync_properties(can::MsgHeader(sync_nh.param("sync_id", 0x080)), sync_ms, sync_overflow); + + std::shared_ptr sync = std::make_shared(can_device, driver, sync_properties); + + std::vector nodes; + + if(sync_nh.getParam("monitored_nodes",nodes)){ + sync->setMonitored(nodes); + }else{ + std::string msg; + if(nh_priv.getParam("heartbeat/msg", msg)){ + can::Frame f = can::toframe(msg); + if(f.isValid() && (f.id & ~canopen::BCMsync::ALL_NODES_MASK) == canopen::BCMsync::HEARTBEAT_ID){ + nodes.push_back(f.id & canopen::BCMsync::ALL_NODES_MASK); + } + } + sync_nh.getParam("ignored_nodes",nodes); + sync->setIgnored(nodes); + } + + canopen::LayerStack stack("SyncNodeLayer"); + + stack.add(std::make_shared(driver, can_device, false, XmlRpcSettings::create(nh_priv, "bus"))); + stack.add(sync); + + diagnostic_updater::Updater diag_updater(nh, nh_priv); + diag_updater.setHardwareID(nh_priv.param("hardware_id", std::string("none"))); + diag_updater.add("sync", std::bind(report_diagnostics,std::ref(stack), std::placeholders::_1)); + + ros::Timer diag_timer = nh.createTimer(ros::Duration(diag_updater.getPeriod()/2.0),std::bind(&diagnostic_updater::Updater::update, &diag_updater)); + + ros::spin(); + return 0; +} diff --git a/Devices/Libraries/Ros/ros_canopen/canopen_chain_node/srv/GetObject.srv b/Devices/Libraries/Ros/ros_canopen/canopen_chain_node/srv/GetObject.srv new file mode 100755 index 0000000..b0fa1ef --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/canopen_chain_node/srv/GetObject.srv @@ -0,0 +1,8 @@ +string node +string object +bool cached +--- +bool success +string message +string value + diff --git a/Devices/Libraries/Ros/ros_canopen/canopen_chain_node/srv/SetObject.srv b/Devices/Libraries/Ros/ros_canopen/canopen_chain_node/srv/SetObject.srv new file mode 100755 index 0000000..c2a2a5f --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/canopen_chain_node/srv/SetObject.srv @@ -0,0 +1,8 @@ +string node +string object +string value +bool cached +--- +bool success +string message + diff --git a/Devices/Libraries/Ros/ros_canopen/canopen_master/CHANGELOG.rst b/Devices/Libraries/Ros/ros_canopen/canopen_master/CHANGELOG.rst new file mode 100755 index 0000000..9399cf0 --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/canopen_master/CHANGELOG.rst @@ -0,0 +1,263 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package canopen_master +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.8.5 (2020-09-22) +------------------ + +0.8.4 (2020-08-22) +------------------ +* added settings parameter to DriverInterface::init +* moved canopen::Settings into can namespace +* Contributors: Mathias Lüdtke + +0.8.3 (2020-05-07) +------------------ +* Bump CMake version to avoid CMP0048 warning + Signed-off-by: ahcorde +* Contributors: ahcorde + +0.8.2 (2019-11-04) +------------------ +* implemented LayerStatus::equals<>() +* added support for SYNC counter in SimpleSyncLayer (`#349 `_) +* enable rosconsole_bridge bindings +* switch to new logging macros +* add logging based on console_bridge +* removed implicit Header operator +* Contributors: Mathias Lüdtke + +0.8.1 (2019-07-14) +------------------ +* Set C++ standard to c++14 +* added Delegate helpers for backwards compatibility +* implemented create\*ListenerM helpers +* Replacing FastDelegate with std::function and std::bind. +* Contributors: Harsh Deshpande, Joshua Whitley, Mathias Lüdtke + +0.8.0 (2018-07-11) +------------------ +* migrated to std::function and std::bind +* migrated to std::atomic +* got rid of boost::noncopyable +* replaced BOOST_FOREACH +* migrated to std::unordered_map and std::unordered_set +* migrated to std pointers +* provided KeyHash + for use with unordered containers +* added c_array access functons to can::Frame +* Contributors: Mathias Lüdtke + +0.7.8 (2018-05-04) +------------------ +* Revert "pull make_shared into namespaces" + This reverts commit 9b2cd05df76d223647ca81917d289ca6330cdee6. +* Contributors: Mathias Lüdtke + +0.7.7 (2018-05-04) +------------------ +* added types for all function objects +* pull make_shared into namespaces +* added types for all shared_ptrs +* migrate to new classloader headers +* throw bad_cast if datatype is not supported +* special handling of std::bad_cast +* address catkin_lint errors/warnings +* removed IPC/SHM based sync masters +* Contributors: Mathias Lüdtke + +0.7.6 (2017-08-30) +------------------ + +0.7.5 (2017-05-29) +------------------ +* added EMCYHandler::resetErrors +* added VectorHelper::callFunc + generalized call templates +* Contributors: Mathias Lüdtke + +0.7.4 (2017-04-25) +------------------ + +0.7.3 (2017-04-25) +------------------ +* enforce boost::chrono-based timer +* Contributors: Mathias Lüdtke + +0.7.2 (2017-03-28) +------------------ +* fix: handle EMCY as error, not as warning +* Contributors: Mathias Lüdtke + +0.7.1 (2017-03-20) +------------------ +* refactored EMCY handling into separate layer +* print EMCY to stdout +* send node start on recover + needed for external sync to work properly +* pass halt on error unconditionally +* added canopen_bcm_sync +* implemented ExternalMaster +* added object access services +* implemented ObjectStorage::getStringReader +* Contributors: Mathias Lüdtke + +0.7.0 (2016-12-13) +------------------ + +0.6.5 (2016-12-10) +------------------ +* Merge pull request `#179 `_ from ipa-mdl/mixed_case_access + support mixed-case access strings in EDS +* decouple listener initialization from 1003 binding +* introduced THROW_WITH_KEY and ObjectDict::key_info +* added access type tests +* convert access string to lowercase +* Do not remove shared memory automatically +* hardened code with the help of cppcheck +* throw verbose exception if AccessType is missing (`#64 `_) +* styled and sorted CMakeLists.txt + * removed boilerplate comments + * indention + * reviewed exported dependencies +* styled and sorted package.xml +* canopen_master needs to depend on rosunit for gtest +* update package URLs +* fixed typo +* do not reset PDO COB-ID if it is not writable +* Do not recurse into sub-objects, handle them as simple data +* strip string before searching for $NODEID +* added NodeID/hex parser test +* do full recover if if driver is not ready +* wait for driver to be shutdown in run() +* limit SDO reader to size of 1 +* do not send abort twice +* removed unnecessary sleep (added for tests only) +* catch all std exceptions in layer handlers +* migrated SDOClient to BufferedReader +* getter for LayerState +* fixed lost wake-up condition, unified SDO accessors +* minor NMT improvements +* removed cond from PDOMapper, it does not wait on empty buffer anymore +* Simple master counts nodes as well +* throw exception on read from empty buffer +* proper initialisation of PDO data from SDOs +* change sync subscription only on change +* shutdown and restart CAN layer on recover +* canopen::Exception is now based on std::runtime_error +* Merge pull request `#109 `_ from ipa-mdl/shutdown-crashes + Fix for pluginlib-related crashes on shutdown +* stop after heartbeat was disabled, do not wait for state switch +* added virtual destructor to SyncCounter +* Use getHeartbeatInterval() +* minor shutdown improvements +* removed unstable StateWaiter::wait_for +* Revert change to handleShutdown +* Heartbeat interval is uint16, not double +* Added validity check to heartbeat\_ (Some devices do not support heartbeat) +* Contributors: Florian Weisshardt, Mathias Lüdtke, Michael Stoll + +0.6.4 (2015-07-03) +------------------ +* added missing include, revised depends etc. + +0.6.3 (2015-06-30) +------------------ +* added Settings class +* added SimpleMaster +* remove boost::posix_time::milliseconds from SyncProperties +* removed support for silence_us since bus timing cannot be guaranteed +* properly handle cases where def_val == init_val +* implemented plugin-based Master allocators, defaults to LocalMaster +* moved master/synclayer base classes to canopen.h +* added support for non-continuous PDO ranges +* added has() check to object dictionary interface +* improved ObjectStorage entry interface +* verbose out_of_range exception +* improved timer: duration cast, autostart flag +* reset sync waiter number after timeout +* verbose timeout exception +* little fix im EMCY diagnostics +* string instead of mulit-char constant +* Merge branch 'hwi_switch' into muparser +* added std::string converters to ObjectDict::Key +* do not warn on profile-only errors +* added get_abs_time without parameter +* link against boost_atomic for platforms with lock-based implementation +* reset sent Reset and Reset_Com, c&p bug +* stop heartbeat after node shutdown +* protect reads of LayerState +* protect layers in VectorHelper +* protect buffer data +* set error only if generic error bit is set, otherwise just warn about it +* Fixes https://github.com/ipa320/ros_canopen/issues/81 +* Update emcy.cpp +* removed debug outputs +* refactored Layer mechanisms +* simplified init +* simplified EMCY handling +* improved hearbeat handling +* do not stop master on slave timeout +* improved pending handling in complex layers +* added set_cached for object entries +* removed IPCLayer sync listener, loopback is disabled per default +* Merge branch 'dummy_interface' into indigo_dev + Conflicts: + canopen_master/src/objdict.cpp +* added sync silence feature +* Merge remote-tracking branch 'origin/fix32bit' into indigo_dev +* require message strings for error indicators, added missing strings, added ROS logging in sync loop +* fix ambiguous buffer access with 32bit compilers +* pad octet strings if necessary +* reset pending to layers.begin() +* enforce RPDO (device-side) transmimssion type to 1 if <=240 +* introduced LayerVector to unify pending support +* introduced read_integer to enfoce hex parsing, closes `#74 `_ +* clear layer before plugin loader is deleted +* Merge branch 'indigo_dev' of https://github.com/ipa320/ros_canopen into indigo_dev +* Merge pull request `#70 `_ from ipa-mdl/pluginlib + added plugin feature to socketcan_interface +* exception-aware get functions +* removed RPDO sync timeout in favour of LayerStatus +* added message string helper +* EDS files are case-insensitive, so switching to iptree +* handle errors entries that are not in the dictionary +* sub entry number must be hex coded +* do not send initilized-only PDO data +* init entries if init value was given and default value was not +* implemented threading in CANLayer +* removed bitrate, added loopback to DriverInterface::init +* removed SimpleLayer, migrated to Layer +* Layer::pending and Layer::halt are now virtual pure as well +* schunk version of reset +* Merge branch 'elmo_console' of https://github.com/ipa-mdl/ros_canopen into dcf_overlay +* remove debug prints +* resize buffer if needed in expedited SDO upload +* fix SDO segment download +* only access EMCY errors if available +* added ObjectStorage:Entry::valid() +* added ObjectDict overlay feature +* Fixes the bus controller problems for the Elmo chain +* Work-around for Elmo SDO bug(?) +* improved PDO buffer initialization, buffer if filled per SDO if needed +* pass permission object +* disable threading interrupts while waiting for SDO response +* Merge branch 'indigo_dev' into merge + Conflicts: + canopen_chain_node/include/canopen_chain_node/chain_ros.h + canopen_master/include/canopen_master/canopen.h + canopen_master/include/canopen_master/layer.h + canopen_master/src/node.cpp + canopen_motor_node/CMakeLists.txt + canopen_motor_node/src/control_node.cpp +* Contributors: Mathias Lüdtke, Thiago de Freitas Oliveira Araujo, ipa-cob4-2, ipa-fmw, thiagodefreitas + +0.6.2 (2014-12-18) +------------------ + +0.6.1 (2014-12-15) +------------------ +* remove ipa_* and IPA_* prefixes +* added descriptions and authors +* renamed ipa_canopen_master to canopen_master +* Contributors: Florian Weisshardt, Mathias Lüdtke diff --git a/Devices/Libraries/Ros/ros_canopen/canopen_master/CMakeLists.txt b/Devices/Libraries/Ros/ros_canopen/canopen_master/CMakeLists.txt new file mode 100755 index 0000000..62ff3a7 --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/canopen_master/CMakeLists.txt @@ -0,0 +1,102 @@ +cmake_minimum_required(VERSION 3.0.2) +project(canopen_master) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_STANDARD 14) +endif() + +find_package(catkin REQUIRED + COMPONENTS + class_loader + socketcan_interface +) + +find_package(Boost REQUIRED + COMPONENTS + atomic + chrono + thread +) + +catkin_package( + INCLUDE_DIRS + include + LIBRARIES + ${PROJECT_NAME} + CATKIN_DEPENDS + socketcan_interface + DEPENDS + Boost +) +include_directories(include ${catkin_INCLUDE_DIRS}) + +add_library(${PROJECT_NAME} + src/emcy.cpp + src/node.cpp + src/objdict.cpp + src/pdo.cpp + src/sdo.cpp +) +target_link_libraries(${PROJECT_NAME} + ${catkin_LIBRARIES} + ${Boost_LIBRARIES} +) + +add_library(${PROJECT_NAME}_plugin + src/master_plugin.cpp +) + +target_link_libraries(${PROJECT_NAME}_plugin + ${catkin_LIBRARIES} + ${Boost_LIBRARIES} + ${PROJECT_NAME} +) + +# canopen_bcm_sync +add_executable(canopen_bcm_sync + src/bcm_sync.cpp +) +target_link_libraries(canopen_bcm_sync + ${Boost_LIBRARIES} + ${catkin_LIBRARIES} +) + +install( + TARGETS + canopen_bcm_sync + ${PROJECT_NAME} + ${PROJECT_NAME}_plugin + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install( + DIRECTORY + include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" +) + +install( + FILES + master_plugin.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +if(CATKIN_ENABLE_TESTING) + catkin_add_gtest(${PROJECT_NAME}-test_parser + test/test_parser.cpp + ) + target_link_libraries(${PROJECT_NAME}-test_parser + ${PROJECT_NAME} + ) + + catkin_add_gtest(${PROJECT_NAME}-test_node + test/test_node.cpp + ) + target_link_libraries(${PROJECT_NAME}-test_node + ${PROJECT_NAME} + ) +endif() diff --git a/Devices/Libraries/Ros/ros_canopen/canopen_master/include/canopen_master/bcm_sync.h b/Devices/Libraries/Ros/ros_canopen/canopen_master/include/canopen_master/bcm_sync.h new file mode 100755 index 0000000..c1cd1c1 --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/canopen_master/include/canopen_master/bcm_sync.h @@ -0,0 +1,167 @@ +#ifndef H_BCM_SYNC +#define H_BCM_SYNC + +#include +#include +#include + +namespace canopen { + +template std::string join(const T &container, const std::string &delim){ + if(container.empty()) return std::string(); + std::stringstream sstr; + typename T::const_iterator it = container.begin(); + sstr << *it; + for(++it; it != container.end(); ++it){ + sstr << delim << *it; + } + return sstr.str(); +} + +class BCMsync : public Layer { + boost::mutex mutex_; + + std::string device_; + + std::set ignored_nodes_; + std::set monitored_nodes_; + std::set known_nodes_; + std::set started_nodes_; + + bool sync_running_; + can::BCMsocket bcm_; + can::SocketCANDriverSharedPtr driver_; + uint16_t sync_ms_; + can::FrameListenerConstSharedPtr handler_; + + std::vector sync_frames_; + + bool skipNode(uint8_t id){ + if(ignored_nodes_.find(id) != ignored_nodes_.end()) return true; + if(!monitored_nodes_.empty() && monitored_nodes_.find(id) == monitored_nodes_.end()) return true; + known_nodes_.insert(id); + return false; + } + + void handleFrame(const can::Frame &frame){ + boost::mutex::scoped_lock lock(mutex_); + + if(frame.id == NMT_ID){ + if(frame.dlc > 1){ + uint8_t cmd = frame.data[0]; + uint8_t id = frame.data[1]; + if(skipNode(id)) return; + + if(cmd == 1){ // started + if(id != 0) started_nodes_.insert(id); + else started_nodes_.insert(known_nodes_.begin(), known_nodes_.end()); // start all + }else{ + if(id != 0) started_nodes_.erase(id); + else started_nodes_.clear(); // stop all + } + } + }else if((frame.id & ~ALL_NODES_MASK) == HEARTBEAT_ID){ + int id = frame.id & ALL_NODES_MASK; + if(skipNode(id)) return; + + if(frame.dlc > 0 && frame.data[0] == canopen::Node::Stopped) started_nodes_.erase(id); + } + + // toggle sync if needed + if(started_nodes_.empty() && sync_running_){ + sync_running_ = !bcm_.stopTX(sync_frames_.front()); + }else if(!started_nodes_.empty() && !sync_running_){ + sync_running_ = bcm_.startTX(boost::chrono::milliseconds(sync_ms_),sync_frames_.front(), sync_frames_.size(), &sync_frames_[0]); + } + } +protected: + virtual void handleRead(LayerStatus &status, const LayerState ¤t_state) { + if(current_state > Init){ + } + } + virtual void handleWrite(LayerStatus &status, const LayerState ¤t_state) { + if(current_state > Init){ + } + } + virtual void handleDiag(LayerReport &report){ + boost::mutex::scoped_lock lock(mutex_); + if(!sync_running_) report.warn("sync is not running"); + + report.add("sync_running", sync_running_); + report.add("known_nodes", join(known_nodes_, ", ")); + report.add("started_nodes", join(started_nodes_, ", ")); + } + + virtual void handleInit(LayerStatus &status){ + boost::mutex::scoped_lock lock(mutex_); + started_nodes_.clear(); + + if(!bcm_.init(device_)){ + status.error("BCM_init failed"); + return; + } + int sc = driver_->getInternalSocket(); + + struct can_filter filter[2]; + + filter[0].can_id = NMT_ID; + filter[0].can_mask = CAN_SFF_MASK; + filter[1].can_id = HEARTBEAT_ID; + filter[1].can_mask = ~ALL_NODES_MASK; + + if(setsockopt(sc, SOL_CAN_RAW, CAN_RAW_FILTER, &filter, sizeof(filter)) < 0){ + status.warn("could not set filter"); + return; + } + + handler_ = driver_->createMsgListenerM(this, &BCMsync::handleFrame); + } + virtual void handleShutdown(LayerStatus &status){ + boost::mutex::scoped_lock lock(mutex_); + handler_.reset(); + bcm_.shutdown(); + } + + virtual void handleHalt(LayerStatus &status) { + boost::mutex::scoped_lock lock(mutex_); + if(sync_running_){ + bcm_.stopTX(sync_frames_.front()); + sync_running_ = false; + started_nodes_.clear(); + } + } + + virtual void handleRecover(LayerStatus &status){ + handleShutdown(status); + handleInit(status); + } +public: + static const uint32_t ALL_NODES_MASK = 127; + static const uint32_t HEARTBEAT_ID = 0x700; + static const uint32_t NMT_ID = 0x000; + + BCMsync(const std::string &device, can::SocketCANDriverSharedPtr driver, const SyncProperties &sync_properties) + : Layer(device + " SyncLayer"), device_(device), sync_running_(false), sync_ms_(sync_properties.period_ms_), driver_(driver) { + if(sync_properties.overflow_ == 0){ + sync_frames_.resize(1); + sync_frames_[0] = can::Frame(sync_properties.header_,0); + }else{ + sync_frames_.resize(sync_properties.overflow_); + for(int i = 0; i < sync_properties.overflow_; ++i){ + sync_frames_[i] = can::Frame(sync_properties.header_,1); + sync_frames_[i].data[0] = i+1; + } + } + } + template void setMonitored(const T &other){ + monitored_nodes_.clear(); + monitored_nodes_.insert(other.begin(), other.end()); + } + template void setIgnored(const T &other){ + ignored_nodes_.clear(); + ignored_nodes_.insert(other.begin(), other.end()); + } +}; + +} +#endif diff --git a/Devices/Libraries/Ros/ros_canopen/canopen_master/include/canopen_master/can_layer.h b/Devices/Libraries/Ros/ros_canopen/canopen_master/include/canopen_master/can_layer.h new file mode 100755 index 0000000..9b94c0c --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/canopen_master/include/canopen_master/can_layer.h @@ -0,0 +1,115 @@ +#ifndef H_CAN_LAYER +#define H_CAN_LAYER + +#include +#include "layer.h" +#include + +namespace canopen{ + +class CANLayer: public Layer{ + boost::mutex mutex_; + can::DriverInterfaceSharedPtr driver_; + const std::string device_; + const bool loopback_; + can::SettingsConstSharedPtr settings_; + can::Frame last_error_; + can::FrameListenerConstSharedPtr error_listener_; + void handleFrame(const can::Frame & msg){ + boost::mutex::scoped_lock lock(mutex_); + last_error_ = msg; + ROSCANOPEN_ERROR("canopen_master", "Received error frame: " << msg); + } + std::shared_ptr thread_; + +public: + CANLayer(const can::DriverInterfaceSharedPtr &driver, const std::string &device, bool loopback, can::SettingsConstSharedPtr settings) + : Layer(device + " Layer"), driver_(driver), device_(device), loopback_(loopback), settings_(settings) { assert(driver_); } + + [[deprecated("provide settings explicitly")]] CANLayer(const can::DriverInterfaceSharedPtr &driver, const std::string &device, bool loopback) + : Layer(device + " Layer"), driver_(driver), device_(device), loopback_(loopback), settings_(can::NoSettings::create()) { assert(driver_); } + + virtual void handleRead(LayerStatus &status, const LayerState ¤t_state) { + if(current_state > Init){ + if(!driver_->getState().isReady()) status.error("CAN not ready"); + } + } + virtual void handleWrite(LayerStatus &status, const LayerState ¤t_state) { + if(current_state > Init){ + if(!driver_->getState().isReady()) status.error("CAN not ready"); + } + } + + virtual void handleDiag(LayerReport &report){ + can::State s = driver_->getState(); + if(!s.isReady()){ + report.error("CAN layer not ready"); + report.add("driver_state", int(s.driver_state)); + } + if(s.error_code){ + report.add("socket_error", s.error_code); + } + if(s.internal_error != 0){ + report.add("internal_error", int(s.internal_error)); + std::string desc; + if(driver_->translateError(s.internal_error, desc)) report.add("internal_error_desc", desc); + std::stringstream sstr; + sstr << std::hex; + { + boost::mutex::scoped_lock lock(mutex_); + for(size_t i=0; i < last_error_.dlc; ++i){ + sstr << (unsigned int) last_error_.data[i] << " "; + } + } + report.add("can_error_frame", sstr.str()); + + } + + } + + virtual void handleInit(LayerStatus &status){ + if(thread_){ + status.warn("CAN thread already running"); + } else if(!driver_->init(device_, loopback_, settings_)) { + status.error("CAN init failed"); + } else { + can::StateWaiter waiter(driver_.get()); + + thread_.reset(new boost::thread(&can::DriverInterface::run, driver_)); + error_listener_ = driver_->createMsgListenerM(can::ErrorHeader(),this, &CANLayer::handleFrame); + + if(!waiter.wait(can::State::ready, boost::posix_time::seconds(1))){ + status.error("CAN init timed out"); + } + } + if(!driver_->getState().isReady()){ + status.error("CAN is not ready"); + } + } + virtual void handleShutdown(LayerStatus &status){ + can::StateWaiter waiter(driver_.get()); + error_listener_.reset(); + driver_->shutdown(); + if(!waiter.wait(can::State::closed, boost::posix_time::seconds(1))){ + status.warn("CAN shutdown timed out"); + } + if(thread_){ + thread_->interrupt(); + thread_->join(); + thread_.reset(); + } + } + + virtual void handleHalt(LayerStatus &status) { /* nothing to do */ } + + virtual void handleRecover(LayerStatus &status){ + if(!driver_->getState().isReady()){ + handleShutdown(status); + handleInit(status); + } + } + +}; +} // namespace canopen + +#endif diff --git a/Devices/Libraries/Ros/ros_canopen/canopen_master/include/canopen_master/canopen.h b/Devices/Libraries/Ros/ros_canopen/canopen_master/include/canopen_master/canopen.h new file mode 100755 index 0000000..dd589c9 --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/canopen_master/include/canopen_master/canopen.h @@ -0,0 +1,331 @@ +#ifndef H_CANOPEN +#define H_CANOPEN + +#include +#include +#include +#include "exceptions.h" +#include "layer.h" +#include "objdict.h" +#include "timer.h" +#include +#include +#include +#include + +namespace canopen{ + +typedef boost::chrono::high_resolution_clock::time_point time_point; +typedef boost::chrono::high_resolution_clock::duration time_duration; +inline time_point get_abs_time(const time_duration& timeout) { return boost::chrono::high_resolution_clock::now() + timeout; } +inline time_point get_abs_time() { return boost::chrono::high_resolution_clock::now(); } + + +template struct FrameOverlay: public can::Frame{ + T &data; + FrameOverlay(const Header &h) : can::Frame(h,sizeof(T)), data(* (T*) can::Frame::c_array()) { + can::Frame::data.fill(0); + } + FrameOverlay(const can::Frame &f) : can::Frame(f), data(* (T*) can::Frame::c_array()) { } +}; + +class SDOClient{ + + can::Header client_id; + + boost::timed_mutex mutex; + + can::BufferedReader reader_; + bool processFrame(const can::Frame & msg); + + String buffer; + size_t offset; + size_t total; + bool done; + can::Frame last_msg; + const canopen::ObjectDict::Entry * current_entry; + + void transmitAndWait(const canopen::ObjectDict::Entry &entry, const String &data, String *result); + void abort(uint32_t reason); + + const can::CommInterfaceSharedPtr interface_; +protected: + void read(const canopen::ObjectDict::Entry &entry, String &data); + void write(const canopen::ObjectDict::Entry &entry, const String &data); +public: + const ObjectStorageSharedPtr storage_; + + void init(); + + SDOClient(const can::CommInterfaceSharedPtr interface, const ObjectDictSharedPtr dict, uint8_t node_id) + : interface_(interface), + storage_(std::make_shared(dict, node_id, + std::bind(&SDOClient::read, this, std::placeholders::_1, std::placeholders::_2), + std::bind(&SDOClient::write, this, std::placeholders::_1, std::placeholders::_2)) + ), + reader_(false, 1) + { + } +}; + +class PDOMapper{ + boost::mutex mutex_; + + class Buffer{ + public: + bool read(uint8_t* b, const size_t len); + void write(const uint8_t* b, const size_t len); + void read(const canopen::ObjectDict::Entry &entry, String &data); + void write(const canopen::ObjectDict::Entry &, const String &data); + void clean() { dirty = false; } + const size_t size; + Buffer(const size_t sz) : size(sz), dirty(false), empty(true), buffer(sz) {} + + private: + boost::mutex mutex; + bool dirty; + bool empty; + std::vector buffer; + }; + typedef std::shared_ptr BufferSharedPtr; + + class PDO { + protected: + void parse_and_set_mapping(const ObjectStorageSharedPtr &storage, const uint16_t &com_index, const uint16_t &map_index, const bool &read, const bool &write); + can::Frame frame; + uint8_t transmission_type; + std::vectorbuffers; + }; + + struct TPDO: public PDO{ + typedef std::shared_ptr TPDOSharedPtr; + void sync(); + static TPDOSharedPtr create(const can::CommInterfaceSharedPtr interface, const ObjectStorageSharedPtr &storage, const uint16_t &com_index, const uint16_t &map_index){ + TPDOSharedPtr tpdo(new TPDO(interface)); + if(!tpdo->init(storage, com_index, map_index)) + tpdo.reset(); + return tpdo; + } + private: + TPDO(const can::CommInterfaceSharedPtr interface) : interface_(interface){} + bool init(const ObjectStorageSharedPtr &storage, const uint16_t &com_index, const uint16_t &map_index); + const can::CommInterfaceSharedPtr interface_; + boost::mutex mutex; + }; + + struct RPDO : public PDO{ + void sync(LayerStatus &status); + typedef std::shared_ptr RPDOSharedPtr; + static RPDOSharedPtr create(const can::CommInterfaceSharedPtr interface, const ObjectStorageSharedPtr &storage, const uint16_t &com_index, const uint16_t &map_index){ + RPDOSharedPtr rpdo(new RPDO(interface)); + if(!rpdo->init(storage, com_index, map_index)) + rpdo.reset(); + return rpdo; + } + private: + bool init(const ObjectStorageSharedPtr &storage, const uint16_t &com_index, const uint16_t &map_index); + RPDO(const can::CommInterfaceSharedPtr interface) : interface_(interface), timeout(-1) {} + boost::mutex mutex; + const can::CommInterfaceSharedPtr interface_; + + can::FrameListenerConstSharedPtr listener_; + void handleFrame(const can::Frame & msg); + int timeout; + }; + + std::unordered_set rpdos_; + std::unordered_set tpdos_; + + const can::CommInterfaceSharedPtr interface_; + +public: + PDOMapper(const can::CommInterfaceSharedPtr interface); + void read(LayerStatus &status); + bool write(); + bool init(const ObjectStorageSharedPtr storage, LayerStatus &status); +}; + +class EMCYHandler : public Layer { + std::atomic has_error_; + ObjectStorage::Entry error_register_; + ObjectStorage::Entry num_errors_; + can::FrameListenerConstSharedPtr emcy_listener_; + void handleEMCY(const can::Frame & msg); + const ObjectStorageSharedPtr storage_; + + virtual void handleDiag(LayerReport &report); + + virtual void handleInit(LayerStatus &status); + virtual void handleRecover(LayerStatus &status); + virtual void handleRead(LayerStatus &status, const LayerState ¤t_state); + virtual void handleWrite(LayerStatus &status, const LayerState ¤t_state); + virtual void handleHalt(LayerStatus &status); + virtual void handleShutdown(LayerStatus &status); + +public: + EMCYHandler(const can::CommInterfaceSharedPtr interface, const ObjectStorageSharedPtr storage); + void resetErrors(LayerStatus &status); +}; + +struct SyncProperties{ + const can::Header header_; + const uint16_t period_ms_; + const uint8_t overflow_; + SyncProperties(const can::Header &h, const uint16_t &p, const uint8_t &o) : header_(h), period_ms_(p), overflow_(o) {} + bool operator==(const SyncProperties &p) const { return p.header_.key() == header_.key() && p.overflow_ == overflow_ && p.period_ms_ == period_ms_; } + +}; + +class SyncCounter { +public: + const SyncProperties properties; + SyncCounter(const SyncProperties &p) : properties(p) {} + virtual void addNode(void * const ptr) = 0; + virtual void removeNode(void * const ptr) = 0; + virtual ~SyncCounter() {} +}; +typedef std::shared_ptr SyncCounterSharedPtr; + +class Node : public Layer{ +public: + enum State{ + Unknown = 255, BootUp = 0, Stopped = 4, Operational = 5 , PreOperational = 127 + }; + const uint8_t node_id_; + Node(const can::CommInterfaceSharedPtr interface, const ObjectDictSharedPtr dict, uint8_t node_id, const SyncCounterSharedPtr sync = SyncCounterSharedPtr()); + + const State getState(); + void enterState(const State &s); + + const ObjectStorageSharedPtr getStorage() { return sdo_.storage_; } + + bool start(); + bool stop(); + bool reset(); + bool reset_com(); + bool prepare(); + + using StateFunc = std::function; + using StateDelegate [[deprecated("use StateFunc instead")]] = can::DelegateHelper; + + typedef can::Listener StateListener; + typedef StateListener::ListenerConstSharedPtr StateListenerConstSharedPtr; + + StateListenerConstSharedPtr addStateListener(const StateFunc & s){ + return state_dispatcher_.createListener(s); + } + + template T get(const ObjectDict::Key& k){ + return getStorage()->entry(k).get(); + } + +private: + virtual void handleDiag(LayerReport &report); + + virtual void handleInit(LayerStatus &status); + virtual void handleRecover(LayerStatus &status); + virtual void handleRead(LayerStatus &status, const LayerState ¤t_state); + virtual void handleWrite(LayerStatus &status, const LayerState ¤t_state); + virtual void handleHalt(LayerStatus &status); + virtual void handleShutdown(LayerStatus &status); + + template int wait_for(const State &s, const T &timeout); + + boost::timed_mutex mutex; + boost::mutex cond_mutex; + boost::condition_variable cond; + + const can::CommInterfaceSharedPtr interface_; + const SyncCounterSharedPtr sync_; + can::FrameListenerConstSharedPtr nmt_listener_; + + ObjectStorage::Entry::type> heartbeat_; + + can::SimpleDispatcher state_dispatcher_; + + void handleNMT(const can::Frame & msg); + void switchState(const uint8_t &s); + + State state_; + SDOClient sdo_; + PDOMapper pdo_; + + boost::chrono::high_resolution_clock::time_point heartbeat_timeout_; + uint16_t getHeartbeatInterval() { return heartbeat_.valid()?heartbeat_.get_cached() : 0; } + void setHeartbeatInterval() { if(heartbeat_.valid()) heartbeat_.set(heartbeat_.desc().value().get()); } + bool checkHeartbeat(); +}; +typedef std::shared_ptr NodeSharedPtr; + +template class Chain{ +public: + typedef std::shared_ptr MemberSharedPtr; + void call(void (T::*func)(void)){ + typename std::vector::iterator it = elements.begin(); + while(it != elements.end()){ + ((**it).*func)(); + ++it; + } + } + template void call(void (T::*func)(const V&), const std::vector &vs){ + typename std::vector::iterator it = elements.begin(); + typename std::vector::const_iterator it_v = vs.begin(); + while(it_v != vs.end() && it != elements.end()){ + ((**it).*func)(*it_v); + ++it; ++it_v; + } + } + template void call(void (T::*func)(V&), std::vector &vs){ + vs.resize(elements.size()); + + typename std::vector::iterator it = elements.begin(); + typename std::vector::iterator it_v = vs.begin(); + while(it_v != vs.end() && it != elements.end()){ + ((**it).*func)(*it_v); + ++it; ++it_v; + } + } + void add(MemberSharedPtr t){ + elements.push_back(t); + } +protected: + std::vector elements; +}; + +template class NodeChain: public Chain{ +public: + const std::vector::MemberSharedPtr>& getElements() { return Chain::elements; } + void start() { this->call(&T::start); } + void stop() { this->call(&T::stop); } + void reset() { this->call(&T::reset); } + void reset_com() { this->call(&T::reset_com); } + void prepare() { this->call(&T::prepare); } +}; + +class SyncLayer: public Layer, public SyncCounter{ +public: + SyncLayer(const SyncProperties &p) : Layer("Sync layer"), SyncCounter(p) {} +}; +typedef std::shared_ptr SyncLayerSharedPtr; + +class Master{ + Master(const Master&) = delete; // prevent copies + Master& operator=(const Master&) = delete; +public: + Master() = default; + virtual SyncLayerSharedPtr getSync(const SyncProperties &properties) = 0; + virtual ~Master() {} + + typedef std::shared_ptr MasterSharedPtr; + class Allocator { + public: + virtual MasterSharedPtr allocate(const std::string &name, can::CommInterfaceSharedPtr interface) = 0; + virtual ~Allocator() {} + }; +}; +typedef Master::MasterSharedPtr MasterSharedPtr; + +using can::Settings; + +} // canopen +#endif // !H_CANOPEN diff --git a/Devices/Libraries/Ros/ros_canopen/canopen_master/include/canopen_master/exceptions.h b/Devices/Libraries/Ros/ros_canopen/canopen_master/include/canopen_master/exceptions.h new file mode 100755 index 0000000..3c10a89 --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/canopen_master/include/canopen_master/exceptions.h @@ -0,0 +1,33 @@ +#ifndef H_EXCEPTIONS +#define H_EXCEPTIONS + +#include +#include +#include + +namespace canopen{ + +class Exception : public std::runtime_error { +public: + Exception(const std::string &w) : std::runtime_error(w) {} +}; + +class PointerInvalid : public Exception{ +public: + PointerInvalid(const std::string &w) : Exception("Pointer invalid") {} +}; + +class ParseException : public Exception{ +public: + ParseException(const std::string &w) : Exception(w) {} +}; + +class TimeoutException : public Exception{ +public: + TimeoutException(const std::string &w) : Exception(w) {} +}; + + +} // canopen + +#endif // !H_EXCEPTIONS diff --git a/Devices/Libraries/Ros/ros_canopen/canopen_master/include/canopen_master/layer.h b/Devices/Libraries/Ros/ros_canopen/canopen_master/include/canopen_master/layer.h new file mode 100755 index 0000000..13f27c1 --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/canopen_master/include/canopen_master/layer.h @@ -0,0 +1,260 @@ +#ifndef H_CANOPEN_LAYER +#define H_CANOPEN_LAYER + +#include +#include +#include +#include +#include + +namespace canopen{ + +class LayerStatus{ + mutable boost::mutex write_mutex_; + enum State{ + OK = 0, WARN = 1, ERROR= 2, STALE = 3, UNBOUNDED = 3 + }; + std::atomic state; + std::string reason_; + + virtual void set(const State &s, const std::string &r){ + boost::mutex::scoped_lock lock(write_mutex_); + if(s > state) state = s; + if(!r.empty()){ + if(reason_.empty()) reason_ = r; + else reason_ += "; " + r; + } + } +public: + struct Ok { static const State state = OK; private: Ok();}; + struct Warn { static const State state = WARN; private: Warn(); }; + struct Error { static const State state = ERROR; private: Error(); }; + struct Stale { static const State state = STALE; private: Stale(); }; + struct Unbounded { static const State state = UNBOUNDED; private: Unbounded(); }; + + template bool bounded() const{ return state <= T::state; } + template bool equals() const{ return state == T::state; } + + LayerStatus() : state(OK) {} + + int get() const { return state; } + + const std::string reason() const { boost::mutex::scoped_lock lock(write_mutex_); return reason_; } + + const void warn(const std::string & r) { set(WARN, r); } + const void error(const std::string & r) { set(ERROR, r); } + const void stale(const std::string & r) { set(STALE, r); } +}; +class LayerReport : public LayerStatus { + std::vector > values_; +public: + const std::vector > &values() const { return values_; } + template void add(const std::string &key, const T &value) { + std::stringstream str; + str << value; + values_.push_back(std::make_pair(key,str.str())); + } +}; + +#define CATCH_LAYER_HANDLER_EXCEPTIONS(command, status) \ + try{ command; } \ + catch(std::exception &e) {status.error(boost::diagnostic_information(e)); } + +class Layer{ +public: + enum LayerState{ + Off, + Init, + Shutdown, + Error, + Halt, + Recover, + Ready + }; + + const std::string name; + + void read(LayerStatus &status) { + if(state > Off) CATCH_LAYER_HANDLER_EXCEPTIONS(handleRead(status, state), status); + } + void write(LayerStatus &status) { + if(state > Off) CATCH_LAYER_HANDLER_EXCEPTIONS(handleWrite(status, state), status); + } + void diag(LayerReport &report) { + if(state > Shutdown) CATCH_LAYER_HANDLER_EXCEPTIONS(handleDiag(report), report); + } + void init(LayerStatus &status) { + if(state == Off){ + if(status.bounded()){ + state = Init; + CATCH_LAYER_HANDLER_EXCEPTIONS(handleInit(status), status); + } + if(!status.bounded()) shutdown(status); + else state = Ready; + } + } + void shutdown(LayerStatus &status) { + if(state != Off){ + state = Shutdown; + CATCH_LAYER_HANDLER_EXCEPTIONS(handleShutdown(status), status); + state = Off; + } + } + void halt(LayerStatus &status) { + if(state > Halt){ + state = Halt; + CATCH_LAYER_HANDLER_EXCEPTIONS(handleHalt(status), status); + state = Error; + } + } + void recover(LayerStatus &status) { + if(state == Error){ + if(status.bounded()){ + state = Recover; + CATCH_LAYER_HANDLER_EXCEPTIONS(handleRecover(status), status); + } + if(!status.bounded()){ + halt(status); + }else{ + state = Ready; + } + } + + } + + LayerState getLayerState() { return state; } + + Layer(const std::string &n) : name(n), state(Off) {} + + virtual ~Layer() {} + +protected: + virtual void handleRead(LayerStatus &status, const LayerState ¤t_state) = 0; + virtual void handleWrite(LayerStatus &status, const LayerState ¤t_state) = 0; + + virtual void handleDiag(LayerReport &report) = 0; + + virtual void handleInit(LayerStatus &status) = 0; + virtual void handleShutdown(LayerStatus &status) = 0; + + virtual void handleHalt(LayerStatus &status) = 0; + virtual void handleRecover(LayerStatus &status) = 0; + +private: + std::atomic state; + +}; + +template class VectorHelper{ +public: + typedef std::shared_ptr VectorMemberSharedPtr; +protected: + typedef std::vector vector_type ; + template typename vector_type::iterator call(FuncType func, Data &status){ + boost::shared_lock lock(mutex); + return call(func, status, layers.begin(), layers.end()); + } + template typename vector_type::iterator call(FuncType func, Data &status){ + boost::shared_lock lock(mutex); + return call(func, status, layers.begin(), layers.end()); + } + template typename vector_type::reverse_iterator call_rev(FuncType func, Data &status){ + boost::shared_lock lock(mutex); + return call(func, status, layers.rbegin(), layers.rend()); + } + template typename vector_type::reverse_iterator call_rev(FuncType func, Data &status){ + boost::shared_lock lock(mutex); + return call(func, status, layers.rbegin(), layers.rend()); + } + void destroy() { boost::unique_lock lock(mutex); layers.clear(); } + +public: + virtual void add(const VectorMemberSharedPtr &l) { boost::unique_lock lock(mutex); layers.push_back(l); } + + template bool callFunc(FuncType func, Data &status){ + boost::shared_lock lock(mutex); + return call(func, status, layers.begin(), layers.end()) == layers.end(); + } +private: + vector_type layers; + boost::shared_mutex mutex; + + template Iterator call(FuncType func, Data &status, const Iterator &begin, const Iterator &end){ + bool okay_on_start = status.template bounded(); + + for(Iterator it = begin; it != end; ++it){ + ((**it).*func)(status); + if(okay_on_start && !status.template bounded()){ + return it; + } + } + return end; + } + template Iterator call(FuncType func, Data &status, const Iterator &begin, const Iterator &end){ + return call(func, status, begin, end); + } +}; + +template class LayerGroup : public Layer, public VectorHelper { +protected: + template void call_or_fail(FuncType func, FailType fail, Data &status){ + this->template call(func, status); + if(!status.template bounded()){ + this->template call(fail, status); + (this->*fail)(status); + } + } + template void call_or_fail_rev(FuncType func, FailType fail, Data &status){ + this->template call_rev(func, status); + if(!status.template bounded()){ + this->template call_rev(fail, status); + (this->*fail)(status); + } + } + + virtual void handleRead(LayerStatus &status, const LayerState ¤t_state) { + this->template call_or_fail(&Layer::read, &Layer::halt, status); + } + virtual void handleWrite(LayerStatus &status, const LayerState ¤t_state) { + this->template call_or_fail(&Layer::write, &Layer::halt, status); + } + + virtual void handleDiag(LayerReport &report) { this->template call(&Layer::diag, report); } + + virtual void handleInit(LayerStatus &status) { this->template call(&Layer::init, status); } + virtual void handleShutdown(LayerStatus &status) { this->template call(&Layer::shutdown, status); } + + virtual void handleHalt(LayerStatus &status) { this->template call(&Layer::halt, status); } + virtual void handleRecover(LayerStatus &status) { this->template call(&Layer::recover, status); } +public: + LayerGroup(const std::string &n) : Layer(n) {} +}; + +class LayerStack : public LayerGroup<>{ + +protected: + virtual void handleWrite(LayerStatus &status, const LayerState ¤t_state) { call_or_fail_rev(&Layer::write, &Layer::halt, status);} + virtual void handleShutdown(LayerStatus &status) { call_rev(&Layer::shutdown, status); } +public: + LayerStack(const std::string &n) : LayerGroup(n) {} +}; + +template class LayerGroupNoDiag : public LayerGroup{ +public: + LayerGroupNoDiag(const std::string &n) : LayerGroup(n) {} + virtual void handleDiag(LayerReport &report) {} +}; + +template class DiagGroup : public VectorHelper{ + typedef VectorHelper V; +public: + virtual void diag(LayerReport &report){ + this->template call(&Layer::diag, report); + } +}; + + + +} // namespace canopen + +#endif diff --git a/Devices/Libraries/Ros/ros_canopen/canopen_master/include/canopen_master/objdict.h b/Devices/Libraries/Ros/ros_canopen/canopen_master/include/canopen_master/objdict.h new file mode 100755 index 0000000..d03f262 --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/canopen_master/include/canopen_master/objdict.h @@ -0,0 +1,587 @@ +#ifndef H_OBJDICT +#define H_OBJDICT + +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include "exceptions.h" + +namespace canopen{ + +class TypeGuard{ + const std::type_info& (*get_type)(); + size_t type_size; + + template class TypeInfo{ + public: + static const std::type_info& id() { return typeid(T); } + }; + TypeGuard(const std::type_info& (*ti)(), const size_t s): get_type(ti), type_size(s) {} +public: + + template bool is_type() const { + return valid() && get_type() == typeid(T); + } + + bool operator==(const TypeGuard &other) const { + return valid() && other.valid() && (get_type() == other.get_type()); + } + + TypeGuard(): get_type(0), type_size(0) {} + bool valid() const { return get_type != 0; } + size_t get_size() const { return type_size; } + template static TypeGuard create() { return TypeGuard(TypeInfo::id, sizeof(T)); } +}; + +class String: public std::vector{ +public: + String() {} + String(const std::string &str) : std::vector(str.begin(), str.end()) {} + operator const char * () const { + return &at(0); + } + operator const std::string () const { + return std::string(begin(), end()); + } +}; + +class HoldAny{ + String buffer; + TypeGuard type_guard; + bool empty; +public: + HoldAny() : empty(true) {} + + const TypeGuard& type() const{ return type_guard; } + + template HoldAny(const T &t) : type_guard(TypeGuard::create()), empty(false){ + buffer.resize(sizeof(T)); + *(T*)&(buffer.front()) = t; + } + HoldAny(const std::string &t): type_guard(TypeGuard::create()), empty(false){ + if(!type_guard.is_type()){ + BOOST_THROW_EXCEPTION(std::bad_cast()); + } + buffer = t; + } + HoldAny(const TypeGuard &t): type_guard(t), empty(true){ } + + bool is_empty() const { return empty; } + + const String& data() const { + if(empty){ + BOOST_THROW_EXCEPTION(std::length_error("buffer empty")); + } + return buffer; + } + + template const T & get() const{ + if(!type_guard.is_type()){ + BOOST_THROW_EXCEPTION(std::bad_cast()); + }else if(empty){ + BOOST_THROW_EXCEPTION(std::length_error("buffer empty")); + } + return *(T*)&(buffer.front()); + } +}; + +template<> const String & HoldAny::get() const; + +struct DeviceInfo{ + std::string vendor_name; + uint32_t vendor_number; + std::string product_name; + uint32_t product_number; + uint32_t revision_number; + std::string order_code; + std::unordered_set baudrates; + bool simple_boot_up_master; + bool simple_boot_up_slave; + uint8_t granularity; + bool dynamic_channels_supported; + bool group_messaging; + uint16_t nr_of_rx_pdo; + uint16_t nr_of_tx_pdo; + bool lss_supported; + std::unordered_set dummy_usage; +}; + +#define THROW_WITH_KEY(e,k) BOOST_THROW_EXCEPTION(boost::enable_error_info(e) << ObjectDict::key_info(k)) + +class ObjectDict{ +protected: +public: + class Key{ + static size_t fromString(const std::string &str); + public: + const size_t hash; + Key(const uint16_t i) : hash((i<<16)| 0xFFFF) {} + Key(const uint16_t i, const uint8_t s): hash((i<<16)| s) {} + Key(const std::string &str): hash(fromString(str)) {} + bool hasSub() const { return (hash & 0xFFFF) != 0xFFFF; } + uint8_t sub_index() const { return hash & 0xFFFF; } + uint16_t index() const { return hash >> 16;} + bool operator==(const Key &other) const { return hash == other.hash; } + operator std::string() const; + }; + struct KeyHash { + std::size_t operator()(const Key& k) const { return k.hash; } + }; + + enum Code{ + NULL_DATA = 0x00, + DOMAIN_DATA = 0x02, + DEFTYPE = 0x05, + DEFSTRUCT = 0x06, + VAR = 0x07, + ARRAY = 0x08, + RECORD = 0x09 + }; + enum DataTypes{ + DEFTYPE_INTEGER8 = 0x0002, + DEFTYPE_INTEGER16 = 0x0003, + DEFTYPE_INTEGER32 = 0x0004, + DEFTYPE_UNSIGNED8 = 0x0005, + DEFTYPE_UNSIGNED16 = 0x0006, + DEFTYPE_UNSIGNED32 = 0x0007, + DEFTYPE_REAL32 = 0x0008, + DEFTYPE_VISIBLE_STRING = 0x0009, + DEFTYPE_OCTET_STRING = 0x000A, + DEFTYPE_UNICODE_STRING = 0x000B, + DEFTYPE_DOMAIN = 0x000F, + DEFTYPE_REAL64 = 0x0010, + DEFTYPE_INTEGER64 = 0x0015, + DEFTYPE_UNSIGNED64 = 0x001B + }; + struct Entry{ + Code obj_code; + uint16_t index; + uint8_t sub_index; + uint16_t data_type; + bool constant; + bool readable; + bool writable; + bool mappable; + std::string desc; + HoldAny def_val; + HoldAny init_val; + + Entry() {} + + Entry(const Code c, const uint16_t i, const uint16_t t, const std::string & d, const bool r = true, const bool w = true, bool m = false, const HoldAny def = HoldAny(), const HoldAny init = HoldAny()): + obj_code(c), index(i), sub_index(0),data_type(t),readable(r), writable(w), mappable(m), desc(d), def_val(def), init_val(init) {} + + Entry(const uint16_t i, const uint8_t s, const uint16_t t, const std::string & d, const bool r = true, const bool w = true, bool m = false, const HoldAny def = HoldAny(), const HoldAny init = HoldAny()): + obj_code(VAR), index(i), sub_index(s),data_type(t),readable(r), writable(w), mappable(m), desc(d), def_val(def), init_val(init) {} + + operator Key() const { return Key(index, sub_index); } + const HoldAny & value() const { return !init_val.is_empty() ? init_val : def_val; } + + }; + typedef std::shared_ptr EntryConstSharedPtr; + + const Entry& operator()(uint16_t i) const{ + return *at(Key(i)); + } + const Entry& operator()(uint16_t i, uint8_t s) const{ + return *at(Key(i,s)); + } + const EntryConstSharedPtr& get(const Key &k) const{ + return at(k); + } + bool has(uint16_t i, uint8_t s) const{ + return dict_.find(Key(i,s)) != dict_.end(); + } + bool has(uint16_t i) const{ + return dict_.find(Key(i)) != dict_.end(); + } + bool has(const Key &k) const{ + return dict_.find(k) != dict_.end(); + } + + bool insert(bool is_sub, EntryConstSharedPtr e){ + return dict_.insert(std::make_pair(is_sub?Key(e->index,e->sub_index):Key(e->index),e)).second; + } + + typedef std::unordered_map ObjectDictMap; + bool iterate(ObjectDictMap::const_iterator &it) const; + typedef std::list > Overlay; + typedef std::shared_ptr ObjectDictSharedPtr; + static ObjectDictSharedPtr fromFile(const std::string &path, const Overlay &overlay = Overlay()); + const DeviceInfo device_info; + + ObjectDict(const DeviceInfo &info): device_info(info) {} + typedef boost::error_info key_info; +protected: + const EntryConstSharedPtr& at(const Key &key) const{ + try{ + return dict_.at(key); + } + catch(const std::out_of_range &e){ + THROW_WITH_KEY(e, key); + } + } + + ObjectDictMap dict_; +}; +typedef ObjectDict::ObjectDictSharedPtr ObjectDictSharedPtr; +typedef std::shared_ptr ObjectDictConstSharedPtr; + +[[deprecated]] +std::size_t hash_value(ObjectDict::Key const& k); + +template class NodeIdOffset{ + T offset; + T (*adder)(const uint8_t &, const T &); + + static T add(const uint8_t &u, const T &t) { + return u+t; + } +public: + NodeIdOffset(const T &t): offset(t), adder(add) {} + + static const T apply(const HoldAny &val, const uint8_t &u){ + if(!val.is_empty()){ + if(TypeGuard::create() == val.type() ){ + return val.get(); + }else{ + const NodeIdOffset &no = val.get< NodeIdOffset >(); + return no.adder(u, no.offset); + } + }else{ + BOOST_THROW_EXCEPTION(std::bad_cast()); + } + + } +}; + +template std::ostream& operator<<(std::ostream& stream, const NodeIdOffset &n) { + //stream << "Offset: " << n.apply(0); + return stream; + } +std::ostream& operator<<(std::ostream& stream, const ObjectDict::Key &k); + +class AccessException : public Exception{ +public: + AccessException(const std::string &w) : Exception(w) {} +}; + + +class ObjectStorage{ +public: + using ReadFunc = std::function; + using ReadDelegate [[deprecated("use ReadFunc instead")]] = can::DelegateHelper; + + using WriteFunc = std::function; + using WriteDelegate [[deprecated("use WriteFunc instead")]] = can::DelegateHelper; + + typedef std::shared_ptr ObjectStorageSharedPtr; + +protected: + class Data { + Data(const Data&) = delete; // prevent copies + Data& operator=(const Data&) = delete; + + boost::mutex mutex; + String buffer; + bool valid; + + ReadFunc read_delegate; + WriteFunc write_delegate; + + template T & access(){ + if(!valid){ + THROW_WITH_KEY(std::length_error("buffer not valid"), key); + } + return *(T*)&(buffer.front()); + } + template T & allocate(){ + if(!valid){ + buffer.resize(sizeof(T)); + valid = true; + } + return access(); + } + public: + const TypeGuard type_guard; + const ObjectDict::EntryConstSharedPtr entry; + const ObjectDict::Key key; + size_t size() { boost::mutex::scoped_lock lock(mutex); return buffer.size(); } + + template Data(const ObjectDict::Key &k, const ObjectDict::EntryConstSharedPtr &e, const T &val, const ReadFunc &r, const WriteFunc &w) + : valid(false), read_delegate(r), write_delegate(w), type_guard(TypeGuard::create()), entry(e), key(k){ + assert(r); + assert(w); + assert(e); + allocate() = val; + } + Data(const ObjectDict::Key &k, const ObjectDict::EntryConstSharedPtr &e, const TypeGuard &t, const ReadFunc &r, const WriteFunc &w) + : valid(false), read_delegate(r), write_delegate(w), type_guard(t), entry(e), key(k){ + assert(r); + assert(w); + assert(e); + assert(t.valid()); + buffer.resize(t.get_size()); + } + void set_delegates(const ReadFunc &r, const WriteFunc &w){ + boost::mutex::scoped_lock lock(mutex); + if(r) read_delegate = r; + if(w) write_delegate = w; + } + template const T get(bool cached) { + boost::mutex::scoped_lock lock(mutex); + + if(!entry->readable){ + THROW_WITH_KEY(AccessException("no read access"), key); + + } + + if(entry->constant) cached = true; + + if(!valid || !cached){ + allocate(); + read_delegate(*entry, buffer); + } + return access(); + } + template void set(const T &val) { + boost::mutex::scoped_lock lock(mutex); + + if(!entry->writable){ + if(access() != val){ + THROW_WITH_KEY(AccessException("no write access"), key); + } + }else{ + allocate() = val; + write_delegate(*entry, buffer); + } + } + template void set_cached(const T &val) { + boost::mutex::scoped_lock lock(mutex); + if(!valid || val != access() ){ + if(!entry->writable){ + THROW_WITH_KEY(AccessException("no write access and not cached"), key); + }else{ + allocate() = val; + write_delegate(*entry, buffer); + } + } + } + void init(); + void reset(); + void force_write(); + + }; + typedef std::shared_ptr DataSharedPtr; +public: + template struct DataType{ + typedef void type; + }; + + template class Entry{ + DataSharedPtr data; + public: + typedef T type; + bool valid() const { return data != 0; } + const T get() { + if(!data) BOOST_THROW_EXCEPTION( PointerInvalid("ObjectStorage::Entry::get()") ); + + return data->get(false); + } + bool get(T & val){ + try{ + val = get(); + return true; + }catch(...){ + return false; + } + } + const T get_cached() { + if(!data) BOOST_THROW_EXCEPTION( PointerInvalid("ObjectStorage::Entry::get_cached()") ); + + return data->get(true); + } + bool get_cached(T & val){ + try{ + val = get_cached(); + return true; + }catch(...){ + return false; + } + } + void set(const T &val) { + if(!data) BOOST_THROW_EXCEPTION( PointerInvalid("ObjectStorage::Entry::set(val)") ); + data->set(val); + } + bool set_cached(const T &val) { + if(!data) return false; + try{ + data->set_cached(val); + return true; + }catch(...){ + return false; + } + } + + Entry() {} + Entry(DataSharedPtr &d) + : data(d){ + assert(data); + } + Entry(ObjectStorageSharedPtr storage, uint16_t index) + : data(storage->entry(index).data) { + assert(data); + } + Entry(ObjectStorageSharedPtr storage, uint16_t index, uint8_t sub_index) + : data(storage->entry(index, sub_index).data) { + assert(data); + } + Entry(ObjectStorageSharedPtr storage, const ObjectDict::Key &k) + : data(storage->entry(k).data) { + assert(data); + } + const ObjectDict::Entry & desc() const{ + return *(data->entry); + } + }; + + void reset(); + +protected: + typedef std::unordered_map ObjectStorageMap; + ObjectStorageMap storage_; + boost::mutex mutex_; + + void init_nolock(const ObjectDict::Key &key, const ObjectDict::EntryConstSharedPtr &entry); + + ReadFunc read_delegate_; + WriteFunc write_delegate_; + size_t map(const ObjectDict::EntryConstSharedPtr &e, const ObjectDict::Key &key, const ReadFunc & read_delegate, const WriteFunc & write_delegate); +public: + template Entry entry(const ObjectDict::Key &key){ + boost::mutex::scoped_lock lock(mutex_); + + ObjectStorageMap::iterator it = storage_.find(key); + + if(it == storage_.end()){ + const ObjectDict::EntryConstSharedPtr e = dict_->get(key); + + DataSharedPtr data; + TypeGuard type = TypeGuard::create(); + + if(!e->def_val.is_empty()){ + T val = NodeIdOffset::apply(e->def_val, node_id_); + data = std::make_shared(key, e,val, read_delegate_, write_delegate_); + }else{ + if(!e->def_val.type().valid() || e->def_val.type() == type) { + data = std::make_shared(key,e,type, read_delegate_, write_delegate_); + }else{ + THROW_WITH_KEY(std::bad_cast(), key); + } + } + + std::pair ok = storage_.insert(std::make_pair(key, data)); + it = ok.first; + } + + if(!it->second->type_guard.is_type()){ + THROW_WITH_KEY(std::bad_cast(), key); + } + return Entry(it->second); + } + + size_t map(uint16_t index, uint8_t sub_index, const ReadFunc & read_delegate, const WriteFunc & write_delegate); + + template Entry entry(uint16_t index){ + return entry(ObjectDict::Key(index)); + } + template Entry entry(uint16_t index, uint8_t sub_index){ + return entry(ObjectDict::Key(index,sub_index)); + } + + template void entry(Entry &e, uint16_t index){ // TODO: migrate to bool + e = entry(ObjectDict::Key(index)); + } + template void entry(Entry &e, uint16_t index, uint8_t sub_index){ // TODO: migrate to bool + e = entry(ObjectDict::Key(index,sub_index)); + } + template bool entry(Entry &e, const ObjectDict::Key &k){ + try{ + e = entry(k); + return true; + }catch(...){ + return false; + } + } + typedef std::function ReadStringFuncType; + ReadStringFuncType getStringReader(const ObjectDict::Key &key, bool cached = false); + typedef std::function WriteStringFuncType; + WriteStringFuncType getStringWriter(const ObjectDict::Key &key, bool cached = false); + + const ObjectDictConstSharedPtr dict_; + const uint8_t node_id_; + + ObjectStorage(ObjectDictConstSharedPtr dict, uint8_t node_id, ReadFunc read_delegate, WriteFunc write_delegate); + + void init(const ObjectDict::Key &key); + void init_all(); +}; +typedef ObjectStorage::ObjectStorageSharedPtr ObjectStorageSharedPtr; + +template<> String & ObjectStorage::Data::access(); +template<> String & ObjectStorage::Data::allocate(); + +template<> struct ObjectStorage::DataType { typedef int8_t type;}; +template<> struct ObjectStorage::DataType { typedef int16_t type;}; +template<> struct ObjectStorage::DataType { typedef int32_t type;}; +template<> struct ObjectStorage::DataType { typedef int64_t type;}; +template<> struct ObjectStorage::DataType { typedef uint8_t type;}; + +template<> struct ObjectStorage::DataType { typedef uint16_t type;}; +template<> struct ObjectStorage::DataType { typedef uint32_t type;}; +template<> struct ObjectStorage::DataType { typedef uint64_t type;}; + +template<> struct ObjectStorage::DataType { typedef float type;}; +template<> struct ObjectStorage::DataType { typedef double type;}; + +template<> struct ObjectStorage::DataType { typedef String type;}; +template<> struct ObjectStorage::DataType { typedef String type;}; +template<> struct ObjectStorage::DataType { typedef String type;}; +template<> struct ObjectStorage::DataType { typedef String type;}; + +template static R *branch_type(const uint16_t data_type){ + switch(ObjectDict::DataTypes(data_type)){ + case ObjectDict::DEFTYPE_INTEGER8: return T::template func< ObjectDict::DEFTYPE_INTEGER8 >; + case ObjectDict::DEFTYPE_INTEGER16: return T::template func< ObjectDict::DEFTYPE_INTEGER16 >; + case ObjectDict::DEFTYPE_INTEGER32: return T::template func< ObjectDict::DEFTYPE_INTEGER32 >; + case ObjectDict::DEFTYPE_INTEGER64: return T::template func< ObjectDict::DEFTYPE_INTEGER64 >; + + case ObjectDict::DEFTYPE_UNSIGNED8: return T::template func< ObjectDict::DEFTYPE_UNSIGNED8 >; + case ObjectDict::DEFTYPE_UNSIGNED16: return T::template func< ObjectDict::DEFTYPE_UNSIGNED16 >; + case ObjectDict::DEFTYPE_UNSIGNED32: return T::template func< ObjectDict::DEFTYPE_UNSIGNED32 >; + case ObjectDict::DEFTYPE_UNSIGNED64: return T::template func< ObjectDict::DEFTYPE_UNSIGNED64 >; + + case ObjectDict::DEFTYPE_REAL32: return T::template func< ObjectDict::DEFTYPE_REAL32 >; + case ObjectDict::DEFTYPE_REAL64: return T::template func< ObjectDict::DEFTYPE_REAL64 >; + + case ObjectDict::DEFTYPE_VISIBLE_STRING: return T::template func< ObjectDict::DEFTYPE_VISIBLE_STRING >; + case ObjectDict::DEFTYPE_OCTET_STRING: return T::template func< ObjectDict::DEFTYPE_OCTET_STRING >; + case ObjectDict::DEFTYPE_UNICODE_STRING: return T::template func< ObjectDict::DEFTYPE_UNICODE_STRING >; + case ObjectDict::DEFTYPE_DOMAIN: return T::template func< ObjectDict::DEFTYPE_DOMAIN >; + + default: + throw std::bad_cast(); + } +} + +} // canopen + +#endif // !H_OBJDICT diff --git a/Devices/Libraries/Ros/ros_canopen/canopen_master/include/canopen_master/timer.h b/Devices/Libraries/Ros/ros_canopen/canopen_master/include/canopen_master/timer.h new file mode 100755 index 0000000..0853a50 --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/canopen_master/include/canopen_master/timer.h @@ -0,0 +1,75 @@ +#ifndef H_CANOPEN_TIMER +#define H_CANOPEN_TIMER + +#include +#include + +#include +#include +#include + +#include + +namespace canopen{ + +class Timer{ +public: + using TimerFunc = std::function; + using TimerDelegate [[deprecated("use TimerFunc instead")]] = can::DelegateHelper; + + Timer():work(io), timer(io),thread(std::bind( + static_cast(&boost::asio::io_service::run), &io)) + { + } + + void stop(){ + boost::mutex::scoped_lock lock(mutex); + timer.cancel(); + } + template void start(const TimerFunc &del, const T &dur, bool start_now = true){ + boost::mutex::scoped_lock lock(mutex); + delegate = del; + period = boost::chrono::duration_cast(dur); + if(start_now){ + timer.expires_from_now(period); + timer.async_wait(std::bind(&Timer::handler, this, std::placeholders::_1)); + } + } + void restart(){ + boost::mutex::scoped_lock lock(mutex); + timer.expires_from_now(period); + timer.async_wait(std::bind(&Timer::handler, this, std::placeholders::_1)); + } + const boost::chrono::high_resolution_clock::duration & getPeriod(){ + boost::mutex::scoped_lock lock(mutex); + return period; + } + ~Timer(){ + io.stop(); + thread.join(); + } + +private: + boost::asio::io_service io; + boost::asio::io_service::work work; + boost::asio::basic_waitable_timer timer; + boost::chrono::high_resolution_clock::duration period; + boost::mutex mutex; + boost::thread thread; + + TimerFunc delegate; + void handler(const boost::system::error_code& ec){ + if(!ec){ + boost::mutex::scoped_lock lock(mutex); + if(delegate && delegate()){ + timer.expires_at(timer.expires_at() + period); + timer.async_wait(std::bind(&Timer::handler, this, std::placeholders::_1)); + } + + } + } +}; + +} + +#endif diff --git a/Devices/Libraries/Ros/ros_canopen/canopen_master/master_plugin.xml b/Devices/Libraries/Ros/ros_canopen/canopen_master/master_plugin.xml new file mode 100755 index 0000000..58a01e3 --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/canopen_master/master_plugin.xml @@ -0,0 +1,8 @@ + + + Allocator for SimpleMaster instances + + + Allocator for ExternalMaster instances + + diff --git a/Devices/Libraries/Ros/ros_canopen/canopen_master/package.xml b/Devices/Libraries/Ros/ros_canopen/canopen_master/package.xml new file mode 100755 index 0000000..125268e --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/canopen_master/package.xml @@ -0,0 +1,30 @@ + + + canopen_master + 0.8.5 + CiA(r) CANopen 301 master implementation with support for interprocess master synchronisation. + + Mathias Lüdtke + Mathias Lüdtke + + LGPLv3 + + http://wiki.ros.org/canopen_master + https://github.com/ros-industrial/ros_canopen + https://github.com/ros-industrial/ros_canopen/issues + + catkin + + libboost-dev + libboost-atomic-dev + libboost-chrono-dev + libboost-thread-dev + class_loader + socketcan_interface + + rosunit + + + + + diff --git a/Devices/Libraries/Ros/ros_canopen/canopen_master/src/bcm_sync.cpp b/Devices/Libraries/Ros/ros_canopen/canopen_master/src/bcm_sync.cpp new file mode 100755 index 0000000..db915e7 --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/canopen_master/src/bcm_sync.cpp @@ -0,0 +1,68 @@ +#include +#include +#include + +int main(int argc, char** argv){ + + if(argc < 4){ + std::cout << "Usage: " << argv[0] << " DEVICE PERIOD_MS HEADER [OVERFLOW] [+ID*] [-ID*] [--]" << std::endl; + return 1; + } + + std::string can_device = argv[1]; + int sync_ms = atoi(argv[2]); + can::Header header = can::toheader(argv[3]); + + if(!header.isValid()){ + std::cout << "header is invalid" << std::endl; + return 1; + } + int sync_overflow = 0; + + int start = 4; + if(argc > start && argv[start][0] != '-' && argv[start][0] != '+'){ + sync_overflow = atoi(argv[4]); + if(sync_overflow == 1 || sync_overflow < 0 || sync_overflow > 240){ + std::cout << "sync overflow is invalid" << std::endl; + return 1; + } + ++start; + } + + std::set monitored, ignored; + + for(; argc > start; ++start){ + if(strncmp("--", argv[start], 2) == 0) break; + int id = atoi(argv[start]); + + if(id > 0 && id < 128) monitored.insert(id); + else if (id < 0 && id > -128) ignored.insert(-id); + else{ + std::cout << "ID is invalid: " << id << std::endl; + return 1; + } + } + + can::SocketCANDriverSharedPtr driver = std::make_shared(); + if(!driver->init(can_device, false, can::NoSettings::create())){ + std::cout << "Could not initialize CAN" << std::endl; + return 1; + } + + canopen::SyncProperties sync_properties(header, sync_ms, sync_overflow); + canopen::BCMsync sync(can_device, driver, sync_properties); + + sync.setMonitored(monitored); + sync.setIgnored(ignored); + + canopen::LayerStatus status; + sync.init(status); + if(!status.bounded()){ + std::cout << "Could not initialize sync" << std::endl; + return 1; + } + + driver->run(); + + return 0; +} diff --git a/Devices/Libraries/Ros/ros_canopen/canopen_master/src/emcy.cpp b/Devices/Libraries/Ros/ros_canopen/canopen_master/src/emcy.cpp new file mode 100755 index 0000000..7e509fc --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/canopen_master/src/emcy.cpp @@ -0,0 +1,148 @@ +#include +#include + +using namespace canopen; + +#pragma pack(push) /* push current alignment to stack */ +#pragma pack(1) /* set alignment to 1 byte boundary */ + +struct EMCYid{ + uint32_t id:29; + uint32_t extended:1; + uint32_t :1; + uint32_t invalid:1; + EMCYid(uint32_t val){ + *(uint32_t*) this = val; + } + can::Header header() { + return can::Header(id, extended, false, false); + } + const uint32_t get() const { return *(uint32_t*) this; } +}; + +struct EMCYfield{ + uint32_t error_code:16; + uint32_t addition_info:16; + EMCYfield(uint32_t val){ + *(uint32_t*) this = val; + } +}; + +struct EMCYmsg{ + uint16_t error_code; + uint8_t error_register; + uint8_t manufacturer_specific_error_field[5]; + + struct Frame: public FrameOverlay{ + Frame(const can::Frame &f) : FrameOverlay(f){ } + }; +}; + +#pragma pack(pop) /* pop previous alignment from stack */ + +void EMCYHandler::handleEMCY(const can::Frame & msg){ + EMCYmsg::Frame em(msg); + if (em.data.error_code == 0) { + ROSCANOPEN_INFO("canopen_master", "EMCY reset: " << msg); + } else { + ROSCANOPEN_ERROR("canopen_master", "EMCY received: " << msg); + } + has_error_ = (em.data.error_register & ~32) != 0; +} + +EMCYHandler::EMCYHandler(const can::CommInterfaceSharedPtr interface, const ObjectStorageSharedPtr storage): Layer("EMCY handler"), storage_(storage), has_error_(true){ + storage_->entry(error_register_, 0x1001); + try{ + storage_->entry(num_errors_, 0x1003,0); + } + catch(...){ + // pass, 1003 is optional + } + try{ + EMCYid emcy_id(storage_->entry(0x1014).get_cached()); + emcy_listener_ = interface->createMsgListenerM(emcy_id.header(), this, &EMCYHandler::handleEMCY); + + + } + catch(...){ + // pass, EMCY is optional + } +} + +void EMCYHandler::handleRead(LayerStatus &status, const LayerState ¤t_state) { + if(current_state == Ready){ + if(has_error_){ + status.error("Node has emergency error"); + } + } +} +void EMCYHandler::handleWrite(LayerStatus &status, const LayerState ¤t_state) { + // noithing to do +} + +void EMCYHandler::handleDiag(LayerReport &report){ + uint8_t error_register = 0; + if(!error_register_.get(error_register)){ + report.error("Could not read error error_register"); + return; + } + + if(error_register){ + if(error_register & 1){ // first bit should be set on all errors + report.error("Node has emergency error"); + }else if(error_register & ~32){ // filter profile-specific bit + report.warn("Error register is not zero"); + } + report.add("error_register", (uint32_t) error_register); + + uint8_t num = num_errors_.valid() ? num_errors_.get() : 0; + std::stringstream buf; + for(size_t i = 0; i < num; ++i) { + if( i!= 0){ + buf << ", "; + } + try{ + ObjectStorage::Entry error; + storage_->entry(error, 0x1003,i+1); + EMCYfield field(error.get()); + buf << std::hex << field.error_code << "#" << field.addition_info; + } + catch (const std::out_of_range & e){ + buf << "NOT_IN_DICT!"; + } + catch (const TimeoutException & e){ + buf << "LIST_UNDERFLOW!"; + break; + } + + } + report.add("errors", buf.str()); + + } +} +void EMCYHandler::handleInit(LayerStatus &status){ + uint8_t error_register = 0; + if(!error_register_.get(error_register)){ + status.error("Could not read error error_register"); + return; + }else if(error_register & 1){ + ROSCANOPEN_ERROR("canopen_master", "error register: " << int(error_register)); + status.error("Node has emergency error"); + return; + } + + resetErrors(status); +} +void EMCYHandler::resetErrors(LayerStatus &status){ + if(num_errors_.valid()) num_errors_.set(0); + has_error_ = false; +} + +void EMCYHandler::handleRecover(LayerStatus &status){ + handleInit(status); +} +void EMCYHandler::handleShutdown(LayerStatus &status){ +} +void EMCYHandler::handleHalt(LayerStatus &status){ + // do nothing +} diff --git a/Devices/Libraries/Ros/ros_canopen/canopen_master/src/master_plugin.cpp b/Devices/Libraries/Ros/ros_canopen/canopen_master/src/master_plugin.cpp new file mode 100755 index 0000000..8f221bf --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/canopen_master/src/master_plugin.cpp @@ -0,0 +1,138 @@ +#include +#include +#include + +#include + +namespace canopen { + +class ManagingSyncLayer: public SyncLayer { +protected: + can::CommInterfaceSharedPtr interface_; + boost::chrono::milliseconds step_, half_step_; + + std::set nodes_; + boost::mutex nodes_mutex_; + std::atomic nodes_size_; + + virtual void handleShutdown(LayerStatus &status) { + } + + virtual void handleHalt(LayerStatus &status) { /* nothing to do */ } + virtual void handleDiag(LayerReport &report) { /* TODO */ } + virtual void handleRecover(LayerStatus &status) { /* TODO */ } + +public: + ManagingSyncLayer(const SyncProperties &p, can::CommInterfaceSharedPtr interface) + : SyncLayer(p), interface_(interface), step_(p.period_ms_), half_step_(p.period_ms_/2), nodes_size_(0) + { + } + + virtual void addNode(void * const ptr) { + boost::mutex::scoped_lock lock(nodes_mutex_); + nodes_.insert(ptr); + nodes_size_ = nodes_.size(); + } + virtual void removeNode(void * const ptr) { + boost::mutex::scoped_lock lock(nodes_mutex_); + nodes_.erase(ptr); + nodes_size_ = nodes_.size(); + } +}; + + +class SimpleSyncLayer: public ManagingSyncLayer { + time_point read_time_, write_time_; + can::Frame frame_; + uint8_t overflow_; + + void resetCounter(){ + frame_.data[0] = 1; // SYNC counter starts at 1 + } + void tryUpdateCounter(){ + if (frame_.dlc > 0) { // sync counter is used + if (frame_.data[0] >= overflow_) { + resetCounter(); + }else{ + ++frame_.data[0]; + } + } + } +protected: + virtual void handleRead(LayerStatus &status, const LayerState ¤t_state) { + if(current_state > Init){ + boost::this_thread::sleep_until(read_time_); + write_time_ += step_; + } + } + virtual void handleWrite(LayerStatus &status, const LayerState ¤t_state) { + if(current_state > Init){ + boost::this_thread::sleep_until(write_time_); + tryUpdateCounter(); + if(nodes_size_){ //) + interface_->send(frame_); + } + read_time_ = get_abs_time(half_step_); + } + } + + virtual void handleInit(LayerStatus &status){ + write_time_ = get_abs_time(step_); + read_time_ = get_abs_time(half_step_); + } +public: + SimpleSyncLayer(const SyncProperties &p, can::CommInterfaceSharedPtr interface) + : ManagingSyncLayer(p, interface), frame_(p.header_, 0), overflow_(p.overflow_) { + if(overflow_ == 1 || overflow_ > 240){ + BOOST_THROW_EXCEPTION(Exception("SYNC counter overflow is invalid")); + }else if(overflow_ > 1){ + frame_.dlc = 1; + resetCounter(); + } + } +}; + +class ExternalSyncLayer: public ManagingSyncLayer { + can::BufferedReader reader_; +protected: + virtual void handleRead(LayerStatus &status, const LayerState ¤t_state) { + can::Frame msg; + if(current_state > Init){ + if(reader_.readUntil(&msg, get_abs_time(step_))){ // wait for sync + boost::this_thread::sleep_until(get_abs_time(half_step_)); // shift readout to middle of period + } + } + } + virtual void handleWrite(LayerStatus &status, const LayerState ¤t_state) { + // nothing to do here + } + virtual void handleInit(LayerStatus &status){ + reader_.listen(interface_, properties.header_); + } +public: + ExternalSyncLayer(const SyncProperties &p, can::CommInterfaceSharedPtr interface) + : ManagingSyncLayer(p, interface), reader_(true,1) {} +}; + + +template class WrapMaster: public Master{ + can::CommInterfaceSharedPtr interface_; +public: + virtual SyncLayerSharedPtr getSync(const SyncProperties &properties){ + return std::make_shared(properties, interface_); + } + WrapMaster(can::CommInterfaceSharedPtr interface) : interface_(interface) {} + + class Allocator : public Master::Allocator{ + public: + virtual MasterSharedPtr allocate(const std::string &name, can::CommInterfaceSharedPtr interface){ + return std::make_shared(interface); + } + }; +}; + +typedef WrapMaster SimpleMaster; +typedef WrapMaster ExternalMaster; +} +CLASS_LOADER_REGISTER_CLASS(canopen::SimpleMaster::Allocator, canopen::Master::Allocator); +CLASS_LOADER_REGISTER_CLASS(canopen::ExternalMaster::Allocator, canopen::Master::Allocator); diff --git a/Devices/Libraries/Ros/ros_canopen/canopen_master/src/node.cpp b/Devices/Libraries/Ros/ros_canopen/canopen_master/src/node.cpp new file mode 100755 index 0000000..21edbd0 --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/canopen_master/src/node.cpp @@ -0,0 +1,218 @@ +#include + +using namespace canopen; + +#pragma pack(push) /* push current alignment to stack */ +#pragma pack(1) /* set alignment to 1 byte boundary */ + + +struct NMTcommand{ + enum Command{ + Start = 1, + Stop = 2, + Prepare = 128, + Reset = 129, + Reset_Com = 130 + }; + uint8_t command; + uint8_t node_id; + + struct Frame: public FrameOverlay{ + Frame(uint8_t node_id, const Command &c) : FrameOverlay(can::Header()) { + data.command = c; + data.node_id = node_id; + } + }; +}; + +#pragma pack(pop) /* pop previous alignment from stack */ + +Node::Node(const can::CommInterfaceSharedPtr interface, const ObjectDictSharedPtr dict, uint8_t node_id, const SyncCounterSharedPtr sync) +: Layer("Node 301"), node_id_(node_id), interface_(interface), sync_(sync) , state_(Unknown), sdo_(interface, dict, node_id), pdo_(interface){ + try{ + getStorage()->entry(heartbeat_, 0x1017); + } + catch(const std::out_of_range){ + } +} + +const Node::State Node::getState(){ + boost::timed_mutex::scoped_lock lock(mutex); // TODO: timed lock? + return state_; +} + +bool Node::reset_com(){ + boost::timed_mutex::scoped_lock lock(mutex); // TODO: timed lock? + getStorage()->reset(); + interface_->send(NMTcommand::Frame(node_id_, NMTcommand::Reset_Com)); + if(wait_for(BootUp, boost::chrono::seconds(10)) != 1){ + return false; + } + state_ = PreOperational; + setHeartbeatInterval(); + return true; +} +bool Node::reset(){ + boost::timed_mutex::scoped_lock lock(mutex); // TODO: timed lock? + getStorage()->reset(); + + interface_->send(NMTcommand::Frame(node_id_, NMTcommand::Reset)); + if(wait_for(BootUp, boost::chrono::seconds(10)) != 1){ + return false; + } + state_ = PreOperational; + setHeartbeatInterval(); + return true; +} + +bool Node::prepare(){ + boost::timed_mutex::scoped_lock lock(mutex); // TODO: timed lock? + if(state_ == BootUp){ + // ERROR + } + interface_->send(NMTcommand::Frame(node_id_, NMTcommand::Prepare)); + return 0 != wait_for(PreOperational, boost::chrono::seconds(2)); +} +bool Node::start(){ + boost::timed_mutex::scoped_lock lock(mutex); // TODO: timed lock? + if(state_ == BootUp){ + // ERROR + } + interface_->send(NMTcommand::Frame(node_id_, NMTcommand::Start)); + return 0 != wait_for(Operational, boost::chrono::seconds(2)); +} +bool Node::stop(){ + boost::timed_mutex::scoped_lock lock(mutex); // TODO: timed lock? + if(sync_) sync_->removeNode(this); + if(state_ == BootUp){ + // ERROR + } + interface_->send(NMTcommand::Frame(node_id_, NMTcommand::Stop)); + return true; +} + +void Node::switchState(const uint8_t &s){ + bool changed = state_ != s; + switch(s){ + case Operational: + if(changed && sync_) sync_->addNode(this); + break; + case BootUp: + case PreOperational: + case Stopped: + if(changed && sync_) sync_->removeNode(this); + break; + default: + //error + ; + } + if(changed){ + state_ = (State) s; + state_dispatcher_.dispatch(state_); + cond.notify_one(); + } +} +void Node::handleNMT(const can::Frame & msg){ + boost::mutex::scoped_lock cond_lock(cond_mutex); + uint16_t interval = getHeartbeatInterval(); + if(interval) heartbeat_timeout_ = get_abs_time(boost::chrono::milliseconds(3*interval)); + assert(msg.dlc == 1); + switchState(msg.data[0]); +} +template int Node::wait_for(const State &s, const T &timeout){ + boost::mutex::scoped_lock cond_lock(cond_mutex); + time_point abs_time = get_abs_time(timeout); + + while(s != state_) { + if(cond.wait_until(cond_lock,abs_time) == boost::cv_status::timeout) + { + break; + } + } + if( s!= state_){ + if(getHeartbeatInterval() == 0){ + switchState(s); + return -1; + } + return 0; + } + return 1; +} +bool Node::checkHeartbeat(){ + if(getHeartbeatInterval() == 0) return true; //disabled + boost::mutex::scoped_lock cond_lock(cond_mutex); + return heartbeat_timeout_ >= boost::chrono::high_resolution_clock::now(); +} + + +void Node::handleRead(LayerStatus &status, const LayerState ¤t_state) { + if(current_state > Init){ + if(!checkHeartbeat()){ + status.error("heartbeat problem"); + } else if(getState() != Operational){ + status.error("not operational"); + } else{ + pdo_.read(status); + } + } +} +void Node::handleWrite(LayerStatus &status, const LayerState ¤t_state) { + if(current_state > Init){ + if(getState() != Operational) status.error("not operational"); + else if(! pdo_.write()) status.error("PDO write problem"); + } +} + + +void Node::handleDiag(LayerReport &report){ + State state = getState(); + if(state != Operational){ + report.error("Mode not operational"); + report.add("Node state", (int)state); + }else if(!checkHeartbeat()){ + report.error("Heartbeat timeout"); + } +} +void Node::handleInit(LayerStatus &status){ + nmt_listener_ = interface_->createMsgListenerM(can::MsgHeader(0x700 + node_id_), this, &Node::handleNMT); + + sdo_.init(); + try{ + if(!reset_com()) BOOST_THROW_EXCEPTION( TimeoutException("reset_timeout") ); + } + catch(const TimeoutException&){ + status.error(boost::str(boost::format("could not reset node '%1%'") % (int)node_id_)); + return; + } + + if(!pdo_.init(getStorage(), status)){ + return; + } + getStorage()->init_all(); + sdo_.init(); // reread SDO paramters; + // TODO: set SYNC data + + try{ + if(!start()) BOOST_THROW_EXCEPTION( TimeoutException("start timeout") ); + } + catch(const TimeoutException&){ + status.error(boost::str(boost::format("could not start node '%1%'") % (int)node_id_)); + } +} +void Node::handleRecover(LayerStatus &status){ + try{ + start(); + } + catch(const TimeoutException&){ + status.error(boost::str(boost::format("could not start node '%1%'") % (int)node_id_)); + } +} +void Node::handleShutdown(LayerStatus &status){ + if(getHeartbeatInterval()> 0) heartbeat_.set(0); + stop(); + nmt_listener_.reset(); + switchState(Unknown); +} +void Node::handleHalt(LayerStatus &status){ + // do nothing +} diff --git a/Devices/Libraries/Ros/ros_canopen/canopen_master/src/objdict.cpp b/Devices/Libraries/Ros/ros_canopen/canopen_master/src/objdict.cpp new file mode 100755 index 0000000..ea47938 --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/canopen_master/src/objdict.cpp @@ -0,0 +1,480 @@ +#include +#include +#include +#include +#include +#include + +namespace canopen{ + size_t hash_value(ObjectDict::Key const& k) { return k.hash; } + std::ostream& operator<<(std::ostream& stream, const ObjectDict::Key &k) { return stream << std::string(k); } +} + +using namespace canopen; + +template<> const String & HoldAny::get() const{ + return buffer; +} + +template<> String & ObjectStorage::Data::access(){ + if(!valid){ + THROW_WITH_KEY(std::length_error("buffer not valid") , key); + } + return buffer; +} +template<> String & ObjectStorage::Data::allocate(){ + buffer.resize(0); + valid = true; + return buffer; +} + +size_t ObjectDict::Key::fromString(const std::string &str){ + uint16_t index = 0; + uint8_t sub_index = 0; + int num = sscanf(str.c_str(),"%hxsub%hhx", &index, &sub_index); + return (index << 16) | (num==2?sub_index:0xFFFF); +} +ObjectDict::Key::operator std::string() const{ + std::stringstream sstr; + sstr << std::hex << index(); + if(hasSub()) sstr << "sub" << (int) sub_index(); + return sstr.str(); +} +void ObjectStorage::Data::init(){ + boost::mutex::scoped_lock lock(mutex); + + if(entry->init_val.is_empty()) return; + + if(valid && !entry->def_val.is_empty() && buffer != entry->def_val.data()) return; // buffer was changed + + if(!valid || buffer != entry->init_val.data()){ + buffer = entry->init_val.data(); + valid = true; + if(entry->writable && (entry->def_val.is_empty() || entry->init_val.data() != entry->def_val.data())) + write_delegate(*entry, buffer); + } +} +void ObjectStorage::Data::force_write(){ + boost::mutex::scoped_lock lock(mutex); + + if(!valid && entry->readable){ + read_delegate(*entry, buffer); + valid = true; + } + if(valid) write_delegate(*entry, buffer); +} + +void ObjectStorage::Data::reset(){ + boost::mutex::scoped_lock lock(mutex); + if(!entry->def_val.is_empty() && entry->def_val.type() == type_guard){ + buffer = entry->def_val.data(); + valid = true; + }else{ + valid = false; + } +} + +bool ObjectDict::iterate(ObjectDict::ObjectDictMap::const_iterator &it) const{ + if(it != ObjectDict::ObjectDictMap::const_iterator()){ + ++it; + }else it = dict_.begin(); + return it != dict_.end(); +} +void set_access( ObjectDict::Entry &entry, std::string access){ + boost::algorithm::to_lower(access); + entry.constant = false; + if(access == "ro"){ + entry.readable = true; + entry.writable = false; + }else if (access == "wo"){ + entry.readable = false; + entry.writable = true; + }else if (access == "rw"){ + entry.readable = true; + entry.writable = true; + }else if (access == "rwr"){ + entry.readable = true; + entry.writable = true; + }else if (access == "rww"){ + entry.readable = true; + entry.writable = true; + }else if (access == "const"){ + entry.readable = true; + entry.writable = false; + entry.constant = true; + }else{ + THROW_WITH_KEY(ParseException("Cannot determine access"), ObjectDict::Key(entry)); + } +} + +template T int_from_string(const std::string &s); + +template<> int8_t int_from_string(const std::string &s){ + return strtol(s.c_str(), 0, 0); +} +template<> uint8_t int_from_string(const std::string &s){ + return strtoul(s.c_str(), 0, 0); +} +template<> int16_t int_from_string(const std::string &s){ + return strtol(s.c_str(), 0, 0); +} +template<> uint16_t int_from_string(const std::string &s){ + return strtoul(s.c_str(), 0, 0); +} +template<> int32_t int_from_string(const std::string &s){ + return strtol(s.c_str(), 0, 0); +} +template<> uint32_t int_from_string(const std::string &s){ + return strtoul(s.c_str(), 0, 0); +} + +template<> int64_t int_from_string(const std::string &s){ + return strtoll(s.c_str(), 0, 0); +} +template<> uint64_t int_from_string(const std::string &s){ + return strtoull(s.c_str(), 0, 0); +} + +template HoldAny parse_int(boost::property_tree::iptree &pt, const std::string &key){ + if(pt.count(key) == 0) return HoldAny(TypeGuard::create()); + + std::string str = boost::trim_copy(pt.get(key)); + if(boost::istarts_with(str,"$NODEID")){ + return HoldAny(NodeIdOffset(int_from_string(boost::trim_copy(str.substr(str.find("+",7)+1))))); + }else if (boost::iends_with(str,"$NODEID")){ + return HoldAny(NodeIdOffset(int_from_string(boost::trim_copy(str.substr(0, str.find("+")+1))))); + }else return HoldAny(int_from_string(str)); +} + +template HoldAny parse_octets(boost::property_tree::iptree &pt, const std::string &key){ + std::string out; + if(pt.count(key) == 0 || can::hex2buffer(out,pt.get(key), true)) return HoldAny(TypeGuard::create()); + return HoldAny(T(out)); +} + +template HoldAny parse_typed_value(boost::property_tree::iptree &pt, const std::string &key){ + if(pt.count(key) == 0) return HoldAny(TypeGuard::create()); + return HoldAny(pt.get(key)); +} +template<> HoldAny parse_typed_value(boost::property_tree::iptree &pt, const std::string &key){ + if(pt.count(key) == 0) return HoldAny(TypeGuard::create()); + return HoldAny(String(pt.get(key))); +} + +struct ReadAnyValue{ + template static HoldAny func(boost::property_tree::iptree &pt, const std::string &key); + static HoldAny read_value(boost::property_tree::iptree &pt, uint16_t data_type, const std::string &key){ + return branch_type(data_type)(pt, key); + } +}; +template<> HoldAny ReadAnyValue::func(boost::property_tree::iptree &pt, const std::string &key){ return parse_int(pt,key); } +template<> HoldAny ReadAnyValue::func(boost::property_tree::iptree &pt, const std::string &key){ return parse_int(pt,key); } +template<> HoldAny ReadAnyValue::func(boost::property_tree::iptree &pt, const std::string &key){ return parse_int(pt,key); } +template<> HoldAny ReadAnyValue::func(boost::property_tree::iptree &pt, const std::string &key){ return parse_int(pt,key); } + +template<> HoldAny ReadAnyValue::func(boost::property_tree::iptree &pt, const std::string &key){ return parse_int(pt,key); } +template<> HoldAny ReadAnyValue::func(boost::property_tree::iptree &pt, const std::string &key){ return parse_int(pt,key); } +template<> HoldAny ReadAnyValue::func(boost::property_tree::iptree &pt, const std::string &key){ return parse_int(pt,key); } +template<> HoldAny ReadAnyValue::func(boost::property_tree::iptree &pt, const std::string &key){ return parse_int(pt,key); } + +template<> HoldAny ReadAnyValue::func(boost::property_tree::iptree &pt, const std::string &key) +{ return parse_octets::type>(pt,key); } + +template<> HoldAny ReadAnyValue::func(boost::property_tree::iptree &pt, const std::string &key) +{ return parse_octets::type>(pt,key); } + +template HoldAny ReadAnyValue::func(boost::property_tree::iptree &pt, const std::string &key){ + return parse_typed_value::type>(pt, key); +} + +template void read_optional(T& var, boost::property_tree::iptree &pt, const std::string &key){ + var = pt.get(key, T()); +} + +template void read_integer(T& var, boost::property_tree::iptree &pt, const std::string &key){ + var = int_from_string(pt.get(key)); +} + +template T read_integer(boost::property_tree::iptree &pt, const std::string &key){ + return int_from_string(pt.get(key, std::string())); +} + + +void read_var(ObjectDict::Entry &entry, boost::property_tree::iptree &object){ + read_integer(entry.data_type, object, "DataType"); + entry.mappable = object.get("PDOMapping", false); + try{ + set_access(entry, object.get("AccessType")); + } + catch(...){ + THROW_WITH_KEY(ParseException("No AccessType") , ObjectDict::Key(entry)); + + } + entry.def_val = ReadAnyValue::read_value(object, entry.data_type, "DefaultValue"); + entry.init_val = ReadAnyValue::read_value(object, entry.data_type, "ParameterValue"); +} + +void parse_object(ObjectDictSharedPtr dict, boost::property_tree::iptree &pt, const std::string &name, const uint8_t* sub_index = 0){ + boost::optional object = pt.get_child_optional(name.substr(2)); + if(!object) return; + + std::shared_ptr entry = std::make_shared(); + try{ + entry->index = int_from_string(name); + entry->obj_code = ObjectDict::Code(int_from_string(object->get("ObjectType", boost::lexical_cast((uint16_t)ObjectDict::VAR)))); + entry->desc = object->get("Denotation",object->get("ParameterName")); + + // std::cout << name << ": "<< entry->desc << std::endl; + if(entry->obj_code == ObjectDict::VAR || entry->obj_code == ObjectDict::DOMAIN_DATA || sub_index){ + entry->sub_index = sub_index? *sub_index: 0; + read_var(*entry, *object); + dict->insert(sub_index != 0, entry); + }else if(entry->obj_code == ObjectDict::ARRAY || entry->obj_code == ObjectDict::RECORD){ + uint8_t subs = read_integer(*object, "CompactSubObj"); + if(subs){ // compact + dict->insert(true, std::make_shared(entry->index, 0, ObjectDict::DEFTYPE_UNSIGNED8, "NrOfObjects", true, false, false, HoldAny(subs))); + + read_var(*entry, *object); + + for(uint8_t i=1; i< subs; ++i){ + std::string subname = pt.get(name.substr(2)+"Name." + boost::lexical_cast((int)i),entry->desc + boost::lexical_cast((int)i)); + subname = pt.get(name.substr(2)+"Denotation." + boost::lexical_cast((int)i), subname); + + dict->insert(true, std::make_shared(entry->index, i, entry->data_type, name, entry->readable, entry->writable, entry->mappable, entry->def_val, + ReadAnyValue::read_value(pt, entry->data_type, name.substr(2)+"Value." + boost::lexical_cast((int)i)))); + } + }else{ + read_integer(subs, *object, "SubNumber"); + for(uint8_t i=0; i< subs; ++i){ + std::stringstream buf; + buf << name << "sub" << std::hex << int(i); + // std::cout << "added " << buf.str() << " " << int(i) << std::endl; + parse_object(dict, pt, buf.str(), &i); + } + } + }else{ + THROW_WITH_KEY(ParseException("Object type not supported") , ObjectDict::Key(*entry)); + } + } + catch(const std::bad_cast &e){ + throw ParseException(std::string("Type of ") + name + " does not match or is not supported"); + } + catch(const std::exception&e){ + throw ParseException(std::string("Cannot process ") + name + ": " + e.what()); + } +} +void parse_objects(ObjectDictSharedPtr dict, boost::property_tree::iptree &pt, const std::string &key){ + if(!pt.count(key)) return; + + boost::property_tree::iptree objects = pt.get_child(key); + uint16_t count = read_integer(objects, "SupportedObjects"); + for(uint16_t i=0; i < count; ++i){ + std::string name = objects.get(boost::lexical_cast(i+1)); + parse_object(dict, pt, name); + } +} +ObjectDictSharedPtr ObjectDict::fromFile(const std::string &path, const ObjectDict::Overlay &overlay){ + DeviceInfo info; + boost::property_tree::iptree pt; + ObjectDictSharedPtr dict; + + boost::property_tree::read_ini(path, pt); + + boost::property_tree::iptree di = pt.get_child("DeviceInfo"); + + read_optional(info.vendor_name, di, "VendorName"); + read_optional(info.vendor_number, di, "VendorNumber"); + read_optional(info.product_name, di, "ProductName"); + read_optional(info.product_number, di, "ProductNumber"); + read_optional(info.revision_number, di, "RevisionNumber"); + read_optional(info.order_code, di, "OrderCode"); + read_optional(info.simple_boot_up_master, di, "SimpleBootUpMaster"); + read_optional(info.simple_boot_up_slave, di, "SimpleBootUpSlave"); + read_optional(info.granularity, di, "Granularity"); + read_optional(info.dynamic_channels_supported, di, "DynamicChannelsSupported"); + read_optional(info.group_messaging, di, "GroupMessaging"); + read_optional(info.nr_of_rx_pdo, di, "NrOfRXPDO"); + read_optional(info.nr_of_tx_pdo, di, "NrOfTXPDO"); + read_optional(info.lss_supported, di, "LSS_Supported"); + + std::unordered_set baudrates; + std::unordered_set dummy_usage; + + for(boost::property_tree::iptree::value_type &v: di){ + if(v.first.find("BaudRate_") == 0){ + uint16_t rate = int_from_string(v.first.substr(9)); + if(v.second.get_value()) + info.baudrates.insert(rate * 1000); + } + } + + if(pt.count("DummyUsage")){ + for(boost::property_tree::iptree::value_type &v: pt.get_child("DummyUsage")){ + if(v.first.find("Dummy") == 0){ + // std::cout << ("0x"+v.first.substr(5)) << std::endl; + uint16_t dummy = int_from_string("0x"+v.first.substr(5)); + if(v.second.get_value()) + info.dummy_usage.insert(dummy); + } + } + } + + dict = std::make_shared(info); + + for(Overlay::const_iterator it= overlay.begin(); it != overlay.end(); ++it){ + pt.get_child(it->first).put("ParameterValue", it->second); + } + + parse_objects(dict, pt, "MandatoryObjects"); + parse_objects(dict, pt, "OptionalObjects"); + parse_objects(dict, pt, "ManufacturerObjects"); + + return dict; +} + +size_t ObjectStorage::map(const ObjectDict::EntryConstSharedPtr &e, const ObjectDict::Key &key, const ReadFunc & read_delegate, const WriteFunc & write_delegate){ + ObjectStorageMap::iterator it = storage_.find(key); + + if(it == storage_.end()){ + + DataSharedPtr data; + + + if(!e->def_val.type().valid()){ + THROW_WITH_KEY(std::bad_cast() , key); + } + + data = std::make_shared(key, e,e->def_val.type(),read_delegate_, write_delegate_); + + std::pair ok = storage_.insert(std::make_pair(key, data)); + it = ok.first; + it->second->reset(); + + } + + if(read_delegate && write_delegate){ + it->second->set_delegates(read_delegate_, write_delegate); + it->second->force_write(); // update buffer + it->second->set_delegates(read_delegate, write_delegate_); + }else if(write_delegate) { + it->second->set_delegates(read_delegate_, write_delegate); + it->second->force_write(); // update buffer + }else if(read_delegate){ + it->second->set_delegates(read_delegate, write_delegate_); + } + return it->second->size(); +} + +size_t ObjectStorage::map(uint16_t index, uint8_t sub_index, const ReadFunc & read_delegate, const WriteFunc & write_delegate){ + boost::mutex::scoped_lock lock(mutex_); + + try{ + ObjectDict::Key key(index,sub_index); + const ObjectDict::EntryConstSharedPtr e = dict_->get(key); + return map(e, key,read_delegate, write_delegate); + } + catch(std::out_of_range) { + if(sub_index != 0) throw; + + ObjectDict::Key key(index); + const ObjectDict::EntryConstSharedPtr e = dict_->get(key); + return map(e, key, read_delegate, write_delegate); + } +} + +ObjectStorage::ObjectStorage(ObjectDictConstSharedPtr dict, uint8_t node_id, ReadFunc read_delegate, WriteFunc write_delegate) +:read_delegate_(read_delegate), write_delegate_(write_delegate), dict_(dict), node_id_(node_id){ + assert(dict_); + assert(read_delegate_); + assert(write_delegate_); +} + +void ObjectStorage::init_nolock(const ObjectDict::Key &key, const ObjectDict::EntryConstSharedPtr &entry){ + + if(!entry->init_val.is_empty()){ + ObjectStorageMap::iterator it = storage_.find(key); + + if(it == storage_.end()){ + DataSharedPtr data = std::make_shared(key,entry, entry->init_val.type(), read_delegate_, write_delegate_); + std::pair ok = storage_.insert(std::make_pair(key, data)); + it = ok.first; + if(!ok.second){ + THROW_WITH_KEY(std::bad_alloc() , key); + } + } + it->second->init(); + } +} +void ObjectStorage::init(const ObjectDict::Key &key){ + boost::mutex::scoped_lock lock(mutex_); + init_nolock(key, dict_->get(key)); +} +void ObjectStorage::init_all(){ + boost::mutex::scoped_lock lock(mutex_); + + ObjectDict::ObjectDictMap::const_iterator entry_it; + while(dict_->iterate(entry_it)){ + init_nolock(entry_it->first, entry_it->second); + } +} + +void ObjectStorage::reset(){ + boost::mutex::scoped_lock lock(mutex_); + for(ObjectStorageMap::iterator it = storage_.begin(); it != storage_.end(); ++it){ + it->second->reset(); + } +} + +template std::string formatValue(const T &value){ + std::stringstream sstr; + sstr << value; + return sstr.str(); +} +template<> std::string formatValue(const std::string &value){ + return can::buffer2hex(value, false); +} +template<> std::string formatValue(const std::string &value){ + return can::buffer2hex(value, false); +} + +struct PrintValue { + template static std::string func(ObjectStorage& storage, const ObjectDict::Key &key, bool cached){ + ObjectStorage::Entry::type> entry = storage.entry::type>(key); + return formatValue
(cached? entry.get_cached() : entry.get() ); + } + static std::function getReader(ObjectStorage& storage, const ObjectDict::Key &key, bool cached){ + ObjectDict::DataTypes data_type = (ObjectDict::DataTypes) storage.dict_->get(key)->data_type; + return std::bind(branch_type(data_type),std::ref(storage), key, cached); + } +}; + +ObjectStorage::ReadStringFuncType ObjectStorage::getStringReader(const ObjectDict::Key &key, bool cached){ + return PrintValue::getReader(*this, key, cached); +} + +struct WriteStringValue { + typedef HoldAny (*reader_type)(boost::property_tree::iptree &, const std::string &); + template static void write(ObjectStorage::Entry entry, bool cached, reader_type reader, const std::string &value){ + boost::property_tree::iptree pt; + pt.put("value", value); + HoldAny any = reader(pt, "value"); + if(cached){ + entry.set_cached(any.get()); + } else { + entry.set(any.get()); + } + } + template static std::function func(ObjectStorage& storage, const ObjectDict::Key &key, bool cached){ + ObjectStorage::Entry::type> entry = storage.entry::type>(key); + reader_type reader = branch_type(dt); + return std::bind(&WriteStringValue::write::type >, entry, cached, reader, std::placeholders::_1); + } + static std::function getWriter(ObjectStorage& storage, const ObjectDict::Key &key, bool cached){ + ObjectDict::DataTypes data_type = (ObjectDict::DataTypes) storage.dict_->get(key)->data_type; + return branch_type (ObjectStorage&, const ObjectDict::Key &, bool)>(data_type)(storage, key, cached); + } +}; + +ObjectStorage::WriteStringFuncType ObjectStorage::getStringWriter(const ObjectDict::Key &key, bool cached){ + return WriteStringValue::getWriter(*this, key, cached); +} diff --git a/Devices/Libraries/Ros/ros_canopen/canopen_master/src/pdo.cpp b/Devices/Libraries/Ros/ros_canopen/canopen_master/src/pdo.cpp new file mode 100755 index 0000000..051ab4e --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/canopen_master/src/pdo.cpp @@ -0,0 +1,383 @@ +#include + +using namespace canopen; + +#pragma pack(push) /* push current alignment to stack */ +#pragma pack(1) /* set alignment to 1 byte boundary */ + +class PDOid{ + const uint32_t value_; +public: + static const unsigned int ID_MASK = (1u << 29)-1; + static const unsigned int EXTENDED_MASK = (1u << 29); + static const unsigned int NO_RTR_MASK = (1u << 30); + static const unsigned int INVALID_MASK = (1u << 31); + + PDOid(const uint32_t &val) + : value_(val) + {} + can::Header header(bool fill_rtr = false) const { + return can::Header(value_ & ID_MASK, value_ & EXTENDED_MASK, fill_rtr && !(value_ & NO_RTR_MASK), false); + } + bool isInvalid() const { return value_ & INVALID_MASK; } +}; + +struct PDOmap{ + uint8_t length; + uint8_t sub_index; + uint16_t index; + PDOmap(uint32_t val) + : length(val & 0xFF), + sub_index((val>>8) & 0xFF), + index(val>>16) + {} +}; + +#pragma pack(pop) /* pop previous alignment from stack */ + + +const uint8_t SUB_COM_NUM = 0; +const uint8_t SUB_COM_COB_ID = 1; +const uint8_t SUB_COM_TRANSMISSION_TYPE = 2; +const uint8_t SUB_COM_RESERVED = 4; + +const uint8_t SUB_MAP_NUM = 0; + +const uint16_t RPDO_COM_BASE =0x1400; +const uint16_t RPDO_MAP_BASE =0x1600; +const uint16_t TPDO_COM_BASE =0x1800; +const uint16_t TPDO_MAP_BASE =0x1A00; + +bool check_com_changed(const ObjectDict &dict, const uint16_t com_id){ + bool com_changed = false; + + // check if com parameter has to be set + for(uint8_t sub = 0; sub <=6 ; ++sub){ + try{ + if(!dict(com_id,sub).init_val.is_empty()){ + com_changed = true; + break; + } + } + catch (std::out_of_range) {} + } + return com_changed; +} + +bool check_map_changed(const uint8_t &num, const ObjectDict &dict, const uint16_t &map_index){ + bool map_changed = false; + + // check if mapping has to be set + if(num <= 0x40){ + for(uint8_t sub = 1; sub <=num ; ++sub){ + try{ + if(!dict(map_index,sub).init_val.is_empty()){ + map_changed = true; + break; + } + } + catch (std::out_of_range) {} + } + }else{ + map_changed = dict( map_index ,0 ).init_val.is_empty(); + } + return map_changed; +} +void PDOMapper::PDO::parse_and_set_mapping(const ObjectStorageSharedPtr &storage, const uint16_t &com_index, const uint16_t &map_index, const bool &read, const bool &write){ + + const canopen::ObjectDict & dict = *storage->dict_; + + ObjectStorage::Entry num_entry; + storage->entry(num_entry, map_index, SUB_MAP_NUM); + + uint8_t map_num; + + try{ + map_num = num_entry.desc().value().get(); + }catch(...){ + map_num = 0; + } + + bool map_changed = check_map_changed(map_num, dict, map_index); + + // disable PDO if needed + ObjectStorage::Entry cob_id; + storage->entry(cob_id, com_index, SUB_COM_COB_ID); + + bool com_changed = check_com_changed(dict, map_index); + if((map_changed || com_changed) && cob_id.desc().writable){ + cob_id.set(cob_id.get() | PDOid::INVALID_MASK); + } + if(map_num > 0 && map_num <= 0x40){ // actual mapping + if(map_changed){ + num_entry.set(0); + } + + frame.dlc = 0; + for(uint8_t sub = 1; sub <=map_num; ++sub){ + ObjectStorage::Entry mapentry; + storage->entry(mapentry, map_index, sub); + const HoldAny init = dict(map_index ,sub).init_val; + if(!init.is_empty()) mapentry.set(init.get()); + + PDOmap param(mapentry.get_cached()); + BufferSharedPtr b = std::make_shared(param.length/8); + if(param.index < 0x1000){ + // TODO: check DummyUsage + }else{ + ObjectStorage::ReadFunc rd; + ObjectStorage::WriteFunc wd; + + if(read){ + rd = std::bind(&Buffer::read, b.get(), std::placeholders::_1, std::placeholders::_2); + } + if(read || write) + { + wd = std::bind(&Buffer::write, b.get(), std::placeholders::_1, std::placeholders::_2); + size_t l = storage->map(param.index, param.sub_index, rd, wd); + assert(l == param.length/8); + } + } + + frame.dlc += b->size; + assert( frame.dlc <= 8 ); + b->clean(); + buffers.push_back(b); + } + } + if(com_changed){ + uint8_t subs = dict(com_index, SUB_COM_NUM).value().get(); + for(uint8_t i = SUB_COM_NUM+1; i <= subs; ++i){ + if(i == SUB_COM_COB_ID || i == SUB_COM_RESERVED) continue; + try{ + storage->init(ObjectDict::Key(com_index, i)); + } + catch (const std::out_of_range &){ + // entry was not provided, so skip it + } + } + } + if(map_changed){ + num_entry.set(map_num); + } + if((com_changed || map_changed) && cob_id.desc().writable){ + storage->init(ObjectDict::Key(com_index, SUB_COM_COB_ID)); + + cob_id.set(NodeIdOffset::apply(dict(com_index, SUB_COM_COB_ID).value(), storage->node_id_)); + } + + +} +PDOMapper::PDOMapper(const can::CommInterfaceSharedPtr interface) +:interface_(interface) +{ +} +bool PDOMapper::init(const ObjectStorageSharedPtr storage, LayerStatus &status){ + boost::mutex::scoped_lock lock(mutex_); + + try{ + rpdos_.clear(); + + const canopen::ObjectDict & dict = *storage->dict_; + for(uint16_t i=0; i < 512 && rpdos_.size() < dict.device_info.nr_of_tx_pdo;++i){ // TPDOs of device + if(!dict.has(TPDO_COM_BASE + i,0) && !dict.has(TPDO_MAP_BASE + i,0)) continue; + + RPDO::RPDOSharedPtr rpdo = RPDO::create(interface_,storage, TPDO_COM_BASE + i, TPDO_MAP_BASE + i); + if(rpdo){ + rpdos_.insert(rpdo); + } + } + // ROSCANOPEN_DEBUG("canopen_master", "RPDOs: " << rpdos_.size()); + + tpdos_.clear(); + for(uint16_t i=0; i < 512 && tpdos_.size() < dict.device_info.nr_of_rx_pdo;++i){ // RPDOs of device + if(!dict.has(RPDO_COM_BASE + i,0) && !dict.has(RPDO_MAP_BASE + i,0)) continue; + + TPDO::TPDOSharedPtr tpdo = TPDO::create(interface_,storage, RPDO_COM_BASE + i, RPDO_MAP_BASE + i); + if(tpdo){ + tpdos_.insert(tpdo); + } + } + // ROSCANOPEN_DEBUG("canopen_master", "TPDOs: " << tpdos_.size()); + + return true; + } + catch(const std::out_of_range &e){ + status.error(std::string("PDO error: ") + e.what()); + return false; + } +} + + +bool PDOMapper::RPDO::init(const ObjectStorageSharedPtr &storage, const uint16_t &com_index, const uint16_t &map_index){ + boost::mutex::scoped_lock lock(mutex); + listener_.reset(); + const canopen::ObjectDict & dict = *storage->dict_; + parse_and_set_mapping(storage, com_index, map_index, true, false); + + PDOid pdoid( NodeIdOffset::apply(dict(com_index, SUB_COM_COB_ID).value(), storage->node_id_) ); + + if(buffers.empty() || pdoid.isInvalid()){ + return false; + } + + frame = pdoid.header(true); + + transmission_type = dict(com_index, SUB_COM_TRANSMISSION_TYPE).value().get(); + + listener_ = interface_->createMsgListenerM(pdoid.header(), this, &RPDO::handleFrame); + + return true; +} + +bool PDOMapper::TPDO::init(const ObjectStorageSharedPtr &storage, const uint16_t &com_index, const uint16_t &map_index){ + boost::mutex::scoped_lock lock(mutex); + const canopen::ObjectDict & dict = *storage->dict_; + + + PDOid pdoid( NodeIdOffset::apply(dict(com_index, SUB_COM_COB_ID).value(), storage->node_id_) ); + frame = pdoid.header(); + + parse_and_set_mapping(storage, com_index, map_index, false, true); + if(buffers.empty() || pdoid.isInvalid()){ + return false; + } + ObjectStorage::Entry tt; + storage->entry(tt, com_index, SUB_COM_TRANSMISSION_TYPE); + transmission_type = tt.desc().value().get(); + + if(transmission_type != 1 && transmission_type <=240){ + tt.set(1); // enforce 1 for compatibility + } + return true; +} + +void PDOMapper::TPDO::sync(){ + boost::mutex::scoped_lock lock(mutex); + + bool updated = false; + size_t len = frame.dlc; + can::Frame::value_type * dest = frame.c_array(); + for(std::vector< BufferSharedPtr >::iterator b_it = buffers.begin(); b_it != buffers.end(); ++b_it){ + Buffer &b = **b_it; + if(len >= b.size){ + updated = b.read(dest, len) || updated; + len -= b.size; + dest += b.size; + }else{ + // ERROR + } + } + + if( len != 0){ + // ERROR + } + if(updated){ + interface_->send( frame ); + }else{ + // TODO: Notify + } +} + +void PDOMapper::RPDO::sync(LayerStatus &status){ + boost::mutex::scoped_lock lock(mutex); + if((transmission_type >= 1 && transmission_type <= 240) || transmission_type == 0xFC){ // cyclic + if(timeout > 0){ + --timeout; + }else if(timeout == 0) { + status.warn("RPDO timeout"); + } + } + if(transmission_type == 0xFC || transmission_type == 0xFD){ + if(frame.is_rtr){ + interface_->send(frame); + } + } +} + +void PDOMapper::RPDO::handleFrame(const can::Frame & msg){ + size_t offset = 0; + const uint8_t * src = msg.data.data(); + for(std::vector::iterator it = buffers.begin(); it != buffers.end(); ++it){ + Buffer &b = **it; + + if( offset + b.size <= msg.dlc ){ + b.write(src+offset, b.size); + offset += b.size; + }else{ + // ERROR + } + } + if( offset != msg.dlc ){ + // ERROR + } + { + boost::mutex::scoped_lock lock(mutex); + if(transmission_type >= 1 && transmission_type <= 240){ + timeout = transmission_type + 2; + }else if(transmission_type == 0xFC || transmission_type == 0xFD){ + if(frame.is_rtr){ + timeout = 1+2; + } + } + } +} + +void PDOMapper::read(LayerStatus &status){ + boost::mutex::scoped_lock lock(mutex_); + for(std::unordered_set::iterator it = rpdos_.begin(); it != rpdos_.end(); ++it){ + (*it)->sync(status); + } +} +bool PDOMapper::write(){ + boost::mutex::scoped_lock lock(mutex_); + for(std::unordered_set::iterator it = tpdos_.begin(); it != tpdos_.end(); ++it){ + (*it)->sync(); + } + return true; // TODO: check for errors +} + +bool PDOMapper::Buffer::read(uint8_t* b, const size_t len){ + boost::mutex::scoped_lock lock(mutex); + if(size > len){ + BOOST_THROW_EXCEPTION( std::bad_cast() ); + } + if(empty) return false; + + memcpy(b,&buffer[0], size); + bool was_dirty = dirty; + dirty = false; + return was_dirty; +} +void PDOMapper::Buffer::write(const uint8_t* b, const size_t len){ + boost::mutex::scoped_lock lock(mutex); + if(size > len){ + BOOST_THROW_EXCEPTION( std::bad_cast() ); + } + empty = false; + dirty = true; + memcpy(&buffer[0], b, size); +} +void PDOMapper::Buffer::read(const canopen::ObjectDict::Entry &entry, String &data){ + boost::mutex::scoped_lock lock(mutex); + time_point abs_time = get_abs_time(boost::chrono::seconds(1)); + if(size != data.size()){ + THROW_WITH_KEY(std::bad_cast(), ObjectDict::Key(entry)); + } + if(empty){ + THROW_WITH_KEY(TimeoutException("PDO data empty"), ObjectDict::Key(entry)); + } + if(dirty){ + data.assign(buffer.begin(), buffer.end()); + dirty = false; + } +} +void PDOMapper::Buffer::write(const canopen::ObjectDict::Entry &entry, const String &data){ + boost::mutex::scoped_lock lock(mutex); + if(size != data.size()){ + THROW_WITH_KEY(std::bad_cast(), ObjectDict::Key(entry)); + } + empty = false; + dirty = true; + buffer.assign(data.begin(),data.end()); +} diff --git a/Devices/Libraries/Ros/ros_canopen/canopen_master/src/sdo.cpp b/Devices/Libraries/Ros/ros_canopen/canopen_master/src/sdo.cpp new file mode 100755 index 0000000..552ad9c --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/canopen_master/src/sdo.cpp @@ -0,0 +1,451 @@ +#include + +using namespace canopen; + +const uint8_t COMMAND_MASK = (1<<7) | (1<<6) | (1<<5); +const uint8_t INITIATE_DOWNLOAD_REQUEST = (0 << 5); +const uint8_t INITIATE_DOWNLOAD_RESPONSE = (1 << 5); +const uint8_t DOWNLOAD_SEGMENT_REQUEST = (1 << 5); +const uint8_t DOWNLOAD_SEGMENT_RESPONSE = (3 << 5); +const uint8_t INITIATE_UPLOAD_REQUEST = (2 << 5); +const uint8_t INITIATE_UPLOAD_RESPONSE = (2 << 5); +const uint8_t UPLOAD_SEGMENT_REQUEST = (3 << 5); +const uint8_t UPLOAD_SEGMENT_RESPONSE = (0 << 5); +const uint8_t ABORT_TRANSFER_REQUEST = (4 << 5); + + +#pragma pack(push) /* push current alignment to stack */ +#pragma pack(1) /* set alignment to 1 byte boundary */ + +struct SDOid{ + uint32_t id:29; + uint32_t extended:1; + uint32_t dynamic:1; + uint32_t invalid:1; + SDOid(uint32_t val){ + *(uint32_t*) this = val; + } + can::Header header() { + return can::Header(id, extended, false, false); + } +}; + +struct InitiateShort{ + uint8_t :5; + uint8_t command:3; + uint16_t index; + uint8_t sub_index; + uint8_t reserved[4]; +}; + +struct InitiateLong{ + uint8_t size_indicated:1; + uint8_t expedited:1; + uint8_t num:2; + uint8_t :1; + uint8_t command:3; + uint16_t index; + uint8_t sub_index; + uint8_t payload[4]; + + size_t data_size(){ + if(expedited && size_indicated) return 4-num; + else if(!expedited && size_indicated) return payload[0] | (payload[3]<<8); + else return 0; + } + size_t apply_buffer(const String &buffer){ + size_t size = buffer.size(); + size_indicated = 1; + if(size > 4){ + expedited = 0; + payload[0] = size & 0xFF; + payload[3] = (size >> 8) & 0xFF; + return 0; + }else{ + expedited = 1; + size_indicated = 1; + num = 4-size; + memcpy(payload, buffer.data(), size); + return size; + } + } +}; + +struct SegmentShort{ + uint8_t :4; + uint8_t toggle:1; + uint8_t command:3; + uint8_t reserved[7]; +}; + +struct SegmentLong{ + uint8_t done:1; + uint8_t num:3; + uint8_t toggle:1; + uint8_t command:3; + uint8_t payload[7]; + size_t data_size(){ + return 7-num; + } + size_t apply_buffer(const String &buffer, const size_t offset){ + size_t size = buffer.size() - offset; + if(size > 7) size = 7; + else done = 1; + num = 7 - size; + memcpy(payload, buffer.data() + offset, size); + return offset + size; + } +}; + +struct DownloadInitiateRequest: public FrameOverlay{ + static const uint8_t command = 1; + + DownloadInitiateRequest(const Header &h, const canopen::ObjectDict::Entry &entry, const String &buffer, size_t &offset) : FrameOverlay(h) { + data.command = command; + data.index = entry.index; + data.sub_index = entry.sub_index; + offset = data.apply_buffer(buffer); + } + DownloadInitiateRequest(const can::Frame &f) : FrameOverlay(f){ } +}; + +struct DownloadInitiateResponse: public FrameOverlay{ + static const uint8_t command = 3; + + DownloadInitiateResponse(const can::Frame &f) : FrameOverlay(f){ } + + bool test(const can::Frame &msg, uint32_t &reason){ + DownloadInitiateRequest req(msg); + if(req.data.command == DownloadInitiateRequest::command && data.index == req.data.index && data.sub_index == req.data.sub_index){ + return true; + } + reason = 0x08000000; // General error + return false; + } +}; + +struct DownloadSegmentRequest: public FrameOverlay{ + static const uint8_t command = 0; + + DownloadSegmentRequest(const can::Frame &f) : FrameOverlay(f){ } + + DownloadSegmentRequest(const Header &h, bool toggle, const String &buffer, size_t& offset) : FrameOverlay(h) { + data.command = command; + data.toggle = toggle?1:0; + offset = data.apply_buffer(buffer, offset); + } +}; + +struct DownloadSegmentResponse : public FrameOverlay{ + static const uint8_t command = 1; + DownloadSegmentResponse(const can::Frame &f) : FrameOverlay(f) { + } + bool test(const can::Frame &msg, uint32_t &reason){ + DownloadSegmentRequest req(msg); + if (req.data.command != DownloadSegmentRequest::command){ + reason = 0x08000000; // General error + return false; + }else if( data.toggle != req.data.toggle){ + reason = 0x05030000; // Toggle bit not alternated + return false; + } + return true; + } +}; + +struct UploadInitiateRequest: public FrameOverlay{ + static const uint8_t command = 2; + UploadInitiateRequest(const Header &h, const canopen::ObjectDict::Entry &entry) : FrameOverlay(h) { + data.command = command; + data.index = entry.index; + data.sub_index = entry.sub_index; + } + UploadInitiateRequest(const can::Frame &f) : FrameOverlay(f){ } +}; + +struct UploadInitiateResponse: public FrameOverlay{ + static const uint8_t command = 2; + UploadInitiateResponse(const can::Frame &f) : FrameOverlay(f) { } + bool test(const can::Frame &msg, size_t size, uint32_t &reason){ + UploadInitiateRequest req(msg); + if(req.data.command == UploadInitiateRequest::command && data.index == req.data.index && data.sub_index == req.data.sub_index){ + size_t ds = data.data_size(); + if(ds == 0 || size == 0 || ds >= size) { // should be ==, but >= is needed for Elmo, it responses with more byte than requested + if(!data.expedited || (ds <= 4 && size <= 4)) return true; + }else{ + reason = 0x06070010; // Data type does not match, length of service parameter does not match + return false; + } + } + reason = 0x08000000; // General error + return false; + } + bool read_data(String & buffer, size_t & offset, size_t & total){ + if(data.size_indicated && total == 0){ + total = data.data_size(); + buffer.resize(total); + } + if(data.expedited){ + memcpy(&buffer.front(), data.payload, buffer.size()); + offset = buffer.size(); + return true; + } + return false; + } +}; +struct UploadSegmentRequest: public FrameOverlay{ + static const uint8_t command = 3; + UploadSegmentRequest(const Header &h, bool toggle) : FrameOverlay(h) { + data.command = command; + data.toggle = toggle?1:0; + } + UploadSegmentRequest(const can::Frame &f) : FrameOverlay(f) { } +}; + +struct UploadSegmentResponse : public FrameOverlay{ + static const uint8_t command = 0; + UploadSegmentResponse(const can::Frame &f) : FrameOverlay(f) { + } + bool test(const can::Frame &msg, uint32_t &reason){ + UploadSegmentRequest req(msg); + if(req.data.command != UploadSegmentRequest::command){ + reason = 0x08000000; // General error + return false; + }else if( data.toggle != req.data.toggle){ + reason = 0x05030000; // Toggle bit not alternated + return false; + } + return true; + } + bool read_data(String & buffer, size_t & offset, const size_t & total){ + uint32_t n = data.data_size(); + if(total == 0){ + buffer.resize(offset + n); + } + if(offset + n <= buffer.size()){ + memcpy(&buffer[offset], data.payload, n); + offset += n; + return true; + } + return false; + } +}; + +struct AbortData{ + uint8_t :5; + uint8_t command:3; + uint16_t index; + uint8_t sub_index; + uint32_t reason; + + const char * text(){ + switch(reason){ + case 0x05030000: return "Toggle bit not alternated."; + case 0x05040000: return "SDO protocol timed out."; + case 0x05040001: return "Client/server command specifier not valid or unknown."; + case 0x05040002: return "Invalid block size (block mode only)."; + case 0x05040003: return "Invalid sequence number (block mode only)."; + case 0x05040004: return "CRC error (block mode only)."; + case 0x05040005: return "Out of memory."; + case 0x06010000: return "Unsupported access to an object."; + case 0x06010001: return "Attempt to read a write only object."; + case 0x06010002: return "Attempt to write a read only object."; + case 0x06020000: return "Object does not exist in the object dictionary."; + case 0x06040041: return "Object cannot be mapped to the PDO."; + case 0x06040042: return "The number and length of the objects to be mapped would exceed PDO length."; + case 0x06040043: return "General parameter incompatibility reason."; + case 0x06040047: return "General internal incompatibility in the device."; + case 0x06060000: return "Access failed due to an hardware error."; + case 0x06070010: return "Data type does not match, length of service parameter does not match"; + case 0x06070012: return "Data type does not match, length of service parameter too high"; + case 0x06070013: return "Data type does not match, length of service parameter too low"; + case 0x06090011: return "Sub-index does not exist."; + case 0x06090030: return "Invalid value for parameter (download only)."; + case 0x06090031: return "Value of parameter written too high (download only)."; + case 0x06090032: return "Value of parameter written too low (download only)."; + case 0x06090036: return "Maximum value is less than minimum value."; + case 0x060A0023: return "Resource not available: SDO connection"; + case 0x08000000: return "General error"; + case 0x08000020: return "Data cannot be transferred or stored to the application."; + case 0x08000021: return "Data cannot be transferred or stored to the application because of local control."; + case 0x08000022: return "Data cannot be transferred or stored to the application because of the present device state."; + case 0x08000023: return "Object dictionary dynamic generation fails or no object dictionary is present (e.g.object dictionary is generated from file and generation fails because of an file error)."; + case 0x08000024: return "No data available"; + default: return "Abort code is reserved"; + } + } +}; + +struct AbortTranserRequest: public FrameOverlay{ + static const uint8_t command = 4; + AbortTranserRequest(const can::Frame &f) : FrameOverlay(f) {} + AbortTranserRequest(const Header &h, uint16_t index, uint8_t sub_index, uint32_t reason) : FrameOverlay(h) { + data.command = command; + data.index = index; + data.sub_index = sub_index; + data.reason = reason; + } +}; + +#pragma pack(pop) /* pop previous alignment from stack */ + +void SDOClient::abort(uint32_t reason){ + if(current_entry){ + interface_->send(last_msg = AbortTranserRequest(client_id, current_entry->index, current_entry->sub_index, reason)); + } +} + +bool SDOClient::processFrame(const can::Frame & msg){ + if(msg.dlc != 8) return false; + + uint32_t reason = 0; + switch(msg.data[0] >> 5){ + case DownloadInitiateResponse::command: + { + DownloadInitiateResponse resp(msg); + if(resp.test(last_msg, reason) ){ + if(offset < total){ + interface_->send(last_msg = DownloadSegmentRequest(client_id, false, buffer, offset)); + }else{ + done = true; + } + } + break; + } + case DownloadSegmentResponse::command: + { + DownloadSegmentResponse resp(msg); + if( resp.test(last_msg, reason) ){ + if(offset < total){ + interface_->send(last_msg = DownloadSegmentRequest(client_id, !resp.data.toggle, buffer, offset)); + }else{ + done = true; + } + } + break; + } + + case UploadInitiateResponse::command: + { + UploadInitiateResponse resp(msg); + if( resp.test(last_msg, total, reason) ){ + if(resp.read_data(buffer, offset, total)){ + done = true; + }else{ + interface_->send(last_msg = UploadSegmentRequest(client_id, false)); + } + } + break; + } + case UploadSegmentResponse::command: + { + UploadSegmentResponse resp(msg); + if( resp.test(last_msg, reason) ){ + if(resp.read_data(buffer, offset, total)){ + if(resp.data.done || offset == total){ + done = true; + }else{ + interface_->send(last_msg = UploadSegmentRequest(client_id, !resp.data.toggle)); + } + }else{ + // abort, size mismatch + ROSCANOPEN_ERROR("canopen_master", "abort, size mismatch" << buffer.size() << " " << resp.data.data_size()); + reason = 0x06070010; // Data type does not match, length of service parameter does not match + } + } + break; + } + case AbortTranserRequest::command: + ROSCANOPEN_ERROR("canopen_master", "abort" << std::hex << (uint32_t) AbortTranserRequest(msg).data.index << "#"<< std::dec << (uint32_t) AbortTranserRequest(msg).data.sub_index << ", reason: " << AbortTranserRequest(msg).data.text()); + offset = 0; + return false; + break; + } + if(reason){ + abort(reason); + offset = 0; + return false; + } + return true; + +} + +void SDOClient::init(){ + assert(storage_); + assert(interface_); + const canopen::ObjectDict & dict = *storage_->dict_; + + try{ + client_id = SDOid(NodeIdOffset::apply(dict(0x1200, 1).value(), storage_->node_id_)).header(); + } + catch(...){ + client_id = can::MsgHeader(0x600+ storage_->node_id_); + } + + last_msg = AbortTranserRequest(client_id, 0,0,0); + current_entry = 0; + + can::Header server_id; + try{ + server_id = SDOid(NodeIdOffset::apply(dict(0x1200, 2).value(), storage_->node_id_)).header(); + } + catch(...){ + server_id = can::MsgHeader(0x580+ storage_->node_id_); + } + reader_.listen(interface_, server_id); +} + +void SDOClient::transmitAndWait(const canopen::ObjectDict::Entry &entry, const String &data, String *result){ + buffer = data; + offset = 0; + total = buffer.size(); + current_entry = &entry; + done = false; + + can::BufferedReader::ScopedEnabler enabler(reader_); + + if(result){ + interface_->send(last_msg = UploadInitiateRequest(client_id, entry)); + }else{ + interface_->send(last_msg = DownloadInitiateRequest(client_id, entry, buffer, offset)); + } + + boost::this_thread::disable_interruption di; + can::Frame msg; + + while(!done){ + if(!reader_.read(&msg,boost::chrono::seconds(1))) + { + abort(0x05040000); // SDO protocol timed out. + ROSCANOPEN_ERROR("canopen_master", "Did not receive a response message"); + break; + } + if(!processFrame(msg)){ + ROSCANOPEN_ERROR("canopen_master", "Could not process message"); + break; + } + } + if(offset == 0 || offset != total){ + THROW_WITH_KEY(TimeoutException("SDO"), ObjectDict::Key(*current_entry)); + } + + if(result) *result=buffer; + +} + +void SDOClient::read(const canopen::ObjectDict::Entry &entry, String &data){ + boost::timed_mutex::scoped_lock lock(mutex, boost::chrono::seconds(2)); + if(lock){ + transmitAndWait(entry, data, &data); + }else{ + THROW_WITH_KEY(TimeoutException("SDO read"), ObjectDict::Key(entry)); + } +} +void SDOClient::write(const canopen::ObjectDict::Entry &entry, const String &data){ + boost::timed_mutex::scoped_lock lock(mutex, boost::chrono::seconds(2)); + if(lock){ + transmitAndWait(entry, data, 0); + }else{ + THROW_WITH_KEY(TimeoutException("SDO write"), ObjectDict::Key(entry)); + } +} diff --git a/Devices/Libraries/Ros/ros_canopen/canopen_master/test/test_node.cpp b/Devices/Libraries/Ros/ros_canopen/canopen_master/test/test_node.cpp new file mode 100755 index 0000000..740a044 --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/canopen_master/test/test_node.cpp @@ -0,0 +1,65 @@ +#include +#include + +// Bring in gtest +#include + +canopen::ObjectDictSharedPtr make_dict(){ + canopen::DeviceInfo info; + info.nr_of_rx_pdo = 0; + info.nr_of_tx_pdo = 0; + + canopen::ObjectDictSharedPtr dict = std::make_shared(info); + dict->insert(false, std::make_shared(canopen::ObjectDict::VAR, + 0x1017, + canopen::ObjectDict::DEFTYPE_UNSIGNED16, + "producer heartbeat", + true, true, false, + canopen::HoldAny((uint16_t)0), + canopen::HoldAny((uint16_t)100) + )); + return dict; +} +TEST(TestNode, testInitandShutdown){ + + can::DummyBus bus("testInitandShutdown"); + + can::ThreadedDummyInterfaceSharedPtr driver = std::make_shared(); + + can::DummyReplay replay; + + replay.add("0#8201", "701#00"); + replay.add("601#2b17100064000000", "581#6017100000000000"); + replay.add("0#0101", "701#05"); + replay.add("601#2b17100000000000", "581#6017100000000000"); + replay.init(bus); + + EXPECT_FALSE(replay.done()); + + auto settings = can::SettingsMap::create(); + settings->set("trace", true); + + driver->init(bus.name, false, settings); + + canopen::NodeSharedPtr node = std::make_shared(driver, make_dict(), 1); + + { + canopen::LayerStatus status; + node->init(status); + ASSERT_TRUE(status.bounded()); + ASSERT_EQ(canopen::Node::Operational, node->getState()); + } + + { + canopen::LayerStatus status; + node->shutdown(status); + ASSERT_TRUE(status.bounded()); + } + EXPECT_TRUE(replay.done()); +} + +// Run all the tests that were declared with TEST() +int main(int argc, char **argv){ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/Devices/Libraries/Ros/ros_canopen/canopen_master/test/test_parser.cpp b/Devices/Libraries/Ros/ros_canopen/canopen_master/test/test_parser.cpp new file mode 100755 index 0000000..6bb30b0 --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/canopen_master/test/test_parser.cpp @@ -0,0 +1,182 @@ +// Bring in my package's API, which is what I'm testing +#include +#include + +// Bring in gtest +#include + + +template canopen::HoldAny parse_int(boost::property_tree::iptree &pt, const std::string &key); + +template canopen::HoldAny prepare_test(const std::string &str){ + boost::property_tree::iptree pt; + pt.put("test", str); + return parse_int(pt, "test"); +} + +template class TestHexTypes : public ::testing::Test{ +public: + static void test_hex(const T& val, const std::string &str){ + EXPECT_EQ(val, prepare_test(str).template get()); + } + static void test_hex_node(const T& val, const std::string &str, const uint8_t offset){ + EXPECT_EQ(val, canopen::NodeIdOffset::apply(prepare_test(str),offset)); + } +}; + +typedef ::testing::Types PosTypes; + +TYPED_TEST_CASE(TestHexTypes, PosTypes); + +TYPED_TEST(TestHexTypes, checkZero){ + TestFixture::test_hex(0,"0"); + TestFixture::test_hex(0,"0x0"); + + for(uint8_t i = 0; i <=127; ++i){ + TestFixture::test_hex_node(0,"0",i); + TestFixture::test_hex_node(0,"0x0",i); + TestFixture::test_hex_node(0+i,"$NODEID+0",i); + TestFixture::test_hex_node(0+i,"$NODEID+0x0",i); + TestFixture::test_hex_node(0+i,"0+$NODEID",i); + TestFixture::test_hex_node(0+i,"0x0+$NODEID",i); + } +} +TEST(TestHex, checkCamelCase){ + TestHexTypes::test_hex(0xABCD, "0xABCD"); + TestHexTypes::test_hex(0xABCD, "0xabcd"); + TestHexTypes::test_hex(0xABCD, "0xAbCd"); + TestHexTypes::test_hex(0xABCD, "0xabCD"); +} + +TEST(TestHex, checkNodeCamelCase){ + for(uint8_t i = 0; i <=127; ++i){ + TestHexTypes::test_hex_node(i," $NODEID",i); + TestHexTypes::test_hex_node(i," $NODeID",i); + TestHexTypes::test_hex_node(i," $NodeId",i); + TestHexTypes::test_hex_node(i," $NodeID",i); + TestHexTypes::test_hex_node(i," $nodeID",i); + } +} + +TEST(TestHex, checkSpaces){ + TestHexTypes::test_hex(0xABCD, " 0xABCD "); + TestHexTypes::test_hex(0xABCD, "0xABCD "); + TestHexTypes::test_hex(0xABCD, " 0xABCD"); +} + +TEST(TestHex, checkNodeSpaces){ + for(uint8_t i = 0; i <=127; ++i){ + TestHexTypes::test_hex_node(i," $NODEID ",i); + TestHexTypes::test_hex_node(i," $NODEID",i); + TestHexTypes::test_hex_node(i,"$NODEID ",i); + TestHexTypes::test_hex_node(i+1,"$NODEID + 1",i); + TestHexTypes::test_hex_node(i+1,"$NODEID+ 1",i); + TestHexTypes::test_hex_node(i+1,"$NODEID+1",i); + TestHexTypes::test_hex_node(i+1,"$NODEID +1",i); + TestHexTypes::test_hex_node(i+1,"$NODEID +0x1 ",i); + TestHexTypes::test_hex_node(i+1,"$NODEID + 0x1",i); + + TestHexTypes::test_hex_node(i+1,"1 + $NODEID",i); + TestHexTypes::test_hex_node(i+1,"1+ $NODEID",i); + TestHexTypes::test_hex_node(i+1,"1+$NODEID",i); + TestHexTypes::test_hex_node(i+1,"1 +$NODEID",i); + TestHexTypes::test_hex_node(i+1,"0x1 + $NODEID ",i); + TestHexTypes::test_hex_node(i+1,"0x1 +$NODEID",i); + } +} +TEST(TestHex, checkCommonObjects){ + for(uint8_t i = 0; i <=127; ++i){ + TestHexTypes::test_hex_node( 0x80+i,"$NODEID+0x80",i); // EMCY + + TestHexTypes::test_hex_node(0x180+i,"$NODEID+0x180",i); //TPDO1 + TestHexTypes::test_hex_node(0x200+i,"$NODEID+0x200",i); //RPDO1 + + TestHexTypes::test_hex_node(0x280+i,"$NODEID+0x280",i); //TPDO2 + TestHexTypes::test_hex_node(0x300+i,"$NODEID+0x300",i); //RPDO2 + + TestHexTypes::test_hex_node(0x380+i,"$NODEID+0x380",i); //TPDO3 + TestHexTypes::test_hex_node(0x400+i,"$NODEID+0x400",i); //RPDO3 + + TestHexTypes::test_hex_node(0x480+i,"$NODEID+0x480",i); //TPDO4 + TestHexTypes::test_hex_node(0x500+i,"$NODEID+0x500",i); //RPDO4 + + TestHexTypes::test_hex_node(0x580+i,"$NODEID+0x580",i); // TSDO + TestHexTypes::test_hex_node(0x600+i,"$NODEID+0x600",i); // RSDO + + TestHexTypes::test_hex_node(0x700+i,"$NODEID+0x700",i); //NMT + + TestHexTypes::test_hex_node( 0x80+i,"0x80+$NODEID",i); // EMCY + + TestHexTypes::test_hex_node(0x180+i,"0x180+$NODEID",i); //TPDO1 + TestHexTypes::test_hex_node(0x200+i,"0x200+$NODEID",i); //RPDO1 + + TestHexTypes::test_hex_node(0x280+i,"0x280+$NODEID",i); //TPDO2 + TestHexTypes::test_hex_node(0x300+i,"0x300+$NODEID",i); //RPDO2 + TestHexTypes::test_hex_node(0x380+i,"0x380+$NODEID",i); //TPDO3 + TestHexTypes::test_hex_node(0x400+i,"0x400+$NODEID",i); //RPDO3 + + TestHexTypes::test_hex_node(0x480+i,"0x480+$NODEID",i); //TPDO4 + TestHexTypes::test_hex_node(0x500+i,"0x500+$NODEID",i); //RPDO4 + + TestHexTypes::test_hex_node(0x580+i,"0x580+$NODEID",i); // TSDO + TestHexTypes::test_hex_node(0x600+i,"0x600+$NODEID",i); // RSDO + + TestHexTypes::test_hex_node(0x700+i,"0x700+$NODEID",i); //NMT + } +} + +void set_access( canopen::ObjectDict::Entry &entry, std::string access); + +void testAccess(bool c, bool r, bool w, const char* variants[]){ + canopen::ObjectDict::Entry entry; + for(const char ** v = variants; *v; ++v){ + SCOPED_TRACE(*v); + entry.constant = !c; + entry.readable = !r; + entry.writable = !w; + + set_access(entry, *v); + + ASSERT_EQ(c, entry.constant); + ASSERT_EQ(r, entry.readable); + ASSERT_EQ(w, entry.writable); + } +} + +TEST(TestAccessString, TestRO) +{ + const char* variants[] = {"ro", "Ro", "rO", "RO", 0}; + testAccess(false, true, false, variants); +} + +TEST(TestAccessString, TestWO) +{ + const char* variants[] = {"wo", "Wo", "wO", "WO", 0}; + testAccess(false, false, true, variants); +} + +TEST(TestAccessString, TestRW) +{ + const char* variants[] = { + "rw" , "Rw" , "rW" , "Rw", + "rwr", "Rwr", "rWr", "Rwr", + "rwR", "RwR", "rWR", "RwR", + "rww", "Rww", "rWw", "Rww", + "rwW", "RwW", "rWW", "RwW", + 0}; + testAccess(false, true, true, variants); +} + +TEST(TestAccessString, TestConst) +{ + const char* variants[] = { + "const" , "Const" , "CONST", 0}; + testAccess(true, true, false, variants); +} + + +// Run all the tests that were declared with TEST() +int main(int argc, char **argv){ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} \ No newline at end of file diff --git a/Devices/Libraries/Ros/ros_canopen/canopen_motor_node/CHANGELOG.rst b/Devices/Libraries/Ros/ros_canopen/canopen_motor_node/CHANGELOG.rst new file mode 100755 index 0000000..d69dbca --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/canopen_motor_node/CHANGELOG.rst @@ -0,0 +1,210 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package canopen_motor_node +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.8.5 (2020-09-22) +------------------ + +0.8.4 (2020-08-22) +------------------ +* moved XmlRpcSettings to socketcan_interface +* Contributors: Mathias Lüdtke + +0.8.3 (2020-05-07) +------------------ +* Bump CMake version to avoid CMP0048 warning + Signed-off-by: ahcorde +* Contributors: ahcorde + +0.8.2 (2019-11-04) +------------------ + +0.8.1 (2019-07-14) +------------------ +* Set C++ standard to c++14 +* inherit LimitsHandle from LimitsHandleBase +* Contributors: Harsh Deshpande, Mathias Lüdtke + +0.8.0 (2018-07-11) +------------------ +* migrated to std::function and std::bind +* use std::isnan +* migrated to std::atomic +* migrated to std::unordered_map and std::unordered_set +* migrated to std pointers +* removed deprecated types +* introduced HandleLayerSharedPtr +* Contributors: Mathias Lüdtke + +0.7.8 (2018-05-04) +------------------ + +0.7.7 (2018-05-04) +------------------ +* added types for all function objects +* added types for all shared_ptrs +* error if muparser is not available +* address catkin_lint errors/warnings +* Contributors: Mathias Lüdtke + +0.7.6 (2017-08-30) +------------------ + +0.7.5 (2017-05-29) +------------------ + +0.7.4 (2017-04-25) +------------------ +* use portable boost::math::isnan +* Contributors: Mathias Lüdtke + +0.7.3 (2017-04-25) +------------------ +* use urdf::JointConstSharedPtr +* Contributors: Mathias Lüdtke + +0.7.2 (2017-03-28) +------------------ + +0.7.1 (2017-03-20) +------------------ +* Decouble RobotLayer by introducing HandleLayerBase +* Split layers into different headers and compile units +* do not call handleReadread in HandleLayer::handleRecover + this prevents a race condition, it is not needed anyway. +* protect ObjectVariables with mutex +* added test for norm function +* fix for joint limit handling +* introduced per-controller enforce_limits parameter +* implemented per-joint limits handling +* check if hardware interface matches mode +* implemented mixed-mode switching (`#197 `_) +* introduced joint reference for *res_it +* Contributors: Mathias Lüdtke, Michael Stoll + +0.7.0 (2016-12-13) +------------------ +* multi-mode controllers are not supported (`#197 `_) +* Adaption to https://github.com/ros-controls/ros_control/commit/afaf9403d1daf6e7f0a93e4a06aa9695e2883632 +* Contributors: Mathias Lüdtke, Michael Stoll + +0.6.5 (2016-12-10) +------------------ +* protect MotorChain setup with RosChain lock +* Merge pull request `#153 `_ from ipa-mdl/deprecated-canswitch + deprecated canSwitch +* fix for issue `#171 `_ +* Merge pull request `#168 `_ from ipa-mdl/state-filters + added filter chain for state values +* do not start driver if filter config fails +* added filter chain for state values +* log control period settings +* use update_period\_ for controll unless use_realtime_period is set true +* better initialize last_time\_ +* removed canSwitch implementation, added compile-time check for prepareSwitch +* exit code for generic error should be 1, not -1 +* styled and sorted CMakeLists.txt + * removed boilerplate comments + * indention + * reviewed exported dependencies +* styled and sorted package.xml +* update package URLs +* foward commands ony if enabled in doSwitch +* moved switch implemenation to non-RT prepareSwitch +* migrated to non-const prepareSwitch +* Splitted control_node.cpp into control_node.cpp, robot_layer.cpp and robot_layer.h +* renamed chain_ros.h to ros_chain.h, fixes `#126 `_ +* added strictness to service call, extend error message for doSwitch fails +* stop controllers that failed switching via service call +* stop all cotnroller joints if one failed to switch +* check for ready state before controller/mode switching +* improved init bevaviour: + * URDF is not read again (was not needed anyway= + * register interfaces only of first init +* remove unnecessary atomic reads +* halt motor if switch failed +* Fix for switching controllers with same mode +* More expressive comments for compile-time check +* Contributors: Mathias Lüdtke, Michael Stoll + +0.6.4 (2015-07-03) +------------------ + +0.6.3 (2015-06-30) +------------------ +* added motor prefix to allocator entry +* only register limit interfaces with actual limits +* added motor_layer settings +* Migrated to ClassAllocator helper +* do not run controller manager on shutdown +* migrated to motor plug-in +* working compile-time check +* reset commands without controllers to current value +* got rid of getModeMask +* added check for old unit factors +* added closing braces in default conversion strings +* forgot var_func assignment in constructor +* ensured UnitConverter access to factory is valid during lifetime +* add unit conversion based on muparser +* dependency on muparser +* Refer to ipa320/ros_control overlay +* migrated to new hwi switch interface +* atomic joint handle pointer +* test if mode is support, add No_Mode +* enabled limit enforcing again +* removed debug output +* Fixes https://github.com/ipa320/ros_canopen/issues/81 +* Enforce limits and current_state necessary for writing +* Merge remote-tracking branch 'mdl/indigo_dev' into refactor_sm + Conflicts: + canopen_402/include/canopen_402/canopen_402.h + canopen_402/src/canopen_402/canopen_402.cpp + canopen_motor_node/src/control_node.cpp +* refactored Layer mechanisms +* Fixes crash for unitialized boost pointer for ``target_vel_`` and ``target_pos_`` +* MotorChain is now a template +* early check if joint is listed in URDF +* introduced 'joint' parameter (defaults to 'name') +* 'modules' was renamed to 'nodes' +* Merge branch 'indigo_dev' of https://github.com/ipa320/ros_canopen into indigo_dev +* Merge pull request `#70 `_ from ipa-mdl/pluginlib + added plugin feature to socketcan_interface +* compile-time check for ros_control notifyHardwareInterface supportcompü +* added driver_plugin parameter for pluginlib look-up +* implemented threading in CANLayer +* removed SimpleLayer, migrated to Layer +* Layer::pending and Layer::halt are now virtual pure as well +* * Eliminates Internal State conflict + * Treats exceptions inside the state machine +* keep loop running +* proper locking for hardware interface switch (might fix `#61 `_) +* Merge branch 'auto_scale' into indigo_dev + Conflicts: + canopen_chain_node/include/canopen_chain_node/chain_ros.h +* Merge remote-tracking branch 'ipa320/indigo_dev' into indigo_dev + Conflicts: + canopen_chain_node/include/canopen_chain_node/chain_ros.h + canopen_motor_node/src/control_node.cpp +* removed MasterType form template +* Merge branch 'indigo_dev' into merge + Conflicts: + canopen_chain_node/include/canopen_chain_node/chain_ros.h + canopen_master/include/canopen_master/canopen.h + canopen_master/include/canopen_master/layer.h + canopen_master/src/node.cpp + canopen_motor_node/CMakeLists.txt + canopen_motor_node/src/control_node.cpp +* added unit factor parameter parsing +* Scale factor acquired from yaml file +* Contributors: Mathias Lüdtke, thiagodefreitas + +0.6.2 (2014-12-18) +------------------ + +0.6.1 (2014-12-15) +------------------ +* remove ipa_* and IPA_* prefixes +* fixed catkin_lint errors +* added descriptions and authors +* renamed ipa_canopen_motor_control to canopen_motor_node +* Contributors: Florian Weisshardt, Mathias Lüdtke diff --git a/Devices/Libraries/Ros/ros_canopen/canopen_motor_node/CMakeLists.txt b/Devices/Libraries/Ros/ros_canopen/canopen_motor_node/CMakeLists.txt new file mode 100755 index 0000000..c7fd960 --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/canopen_motor_node/CMakeLists.txt @@ -0,0 +1,128 @@ +cmake_minimum_required(VERSION 3.0.2) +project(canopen_motor_node) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_STANDARD 14) +endif() + +find_package(catkin REQUIRED + COMPONENTS + canopen_402 + canopen_chain_node + canopen_master + controller_manager + controller_manager_msgs + filters + hardware_interface + joint_limits_interface + roscpp + urdf +) + +find_package(Boost REQUIRED + COMPONENTS + thread +) + +find_package(PkgConfig) +pkg_check_modules(PC_MUPARSER QUIET muparser) +set(MUPARSER_DEFINITIONS ${PC_MUPARSER_CFLAGS_OTHER}) + +find_path(MUPARSER_INCLUDE_DIR muParser.h + HINTS ${PC_MUPARSER_INCLUDEDIR} ${PC_MUPARSER_INCLUDE_DIRS} +) + +find_library(MUPARSER_LIBRARY NAMES muparser libmurser + HINTS ${PC_MUPARSER_LIBDIR} ${PC_MUPARSER_LIBRARY_DIRS} ) + +include(FindPackageHandleStandardArgs) +# handle the QUIETLY and REQUIRED arguments and set MUPARSER_FOUND to TRUE +# if all listed variables are TRUE +find_package_handle_standard_args(MUPARSER DEFAULT_MSG + MUPARSER_LIBRARY MUPARSER_INCLUDE_DIR) +mark_as_advanced(MUPARSER_INCLUDE_DIR MUPARSER_LIBRARY ) + +if(NOT ${MUPARSER_FOUND}) + message(FATAL_ERROR "muparser library not found") +endif() + +set(MUPARSER_LIBRARIES ${MUPARSER_LIBRARY} ) +set(MUPARSER_INCLUDE_DIRS ${MUPARSER_INCLUDE_DIR} ) + +catkin_package( + INCLUDE_DIRS + include + LIBRARIES + canopen_motor + CATKIN_DEPENDS + canopen_402 + canopen_chain_node + canopen_master + controller_manager + hardware_interface + joint_limits_interface + roscpp + urdf + DEPENDS + Boost + MUPARSER +) + +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${MUPARSER_INCLUDE_DIRS} +) + +# canopen_motor +add_library(canopen_motor + src/controller_manager_layer.cpp + src/handle_layer.cpp + src/motor_chain.cpp + src/robot_layer.cpp +) +target_link_libraries(canopen_motor + ${catkin_LIBRARIES} + ${MUPARSER_LIBRARIES} +) +add_dependencies(canopen_motor + ${catkin_EXPORTED_TARGETS} +) + +# canopen_motor_node +add_executable(${PROJECT_NAME} + src/canopen_motor_chain_node.cpp +) +target_link_libraries(${PROJECT_NAME} + canopen_motor + ${catkin_LIBRARIES} +) +add_dependencies(${PROJECT_NAME} + ${catkin_EXPORTED_TARGETS} +) + +install( + TARGETS + canopen_motor + ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" +) + +if(CATKIN_ENABLE_TESTING) + catkin_add_gtest(${PROJECT_NAME}-test_muparser + test/test_muparser.cpp + ) + target_link_libraries(${PROJECT_NAME}-test_muparser + canopen_motor + ${catkin_LIBRARIES} + ${MUPARSER_LIBRARIES} + ) +endif() diff --git a/Devices/Libraries/Ros/ros_canopen/canopen_motor_node/include/canopen_motor_node/controller_manager_layer.h b/Devices/Libraries/Ros/ros_canopen/canopen_motor_node/include/canopen_motor_node/controller_manager_layer.h new file mode 100755 index 0000000..253ca82 --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/canopen_motor_node/include/canopen_motor_node/controller_manager_layer.h @@ -0,0 +1,44 @@ + +#ifndef CANOPEN_MOTOR_NODE_CONTROLLER_MANAGER_LAYER_H_ +#define CANOPEN_MOTOR_NODE_CONTROLLER_MANAGER_LAYER_H_ + +#include + +#include +#include +#include +#include + +// forward declarations +namespace controller_manager { + class ControllerManager; +} + +namespace canopen { + +class ControllerManagerLayer : public canopen::Layer { + std::shared_ptr cm_; + canopen::RobotLayerSharedPtr robot_; + ros::NodeHandle nh_; + + canopen::time_point last_time_; + std::atomic recover_; + const ros::Duration fixed_period_; + +public: + ControllerManagerLayer(const canopen::RobotLayerSharedPtr robot, const ros::NodeHandle &nh, const ros::Duration &fixed_period) + :Layer("ControllerManager"), robot_(robot), nh_(nh), fixed_period_(fixed_period) { + } + + virtual void handleRead(canopen::LayerStatus &status, const LayerState ¤t_state); + virtual void handleWrite(canopen::LayerStatus &status, const LayerState ¤t_state); + virtual void handleDiag(canopen::LayerReport &report) { /* nothing to do */ } + virtual void handleHalt(canopen::LayerStatus &status) { /* nothing to do (?) */ } + virtual void handleInit(canopen::LayerStatus &status); + virtual void handleRecover(canopen::LayerStatus &status); + virtual void handleShutdown(canopen::LayerStatus &status); +}; + +} // namespace canopen + +#endif /* CANOPEN_MOTOR_NODE_CONTROLLER_MANAGER_LAYER_H_ */ diff --git a/Devices/Libraries/Ros/ros_canopen/canopen_motor_node/include/canopen_motor_node/handle_layer.h b/Devices/Libraries/Ros/ros_canopen/canopen_motor_node/include/canopen_motor_node/handle_layer.h new file mode 100755 index 0000000..1361760 --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/canopen_motor_node/include/canopen_motor_node/handle_layer.h @@ -0,0 +1,176 @@ +#ifndef CANOPEN_MOTOR_NODE_HANDLE_LAYER_H_ +#define CANOPEN_MOTOR_NODE_HANDLE_LAYER_H_ + +#include +#include + +#include +#include +#include +#include // for ROS_VERSION_MINIMUM +#if ROS_VERSION_MINIMUM(1, 15, 0) +#include +#else +#include +#endif +#include +#include +#include +#include +#include +#include +#include +#include + +namespace canopen { + +class LimitsHandleBase { +public: + virtual void enforce(const ros::Duration &period) = 0; + virtual void reset() = 0; + virtual ~LimitsHandleBase() = default; +}; +typedef std::shared_ptr LimitsHandleBaseSharedPtr; + +class ObjectVariables { + const ObjectStorageSharedPtr storage_; + struct Getter { + std::shared_ptr val_ptr; + std::function func; + bool operator ()() { return func(*val_ptr); } + template Getter(const ObjectStorage::Entry &entry): func(std::bind(&Getter::readObject, entry, std::placeholders::_1)), val_ptr(new double) { } + template static bool readObject(ObjectStorage::Entry &entry, double &res){ + T val; + if(!entry.get(val)) return false; + res = val; + return true; + } + operator double*() const { return val_ptr.get(); } + }; + typedef std::unordered_map GetterMap; + GetterMap getters_; + boost::mutex mutex_; +public: + template static double* func(ObjectVariables &list, const canopen::ObjectDict::Key &key){ + typedef typename ObjectStorage::DataType
::type type; + return list.getters_.insert(std::make_pair(key, Getter(list.storage_->entry(key)))).first->second; + } + ObjectVariables(const ObjectStorageSharedPtr storage) : storage_(storage) {} + bool sync(){ + boost::mutex::scoped_lock lock(mutex_); + bool ok = true; + for(GetterMap::iterator it = getters_.begin(); it != getters_.end(); ++it){ + ok = it->second() && ok; + } + return ok; + } + double * getVariable(const std::string &n) { + boost::mutex::scoped_lock lock(mutex_); + try{ + if(n.find("obj") == 0){ + canopen::ObjectDict::Key key(n.substr(3)); + GetterMap::const_iterator it = getters_.find(key); + if(it != getters_.end()) return it->second; + return canopen::branch_type(storage_->dict_->get(key)->data_type)(*this, key); + } + } + catch( const std::exception &e){ + ROS_ERROR_STREAM("Could not find variable '" << n << "', reason: " << boost::diagnostic_information(e)); + } + return 0; + } +}; + +template<> inline double* ObjectVariables::func(ObjectVariables &, const canopen::ObjectDict::Key &){ return 0; } +template<> inline double* ObjectVariables::func(ObjectVariables &, const canopen::ObjectDict::Key &){ return 0; } +template<> inline double* ObjectVariables::func(ObjectVariables &, const canopen::ObjectDict::Key &){ return 0; } +template<> inline double* ObjectVariables::func(ObjectVariables &, const canopen::ObjectDict::Key &){ return 0; } + + +class HandleLayer: public canopen::HandleLayerBase { + canopen::MotorBaseSharedPtr motor_; + double pos_, vel_, eff_; + + double cmd_pos_, cmd_vel_, cmd_eff_; + + ObjectVariables variables_; + std::unique_ptr conv_target_pos_, conv_target_vel_, conv_target_eff_; + std::unique_ptr conv_pos_, conv_vel_, conv_eff_; + + filters::FilterChain filter_pos_, filter_vel_, filter_eff_; + XmlRpc::XmlRpcValue options_; + + hardware_interface::JointStateHandle jsh_; + hardware_interface::JointHandle jph_, jvh_, jeh_; + std::atomic jh_; + std::atomic forward_command_; + + typedef std::unordered_map< MotorBase::OperationMode,hardware_interface::JointHandle* > CommandMap; + CommandMap commands_; + + template hardware_interface::JointHandle* addHandle( T &iface, hardware_interface::JointHandle *jh, const std::vector & modes){ + + bool supported = false; + for(size_t i=0; i < modes.size(); ++i){ + if(motor_->isModeSupported(modes[i])){ + supported = true; + break; + } + } + if(!supported) return 0; + + iface.registerHandle(*jh); + + for(size_t i=0; i < modes.size(); ++i){ + commands_[modes[i]] = jh; + } + return jh; + } + + bool select(const canopen::MotorBase::OperationMode &m); + std::vector limits_; + bool enable_limits_; +public: + HandleLayer(const std::string &name, const canopen::MotorBaseSharedPtr & motor, const canopen::ObjectStorageSharedPtr storage, XmlRpc::XmlRpcValue & options); + static double * assignVariable(const std::string &name, double * ptr, const std::string &req) { return name == req ? ptr : 0; } + + CanSwitchResult canSwitch(const canopen::MotorBase::OperationMode &m); + bool switchMode(const canopen::MotorBase::OperationMode &m); + bool forwardForMode(const canopen::MotorBase::OperationMode &m); + + void registerHandle(hardware_interface::JointStateInterface &iface){ + iface.registerHandle(jsh_); + } + hardware_interface::JointHandle* registerHandle(hardware_interface::PositionJointInterface &iface, + const joint_limits_interface::JointLimits &limits, + const joint_limits_interface::SoftJointLimits *soft_limits = 0); + hardware_interface::JointHandle* registerHandle(hardware_interface::VelocityJointInterface &iface, + const joint_limits_interface::JointLimits &limits, + const joint_limits_interface::SoftJointLimits *soft_limits = 0); + hardware_interface::JointHandle* registerHandle(hardware_interface::EffortJointInterface &iface, + const joint_limits_interface::JointLimits &limits, + const joint_limits_interface::SoftJointLimits *soft_limits = 0); + + void enforceLimits(const ros::Duration &period, bool reset); + void enableLimits(bool enable); + + bool prepareFilters(canopen::LayerStatus &status); + +private: + virtual void handleRead(canopen::LayerStatus &status, const LayerState ¤t_state); + virtual void handleWrite(canopen::LayerStatus &status, const LayerState ¤t_state); + virtual void handleInit(canopen::LayerStatus &status); + virtual void handleDiag(canopen::LayerReport &report) { /* nothing to do */ } + virtual void handleShutdown(canopen::LayerStatus &status) { /* nothing to do */ } + virtual void handleHalt(canopen::LayerStatus &status) { /* TODO */ } + virtual void handleRecover(canopen::LayerStatus &status) { /* nothing to do */ } + +}; + +typedef std::shared_ptr HandleLayerSharedPtr; + +} // namespace canopen + + + +#endif /* INCLUDE_CANOPEN_MOTOR_NODE_HANDLE_LAYER_H_ */ diff --git a/Devices/Libraries/Ros/ros_canopen/canopen_motor_node/include/canopen_motor_node/handle_layer_base.h b/Devices/Libraries/Ros/ros_canopen/canopen_motor_node/include/canopen_motor_node/handle_layer_base.h new file mode 100755 index 0000000..13b3096 --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/canopen_motor_node/include/canopen_motor_node/handle_layer_base.h @@ -0,0 +1,45 @@ + +#ifndef CANOPEN_MOTOR_NODE_HANDLE_LAYER_BASE_H_ +#define CANOPEN_MOTOR_NODE_HANDLE_LAYER_BASE_H_ + +#include +#include +#include + +namespace canopen { + +class HandleLayerBase: public canopen::Layer{ +public: + HandleLayerBase(const std::string &name) : Layer(name) {} + + enum CanSwitchResult{ + NotSupported, + NotReadyToSwitch, + ReadyToSwitch, + NoNeedToSwitch + }; + + virtual CanSwitchResult canSwitch(const canopen::MotorBase::OperationMode &m) = 0; + virtual bool switchMode(const canopen::MotorBase::OperationMode &m) = 0; + + virtual bool forwardForMode(const canopen::MotorBase::OperationMode &m) = 0; + + virtual void registerHandle(hardware_interface::JointStateInterface &iface) = 0; + virtual hardware_interface::JointHandle* registerHandle(hardware_interface::PositionJointInterface &iface, + const joint_limits_interface::JointLimits &limits, + const joint_limits_interface::SoftJointLimits *soft_limits = 0) = 0; + virtual hardware_interface::JointHandle* registerHandle(hardware_interface::VelocityJointInterface &iface, + const joint_limits_interface::JointLimits &limits, + const joint_limits_interface::SoftJointLimits *soft_limits = 0) = 0; + virtual hardware_interface::JointHandle* registerHandle(hardware_interface::EffortJointInterface &iface, + const joint_limits_interface::JointLimits &limits, + const joint_limits_interface::SoftJointLimits *soft_limits = 0) = 0; + + virtual void enforceLimits(const ros::Duration &period, bool reset) = 0; + virtual void enableLimits(bool enable) = 0; +}; +typedef std::shared_ptr HandleLayerBaseSharedPtr; + +} // namespace canopen + +#endif /* CANOPEN_MOTOR_NODE_HANDLE_LAYER_BASE_H_ */ diff --git a/Devices/Libraries/Ros/ros_canopen/canopen_motor_node/include/canopen_motor_node/motor_chain.h b/Devices/Libraries/Ros/ros_canopen/canopen_motor_node/include/canopen_motor_node/motor_chain.h new file mode 100755 index 0000000..41df00d --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/canopen_motor_node/include/canopen_motor_node/motor_chain.h @@ -0,0 +1,33 @@ + +#ifndef CANOPEN_MOTOR_NODE_MOTOR_CHAIN_H_ +#define CANOPEN_MOTOR_NODE_MOTOR_CHAIN_H_ + +#include + +#include +#include + +#include +#include + + +namespace canopen { + +class MotorChain : public canopen::RosChain { + ClassAllocator motor_allocator_; + std::shared_ptr< canopen::LayerGroupNoDiag > motors_; + RobotLayerSharedPtr robot_layer_; + + std::shared_ptr cm_; + + virtual bool nodeAdded(XmlRpc::XmlRpcValue ¶ms, const canopen::NodeSharedPtr &node, const LoggerSharedPtr &logger); + +public: + MotorChain(const ros::NodeHandle &nh, const ros::NodeHandle &nh_priv); + + virtual bool setup_chain(); +}; + +} // namespace canopen + +#endif /* INCLUDE_CANOPEN_MOTOR_NODE_MOTOR_CHAIN_H_ */ diff --git a/Devices/Libraries/Ros/ros_canopen/canopen_motor_node/include/canopen_motor_node/robot_layer.h b/Devices/Libraries/Ros/ros_canopen/canopen_motor_node/include/canopen_motor_node/robot_layer.h new file mode 100755 index 0000000..99e763e --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/canopen_motor_node/include/canopen_motor_node/robot_layer.h @@ -0,0 +1,64 @@ + +#ifndef CANOPEN_MOTOR_NODE_ROBOT_LAYER_H_ +#define CANOPEN_MOTOR_NODE_ROBOT_LAYER_H_ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + + +namespace canopen { + +class RobotLayer : public LayerGroupNoDiag, public hardware_interface::RobotHW{ + hardware_interface::JointStateInterface state_interface_; + hardware_interface::PositionJointInterface pos_interface_; + hardware_interface::VelocityJointInterface vel_interface_; + hardware_interface::EffortJointInterface eff_interface_; + + joint_limits_interface::PositionJointSoftLimitsInterface pos_soft_limits_interface_; + joint_limits_interface::PositionJointSaturationInterface pos_saturation_interface_; + joint_limits_interface::VelocityJointSoftLimitsInterface vel_soft_limits_interface_; + joint_limits_interface::VelocityJointSaturationInterface vel_saturation_interface_; + joint_limits_interface::EffortJointSoftLimitsInterface eff_soft_limits_interface_; + joint_limits_interface::EffortJointSaturationInterface eff_saturation_interface_; + + ros::NodeHandle nh_; + urdf::Model urdf_; + + typedef std::unordered_map< std::string, HandleLayerBaseSharedPtr > HandleMap; + HandleMap handles_; + struct SwitchData { + HandleLayerBaseSharedPtr handle; + canopen::MotorBase::OperationMode mode; + bool enforce_limits; + }; + typedef std::vector SwitchContainer; + typedef std::unordered_map SwitchMap; + SwitchMap switch_map_; + + std::atomic first_init_; + + void stopControllers(const std::vector controllers); +public: + void add(const std::string &name, HandleLayerBaseSharedPtr handle); + RobotLayer(ros::NodeHandle nh); + urdf::JointConstSharedPtr getJoint(const std::string &n) const { return urdf_.getJoint(n); } + + virtual void handleInit(canopen::LayerStatus &status); + void enforce(const ros::Duration &period, bool reset); + virtual bool prepareSwitch(const std::list &start_list, const std::list &stop_list); + virtual void doSwitch(const std::list &start_list, const std::list &stop_list); +}; + +typedef std::shared_ptr RobotLayerSharedPtr; + +} // namespace canopen + +#endif diff --git a/Devices/Libraries/Ros/ros_canopen/canopen_motor_node/include/canopen_motor_node/unit_converter.h b/Devices/Libraries/Ros/ros_canopen/canopen_motor_node/include/canopen_motor_node/unit_converter.h new file mode 100755 index 0000000..4a9bcb0 --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/canopen_motor_node/include/canopen_motor_node/unit_converter.h @@ -0,0 +1,89 @@ + +#ifndef CANOPEN_MOTOR_NODE_UNIT_CONVERTER_H_ +#define CANOPEN_MOTOR_NODE_UNIT_CONVERTER_H_ + +#include +#include +#include + +#include +#include "muParser.h" + +namespace canopen { +class UnitConverter{ +public: + typedef std::function GetVarFuncType; + + UnitConverter(const std::string &expression, GetVarFuncType var_func) + : var_func_(var_func) + { + parser_.SetVarFactory(UnitConverter::createVariable, this); + + parser_.DefineConst("pi", M_PI); + parser_.DefineConst("nan", std::numeric_limits::quiet_NaN()); + + parser_.DefineFun("rad2deg", UnitConverter::rad2deg); + parser_.DefineFun("deg2rad", UnitConverter::deg2rad); + parser_.DefineFun("norm", UnitConverter::norm); + parser_.DefineFun("smooth", UnitConverter::smooth); + parser_.DefineFun("avg", UnitConverter::avg); + + parser_.SetExpr(expression); + } + + void reset(){ + for(variable_ptr_list::iterator it = var_list_.begin(); it != var_list_.end(); ++it){ + **it = std::numeric_limits::quiet_NaN(); + } + } + double evaluate() { int num; return parser_.Eval(num)[0]; } +private: + typedef std::shared_ptr variable_ptr; + typedef std::list variable_ptr_list; + + static double* createVariable(const char *name, void * userdata) { + UnitConverter * uc = static_cast(userdata); + double *p = uc->var_func_ ? uc->var_func_(name) : 0; + if(!p){ + p = new double(std::numeric_limits::quiet_NaN()); + uc->var_list_.push_back(variable_ptr(p)); + } + return p; + } + variable_ptr_list var_list_; + GetVarFuncType var_func_; + + mu::Parser parser_; + + static double rad2deg(double r){ + return r*180.0/M_PI; + } + static double deg2rad(double d){ + return d*M_PI/180.0; + } + static double norm(double val, double min, double max){ + while(val >= max) val -= (max-min); + while(val < min) val += (max-min); + return val; + } + static double smooth(double val, double old_val, double alpha){ + if(std::isnan(val)) return 0; + if(std::isnan(old_val)) return val; + return alpha*val + (1.0-alpha)*old_val; + } + static double avg(const double *vals, int num) + { + double s = 0.0; + int i=0; + for (; i + + canopen_motor_node + 0.8.5 + canopen_chain_node specialization for handling of canopen_402 motor devices. It facilitates interface abstraction with ros_control. + + Mathias Lüdtke + Mathias Lüdtke + + LGPLv3 + + http://wiki.ros.org/canopen_motor_node + https://github.com/ros-industrial/ros_canopen + https://github.com/ros-industrial/ros_canopen/issues + + catkin + + libboost-dev + libboost-thread-dev + canopen_402 + canopen_chain_node + canopen_master + controller_manager + filters + hardware_interface + joint_limits_interface + muparser + roscpp + urdf + + controller_manager_msgs + controller_manager_msgs + + rosunit + + diff --git a/Devices/Libraries/Ros/ros_canopen/canopen_motor_node/src/canopen_motor_chain_node.cpp b/Devices/Libraries/Ros/ros_canopen/canopen_motor_node/src/canopen_motor_chain_node.cpp new file mode 100755 index 0000000..4da397c --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/canopen_motor_node/src/canopen_motor_chain_node.cpp @@ -0,0 +1,22 @@ +#include + +using namespace canopen; + + +int main(int argc, char** argv){ + ros::init(argc, argv, "canopen_motor_chain_node"); + ros::AsyncSpinner spinner(0); + spinner.start(); + + ros::NodeHandle nh; + ros::NodeHandle nh_priv("~"); + + MotorChain chain(nh, nh_priv); + + if(!chain.setup()){ + return 1; + } + + ros::waitForShutdown(); + return 0; +} diff --git a/Devices/Libraries/Ros/ros_canopen/canopen_motor_node/src/controller_manager_layer.cpp b/Devices/Libraries/Ros/ros_canopen/canopen_motor_node/src/controller_manager_layer.cpp new file mode 100755 index 0000000..648b451 --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/canopen_motor_node/src/controller_manager_layer.cpp @@ -0,0 +1,54 @@ + +#include +#include + +using namespace canopen; + +void ControllerManagerLayer::handleRead(canopen::LayerStatus &status, const LayerState ¤t_state) { + if(current_state > Shutdown){ + if(!cm_) status.error("controller_manager is not intialized"); + } +} + +void ControllerManagerLayer::handleWrite(canopen::LayerStatus &status, const LayerState ¤t_state) { + if(current_state > Shutdown){ + if(!cm_){ + status.error("controller_manager is not intialized"); + }else{ + time_point abs_now = canopen::get_abs_time(); + ros::Time now = ros::Time::now(); + + ros::Duration period = fixed_period_; + + if(period.isZero()) { + period.fromSec(boost::chrono::duration(abs_now -last_time_).count()); + } + + last_time_ = abs_now; + + bool recover = recover_.exchange(false); + cm_->update(now, period, recover); + robot_->enforce(period, recover); + } + } +} + +void ControllerManagerLayer::handleInit(canopen::LayerStatus &status) { + if(cm_){ + status.warn("controller_manager is already intialized"); + }else{ + recover_ = true; + last_time_ = canopen::get_abs_time(); + cm_.reset(new controller_manager::ControllerManager(robot_.get(), nh_)); + } +} + +void ControllerManagerLayer::handleRecover(canopen::LayerStatus &status) { + if(!cm_) status.error("controller_manager is not intialized"); + else recover_ = true; +} + +void ControllerManagerLayer::handleShutdown(canopen::LayerStatus &status) { + cm_.reset(); +} + diff --git a/Devices/Libraries/Ros/ros_canopen/canopen_motor_node/src/handle_layer.cpp b/Devices/Libraries/Ros/ros_canopen/canopen_motor_node/src/handle_layer.cpp new file mode 100755 index 0000000..484f56c --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/canopen_motor_node/src/handle_layer.cpp @@ -0,0 +1,215 @@ + +#include +#include "interface_mapping.h" + +using namespace canopen; + + +template class LimitsHandle : public LimitsHandleBase { + T limits_handle_; +public: + LimitsHandle(const T &handle) : limits_handle_(handle) {} + virtual void enforce(const ros::Duration &period) override { limits_handle_.enforceLimits(period); } + virtual void reset() override {} +}; + +template<> void LimitsHandle::reset() { limits_handle_.reset(); } +template<>void LimitsHandle::reset() { limits_handle_.reset(); } + +bool HandleLayer::select(const MotorBase::OperationMode &m){ + CommandMap::iterator it = commands_.find(m); + if(it == commands_.end()) return false; + jh_ = it->second; + return true; +} + +HandleLayer::HandleLayer(const std::string &name, const MotorBaseSharedPtr & motor, const ObjectStorageSharedPtr storage, XmlRpc::XmlRpcValue & options) +: HandleLayerBase(name + " Handle"), motor_(motor), variables_(storage), jsh_(name, &pos_, &vel_, &eff_), jph_(jsh_, &cmd_pos_), jvh_(jsh_, &cmd_vel_), jeh_(jsh_, &cmd_eff_), jh_(0), forward_command_(false), + filter_pos_("double"), filter_vel_("double"), filter_eff_("double"), options_(options), enable_limits_(true) +{ + commands_[MotorBase::No_Mode] = 0; + + std::string p2d("rint(rad2deg(pos)*1000)"), v2d("rint(rad2deg(vel)*1000)"), e2d("rint(eff)"); + std::string p2r("deg2rad(obj6064)/1000"), v2r("deg2rad(obj606C)/1000"), e2r("0"); + + if(options.hasMember("pos_unit_factor") || options.hasMember("vel_unit_factor") || options.hasMember("eff_unit_factor")){ + const std::string reason("*_unit_factor parameters are not supported anymore, please migrate to conversion functions."); + ROS_FATAL_STREAM(reason); + throw std::invalid_argument(reason); + } + + if(options.hasMember("pos_to_device")) p2d = (const std::string&) options["pos_to_device"]; + if(options.hasMember("pos_from_device")) p2r = (const std::string&) options["pos_from_device"]; + + if(options.hasMember("vel_to_device")) v2d = (const std::string&) options["vel_to_device"]; + if(options.hasMember("vel_from_device")) v2r = (const std::string&) options["vel_from_device"]; + + if(options.hasMember("eff_to_device")) e2d = (const std::string&) options["eff_to_device"]; + if(options.hasMember("eff_from_device")) e2r = (const std::string&) options["eff_from_device"]; + + conv_target_pos_.reset(new UnitConverter(p2d, std::bind(assignVariable, "pos", &cmd_pos_, std::placeholders::_1))); + conv_target_vel_.reset(new UnitConverter(v2d, std::bind(assignVariable, "vel", &cmd_vel_, std::placeholders::_1))); + conv_target_eff_.reset(new UnitConverter(e2d, std::bind(assignVariable, "eff", &cmd_eff_, std::placeholders::_1))); + + conv_pos_.reset(new UnitConverter(p2r, std::bind(&ObjectVariables::getVariable, &variables_, std::placeholders::_1))); + conv_vel_.reset(new UnitConverter(v2r, std::bind(&ObjectVariables::getVariable, &variables_, std::placeholders::_1))); + conv_eff_.reset(new UnitConverter(e2r, std::bind(&ObjectVariables::getVariable, &variables_, std::placeholders::_1))); +} + +HandleLayer::CanSwitchResult HandleLayer::canSwitch(const MotorBase::OperationMode &m){ + if(!motor_->isModeSupported(m) || commands_.find(m) == commands_.end()){ + return NotSupported; + }else if(motor_->getMode() == m){ + return NoNeedToSwitch; + }else if(motor_->getLayerState() == Ready){ + return ReadyToSwitch; + }else{ + return NotReadyToSwitch; + } +} + +bool HandleLayer::switchMode(const MotorBase::OperationMode &m){ + if(motor_->getMode() != m){ + forward_command_ = false; + jh_ = 0; // disconnect handle + if(!motor_->enterModeAndWait(m)){ + ROS_ERROR_STREAM(jsh_.getName() << "could not enter mode " << (int)m); + LayerStatus s; + motor_->halt(s); + return false; + } + } + return select(m); +} + +bool HandleLayer::forwardForMode(const MotorBase::OperationMode &m){ + if(motor_->getMode() == m){ + forward_command_ = true; + return true; + } + return false; +} + + +template void addLimitsHandle(std::vector &limits, const T &t) { + LimitsHandleBaseSharedPtr p = std::make_shared>(t); + limits.push_back(p); +} + +hardware_interface::JointHandle* HandleLayer::registerHandle(hardware_interface::PositionJointInterface &iface, + const joint_limits_interface::JointLimits &limits, + const joint_limits_interface::SoftJointLimits *soft_limits){ + hardware_interface::JointHandle* h = addHandle(iface, &jph_, g_interface_mapping.getInterfaceModes("hardware_interface::PositionJointInterface")); + if(h && limits.has_position_limits){ + addLimitsHandle(limits_, joint_limits_interface::PositionJointSaturationHandle(*h, limits)); + if(soft_limits){ + addLimitsHandle(limits_, joint_limits_interface::PositionJointSoftLimitsHandle(*h, limits, *soft_limits)); + } + } + return h; +} + +hardware_interface::JointHandle* HandleLayer::registerHandle(hardware_interface::VelocityJointInterface &iface, + const joint_limits_interface::JointLimits&limits, + const joint_limits_interface::SoftJointLimits *soft_limits){ + hardware_interface::JointHandle* h = addHandle(iface, &jvh_, g_interface_mapping.getInterfaceModes("hardware_interface::VelocityJointInterface")); + if(h && limits.has_velocity_limits){ + addLimitsHandle(limits_, joint_limits_interface::VelocityJointSaturationHandle(*h, limits)); + if(soft_limits){ + addLimitsHandle(limits_, joint_limits_interface::VelocityJointSoftLimitsHandle(*h, limits, *soft_limits)); + } + } + return h; +} + +hardware_interface::JointHandle* HandleLayer::registerHandle(hardware_interface::EffortJointInterface &iface, + const joint_limits_interface::JointLimits&limits, + const joint_limits_interface::SoftJointLimits *soft_limits){ + hardware_interface::JointHandle* h = addHandle(iface, &jeh_, g_interface_mapping.getInterfaceModes("hardware_interface::EffortJointInterface")); + if(h && limits.has_effort_limits){ + addLimitsHandle(limits_, joint_limits_interface::EffortJointSaturationHandle(*h, limits)); + if(soft_limits){ + addLimitsHandle(limits_, joint_limits_interface::EffortJointSoftLimitsHandle(*h, limits, *soft_limits)); + } + } + return h; +} + +void HandleLayer::handleRead(LayerStatus &status, const LayerState ¤t_state) { + if(current_state > Shutdown){ + variables_.sync(); + filter_pos_.update(conv_pos_->evaluate(), pos_); + filter_vel_.update(conv_vel_->evaluate(), vel_); + filter_eff_.update(conv_eff_->evaluate(), eff_); + } +} +void HandleLayer::handleWrite(LayerStatus &status, const LayerState ¤t_state) { + if(current_state == Ready){ + hardware_interface::JointHandle* jh = 0; + if(forward_command_) jh = jh_; + + if(jh == &jph_){ + motor_->setTarget(conv_target_pos_->evaluate()); + cmd_vel_ = vel_; + cmd_eff_ = eff_; + }else if(jh == &jvh_){ + motor_->setTarget(conv_target_vel_->evaluate()); + cmd_pos_ = pos_; + cmd_eff_ = eff_; + }else if(jh == &jeh_){ + motor_->setTarget(conv_target_eff_->evaluate()); + cmd_pos_ = pos_; + cmd_vel_ = vel_; + }else{ + cmd_pos_ = pos_; + cmd_vel_ = vel_; + cmd_eff_ = eff_; + if(jh) status.warn("unsupported mode active"); + } + } +} + +bool prepareFilter(const std::string& joint_name, const std::string& filter_name, filters::FilterChain &filter, XmlRpc::XmlRpcValue & options, canopen::LayerStatus &status){ + filter.clear(); + if(options.hasMember(filter_name)){ + if(!filter.configure(options[filter_name],joint_name + "/" + filter_name)){ + status.error("could not configure " + filter_name+ " for " + joint_name); + return false; + } + } + + return true; +} + +bool HandleLayer::prepareFilters(canopen::LayerStatus &status){ + return prepareFilter(jsh_.getName(), "position_filters", filter_pos_, options_, status) && + prepareFilter(jsh_.getName(), "velocity_filters", filter_vel_, options_, status) && + prepareFilter(jsh_.getName(), "effort_filters", filter_eff_, options_, status); +} + +void HandleLayer::handleInit(LayerStatus &status){ + // TODO: implement proper init + conv_pos_->reset(); + conv_vel_->reset(); + conv_eff_->reset(); + conv_target_pos_->reset(); + conv_target_vel_->reset(); + conv_target_eff_->reset(); + + + if(prepareFilters(status)) + { + handleRead(status, Layer::Ready); + } +} + +void HandleLayer::enforceLimits(const ros::Duration &period, bool reset){ + for(std::vector::iterator it = limits_.begin(); it != limits_.end(); ++it){ + if(reset) (*it)->reset(); + if(enable_limits_) (*it)->enforce(period); + } +} + +void HandleLayer::enableLimits(bool enable){ + enable_limits_ = enable; +} diff --git a/Devices/Libraries/Ros/ros_canopen/canopen_motor_node/src/interface_mapping.h b/Devices/Libraries/Ros/ros_canopen/canopen_motor_node/src/interface_mapping.h new file mode 100755 index 0000000..fdcd37c --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/canopen_motor_node/src/interface_mapping.h @@ -0,0 +1,48 @@ + +#ifndef INTERFACE_MAPPING_H_ +#define INTERFACE_MAPPING_H_ + +#include + +#include +#include +#include + +#include + +class InterfaceMapping { + typedef boost::bimap, boost::bimaps::set_of > bimap_type; + bimap_type mapping_; +public: + InterfaceMapping(){ + mapping_.insert(bimap_type::value_type("hardware_interface::PositionJointInterface" ,canopen::MotorBase::Profiled_Position)); + mapping_.insert(bimap_type::value_type("hardware_interface::PositionJointInterface" ,canopen::MotorBase::Interpolated_Position)); + mapping_.insert(bimap_type::value_type("hardware_interface::PositionJointInterface" ,canopen::MotorBase::Cyclic_Synchronous_Position)); + + mapping_.insert(bimap_type::value_type("hardware_interface::VelocityJointInterface" ,canopen::MotorBase::Velocity)); + mapping_.insert(bimap_type::value_type("hardware_interface::VelocityJointInterface" ,canopen::MotorBase::Profiled_Velocity)); + mapping_.insert(bimap_type::value_type("hardware_interface::VelocityJointInterface" ,canopen::MotorBase::Cyclic_Synchronous_Velocity)); + + mapping_.insert(bimap_type::value_type("hardware_interface::EffortJointInterface" ,canopen::MotorBase::Profiled_Torque)); + mapping_.insert(bimap_type::value_type("hardware_interface::EffortJointInterface" ,canopen::MotorBase::Cyclic_Synchronous_Torque)); + } + std::vector getInterfaceModes(const std::string &interface){ + std::vector modes; + BOOST_FOREACH(bimap_type::left_reference i, mapping_.left.equal_range(interface)){ + modes.push_back(i.second); + } + return modes; + } + bool hasConflict(const std::string &interface, canopen::MotorBase::OperationMode mode){ + bimap_type::right_const_iterator it; + if((it = mapping_.right.find(mode)) != mapping_.right.end()){ + return it->second != interface; + } + return false; + } + +}; + +extern InterfaceMapping g_interface_mapping; + +#endif /* INTERFACE_MAPPING_H_ */ diff --git a/Devices/Libraries/Ros/ros_canopen/canopen_motor_node/src/motor_chain.cpp b/Devices/Libraries/Ros/ros_canopen/canopen_motor_node/src/motor_chain.cpp new file mode 100755 index 0000000..cbd37f9 --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/canopen_motor_node/src/motor_chain.cpp @@ -0,0 +1,84 @@ + +#include +#include +#include +using namespace canopen; + +MotorChain::MotorChain(const ros::NodeHandle &nh, const ros::NodeHandle &nh_priv) : + RosChain(nh, nh_priv), motor_allocator_("canopen_402", "canopen::MotorBase::Allocator") {} + +bool MotorChain::nodeAdded(XmlRpc::XmlRpcValue ¶ms, const canopen::NodeSharedPtr &node, const LoggerSharedPtr &logger) +{ + std::string name = params["name"]; + std::string &joint = name; + if(params.hasMember("joint")) joint.assign(params["joint"]); + + if(!robot_layer_->getJoint(joint)){ + ROS_ERROR_STREAM("joint " + joint + " was not found in URDF"); + return false; + } + + std::string alloc_name = "canopen::Motor402::Allocator"; + if(params.hasMember("motor_allocator")) alloc_name.assign(params["motor_allocator"]); + + XmlRpcSettings settings; + if(params.hasMember("motor_layer")) settings = params["motor_layer"]; + + MotorBaseSharedPtr motor; + + try{ + motor = motor_allocator_.allocateInstance(alloc_name, name + "_motor", node->getStorage(), settings); + } + catch( const std::exception &e){ + std::string info = boost::diagnostic_information(e); + ROS_ERROR_STREAM(info); + return false; + } + + if(!motor){ + ROS_ERROR_STREAM("Could not allocate motor."); + return false; + } + + motor->registerDefaultModes(node->getStorage()); + motors_->add(motor); + logger->add(motor); + + HandleLayerSharedPtr handle = std::make_shared(joint, motor, node->getStorage(), params); + + canopen::LayerStatus s; + if(!handle->prepareFilters(s)){ + ROS_ERROR_STREAM(s.reason()); + return false; + } + + robot_layer_->add(joint, handle); + logger->add(handle); + + return true; +} + +bool MotorChain::setup_chain() { + motors_.reset(new LayerGroupNoDiag("402 Layer")); + robot_layer_.reset(new RobotLayer(nh_)); + + ros::Duration dur(0.0) ; + + if(RosChain::setup_chain()){ + add(motors_); + add(robot_layer_); + + if(!nh_.param("use_realtime_period", false)){ + dur.fromSec(boost::chrono::duration(update_duration_).count()); + ROS_INFO_STREAM("Using fixed control period: " << dur); + }else{ + ROS_INFO("Using real-time control period"); + } + cm_.reset(new ControllerManagerLayer(robot_layer_, nh_, dur)); + add(cm_); + + return true; + } + + return false; +} diff --git a/Devices/Libraries/Ros/ros_canopen/canopen_motor_node/src/robot_layer.cpp b/Devices/Libraries/Ros/ros_canopen/canopen_motor_node/src/robot_layer.cpp new file mode 100755 index 0000000..a9bee04 --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/canopen_motor_node/src/robot_layer.cpp @@ -0,0 +1,246 @@ + + +#include +#include +#include + +#include +#include + +#include + +#include "interface_mapping.h" + +using namespace canopen; + +InterfaceMapping g_interface_mapping; + +void RobotLayer::stopControllers(const std::vector controllers){ + controller_manager_msgs::SwitchController srv; + srv.request.stop_controllers = controllers; + srv.request.strictness = srv.request.BEST_EFFORT; + boost::thread call(std::bind(ros::service::call, "controller_manager/switch_controller", srv)); + call.detach(); +} + +void RobotLayer::add(const std::string &name, HandleLayerBaseSharedPtr handle){ + LayerGroupNoDiag::add(handle); + handles_.insert(std::make_pair(name, handle)); +} + +RobotLayer::RobotLayer(ros::NodeHandle nh) : LayerGroupNoDiag("RobotLayer"), nh_(nh), first_init_(true) +{ + registerInterface(&state_interface_); + registerInterface(&pos_interface_); + registerInterface(&vel_interface_); + registerInterface(&eff_interface_); + + urdf_.initParam("robot_description"); +} + +void RobotLayer::handleInit(LayerStatus &status){ + if(first_init_){ + for(HandleMap::iterator it = handles_.begin(); it != handles_.end(); ++it){ + joint_limits_interface::JointLimits limits; + joint_limits_interface::SoftJointLimits soft_limits; + + urdf::JointConstSharedPtr joint = getJoint(it->first); + + if(!joint){ + status.error("joint " + it->first + " not found"); + return; + } + + bool has_joint_limits = joint_limits_interface::getJointLimits(joint, limits); + + has_joint_limits = joint_limits_interface::getJointLimits(it->first, nh_, limits) || has_joint_limits; + + bool has_soft_limits = has_joint_limits && joint_limits_interface::getSoftJointLimits(joint, soft_limits); + + if(!has_joint_limits){ + ROS_WARN_STREAM("No limits found for " << it->first); + } + + it->second->registerHandle(state_interface_); + + const hardware_interface::JointHandle *h = 0; + + it->second->registerHandle(pos_interface_, limits, has_soft_limits ? &soft_limits : 0); + it->second->registerHandle(vel_interface_, limits, has_soft_limits ? &soft_limits : 0); + it->second->registerHandle(eff_interface_, limits, has_soft_limits ? &soft_limits : 0); + } + first_init_ = false; + } + LayerGroupNoDiag::handleInit(status); +} + +void RobotLayer::enforce(const ros::Duration &period, bool reset){ + for(HandleMap::iterator it = handles_.begin(); it != handles_.end(); ++it){ + it->second->enforceLimits(period, reset); + } +} +class ModeLookup { + int default_mode_; + bool has_default_mode_; + std::map lookup_; + bool has_lookup_; +public: + ModeLookup(ros::NodeHandle &nh_c){ + has_default_mode_ = nh_c.getParam("required_drive_mode", default_mode_); + has_lookup_ = nh_c.getParam("required_drive_modes", lookup_); + } + bool hasModes() { return has_default_mode_ || has_lookup_; } + bool hasMixedModes() { return has_lookup_; } + bool getMode(MotorBase::OperationMode &om, const std::string &key) { + std::map::iterator f = lookup_.find(key); + if(f != lookup_.end()){ + om = MotorBase::OperationMode(f->second); + return true; + }else if (has_default_mode_) { + om = MotorBase::OperationMode(default_mode_); + return true; + } + return false; + } +}; +bool RobotLayer::prepareSwitch(const std::list &start_list, const std::list &stop_list) { + // compile-time check for mode switching support in ros_control + (void) &hardware_interface::RobotHW::prepareSwitch; // please upgrade to ros_control/contoller_manager 0.9.4 or newer + + // stop handles + for (std::list::const_iterator controller_it = stop_list.begin(); controller_it != stop_list.end(); ++controller_it){ + + if(switch_map_.find(controller_it->name) == switch_map_.end()){ + ROS_ERROR_STREAM(controller_it->name << " was not started before"); + return false; + } + } + + // start handles + for (std::list::const_iterator controller_it = start_list.begin(); controller_it != start_list.end(); ++controller_it){ + SwitchContainer to_switch; + ros::NodeHandle nh(nh_, controller_it->name); + ModeLookup ml(nh); + + std::set claimed_interfaces; + + if(controller_it->claimed_resources.size() > 0){ + for (std::vector::const_iterator cres_it = controller_it->claimed_resources.begin(); cres_it != controller_it->claimed_resources.end(); ++cres_it){ + for (std::set::const_iterator res_it = cres_it->resources.begin(); res_it != cres_it->resources.end(); ++res_it){ + claimed_interfaces.insert(cres_it->hardware_interface); + if(!ml.hasModes()){ + ROS_ERROR_STREAM("Please set required_drive_mode(s) for controller " << controller_it->name); + return false; + } + if(claimed_interfaces.size() > 1 && !ml.hasMixedModes()){ + ROS_ERROR_STREAM("controller "<< controller_it->name << " has mixed interfaces, please set required_drive_modes."); + return false; + } + + std::unordered_map< std::string, HandleLayerBaseSharedPtr >::const_iterator h_it = handles_.find(*res_it); + + const std::string & joint = *res_it; + + if(h_it == handles_.end()){ + ROS_ERROR_STREAM(joint << " not found"); + return false; + } + SwitchData sd; + sd.enforce_limits = nh.param("enforce_limits", true); + + if(!ml.getMode(sd.mode, joint)){ + ROS_ERROR_STREAM("could not determine drive mode for " << joint); + return false; + } + + if(g_interface_mapping.hasConflict(cres_it->hardware_interface, sd.mode)){ + ROS_ERROR_STREAM(cres_it->hardware_interface << " cannot be provided in mode " << sd.mode); + return false; + } + + HandleLayerBase::CanSwitchResult res = h_it->second->canSwitch(sd.mode); + + switch(res){ + case HandleLayerBase::NotSupported: + ROS_ERROR_STREAM("Mode " << sd.mode << " is not available for " << joint); + return false; + case HandleLayerBase::NotReadyToSwitch: + ROS_ERROR_STREAM(joint << " is not ready to switch mode"); + return false; + case HandleLayerBase::ReadyToSwitch: + case HandleLayerBase::NoNeedToSwitch: + sd.handle = h_it->second; + to_switch.push_back(sd); + } + } + } + } + switch_map_.insert(std::make_pair(controller_it->name, to_switch)); + } + + // perform mode switches + std::unordered_set to_stop; + std::vector failed_controllers; + for (std::list::const_iterator controller_it = stop_list.begin(); controller_it != stop_list.end(); ++controller_it){ + SwitchContainer &to_switch = switch_map_.at(controller_it->name); + for(RobotLayer::SwitchContainer::iterator it = to_switch.begin(); it != to_switch.end(); ++it){ + to_stop.insert(it->handle); + } + } + for (std::list::const_iterator controller_it = start_list.begin(); controller_it != start_list.end(); ++controller_it){ + SwitchContainer &to_switch = switch_map_.at(controller_it->name); + bool okay = true; + for(RobotLayer::SwitchContainer::iterator it = to_switch.begin(); it != to_switch.end(); ++it){ + it->handle->switchMode(MotorBase::No_Mode); // stop all + } + for(RobotLayer::SwitchContainer::iterator it = to_switch.begin(); it != to_switch.end(); ++it){ + if(!it->handle->switchMode(it->mode)){ + failed_controllers.push_back(controller_it->name); + ROS_ERROR_STREAM("Could not switch one joint for " << controller_it->name << ", will stop all related joints and the controller."); + for(RobotLayer::SwitchContainer::iterator stop_it = to_switch.begin(); stop_it != to_switch.end(); ++stop_it){ + to_stop.insert(stop_it->handle); + } + okay = false; + break; + }else{ + it->handle->enableLimits(it->enforce_limits); + } + to_stop.erase(it->handle); + } + } + for(std::unordered_set::iterator it = to_stop.begin(); it != to_stop.end(); ++it){ + (*it)->switchMode(MotorBase::No_Mode); + } + if(!failed_controllers.empty()){ + stopControllers(failed_controllers); + // will not return false here since this would prevent other controllers to be started and therefore lead to an inconsistent state + } + + return true; +} + +void RobotLayer::doSwitch(const std::list &start_list, const std::list &stop_list) { + std::vector failed_controllers; + for (std::list::const_iterator controller_it = start_list.begin(); controller_it != start_list.end(); ++controller_it){ + try{ + SwitchContainer &to_switch = switch_map_.at(controller_it->name); + for(RobotLayer::SwitchContainer::iterator it = to_switch.begin(); it != to_switch.end(); ++it){ + if(!it->handle->forwardForMode(it->mode)){ + failed_controllers.push_back(controller_it->name); + ROS_ERROR_STREAM("Could not switch one joint for " << controller_it->name << ", will stop all related joints and the controller."); + for(RobotLayer::SwitchContainer::iterator stop_it = to_switch.begin(); stop_it != to_switch.end(); ++stop_it){ + it->handle->switchMode(MotorBase::No_Mode); + } + break; + } + } + + }catch(const std::out_of_range&){ + ROS_ERROR_STREAM("Conttroller " << controller_it->name << "not found, will stop it"); + failed_controllers.push_back(controller_it->name); + } + } + if(!failed_controllers.empty()){ + stopControllers(failed_controllers); + } +} diff --git a/Devices/Libraries/Ros/ros_canopen/canopen_motor_node/test/test_muparser.cpp b/Devices/Libraries/Ros/ros_canopen/canopen_motor_node/test/test_muparser.cpp new file mode 100755 index 0000000..b259e50 --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/canopen_motor_node/test/test_muparser.cpp @@ -0,0 +1,29 @@ +#include +#include +#include +#include + +using namespace canopen; + +double * mapVariable(const std::string &, double *p) { + return p; +} + +TEST(TestMuparser, CheckNorm){ + double input = 0; + UnitConverter uc("norm(in,-1000,1000)", std::bind(HandleLayer::assignVariable, std::placeholders::_1, &input, "in")); + input = 0; EXPECT_EQ(0, uc.evaluate()); + input = 10; EXPECT_EQ(10, uc.evaluate()); + input = -10; EXPECT_EQ(-10, uc.evaluate()); + input = 1000; EXPECT_EQ(-1000, uc.evaluate()); + input = 1001; EXPECT_EQ(-999, uc.evaluate()); + input = 2000; EXPECT_EQ(0, uc.evaluate()); + input = 2001; EXPECT_EQ(1, uc.evaluate()); + input = -1000; EXPECT_EQ(-1000, uc.evaluate()); + input = 999; EXPECT_EQ(999, uc.evaluate()); +} + +int main(int argc, char **argv){ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/Devices/Libraries/Ros/ros_canopen/ros_canopen/CHANGELOG.rst b/Devices/Libraries/Ros/ros_canopen/ros_canopen/CHANGELOG.rst new file mode 100755 index 0000000..f40cf07 --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/ros_canopen/CHANGELOG.rst @@ -0,0 +1,76 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package ros_canopen +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.8.5 (2020-09-22) +------------------ + +0.8.4 (2020-08-22) +------------------ + +0.8.3 (2020-05-07) +------------------ +* Bump CMake version to avoid CMP0048 warning + Signed-off-by: ahcorde +* Contributors: ahcorde + +0.8.2 (2019-11-04) +------------------ + +0.8.1 (2019-07-14) +------------------ + +0.8.0 (2018-07-11) +------------------ + +0.7.8 (2018-05-04) +------------------ + +0.7.7 (2018-05-04) +------------------ + +0.7.6 (2017-08-30) +------------------ + +0.7.5 (2017-05-29) +------------------ + +0.7.4 (2017-04-25) +------------------ + +0.7.3 (2017-04-25) +------------------ + +0.7.2 (2017-03-28) +------------------ + +0.7.1 (2017-03-20) +------------------ + +0.7.0 (2016-12-13) +------------------ + +0.6.5 (2016-12-10) +------------------ +* updated metapackage + * format 2 + * updated maintaner + * added new packages +* update package URLs +* Contributors: Mathias Lüdtke + +0.6.4 (2015-07-03) +------------------ + +0.6.3 (2015-06-30) +------------------ + +0.6.2 (2014-12-18) +------------------ +* remove canopen_test_utils from metapackage +* Contributors: Florian Weisshardt + +0.6.1 (2014-12-15) +------------------ +* add metapackage +* Contributors: Florian Weisshardt diff --git a/Devices/Libraries/Ros/ros_canopen/ros_canopen/CMakeLists.txt b/Devices/Libraries/Ros/ros_canopen/ros_canopen/CMakeLists.txt new file mode 100755 index 0000000..6fe39f3 --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/ros_canopen/CMakeLists.txt @@ -0,0 +1,4 @@ +cmake_minimum_required(VERSION 3.0.2) +project(ros_canopen) +find_package(catkin REQUIRED) +catkin_metapackage() diff --git a/Devices/Libraries/Ros/ros_canopen/ros_canopen/package.xml b/Devices/Libraries/Ros/ros_canopen/ros_canopen/package.xml new file mode 100755 index 0000000..82458f3 --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/ros_canopen/package.xml @@ -0,0 +1,30 @@ + + + ros_canopen + 0.8.5 + A generic canopen implementation for ROS + + LGPL + + http://ros.org/wiki/ros_canopen + https://github.com/ros-industrial/ros_canopen + https://github.com/ros-industrial/ros_canopen/issues + + Mathias Lüdtke + Florian Weisshardt + + catkin + + can_msgs + canopen_402 + canopen_chain_node + canopen_master + canopen_motor_node + + socketcan_bridge + socketcan_interface + + + + + diff --git a/Devices/Libraries/Ros/ros_canopen/socketcan_bridge/CHANGELOG.rst b/Devices/Libraries/Ros/ros_canopen/socketcan_bridge/CHANGELOG.rst new file mode 100755 index 0000000..1618fb0 --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/socketcan_bridge/CHANGELOG.rst @@ -0,0 +1,128 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package socketcan_bridge +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.8.5 (2020-09-22) +------------------ + +0.8.4 (2020-08-22) +------------------ +* pass settings from ROS node to SocketCANInterface +* Contributors: Mathias Lüdtke + +0.8.3 (2020-05-07) +------------------ +* add includes to +* Bump CMake version to avoid CMP0048 warning + Signed-off-by: ahcorde +* Contributors: Mathias Lüdtke, ahcorde + +0.8.2 (2019-11-04) +------------------ +* fix roslint errors in socketcan_bridge +* run roslint as part of run_tests +* enable rosconsole_bridge bindings +* Contributors: Mathias Lüdtke + +0.8.1 (2019-07-14) +------------------ +* Added configurable queue sizes +* Set C++ standard to c++14 +* implemented create\*ListenerM helpers +* Replacing FastDelegate with std::function and std::bind. +* Contributors: Harsh Deshpande, JeremyZoss, Joshua Whitley, Mathias Lüdtke, rchristopher + +0.8.0 (2018-07-11) +------------------ +* keep NodeHandle alive in socketcan_bridge tests +* migrated to std::function and std::bind +* migrated to std pointers +* compare can_msgs::Frame and can::Frame properly +* Contributors: Mathias Lüdtke + +0.7.8 (2018-05-04) +------------------ +* Revert "pull make_shared into namespaces" + This reverts commit 9b2cd05df76d223647ca81917d289ca6330cdee6. +* Contributors: Mathias Lüdtke + +0.7.7 (2018-05-04) +------------------ +* pull make_shared into namespaces +* added types for all shared_ptrs +* address catkin_lint errors/warnings +* protect tests from accessing front() or back() of empty list +* added checkMaskFilter for socketcan_bridge +* remove isValid work-around +* added unit test for can id pass filter +* add CAN filter to socketcan_bridge nodes +* Contributors: Benjamin Maidel, Mathias Lüdtke + +0.7.6 (2017-08-30) +------------------ + +0.7.5 (2017-05-29) +------------------ + +0.7.4 (2017-04-25) +------------------ + +0.7.3 (2017-04-25) +------------------ + +0.7.2 (2017-03-28) +------------------ + +0.7.1 (2017-03-20) +------------------ + +0.7.0 (2016-12-13) +------------------ + +0.6.5 (2016-12-10) +------------------ +* hamonized versions +* styled and sorted CMakeLists.txt + * removed boilerplate comments + * indention + * reviewed exported dependencies +* styled and sorted package.xml +* Removes gtest from test dependencies. + This dependency is covered by the rosunit dependency. +* Removes dependency on Boost, adds rosunit dependency. + The dependency on Boost was unnecessary, rosunit is required for gtest. +* Improves StateInterface implementation of the DummyInterface. + The doesLoopBack() method now returns the correct value. A state change is + correctly dispatched when the init() method is called. +* Changes the exit code of the nodes if device init fails. + Now exits with 1 if the initialization of the can device fails. +* Changes the frame field for the published messages. + An empty frame name is more commonly used to denote that there is no frame + associated with the message. +* Changes return type of setup() method. + Setup() calls the CreateMsgListener and CreateStateListener, it does not attempt + to verify if the interface is ready, which makes void more applicable. +* Cleanup, fixes and improvements in CmakeLists. + Adds the REQUIRED COMPONENTS packages to the CATKIN_DEPENDS. + Improves add_dependency on the messages to be built. + Removes unnecessary FILES_MATCHING. + Moves the roslint_cpp macro to the testing block. +* Finalizes work on the socketcan_bridge and can_msgs. + Readies the packages socketcan_bridge and can_msgs for the merge with ros_canopen. + Bumps the version for both packages to 0.1.0. Final cleanup in CMakeLists, added + comments to the shell script and launchfile used for testing. +* Adds tests for socketcan_bridge and bugfixes. + Uses rostests and the modified DummyInterface to test whether behaviour + is correct. Prevented possible crashes when can::tostring was called on + invalid frames. +* Adds conversion test between msg and SocketCAN Frame. + This test only covers the conversion between the can_msgs::Frame message and can::Frame from SocketCAN. +* Introduces topic_to_socketcan and the bridge. + Adds TopicToSocketCAN, the counterpart to the SocketCANToTopic class. + Also introduces a node to use this class and a node which combines the two + classes into a bidirectional bridge. + Contrary to the previous commit message the send() method appears to be + available from the ThreadedSocketCANInterface afterall. +* Introduces socketcan_to_topic and its node. + This is based on the ThreadedSocketCANInterface from the socketcan_interface package. Sending might become problematic with this class however, as the send() method is not exposed through the Threading wrappers. +* Contributors: Ivor Wanders, Mathias Lüdtke diff --git a/Devices/Libraries/Ros/ros_canopen/socketcan_bridge/CMakeLists.txt b/Devices/Libraries/Ros/ros_canopen/socketcan_bridge/CMakeLists.txt new file mode 100755 index 0000000..1996141 --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/socketcan_bridge/CMakeLists.txt @@ -0,0 +1,144 @@ +cmake_minimum_required(VERSION 3.0.2) +project(socketcan_bridge) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_STANDARD 14) +endif() + +find_package(catkin REQUIRED + COMPONENTS + can_msgs + rosconsole_bridge + roscpp + socketcan_interface +) + +catkin_package( + INCLUDE_DIRS + include + LIBRARIES + socketcan_to_topic + topic_to_socketcan + CATKIN_DEPENDS + can_msgs + rosconsole_bridge + roscpp + socketcan_interface +) + +include_directories( + include + ${catkin_INCLUDE_DIRS} +) + + +# socketcan_to_topic +add_library(socketcan_to_topic + src/rosconsole_bridge.cpp + src/socketcan_to_topic.cpp +) +target_link_libraries(socketcan_to_topic + ${catkin_LIBRARIES} +) +add_dependencies(socketcan_to_topic + ${catkin_EXPORTED_TARGETS} +) + +# topic_to_socketcan +add_library(topic_to_socketcan + src/rosconsole_bridge.cpp + src/topic_to_socketcan.cpp +) +target_link_libraries(topic_to_socketcan + ${catkin_LIBRARIES} +) +add_dependencies(topic_to_socketcan + ${catkin_EXPORTED_TARGETS} +) + +# socketcan_to_topic_node +add_executable(socketcan_to_topic_node + src/socketcan_to_topic_node.cpp +) +target_link_libraries(socketcan_to_topic_node + socketcan_to_topic + ${catkin_LIBRARIES} +) + +# topic_to_socketcan_node +add_executable(topic_to_socketcan_node + src/topic_to_socketcan_node.cpp + +) +target_link_libraries(topic_to_socketcan_node + topic_to_socketcan + ${catkin_LIBRARIES} +) + +# socketcan_bridge_node +add_executable(${PROJECT_NAME}_node + src/${PROJECT_NAME}_node.cpp +) +target_link_libraries(${PROJECT_NAME}_node + topic_to_socketcan + socketcan_to_topic + ${catkin_LIBRARIES} +) + +install( + TARGETS + ${PROJECT_NAME}_node + socketcan_to_topic + socketcan_to_topic_node + topic_to_socketcan + topic_to_socketcan_node + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install( + DIRECTORY + include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +) + +if(CATKIN_ENABLE_TESTING) + find_package(rostest REQUIRED) + find_package(roslint REQUIRED) + + roslint_cpp() + roslint_add_test() + + # unit test for the can_msgs::Frame and can::Frame types. + catkin_add_gtest(test_conversion + test/test_conversion.cpp + ) + target_link_libraries(test_conversion + topic_to_socketcan + socketcan_to_topic + ${catkin_LIBRARIES} + ) + + + add_rostest_gtest(test_to_socketcan + test/to_socketcan.test + test/to_socketcan_test.cpp + ) + target_link_libraries(test_to_socketcan + topic_to_socketcan + ${catkin_LIBRARIES} + ) + + add_rostest_gtest(test_to_topic + test/to_topic.test + test/to_topic_test.cpp + ) + target_link_libraries(test_to_topic + socketcan_to_topic + topic_to_socketcan + ${catkin_LIBRARIES} + ) + +endif() diff --git a/Devices/Libraries/Ros/ros_canopen/socketcan_bridge/include/socketcan_bridge/socketcan_to_topic.h b/Devices/Libraries/Ros/ros_canopen/socketcan_bridge/include/socketcan_bridge/socketcan_to_topic.h new file mode 100755 index 0000000..21ef242 --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/socketcan_bridge/include/socketcan_bridge/socketcan_to_topic.h @@ -0,0 +1,76 @@ +/* + * Copyright (c) 2016, Ivor Wanders + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef SOCKETCAN_BRIDGE_SOCKETCAN_TO_TOPIC_H +#define SOCKETCAN_BRIDGE_SOCKETCAN_TO_TOPIC_H + +#include +#include +#include +#include + +namespace socketcan_bridge +{ +class SocketCANToTopic +{ + public: + SocketCANToTopic(ros::NodeHandle* nh, ros::NodeHandle* nh_param, can::DriverInterfaceSharedPtr driver); + void setup(); + void setup(const can::FilteredFrameListener::FilterVector &filters); + void setup(XmlRpc::XmlRpcValue filters); + void setup(ros::NodeHandle nh); + + private: + ros::Publisher can_topic_; + can::DriverInterfaceSharedPtr driver_; + + can::FrameListenerConstSharedPtr frame_listener_; + can::StateListenerConstSharedPtr state_listener_; + + + void frameCallback(const can::Frame& f); + void stateCallback(const can::State & s); +}; + +void convertSocketCANToMessage(const can::Frame& f, can_msgs::Frame& m) +{ + m.id = f.id; + m.dlc = f.dlc; + m.is_error = f.is_error; + m.is_rtr = f.is_rtr; + m.is_extended = f.is_extended; + + for (int i = 0; i < 8; i++) // always copy all data, regardless of dlc. + { + m.data[i] = f.data[i]; + } +}; + +}; // namespace socketcan_bridge + + +#endif // SOCKETCAN_BRIDGE_SOCKETCAN_TO_TOPIC_H diff --git a/Devices/Libraries/Ros/ros_canopen/socketcan_bridge/include/socketcan_bridge/topic_to_socketcan.h b/Devices/Libraries/Ros/ros_canopen/socketcan_bridge/include/socketcan_bridge/topic_to_socketcan.h new file mode 100755 index 0000000..0474ee3 --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/socketcan_bridge/include/socketcan_bridge/topic_to_socketcan.h @@ -0,0 +1,70 @@ +/* + * Copyright (c) 2016, Ivor Wanders + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef SOCKETCAN_BRIDGE_TOPIC_TO_SOCKETCAN_H +#define SOCKETCAN_BRIDGE_TOPIC_TO_SOCKETCAN_H + +#include +#include +#include + +namespace socketcan_bridge +{ +class TopicToSocketCAN +{ + public: + TopicToSocketCAN(ros::NodeHandle* nh, ros::NodeHandle* nh_param, can::DriverInterfaceSharedPtr driver); + void setup(); + + private: + ros::Subscriber can_topic_; + can::DriverInterfaceSharedPtr driver_; + + can::StateListenerConstSharedPtr state_listener_; + + void msgCallback(const can_msgs::Frame::ConstPtr& msg); + void stateCallback(const can::State & s); +}; + +void convertMessageToSocketCAN(const can_msgs::Frame& m, can::Frame& f) +{ + f.id = m.id; + f.dlc = m.dlc; + f.is_error = m.is_error; + f.is_rtr = m.is_rtr; + f.is_extended = m.is_extended; + + for (int i = 0; i < 8; i++) // always copy all data, regardless of dlc. + { + f.data[i] = m.data[i]; + } +}; + +}; // namespace socketcan_bridge + + +#endif // SOCKETCAN_BRIDGE_TOPIC_TO_SOCKETCAN_H diff --git a/Devices/Libraries/Ros/ros_canopen/socketcan_bridge/package.xml b/Devices/Libraries/Ros/ros_canopen/socketcan_bridge/package.xml new file mode 100755 index 0000000..16e4c20 --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/socketcan_bridge/package.xml @@ -0,0 +1,26 @@ + + + socketcan_bridge + 0.8.5 + Conversion nodes for messages from SocketCAN to a ROS Topic and vice versa. + + Mathias Lüdtke + Ivor Wanders + + BSD + + http://wiki.ros.org/socketcan_bridge + https://github.com/ros-industrial/ros_canopen + https://github.com/ros-industrial/ros_canopen/issues + + catkin + + can_msgs + rosconsole_bridge + roscpp + socketcan_interface + + roslint + rostest + rosunit + diff --git a/Devices/Libraries/Ros/ros_canopen/socketcan_bridge/src/rosconsole_bridge.cpp b/Devices/Libraries/Ros/ros_canopen/socketcan_bridge/src/rosconsole_bridge.cpp new file mode 100755 index 0000000..05eebd0 --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/socketcan_bridge/src/rosconsole_bridge.cpp @@ -0,0 +1,29 @@ +/* + * Copyright (c) 2019, Mathias Lüdtke + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +REGISTER_ROSCONSOLE_BRIDGE; diff --git a/Devices/Libraries/Ros/ros_canopen/socketcan_bridge/src/socketcan_bridge_node.cpp b/Devices/Libraries/Ros/ros_canopen/socketcan_bridge/src/socketcan_bridge_node.cpp new file mode 100755 index 0000000..c3709b4 --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/socketcan_bridge/src/socketcan_bridge_node.cpp @@ -0,0 +1,72 @@ +/* + * Copyright (c) 2016, Ivor Wanders + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include +#include +#include +#include +#include + + +int main(int argc, char *argv[]) +{ + ros::init(argc, argv, "socketcan_bridge_node"); + + ros::NodeHandle nh(""), nh_param("~"); + + std::string can_device; + nh_param.param("can_device", can_device, "can0"); + + can::ThreadedSocketCANInterfaceSharedPtr driver = std::make_shared (); + + // initialize device at can_device, 0 for no loopback. + if (!driver->init(can_device, 0, XmlRpcSettings::create(nh_param))) + { + ROS_FATAL("Failed to initialize can_device at %s", can_device.c_str()); + return 1; + } + else + { + ROS_INFO("Successfully connected to %s.", can_device.c_str()); + } + + // initialize the bridge both ways. + socketcan_bridge::TopicToSocketCAN to_socketcan_bridge(&nh, &nh_param, driver); + to_socketcan_bridge.setup(); + + socketcan_bridge::SocketCANToTopic to_topic_bridge(&nh, &nh_param, driver); + to_topic_bridge.setup(nh_param); + + ros::spin(); + + driver->shutdown(); + driver.reset(); + + ros::waitForShutdown(); +} diff --git a/Devices/Libraries/Ros/ros_canopen/socketcan_bridge/src/socketcan_to_topic.cpp b/Devices/Libraries/Ros/ros_canopen/socketcan_bridge/src/socketcan_to_topic.cpp new file mode 100755 index 0000000..75941ea --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/socketcan_bridge/src/socketcan_to_topic.cpp @@ -0,0 +1,133 @@ +/* + * Copyright (c) 2016, Ivor Wanders + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include +#include + +namespace can +{ +template<> can::FrameFilterSharedPtr tofilter(const XmlRpc::XmlRpcValue &ct) +{ + XmlRpc::XmlRpcValue t(ct); + try // try to read as integer + { + uint32_t id = static_cast(t); + return tofilter(id); + } + catch(...) // else read as string + { + return tofilter(static_cast(t)); + } +} +} // namespace can + +namespace socketcan_bridge +{ + SocketCANToTopic::SocketCANToTopic(ros::NodeHandle* nh, ros::NodeHandle* nh_param, + can::DriverInterfaceSharedPtr driver) + { + can_topic_ = nh->advertise("received_messages", + nh_param->param("received_messages_queue_size", 10)); + driver_ = driver; + }; + + void SocketCANToTopic::setup() + { + // register handler for frames and state changes. + frame_listener_ = driver_->createMsgListenerM(this, &SocketCANToTopic::frameCallback); + state_listener_ = driver_->createStateListenerM(this, &SocketCANToTopic::stateCallback); + }; + + void SocketCANToTopic::setup(const can::FilteredFrameListener::FilterVector &filters) + { + frame_listener_.reset(new can::FilteredFrameListener(driver_, + std::bind(&SocketCANToTopic::frameCallback, + this, + std::placeholders::_1), + filters)); + + state_listener_ = driver_->createStateListenerM(this, &SocketCANToTopic::stateCallback); + } + + void SocketCANToTopic::setup(XmlRpc::XmlRpcValue filters) + { + setup(can::tofilters(filters)); + } + void SocketCANToTopic::setup(ros::NodeHandle nh) + { + XmlRpc::XmlRpcValue filters; + if (nh.getParam("can_ids", filters)) return setup(filters); + return setup(); + } + + + void SocketCANToTopic::frameCallback(const can::Frame& f) + { + // ROS_DEBUG("Message came in: %s", can::tostring(f, true).c_str()); + if (!f.isValid()) + { + ROS_ERROR("Invalid frame from SocketCAN: id: %#04x, length: %d, is_extended: %d, is_error: %d, is_rtr: %d", + f.id, f.dlc, f.is_extended, f.is_error, f.is_rtr); + return; + } + else + { + if (f.is_error) + { + // can::tostring cannot be used for dlc > 8 frames. It causes an crash + // due to usage of boost::array for the data array. The should always work. + ROS_WARN("Received frame is error: %s", can::tostring(f, true).c_str()); + } + } + + can_msgs::Frame msg; + // converts the can::Frame (socketcan.h) to can_msgs::Frame (ROS msg) + convertSocketCANToMessage(f, msg); + + msg.header.frame_id = ""; // empty frame is the de-facto standard for no frame. + msg.header.stamp = ros::Time::now(); + + can_topic_.publish(msg); + }; + + + void SocketCANToTopic::stateCallback(const can::State & s) + { + std::string err; + driver_->translateError(s.internal_error, err); + if (!s.internal_error) + { + ROS_INFO("State: %s, asio: %s", err.c_str(), s.error_code.message().c_str()); + } + else + { + ROS_ERROR("Error: %s, asio: %s", err.c_str(), s.error_code.message().c_str()); + } + }; +}; // namespace socketcan_bridge diff --git a/Devices/Libraries/Ros/ros_canopen/socketcan_bridge/src/socketcan_to_topic_node.cpp b/Devices/Libraries/Ros/ros_canopen/socketcan_bridge/src/socketcan_to_topic_node.cpp new file mode 100755 index 0000000..2cfac50 --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/socketcan_bridge/src/socketcan_to_topic_node.cpp @@ -0,0 +1,69 @@ +/* + * Copyright (c) 2016, Ivor Wanders + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include +#include +#include +#include +#include + + + +int main(int argc, char *argv[]) +{ + ros::init(argc, argv, "socketcan_to_topic_node"); + + ros::NodeHandle nh(""), nh_param("~"); + + std::string can_device; + nh_param.param("can_device", can_device, "can0"); + + can::ThreadedSocketCANInterfaceSharedPtr driver = std::make_shared (); + + // initialize device at can_device, 0 for no loopback. + if (!driver->init(can_device, 0, XmlRpcSettings::create(nh_param))) + { + ROS_FATAL("Failed to initialize can_device at %s", can_device.c_str()); + return 1; + } + else + { + ROS_INFO("Successfully connected to %s.", can_device.c_str()); + } + + socketcan_bridge::SocketCANToTopic to_topic_bridge(&nh, &nh_param, driver); + to_topic_bridge.setup(nh_param); + + ros::spin(); + + driver->shutdown(); + driver.reset(); + + ros::waitForShutdown(); +} diff --git a/Devices/Libraries/Ros/ros_canopen/socketcan_bridge/src/topic_to_socketcan.cpp b/Devices/Libraries/Ros/ros_canopen/socketcan_bridge/src/topic_to_socketcan.cpp new file mode 100755 index 0000000..c5063c6 --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/socketcan_bridge/src/topic_to_socketcan.cpp @@ -0,0 +1,91 @@ +/* + * Copyright (c) 2016, Ivor Wanders + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include + +namespace socketcan_bridge +{ + TopicToSocketCAN::TopicToSocketCAN(ros::NodeHandle* nh, ros::NodeHandle* nh_param, + can::DriverInterfaceSharedPtr driver) + { + can_topic_ = nh->subscribe("sent_messages", nh_param->param("sent_messages_queue_size", 10), + std::bind(&TopicToSocketCAN::msgCallback, this, std::placeholders::_1)); + driver_ = driver; + }; + + void TopicToSocketCAN::setup() + { + state_listener_ = driver_->createStateListener( + std::bind(&TopicToSocketCAN::stateCallback, this, std::placeholders::_1)); + }; + + void TopicToSocketCAN::msgCallback(const can_msgs::Frame::ConstPtr& msg) + { + // ROS_DEBUG("Message came from sent_messages topic"); + + // translate it to the socketcan frame type. + + can_msgs::Frame m = *msg.get(); // ROS message + can::Frame f; // socketcan type + + // converts the can_msgs::Frame (ROS msg) to can::Frame (socketcan.h) + convertMessageToSocketCAN(m, f); + + if (!f.isValid()) // check if the id and flags are appropriate. + { + // ROS_WARN("Refusing to send invalid frame: %s.", can::tostring(f, true).c_str()); + // can::tostring cannot be used for dlc > 8 frames. It causes an crash + // due to usage of boost::array for the data array. The should always work. + ROS_ERROR("Invalid frame from topic: id: %#04x, length: %d, is_extended: %d", m.id, m.dlc, m.is_extended); + return; + } + + bool res = driver_->send(f); + if (!res) + { + ROS_ERROR("Failed to send message: %s.", can::tostring(f, true).c_str()); + } + }; + + + + void TopicToSocketCAN::stateCallback(const can::State & s) + { + std::string err; + driver_->translateError(s.internal_error, err); + if (!s.internal_error) + { + ROS_INFO("State: %s, asio: %s", err.c_str(), s.error_code.message().c_str()); + } + else + { + ROS_ERROR("Error: %s, asio: %s", err.c_str(), s.error_code.message().c_str()); + } + }; +}; // namespace socketcan_bridge diff --git a/Devices/Libraries/Ros/ros_canopen/socketcan_bridge/src/topic_to_socketcan_node.cpp b/Devices/Libraries/Ros/ros_canopen/socketcan_bridge/src/topic_to_socketcan_node.cpp new file mode 100755 index 0000000..3849f56 --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/socketcan_bridge/src/topic_to_socketcan_node.cpp @@ -0,0 +1,69 @@ +/* + * Copyright (c) 2016, Ivor Wanders + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include +#include +#include +#include +#include + + + +int main(int argc, char *argv[]) +{ + ros::init(argc, argv, "topic_to_socketcan_node"); + + ros::NodeHandle nh(""), nh_param("~"); + + std::string can_device; + nh_param.param("can_device", can_device, "can0"); + + can::ThreadedSocketCANInterfaceSharedPtr driver = std::make_shared (); + + // initialize device at can_device, 0 for no loopback. + if (!driver->init(can_device, 0, XmlRpcSettings::create(nh_param))) + { + ROS_FATAL("Failed to initialize can_device at %s", can_device.c_str()); + return 1; + } + else + { + ROS_INFO("Successfully connected to %s.", can_device.c_str()); + } + + socketcan_bridge::TopicToSocketCAN to_socketcan_bridge(&nh, &nh_param, driver); + to_socketcan_bridge.setup(); + + ros::spin(); + + driver->shutdown(); + driver.reset(); + + ros::waitForShutdown(); +} diff --git a/Devices/Libraries/Ros/ros_canopen/socketcan_bridge/test/initialize_vcan.sh b/Devices/Libraries/Ros/ros_canopen/socketcan_bridge/test/initialize_vcan.sh new file mode 100755 index 0000000..1829b46 --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/socketcan_bridge/test/initialize_vcan.sh @@ -0,0 +1,35 @@ +#!/bin/bash + +# sets up two virtual can interfaces, vcan0 and vcan1 + +lsmod | grep -q "vcan" +VCAN_NOT_LOADED=$? + +if [ $VCAN_NOT_LOADED -eq 1 ]; then + echo "vcan kernel module is not available..." + echo "loading it;" + sudo modprobe -a vcan +fi + +ifconfig vcan0 > /dev/null +VCAN_NOT_EXIST=$? + +if [ $VCAN_NOT_EXIST -eq 1 ]; then + echo "vcan0 does not exist, creating it." + sudo ip link add dev vcan0 type vcan + sudo ip link set vcan0 up +else + echo "vcan0 already exists." +fi + + +ifconfig vcan1 > /dev/null +VCAN_NOT_EXIST=$? +if [ $VCAN_NOT_EXIST -eq 1 ]; then + echo "vcan0 does not exist, creating it." + sudo ip link add dev vcan1 type vcan + sudo ip link set vcan1 up +else + echo "vcan0 already exists." +fi + diff --git a/Devices/Libraries/Ros/ros_canopen/socketcan_bridge/test/test_conversion.cpp b/Devices/Libraries/Ros/ros_canopen/socketcan_bridge/test/test_conversion.cpp new file mode 100755 index 0000000..fcab66b --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/socketcan_bridge/test/test_conversion.cpp @@ -0,0 +1,140 @@ +/* + * Copyright (c) 2016, Ivor Wanders + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#include +#include + +#include +#include + +// Bring in gtest +#include + +// test whether the content of a conversion from a SocketCAN frame +// to a ROS message correctly maintains the data. +TEST(ConversionTest, socketCANToTopicStandard) +{ + can::Frame f; + can_msgs::Frame m; + f.id = 127; + f.dlc = 8; + f.is_error = false; + f.is_rtr = false; + f.is_extended = false; + for (uint8_t i = 0; i < f.dlc; ++i) + { + f.data[i] = i; + } + socketcan_bridge::convertSocketCANToMessage(f, m); + EXPECT_EQ(127, m.id); + EXPECT_EQ(8, m.dlc); + EXPECT_EQ(false, m.is_error); + EXPECT_EQ(false, m.is_rtr); + EXPECT_EQ(false, m.is_extended); + + for (uint8_t i=0; i < 8; i++) + { + EXPECT_EQ(i, m.data[i]); + } +} + +// test all three flags seperately. +TEST(ConversionTest, socketCANToTopicFlags) +{ + can::Frame f; + can_msgs::Frame m; + + f.is_error = true; + socketcan_bridge::convertSocketCANToMessage(f, m); + EXPECT_EQ(true, m.is_error); + f.is_error = false; + + f.is_rtr = true; + socketcan_bridge::convertSocketCANToMessage(f, m); + EXPECT_EQ(true, m.is_rtr); + f.is_rtr = false; + + f.is_extended = true; + socketcan_bridge::convertSocketCANToMessage(f, m); + EXPECT_EQ(true, m.is_extended); + f.is_extended = false; +} + +// idem, but the other way around. +TEST(ConversionTest, topicToSocketCANStandard) +{ + can::Frame f; + can_msgs::Frame m; + m.id = 127; + m.dlc = 8; + m.is_error = false; + m.is_rtr = false; + m.is_extended = false; + for (uint8_t i = 0; i < m.dlc; ++i) + { + m.data[i] = i; + } + socketcan_bridge::convertMessageToSocketCAN(m, f); + EXPECT_EQ(127, f.id); + EXPECT_EQ(8, f.dlc); + EXPECT_EQ(false, f.is_error); + EXPECT_EQ(false, f.is_rtr); + EXPECT_EQ(false, f.is_extended); + + + for (uint8_t i=0; i < 8; i++) + { + EXPECT_EQ(i, f.data[i]); + } +} + +TEST(ConversionTest, topicToSocketCANFlags) +{ + can::Frame f; + can_msgs::Frame m; + + m.is_error = true; + socketcan_bridge::convertMessageToSocketCAN(m, f); + EXPECT_EQ(true, f.is_error); + m.is_error = false; + + m.is_rtr = true; + socketcan_bridge::convertMessageToSocketCAN(m, f); + EXPECT_EQ(true, f.is_rtr); + m.is_rtr = false; + + m.is_extended = true; + socketcan_bridge::convertMessageToSocketCAN(m, f); + EXPECT_EQ(true, f.is_extended); + m.is_extended = false; +} + +// Run all the tests that were declared with TEST() +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/Devices/Libraries/Ros/ros_canopen/socketcan_bridge/test/to_socketcan.test b/Devices/Libraries/Ros/ros_canopen/socketcan_bridge/test/to_socketcan.test new file mode 100755 index 0000000..73b9466 --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/socketcan_bridge/test/to_socketcan.test @@ -0,0 +1,3 @@ + + + \ No newline at end of file diff --git a/Devices/Libraries/Ros/ros_canopen/socketcan_bridge/test/to_socketcan_test.cpp b/Devices/Libraries/Ros/ros_canopen/socketcan_bridge/test/to_socketcan_test.cpp new file mode 100755 index 0000000..fae1c2e --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/socketcan_bridge/test/to_socketcan_test.cpp @@ -0,0 +1,194 @@ +/* + * Copyright (c) 2016, Ivor Wanders + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#include +#include + +#include +#include +#include + +#include +#include +#include +#include + +class frameCollector +{ + public: + std::list frames; + + frameCollector() {} + + void frameCallback(const can::Frame& f) + { + frames.push_back(f); + } +}; + +TEST(TopicToSocketCANTest, checkCorrectData) +{ + ros::NodeHandle nh(""), nh_param("~"); + + can::DummyBus bus("checkCorrectData"); + // create the dummy interface + can::ThreadedDummyInterfaceSharedPtr dummy = std::make_shared(); + + // start the to topic bridge. + socketcan_bridge::TopicToSocketCAN to_socketcan_bridge(&nh, &nh_param, dummy); + to_socketcan_bridge.setup(); + + // init the driver to test stateListener (not checked automatically). + dummy->init(bus.name, true, can::NoSettings::create()); + + // register for messages on received_messages. + ros::Publisher publisher_ = nh.advertise("sent_messages", 10); + + // create a frame collector. + frameCollector frame_collector_; + + // driver->createMsgListener(&frameCallback); + can::FrameListenerConstSharedPtr frame_listener_ = dummy->createMsgListener( + + std::bind(&frameCollector::frameCallback, &frame_collector_, std::placeholders::_1)); + + // create a message + can_msgs::Frame msg; + msg.is_extended = true; + msg.is_rtr = false; + msg.is_error = false; + msg.id = 0x1337; + msg.dlc = 8; + for (uint8_t i=0; i < msg.dlc; i++) + { + msg.data[i] = i; + } + + msg.header.frame_id = "0"; // "0" for no frame. + msg.header.stamp = ros::Time::now(); + + // send the can_frame::Frame message to the sent_messages topic. + publisher_.publish(msg); + + // give some time for the interface some time to process the message + ros::WallDuration(1.0).sleep(); + ros::spinOnce(); + + dummy->flush(); + + can_msgs::Frame received; + can::Frame f = frame_collector_.frames.back(); + socketcan_bridge::convertSocketCANToMessage(f, received); + + EXPECT_EQ(received.id, msg.id); + EXPECT_EQ(received.dlc, msg.dlc); + EXPECT_EQ(received.is_extended, msg.is_extended); + EXPECT_EQ(received.is_rtr, msg.is_rtr); + EXPECT_EQ(received.is_error, msg.is_error); + EXPECT_EQ(received.data, msg.data); +} + +TEST(TopicToSocketCANTest, checkInvalidFrameHandling) +{ + // - tries to send a non-extended frame with an id larger than 11 bits. + // that should not be sent. + // - verifies that sending one larger than 11 bits actually works. + // - tries sending a message with a dlc > 8 bytes, this should not be sent. + // sending with 8 bytes is verified by the checkCorrectData testcase. + + ros::NodeHandle nh(""), nh_param("~"); + + + can::DummyBus bus("checkInvalidFrameHandling"); + + // create the dummy interface + can::ThreadedDummyInterfaceSharedPtr dummy = std::make_shared(); + + // start the to topic bridge. + socketcan_bridge::TopicToSocketCAN to_socketcan_bridge(&nh, &nh_param, dummy); + to_socketcan_bridge.setup(); + + dummy->init(bus.name, true, can::NoSettings::create()); + + // register for messages on received_messages. + ros::Publisher publisher_ = nh.advertise("sent_messages", 10); + + // create a frame collector. + frameCollector frame_collector_; + + // add callback to the dummy interface. + can::FrameListenerConstSharedPtr frame_listener_ = dummy->createMsgListener( + std::bind(&frameCollector::frameCallback, &frame_collector_, std::placeholders::_1)); + + // create a message + can_msgs::Frame msg; + msg.is_extended = false; + msg.id = (1<<11)+1; // this is an illegal CAN packet... should not be sent. + msg.header.frame_id = "0"; // "0" for no frame. + msg.header.stamp = ros::Time::now(); + + // send the can_frame::Frame message to the sent_messages topic. + publisher_.publish(msg); + + // give some time for the interface some time to process the message + ros::WallDuration(1.0).sleep(); + ros::spinOnce(); + dummy->flush(); + + EXPECT_EQ(frame_collector_.frames.size(), 0); + + msg.is_extended = true; + msg.id = (1<<11)+1; // now it should be alright. + // send the can_frame::Frame message to the sent_messages topic. + publisher_.publish(msg); + ros::WallDuration(1.0).sleep(); + ros::spinOnce(); + dummy->flush(); + + EXPECT_EQ(frame_collector_.frames.size(), 1); + + // wipe the frame queue. + frame_collector_.frames.clear(); + + + // finally, check if frames with a dlc > 8 are discarded. + msg.dlc = 10; + publisher_.publish(msg); + ros::WallDuration(1.0).sleep(); + ros::spinOnce(); + dummy->flush(); + + EXPECT_EQ(frame_collector_.frames.size(), 0); +} + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "test_to_topic"); + ros::NodeHandle nh; + ros::WallDuration(1.0).sleep(); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/Devices/Libraries/Ros/ros_canopen/socketcan_bridge/test/to_topic.test b/Devices/Libraries/Ros/ros_canopen/socketcan_bridge/test/to_topic.test new file mode 100755 index 0000000..b99b62e --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/socketcan_bridge/test/to_topic.test @@ -0,0 +1,3 @@ + + + \ No newline at end of file diff --git a/Devices/Libraries/Ros/ros_canopen/socketcan_bridge/test/to_topic_test.cpp b/Devices/Libraries/Ros/ros_canopen/socketcan_bridge/test/to_topic_test.cpp new file mode 100755 index 0000000..617d89b --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/socketcan_bridge/test/to_topic_test.cpp @@ -0,0 +1,325 @@ +/* + * Copyright (c) 2016, Ivor Wanders + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +class msgCollector +{ + public: + std::list messages; + + msgCollector() {} + + void msgCallback(const can_msgs::Frame& f) + { + messages.push_back(f); + } +}; + +std::string convertMessageToString(const can_msgs::Frame &msg, bool lc = true) +{ + can::Frame f; + socketcan_bridge::convertMessageToSocketCAN(msg, f); + return can::tostring(f, lc); +} + +TEST(SocketCANToTopicTest, checkCorrectData) +{ + ros::NodeHandle nh(""), nh_param("~"); + + can::DummyBus bus("checkCorrectData"); + + // create the dummy interface + can::ThreadedDummyInterfaceSharedPtr dummy = std::make_shared(); + + // start the to topic bridge. + socketcan_bridge::SocketCANToTopic to_topic_bridge(&nh, &nh_param, dummy); + to_topic_bridge.setup(); // initiate the message callbacks + + // init the driver to test stateListener (not checked automatically). + dummy->init(bus.name, true, can::NoSettings::create()); + + // create a frame collector. + msgCollector message_collector_; + + // register for messages on received_messages. + ros::Subscriber subscriber_ = nh.subscribe("received_messages", 1, &msgCollector::msgCallback, &message_collector_); + + // create a can frame + can::Frame f; + f.is_extended = true; + f.is_rtr = false; + f.is_error = false; + f.id = 0x1337; + f.dlc = 8; + for (uint8_t i=0; i < f.dlc; i++) + { + f.data[i] = i; + } + + // send the can frame to the driver + dummy->send(f); + + // give some time for the interface some time to process the message + ros::WallDuration(1.0).sleep(); + ros::spinOnce(); + + ASSERT_EQ(1, message_collector_.messages.size()); + + // compare the received can_msgs::Frame message to the sent can::Frame. + can::Frame received; + can_msgs::Frame msg = message_collector_.messages.back(); + socketcan_bridge::convertMessageToSocketCAN(msg, received); + + EXPECT_EQ(received.id, f.id); + EXPECT_EQ(received.dlc, f.dlc); + EXPECT_EQ(received.is_extended, f.is_extended); + EXPECT_EQ(received.is_rtr, f.is_rtr); + EXPECT_EQ(received.is_error, f.is_error); + EXPECT_EQ(received.data, f.data); +} + +TEST(SocketCANToTopicTest, checkInvalidFrameHandling) +{ + // - tries to send a non-extended frame with an id larger than 11 bits. + // that should not be sent. + // - verifies that sending one larger than 11 bits actually works. + + // sending a message with a dlc > 8 is not possible as the DummyInterface + // causes a crash then. + + ros::NodeHandle nh(""), nh_param("~"); + + can::DummyBus bus("checkInvalidFrameHandling"); + + // create the dummy interface + can::ThreadedDummyInterfaceSharedPtr dummy = std::make_shared(); + + // start the to topic bridge. + socketcan_bridge::SocketCANToTopic to_topic_bridge(&nh, &nh_param, dummy); + to_topic_bridge.setup(); // initiate the message callbacks + + dummy->init(bus.name, true, can::NoSettings::create()); + + // create a frame collector. + msgCollector message_collector_; + + // register for messages on received_messages. + ros::Subscriber subscriber_ = nh.subscribe("received_messages", 1, &msgCollector::msgCallback, &message_collector_); + + // create a message + can::Frame f; + f.is_extended = false; + f.id = (1<<11)+1; // this is an illegal CAN packet... should not be sent. + + // send the can::Frame over the driver. + // dummy->send(f); + + // give some time for the interface some time to process the message + ros::WallDuration(1.0).sleep(); + ros::spinOnce(); + EXPECT_EQ(message_collector_.messages.size(), 0); + + f.is_extended = true; + f.id = (1<<11)+1; // now it should be alright. + + dummy->send(f); + ros::WallDuration(1.0).sleep(); + ros::spinOnce(); + EXPECT_EQ(message_collector_.messages.size(), 1); +} + +TEST(SocketCANToTopicTest, checkCorrectCanIdFilter) +{ + ros::NodeHandle nh(""), nh_param("~"); + + can::DummyBus bus("checkCorrectCanIdFilter"); + + // create the dummy interface + can::ThreadedDummyInterfaceSharedPtr dummy = std::make_shared(); + + // create can_id vector with id that should be passed and published to ros + std::vector pass_can_ids; + pass_can_ids.push_back(0x1337); + + // start the to topic bridge. + socketcan_bridge::SocketCANToTopic to_topic_bridge(&nh, &nh_param, dummy); + to_topic_bridge.setup(can::tofilters(pass_can_ids)); // initiate the message callbacks + + // init the driver to test stateListener (not checked automatically). + dummy->init(bus.name, true, can::NoSettings::create()); + + // create a frame collector. + msgCollector message_collector_; + + // register for messages on received_messages. + ros::Subscriber subscriber_ = nh.subscribe("received_messages", 1, &msgCollector::msgCallback, &message_collector_); + + // create a can frame + can::Frame f; + f.is_extended = true; + f.is_rtr = false; + f.is_error = false; + f.id = 0x1337; + f.dlc = 8; + for (uint8_t i=0; i < f.dlc; i++) + { + f.data[i] = i; + } + + // send the can frame to the driver + dummy->send(f); + + // give some time for the interface some time to process the message + ros::WallDuration(1.0).sleep(); + ros::spinOnce(); + + ASSERT_EQ(1, message_collector_.messages.size()); + + // compare the received can_msgs::Frame message to the sent can::Frame. + can::Frame received; + can_msgs::Frame msg = message_collector_.messages.back(); + socketcan_bridge::convertMessageToSocketCAN(msg, received); + + EXPECT_EQ(received.id, f.id); + EXPECT_EQ(received.dlc, f.dlc); + EXPECT_EQ(received.is_extended, f.is_extended); + EXPECT_EQ(received.is_rtr, f.is_rtr); + EXPECT_EQ(received.is_error, f.is_error); + EXPECT_EQ(received.data, f.data); +} + +TEST(SocketCANToTopicTest, checkInvalidCanIdFilter) +{ + ros::NodeHandle nh(""), nh_param("~"); + + can::DummyBus bus("checkInvalidCanIdFilter"); + + // create the dummy interface + can::ThreadedDummyInterfaceSharedPtr dummy = std::make_shared(); + + // create can_id vector with id that should not be received on can bus + std::vector pass_can_ids; + pass_can_ids.push_back(0x300); + + // start the to topic bridge. + socketcan_bridge::SocketCANToTopic to_topic_bridge(&nh, &nh_param, dummy); + to_topic_bridge.setup(can::tofilters(pass_can_ids)); // initiate the message callbacks + + // init the driver to test stateListener (not checked automatically). + dummy->init(bus.name, true, can::NoSettings::create()); + + // create a frame collector. + msgCollector message_collector_; + + // register for messages on received_messages. + ros::Subscriber subscriber_ = nh.subscribe("received_messages", 1, &msgCollector::msgCallback, &message_collector_); + + // create a can frame + can::Frame f; + f.is_extended = true; + f.is_rtr = false; + f.is_error = false; + f.id = 0x1337; + f.dlc = 8; + for (uint8_t i=0; i < f.dlc; i++) + { + f.data[i] = i; + } + + // send the can frame to the driver + dummy->send(f); + + // give some time for the interface some time to process the message + ros::WallDuration(1.0).sleep(); + ros::spinOnce(); + + EXPECT_EQ(0, message_collector_.messages.size()); +} + +TEST(SocketCANToTopicTest, checkMaskFilter) +{ + ros::NodeHandle nh(""), nh_param("~"); + + can::DummyBus bus("checkMaskFilter"); + + // create the dummy interface + can::ThreadedDummyInterfaceSharedPtr dummy = std::make_shared(); + + // setup filter + can::FilteredFrameListener::FilterVector filters; + filters.push_back(can::tofilter("300:ffe")); + + // start the to topic bridge. + socketcan_bridge::SocketCANToTopic to_topic_bridge(&nh, &nh_param, dummy); + to_topic_bridge.setup(filters); // initiate the message callbacks + + // init the driver to test stateListener (not checked automatically). + dummy->init(bus.name, true, can::NoSettings::create()); + + // create a frame collector. + msgCollector message_collector_; + + // register for messages on received_messages. + ros::Subscriber subscriber_ = nh.subscribe("received_messages", 10, &msgCollector::msgCallback, &message_collector_); + + const std::string pass1("300#1234"), nopass1("302#9999"), pass2("301#5678"); + + // send the can framew to the driver + dummy->send(can::toframe(pass1)); + dummy->send(can::toframe(nopass1)); + dummy->send(can::toframe(pass2)); + + // give some time for the interface some time to process the message + ros::WallDuration(1.0).sleep(); + ros::spinOnce(); + + // compare the received can_msgs::Frame message to the sent can::Frame. + ASSERT_EQ(2, message_collector_.messages.size()); + EXPECT_EQ(pass1, convertMessageToString(message_collector_.messages.front())); + EXPECT_EQ(pass2, convertMessageToString(message_collector_.messages.back())); +} + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "test_to_topic"); + ros::NodeHandle nh; + ros::WallDuration(1.0).sleep(); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/Devices/Libraries/Ros/ros_canopen/socketcan_bridge/test/vcan0_to_vcan1.launch b/Devices/Libraries/Ros/ros_canopen/socketcan_bridge/test/vcan0_to_vcan1.launch new file mode 100755 index 0000000..2861c10 --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/socketcan_bridge/test/vcan0_to_vcan1.launch @@ -0,0 +1,22 @@ + + + + + + + + + + \ No newline at end of file diff --git a/Devices/Libraries/Ros/ros_canopen/socketcan_interface/CHANGELOG.rst b/Devices/Libraries/Ros/ros_canopen/socketcan_interface/CHANGELOG.rst new file mode 100755 index 0000000..d16e009 --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/socketcan_interface/CHANGELOG.rst @@ -0,0 +1,189 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package socketcan_interface +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.8.5 (2020-09-22) +------------------ +* check settings pointer and print error if null +* initalize settings properly in deprecated SocketCANInterface::init +* Contributors: Mathias Lüdtke + +0.8.4 (2020-08-22) +------------------ +* make parse_error_mask a static member function +* pass settings from ROS node to SocketCANInterface +* add support for recursive XmlRpcSettings lookups +* implemented report-only and fatal errors for SocketCANInterface +* added settings parameter to DriverInterface::init +* moved XmlRpcSettings to socketcan_interface +* moved canopen::Settings into can namespace +* Contributors: Mathias Lüdtke + +0.8.3 (2020-05-07) +------------------ +* Fixed Boost link in test-dispacher + Signed-off-by: ahcorde +* Bump CMake version to avoid CMP0048 warning + Signed-off-by: ahcorde +* do not print ERROR in candump +* Contributors: Mathias Lüdtke, ahcorde + +0.8.2 (2019-11-04) +------------------ +* enable rosconsole_bridge bindings +* switch to new logging macros +* add logging based on console_bridge +* handle extended frame strings like candump +* implement Frame::fullid() +* removed implicit Header operator +* move stream operators into can namespace +* Contributors: Mathias Lüdtke + +0.8.1 (2019-07-14) +------------------ +* Set C++ standard to c++14 +* implemented test for dispatcher +* Replacing typedefs in socketcan_interface with using aliases. +* added Delegate helpers for backwards compatibility +* implemented create\*ListenerM helpers +* Replacing FastDelegate with std::function and std::bind. +* Contributors: Harsh Deshpande, Joshua Whitley, Mathias Lüdtke, pzzlr + +0.8.0 (2018-07-11) +------------------ +* migrated to std::function and std::bind +* got rid of boost::noncopyable +* replaced BOOST_FOREACH +* migrated to std::unordered_map and std::unordered_set +* migrated to std:array +* migrated to std pointers +* removed deprecated types +* introduced ROSCANOPEN_MAKE_SHARED +* added c_array access functons to can::Frame +* Contributors: Mathias Lüdtke + +0.7.8 (2018-05-04) +------------------ +* Revert "pull make_shared into namespaces" + This reverts commit 9b2cd05df76d223647ca81917d289ca6330cdee6. +* Contributors: Mathias Lüdtke + +0.7.7 (2018-05-04) +------------------ +* pull make_shared into namespaces +* added types for all shared_ptrs +* fix catkin_lint warnings in filter tests +* migrate to new classloader headers +* find and link the thread library properly +* compile also with boost >= 1.66.0 +* explicitly include iostream to compile with boost >= 1.65.0 +* address catkin_lint errors/warnings +* added test for FilteredFrameListener +* fix string parsers +* default to relaxed filter handling + works for standard and extended frames +* fix string handling of extended frames +* added filter parsers + should work for vector, vector and custom vector-like classes +* implemented mask and range filters for can::Frame +* Contributors: Lukas Bulwahn, Mathias Lüdtke + +0.7.6 (2017-08-30) +------------------ +* make can::Header/Frame::isValid() const +* Contributors: Mathias Lüdtke + +0.7.5 (2017-05-29) +------------------ +* fix rosdep dependency on kernel headers +* Contributors: Mathias Lüdtke + +0.7.4 (2017-04-25) +------------------ + +0.7.3 (2017-04-25) +------------------ + +0.7.2 (2017-03-28) +------------------ + +0.7.1 (2017-03-20) +------------------ +* stop CAN driver on read errors as well +* expose socketcan handle +* implemented BCMsocket +* introduced BufferedReader::readUntil +* Contributors: Mathias Lüdtke + +0.7.0 (2016-12-13) +------------------ + +0.6.5 (2016-12-10) +------------------ +* removed Baseclass typedef since its use prevented virtual functions calls +* add missing chrono dependency +* Added catch-all features in BufferedReader +* hardened code with the help of cppcheck +* styled and sorted CMakeLists.txt + * removed boilerplate comments + * indention + * reviewed exported dependencies +* styled and sorted package.xml +* update package URLs +* Improves StateInterface implementation of the DummyInterface. + The doesLoopBack() method now returns the correct value. A state change is + correctly dispatched when the init() method is called. +* Changes inheritance of DummyInterface to DriverInterface. + Such that this interface can also be used for tests requiring a DriverInterface + class. + Test results of the socketcan_interface tests are unchanged by this + modification as it only uses the CommInterface methods. +* added socketcan_interface_string to test +* moved string functions into separate lib +* Introduced setNotReady, prevent enqueue() to switch from closed to open +* Reading state\_ should be protected by lock +* improved BufferedReader interface and ScopedEnabler +* added flush() and max length support to BufferedReader +* added BufferedReader +* wake multiple waiting threads if needed +* pad hex buffer strings in all cases +* removed unstable StateWaiter::wait_for +* Contributors: Ivor Wanders, Mathias Lüdtke, Michael Stoll + +0.6.4 (2015-07-03) +------------------ +* added missing include, revised depends etc. + + +0.6.3 (2015-06-30) +------------------ +* dependencies revised +* reordering fix for `#87 `_ +* intialize structs +* tostring fixed for headers +* removed empty test +* added DummyInterface with first test +* added message string helper +* added missing include +* install socketcan_interface_plugin.xml +* migrated to class_loader for non-ROS parts +* moved ThreadedInterface to dedicated header +* removed bitrate, added loopback to DriverInterface::init +* added socketcan plugin +* CommInterstate and StateInterface are now bases of DriverInterface. + Therefore DispatchedInterface was moved into AsioBase. +* remove debug prints +* shutdown asio driver in destructor +* proper mask shifts +* Contributors: Mathias Lüdtke + +0.6.2 (2014-12-18) +------------------ + +0.6.1 (2014-12-15) +------------------ +* remove ipa_* and IPA_* prefixes +* fixed catkin_lint errors +* added descriptions and authors +* renamed ipa_can_interface to socketcaninterface +* Contributors: Florian Weisshardt, Mathias Lüdtke diff --git a/Devices/Libraries/Ros/ros_canopen/socketcan_interface/CMakeLists.txt b/Devices/Libraries/Ros/ros_canopen/socketcan_interface/CMakeLists.txt new file mode 100755 index 0000000..91f2594 --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/socketcan_interface/CMakeLists.txt @@ -0,0 +1,168 @@ +cmake_minimum_required(VERSION 3.0.2) +project(socketcan_interface) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_STANDARD 14) +endif() + +find_package(catkin REQUIRED + COMPONENTS + class_loader +) + +find_package(console_bridge REQUIRED) + +find_package(Boost REQUIRED + COMPONENTS + chrono + system + thread +) + +find_package(Threads REQUIRED) + +catkin_package( + INCLUDE_DIRS + include + LIBRARIES + ${PROJECT_NAME}_string + CATKIN_DEPENDS + DEPENDS + Boost + console_bridge +) + +include_directories( + include + ${Boost_INCLUDE_DIRS} + ${catkin_INCLUDE_DIRS} + ${console_bridge_INCLUDE_DIRS} +) + +# ${PROJECT_NAME}_string +add_library(${PROJECT_NAME}_string + src/string.cpp +) + +# socketcan_dump +add_executable(socketcan_dump + src/candump.cpp +) + +target_link_libraries(socketcan_dump + ${PROJECT_NAME}_string + ${console_bridge_LIBRARIES} + ${catkin_LIBRARIES} + ${Boost_LIBRARIES} + ${CMAKE_THREAD_LIBS_INIT} +) + +# socketcan_bcm +add_executable(socketcan_bcm + src/canbcm.cpp +) + +target_link_libraries(socketcan_bcm + ${PROJECT_NAME}_string + ${console_bridge_LIBRARIES} + ${catkin_LIBRARIES} + ${Boost_LIBRARIES} +) + +# ${PROJECT_NAME}_plugin +add_library(${PROJECT_NAME}_plugin + src/${PROJECT_NAME}_plugin.cpp +) +target_link_libraries(${PROJECT_NAME}_plugin + ${console_bridge_LIBRARIES} + ${catkin_LIBRARIES} + ${Boost_LIBRARIES} +) + +install( + TARGETS + socketcan_bcm + socketcan_dump + ${PROJECT_NAME}_plugin + ${PROJECT_NAME}_string + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install( + DIRECTORY + include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" +) + +install( + FILES + ${PROJECT_NAME}_plugin.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) +if(CATKIN_ENABLE_TESTING) + find_package(xmlrpcpp REQUIRED) + + catkin_add_gtest(${PROJECT_NAME}-test_dummy_interface + test/test_dummy_interface.cpp + ) + target_link_libraries(${PROJECT_NAME}-test_dummy_interface + ${PROJECT_NAME}_string + ${console_bridge_LIBRARIES} + ${catkin_LIBRARIES} + ${Boost_LIBRARIES} + ) + + catkin_add_gtest(${PROJECT_NAME}-test_delegates + test/test_delegates.cpp + ) + target_link_libraries(${PROJECT_NAME}-test_delegates + ${PROJECT_NAME}_string + ${catkin_LIBRARIES} + ${Boost_LIBRARIES} + ) + + catkin_add_gtest(${PROJECT_NAME}-test_settings + test/test_settings.cpp + ) + target_include_directories(${PROJECT_NAME}-test_settings PRIVATE + ${xmlrpcpp_INCLUDE_DIRS} + ) + target_link_libraries(${PROJECT_NAME}-test_settings + ${PROJECT_NAME}_string + ${catkin_LIBRARIES} + ${xmlrpcpp_LIBRARIES} + ) + + catkin_add_gtest(${PROJECT_NAME}-test_string + test/test_string.cpp + ) + target_link_libraries(${PROJECT_NAME}-test_string + ${PROJECT_NAME}_string + ${console_bridge_LIBRARIES} + ${catkin_LIBRARIES} + ) + + catkin_add_gtest(${PROJECT_NAME}-test_filter + test/test_filter.cpp + ) + target_link_libraries(${PROJECT_NAME}-test_filter + ${PROJECT_NAME}_string + ${console_bridge_LIBRARIES} + ${catkin_LIBRARIES} + ${Boost_LIBRARIES} + ) + + catkin_add_gtest(${PROJECT_NAME}-test_dispatcher + test/test_dispatcher.cpp + ) + target_link_libraries(${PROJECT_NAME}-test_dispatcher + ${PROJECT_NAME}_string + ${catkin_LIBRARIES} + ${Boost_LIBRARIES} + ) + target_compile_options(${PROJECT_NAME}-test_dispatcher PRIVATE -Wno-deprecated-declarations) +endif() diff --git a/Devices/Libraries/Ros/ros_canopen/socketcan_interface/include/socketcan_interface/asio_base.h b/Devices/Libraries/Ros/ros_canopen/socketcan_interface/include/socketcan_interface/asio_base.h new file mode 100755 index 0000000..cf36ca1 --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/socketcan_interface/include/socketcan_interface/asio_base.h @@ -0,0 +1,136 @@ +#ifndef H_ASIO_BASE +#define H_ASIO_BASE + +#include +#include +#include +#include +#include +#include + +namespace can{ + + +template class AsioDriver : public DriverInterface{ + using FrameDispatcher = FilteredDispatcher; + using StateDispatcher = SimpleDispatcher; + FrameDispatcher frame_dispatcher_; + StateDispatcher state_dispatcher_; + + State state_; + boost::mutex state_mutex_; + boost::mutex socket_mutex_; + + void shutdown_internal(){ + if(socket_.is_open()){ + socket_.cancel(); + socket_.close(); + } + io_service_.stop(); + } + +protected: + boost::asio::io_service io_service_; +#if BOOST_ASIO_VERSION >= 101200 // Boost 1.66+ + boost::asio::io_context::strand strand_; +#else + boost::asio::strand strand_; +#endif + Socket socket_; + Frame input_; + + virtual void triggerReadSome() = 0; + virtual bool enqueue(const Frame & msg) = 0; + + void dispatchFrame(const Frame &msg){ + strand_.post([this, msg]{ frame_dispatcher_.dispatch(msg.key(), msg);} ); // copies msg + } + void setErrorCode(const boost::system::error_code& error){ + boost::mutex::scoped_lock lock(state_mutex_); + if(state_.error_code != error){ + state_.error_code = error; + state_dispatcher_.dispatch(state_); + } + } + void setInternalError(unsigned int internal_error){ + boost::mutex::scoped_lock lock(state_mutex_); + if(state_.internal_error != internal_error){ + state_.internal_error = internal_error; + state_dispatcher_.dispatch(state_); + } + } + + void setDriverState(State::DriverState state){ + boost::mutex::scoped_lock lock(state_mutex_); + if(state_.driver_state != state){ + state_.driver_state = state; + state_dispatcher_.dispatch(state_); + } + } + void setNotReady(){ + setDriverState(socket_.is_open()?State::open : State::closed); + } + + void frameReceived(const boost::system::error_code& error){ + if(!error){ + dispatchFrame(input_); + triggerReadSome(); + }else{ + setErrorCode(error); + setNotReady(); + } + } + + AsioDriver() + : strand_(io_service_), socket_(io_service_) + {} + +public: + virtual ~AsioDriver() { shutdown_internal(); } + + State getState(){ + boost::mutex::scoped_lock lock(state_mutex_); + return state_; + } + virtual void run(){ + setNotReady(); + + if(getState().driver_state == State::open){ + io_service_.reset(); + boost::asio::io_service::work work(io_service_); + setDriverState(State::ready); + + boost::thread post_thread([this]() { io_service_.run(); }); + + triggerReadSome(); + + boost::system::error_code ec; + io_service_.run(ec); + setErrorCode(ec); + + setNotReady(); + } + state_dispatcher_.dispatch(getState()); + } + virtual bool send(const Frame & msg){ + return getState().driver_state == State::ready && enqueue(msg); + } + + virtual void shutdown(){ + shutdown_internal(); + } + + virtual FrameListenerConstSharedPtr createMsgListener(const FrameFunc &delegate){ + return frame_dispatcher_.createListener(delegate); + } + virtual FrameListenerConstSharedPtr createMsgListener(const Frame::Header&h , const FrameFunc &delegate){ + return frame_dispatcher_.createListener(h.key(), delegate); + } + virtual StateListenerConstSharedPtr createStateListener(const StateFunc &delegate){ + return state_dispatcher_.createListener(delegate); + } + +}; + +} // namespace can +#endif diff --git a/Devices/Libraries/Ros/ros_canopen/socketcan_interface/include/socketcan_interface/bcm.h b/Devices/Libraries/Ros/ros_canopen/socketcan_interface/include/socketcan_interface/bcm.h new file mode 100755 index 0000000..cbf456f --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/socketcan_interface/include/socketcan_interface/bcm.h @@ -0,0 +1,120 @@ +#ifndef H_CAN_BCM +#define H_CAN_BCM + +#include + +#include +#include +#include +#include + +#include +#include +#include + +#include + +#include + +namespace can { + +class BCMsocket{ + int s_; + struct Message { + size_t size; + uint8_t *data; + Message(size_t n) + : size(sizeof(bcm_msg_head) + sizeof(can_frame)*n), data(new uint8_t[size]) + { + assert(n<=256); + std::memset(data, 0, size); + head().nframes = n; + } + bcm_msg_head& head() { + return *(bcm_msg_head*)data; + } + template void setIVal2(T period){ + long long usec = boost::chrono::duration_cast(period).count(); + head().ival2.tv_sec = usec / 1000000; + head().ival2.tv_usec = usec % 1000000; + } + void setHeader(Header header){ + head().can_id = header.id | (header.is_extended?CAN_EFF_FLAG:0); + } + bool write(int s){ + return ::write(s, data, size) > 0; + } + ~Message(){ + delete[] data; + data = 0; + size = 0; + } + }; +public: + BCMsocket():s_(-1){ + } + bool init(const std::string &device){ + s_ = socket(PF_CAN, SOCK_DGRAM, CAN_BCM); + + if(s_ < 0 ) return false; + struct ifreq ifr; + std::strcpy(ifr.ifr_name, device.c_str()); + int ret = ioctl(s_, SIOCGIFINDEX, &ifr); + + if(ret != 0){ + shutdown(); + return false; + } + + struct sockaddr_can addr = {0}; + addr.can_family = AF_CAN; + addr.can_ifindex = ifr.ifr_ifindex; + + ret = connect(s_, (struct sockaddr *)&addr, sizeof(addr)); + + if(ret < 0){ + shutdown(); + return false; + } + return true; + } + template bool startTX(DurationType period, Header header, size_t num, Frame *frames) { + Message msg(num); + msg.setHeader(header); + msg.setIVal2(period); + + bcm_msg_head &head = msg.head(); + + head.opcode = TX_SETUP; + head.flags |= SETTIMER | STARTTIMER; + + for(size_t i=0; i < num; ++i){ // msg nr + head.frames[i].can_dlc = frames[i].dlc; + head.frames[i].can_id = head.can_id; + for(size_t j = 0; j < head.frames[i].can_dlc; ++j){ // byte nr + head.frames[i].data[j] = frames[i].data[j]; + } + } + return msg.write(s_); + } + bool stopTX(Header header){ + Message msg(0); + msg.head().opcode = TX_DELETE; + msg.setHeader(header); + return msg.write(s_); + } + void shutdown(){ + if(s_ > 0){ + close(s_); + s_ = -1; + } + } + + virtual ~BCMsocket(){ + shutdown(); + } +}; + +} + +#endif diff --git a/Devices/Libraries/Ros/ros_canopen/socketcan_interface/include/socketcan_interface/delegates.h b/Devices/Libraries/Ros/ros_canopen/socketcan_interface/include/socketcan_interface/delegates.h new file mode 100755 index 0000000..ffd9c81 --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/socketcan_interface/include/socketcan_interface/delegates.h @@ -0,0 +1,24 @@ +#ifndef SOCKETCAN_INTERFACE_DELEGATES_H_ +#define SOCKETCAN_INTERFACE_DELEGATES_H_ + +#include + +namespace can +{ + +template class DelegateHelper : public T { +public: + template + DelegateHelper(Object &&o, typename T::result_type (Instance::*member)(Args... args)) : + T([o, member](Args... args) -> typename T::result_type { return ((*o).*member)(args...); }) + { + } + template + DelegateHelper(Callable &&c) : T(c) + { + } +}; + +} // namespace can + +#endif // SOCKETCAN_INTERFACE_DELEGATES_H_ diff --git a/Devices/Libraries/Ros/ros_canopen/socketcan_interface/include/socketcan_interface/dispatcher.h b/Devices/Libraries/Ros/ros_canopen/socketcan_interface/include/socketcan_interface/dispatcher.h new file mode 100755 index 0000000..d31a348 --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/socketcan_interface/include/socketcan_interface/dispatcher.h @@ -0,0 +1,120 @@ +#ifndef H_CAN_DISPATCHER +#define H_CAN_DISPATCHER + +#include +#include +#include +#include + +#include +#include + +namespace can{ + +template< typename Listener > class SimpleDispatcher{ +public: + using Callable = typename Listener::Callable; + using Type = typename Listener::Type; + using ListenerConstSharedPtr = typename Listener::ListenerConstSharedPtr; +protected: + class DispatcherBase; + using DispatcherBaseSharedPtr = std::shared_ptr; + class DispatcherBase { + DispatcherBase(const DispatcherBase&) = delete; // prevent copies + DispatcherBase& operator=(const DispatcherBase&) = delete; + + class GuardedListener: public Listener{ + std::weak_ptr guard_; + public: + GuardedListener(DispatcherBaseSharedPtr g, const Callable &callable): Listener(callable), guard_(g){} + virtual ~GuardedListener() { + DispatcherBaseSharedPtr d = guard_.lock(); + if(d){ + d->remove(this); + } + } + }; + + boost::mutex &mutex_; + std::list listeners_; + public: + DispatcherBase(boost::mutex &mutex) : mutex_(mutex) {} + void dispatch_nolock(const Type &obj, const Listener* loopback=nullptr) const{ + for(typename std::list::const_iterator it=listeners_.begin(); it != listeners_.end(); ++it){ + if (loopback != *it) { + (**it)(obj); + } + } + } + void remove(Listener *d){ + boost::mutex::scoped_lock lock(mutex_); + listeners_.remove(d); + } + size_t numListeners(){ + boost::mutex::scoped_lock lock(mutex_); + return listeners_.size(); + } + + static ListenerConstSharedPtr createListener(DispatcherBaseSharedPtr dispatcher, const Callable &callable){ + ListenerConstSharedPtr l(new GuardedListener(dispatcher,callable)); + dispatcher->listeners_.push_back(l.get()); + return l; + } + }; + boost::mutex mutex_; + DispatcherBaseSharedPtr dispatcher_; +public: + SimpleDispatcher() : dispatcher_(new DispatcherBase(mutex_)) {} + ListenerConstSharedPtr createListener(const Callable &callable){ + boost::mutex::scoped_lock lock(mutex_); + return DispatcherBase::createListener(dispatcher_, callable); + } + void dispatch(const Type &obj){ + boost::mutex::scoped_lock lock(mutex_); + dispatcher_->dispatch_nolock(obj); + } + void dispatch_filtered(const Type &obj, ListenerConstSharedPtr without){ + boost::mutex::scoped_lock lock(mutex_); + dispatcher_->dispatch_nolock(obj, without.get()); + } + size_t numListeners(){ + return dispatcher_->numListeners(); + } + operator Callable() { return Callable(this,&SimpleDispatcher::dispatch); } +}; + +template > class FilteredDispatcher: public SimpleDispatcher{ + using BaseClass = SimpleDispatcher; + std::unordered_map filtered_; +public: + using BaseClass::createListener; + typename BaseClass::ListenerConstSharedPtr createListener(const K &key, const typename BaseClass::Callable &callable){ + boost::mutex::scoped_lock lock(BaseClass::mutex_); + typename BaseClass::DispatcherBaseSharedPtr &ptr = filtered_[key]; + if(!ptr) ptr.reset(new typename BaseClass::DispatcherBase(BaseClass::mutex_)); + return BaseClass::DispatcherBase::createListener(ptr, callable); + } + + template + [[deprecated("provide key explicitly")]] + typename BaseClass::ListenerConstSharedPtr createListener(const T &key, const typename BaseClass::Callable &callable){ + return createListener(static_cast(key), callable); + } + + void dispatch(const K &key, const typename BaseClass::Type &obj){ + boost::mutex::scoped_lock lock(BaseClass::mutex_); + typename BaseClass::DispatcherBaseSharedPtr &ptr = filtered_[key]; + if(ptr) ptr->dispatch_nolock(obj); + BaseClass::dispatcher_->dispatch_nolock(obj); + } + + [[deprecated("provide key explicitly")]] + void dispatch(const typename BaseClass::Type &obj){ + return dispatch(static_cast(obj), obj); + } + + operator typename BaseClass::Callable() { return typename BaseClass::Callable(this,&FilteredDispatcher::dispatch); } +}; + +} // namespace can +#endif diff --git a/Devices/Libraries/Ros/ros_canopen/socketcan_interface/include/socketcan_interface/dummy.h b/Devices/Libraries/Ros/ros_canopen/socketcan_interface/include/socketcan_interface/dummy.h new file mode 100755 index 0000000..1bcf7ea --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/socketcan_interface/include/socketcan_interface/dummy.h @@ -0,0 +1,245 @@ +#ifndef SOCKETCAN_INTERFACE_DUMMY_H +#define SOCKETCAN_INTERFACE_DUMMY_H + +#include +#include + +#include "interface.h" +#include "dispatcher.h" +#include "string.h" +#include "logging.h" +#include "threading.h" +#include + + +namespace can { + +class DummyBus { +public: + using FrameDispatcher = SimpleDispatcher; + using FrameDispatcherSharedPtr = std::shared_ptr; + using Buses = std::unordered_map; +private: + static Buses& get_buses() { + static Buses buses; + return buses; + } + FrameDispatcherSharedPtr bus_; +public: + const std::string name; + DummyBus(const std::string& name) : name(name), bus_(get_buses().emplace(name, std::make_shared()).first->second) { + } + ~DummyBus() { + get_buses().erase(name); + } + class Connection { + public: + inline Connection(FrameDispatcherSharedPtr bus, FrameListenerConstSharedPtr listener) + : bus_(bus), listener_(listener) + {} + void dispatch(const Frame & msg){ + bus_->dispatch_filtered(msg, listener_); + } + private: + FrameDispatcherSharedPtr bus_; + FrameListenerConstSharedPtr listener_; + }; + using ConnectionSharedPtr = std::shared_ptr; + template static inline ConnectionSharedPtr connect(const std::string & name, Instance inst, Callable callable) { + FrameDispatcherSharedPtr bus = get_buses().at(name); + return std::make_shared(bus, bus->createListener(std::bind(callable, inst, std::placeholders::_1))); + } +}; + +class DummyInterface : public DriverInterface{ + using FrameDispatcher = FilteredDispatcher; + using StateDispatcher = SimpleDispatcher; + FrameDispatcher frame_dispatcher_; + StateDispatcher state_dispatcher_; + DummyBus::ConnectionSharedPtr bus_; + State state_; + std::deque in_; + bool loopback_; + bool trace_; + boost::mutex mutex_; + boost::condition_variable cond_; + + void setDriverState(State::DriverState state){ + boost::mutex::scoped_lock lock(mutex_); + if(state_.driver_state != state){ + state_.driver_state = state; + state_dispatcher_.dispatch(state_); + } + cond_.notify_all(); + } + void enqueue(const Frame & msg){ + boost::mutex::scoped_lock cond_lock(mutex_); + in_.push_back(msg); + cond_lock.unlock(); + cond_.notify_all(); + } + + void shutdown_internal(){ + setDriverState(State::closed); + bus_.reset(); + }; +public: + DummyInterface() : loopback_(false), trace_(false) {} + DummyInterface(bool loopback) : loopback_(loopback), trace_(false) {} + virtual ~DummyInterface() { shutdown_internal(); } + + + virtual bool send(const Frame & msg){ + if (trace_) { + ROSCANOPEN_DEBUG("socketcan_interface", "send: " << msg); + } + if (loopback_) { + enqueue(msg); + } + bus_->dispatch(msg); + return true; + } + + virtual FrameListenerConstSharedPtr createMsgListener(const FrameFunc &delegate){ + return frame_dispatcher_.createListener(delegate); + } + virtual FrameListenerConstSharedPtr createMsgListener(const Frame::Header&h , const FrameFunc &delegate){ + return frame_dispatcher_.createListener(h.key(), delegate); + } + + // methods from StateInterface + virtual bool recover(){return false;}; + + virtual State getState(){ + boost::mutex::scoped_lock cond_lock(mutex_); + return state_; + } + + virtual void shutdown(){ + flush(); + shutdown_internal(); + }; + + virtual bool translateError(unsigned int internal_error, std::string & str){ + if (!internal_error) { + str = "OK"; + return true; + } + return false; + }; + + virtual bool doesLoopBack() const { + return loopback_; + }; + + void flush(){ + while (true) { + { + boost::mutex::scoped_lock cond_lock(mutex_); + if (in_.empty() || state_.driver_state == State::closed) { + return; + } + } + boost::this_thread::sleep_for(boost::chrono::milliseconds(100)); + } + } + + virtual void run(){ + boost::mutex::scoped_lock cond_lock(mutex_); + while (true) { + + state_.driver_state = State::ready; + state_dispatcher_.dispatch(state_); + + cond_.wait_for(cond_lock, boost::chrono::seconds(1)); + while(!in_.empty()){ + const can::Frame msg = in_.front(); + in_.pop_front(); + if (trace_) { + ROSCANOPEN_DEBUG("socketcan_interface", "receive: " << msg); + } + frame_dispatcher_.dispatch(msg.key(), msg); + } + if (state_.driver_state == State::closed) { + return; + } + } + } + + bool init(const std::string &device, bool loopback){ + loopback_ = loopback; + bus_ = DummyBus::connect(device, this, &DummyInterface::enqueue); + setDriverState(State::open); + return true; + } + + virtual bool init(const std::string &device, bool loopback, SettingsConstSharedPtr settings) { + if(DummyInterface::init(device, loopback)) { + trace_ = settings->get_optional("trace", false); + return true; + } else { + return false; + } + } + virtual StateListenerConstSharedPtr createStateListener(const StateFunc &delegate){ + return state_dispatcher_.createListener(delegate); + } + +}; + +using DummyInterfaceSharedPtr = std::shared_ptr; +using ThreadedDummyInterface = ThreadedInterface; +using ThreadedDummyInterfaceSharedPtr = std::shared_ptr; + +class DummyResponder { +public: + DummyResponder() : dummy_(), listener_(dummy_.createMsgListenerM(this, &DummyResponder::respond)) { + } + bool init(const DummyBus &bus) { + return dummy_.init(bus.name, false, NoSettings::create()); + } + void flush() { + dummy_.flush(); + } + virtual ~DummyResponder() {} +protected: + void send(const Frame & msg) { + dummy_.send(msg); + } +private: + ThreadedDummyInterface dummy_; + FrameListenerConstSharedPtr listener_; + virtual void respond(const Frame & msg) = 0; +}; + +class DummyReplay : public DummyResponder { +private: + virtual void respond(const Frame & msg) { + const auto &front = replay_.front(); + if (tostring(msg, true) == front.first) { + for(auto &f: front.second) { + send(f); + } + replay_.pop_front(); + } + } + std::list > > replay_; + bool error_; +public: + void add(const std::string &read, const std::initializer_list &write){ + std::vector frames; + frames.reserve(write.size()); + for(auto &w : write) { + frames.push_back(toframe(w)); + } + replay_.push_back(std::make_pair(boost::to_lower_copy(read), frames)); + } + void add(const std::string &read, const std::string &write){ + add(read, {write}); + } + bool done() { return replay_.empty(); } +}; + +} + +#endif diff --git a/Devices/Libraries/Ros/ros_canopen/socketcan_interface/include/socketcan_interface/filter.h b/Devices/Libraries/Ros/ros_canopen/socketcan_interface/include/socketcan_interface/filter.h new file mode 100755 index 0000000..81e96f0 --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/socketcan_interface/include/socketcan_interface/filter.h @@ -0,0 +1,70 @@ +#ifndef SOCKETCAN_INTERFACE_FILTER_H +#define SOCKETCAN_INTERFACE_FILTER_H + +#include + +#include "interface.h" + +namespace can { + +class FrameFilter { +public: + virtual bool pass(const can::Frame &frame) const = 0; + virtual ~FrameFilter() {} +}; +using FrameFilterSharedPtr = std::shared_ptr; + +class FrameMaskFilter : public FrameFilter { +public: + static const uint32_t MASK_ALL = 0xffffffff; + static const uint32_t MASK_RELAXED = ~Frame::EXTENDED_MASK; + FrameMaskFilter(uint32_t can_id, uint32_t mask = MASK_RELAXED, bool invert = false) + : mask_(mask), masked_id_(can_id & mask), invert_(invert) + {} + virtual bool pass(const can::Frame &frame) const{ + const uint32_t k = frame.key(); + return ((mask_ & k) == masked_id_) != invert_; + } +private: + const uint32_t mask_; + const uint32_t masked_id_; + const bool invert_; +}; + +class FrameRangeFilter : public FrameFilter { +public: + FrameRangeFilter(uint32_t min_id, uint32_t max_id, bool invert = false) + : min_id_(min_id), max_id_(max_id), invert_(invert) + {} + virtual bool pass(const can::Frame &frame) const{ + const uint32_t k = frame.key(); + return (min_id_ <= k && k <= max_id_) != invert_; + } +private: + const uint32_t min_id_; + const uint32_t max_id_; + const bool invert_; +}; + +class FilteredFrameListener : public CommInterface::FrameListener { +public: + using FilterVector = std::vector; + FilteredFrameListener(CommInterfaceSharedPtr comm, const Callable &callable, const FilterVector &filters) + : CommInterface::FrameListener(callable), + filters_(filters), + listener_(comm->createMsgListener([this](const Frame &frame) { + for(FilterVector::const_iterator it=this->filters_.begin(); it != this->filters_.end(); ++it) { + if((*it)->pass(frame)){ + (*this)(frame); + break; + } + } + })) + {} + const std::vector filters_; + CommInterface::FrameListenerConstSharedPtr listener_; +}; + +} // namespace can + +#endif /*SOCKETCAN_INTERFACE_FILTER_H*/ diff --git a/Devices/Libraries/Ros/ros_canopen/socketcan_interface/include/socketcan_interface/interface.h b/Devices/Libraries/Ros/ros_canopen/socketcan_interface/include/socketcan_interface/interface.h new file mode 100755 index 0000000..6ac79f1 --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/socketcan_interface/include/socketcan_interface/interface.h @@ -0,0 +1,230 @@ +#ifndef H_CAN_INTERFACE +#define H_CAN_INTERFACE + +#include +#include +#include + +#include + +#include "socketcan_interface/delegates.h" +#include "socketcan_interface/logging.h" +#include "socketcan_interface/settings.h" + +namespace can{ + +/** Header for CAN id an meta data*/ +struct Header{ + static const unsigned int ID_MASK = (1u << 29)-1; + static const unsigned int ERROR_MASK = (1u << 29); + static const unsigned int RTR_MASK = (1u << 30); + static const unsigned int EXTENDED_MASK = (1u << 31); + + unsigned int id:29; ///< CAN ID (11 or 29 bits valid, depending on is_extended member + unsigned int is_error:1; ///< marks an error frame (only used internally) + unsigned int is_rtr:1; ///< frame is a remote transfer request + unsigned int is_extended:1; ///< frame uses 29 bit CAN identifier + /** check if frame header is valid*/ + bool isValid() const{ + return id < (is_extended?(1<<29):(1<<11)); + } + unsigned int fullid() const { return id | (is_error?ERROR_MASK:0) | (is_rtr?RTR_MASK:0) | (is_extended?EXTENDED_MASK:0); } + unsigned int key() const { return is_error ? (ERROR_MASK) : fullid(); } + [[deprecated("use key() instead")]] explicit operator unsigned int() const { return key(); } + + /** constructor with default parameters + * @param[in] i: CAN id, defaults to 0 + * @param[in] extended: uses 29 bit identifier, defaults to false + * @param[in] rtr: is rtr frame, defaults to false + */ + + Header() + : id(0),is_error(0),is_rtr(0), is_extended(0) {} + + Header(unsigned int i, bool extended, bool rtr, bool error) + : id(i),is_error(error?1:0),is_rtr(rtr?1:0), is_extended(extended?1:0) {} +}; + + +struct MsgHeader : public Header{ + MsgHeader(unsigned int i=0, bool rtr = false) : Header(i, false, rtr, false) {} +}; +struct ExtendedHeader : public Header{ + ExtendedHeader(unsigned int i=0, bool rtr = false) : Header(i, true, rtr, false) {} +}; +struct ErrorHeader : public Header{ + ErrorHeader(unsigned int i=0) : Header(i, false, false, true) {} +}; + + + +/** representation of a CAN frame */ +struct Frame: public Header{ + using value_type = unsigned char; + std::array data; ///< array for 8 data bytes with bounds checking + unsigned char dlc; ///< len of data + + /** check if frame header and length are valid*/ + bool isValid() const{ + return (dlc <= 8) && Header::isValid(); + } + /** + * constructor with default parameters + * @param[in] i: CAN id, defaults to 0 + * @param[in] l: number of data bytes, defaults to 0 + * @param[in] extended: uses 29 bit identifier, defaults to false + * @param[in] rtr: is rtr frame, defaults to false + */ + Frame() : Header(), dlc(0) {} + Frame(const Header &h, unsigned char l = 0) : Header(h), dlc(l) {} + + value_type * c_array() { return data.data(); } + const value_type * c_array() const { return data.data(); } +}; + +/** extended error information */ +class State{ +public: + enum DriverState{ + closed, open, ready + } driver_state; + boost::system::error_code error_code; ///< device access error + unsigned int internal_error; ///< driver specific error + + State() : driver_state(closed), internal_error(0) {} + virtual bool isReady() const { return driver_state == ready; } + virtual ~State() {} +}; + +/** template for Listener interface */ +template class Listener{ + const T callable_; +public: + using Type = U; + using Callable = T; + using ListenerConstSharedPtr = std::shared_ptr; + + Listener(const T &callable):callable_(callable){ } + void operator()(const U & u) const { if(callable_) callable_(u); } + virtual ~Listener() {} +}; + +class StateInterface{ +public: + using StateFunc = std::function; + using StateDelegate [[deprecated("use StateFunc instead")]] = DelegateHelper; + using StateListener = Listener; + using StateListenerConstSharedPtr = StateListener::ListenerConstSharedPtr; + + /** + * acquire a listener for the specified delegate, that will get called for all state changes + * + * @param[in] delegate: delegate to be bound by the listener + * @return managed pointer to listener + */ + virtual StateListenerConstSharedPtr createStateListener(const StateFunc &delegate) = 0; + template inline StateListenerConstSharedPtr createStateListenerM(Instance inst, Callable callable) { + return this->createStateListener(std::bind(callable, inst, std::placeholders::_1)); + } + + virtual ~StateInterface() {} +}; +using StateInterfaceSharedPtr = std::shared_ptr; +using StateListenerConstSharedPtr = StateInterface::StateListenerConstSharedPtr; + +class CommInterface{ +public: + using FrameFunc = std::function; + using FrameDelegate [[deprecated("use FrameFunc instead")]] = DelegateHelper; + using FrameListener = Listener; + using FrameListenerConstSharedPtr = FrameListener::ListenerConstSharedPtr; + + /** + * enqueue frame for sending + * + * @param[in] msg: message to be enqueued + * @return true if frame was enqueued succesfully, otherwise false + */ + virtual bool send(const Frame & msg) = 0; + + /** + * acquire a listener for the specified delegate, that will get called for all messages + * + * @param[in] delegate: delegate to be bound by the listener + * @return managed pointer to listener + */ + virtual FrameListenerConstSharedPtr createMsgListener(const FrameFunc &delegate) = 0; + template inline FrameListenerConstSharedPtr createMsgListenerM(Instance inst, Callable callable) { + return this->createMsgListener(std::bind(callable, inst, std::placeholders::_1)); + } + + /** + * acquire a listener for the specified delegate, that will get called for messages with demanded ID + * + * @param[in] header: CAN header to restrict listener on + * @param[in] delegate: delegate to be bound listener + * @return managed pointer to listener + */ + virtual FrameListenerConstSharedPtr createMsgListener(const Frame::Header&, const FrameFunc &delegate) = 0; + template inline FrameListenerConstSharedPtr createMsgListenerM(const Frame::Header& header, Instance inst, Callable callable) { + return this->createMsgListener(header, std::bind(callable, inst, std::placeholders::_1)); + } + + virtual ~CommInterface() {} +}; +using CommInterfaceSharedPtr = std::shared_ptr; +using FrameListenerConstSharedPtr = CommInterface::FrameListenerConstSharedPtr; + +class DriverInterface : public CommInterface, public StateInterface { +public: + [[deprecated("provide settings explicitly")]] virtual bool init(const std::string &device, bool loopback) = 0; + + /** + * initialize interface + * + * @param[in] device: driver-specific device name/path + * @param[in] loopback: loop-back own messages + * @param[in] settings: driver-specific settings + * @return true if device was initialized succesfully, false otherwise + */ + virtual bool init(const std::string &device, bool loopback, SettingsConstSharedPtr settings) { + ROSCANOPEN_ERROR("socketcan_interface", "Driver does not support custom settings"); + #pragma GCC diagnostic push + #pragma GCC diagnostic ignored "-Wdeprecated-declarations" + return init(device, loopback); + #pragma GCC diagnostic pop + } + + /** + * Recover interface after errors and emergency stops + * + * @return true if device was recovered succesfully, false otherwise + */ + virtual bool recover() = 0; + + /** + * @return current state of driver + */ + virtual State getState() = 0; + + /** + * shutdown interface + * + * @return true if shutdown was succesful, false otherwise + */ + virtual void shutdown() = 0; + + virtual bool translateError(unsigned int internal_error, std::string & str) = 0; + + virtual bool doesLoopBack() const = 0; + + virtual void run() = 0; + + virtual ~DriverInterface() {} +}; +using DriverInterfaceSharedPtr = std::shared_ptr; + + +} // namespace can + +#endif diff --git a/Devices/Libraries/Ros/ros_canopen/socketcan_interface/include/socketcan_interface/logging.h b/Devices/Libraries/Ros/ros_canopen/socketcan_interface/include/socketcan_interface/logging.h new file mode 100755 index 0000000..caf8d2e --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/socketcan_interface/include/socketcan_interface/logging.h @@ -0,0 +1,17 @@ +#ifndef SOCKETCAN_INTERFACE_LOGGING_H +#define SOCKETCAN_INTERFACE_LOGGING_H + +#include +#include + +#define ROSCANOPEN_LOG(name, file, line, level, args) { std::stringstream sstr; sstr << name << ": " << args; console_bridge::getOutputHandler()->log(sstr.str(), level, file, line); } + +#define ROSCANOPEN_ERROR(name, args) ROSCANOPEN_LOG(name, __FILE__, __LINE__, console_bridge::CONSOLE_BRIDGE_LOG_ERROR, args) +#define ROSCANOPEN_INFO(name, args) ROSCANOPEN_LOG(name, __FILE__, __LINE__, console_bridge::CONSOLE_BRIDGE_LOG_INFO, args) +#define ROSCANOPEN_WARN(name, args) ROSCANOPEN_LOG(name, __FILE__, __LINE__, console_bridge::CONSOLE_BRIDGE_LOG_WARN, args) +#define ROSCANOPEN_DEBUG(name, args) ROSCANOPEN_LOG(name, __FILE__, __LINE__,console_bridge::CONSOLE_BRIDGE_LOG_DEBUG, args) + +// extra function to mark it as deprecated +inline __attribute__ ((deprecated("please use ROSCANOPEN_* macros"))) void roscanopen_log_deprecated(const std::string s, const char* f, int l) { console_bridge::getOutputHandler()->log(s, console_bridge::CONSOLE_BRIDGE_LOG_ERROR, f, l); } +#define LOG(args) { std::stringstream sstr; sstr << "LOG: " << args; roscanopen_log_deprecated(sstr.str(), __FILE__, __LINE__); } +#endif diff --git a/Devices/Libraries/Ros/ros_canopen/socketcan_interface/include/socketcan_interface/make_shared.h b/Devices/Libraries/Ros/ros_canopen/socketcan_interface/include/socketcan_interface/make_shared.h new file mode 100755 index 0000000..aedb0ba --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/socketcan_interface/include/socketcan_interface/make_shared.h @@ -0,0 +1,7 @@ +#ifndef SOCKETCAN_INTERFACE_MAKE_SHARED_H +#define SOCKETCAN_INTERFACE_MAKE_SHARED_H + +#include +#define ROSCANOPEN_MAKE_SHARED std::make_shared + +#endif // ! SOCKETCAN_INTERFACE_MAKE_SHARED_H diff --git a/Devices/Libraries/Ros/ros_canopen/socketcan_interface/include/socketcan_interface/reader.h b/Devices/Libraries/Ros/ros_canopen/socketcan_interface/include/socketcan_interface/reader.h new file mode 100755 index 0000000..8c6f2aa --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/socketcan_interface/include/socketcan_interface/reader.h @@ -0,0 +1,114 @@ +#ifndef H_CAN_BUFFERED_READER +#define H_CAN_BUFFERED_READER + +#include +#include + +#include +#include +#include + +namespace can{ + +class BufferedReader { + std::deque buffer_; + boost::mutex mutex_; + boost::condition_variable cond_; + CommInterface::FrameListenerConstSharedPtr listener_; + bool enabled_; + size_t max_len_; + + void trim(){ + if(max_len_ > 0){ + while(buffer_.size() > max_len_){ + ROSCANOPEN_ERROR("socketcan_interface", "buffer overflow, discarded oldest message " /*<< tostring(buffer_.front())*/); // enable message printing + buffer_.pop_front(); + } + } + } + void handleFrame(const can::Frame & msg){ + boost::mutex::scoped_lock lock(mutex_); + if(enabled_){ + buffer_.push_back(msg); + trim(); + cond_.notify_one(); + }else{ + ROSCANOPEN_WARN("socketcan_interface", "discarded message " /*<< tostring(msg)*/); // enable message printing + } + } +public: + class ScopedEnabler{ + BufferedReader &reader_; + bool before_; + public: + ScopedEnabler(BufferedReader &reader) : reader_(reader), before_(reader_.setEnabled(true)) {} + ~ScopedEnabler() { reader_.setEnabled(before_); } + }; + + BufferedReader() : enabled_(true), max_len_(0) {} + BufferedReader(bool enable, size_t max_len = 0) : enabled_(enable), max_len_(max_len) {} + + void flush(){ + boost::mutex::scoped_lock lock(mutex_); + buffer_.clear(); + } + void setMaxLen(size_t max_len){ + boost::mutex::scoped_lock lock(mutex_); + max_len_ = max_len; + trim(); + } + bool isEnabled(){ + boost::mutex::scoped_lock lock(mutex_); + return enabled_; + } + bool setEnabled(bool enabled){ + boost::mutex::scoped_lock lock(mutex_); + bool before = enabled_; + enabled_ = enabled; + return before; + } + void enable(){ + boost::mutex::scoped_lock lock(mutex_); + enabled_ = true; + } + + void disable(){ + boost::mutex::scoped_lock lock(mutex_); + enabled_ = false; + } + + void listen(CommInterfaceSharedPtr interface){ + boost::mutex::scoped_lock lock(mutex_); + listener_ = interface->createMsgListenerM(this, &BufferedReader::handleFrame); + buffer_.clear(); + } + void listen(CommInterfaceSharedPtr interface, const Frame::Header& h){ + boost::mutex::scoped_lock lock(mutex_); + listener_ = interface->createMsgListenerM(h, this, &BufferedReader::handleFrame); + buffer_.clear(); + } + + template bool read(can::Frame * msg, const DurationType &duration){ + return readUntil(msg, boost::chrono::high_resolution_clock::now() + duration); + } + bool readUntil(can::Frame * msg, boost::chrono::high_resolution_clock::time_point abs_time){ + boost::mutex::scoped_lock lock(mutex_); + + while(buffer_.empty() && cond_.wait_until(lock,abs_time) != boost::cv_status::timeout) + {} + + if(buffer_.empty()){ + return false; + } + + if(msg){ + *msg = buffer_.front(); + buffer_.pop_front(); + } + return true; + } + +}; + +} // namespace can +#endif diff --git a/Devices/Libraries/Ros/ros_canopen/socketcan_interface/include/socketcan_interface/settings.h b/Devices/Libraries/Ros/ros_canopen/socketcan_interface/include/socketcan_interface/settings.h new file mode 100755 index 0000000..10d674d --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/socketcan_interface/include/socketcan_interface/settings.h @@ -0,0 +1,59 @@ +#ifndef SOCKETCAN_INTERFACE_SETTINGS_H +#define SOCKETCAN_INTERFACE_SETTINGS_H + +#include +#include +#include + +#include + +namespace can { + class Settings + { + public: + template T get_optional(const std::string &n, const T& def) const { + std::string repr; + if(!getRepr(n, repr)){ + return def; + } + return boost::lexical_cast(repr); + } + template bool get(const std::string &n, T& val) const { + std::string repr; + if(!getRepr(n, repr)) return false; + val = boost::lexical_cast(repr); + return true; + } + virtual ~Settings() {} + private: + virtual bool getRepr(const std::string &n, std::string & repr) const = 0; + }; + using SettingsConstSharedPtr = std::shared_ptr; + using SettingsSharedPtr = std::shared_ptr; + + class NoSettings : public Settings { + public: + static SettingsConstSharedPtr create() { return SettingsConstSharedPtr(new NoSettings); } + private: + virtual bool getRepr(const std::string &n, std::string & repr) const { return false; } + }; + + class SettingsMap : public Settings { + std::map settings_; + virtual bool getRepr(const std::string &n, std::string & repr) const { + std::map::const_iterator it = settings_.find(n); + if (it == settings_.cend()) return false; + repr = it->second; + return true; + } + public: + template void set(const std::string &n, const T& val) { + settings_[n] = boost::lexical_cast(val); + } + static std::shared_ptr create() { return std::shared_ptr(new SettingsMap); } + }; + + +} // can + +#endif diff --git a/Devices/Libraries/Ros/ros_canopen/socketcan_interface/include/socketcan_interface/socketcan.h b/Devices/Libraries/Ros/ros_canopen/socketcan_interface/include/socketcan_interface/socketcan.h new file mode 100755 index 0000000..10dd786 --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/socketcan_interface/include/socketcan_interface/socketcan.h @@ -0,0 +1,276 @@ +#ifndef H_SOCKETCAN_DRIVER +#define H_SOCKETCAN_DRIVER + +#include +#include + +#include +#include +#include +#include + +#include +#include +#include + +#include + +#include +#include + +namespace can { + +class SocketCANInterface : public AsioDriver { + bool loopback_; + int sc_; + can_err_mask_t error_mask_, fatal_error_mask_; + + static can_err_mask_t parse_error_mask(SettingsConstSharedPtr settings, const std::string &entry, can_err_mask_t defaults) { + can_err_mask_t mask = 0; + + #define add_bit(e) mask |= (settings->get_optional(entry + "/" + #e, (defaults & e) != 0) ? e : 0) + add_bit(CAN_ERR_LOSTARB); + add_bit(CAN_ERR_CRTL); + add_bit(CAN_ERR_PROT); + add_bit(CAN_ERR_TRX); + add_bit(CAN_ERR_ACK); + add_bit(CAN_ERR_TX_TIMEOUT); + add_bit(CAN_ERR_BUSOFF); + add_bit(CAN_ERR_BUSERROR); + add_bit(CAN_ERR_RESTARTED); + #undef add_bit + + return mask; + } +public: + SocketCANInterface() + : loopback_(false), sc_(-1), error_mask_(0), fatal_error_mask_(0) + {} + + bool doesLoopBack() const override{ + return loopback_; + } + + can_err_mask_t getErrorMask() const { + return error_mask_; + } + + can_err_mask_t getFatalErrorMask() const { + return fatal_error_mask_; + } + [[deprecated("provide settings explicitly")]] virtual bool init(const std::string &device, bool loopback) override { + return SocketCANInterface::init(device, loopback, NoSettings::create()); + } + virtual bool init(const std::string &device, bool loopback, SettingsConstSharedPtr settings) override { + if (!settings) { + ROSCANOPEN_ERROR("socketcan_interface", "settings must not be a null pointer"); + return false; + } + const can_err_mask_t fatal_errors = ( CAN_ERR_TX_TIMEOUT /* TX timeout (by netdevice driver) */ + | CAN_ERR_BUSOFF /* bus off */ + | CAN_ERR_BUSERROR /* bus error (may flood!) */ + | CAN_ERR_RESTARTED /* controller restarted */ + ); + const can_err_mask_t report_errors = ( CAN_ERR_LOSTARB /* lost arbitration / data[0] */ + | CAN_ERR_CRTL /* controller problems / data[1] */ + | CAN_ERR_PROT /* protocol violations / data[2..3] */ + | CAN_ERR_TRX /* transceiver status / data[4] */ + | CAN_ERR_ACK /* received no ACK on transmission */ + ); + can_err_mask_t fatal_error_mask = parse_error_mask(settings, "fatal_error_mask", fatal_errors) | CAN_ERR_BUSOFF; + can_err_mask_t error_mask = parse_error_mask(settings, "error_mask", report_errors | fatal_error_mask) | fatal_error_mask; + return init(device, loopback, error_mask, fatal_error_mask); + } + + bool recover() override{ + if(!getState().isReady()){ + shutdown(); + return init(device_, loopback_, error_mask_, fatal_error_mask_); + } + return getState().isReady(); + } + bool translateError(unsigned int internal_error, std::string & str) override{ + + bool ret = false; + if(!internal_error){ + str = "OK"; + ret = true; + } + if( internal_error & CAN_ERR_TX_TIMEOUT){ + str += "TX timeout (by netdevice driver);"; + ret = true; + } + if( internal_error & CAN_ERR_LOSTARB){ + str += "lost arbitration;"; + ret = true; + } + if( internal_error & CAN_ERR_CRTL){ + str += "controller problems;"; + ret = true; + } + if( internal_error & CAN_ERR_PROT){ + str += "protocol violations;"; + ret = true; + } + if( internal_error & CAN_ERR_TRX){ + str += "transceiver status;"; + ret = true; + } + if( internal_error & CAN_ERR_BUSOFF){ + str += "bus off;"; + ret = true; + } + if( internal_error & CAN_ERR_RESTARTED){ + str += "controller restarted;"; + ret = true; + } + return ret; + } + int getInternalSocket() { + return sc_; + } +protected: + std::string device_; + can_frame frame_; + + bool init(const std::string &device, bool loopback, can_err_mask_t error_mask, can_err_mask_t fatal_error_mask) { + State s = getState(); + if(s.driver_state == State::closed){ + sc_ = 0; + device_ = device; + loopback_ = loopback; + error_mask_ = error_mask; + fatal_error_mask_ = fatal_error_mask; + + int sc = socket( PF_CAN, SOCK_RAW, CAN_RAW ); + if(sc < 0){ + setErrorCode(boost::system::error_code(sc,boost::system::system_category())); + return false; + } + + struct ifreq ifr; + strcpy(ifr.ifr_name, device_.c_str()); + int ret = ioctl(sc, SIOCGIFINDEX, &ifr); + + if(ret != 0){ + setErrorCode(boost::system::error_code(ret,boost::system::system_category())); + close(sc); + return false; + } + ret = setsockopt(sc, SOL_CAN_RAW, CAN_RAW_ERR_FILTER, + &error_mask, sizeof(error_mask)); + + if(ret != 0){ + setErrorCode(boost::system::error_code(ret,boost::system::system_category())); + close(sc); + return false; + } + + if(loopback_){ + int recv_own_msgs = 1; /* 0 = disabled (default), 1 = enabled */ + ret = setsockopt(sc, SOL_CAN_RAW, CAN_RAW_RECV_OWN_MSGS, &recv_own_msgs, sizeof(recv_own_msgs)); + + if(ret != 0){ + setErrorCode(boost::system::error_code(ret,boost::system::system_category())); + close(sc); + return false; + } + } + + struct sockaddr_can addr = {0}; + addr.can_family = AF_CAN; + addr.can_ifindex = ifr.ifr_ifindex; + ret = bind( sc, (struct sockaddr*)&addr, sizeof(addr) ); + + if(ret != 0){ + setErrorCode(boost::system::error_code(ret,boost::system::system_category())); + close(sc); + return false; + } + + boost::system::error_code ec; + socket_.assign(sc,ec); + + setErrorCode(ec); + + if(ec){ + close(sc); + return false; + } + setInternalError(0); + setDriverState(State::open); + sc_ = sc; + return true; + } + return getState().isReady(); + } + + void triggerReadSome() override{ + boost::mutex::scoped_lock lock(send_mutex_); + socket_.async_read_some(boost::asio::buffer(&frame_, sizeof(frame_)), boost::bind( &SocketCANInterface::readFrame,this, boost::asio::placeholders::error)); + } + + bool enqueue(const Frame & msg) override{ + boost::mutex::scoped_lock lock(send_mutex_); //TODO: timed try lock + + can_frame frame = {0}; + frame.can_id = msg.id | (msg.is_extended?CAN_EFF_FLAG:0) | (msg.is_rtr?CAN_RTR_FLAG:0);; + frame.can_dlc = msg.dlc; + + + for(int i=0; i < frame.can_dlc;++i) + frame.data[i] = msg.data[i]; + + boost::system::error_code ec; + boost::asio::write(socket_, boost::asio::buffer(&frame, sizeof(frame)),boost::asio::transfer_all(), ec); + if(ec){ + ROSCANOPEN_ERROR("socketcan_interface", "FAILED " << ec); + setErrorCode(ec); + setNotReady(); + return false; + } + + return true; + } + + void readFrame(const boost::system::error_code& error){ + if(!error){ + input_.dlc = frame_.can_dlc; + for(int i=0;i; +using SocketCANInterfaceSharedPtr = std::shared_ptr; + +template class ThreadedInterface; +using ThreadedSocketCANInterface = ThreadedInterface; +using ThreadedSocketCANInterfaceSharedPtr = std::shared_ptr; + + +} // namespace can +#endif diff --git a/Devices/Libraries/Ros/ros_canopen/socketcan_interface/include/socketcan_interface/string.h b/Devices/Libraries/Ros/ros_canopen/socketcan_interface/include/socketcan_interface/string.h new file mode 100755 index 0000000..390e896 --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/socketcan_interface/include/socketcan_interface/string.h @@ -0,0 +1,47 @@ +#ifndef SOCKETCAN_INTERFACE_STRING_H +#define SOCKETCAN_INTERFACE_STRING_H + +#include "interface.h" +#include "filter.h" +#include + +namespace can { + +bool hex2dec(uint8_t& d, const char& h); + +bool hex2buffer(std::string& out, const std::string& in_raw, bool pad); + +bool dec2hex(char& h, const uint8_t& d, bool lc); + +std::string byte2hex(const uint8_t& d, bool pad, bool lc); + + +std::string buffer2hex(const std::string& in, bool lc); + +std::string tostring(const Header& h, bool lc); + +Header toheader(const std::string& s); + +std::string tostring(const Frame& f, bool lc); + +Frame toframe(const std::string& s); + +template FrameFilterSharedPtr tofilter(const T &ct); +template<> FrameFilterSharedPtr tofilter(const std::string &s); +template<> FrameFilterSharedPtr tofilter(const uint32_t &id); + +FrameFilterSharedPtr tofilter(const char* s); + +template FilteredFrameListener::FilterVector tofilters(const T& v) { + FilteredFrameListener::FilterVector filters; + for(size_t i = 0; i < static_cast(v.size()); ++i){ + filters.push_back(tofilter(v[i])); + } + return filters; +} + +std::ostream& operator <<(std::ostream& stream, const Header& h); +std::ostream& operator <<(std::ostream& stream, const Frame& f); + +} +#endif diff --git a/Devices/Libraries/Ros/ros_canopen/socketcan_interface/include/socketcan_interface/threading.h b/Devices/Libraries/Ros/ros_canopen/socketcan_interface/include/socketcan_interface/threading.h new file mode 100755 index 0000000..62eb45c --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/socketcan_interface/include/socketcan_interface/threading.h @@ -0,0 +1,92 @@ +#ifndef H_CAN_THREADING_BASE +#define H_CAN_THREADING_BASE + +#include +#include + +namespace can{ + +class StateWaiter{ + boost::mutex mutex_; + boost::condition_variable cond_; + can::StateInterface::StateListenerConstSharedPtr state_listener_; + can::State state_; + void updateState(const can::State &s){ + boost::mutex::scoped_lock lock(mutex_); + state_ = s; + lock.unlock(); + cond_.notify_all(); + } +public: + template StateWaiter(InterfaceType *interface){ + state_ = interface->getState(); + state_listener_ = interface->createStateListener(std::bind(&StateWaiter::updateState, this, std::placeholders::_1)); + } + template bool wait(const can::State::DriverState &s, const DurationType &duration){ + boost::mutex::scoped_lock cond_lock(mutex_); + boost::system_time abs_time = boost::get_system_time() + duration; + while(s != state_.driver_state) + { + if(!cond_.timed_wait(cond_lock,abs_time)) + { + return false; + } + } + return true; + } +}; + +template class ThreadedInterface : public WrappedInterface{ + std::shared_ptr thread_; + void run_thread(){ + WrappedInterface::run(); + } + void shutdown_internal(){ + if(thread_){ + thread_->interrupt(); + thread_->join(); + thread_.reset(); + } + } +public: + [[deprecated("provide settings explicitly")]] bool init(const std::string &device, bool loopback) override { + #pragma GCC diagnostic push + #pragma GCC diagnostic ignored "-Wdeprecated-declarations" + if(!thread_ && WrappedInterface::init(device, loopback)){ + StateWaiter waiter(this); + thread_.reset(new boost::thread(&ThreadedInterface::run_thread, this)); + return waiter.wait(can::State::ready, boost::posix_time::seconds(1)); + } + return WrappedInterface::getState().isReady(); + #pragma GCC diagnostic pop + } + bool init(const std::string &device, bool loopback, SettingsConstSharedPtr settings) override { + if(!thread_ && WrappedInterface::init(device, loopback, settings)){ + StateWaiter waiter(this); + thread_.reset(new boost::thread(&ThreadedInterface::run_thread, this)); + return waiter.wait(can::State::ready, boost::posix_time::seconds(1)); + } + return WrappedInterface::getState().isReady(); + } + + void shutdown() override{ + WrappedInterface::shutdown(); + shutdown_internal(); + } + void join(){ + if(thread_){ + thread_->join(); + } + } + virtual ~ThreadedInterface() { + shutdown_internal(); + } + ThreadedInterface(): WrappedInterface() {} + template ThreadedInterface(const T1 &t1): WrappedInterface(t1) {} + template ThreadedInterface(const T1 &t1, const T2 &t2): WrappedInterface(t1, t2) {} + +}; + + +} // namespace can +#endif diff --git a/Devices/Libraries/Ros/ros_canopen/socketcan_interface/include/socketcan_interface/xmlrpc_settings.h b/Devices/Libraries/Ros/ros_canopen/socketcan_interface/include/socketcan_interface/xmlrpc_settings.h new file mode 100755 index 0000000..f76e112 --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/socketcan_interface/include/socketcan_interface/xmlrpc_settings.h @@ -0,0 +1,44 @@ +#ifndef SOCKETCAN_INTERFACE_XMLRPC_SETTINGS_H +#define SOCKETCAN_INTERFACE_XMLRPC_SETTINGS_H +#include + +#include +#include "xmlrpcpp/XmlRpcValue.h" +#include +#include + +class XmlRpcSettings : public can::Settings { +public: + XmlRpcSettings() {} + XmlRpcSettings(const XmlRpc::XmlRpcValue &v) : value_(v) {} + XmlRpcSettings& operator=(const XmlRpc::XmlRpcValue &v) { value_ = v; return *this; } + template static can::SettingsConstSharedPtr create(T nh, const std::string &ns="/") { + std::shared_ptr settings(new XmlRpcSettings); + nh.getParam(ns, settings->value_); + return settings; + } +private: + virtual bool getRepr(const std::string &name, std::string & repr) const { + const XmlRpc::XmlRpcValue *value = &value_; + + std::string n = name; + size_t delim_pos; + while (value->getType() == XmlRpc::XmlRpcValue::TypeStruct && (delim_pos = n.find('/')) != std::string::npos){ + std::string segment = n.substr(0, delim_pos); + if (!value->hasMember(segment)) return false; + value = &((*value)[segment]); + n.erase(0, delim_pos+1); + } + if(value->hasMember(n)){ + std::stringstream sstr; + sstr << (*value)[n]; + repr = sstr.str(); + return true; + } + return false; + } + XmlRpc::XmlRpcValue value_; + +}; + +#endif diff --git a/Devices/Libraries/Ros/ros_canopen/socketcan_interface/package.xml b/Devices/Libraries/Ros/ros_canopen/socketcan_interface/package.xml new file mode 100755 index 0000000..2ff165d --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/socketcan_interface/package.xml @@ -0,0 +1,32 @@ + + + socketcan_interface + 0.8.5 + Generic CAN interface description with helpers for filtering and driver implementation. Further a socketcan implementation based on boost::asio is included. + + Mathias Lüdtke + Mathias Lüdtke + + LGPLv3 + + http://wiki.ros.org/socketcan_interface + https://github.com/ros-industrial/ros_canopen + https://github.com/ros-industrial/ros_canopen/issues + + catkin + + libboost-dev + libboost-chrono-dev + libboost-system-dev + libboost-thread-dev + class_loader + libconsole-bridge-dev + linux-kernel-headers + + rosunit + xmlrpcpp + + + + + diff --git a/Devices/Libraries/Ros/ros_canopen/socketcan_interface/socketcan_interface_plugin.xml b/Devices/Libraries/Ros/ros_canopen/socketcan_interface/socketcan_interface_plugin.xml new file mode 100755 index 0000000..98f79aa --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/socketcan_interface/socketcan_interface_plugin.xml @@ -0,0 +1,5 @@ + + + SocketCAN inteface plugin. + + \ No newline at end of file diff --git a/Devices/Libraries/Ros/ros_canopen/socketcan_interface/src/canbcm.cpp b/Devices/Libraries/Ros/ros_canopen/socketcan_interface/src/canbcm.cpp new file mode 100755 index 0000000..2afe0a0 --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/socketcan_interface/src/canbcm.cpp @@ -0,0 +1,39 @@ +#include +#include + +using namespace can; + +#include + +int main(int argc, char *argv[]){ + BCMsocket bcm; + + int extra_frames = argc - 4; + + if(extra_frames < 0){ + std::cout << "usage: "<< argv[0] << " DEVICE PERIOD HEADER#DATA [DATA*]" << std::endl; + return 1; + } + + if(!bcm.init(argv[1])){ + return 2; + } + + int num_frames = extra_frames+1; + Frame *frames = new Frame[num_frames]; + Header header = frames[0] = toframe(argv[3]); + + if(extra_frames > 0){ + for(int i=0; i< extra_frames; ++i){ + frames[1+i] = toframe(tostring(header,true)+"#"+argv[4+i]); + } + } + for(int i = 0; i < num_frames; ++i){ + std::cout << frames[i] << std::endl; + } + if(bcm.startTX(boost::chrono::duration(atof(argv[2])), header, num_frames, frames)){ + pause(); + return 0; + } + return 4; +} \ No newline at end of file diff --git a/Devices/Libraries/Ros/ros_canopen/socketcan_interface/src/candump.cpp b/Devices/Libraries/Ros/ros_canopen/socketcan_interface/src/candump.cpp new file mode 100755 index 0000000..3df34cd --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/socketcan_interface/src/candump.cpp @@ -0,0 +1,90 @@ +#include +#include +#include + +#include +#include +#include + +using namespace can; + +#include + +void print_frame(const Frame &f){ + + if(f.is_error){ + std::cout << "E " << std::hex << f.id << std::dec; + }else if(f.is_extended){ + std::cout << "e " << std::hex << f.id << std::dec; + }else{ + std::cout << "s " << std::hex << f.id << std::dec; + } + + std::cout << "\t"; + + if(f.is_rtr){ + std::cout << "r"; + }else{ + std::cout << (int) f.dlc << std::hex; + + for(int i=0; i < f.dlc; ++i){ + std::cout << std::hex << " " << (int) f.data[i]; + } + } + + std::cout << std::dec << std::endl; +} + + +std::shared_ptr g_loader; +DriverInterfaceSharedPtr g_driver; + +void print_state(const State & s){ + std::string err; + g_driver->translateError(s.internal_error,err); + std::cout << "STATE: driver_state=" << s.driver_state << " internal_error=" << s.internal_error << "('" << err << "') asio: " << s.error_code << std::endl; +} + + +int main(int argc, char *argv[]){ + + if(argc != 2 && argc != 4){ + std::cout << "usage: "<< argv[0] << " DEVICE [PLUGIN_PATH PLUGIN_NAME]" << std::endl; + return 1; + } + + if(argc == 4 ){ + try + { + g_loader = std::make_shared(argv[2]); + g_driver = g_loader->createUniqueInstance(argv[3]); + } + + catch(std::exception& ex) + { + std::cerr << boost::diagnostic_information(ex) << std::endl;; + return 1; + } + }else{ + g_driver = std::make_shared(); + } + + + + FrameListenerConstSharedPtr frame_printer = g_driver->createMsgListener(print_frame); + StateListenerConstSharedPtr error_printer = g_driver->createStateListener(print_state); + + if(!g_driver->init(argv[1], false, can::NoSettings::create())){ + print_state(g_driver->getState()); + return 1; + } + + g_driver->run(); + + g_driver->shutdown(); + g_driver.reset(); + g_loader.reset(); + + return 0; + +} diff --git a/Devices/Libraries/Ros/ros_canopen/socketcan_interface/src/socketcan_interface_plugin.cpp b/Devices/Libraries/Ros/ros_canopen/socketcan_interface/src/socketcan_interface_plugin.cpp new file mode 100755 index 0000000..b4fb432 --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/socketcan_interface/src/socketcan_interface_plugin.cpp @@ -0,0 +1,4 @@ +#include +#include + +CLASS_LOADER_REGISTER_CLASS(can::SocketCANInterface, can::DriverInterface); diff --git a/Devices/Libraries/Ros/ros_canopen/socketcan_interface/src/string.cpp b/Devices/Libraries/Ros/ros_canopen/socketcan_interface/src/string.cpp new file mode 100755 index 0000000..5af94cb --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/socketcan_interface/src/string.cpp @@ -0,0 +1,182 @@ +#include +#include + +namespace can { + +bool hex2dec(uint8_t& d, const char& h) { + if ('0' <= h && h <= '9') + d = h - '0'; + else if ('a' <= h && h <= 'f') + d = h - 'a' + 10; + else if ('A' <= h && h <= 'F') + d = h - 'A' + 10; + else + return false; + + return true; +} + +bool hex2buffer(std::string& out, const std::string& in_raw, bool pad) { + std::string in(in_raw); + if ((in.size() % 2) != 0) { + if (pad) + in.insert(0, "0"); + else + return false; + } + out.resize(in.size() >> 1); + for (size_t i = 0; i < out.size(); ++i) { + uint8_t hi, lo; + if (!hex2dec(hi, in[i << 1]) || !hex2dec(lo, in[(i << 1) + 1])) + return false; + + out[i] = (hi << 4) | lo; + } + return true; +} + +bool dec2hex(char& h, const uint8_t& d, bool lc) { + if (d < 10) { + h = '0' + d; + } else if (d < 16 && lc) { + h = 'a' + (d - 10); + } else if (d < 16 && !lc) { + h = 'A' + (d - 10); + } else { + h='?'; + return false; + } + return true; +} + +std::string byte2hex(const uint8_t& d, bool pad, bool lc) { + uint8_t hi = d >> 4; + char c=0; + std::string s; + if (hi || pad) { + dec2hex(c, hi, lc); + s += c; + } + dec2hex(c, d & 0xf, lc); + s += c; + return s; +} + +std::string buffer2hex(const std::string& in, bool lc) { + std::string s; + s.reserve(in.size() * 2); + for (size_t i = 0; i < in.size(); ++i) { + std::string b = byte2hex(in[i], true, lc); + if (b.empty()) + return b; + + s += b; + } + return s; +} + +std::string tostring(const Header& h, bool lc) { + std::string s; + s.reserve(8); + std::stringstream buf; + buf << std::hex; + if (lc) + buf << std::nouppercase; + else + buf << std::uppercase; + + if (h.is_extended) + buf << std::setfill('0') << std::setw(8); + + buf << (h.fullid() & ~Header::EXTENDED_MASK); + return buf.str(); +} + +uint32_t tohex(const std::string& s) { + unsigned int h = 0; + std::stringstream stream; + stream << std::hex << s; + stream >> h; + return h; +} + +Header toheader(const std::string& s) { + unsigned int h = tohex(s); + unsigned int id = h & Header::ID_MASK; + return Header(id, h & Header::EXTENDED_MASK || (s.size() == 8 && id >= (1<<11)), + h & Header::RTR_MASK, h & Header::ERROR_MASK); +} + +std::string tostring(const Frame& f, bool lc) { + std::string s; + s.resize(f.dlc); + for (uint8_t i = 0; i < f.dlc; ++i) { + s[i] = f.data[i]; + } + return tostring((const Header&) (f), lc) + '#' + buffer2hex(s, lc); +} + +Frame toframe(const std::string& s) { + size_t sep = s.find('#'); + if (sep == std::string::npos) + return MsgHeader(0xfff); + + Header header = toheader(s.substr(0, sep)); + Frame frame(header); + std::string buffer; + if (header.isValid() && hex2buffer(buffer, s.substr(sep + 1), false)) { + if (buffer.size() > 8) + return MsgHeader(0xfff); + + for (size_t i = 0; i < buffer.size(); ++i) { + frame.data[i] = buffer[i]; + } + frame.dlc = buffer.size(); + } + return frame; +} + +template<> FrameFilterSharedPtr tofilter(const std::string &s){ + FrameFilter * filter = 0; + size_t delim = s.find_first_of(":~-_"); + + uint32_t second = FrameMaskFilter::MASK_RELAXED; + bool invert = false; + char type = ':'; + + if(delim != std::string::npos) { + type = s.at(delim); + second = tohex(s.substr(delim +1)); + } + uint32_t first = toheader(s.substr(0, delim)).fullid(); + switch (type) { + case '~': + invert = true; + case ':': + filter = new FrameMaskFilter(first, second, invert); + break; + case '_': + invert = true; + case '-': + filter = new FrameRangeFilter(first, second, invert); + break; + } + return FrameFilterSharedPtr(filter); +} +template<> FrameFilterSharedPtr tofilter(const uint32_t &id){ + return FrameFilterSharedPtr(new FrameMaskFilter(id)); +} + +FrameFilterSharedPtr tofilter(const char* s){ + return tofilter(s); +} + +std::ostream& operator <<(std::ostream& stream, const Header& h) { + return stream << can::tostring(h, true); +} + +std::ostream& operator <<(std::ostream& stream, const Frame& f) { + return stream << can::tostring(f, true); +} + +} diff --git a/Devices/Libraries/Ros/ros_canopen/socketcan_interface/test/test_delegates.cpp b/Devices/Libraries/Ros/ros_canopen/socketcan_interface/test/test_delegates.cpp new file mode 100755 index 0000000..7d1e73f --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/socketcan_interface/test/test_delegates.cpp @@ -0,0 +1,102 @@ +// Bring in my package's API, which is what I'm testing +#include +#include +#include + +// Bring in gtest +#include + +class Receiver { +public: + std::list responses; + void handle(const can::Frame &f){ + responses.push_back(can::tostring(f, true)); + } + Receiver() = default; + ~Receiver() = default; + +private: + Receiver(const Receiver&) = delete; + Receiver& operator=(const Receiver&) = delete; +}; + +Receiver g_r2; + +void fill_r2(const can::Frame &f){ + g_r2.handle(f); +} + +void fill_any(Receiver &r, const can::Frame &f){ + r.handle(f); +} + +TEST(DelegatesTest, testFrameDelegate) +{ + + can::DummyBus bus("testFrameDelegate"); + can::ThreadedDummyInterface dummy; + dummy.init(bus.name, true, can::NoSettings::create()); + Receiver r1, r3, r4, r5; + boost::shared_ptr r6(new Receiver()); + std::shared_ptr r7(new Receiver()); + + can::FrameListenerConstSharedPtr l1 = dummy.createMsgListener(can::CommInterface::FrameDelegate(&r1, &Receiver::handle)); + can::FrameListenerConstSharedPtr l2 = dummy.createMsgListener(can::CommInterface::FrameDelegate(fill_r2)); + can::FrameListenerConstSharedPtr l3 = dummy.createMsgListener(can::CommInterface::FrameDelegate(std::bind(fill_any, std::ref(r3), std::placeholders::_1))); + can::FrameListenerConstSharedPtr l4 = dummy.createMsgListener(std::bind(fill_any, std::ref(r4), std::placeholders::_1)); + can::FrameListenerConstSharedPtr l5 = dummy.createMsgListener(boost::bind(fill_any, boost::ref(r5), boost::placeholders::_1)); + can::FrameListenerConstSharedPtr l6 = dummy.createMsgListener(can::CommInterface::FrameDelegate(r6, &Receiver::handle)); + can::FrameListenerConstSharedPtr l7 = dummy.createMsgListener(can::CommInterface::FrameDelegate(r7, &Receiver::handle)); + + std::list expected; + dummy.send(can::toframe("0#8200")); + dummy.flush(); + expected.push_back("0#8200"); + + EXPECT_EQ(expected, r1.responses); + EXPECT_EQ(expected, g_r2.responses); + EXPECT_EQ(expected, r3.responses); + EXPECT_EQ(expected, r4.responses); + EXPECT_EQ(expected, r5.responses); + EXPECT_EQ(expected, r6->responses); + EXPECT_EQ(expected, r7->responses); +} + +class BoolTest { + bool ret_; +public: + bool test_bool() { return ret_; } + BoolTest(bool ret) : ret_(ret) {} +public: + BoolTest(const BoolTest&) = delete; + BoolTest& operator=(const BoolTest&) = delete; +}; + +TEST(DelegatesTest, testBoolFunc) +{ + + using BoolFunc = std::function; + using BoolDelegate = can::DelegateHelper; + + BoolDelegate d1([]() { return false; }); + BoolDelegate d2([]() { return true; }); + + EXPECT_FALSE(d1()); + EXPECT_TRUE(d2()); + + BoolTest b1(false); + BoolTest b2(true); + + BoolDelegate d3(&b1, &BoolTest::test_bool); + BoolDelegate d4(&b2, &BoolTest::test_bool); + EXPECT_FALSE(d3()); + EXPECT_TRUE(d4()); + +} + + +// Run all the tests that were declared with TEST() +int main(int argc, char **argv){ +testing::InitGoogleTest(&argc, argv); +return RUN_ALL_TESTS(); +} diff --git a/Devices/Libraries/Ros/ros_canopen/socketcan_interface/test/test_dispatcher.cpp b/Devices/Libraries/Ros/ros_canopen/socketcan_interface/test/test_dispatcher.cpp new file mode 100755 index 0000000..aaa6684 --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/socketcan_interface/test/test_dispatcher.cpp @@ -0,0 +1,84 @@ +// Bring in my package's API, which is what I'm testing +#include + +// Bring in gtest +#include + +class Counter { +public: + size_t counter_; + Counter() : counter_(0) {} + void count(const can::Frame &msg) { + ++counter_; + } +}; + +TEST(DispatcherTest, testFilteredDispatcher) +{ + can::FilteredDispatcher dispatcher; + Counter counter1; + Counter counter2; + std::vector listeners; + const size_t max_id = (1<<11); + for(size_t i=0; i < max_id; i+=2) { + listeners.push_back(dispatcher.createListener(can::MsgHeader(i), can::CommInterface::FrameDelegate(&counter1, &Counter::count))); + listeners.push_back(dispatcher.createListener(can::MsgHeader(i+1), can::CommInterface::FrameDelegate(&counter2, &Counter::count))); + } + + boost::chrono::steady_clock::time_point start = boost::chrono::steady_clock::now(); + const size_t num = 1000 * max_id; + for(size_t i=0; i < num; ++i) { + dispatcher.dispatch(can::Frame(can::MsgHeader(i%max_id))); + } + boost::chrono::steady_clock::time_point now = boost::chrono::steady_clock::now(); + double diff = boost::chrono::duration_cast >(now-start).count(); + + EXPECT_EQ(num, counter1.counter_+ counter2.counter_); + EXPECT_EQ(counter1.counter_, counter2.counter_); + std::cout << std::fixed << diff << "\t" << num << "\t" << num / diff << std::endl; + +} + +TEST(DispatcherTest, testSimpleDispatcher) +{ + can::SimpleDispatcher dispatcher; + Counter counter; + can::CommInterface::FrameListenerConstSharedPtr listener = dispatcher.createListener(can::CommInterface::FrameDelegate(&counter, &Counter::count)); + + boost::chrono::steady_clock::time_point start = boost::chrono::steady_clock::now(); + const size_t max_id = (1<<11); + const size_t num = 1000*max_id; + for(size_t i=0; i < num; ++i) { + dispatcher.dispatch(can::Frame(can::MsgHeader(i%max_id))); + } + boost::chrono::steady_clock::time_point now = boost::chrono::steady_clock::now(); + double diff = boost::chrono::duration_cast >(now-start).count(); + + EXPECT_EQ(num, counter.counter_); + std::cout << std::fixed << diff << "\t" << num << "\t" << num / diff << std::endl; +} + +TEST(DispatcherTest, testDelegateOnly) +{ + Counter counter; + can::CommInterface::FrameDelegate delegate(&counter, &Counter::count); + + boost::chrono::steady_clock::time_point start = boost::chrono::steady_clock::now(); + const size_t max_id = (1<<11); + const size_t num = 10000*max_id; + for(size_t i=0; i < num; ++i) { + delegate(can::Frame(can::MsgHeader(i%max_id))); + } + boost::chrono::steady_clock::time_point now = boost::chrono::steady_clock::now(); + double diff = boost::chrono::duration_cast >(now-start).count(); + + EXPECT_EQ(num, counter.counter_); + std::cout << std::fixed << diff << "\t" << num << "\t" << num / diff << std::endl; + +} + +// Run all the tests that were declared with TEST() +int main(int argc, char **argv){ +testing::InitGoogleTest(&argc, argv); +return RUN_ALL_TESTS(); +} diff --git a/Devices/Libraries/Ros/ros_canopen/socketcan_interface/test/test_dummy_interface.cpp b/Devices/Libraries/Ros/ros_canopen/socketcan_interface/test/test_dummy_interface.cpp new file mode 100755 index 0000000..f90233a --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/socketcan_interface/test/test_dummy_interface.cpp @@ -0,0 +1,42 @@ +// Bring in my package's API, which is what I'm testing +#include +#include + +// Bring in gtest +#include + +// Declare a test +TEST(DummyInterfaceTest, testCase1) +{ + can::DummyBus bus("testCase1"); + can::ThreadedDummyInterface dummy; + dummy.init(bus.name, true, can::NoSettings::create()); + + can::DummyReplay replay; + replay.add("0#8200", {"701#00", "701#04"}); + replay.init(bus); + + std::list expected{"0#8200", "701#00", "701#04"}; + std::list responses; + + auto listener = dummy.createMsgListener([&responses](auto& f) { + responses.push_back(can::tostring(f, true)); + }); + + EXPECT_FALSE(replay.done()); + + dummy.send(can::toframe("0#8200")); + + replay.flush(); + dummy.flush(); + + EXPECT_EQ(expected, responses); + EXPECT_TRUE(replay.done()); +} + + +// Run all the tests that were declared with TEST() +int main(int argc, char **argv){ +testing::InitGoogleTest(&argc, argv); +return RUN_ALL_TESTS(); +} diff --git a/Devices/Libraries/Ros/ros_canopen/socketcan_interface/test/test_filter.cpp b/Devices/Libraries/Ros/ros_canopen/socketcan_interface/test/test_filter.cpp new file mode 100755 index 0000000..3a94236 --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/socketcan_interface/test/test_filter.cpp @@ -0,0 +1,115 @@ +// Bring in my package's API, which is what I'm testing +#include + + +#include +#include + +// Bring in gtest +#include + + +TEST(FilterTest, simpleMask) +{ + const std::string msg1("123#"); + const std::string msg2("124#"); + + can::FrameFilterSharedPtr f1 = can::tofilter("123"); + + EXPECT_TRUE(f1->pass(can::toframe(msg1))); + EXPECT_FALSE(f1->pass(can::toframe(msg2))); +} + +TEST(FilterTest, maskTests) +{ + const std::string msg1("123#"); + const std::string msg2("124#"); + const std::string msg3("122#"); + + can::FrameFilterSharedPtr f1 = can::tofilter("123:123"); + can::FrameFilterSharedPtr f2 = can::tofilter("123:ffe"); + can::FrameFilterSharedPtr f3 = can::tofilter("123~123"); + + EXPECT_TRUE(f1->pass(can::toframe(msg1))); + EXPECT_FALSE(f1->pass(can::toframe(msg2))); + EXPECT_FALSE(f1->pass(can::toframe(msg3))); + + + EXPECT_TRUE(f2->pass(can::toframe(msg1))); + EXPECT_FALSE(f2->pass(can::toframe(msg2))); + EXPECT_TRUE(f2->pass(can::toframe(msg3))); + + EXPECT_FALSE(f3->pass(can::toframe(msg1))); + EXPECT_TRUE(f3->pass(can::toframe(msg2))); + EXPECT_TRUE(f3->pass(can::toframe(msg3))); + +} + +TEST(FilterTest, rangeTest) +{ + const std::string msg1("120#"); + const std::string msg2("125#"); + const std::string msg3("130#"); + + can::FrameFilterSharedPtr f1 = can::tofilter("120-120"); + can::FrameFilterSharedPtr f2 = can::tofilter("120_120"); + can::FrameFilterSharedPtr f3 = can::tofilter("120-125"); + + EXPECT_TRUE(f1->pass(can::toframe(msg1))); + EXPECT_FALSE(f1->pass(can::toframe(msg2))); + EXPECT_FALSE(f1->pass(can::toframe(msg3))); + + EXPECT_FALSE(f2->pass(can::toframe(msg1))); + EXPECT_TRUE(f2->pass(can::toframe(msg2))); + EXPECT_TRUE(f2->pass(can::toframe(msg3))); + + EXPECT_TRUE(f3->pass(can::toframe(msg1))); + EXPECT_TRUE(f3->pass(can::toframe(msg2))); + EXPECT_FALSE(f3->pass(can::toframe(msg3))); + +} + +class Counter { +public: + size_t count_; + Counter(): count_(0) {} + void count(const can::Frame &frame) { + ++count_; + } +}; + +TEST(FilterTest, listenerTest) +{ + + Counter counter; + can::DummyBus bus("listenerTest"); + can::ThreadedDummyInterfaceSharedPtr dummy = std::make_shared(); + dummy->init(bus.name, true, can::NoSettings::create()); + + + can::FilteredFrameListener::FilterVector filters; + filters.push_back(can::tofilter("123:FFE")); + + can::FrameListenerConstSharedPtr listener(new can::FilteredFrameListener(dummy,std::bind(&Counter::count, std::ref(counter), std::placeholders::_1), filters)); + + can::Frame f1 = can::toframe("123#"); + can::Frame f2 = can::toframe("124#"); + can::Frame f3 = can::toframe("122#"); + + dummy->send(f1); + dummy->flush(); + EXPECT_EQ(1, counter.count_); + dummy->send(f2); + dummy->flush(); + EXPECT_EQ(1, counter.count_); + dummy->send(f3); + dummy->flush(); + EXPECT_EQ(2, counter.count_); + +} + +// Run all the tests that were declared with TEST() +int main(int argc, char **argv){ +testing::InitGoogleTest(&argc, argv); +return RUN_ALL_TESTS(); +} diff --git a/Devices/Libraries/Ros/ros_canopen/socketcan_interface/test/test_settings.cpp b/Devices/Libraries/Ros/ros_canopen/socketcan_interface/test/test_settings.cpp new file mode 100755 index 0000000..1ebae09 --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/socketcan_interface/test/test_settings.cpp @@ -0,0 +1,91 @@ +// Bring in my package's API, which is what I'm testing +#include +#include +#include + +// Bring in gtest +#include + +TEST(SettingTest, socketcan_masks) +{ + const can_err_mask_t fatal_errors = ( CAN_ERR_TX_TIMEOUT /* TX timeout (by netdevice driver) */ + | CAN_ERR_BUSOFF /* bus off */ + | CAN_ERR_BUSERROR /* bus error (may flood!) */ + | CAN_ERR_RESTARTED /* controller restarted */ + ); + const can_err_mask_t report_errors = ( CAN_ERR_LOSTARB /* lost arbitration / data[0] */ + | CAN_ERR_CRTL /* controller problems / data[1] */ + | CAN_ERR_PROT /* protocol violations / data[2..3] */ + | CAN_ERR_TRX /* transceiver status / data[4] */ + | CAN_ERR_ACK /* received no ACK on transmission */ + ); + can::SocketCANInterface sci; + + // defaults + sci.init("None", false, can::NoSettings::create()); + EXPECT_EQ(sci.getErrorMask(), fatal_errors | report_errors); + EXPECT_EQ(sci.getFatalErrorMask(), fatal_errors); + + // remove report-only flag + auto m1 = can::SettingsMap::create(); + m1->set("error_mask/CAN_ERR_LOSTARB", false); + sci.init("None", false, m1); + EXPECT_EQ(sci.getErrorMask(), (fatal_errors | report_errors) & (~CAN_ERR_LOSTARB)); + EXPECT_EQ(sci.getFatalErrorMask(), fatal_errors); + + // cannot remove fatal flag from report only + auto m2 = can::SettingsMap::create(); + m2->set("error_mask/CAN_ERR_TX_TIMEOUT", false); + sci.init("None", false, m2); + EXPECT_EQ(sci.getErrorMask(), (fatal_errors | report_errors)); + EXPECT_EQ(sci.getFatalErrorMask(), fatal_errors); + + // remove fatal flag + auto m3 = can::SettingsMap::create(); + m3->set("fatal_error_mask/CAN_ERR_TX_TIMEOUT", false); + sci.init("None", false, m3); + EXPECT_EQ(sci.getErrorMask(), (fatal_errors | report_errors) & (~CAN_ERR_TX_TIMEOUT)); + EXPECT_EQ(sci.getFatalErrorMask(), fatal_errors & (~CAN_ERR_TX_TIMEOUT)); + + // remove fatal and report flag + auto m4 = can::SettingsMap::create(); + m4->set("fatal_error_mask/CAN_ERR_TX_TIMEOUT", false); + m4->set("error_mask/CAN_ERR_LOSTARB", false); + sci.init("None", false, m4); + EXPECT_EQ(sci.getErrorMask(), (fatal_errors | report_errors) & ~(CAN_ERR_TX_TIMEOUT|CAN_ERR_LOSTARB)); + EXPECT_EQ(sci.getFatalErrorMask(), fatal_errors & (~CAN_ERR_TX_TIMEOUT)); +} + +TEST(SettingTest, xmlrpc) +{ + XmlRpc::XmlRpcValue value; + value["param"] = 1; + XmlRpc::XmlRpcValue segment; + segment["param"] = 2; + value["segment"] = segment; + XmlRpcSettings settings(value); + + ASSERT_TRUE(value["segment"].hasMember(std::string("param"))); + + int res; + EXPECT_TRUE(settings.get("param", res)); + EXPECT_EQ(res, 1); + EXPECT_EQ(settings.get_optional("param", 0), 1); + EXPECT_FALSE(settings.get("param2", res)); + EXPECT_EQ(settings.get_optional("param2", 0), 0); + + EXPECT_TRUE(settings.get("segment/param", res)); + EXPECT_EQ(res, 2); + EXPECT_EQ(settings.get_optional("segment/param", 0), 2); + + EXPECT_FALSE(settings.get("segment/param2", res)); + EXPECT_EQ(settings.get_optional("segment/param2", 0), 0); + EXPECT_FALSE(settings.get("segment2/param", res)); + EXPECT_EQ(settings.get_optional("segment2/param", 0), 0); +} + +// Run all the tests that were declared with TEST() +int main(int argc, char **argv){ +testing::InitGoogleTest(&argc, argv); +return RUN_ALL_TESTS(); +} diff --git a/Devices/Libraries/Ros/ros_canopen/socketcan_interface/test/test_string.cpp b/Devices/Libraries/Ros/ros_canopen/socketcan_interface/test/test_string.cpp new file mode 100755 index 0000000..4e3a339 --- /dev/null +++ b/Devices/Libraries/Ros/ros_canopen/socketcan_interface/test/test_string.cpp @@ -0,0 +1,53 @@ +// Bring in my package's API, which is what I'm testing +#include + +// Bring in gtest +#include + + +TEST(StringTest, stringconversion) +{ + const std::string s1("123#1234567812345678"); + can::Frame f1 = can::toframe(s1); + EXPECT_EQ(s1, can::tostring(f1, true)); + + const std::string s2("1337#1234567812345678"); + can::Frame f2 = can::toframe(s2); + EXPECT_FALSE(f2.isValid()); + + const std::string s3("80001337#1234567812345678"); + const std::string s4("00001337#1234567812345678"); + + can::Frame f3 = can::toframe(s3); + EXPECT_EQ(f3.fullid(), 0x80001337); + EXPECT_TRUE(f3.isValid()); + EXPECT_TRUE(f3.is_extended); + EXPECT_EQ(s4, can::tostring(f3, true)); // 8000 is converted to 0000 + + can::Frame f4 = can::toframe(s4); + EXPECT_EQ(f4.fullid(), 0x80001337); + EXPECT_TRUE(f4.isValid()); + EXPECT_TRUE(f4.is_extended); + EXPECT_EQ(s4, can::tostring(f4, true)); + + const std::string s5("20001337#1234567812345678"); + can::Frame f5 = can::toframe(s5); + EXPECT_EQ(f5.fullid(), 0xA0001337); + EXPECT_TRUE(f5.isValid()); + EXPECT_TRUE(f5.is_error); + EXPECT_EQ(s5, can::tostring(f5, true)); + + const std::string s6("40001337#1234567812345678"); + can::Frame f6 = can::toframe(s6); + EXPECT_EQ(f6.fullid(), 0xC0001337); + EXPECT_TRUE(f6.isValid()); + EXPECT_TRUE(f6.is_rtr); + EXPECT_EQ(s6, can::tostring(f6, true)); + +} + +// Run all the tests that were declared with TEST() +int main(int argc, char **argv){ +testing::InitGoogleTest(&argc, argv); +return RUN_ALL_TESTS(); +} diff --git a/Devices/Libraries/Systems/CANopenSocket/.gitignore b/Devices/Libraries/Systems/CANopenSocket/.gitignore new file mode 100755 index 0000000..dd420ca --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/.gitignore @@ -0,0 +1,16 @@ +/cocomm/cocomm +/canopencgi/canopen.cgi +*.o +*.bak +tmp +index.html + +/tools/EDSEditor + +# eclipse +.cproject +.project +.settings/ + +# kdevelop +*.kdev4 diff --git a/Devices/Libraries/Systems/CANopenSocket/.gitmodules b/Devices/Libraries/Systems/CANopenSocket/.gitmodules new file mode 100755 index 0000000..5779331 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/.gitmodules @@ -0,0 +1,12 @@ +[submodule "CANopenNode"] + path = CANopenNode + url = https://github.com/CANopenNode/CANopenNode.git +[submodule "test/libs/bats-core"] + path = test/libs/bats-core + url = https://github.com/bats-core/bats-core.git +[submodule "test/libs/bats-support"] + path = test/libs/bats-support + url = https://github.com/bats-core/bats-support.git +[submodule "test/libs/bats-assert"] + path = test/libs/bats-assert + url = https://github.com/bats-core/bats-assert.git diff --git a/Devices/Libraries/Systems/CANopenSocket/LICENSE b/Devices/Libraries/Systems/CANopenSocket/LICENSE new file mode 100755 index 0000000..d645695 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/LICENSE @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/Devices/Libraries/Systems/CANopenSocket/README.md b/Devices/Libraries/Systems/CANopenSocket/README.md new file mode 100755 index 0000000..5ba74a4 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/README.md @@ -0,0 +1,79 @@ +CANopenSocket +============= + +**THIS REPOSITORY IS OBSOLETE, ITS CONTENTS HAS BEEN MOVED TO:** +- https://github.com/CANopenNode/CANopenDemo +- https://github.com/CANopenNode/CANopenLinux + +-------------------------- + +CANopenSocket is a collection of CANopen tools and examples. + +It is based on [CANopenNode](https://github.com/CANopenNode/CANopenNode), which is free and open source CANopen Stack and is included as a git submodule. + +CANopen is the internationally standardized (EN 50325-4) ([CiA301](http://can-cia.org/standardization/technical-documents)) CAN-based higher-layer protocol for embedded control system. For more information on CANopen see http://www.can-cia.org/ + +Getting or updating the project +------------------------------- +Clone the project from git repository and get submodules: + + git clone https://github.com/CANopenNode/CANopenSocket.git + cd CANopenSocket + git submodule init + git submodule update + +Update the project: + + cd CANopenSocket + # make sure, project is clean (git status); changes should be committed (git gui) + git pull # or: git fetch -> gitk --all (inspect the changes) -> git merge + # git submodule init # if new git modules + git submodule update + +Get/update other tools (EDSeditor): + + cd tools + ./get_tools.sh + + +Getting started +--------------- +First information is in CANopenNode/README. + +Basic getting started guide is available in `CANopenNode/doc/gettingStarted.md`. If everything works there as expected, with virtual or real CAN, then CANopen network is established. With at least basic operation of: CAN reception, transmission, NMT, Heartbeat, Service Communication Objects (SDO). + +Next is Basic CANopen Device example program with source code, custom Object Dictionary access functions and advanced CANopen gateway communication. See [basicDevice/README.md](examples/basicDevice/README.md). + + +CANopenSocket contents +---------------------- + - **CANopenNode** - free and open source CANopen Stack (git submodule) + - **docs** - CANopenNode documentation, generated by doxygen, available also online at https://canopennode.github.io/CANopenSocket/ + - **cocomm** - Linux commmand line tool, which communicates with CANopen gateway device via local or TCP socket. Similar as `nc` tool available in Linux. `make` the executable and type `./cocomm --help` or see [basicDevice/README.md](examples/basicDevice/README.md) for example usage. + - **canopencgi** - CGI interface for Apache web server, experimental. + - **tools** + - **EDSEditor** - Object Dictionary Editor from https://github.com/robincornelius/libedssharp. Updated with `get_tools.sh`. + - **get_tools.sh** - script for getting/updating the tools. + - **update_docs.sh** - script runs doxygen on CANopenNode and creates softlink index.html. + - **run_canopend_candump.sh** - script creates virtual CAN device `vcan0` and runs `candump` and `canopend` with gateway interface on it. + - **examples** + - **basicDevice** - Example CANopenNode device with or without gateway interface. Includes example for different testing variables: 64-bit integers, real numbers, strings, domain, object oriented principle with C, etc. See [basicDevice/README.md](examples/basicDevice/README.md). + - **test** - Different CANopen testing tools + - **libs/bats-*** [Bash Automated Testing System](https://github.com/bats-core/bats-core), included as git submodules. + - **running_canopen** - tests on running CANopen network, based on bats. On virtual or real CAN network. See [test.md](test/test.md) + - **docker** - Dockerfile with readme. + + +License +------- +Licensed under the Apache License, Version 2.0 (the "License"); +you may not use this file except in compliance with the License. +You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + +Unless required by applicable law or agreed to in writing, software +distributed under the License is distributed on an "AS IS" BASIS, +WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +See the License for the specific language governing permissions and +limitations under the License. diff --git a/Devices/Libraries/Systems/CANopenSocket/canopencgi/CANopenCGI.c b/Devices/Libraries/Systems/CANopenSocket/canopencgi/CANopenCGI.c new file mode 100755 index 0000000..e08ebf6 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/canopencgi/CANopenCGI.c @@ -0,0 +1,413 @@ +/* + * Client socket command interface (Apache CGI) for CANopenSocket. + * + * @file CANopenCGI.c + * @author Janez Paternoster + * @copyright 2016 - 2020 Janez Paternoster + * + * This file is part of CANopenSocket, a Linux implementation of CANopen + * stack with master functionality. Project home page is + * . CANopenSocket is based + * on CANopenNode: . + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +#ifndef BUF_SIZE +#define BUF_SIZE 1000000 +#endif + + +/* Helper functions */ +static void errExitErrno(char* msg) { + printf("%s: %s\n", msg, strerror(errno)); + exit(EXIT_FAILURE); +} + +static void errExitErrnoUGID(char* msg) { + struct passwd *user; + struct group *grp; + + printf("%s: %s\n", msg, strerror(errno)); + + user = getpwuid(getuid()); + grp = getgrgid(getgid()); + + printf("Access from user: %s, group: %s\n", + (user == NULL) ? NULL : user->pw_name, + (grp == NULL) ? NULL : grp->gr_name); + + exit(EXIT_FAILURE); +} + +static void errExit(char* msg) { + printf("%s\n", msg); + exit(EXIT_FAILURE); +} + +static void strcpyToUpper(char *dest, const char *src) { + char in; + + do { + in = *(src++); + *(dest++) = toupper(in); + } while(in != 0); +} + +static void strToUpper(char *str) { + char c; + + do { + c = *(str); + *(str++) = toupper(c); + } while(c != 0); +} + +static void strToLower(char *str) { + char c; + + do { + c = *(str); + *(str++) = tolower(c); + } while(c != 0); +} + +/* Decode hex string 'str' of length 'len' and return numerical value. + * In case of error in string, set 'err' to 1. */ +static unsigned int hex2dec(const char *str, int len, int *err){ + unsigned int val = 0; + int i; + + for(i=0; i= '0' && c <= '9') { + c = c - '0'; + } else if (c >= 'A' && c <= 'F') { + c = c - ('A' - 10); + } + else { + *err = 1; + return 0; + } + val = val << 4 | c; + } + return val; +} + +static void sendCommand(int fd, int sequence, char* command); + +static void printUsage(void) { +printf( +"Usage: canopen.cgi?wnniiiissdd=xxxx[&rnniiiissdd=]\n" +" - w - One digit - 'W'rite or 'R'ead.\n" +" - nn - Two hex digits of node ID.\n" +" - iiii - Four hex digits of Object Dictionary Index.\n" +" - ss - Two hex digits of Object Dictionary Subindex.\n" +" - dd - One to three digits of data type.\n" +" - xxxx - Value to be written.\n" +"\n" +"Datatypes:\n" +" - b - Boolean.\n" +" - u8, u16, u32, u64 - Unsigned integers.\n" +" - i8, i16, i32, i64 - Signed integers.\n" +" - r32, r64 - Real numbers.\n" +" - t, td - Time of day, time difference.\n" +" - vs - Visible string (between double quotes).\n" +" - os, us, d - Octet string, unicode string, domain." +); +} + + +/******************************************************************************/ +int main (int argc, char *argv[], char *env[]) { + char socketPath[260] = {0}; /* Name of the local domain socket. */ + + FILE *fp; + int fdSocket; + struct sockaddr_un addr; + char *queryString; + int queryStringAllocated = 0; + + /* whitelist and blacklist are arrays of null separated strings, which + * contains patterns for comparision with commands from query string. */ + char *whitelist; + char *blacklist; + int whitelistLen; + int blacklistLen; + + + /* Print mime */ + printf("Content-type:text/plain\n\n"); + + + /* Get program options from configuration file */ + fp = fopen("canopen.conf", "r"); + if(fp == NULL) { + errExitErrno("Can't open configuration file"); + } + else { + const char spaceDelim[] = " \t\n\r\f\v"; + char buf[1000]; + int wlSize = 1000; /* byte length */ + int blSize = 1000; + int wlDataSize = 0; + int blDataSize = 0; + + whitelist = (char *) malloc(wlSize); + blacklist = (char *) malloc(blSize);; + whitelistLen = 0; /* number of tokens in list */ + blacklistLen = 0; + if(whitelist == NULL || blacklist == NULL) { + errExitErrno("Whitelist or Blacklist can't be allocated."); + } + + while(fgets(buf, sizeof(buf), fp) != NULL) { + char *token; + token = strtok(buf, spaceDelim); + + if(token == NULL) { + + } + else if(strcasecmp(token, "socketPath") == 0) { + if(strlen(socketPath) != 0) { + errExit("Duplicate 'socketPath' in canopen.conf."); + } + strncpy(socketPath, strtok(NULL, spaceDelim), sizeof(socketPath)); + socketPath[sizeof(socketPath)-1] = 0; + } + else if(strcasecmp(token, "allow") == 0) { + int prevDataSize = wlDataSize; + + token = strtok(NULL, spaceDelim); + wlDataSize += (strlen(token) + 1); + while(wlDataSize > wlSize) { + wlSize *= 2; + whitelist = (char *) realloc(whitelist, wlSize); + if(whitelist == NULL) { + errExitErrno("Whitelist can't be allocated."); + } + } + strcpyToUpper(&whitelist[prevDataSize], token); + whitelistLen ++; + } + else if(strcasecmp(token, "deny") == 0) { + int prevDataSize = blDataSize; + + token = strtok(NULL, spaceDelim); + blDataSize += (strlen(token) + 1); + while(blDataSize > blSize) { + blSize *= 2; + blacklist = (char *) realloc(blacklist, blSize); + if(blacklist == NULL) { + errExitErrno("Blacklist can't be allocated."); + } + } + strcpyToUpper(&blacklist[prevDataSize], token); + blacklistLen ++; + } + } + } + + fclose(fp); + + + /* Create and connect client socket */ + fdSocket = socket(AF_UNIX, SOCK_STREAM, 0); + if(fdSocket == -1) { + errExitErrno("Socket creation failed"); + } + + memset(&addr, 0, sizeof(struct sockaddr_un)); + addr.sun_family = AF_UNIX; + strncpy(addr.sun_path, socketPath, sizeof(addr.sun_path) - 1); + + if(connect(fdSocket, (struct sockaddr *)&addr, sizeof(struct sockaddr_un)) == -1) { + errExitErrnoUGID("Socket connection failed"); + } + + + /* get query string */ + queryString = getenv("QUERY_STRING"); /* HTTP GET method. */ + if(queryString != NULL && strlen(queryString) == 0) { + queryString = malloc(BUF_SIZE); + if(queryString == NULL) { + errExitErrno("queryString can't be allocated."); + } + queryStringAllocated = 1; + fgets(queryString, BUF_SIZE, stdin); /* HTTP POST method. */ + } + if(queryString == NULL && argc >= 2) { + queryString = argv[1]; /* If no query string, try first argument. */ + } + + + /* get commands from query string */ + if(queryString != NULL && strlen(queryString) > 0) { + char *command; + int sequence = 1; + + /* put whole query string to upper case */ + strToUpper(queryString); + + command = strtok(queryString, "&"); + while(command != NULL) { + int i; + int offset; + int passed = 0; + + /* Test whitelist and blacklist */ + offset = 0; + for(i=0; i 127) { + err = 1; + } + + idx = hex2dec(&command[3], 4, &err); + sidx = hex2dec(&command[7], 2, &err); + + for(i=0; i 3) { + err = 1; + dataType[0] = 0; + } + if(strlen(value) > (sizeof(buf) - 50)) { + err = 1; + } + + /* Write command according to CiA309-3. */ + if(err == 0) { + size_t wlen, rlen; + + strToLower(dataType); + + wlen = sprintf(buf, "[%d] 0x%02X %c 0x%04X 0x%02X %s %s\n", + sequence, nodeId, tolower(comm), idx, sidx, dataType, value); + + if (write(fd, buf, wlen) != wlen) { + errExit("Socket write failed"); + } + + rlen = read(fd, buf, sizeof(buf) - 1); + + if(rlen < 0) { + errExit("Socket read failed"); + } + else { + buf[rlen] = 0; + } + + printf("%c %02X%04X%02X %s", + comm, nodeId, idx, sidx, buf); + } + else { + printf("? %s [%d] ERROR: 101 - Syntax error in command.\n", + command, sequence); + } +} diff --git a/Devices/Libraries/Systems/CANopenSocket/canopencgi/Makefile b/Devices/Libraries/Systems/CANopenSocket/canopencgi/Makefile new file mode 100755 index 0000000..067c87b --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/canopencgi/Makefile @@ -0,0 +1,33 @@ +# Makefile for CANopenCGI. + + +APPL_SRC = . + + +LINK_TARGET = canopen.cgi + + +INCLUDE_DIRS = -I$(APPL_SRC) + + +SOURCES = $(APPL_SRC)/CANopenCGI.c + + +OBJS = $(SOURCES:%.c=%.o) +CC ?= gcc +CFLAGS = -Wall -g $(INCLUDE_DIRS) +LDFLAGS = + + +.PHONY: all clean + +all: clean $(LINK_TARGET) + +clean: + rm -f $(OBJS) $(LINK_TARGET) + +%.o: %.c + $(CC) $(CFLAGS) -c $< -o $@ + +$(LINK_TARGET): $(OBJS) + $(CC) $(LDFLAGS) $^ -o $@ diff --git a/Devices/Libraries/Systems/CANopenSocket/canopencgi/canopen.conf b/Devices/Libraries/Systems/CANopenSocket/canopencgi/canopen.conf new file mode 100755 index 0000000..3ecc932 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/canopencgi/canopen.conf @@ -0,0 +1,56 @@ +# Configuration file for canopen.cgi + +# canopen.cgi is an application usually accessible by a web server like +# apache. Query string contains commands to canopend. canopend is an +# application, which runs in background. It is a CANopen device with +# master functionality and command interface (via socket). One typical +# command is read or write specific object(variable) in specific device. +# (Each CANopen device has own Object Dictionary, collection of all +# variables. Any variable on any CANopen device can be read or written, +# if we know its address (nodeID + index + subindex). CANopen SDO client +# (master device - canopend) sends Service Data Object (SDO) via CANopen +# network, and one of the SDO servers (slave devices) responses with +# requested information.) + + +# Name of the local domain socket, link to canopend. */ +socketPath /tmp/CO_command_socket + + +# Whitelist and Blacklist of CANopen objects, which are allowed or not +# allowed to be accessed by canopen.cgi. Whitelist is generated from all +# lines, which begins with 'allow'. Blacklist is generated from all +# lines, which begins with 'deny'. +# Each Object from query string is first NOT allowed, to be accessed. If +# it matches ANY pattern from the whitelist, it is allowed. (Whitelist, +# which allows everything would then be just one line: 'allow *'.) On +# the end, object is verified with blacklist. If it matches any pattern +# from there, it won't be passed as command to canopend. +# +# Format of the parameter in Query string: wnniiiissll=xxxx +# Where w, nn, iiii, ss, ll, xxxx are fixed width fields with the following meaning: +# w - 'W'rite or 'R'ead. +# nn - node ID in hex format. +# iiii - Object Dictionary index in hex format. +# ss - Object Dictionary subindex in hex format. +# ll - length of variable (1 to FFFFFFFF) in hex format. If reading, this value is ignored. +# xxxx - Value to be written in hex and little endian format. If reading, this value is ignored. +# +# Parameter and pattern strings are not case sensitive. + + +# Communication segment - up to 0x1FFF +allow R??[01]* # allow read all. + + +# Manufacturer segment - 0x2000 to 0x5FFF +allow R??[2-5]* # allow read all. + +# Example of write access to some objects +allow [RW]032110* # allow r/w on NodeId 3, index 0x2110, any subindex. +deny W0321100[2-4E]* # deny write on NodeId 3, index 0x2110, subindex 0x02, 0x03, 0x04 and 0x0E. +allow [RW]0321200[!45]* # allow r/w on NodeId 3, index 0x2120, subindex 0x00-0x03 and 0x06-0x09. + + +# Device profile segment - above 0x6000 +allow R??[6-9A_F]* # allow read all diff --git a/Devices/Libraries/Systems/CANopenSocket/cocomm/Makefile b/Devices/Libraries/Systems/CANopenSocket/cocomm/Makefile new file mode 100755 index 0000000..d770e68 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/cocomm/Makefile @@ -0,0 +1,26 @@ +# Makefile for CANopenCommand. + +APPL_SRC = . +LINK_TARGET = cocomm +INCLUDE_DIRS = -I$(APPL_SRC) +SOURCES = $(APPL_SRC)/cocomm.c + +OBJS = $(SOURCES:%.c=%.o) +CC ?= gcc +OPT = -g +#OPT = -g -pedantic -Wshadow -fanalyzer +CFLAGS = -Wall $(OPT) $(INCLUDE_DIRS) +LDFLAGS = + +.PHONY: all clean + +all: clean $(LINK_TARGET) + +clean: + rm -f $(OBJS) $(LINK_TARGET) + +%.o: %.c + $(CC) $(CFLAGS) -c $< -o $@ + +$(LINK_TARGET): $(OBJS) + $(CC) $(LDFLAGS) $^ -o $@ diff --git a/Devices/Libraries/Systems/CANopenSocket/cocomm/cocomm.c b/Devices/Libraries/Systems/CANopenSocket/cocomm/cocomm.c new file mode 100755 index 0000000..b04ae7e --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/cocomm/cocomm.c @@ -0,0 +1,559 @@ +/* + * Client socket command interface for CANopenSocket. + * + * @file cocomm.c + * @author Janez Paternoster + * @copyright 2020 Janez Paternoster + * + * This file is part of CANopenSocket, a Linux implementation of CANopen + * stack with master functionality. Project home page is + * . CANopenSocket is based + * on CANopenNode: . + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#ifndef BUF_SIZE +#define BUF_SIZE 1000 +#endif +#define BUF_LAG 100 /* max size of error response */ + +/* colors and stream for printing status */ +char *greenC, *redC, *resetC; +FILE *errStream; + + +static void printUsage(char *progName) { +fprintf(errStream, +"Usage: %s [options] ['' [''] ...]\n" +"\n" +"Program reads CANopen gateway command strings from arguments, standard input or\n" +"file. It sends commands to canopend via socket, line after line. Response status\n" +"is printed to standard error and response data (for example, value from write\n" +"command) is printed to standard output. Command strings from arguments must\n" +"be quoted. Socket is either unix domain socket (default) or a remote tcp socket\n" +"(option -t). For more information see http://www.can-cia.org/, CiA 309 standard.\n" +"\n" +"Options:\n" +" -f Path to the input file.\n" +" -s Path to the unix socket. If not specified, path is obtained\n" +" from environmental variable, configured with:\n" +" 'export cocomm_socket='. If latter is not\n" +" specified, default value is used: '/tmp/CO_command_socket'.\n" +" -t Connect via tcp to remote . Set also with\n" +" 'export cocomm_host='. Unix socket is used by default.\n" +" -p Tcp port to connect to when using -t. Set also with\n" +" 'export cocomm_port='. Default is 60000.\n" +" -i If set, then standard input will be read after each command\n" +" string from arguments. Useful with write commands.\n" +" -o all|data|flat By defult (setting 'all') outupt is split to colored stderr\n" +" and stdout. 'data' prints data only to stdout. 'flat' prints\n" +" all to stdout, set also with 'export cocomm_flat=<0|1>'.\n" +" -d If specified, then candump of specified CAN device will be\n" +" printed after the command response. Set also with\n" +" 'export cocomm_candump='. Not used by default.\n" +" -n Print of candump messages, then exit. Set also with\n" +" 'export cocomm_candump_count='. Default is 10.\n" +" -T Exit candump after without reception. Set also with\n" +" 'export cocomm_candump_timeout='. Default is 1000.\n" +" --help Display this help.\n" +"\n" +"For help on command strings type '%s \"help\"'.\n" +"\n" +"See also: https://github.com/CANopenNode/CANopenSocket\n" +"\n", progName, progName); +} + +/* print reply, status to errStream (red or green), value to stdout */ +static int printReply(int fd_gtw) { + char* replyBuf = malloc(BUF_SIZE + BUF_LAG + 1); + size_t count = 0; /* count of bytes in replyBuf */ + int firstPass = 1; + int ret = EXIT_SUCCESS; + + if(replyBuf == NULL) { + perror("replyBuf malloc"); + exit(EXIT_FAILURE); + } + + for (;;) { + ssize_t nRead = read(fd_gtw, &replyBuf[count], BUF_SIZE); /* blocking*/ + + if(nRead > 0) { + count += nRead; + replyBuf[count] = 0; + + if (firstPass == 1) { + /* check for response type. Only response value goes to stdout*/ + if (strstr(replyBuf, "] ERROR:") != NULL + && strstr(replyBuf, "\r\n") != NULL + ) { + fprintf(errStream, "%s%s%s", redC, replyBuf, resetC); + ret = EXIT_FAILURE; + break; + } + else if (strstr(replyBuf, "] OK\r\n") != NULL) { + fprintf(errStream, "%s%s%s", greenC, replyBuf, resetC); + break; + } + else { + char *replyBufTrimmed = replyBuf; + char *seq = strstr(replyBuf, "] "); + char *end = strstr(replyBuf, "\r\n"); + if (seq != NULL && (size_t)(seq - replyBuf) < 15) { + replyBufTrimmed = seq + 2; + seq[1] = 0; + count -= strlen(replyBuf) + 1; + fprintf(errStream, "%s%s%s ", greenC, replyBuf, resetC); + } + if (end != NULL) { + end[0] = 0; + fputs(replyBufTrimmed, stdout); + fflush(stdout); + fputs("\r\n", errStream); + break; + } + else { + /* print replyBuf to stdout, except last BUF_LAG bytes. + * move them to the beginning of the replyBuf */ + if (count > BUF_LAG) { + size_t nWrite = count - BUF_LAG; + fwrite(replyBufTrimmed, 1, nWrite, stdout); + replyBufTrimmed += nWrite; + count = BUF_LAG; + } + memmove(replyBuf, replyBufTrimmed, count); + } + } + firstPass = 0; + } + else { + char *end = strstr(replyBuf, "\r\n"); + if (end != NULL) { + char *errResp = strstr(replyBuf, "\n...ERROR:0x"); + if (errResp != NULL) { + errResp[0] = 0; + fputs(replyBuf, stdout); + fflush(stdout); + fprintf(errStream, "\n%s%s%s", redC, &errResp[1], resetC); + ret = EXIT_FAILURE; + } + else { + end[0] = 0; + fputs(replyBuf, stdout); + fflush(stdout); + fprintf(errStream, "\n%s...success%s\r\n", greenC, resetC); + } + break; + } + /* print replyBuf to stdout, except last BUF_LAG bytes. + * move them to the beginning of the replyBuf */ + if (count > BUF_LAG) { + size_t nWrite = count - BUF_LAG; + fwrite(replyBuf, 1, nWrite, stdout); + count = BUF_LAG; + memmove(replyBuf, &replyBuf[nWrite], count); + } + } + } + else if (nRead == 0) { + fprintf(errStream, "%sError, zero response%s\n", redC, resetC); + ret = EXIT_FAILURE; + break; + } + else { + perror("Socket read failed"); + free(replyBuf); + exit(EXIT_FAILURE); + } + fflush(stdout); + } + free(replyBuf); + + return ret; +} + + +/******************************************************************************/ +int main (int argc, char *argv[]) { + /* configurable options */ + enum {out_all, out_data, out_flat} outputType = out_all; + char *inputFilePath = NULL; + char *socketPath = "/tmp/CO_command_socket"; /* Name of the local domain socket */ + char hostname[HOST_NAME_MAX]; /* name of the remote TCP host */ + char tcpPort[20] = "60000"; /* default port when used in tcp mode */ + int additionalReadStdin = 0; + char *candump = NULL; + long candumpCount = 10; + long candumpTmo = 1000; + + char* commBuf; + int fd_gtw; + int fd_candump; + int opt; + struct sockaddr_un addr_un; + sa_family_t addrFamily = AF_UNIX; + errStream = stderr; + + if(argc >= 2 && strcmp(argv[1], "--help") == 0) { + printUsage(argv[0]); + exit(EXIT_SUCCESS); + } + + /* Get program options from environment variables */ + char *env; + if ((env = getenv("cocomm_host")) != NULL) { + strncpy(hostname, env, sizeof(hostname)); + addrFamily = AF_INET; + } + if ((env = getenv("cocomm_port")) != NULL) { + strncpy(tcpPort, env, sizeof(tcpPort)); + } + if ((env = getenv("cocomm_socket")) != NULL) { + socketPath = env; + addrFamily = AF_UNIX; + } + if ((env = getenv("cocomm_flat")) != NULL) { + if (strcmp(env, "1") == 0) { + outputType = out_flat; + } + } + if ((env = getenv("cocomm_candump")) != NULL) { + candump = env; + } + if ((env = getenv("cocomm_candump_count")) != NULL) { + candumpCount = atol(env); + } + if ((env = getenv("cocomm_candump_timeout")) != NULL) { + candumpTmo = atol(env); + } + + /* Get program options from arguments */ + while((opt = getopt(argc, argv, "f:s:t:p:io:d:n:T:")) != -1) { + switch (opt) { + case 'f': + inputFilePath = optarg; + break; + case 's': + addrFamily = AF_UNIX; + socketPath = optarg; + break; + case 't': + addrFamily = AF_INET; + strncpy(hostname, optarg, sizeof(hostname)); + break; + case 'p': + strncpy(tcpPort, optarg, sizeof(tcpPort)); + break; + case 'i': + additionalReadStdin = 1; + break; + case 'o': + if (strcmp(optarg, "data") == 0) + outputType = out_data; + else if (strcmp(optarg, "flat") == 0) + outputType = out_flat; + break; + case 'd': + candump = optarg; + break; + case 'n': + candumpCount = atol(optarg); + break; + case 'T': + candumpTmo = atol(optarg); + break; + default: + printUsage(argv[0]); + exit(EXIT_FAILURE); + } + } + + switch (outputType) { + default: + case out_all: + greenC = "\033[32m"; + redC = "\033[31m"; + resetC = "\033[0m"; + errStream = stderr; + break; + case out_data: + greenC = redC = resetC = ""; + errStream = fopen("/dev/null", "w+"); + break; + case out_flat: + greenC = redC = resetC = ""; + errStream = stdout; + break; + } + + /* Ignore the SIGPIPE signal, which may happen, if gateway broke + * the connection. Program will exit with error message anyway. + * Signal may be triggered by socket write call. */ + if (signal(SIGPIPE, SIG_IGN) == SIG_ERR) { + perror("signal"); + exit(EXIT_FAILURE); + } + + /* Create and connect client socket */ + if(addrFamily == AF_INET) { + + struct addrinfo hints, *res, *rp; + int errcode; + + memset(&hints, 0, sizeof(hints)); + hints.ai_family = AF_INET; + hints.ai_socktype = SOCK_STREAM; + hints.ai_flags |= AI_CANONNAME; + + errcode = getaddrinfo(hostname, tcpPort, &hints, &res); + if (errcode != 0) { + fprintf(stderr, "Error! Getaddrinfo for host %s failed\n", hostname); + exit(EXIT_FAILURE); + } + + /* getaddrinfo() returns a list of address structures. Try each address + * until we successfully connect. If socket (or connect) fails, + * we (close the socket and) try the next address. */ + + for (rp = res; rp != NULL; rp = rp->ai_next) { + fd_gtw = socket(rp->ai_family, rp->ai_socktype, rp->ai_protocol); + if (fd_gtw == -1) { + continue; + } + + if (connect(fd_gtw, rp->ai_addr, rp->ai_addrlen) != -1) { + break; /* Success */ + } + + close(fd_gtw); + perror("Socket connection failed"); + exit(EXIT_FAILURE); + } + } + else { // addrFamily == AF_UNIX + fd_gtw = socket(AF_UNIX, SOCK_STREAM, 0); + if(fd_gtw == -1) { + perror("Socket creation failed"); + exit(EXIT_FAILURE); + } + + memset(&addr_un, 0, sizeof(struct sockaddr_un)); + addr_un.sun_family = addrFamily; + strncpy(addr_un.sun_path, socketPath, sizeof(addr_un.sun_path) - 1); + + if(connect(fd_gtw, (struct sockaddr *)&addr_un, sizeof(struct sockaddr_un)) == -1) { + fprintf(stderr, "Socket connection failed \"%s\": ", socketPath); + perror(NULL); + exit(EXIT_FAILURE); + } + } + + /* Prepare candump */ + if (candump != NULL && candumpCount > 0 && candumpTmo > 0) { + struct sockaddr_can sockAddr; + + fd_candump = socket(PF_CAN, SOCK_RAW, CAN_RAW); + if (fd_candump < 0) { + perror("CAN socket creation failed"); + exit(EXIT_FAILURE); + } + + struct timeval tv; + tv.tv_sec = candumpTmo / 1000; + tv.tv_usec = (candumpTmo % 1000) * 1000; + setsockopt(fd_candump, SOL_SOCKET, SO_RCVTIMEO, (const char*)&tv, sizeof tv); + + memset(&sockAddr, 0, sizeof(sockAddr)); + sockAddr.can_family = AF_CAN; + sockAddr.can_ifindex = if_nametoindex(candump); + int ret = bind(fd_candump, (struct sockaddr*)&sockAddr, sizeof(sockAddr)); + if (ret < 0) { + fprintf(stderr, "CAN Socket binding failed \"%s\": ", candump); + perror(NULL); + exit(EXIT_FAILURE); + } + } + else { + candumpCount = 0; + } + + /* get commands from input file, line after line */ + int ret = EXIT_SUCCESS; + + commBuf = malloc(BUF_SIZE); + if(commBuf == NULL) { + perror("commBuf malloc"); + exit(EXIT_FAILURE); + } + + if(inputFilePath != NULL) { + FILE *fp = fopen(inputFilePath, "r"); + if(fp == NULL) { + perror("Can't open input file"); + free(commBuf); + exit(EXIT_FAILURE); + } + + while(fgets(commBuf, BUF_SIZE, fp) != NULL) { + size_t len = strlen(commBuf); + if (len < 1) continue; + + // send command + if (write(fd_gtw, commBuf, len) != len) { /* blocking function */ + perror("Socket write failed"); + free(commBuf); + exit(EXIT_FAILURE); + } + + // print reply, if command is complete + if (commBuf[len - 1] == '\n') { + if (printReply(fd_gtw) == EXIT_FAILURE) { + ret = EXIT_FAILURE; + } + } + } + + fclose(fp); + } + + /* get command from arguments */ + else if(optind < argc) { + for(int i = optind; i < argc; i++) { + char *comm = argv[i]; + commBuf[0] = 0; + + /* Add sequence number if not present on command line argument */ + if(comm[0] != '[' && comm[0] != '#') { + sprintf(commBuf, "[%d] ", i - optind + 1); + } + + if((strlen(commBuf) + strlen(comm)) >= (BUF_SIZE - 2)) { + fprintf(errStream, "%sCommand string too long!%s\n", redC, greenC); + continue; + } + + strcat(commBuf, comm); + + if (additionalReadStdin == 0) { + strcat(commBuf, "\n"); + size_t len = strlen(commBuf); + if (write(fd_gtw, commBuf, len) != len) { /* blocking function */ + perror("Socket write failed"); + free(commBuf); + exit(EXIT_FAILURE); + } + } + else { + strcat(commBuf, " "); + size_t len = strlen(commBuf); + char lastChar; + + do { + if (fgets(commBuf+len, BUF_SIZE-1-len, stdin) == NULL) + strcat(commBuf, "\n"); + + len = strlen(commBuf); + lastChar = commBuf[len - 1]; + + if (len < BUF_SIZE-2 && lastChar != '\n') + continue; + + // send command + if (write(fd_gtw, commBuf, len) != len) { /* blocking f. */ + perror("Socket write failed"); + free(commBuf); + exit(EXIT_FAILURE); + } + + commBuf[0] = 0; + len = 0; + } while (lastChar != '\n'); + } + + if (printReply(fd_gtw) == EXIT_FAILURE) { + ret = EXIT_FAILURE; + } + } + } + + /* get commands from stdin, line after line */ + else { + while(fgets(commBuf, BUF_SIZE, stdin) != NULL) { + size_t len = strlen(commBuf); + if (len < 1) continue; + + // send command + if (write(fd_gtw, commBuf, len) != len) { /* blocking function */ + perror("Socket write failed"); + free(commBuf); + exit(EXIT_FAILURE); + } + + // print reply, if command is complete + if (commBuf[len - 1] == '\n') { + if (printReply(fd_gtw) == EXIT_FAILURE) { + ret = EXIT_FAILURE; + } + } + } + } + + close(fd_gtw); + + free(commBuf); + + /* candump output */ + if (candumpCount > 0) { + for (int i = 0; i < candumpCount; i++) { + const char hex_asc[] = "0123456789ABCDEF"; + struct can_frame canFrame; + + ssize_t nbytes = read(fd_candump, &canFrame, sizeof(struct can_frame)); + + if (nbytes < (ssize_t)sizeof(struct can_frame)) { + perror("CAN raw socket read timeout"); + exit(EXIT_FAILURE); + } + + char buf[17]; + char* pbuf = &buf[0]; + for (int j = 0; j < canFrame.can_dlc && j < sizeof(buf); j++) { + uint8_t byte = canFrame.data[j]; + *pbuf++ = hex_asc[byte >> 4]; + *pbuf++ = hex_asc[byte & 0x0F]; + } + *pbuf = 0; + + printf ("%03X#%s\n", canFrame.can_id, buf); + } + close(fd_candump); + } + + exit(ret); +} diff --git a/Devices/Libraries/Systems/CANopenSocket/docker/Dockerfile b/Devices/Libraries/Systems/CANopenSocket/docker/Dockerfile new file mode 100755 index 0000000..3009e05 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docker/Dockerfile @@ -0,0 +1,37 @@ +FROM ubuntu:16.04 + +ENV CANOPENSOCKET_SRC https://github.com/CANopenNode/CANopenSocket.git +ENV CANOPENSOCKET_REVISION ee1674c1ca790fc0d5c07a28f4a701d3c929f401 +ENV CANOPENSOCKET_DIR /CANopenSocket +ENV APP_DIR /app + +RUN apt-get update && apt-get install -y \ + net-tools \ + can-utils \ + git \ + build-essential \ + && apt-get -q autoremove \ + && apt-get -q clean -y \ + && rm -rf /var/lib/apt/lists/* \ + && rm -f /var/cache/apt/*.bin + +RUN git clone $CANOPENSOCKET_SRC $CANOPENSOCKET_DIR +WORKDIR $CANOPENSOCKET_DIR +RUN git checkout $CANOPENSOCKET_REVISION +RUN git submodule init; git submodule update + +WORKDIR canopend +RUN make + +RUN mkdir $APP_DIR +WORKDIR $APP_DIR +ENV PATH="$APP_DIR:${PATH}" + +RUN cp $CANOPENSOCKET_DIR/canopend/app/canopend $APP_DIR/canopend +RUN cp $CANOPENSOCKET_DIR/canopend/app/od_storage $APP_DIR/od_storage +RUN cp $CANOPENSOCKET_DIR/canopend/app/od_storage_auto $APP_DIR/od_storage_auto + +# Standard canopend port +EXPOSE 6000 + +CMD ./canopend $CAN -i 1 -c "" -t 6000 diff --git a/Devices/Libraries/Systems/CANopenSocket/docker/README.md b/Devices/Libraries/Systems/CANopenSocket/docker/README.md new file mode 100755 index 0000000..c1b7500 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docker/README.md @@ -0,0 +1,133 @@ +# CANopen gateway in Docker + +The dockerfile creates a CANopen master with gateway functionality. +Using the commands interface one can control a CANopen network. +The gateway is exposed at port *6000*. + +## Dependencies + +This project is based upon [CANopenSocket](https://github.com/CANopenNode/CANopenSocket) and [CANopenNode](https://github.com/CANopenNode/CANopenNode). + +## Building the container image + +This project should build with the default Docker build engine, but also *buildx* is supported for creating cross-platform container images. +Considering you're targetting embedded devices I'd guess you're cross building, so let's demonstrate *docker buildx* here. + +1. Make sure that Docker buildx is installed and that the Docker experimental features are enabled in your environment. +2. Either create a new or use a already created buildx engine: + +```bash +$ docker buildx create --name crossbuilder +$ docker buildx use crossbuilder +``` + +3. When building, we also want to push the build output to Docker hub so make sure this is also setup correctly and that're logging in: + +```bash +$ docker login +``` + +4. Now build (and push) the container image: + +```bash +$ docker buildx build --platform linux/arm,linux/arm64,linux/amd64 -t geoffreyvl/canopensocket . --push +``` + +5. On Docker Hub your container image will appear +6. We can now use the container image, for example let's inspect it: + +```bash +$ docker buildx imagetools inspect geoffreyvl/canopensocket +Name: docker.io/geoffreyvl/canopensocket:latest +MediaType: application/vnd.docker.distribution.manifest.list.v2+json +Digest: sha256:bd24fa61e43427f1113027e299b63c455510bb5d0fe9e2600d7db3fdde7ea58a + +Manifests: +Name: docker.io/geoffreyvl/canopensocket:latest@sha256:8d953afb62cd220e44c45abb0247c6ce6726b43bfe2b825475672e14e1996684 +MediaType: application/vnd.docker.distribution.manifest.v2+json +Platform: linux/arm/v7 + +Name: docker.io/geoffreyvl/canopensocket:latest@sha256:388e652c8e7796f6746fa45afedbb0c1fd8d2ad29d28d4c3c6228084b549b98c +MediaType: application/vnd.docker.distribution.manifest.v2+json +Platform: linux/arm64 + +Name: docker.io/geoffreyvl/canopensocket:latest@sha256:6bfeb074195e93e4f4774bc08f9539c6f4f34042e124805485af0930d51e4ceb +MediaType: application/vnd.docker.distribution.manifest.v2+json +Platform: linux/amd64 +``` + +## Using the container image + +### Setup CAN + +On your native host machine make sure that your CAN interface is up and running. +We will use that interface within the Docker container. + +```bash +$ ip link set can0 type can bitrate 250000 +$ ip link set can0 up +``` + +If you don't own a can interface can create a virtual can device: + +```bash +$ sudo modprobe vcan +$ sudo ip link add dev vcan0 type vcan +$ sudo ip link set up vcan0 +``` + +Verify using ifconfig: + +```bash +$ ifconfig +vcan0: flags=193 mtu 72 + unspec 00-00-00-00-00-00-00-00-00-00-00-00-00-00-00-00 txqueuelen 1000 (UNSPEC) + RX packets 0 bytes 0 (0.0 B) + RX errors 0 dropped 0 overruns 0 frame 0 + TX packets 0 bytes 0 (0.0 B) + TX errors 0 dropped 0 overruns 0 carrier 0 collisions 0 +``` + +### Running a container + +Either you build your own Docker image using the provided Dockerfile, or you use a [3rd party](https://hub.docker.com/repository/docker/geoffreyvl/canopensocket) provided Docker image. +Following guidelines assume the latter. + +To run the container software you must pass the can interface into the container using the `-e CAN=` option. +To run the process in background/detached mode pass the `-d` option. +Run the container as following: + +```bash +$ docker run --rm --network host -d -e CAN=can0 "geoffreyvl/canopensocket:latest" +``` + +Verify that the container is running: + +```bash +$ docker container ls +``` + +## Interact with the CANopen gateway + +On your host system make sure to have the `canopencomm` application. +It's part of the [CANopenSocket](https://github.com/CANopenNode/CANopenSocket) sources, but you need to compile it from source. +To send commands through your gateway, use syntax according to the example below: + +```bash +$ canopencomm -t localhost -p 6000 +``` + +For more examples visit the [CANopenSocket](https://github.com/CANopenNode/CANopenSocket) repo. + +## Monitoring + +You can use a second terminal to dump the traffic at your can0 interface: + +```bash +$ candump can0 +... + can0 701 [1] 7F + can0 000 [2] 01 01 + can0 181 [2] 00 00 + can0 701 [1] 05 +``` diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/CANopen_8h.html b/Devices/Libraries/Systems/CANopenSocket/docs/CANopen_8h.html new file mode 100755 index 0000000..ddd2687 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/CANopen_8h.html @@ -0,0 +1,200 @@ + + + + + + + +CANopenNode: CANopen.h File Reference + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Data Structures | +Macros | +Functions
+
+
CANopen.h File Reference
CANopen
+
+
+ +

Main CANopenNode file. +More...

+
#include "301/CO_driver.h"
+#include "301/CO_ODinterface.h"
+#include "301/CO_NMT_Heartbeat.h"
+#include "301/CO_HBconsumer.h"
+#include "301/CO_Emergency.h"
+#include "301/CO_SDOserver.h"
+#include "301/CO_SDOclient.h"
+#include "301/CO_SYNC.h"
+#include "301/CO_PDO.h"
+#include "301/CO_TIME.h"
+#include "303/CO_LEDs.h"
+#include "304/CO_GFC.h"
+#include "304/CO_SRDO.h"
+#include "305/CO_LSSslave.h"
+#include "305/CO_LSSmaster.h"
+#include "309/CO_gateway_ascii.h"
+#include "extra/CO_trace.h"
+
+

Go to the source code of this file.

+ + + + + + + + +

+Data Structures

struct  CO_config_t
 CANopen configuration, used with CO_new() More...
 
struct  CO_t
 CANopen object - collection of all CANopenNode objects. More...
 
+ + + + + + + +

+Macros

#define CO_MULTIPLE_OD
 If macro is defined externally, then configuration with multiple object dictionaries will be possible. More...
 
#define CO_USE_GLOBALS
 If macro is defined externally, then global variables for CANopen objects will be used instead of heap. More...
 
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

+Functions

CO_t * CO_new (CO_config_t *config, uint32_t *heapMemoryUsed)
 Create new CANopen object. More...
 
void CO_delete (CO_t *co)
 Delete CANopen object and free memory. More...
 
bool_t CO_isLSSslaveEnabled (CO_t *co)
 Test if LSS slave is enabled. More...
 
CO_ReturnError_t CO_CANinit (CO_t *co, void *CANptr, uint16_t bitRate)
 Initialize CAN driver. More...
 
CO_ReturnError_t CO_LSSinit (CO_t *co, CO_LSS_address_t *lssAddress, uint8_t *pendingNodeID, uint16_t *pendingBitRate)
 Initialize CANopen LSS slave. More...
 
CO_ReturnError_t CO_CANopenInit (CO_t *co, CO_NMT_t *NMT, CO_EM_t *em, const OD_t *od, const OD_entry_t *OD_statusBits, CO_NMT_control_t NMTcontrol, uint16_t firstHBTime_ms, uint16_t SDOserverTimeoutTime_ms, uint16_t SDOclientTimeoutTime_ms, bool_t SDOclientBlockTransfer, uint8_t nodeId)
 Initialize CANopenNode. More...
 
CO_NMT_reset_cmd_t CO_process (CO_t *co, bool_t enableGateway, uint32_t timeDifference_us, uint32_t *timerNext_us)
 Process CANopen objects. More...
 
bool_t CO_process_SYNC (CO_t *co, uint32_t timeDifference_us, uint32_t *timerNext_us)
 Process CANopen SYNC objects. More...
 
void CO_process_RPDO (CO_t *co, bool_t syncWas)
 Process CANopen RPDO objects. More...
 
void CO_process_TPDO (CO_t *co, bool_t syncWas, uint32_t timeDifference_us, uint32_t *timerNext_us)
 Process CANopen TPDO objects. More...
 
void CO_process_SRDO (CO_t *co, uint32_t timeDifference_us, uint32_t *timerNext_us)
 Process CANopen SRDO objects. More...
 
+

Detailed Description

+

Main CANopenNode file.

+
Author
Janez Paternoster
+
+Uwe Kindler
+
Copyright
2010 - 2020 Janez Paternoster
+

This file is part of CANopenNode, an opensource CANopen Stack. Project home page is https://github.com/CANopenNode/CANopenNode. For more information on CANopen see http://www.can-cia.org/.

+

Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. You may obtain a copy of the License at 

http://www.apache.org/licenses/LICENSE-2.0
+

Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License.

+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/CANopen_8h.js b/Devices/Libraries/Systems/CANopenSocket/docs/CANopen_8h.js new file mode 100755 index 0000000..e7ecf2c --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/CANopen_8h.js @@ -0,0 +1,16 @@ +var CANopen_8h = +[ + [ "CO_MULTIPLE_OD", "group__CO__CANopen.html#gae3927c69d6937caf2b67ac49c5e41982", null ], + [ "CO_USE_GLOBALS", "group__CO__CANopen.html#gaab00eab90dbe59885c98831e2d819e56", null ], + [ "CO_new", "group__CO__CANopen.html#gaf6ef29daa2063de90b4799ae795c7027", null ], + [ "CO_delete", "group__CO__CANopen.html#ga7023592c26cf6649aa67bc2c04dfd95d", null ], + [ "CO_isLSSslaveEnabled", "group__CO__CANopen.html#ga4c9143f717e8df279034fb897e39b517", null ], + [ "CO_CANinit", "group__CO__CANopen.html#ga619d9ee70c17464bb819b48b5eddb074", null ], + [ "CO_LSSinit", "group__CO__CANopen.html#ga0e11332fd28af597bc47b21327cdc8a3", null ], + [ "CO_CANopenInit", "group__CO__CANopen.html#ga0b64a860299af6e96f5663419aa6d446", null ], + [ "CO_process", "group__CO__CANopen.html#ga895d7fad40b60aacdac3cb0615729b5e", null ], + [ "CO_process_SYNC", "group__CO__CANopen.html#gaad5c15c3ca475912661f512d37413b12", null ], + [ "CO_process_RPDO", "group__CO__CANopen.html#ga4318848921c35e8bb5a7d97dca5668a0", null ], + [ "CO_process_TPDO", "group__CO__CANopen.html#ga8c62a2afd2762d99e9c9be13a3d9a7a8", null ], + [ "CO_process_SRDO", "group__CO__CANopen.html#gab76d7283fe5190d3a0009b423a9ba8b1", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/CANopen_8h_source.html b/Devices/Libraries/Systems/CANopenSocket/docs/CANopen_8h_source.html new file mode 100755 index 0000000..eaa5c0b --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/CANopen_8h_source.html @@ -0,0 +1,579 @@ + + + + + + + +CANopenNode: CANopen.h Source File + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+
CANopen.h
+
+
+Go to the documentation of this file.
1 
+
28 #ifndef CANopen_H
+
29 #define CANopen_H
+
30 
+
31 #include "301/CO_driver.h"
+
32 #include "301/CO_ODinterface.h"
+
33 #include "301/CO_NMT_Heartbeat.h"
+
34 
+
35 #if ((CO_CONFIG_HB_CONS) & CO_CONFIG_HB_CONS_ENABLE) || defined CO_DOXYGEN
+
36 #include "301/CO_HBconsumer.h"
+
37 #endif
+
38 
+
39 #include "301/CO_Emergency.h"
+
40 #include "301/CO_SDOserver.h"
+
41 
+
42 #if ((CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_ENABLE) || defined CO_DOXYGEN
+
43 #include "301/CO_SDOclient.h"
+
44 #endif
+
45 
+
46 #if ((CO_CONFIG_SYNC) & CO_CONFIG_SYNC_ENABLE) || defined CO_DOXYGEN
+
47 #include "301/CO_SYNC.h"
+
48 #endif
+
49 
+
50 #if ((CO_CONFIG_PDO) & (CO_CONFIG_RPDO_ENABLE | CO_CONFIG_TPDO_ENABLE)) || defined CO_DOXYGEN
+
51 #include "301/CO_PDO.h"
+
52 #endif
+
53 
+
54 #if ((CO_CONFIG_TIME) & CO_CONFIG_TIME_ENABLE) || defined CO_DOXYGEN
+
55 #include "301/CO_TIME.h"
+
56 #endif
+
57 
+
58 #if ((CO_CONFIG_LEDS) & CO_CONFIG_LEDS_ENABLE) || defined CO_DOXYGEN
+
59 #include "303/CO_LEDs.h"
+
60 #endif
+
61 
+
62 #if ((CO_CONFIG_GFC) & CO_CONFIG_GFC_ENABLE) || defined CO_DOXYGEN
+
63 #include "304/CO_GFC.h"
+
64 #endif
+
65 
+
66 #if ((CO_CONFIG_SRDO) & CO_CONFIG_SRDO_ENABLE) || defined CO_DOXYGEN
+
67 #include "304/CO_SRDO.h"
+
68 #endif
+
69 
+
70 #if ((CO_CONFIG_LSS) & CO_CONFIG_LSS_SLAVE) || defined CO_DOXYGEN
+
71 #include "305/CO_LSSslave.h"
+
72 #endif
+
73 
+
74 #if ((CO_CONFIG_LSS) & CO_CONFIG_LSS_MASTER) || defined CO_DOXYGEN
+
75 #include "305/CO_LSSmaster.h"
+
76 #endif
+
77 
+
78 #if ((CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII) || defined CO_DOXYGEN
+
79 #include "309/CO_gateway_ascii.h"
+
80 #endif
+
81 
+
82 #if ((CO_CONFIG_TRACE) & CO_CONFIG_TRACE_ENABLE) || defined CO_DOXYGEN
+
83 #include "extra/CO_trace.h"
+
84 #endif
+
85 
+
86 
+
87 #ifdef __cplusplus
+
88 extern "C" {
+
89 #endif
+
90 
+
206 #ifdef CO_DOXYGEN
+
207 #define CO_MULTIPLE_OD
+
208 #endif
+
209 
+
215 #ifdef CO_DOXYGEN
+
216 #define CO_USE_GLOBALS
+
217 #endif
+
218 
+
219 
+
220 #if defined CO_MULTIPLE_OD || defined CO_DOXYGEN
+
221 
+
227 typedef struct {
+
232  uint8_t CNT_NMT;
+
233  const OD_entry_t *ENTRY_H1017;
+
237  uint8_t CNT_HB_CONS;
+
238  const OD_entry_t *ENTRY_H1016;
+
242  uint8_t CNT_EM;
+
243  const OD_entry_t *ENTRY_H1001;
+
244  const OD_entry_t *ENTRY_H1014;
+
245  const OD_entry_t *ENTRY_H1015;
+
246  const OD_entry_t *ENTRY_H1003;
+
249  uint8_t CNT_SDO_SRV;
+
250  const OD_entry_t *ENTRY_H1200;
+
252  uint8_t CNT_SDO_CLI;
+
253  const OD_entry_t *ENTRY_H1280;
+
256  uint8_t CNT_TIME;
+
257  const OD_entry_t *ENTRY_H1012;
+
260  uint8_t CNT_SYNC;
+
261  const OD_entry_t *ENTRY_H1005;
+
262  const OD_entry_t *ENTRY_H1006;
+
263  const OD_entry_t *ENTRY_H1007;
+
264  const OD_entry_t *ENTRY_H1019;
+
266  uint16_t CNT_RPDO;
+
267  const OD_entry_t *ENTRY_H1400;
+
268  const OD_entry_t *ENTRY_H1600;
+
270  uint16_t CNT_TPDO;
+
271  const OD_entry_t *ENTRY_H1800;
+
272  const OD_entry_t *ENTRY_H1A00;
+
274  uint8_t CNT_LEDS;
+
276  uint8_t CNT_GFC;
+
277  const OD_entry_t *ENTRY_H1300;
+
279  uint8_t CNT_SRDO;
+
280  const OD_entry_t *ENTRY_H1301;
+
281  const OD_entry_t *ENTRY_H1381;
+
282  const OD_entry_t *ENTRY_H13FE;
+
283  const OD_entry_t *ENTRY_H13FF;
+
285  uint8_t CNT_LSS_SLV;
+
287  uint8_t CNT_LSS_MST;
+
289  uint8_t CNT_GTWA;
+
291  uint16_t CNT_TRACE;
+
292 } CO_config_t;
+
293 #else
+
294 typedef void CO_config_t;
+
295 #endif /* CO_MULTIPLE_OD */
+
296 
+
297 
+
301 typedef struct {
+
302  bool_t nodeIdUnconfigured;
+
303  #if defined CO_MULTIPLE_OD || defined CO_DOXYGEN
+
304  CO_config_t *config;
+
305  #endif
+
306 
+
307  CO_CANmodule_t *CANmodule;
+
308  CO_CANrx_t *CANrx;
+
309  CO_CANtx_t *CANtx;
+
310  #if defined CO_MULTIPLE_OD || defined CO_DOXYGEN
+
311  uint16_t CNT_ALL_RX_MSGS;
+
312  uint16_t CNT_ALL_TX_MSGS;
+
313  #endif
+
314 
+
315  CO_NMT_t *NMT;
+
316  #if defined CO_MULTIPLE_OD || defined CO_DOXYGEN
+
317  uint16_t RX_IDX_NMT_SLV;
+
318  uint16_t TX_IDX_NMT_MST;
+
319  uint16_t TX_IDX_HB_PROD;
+
320  #endif
+
321 #if ((CO_CONFIG_HB_CONS) & CO_CONFIG_HB_CONS_ENABLE) || defined CO_DOXYGEN
+
322 
+
323  CO_HBconsumer_t *HBcons;
+
324  #if defined CO_MULTIPLE_OD || defined CO_DOXYGEN
+
325  uint16_t RX_IDX_HB_CONS;
+
326  #endif
+
327 #endif
+
328 
+
329  CO_EM_t *em;
+
330  #if defined CO_MULTIPLE_OD || defined CO_DOXYGEN
+
331  uint16_t RX_IDX_EM_CONS;
+
332  uint16_t TX_IDX_EM_PROD;
+
333  #endif
+
334 
+
335  CO_SDOserver_t *SDOserver;
+
336  #if defined CO_MULTIPLE_OD || defined CO_DOXYGEN
+
337  uint16_t RX_IDX_SDO_SRV;
+
338  uint16_t TX_IDX_SDO_SRV;
+
339  #endif
+
340 #if ((CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_ENABLE) || defined CO_DOXYGEN
+
341 
+
342  CO_SDOclient_t *SDOclient;
+
343  #if defined CO_MULTIPLE_OD || defined CO_DOXYGEN
+
344  uint16_t RX_IDX_SDO_CLI;
+
345  uint16_t TX_IDX_SDO_CLI;
+
346  #endif
+
347 #endif
+
348 #if ((CO_CONFIG_TIME) & CO_CONFIG_TIME_ENABLE) || defined CO_DOXYGEN
+
349 
+
350  CO_TIME_t *TIME;
+
351  #if defined CO_MULTIPLE_OD || defined CO_DOXYGEN
+
352  uint16_t RX_IDX_TIME;
+
353  uint16_t TX_IDX_TIME;
+
354  #endif
+
355 #endif
+
356 #if ((CO_CONFIG_SYNC) & CO_CONFIG_SYNC_ENABLE) || defined CO_DOXYGEN
+
357 
+
358  CO_SYNC_t *SYNC;
+
359  #if defined CO_MULTIPLE_OD || defined CO_DOXYGEN
+
360  uint16_t RX_IDX_SYNC;
+
361  uint16_t TX_IDX_SYNC;
+
362  #endif
+
363 #endif
+
364 #if ((CO_CONFIG_PDO) & CO_CONFIG_RPDO_ENABLE) || defined CO_DOXYGEN
+
365 
+
366  CO_RPDO_t *RPDO;
+
367  #if defined CO_MULTIPLE_OD || defined CO_DOXYGEN
+
368  uint16_t RX_IDX_RPDO;
+
369  #endif
+
370 #endif
+
371 #if ((CO_CONFIG_PDO) & CO_CONFIG_TPDO_ENABLE) || defined CO_DOXYGEN
+
372 
+
373  CO_TPDO_t *TPDO;
+
374  #if defined CO_MULTIPLE_OD || defined CO_DOXYGEN
+
375  uint16_t TX_IDX_TPDO;
+
376  #endif
+
377 #endif
+
378 #if ((CO_CONFIG_LEDS) & CO_CONFIG_LEDS_ENABLE) || defined CO_DOXYGEN
+
379 
+
380  CO_LEDs_t *LEDs;
+
381 #endif
+
382 #if ((CO_CONFIG_GFC) & CO_CONFIG_GFC_ENABLE) || defined CO_DOXYGEN
+
383 
+
384  CO_GFC_t *GFC;
+
385  #if defined CO_MULTIPLE_OD || defined CO_DOXYGEN
+
386  uint16_t RX_IDX_GFC;
+
387  uint16_t TX_IDX_GFC;
+
388  #endif
+
389 #endif
+
390 #if ((CO_CONFIG_SRDO) & CO_CONFIG_SRDO_ENABLE) || defined CO_DOXYGEN
+
391 
+
393  CO_SRDOGuard_t *SRDOGuard;
+
395  CO_SRDO_t *SRDO;
+
396  #if defined CO_MULTIPLE_OD || defined CO_DOXYGEN
+
397  uint16_t RX_IDX_SRDO;
+
398  uint16_t TX_IDX_SRDO;
+
399  #endif
+
400 #endif
+
401 #if ((CO_CONFIG_LSS) & CO_CONFIG_LSS_SLAVE) || defined CO_DOXYGEN
+
402 
+
403  CO_LSSslave_t *LSSslave;
+
404  #if defined CO_MULTIPLE_OD || defined CO_DOXYGEN
+
405  uint16_t RX_IDX_LSS_SLV;
+
406  uint16_t TX_IDX_LSS_SLV;
+
407  #endif
+
408 #endif
+
409 #if ((CO_CONFIG_LSS) & CO_CONFIG_LSS_MASTER) || defined CO_DOXYGEN
+
410 
+
411  CO_LSSmaster_t *LSSmaster;
+
412  #if defined CO_MULTIPLE_OD || defined CO_DOXYGEN
+
413  uint16_t RX_IDX_LSS_MST;
+
414  uint16_t TX_IDX_LSS_MST;
+
415  #endif
+
416 #endif
+
417 #if ((CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII) || defined CO_DOXYGEN
+
418 
+
419  CO_GTWA_t *gtwa;
+
420  #if defined CO_MULTIPLE_OD || defined CO_DOXYGEN
+
421  #endif
+
422 #endif
+
423 #if ((CO_CONFIG_TRACE) & CO_CONFIG_TRACE_ENABLE) || defined CO_DOXYGEN
+
424 
+
425  CO_trace_t *trace;
+
426 #endif
+
427 } CO_t;
+
428 
+
429 
+
449 CO_t *CO_new(CO_config_t *config, uint32_t *heapMemoryUsed);
+
450 
+
451 
+
457 void CO_delete(CO_t *co);
+
458 
+
459 
+
467 bool_t CO_isLSSslaveEnabled(CO_t *co);
+
468 
+
469 
+
481 CO_ReturnError_t CO_CANinit(CO_t *co, void *CANptr, uint16_t bitRate);
+
482 
+
483 
+
484 #if ((CO_CONFIG_LSS) & CO_CONFIG_LSS_SLAVE) || defined CO_DOXYGEN
+
485 
+
499 CO_ReturnError_t CO_LSSinit(CO_t *co,
+
500  CO_LSS_address_t *lssAddress,
+
501  uint8_t *pendingNodeID,
+
502  uint16_t *pendingBitRate);
+
503 #endif
+
504 
+
505 
+
534 CO_ReturnError_t CO_CANopenInit(CO_t *co,
+
535  CO_NMT_t *NMT,
+
536  CO_EM_t *em,
+
537  const OD_t *od,
+
538  const OD_entry_t *OD_statusBits,
+
539  CO_NMT_control_t NMTcontrol,
+
540  uint16_t firstHBTime_ms,
+
541  uint16_t SDOserverTimeoutTime_ms,
+
542  uint16_t SDOclientTimeoutTime_ms,
+
543  bool_t SDOclientBlockTransfer,
+
544  uint8_t nodeId);
+
545 
+
546 
+
569 CO_NMT_reset_cmd_t CO_process(CO_t *co,
+
570  bool_t enableGateway,
+
571  uint32_t timeDifference_us,
+
572  uint32_t *timerNext_us);
+
573 
+
574 
+
575 #if ((CO_CONFIG_SYNC) & CO_CONFIG_SYNC_ENABLE) || defined CO_DOXYGEN
+
576 
+
590 bool_t CO_process_SYNC(CO_t *co,
+
591  uint32_t timeDifference_us,
+
592  uint32_t *timerNext_us);
+
593 #endif
+
594 
+
595 
+
596 #if ((CO_CONFIG_PDO) & CO_CONFIG_RPDO_ENABLE) || defined CO_DOXYGEN
+
597 
+
608 void CO_process_RPDO(CO_t *co, bool_t syncWas);
+
609 #endif
+
610 
+
611 
+
612 #if ((CO_CONFIG_PDO) & CO_CONFIG_TPDO_ENABLE) || defined CO_DOXYGEN
+
613 
+
627 void CO_process_TPDO(CO_t *co,
+
628  bool_t syncWas,
+
629  uint32_t timeDifference_us,
+
630  uint32_t *timerNext_us);
+
631 #endif
+
632 
+
633 
+
634 #if ((CO_CONFIG_SRDO) & CO_CONFIG_SRDO_ENABLE) || defined CO_DOXYGEN
+
635 
+
647 void CO_process_SRDO(CO_t *co,
+
648  uint32_t timeDifference_us,
+
649  uint32_t *timerNext_us);
+
650 #endif
+
651  /* CO_CANopen */
+
653 
+
654 #ifdef __cplusplus
+
655 }
+
656 #endif /* __cplusplus */
+
657 
+
658 #endif /* CANopen_H */
+
+
+
CO_config_t::CNT_GFC
uint8_t CNT_GFC
Number of GFC objects, 0 or 1 (CANrx + CANtx).
Definition: CANopen.h:276
+
CO_new
CO_t * CO_new(CO_config_t *config, uint32_t *heapMemoryUsed)
Create new CANopen object.
+
uint32_t
unsigned long int uint32_t
UNSIGNED32 in CANopen (0007h), 32-bit unsigned integer.
Definition: CO_driver.h:155
+
CO_t::TX_IDX_SDO_SRV
uint16_t TX_IDX_SDO_SRV
Start index in CANtx.
Definition: CANopen.h:338
+
CO_process
CO_NMT_reset_cmd_t CO_process(CO_t *co, bool_t enableGateway, uint32_t timeDifference_us, uint32_t *timerNext_us)
Process CANopen objects.
+
CO_config_t::ENTRY_H1006
const OD_entry_t * ENTRY_H1006
OD entry for CO_SYNC_init()
Definition: CANopen.h:262
+
CO_driver.h
Interface between CAN hardware and CANopenNode.
+
CO_trace.h
CANopen trace object for recording variables over time.
+
CO_t::TX_IDX_GFC
uint16_t TX_IDX_GFC
Start index in CANtx.
Definition: CANopen.h:387
+
CO_config_t::ENTRY_H1014
const OD_entry_t * ENTRY_H1014
OD entry for CO_EM_init()
Definition: CANopen.h:244
+
CO_config_t::ENTRY_H13FF
const OD_entry_t * ENTRY_H13FF
OD entry for CO_SRDOGuard_init()
Definition: CANopen.h:283
+
CO_config_t::CNT_EM
uint8_t CNT_EM
Number of Emergency objects, 0 or 1: optional producer (CANtx) + optional consumer (CANrx),...
Definition: CANopen.h:242
+
CO_config_t::CNT_SYNC
uint8_t CNT_SYNC
Number of SYNC objects, 0 or 1: consumer (CANrx) + optional producer (CANtx), configurable by CO_CONF...
Definition: CANopen.h:260
+
CO_config_t::CNT_HB_CONS
uint8_t CNT_HB_CONS
Number of Heartbeat consumer objects, 0 or 1: object uses from 1 to 127 internal consumers (CANrx),...
Definition: CANopen.h:237
+
CO_t::TX_IDX_SRDO
uint16_t TX_IDX_SRDO
Start index in CANtx.
Definition: CANopen.h:398
+
CO_SDOserver_t
SDO server object.
Definition: CO_SDOserver.h:439
+
CO_config_t::ENTRY_H1381
const OD_entry_t * ENTRY_H1381
OD entry for CO_SRDO_init()
Definition: CANopen.h:281
+
CO_RPDO_t
RPDO object.
Definition: CO_PDO.h:179
+
CO_t::CANtx
CO_CANtx_t * CANtx
CAN transmit message objects.
Definition: CANopen.h:309
+
CO_LEDs_t
LEDs object, initialized by CO_LEDs_init()
Definition: CO_LEDs.h:93
+
CO_config_t::ENTRY_H13FE
const OD_entry_t * ENTRY_H13FE
OD entry for CO_SRDOGuard_init()
Definition: CANopen.h:282
+
CO_trace_t
Trace object.
Definition: CO_trace.h:93
+
CO_t::RX_IDX_EM_CONS
uint16_t RX_IDX_EM_CONS
Start index in CANrx.
Definition: CANopen.h:331
+
CO_config_t::ENTRY_H1005
const OD_entry_t * ENTRY_H1005
OD entry for CO_SYNC_init()
Definition: CANopen.h:261
+
CO_t::TX_IDX_NMT_MST
uint16_t TX_IDX_NMT_MST
Start index in CANtx.
Definition: CANopen.h:318
+
CO_SDOclient_t
SDO client object.
Definition: CO_SDOclient.h:67
+
CO_t::RX_IDX_RPDO
uint16_t RX_IDX_RPDO
Start index in CANrx.
Definition: CANopen.h:368
+
uint16_t
unsigned int uint16_t
UNSIGNED16 in CANopen (0006h), 16-bit unsigned integer.
Definition: CO_driver.h:153
+
CO_delete
void CO_delete(CO_t *co)
Delete CANopen object and free memory.
+
CO_SDOclient.h
CANopen Service Data Object - client protocol.
+
CO_config_t::ENTRY_H1015
const OD_entry_t * ENTRY_H1015
OD entry for CO_EM_init()
Definition: CANopen.h:245
+
CO_t::CANmodule
CO_CANmodule_t * CANmodule
One CAN module object, initialised by CO_CANmodule_init()
Definition: CANopen.h:307
+
CO_config_t::ENTRY_H1600
const OD_entry_t * ENTRY_H1600
OD entry for CO_RPDO_init()
Definition: CANopen.h:268
+
CO_GFC_t
GFC object.
Definition: CO_GFC.h:60
+
CO_t::TX_IDX_TPDO
uint16_t TX_IDX_TPDO
Start index in CANtx.
Definition: CANopen.h:375
+
CO_ReturnError_t
CO_ReturnError_t
Return values of some CANopen functions.
Definition: CO_driver.h:488
+
CO_HBconsumer.h
CANopen Heartbeat consumer protocol.
+
CO_config_t::ENTRY_H1800
const OD_entry_t * ENTRY_H1800
OD entry for CO_TPDO_init()
Definition: CANopen.h:271
+
CO_process_SRDO
void CO_process_SRDO(CO_t *co, uint32_t timeDifference_us, uint32_t *timerNext_us)
Process CANopen SRDO objects.
+
CO_config_t::CNT_LSS_SLV
uint8_t CNT_LSS_SLV
Number of LSSslave objects, 0 or 1 (CANrx + CANtx).
Definition: CANopen.h:285
+
CO_config_t::ENTRY_H1007
const OD_entry_t * ENTRY_H1007
OD entry for CO_SYNC_init()
Definition: CANopen.h:263
+
bool_t
unsigned char bool_t
Boolean data type for general use.
Definition: CO_driver.h:141
+
CO_config_t::CNT_LEDS
uint8_t CNT_LEDS
Number of LEDs objects, 0 or 1.
Definition: CANopen.h:274
+
CO_config_t::CNT_SDO_SRV
uint8_t CNT_SDO_SRV
Number of SDO server objects, from 0 to 128 (CANrx + CANtx).
Definition: CANopen.h:249
+
CO_LSSmaster.h
CANopen Layer Setting Service - master protocol.
+
CO_t::TX_IDX_SYNC
uint16_t TX_IDX_SYNC
Start index in CANtx.
Definition: CANopen.h:361
+
CO_NMT_reset_cmd_t
CO_NMT_reset_cmd_t
Return code from CO_NMT_process() that tells application code what to reset.
Definition: CO_NMT_Heartbeat.h:112
+
CO_config_t::ENTRY_H1016
const OD_entry_t * ENTRY_H1016
OD entry for CO_HBconsumer_init()
Definition: CANopen.h:238
+
CO_config_t
CANopen configuration, used with CO_new()
Definition: CANopen.h:227
+
CO_config_t::ENTRY_H1019
const OD_entry_t * ENTRY_H1019
OD entry for CO_SYNC_init()
Definition: CANopen.h:264
+
CO_t::RX_IDX_TIME
uint16_t RX_IDX_TIME
Start index in CANrx.
Definition: CANopen.h:352
+
CO_t::GFC
CO_GFC_t * GFC
GFC object, initialised by CO_GFC_init()
Definition: CANopen.h:384
+
CO_t::RX_IDX_NMT_SLV
uint16_t RX_IDX_NMT_SLV
Start index in CANrx.
Definition: CANopen.h:317
+
CO_PDO.h
CANopen Process Data Object protocol.
+
CO_t::RX_IDX_HB_CONS
uint16_t RX_IDX_HB_CONS
Start index in CANrx.
Definition: CANopen.h:325
+
CO_t::TX_IDX_HB_PROD
uint16_t TX_IDX_HB_PROD
Start index in CANtx.
Definition: CANopen.h:319
+
CO_NMT_t
NMT consumer and Heartbeat producer object.
Definition: CO_NMT_Heartbeat.h:162
+
CO_SDOserver.h
CANopen Service Data Object - server protocol.
+
CO_isLSSslaveEnabled
bool_t CO_isLSSslaveEnabled(CO_t *co)
Test if LSS slave is enabled.
+
CO_SYNC.h
CANopen Synchronisation protocol.
+
CO_CANinit
CO_ReturnError_t CO_CANinit(CO_t *co, void *CANptr, uint16_t bitRate)
Initialize CAN driver.
+
CO_config_t::ENTRY_H1012
const OD_entry_t * ENTRY_H1012
OD entry for CO_TIME_init()
Definition: CANopen.h:257
+
CO_TIME_t
TIME producer and consumer object.
Definition: CO_TIME.h:99
+
CO_t::SRDOGuard
CO_SRDOGuard_t * SRDOGuard
SRDO object, initialised by CO_SRDOGuard_init(), single SRDOGuard object is included inside all SRDO ...
Definition: CANopen.h:393
+
CO_t::NMT
CO_NMT_t * NMT
NMT and heartbeat object, initialised by CO_NMT_init()
Definition: CANopen.h:315
+
CO_t::RPDO
CO_RPDO_t * RPDO
RPDO objects, initialised by CO_RPDO_init()
Definition: CANopen.h:366
+
CO_t::CNT_ALL_TX_MSGS
uint16_t CNT_ALL_TX_MSGS
Number of all CAN transmit message objects.
Definition: CANopen.h:312
+
CO_LSSslave_t
LSS slave object.
Definition: CO_LSSslave.h:90
+
CO_HBconsumer_t
Heartbeat consumer object.
Definition: CO_HBconsumer.h:138
+
CO_config_t::CNT_NMT
uint8_t CNT_NMT
Number of NMT objects, 0 or 1: NMT slave (CANrx) + Heartbeat producer (CANtx) + optional NMT master (...
Definition: CANopen.h:232
+
CO_GFC.h
CANopen Global fail-safe command protocol.
+
CO_config_t::ENTRY_H1A00
const OD_entry_t * ENTRY_H1A00
OD entry for CO_TPDO_init()
Definition: CANopen.h:272
+
CO_t::LSSmaster
CO_LSSmaster_t * LSSmaster
LSS master object, initialised by CO_LSSmaster_init().
Definition: CANopen.h:411
+
CO_TIME.h
CANopen Time-stamp protocol.
+
CO_config_t::ENTRY_H1200
const OD_entry_t * ENTRY_H1200
OD entry for CO_SDOserver_init()
Definition: CANopen.h:250
+
CO_LSSmaster_t
LSS master object.
Definition: CO_LSSmaster.h:108
+
CO_t::TX_IDX_LSS_MST
uint16_t TX_IDX_LSS_MST
Start index in CANtx.
Definition: CANopen.h:414
+
CO_t::TPDO
CO_TPDO_t * TPDO
TPDO objects, initialised by CO_TPDO_init()
Definition: CANopen.h:373
+
CO_t::TX_IDX_LSS_SLV
uint16_t TX_IDX_LSS_SLV
Start index in CANtx.
Definition: CANopen.h:406
+
CO_config_t::ENTRY_H1301
const OD_entry_t * ENTRY_H1301
OD entry for CO_SRDO_init()
Definition: CANopen.h:280
+
CO_t::trace
CO_trace_t * trace
Trace object, initialised by CO_trace_init().
Definition: CANopen.h:425
+
CO_t::RX_IDX_SYNC
uint16_t RX_IDX_SYNC
Start index in CANrx.
Definition: CANopen.h:360
+
CO_t::SYNC
CO_SYNC_t * SYNC
SYNC object, initialised by CO_SYNC_init()
Definition: CANopen.h:358
+
CO_t::SRDO
CO_SRDO_t * SRDO
SRDO objects, initialised by CO_SRDO_init()
Definition: CANopen.h:395
+
CO_t::RX_IDX_LSS_SLV
uint16_t RX_IDX_LSS_SLV
Start index in CANrx.
Definition: CANopen.h:405
+
CO_t::RX_IDX_SRDO
uint16_t RX_IDX_SRDO
Start index in CANrx.
Definition: CANopen.h:397
+
CO_config_t::ENTRY_H1017
const OD_entry_t * ENTRY_H1017
OD entry for CO_NMT_init()
Definition: CANopen.h:233
+
CO_config_t::CNT_TRACE
uint16_t CNT_TRACE
Number of trace objects, 0 or more.
Definition: CANopen.h:291
+
CO_EM_t
Emergency object.
Definition: CO_Emergency.h:369
+
CO_t::CANrx
CO_CANrx_t * CANrx
CAN receive message objects.
Definition: CANopen.h:308
+
CO_t::RX_IDX_GFC
uint16_t RX_IDX_GFC
Start index in CANrx.
Definition: CANopen.h:386
+
CO_config_t::ENTRY_H1280
const OD_entry_t * ENTRY_H1280
OD entry for CO_SDOclient_init()
Definition: CANopen.h:253
+
CO_NMT_Heartbeat.h
CANopen Network management and Heartbeat producer protocol.
+
CO_t::RX_IDX_SDO_SRV
uint16_t RX_IDX_SDO_SRV
Start index in CANrx.
Definition: CANopen.h:337
+
CO_t::CNT_ALL_RX_MSGS
uint16_t CNT_ALL_RX_MSGS
Number of all CAN receive message objects.
Definition: CANopen.h:311
+
CO_ODinterface.h
CANopen Object Dictionary interface.
+
CO_process_SYNC
bool_t CO_process_SYNC(CO_t *co, uint32_t timeDifference_us, uint32_t *timerNext_us)
Process CANopen SYNC objects.
+
CO_config_t::CNT_LSS_MST
uint8_t CNT_LSS_MST
Number of LSSmaster objects, 0 or 1 (CANrx + CANtx).
Definition: CANopen.h:287
+
CO_Emergency.h
CANopen Emergency protocol.
+
CO_t::config
CO_config_t * config
Remember the configuration parameters.
Definition: CANopen.h:304
+
CO_config_t::CNT_TPDO
uint16_t CNT_TPDO
Number of TPDO objects, from 0 to 512 producers (CANtx)
Definition: CANopen.h:270
+
OD_t
Object Dictionary.
Definition: CO_ODinterface.h:352
+
CO_t::gtwa
CO_GTWA_t * gtwa
Gateway-ascii object, initialised by CO_GTWA_init().
Definition: CANopen.h:419
+
CO_CANrx_t
Configuration object for CAN received message for specific CANopenNode Object.
Definition: CO_driver.h:257
+
CO_t::em
CO_EM_t * em
Emergency object, initialised by CO_EM_init()
Definition: CANopen.h:329
+
CO_config_t::ENTRY_H1001
const OD_entry_t * ENTRY_H1001
OD entry for CO_EM_init()
Definition: CANopen.h:243
+
CO_config_t::ENTRY_H1400
const OD_entry_t * ENTRY_H1400
OD entry for CO_RPDO_init()
Definition: CANopen.h:267
+
CO_config_t::CNT_SDO_CLI
uint8_t CNT_SDO_CLI
Number of SDO client objects, from 0 to 128 (CANrx + CANtx).
Definition: CANopen.h:252
+
CO_t::TX_IDX_TIME
uint16_t TX_IDX_TIME
Start index in CANtx.
Definition: CANopen.h:353
+
CO_t::TX_IDX_SDO_CLI
uint16_t TX_IDX_SDO_CLI
Start index in CANtx.
Definition: CANopen.h:345
+
CO_CANmodule_t
Complete CAN module object.
Definition: CO_driver.h:319
+
CO_t::TX_IDX_EM_PROD
uint16_t TX_IDX_EM_PROD
Start index in CANtx.
Definition: CANopen.h:332
+
CO_gateway_ascii.h
CANopen access from other networks - ASCII mapping (CiA 309-3 DS v3.0.0)
+
CO_SRDO_t
SRDO object.
Definition: CO_SRDO.h:124
+
CO_config_t::ENTRY_H1300
const OD_entry_t * ENTRY_H1300
OD entry for CO_GFC_init()
Definition: CANopen.h:277
+
CO_t::RX_IDX_SDO_CLI
uint16_t RX_IDX_SDO_CLI
Start index in CANrx.
Definition: CANopen.h:344
+
OD_entry_t
Object Dictionary entry for one OD object.
Definition: CO_ODinterface.h:336
+
CO_SRDOGuard_t
Gurad Object for SRDO monitors:
Definition: CO_SRDO.h:114
+
CO_t::SDOserver
CO_SDOserver_t * SDOserver
SDO server objects, initialised by CO_SDOserver_init()
Definition: CANopen.h:335
+
CO_t
CANopen object - collection of all CANopenNode objects.
Definition: CANopen.h:301
+
CO_SRDO.h
CANopen Safety Related Data Object protocol.
+
CO_LEDs.h
CANopen Indicator specification (CiA 303-3 v1.4.0)
+
CO_CANopenInit
CO_ReturnError_t CO_CANopenInit(CO_t *co, CO_NMT_t *NMT, CO_EM_t *em, const OD_t *od, const OD_entry_t *OD_statusBits, CO_NMT_control_t NMTcontrol, uint16_t firstHBTime_ms, uint16_t SDOserverTimeoutTime_ms, uint16_t SDOclientTimeoutTime_ms, bool_t SDOclientBlockTransfer, uint8_t nodeId)
Initialize CANopenNode.
+
CO_SYNC_t
SYNC producer and consumer object.
Definition: CO_SYNC.h:75
+
CO_t::LEDs
CO_LEDs_t * LEDs
LEDs object, initialised by CO_LEDs_init()
Definition: CANopen.h:380
+
CO_t::nodeIdUnconfigured
bool_t nodeIdUnconfigured
True in un-configured LSS slave.
Definition: CANopen.h:302
+
CO_t::SDOclient
CO_SDOclient_t * SDOclient
SDO client objects, initialised by CO_SDOclient_init()
Definition: CANopen.h:342
+
CO_config_t::CNT_GTWA
uint8_t CNT_GTWA
Number of gateway ascii objects, 0 or 1.
Definition: CANopen.h:289
+
CO_t::HBcons
CO_HBconsumer_t * HBcons
Heartbeat consumer object, initialised by CO_HBconsumer_init()
Definition: CANopen.h:323
+
CO_t::RX_IDX_LSS_MST
uint16_t RX_IDX_LSS_MST
Start index in CANrx.
Definition: CANopen.h:413
+
CO_NMT_control_t
CO_NMT_control_t
NMT control bitfield for NMT internal state.
Definition: CO_NMT_Heartbeat.h:135
+
CO_t::TIME
CO_TIME_t * TIME
TIME object, initialised by CO_TIME_init()
Definition: CANopen.h:350
+
CO_LSS_address_t
The LSS address is a 128 bit number, uniquely identifying each node.
Definition: CO_LSS.h:162
+
CO_config_t::CNT_SRDO
uint8_t CNT_SRDO
Number of SRDO objects, from 0 to 64 (2*CANrx + 2*CANtx).
Definition: CANopen.h:279
+
CO_t::LSSslave
CO_LSSslave_t * LSSslave
LSS slave object, initialised by CO_LSSslave_init().
Definition: CANopen.h:403
+
CO_process_RPDO
void CO_process_RPDO(CO_t *co, bool_t syncWas)
Process CANopen RPDO objects.
+
CO_LSSslave.h
CANopen Layer Setting Service - slave protocol.
+
CO_config_t::ENTRY_H1003
const OD_entry_t * ENTRY_H1003
OD entry for CO_EM_init()
Definition: CANopen.h:246
+
CO_CANtx_t
Configuration object for CAN transmit message for specific CANopenNode Object.
Definition: CO_driver.h:299
+
CO_TPDO_t
TPDO object.
Definition: CO_PDO.h:220
+
CO_process_TPDO
void CO_process_TPDO(CO_t *co, bool_t syncWas, uint32_t timeDifference_us, uint32_t *timerNext_us)
Process CANopen TPDO objects.
+
CO_GTWA_t
CANopen Gateway-ascii object.
Definition: CO_gateway_ascii.h:296
+
uint8_t
unsigned char uint8_t
UNSIGNED8 in CANopen (0005h), 8-bit unsigned integer.
Definition: CO_driver.h:151
+
CO_LSSinit
CO_ReturnError_t CO_LSSinit(CO_t *co, CO_LSS_address_t *lssAddress, uint8_t *pendingNodeID, uint16_t *pendingBitRate)
Initialize CANopen LSS slave.
+
CO_config_t::CNT_RPDO
uint16_t CNT_RPDO
Number of RPDO objects, from 0 to 512 consumers (CANrx)
Definition: CANopen.h:266
+
CO_config_t::CNT_TIME
uint8_t CNT_TIME
Number of TIME objects, 0 or 1: consumer (CANrx) + optional producer (CANtx), configurable by CO_CONF...
Definition: CANopen.h:256
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/CO__Emergency_8h.html b/Devices/Libraries/Systems/CANopenSocket/docs/CO__Emergency_8h.html new file mode 100755 index 0000000..ffe2801 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/CO__Emergency_8h.html @@ -0,0 +1,316 @@ + + + + + + + +CANopenNode: 301/CO_Emergency.h File Reference + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Data Structures | +Macros | +Enumerations | +Functions
+
+
CO_Emergency.h File Reference
CANopen_301 » Emergency
+
+
+ +

CANopen Emergency protocol. +More...

+
#include "301/CO_driver.h"
+#include "301/CO_ODinterface.h"
+
+

Go to the source code of this file.

+ + + + + +

+Data Structures

struct  CO_EM_t
 Emergency object. More...
 
+ + + + + + + +

+Macros

+#define CO_errorReport(em, errorBit, errorCode, infoCode)   CO_error(em, true, errorBit, errorCode, infoCode)
 Report error condition, for description of parameters see CO_error.
 
+#define CO_errorReset(em, errorBit, infoCode)   CO_error(em, false, errorBit, CO_EMC_NO_ERROR, infoCode)
 Reset error condition, for description of parameters see CO_error.
 
+ + + + + + + + + + +

+Enumerations

enum  CO_errorRegister_t {
+  CO_ERR_REG_GENERIC_ERR = 0x01U, +CO_ERR_REG_CURRENT = 0x02U, +CO_ERR_REG_VOLTAGE = 0x04U, +CO_ERR_REG_TEMPERATURE = 0x08U, +
+  CO_ERR_REG_COMMUNICATION = 0x10U, +CO_ERR_REG_DEV_PROFILE = 0x20U, +CO_ERR_REG_RESERVED = 0x40U, +CO_ERR_REG_MANUFACTURER = 0x80U +
+ }
 CANopen Error register. More...
 
enum  CO_EM_errorCode_t {
+  CO_EMC_NO_ERROR = 0x0000U, +CO_EMC_GENERIC = 0x1000U, +CO_EMC_CURRENT = 0x2000U, +CO_EMC_CURRENT_INPUT = 0x2100U, +
+  CO_EMC_CURRENT_INSIDE = 0x2200U, +CO_EMC_CURRENT_OUTPUT = 0x2300U, +CO_EMC_VOLTAGE = 0x3000U, +CO_EMC_VOLTAGE_MAINS = 0x3100U, +
+  CO_EMC_VOLTAGE_INSIDE = 0x3200U, +CO_EMC_VOLTAGE_OUTPUT = 0x3300U, +CO_EMC_TEMPERATURE = 0x4000U, +CO_EMC_TEMP_AMBIENT = 0x4100U, +
+  CO_EMC_TEMP_DEVICE = 0x4200U, +CO_EMC_HARDWARE = 0x5000U, +CO_EMC_SOFTWARE_DEVICE = 0x6000U, +CO_EMC_SOFTWARE_INTERNAL = 0x6100U, +
+  CO_EMC_SOFTWARE_USER = 0x6200U, +CO_EMC_DATA_SET = 0x6300U, +CO_EMC_ADDITIONAL_MODUL = 0x7000U, +CO_EMC_MONITORING = 0x8000U, +
+  CO_EMC_COMMUNICATION = 0x8100U, +CO_EMC_CAN_OVERRUN = 0x8110U, +CO_EMC_CAN_PASSIVE = 0x8120U, +CO_EMC_HEARTBEAT = 0x8130U, +
+  CO_EMC_BUS_OFF_RECOVERED = 0x8140U, +CO_EMC_CAN_ID_COLLISION = 0x8150U, +CO_EMC_PROTOCOL_ERROR = 0x8200U, +CO_EMC_PDO_LENGTH = 0x8210U, +
+  CO_EMC_PDO_LENGTH_EXC = 0x8220U, +CO_EMC_DAM_MPDO = 0x8230U, +CO_EMC_SYNC_DATA_LENGTH = 0x8240U, +CO_EMC_RPDO_TIMEOUT = 0x8250U, +
+  CO_EMC_TIME_DATA_LENGTH = 0x8260U, +CO_EMC_EXTERNAL_ERROR = 0x9000U, +CO_EMC_ADDITIONAL_FUNC = 0xF000U, +CO_EMC_DEVICE_SPECIFIC = 0xFF00U, +
+  CO_EMC401_OUT_CUR_HI = 0x2310U, +CO_EMC401_OUT_SHORTED = 0x2320U, +CO_EMC401_OUT_LOAD_DUMP = 0x2330U, +CO_EMC401_IN_VOLT_HI = 0x3110U, +
+  CO_EMC401_IN_VOLT_LOW = 0x3120U, +CO_EMC401_INTERN_VOLT_HI = 0x3210U, +CO_EMC401_INTERN_VOLT_LO = 0x3220U, +CO_EMC401_OUT_VOLT_HIGH = 0x3310U, +
+  CO_EMC401_OUT_VOLT_LOW = 0x3320U +
+ }
 CANopen Error code. More...
 
enum  CO_EM_errorStatusBits_t {
+  CO_EM_NO_ERROR = 0x00U, +CO_EM_CAN_BUS_WARNING = 0x01U, +CO_EM_RXMSG_WRONG_LENGTH = 0x02U, +CO_EM_RXMSG_OVERFLOW = 0x03U, +
+  CO_EM_RPDO_WRONG_LENGTH = 0x04U, +CO_EM_RPDO_OVERFLOW = 0x05U, +CO_EM_CAN_RX_BUS_PASSIVE = 0x06U, +CO_EM_CAN_TX_BUS_PASSIVE = 0x07U, +
+  CO_EM_NMT_WRONG_COMMAND = 0x08U, +CO_EM_TIME_TIMEOUT = 0x09U, +CO_EM_TIME_LENGTH = 0x0AU, +CO_EM_0B_unused = 0x0BU, +
+  CO_EM_0C_unused = 0x0CU, +CO_EM_0D_unused = 0x0DU, +CO_EM_0E_unused = 0x0EU, +CO_EM_0F_unused = 0x0FU, +
+  CO_EM_10_unused = 0x10U, +CO_EM_11_unused = 0x11U, +CO_EM_CAN_TX_BUS_OFF = 0x12U, +CO_EM_CAN_RXB_OVERFLOW = 0x13U, +
+  CO_EM_CAN_TX_OVERFLOW = 0x14U, +CO_EM_TPDO_OUTSIDE_WINDOW = 0x15U, +CO_EM_16_unused = 0x16U, +CO_EM_17_unused = 0x17U, +
+  CO_EM_SYNC_TIME_OUT = 0x18U, +CO_EM_SYNC_LENGTH = 0x19U, +CO_EM_PDO_WRONG_MAPPING = 0x1AU, +CO_EM_HEARTBEAT_CONSUMER = 0x1BU, +
+  CO_EM_HB_CONSUMER_REMOTE_RESET = 0x1CU, +CO_EM_1D_unused = 0x1DU, +CO_EM_1E_unused = 0x1EU, +CO_EM_1F_unused = 0x1FU, +
+  CO_EM_EMERGENCY_BUFFER_FULL = 0x20U, +CO_EM_21_unused = 0x21U, +CO_EM_MICROCONTROLLER_RESET = 0x22U, +CO_EM_23_unused = 0x23U, +
+  CO_EM_24_unused = 0x24U, +CO_EM_25_unused = 0x25U, +CO_EM_26_unused = 0x26U, +CO_EM_27_unused = 0x27U, +
+  CO_EM_WRONG_ERROR_REPORT = 0x28U, +CO_EM_ISR_TIMER_OVERFLOW = 0x29U, +CO_EM_MEMORY_ALLOCATION_ERROR = 0x2AU, +CO_EM_GENERIC_ERROR = 0x2BU, +
+  CO_EM_GENERIC_SOFTWARE_ERROR = 0x2CU, +CO_EM_INCONSISTENT_OBJECT_DICT = 0x2DU, +CO_EM_CALCULATION_OF_PARAMETERS = 0x2EU, +CO_EM_NON_VOLATILE_MEMORY = 0x2FU, +
+  CO_EM_MANUFACTURER_START = 0x30U, +CO_EM_MANUFACTURER_END = CO_CONFIG_EM_ERR_STATUS_BITS_COUNT - 1 +
+ }
 Error status bits. More...
 
+ + + + + + + + + + + + + + + + + + + + + + +

+Functions

CO_ReturnError_t CO_EM_init (CO_EM_t *em, const OD_entry_t *OD_1001_errReg, const OD_entry_t *OD_1014_cobIdEm, CO_CANmodule_t *CANdevTx, uint16_t CANdevTxIdx, const OD_entry_t *OD_1015_InhTime, const OD_entry_t *OD_1003_preDefErr, const OD_entry_t *OD_statusBits, CO_CANmodule_t *CANdevRx, uint16_t CANdevRxIdx, const uint8_t nodeId)
 Initialize Emergency object. More...
 
void CO_EM_initCallbackPre (CO_EM_t *em, void *object, void(*pFunctSignal)(void *object))
 Initialize Emergency callback function. More...
 
void CO_EM_initCallbackRx (CO_EM_t *em, void(*pFunctSignalRx)(const uint16_t ident, const uint16_t errorCode, const uint8_t errorRegister, const uint8_t errorBit, const uint32_t infoCode))
 Initialize Emergency received callback function. More...
 
void CO_EM_process (CO_EM_t *em, bool_t NMTisPreOrOperational, uint32_t timeDifference_us, uint32_t *timerNext_us)
 Process Error control and Emergency object. More...
 
void CO_error (CO_EM_t *em, bool_t setError, const uint8_t errorBit, uint16_t errorCode, uint32_t infoCode)
 Set or reset error condition. More...
 
static bool_t CO_isError (CO_EM_t *em, const uint8_t errorBit)
 Check specific error condition. More...
 
static uint8_t CO_getErrorRegister (CO_EM_t *em)
 Get error register. More...
 
+

Detailed Description

+

CANopen Emergency protocol.

+
Author
Janez Paternoster
+
Copyright
2020 Janez Paternoster
+

This file is part of CANopenNode, an opensource CANopen Stack. Project home page is https://github.com/CANopenNode/CANopenNode. For more information on CANopen see http://www.can-cia.org/.

+

Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. You may obtain a copy of the License at 

http://www.apache.org/licenses/LICENSE-2.0
+

Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License.

+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/CO__Emergency_8h.js b/Devices/Libraries/Systems/CANopenSocket/docs/CO__Emergency_8h.js new file mode 100755 index 0000000..f5e4f55 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/CO__Emergency_8h.js @@ -0,0 +1,121 @@ +var CO__Emergency_8h = +[ + [ "CO_errorReport", "group__CO__Emergency.html#gab66d4a6daa5f7492704b56a46b135f71", null ], + [ "CO_errorReset", "group__CO__Emergency.html#ga24e2a9311cf704ec6ed43b0ea730c4a3", null ], + [ "CO_errorRegister_t", "group__CO__Emergency.html#ga2cfc261cce03577083ee3f1a31d5e03c", [ + [ "CO_ERR_REG_GENERIC_ERR", "group__CO__Emergency.html#gga2cfc261cce03577083ee3f1a31d5e03ca92a7e121ae04a022fc2fe604eb1c148e", null ], + [ "CO_ERR_REG_CURRENT", "group__CO__Emergency.html#gga2cfc261cce03577083ee3f1a31d5e03ca61eded29fb0fcd95b2f66c2682de0f2b", null ], + [ "CO_ERR_REG_VOLTAGE", "group__CO__Emergency.html#gga2cfc261cce03577083ee3f1a31d5e03ca360c75e04303d1c55e2bc8528407cb87", null ], + [ "CO_ERR_REG_TEMPERATURE", "group__CO__Emergency.html#gga2cfc261cce03577083ee3f1a31d5e03cab12f2b419af0aeb8aae83a13d5c8b7bf", null ], + [ "CO_ERR_REG_COMMUNICATION", "group__CO__Emergency.html#gga2cfc261cce03577083ee3f1a31d5e03ca2f3b7aeac7282281c1d17895406c006a", null ], + [ "CO_ERR_REG_DEV_PROFILE", "group__CO__Emergency.html#gga2cfc261cce03577083ee3f1a31d5e03cab137d1705e9ab20e2caeb22f57dd4716", null ], + [ "CO_ERR_REG_RESERVED", "group__CO__Emergency.html#gga2cfc261cce03577083ee3f1a31d5e03caffdf7f5d9f9ae52fa1bf97a3fb3d848b", null ], + [ "CO_ERR_REG_MANUFACTURER", "group__CO__Emergency.html#gga2cfc261cce03577083ee3f1a31d5e03ca207eee1d9638f61166bc395ee71b84a3", null ] + ] ], + [ "CO_EM_errorCode_t", "group__CO__Emergency.html#ga0653c307fd6bc5238babf48e01c9fa02", [ + [ "CO_EMC_NO_ERROR", "group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02aa60e1333102cbe544eccbaad8e77f6f7", null ], + [ "CO_EMC_GENERIC", "group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a2eaf45ca12b32b7bcc58df91becda767", null ], + [ "CO_EMC_CURRENT", "group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02abad9ca04a37cc43cacabfef9483699cf", null ], + [ "CO_EMC_CURRENT_INPUT", "group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02ab792c971a569d1175666b3fff9ffbe70", null ], + [ "CO_EMC_CURRENT_INSIDE", "group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a3ded1c05cbf37d2d7d286af97e833e65", null ], + [ "CO_EMC_CURRENT_OUTPUT", "group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02ad42e8ab666fd3da75d1fa3a7b8708efc", null ], + [ "CO_EMC_VOLTAGE", "group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a84a8f0dfb97e0ec13be9a4cdb0d71233", null ], + [ "CO_EMC_VOLTAGE_MAINS", "group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02ab4b095d1d9e7e7f5150bc2ecd83bc140", null ], + [ "CO_EMC_VOLTAGE_INSIDE", "group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a78dfa1d496a010ae7ae2e8b6edc1362a", null ], + [ "CO_EMC_VOLTAGE_OUTPUT", "group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a9c5becd591c91bb3e255badf0a308c2d", null ], + [ "CO_EMC_TEMPERATURE", "group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02aa24dfa4c6948187f62d3e8182285d4a3", null ], + [ "CO_EMC_TEMP_AMBIENT", "group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02ae5256d8178374a48750537c3d04c0a30", null ], + [ "CO_EMC_TEMP_DEVICE", "group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a6c29a4b49fae39b45c5c0e553ef6668f", null ], + [ "CO_EMC_HARDWARE", "group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a33344d49b9667151d86aef28a73e6f66", null ], + [ "CO_EMC_SOFTWARE_DEVICE", "group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a6d0b3c0c31228e0bc57fc080c754fefa", null ], + [ "CO_EMC_SOFTWARE_INTERNAL", "group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a62e0949639733e85c2b6d4c8b099d467", null ], + [ "CO_EMC_SOFTWARE_USER", "group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a7b6ae38c015688128890bfe42b0271e5", null ], + [ "CO_EMC_DATA_SET", "group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02ad22329fc3e44867a365401458e691ddc", null ], + [ "CO_EMC_ADDITIONAL_MODUL", "group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02ae210dc1069c7b046527f7d7903ef82cb", null ], + [ "CO_EMC_MONITORING", "group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a8ecd7e45af52d83d986e3de8e957a986", null ], + [ "CO_EMC_COMMUNICATION", "group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02aab2946bf800f551bcae55dd299ff315b", null ], + [ "CO_EMC_CAN_OVERRUN", "group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a1f04b4ffe9cc1d8f2b294261909dec4e", null ], + [ "CO_EMC_CAN_PASSIVE", "group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02aa024c00c21f705474355b9ca7d7ce948", null ], + [ "CO_EMC_HEARTBEAT", "group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02aff47b500e2e760355ca653b247e4b93f", null ], + [ "CO_EMC_BUS_OFF_RECOVERED", "group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a2fd717ed311007b4dd6fe92443f134b0", null ], + [ "CO_EMC_CAN_ID_COLLISION", "group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a683bff5350b0cbab24aef2fc8eac363a", null ], + [ "CO_EMC_PROTOCOL_ERROR", "group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02ab884b23b23af9671d99cca5865549e5a", null ], + [ "CO_EMC_PDO_LENGTH", "group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a80fedd7bbb98ddf1ec26d4b31ed6d749", null ], + [ "CO_EMC_PDO_LENGTH_EXC", "group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a88bac871b7539a579fba73825a2e240a", null ], + [ "CO_EMC_DAM_MPDO", "group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02ab58df03302ab06710f7455d37039dea3", null ], + [ "CO_EMC_SYNC_DATA_LENGTH", "group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a81aa2a66727d1fe29720067dc4e20879", null ], + [ "CO_EMC_RPDO_TIMEOUT", "group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a4ca48c8d1be6a42ac0c13e551e12b230", null ], + [ "CO_EMC_TIME_DATA_LENGTH", "group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a018485be8125a3515ecd127a08e2e2f1", null ], + [ "CO_EMC_EXTERNAL_ERROR", "group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a1d76eff88ebd6050377c393533aebc8d", null ], + [ "CO_EMC_ADDITIONAL_FUNC", "group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02af5f9446049023ceae559562998172278", null ], + [ "CO_EMC_DEVICE_SPECIFIC", "group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02ad7b895b5e7d0f3fa7ff422157ac36c70", null ], + [ "CO_EMC401_OUT_CUR_HI", "group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02aa432d9c66bb0f6eecc38d720cae6c32e", null ], + [ "CO_EMC401_OUT_SHORTED", "group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a182a0c7afc0cb1c30af42a05430da353", null ], + [ "CO_EMC401_OUT_LOAD_DUMP", "group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a21cd31a1455c9dc379796798f0eecd32", null ], + [ "CO_EMC401_IN_VOLT_HI", "group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02aacdc3517e800b037b46c1b54f454b562", null ], + [ "CO_EMC401_IN_VOLT_LOW", "group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a867eb16fce01ade3c728df7c7527e311", null ], + [ "CO_EMC401_INTERN_VOLT_HI", "group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a9cd0f1a897f40a3a43198ba05de4a11b", null ], + [ "CO_EMC401_INTERN_VOLT_LO", "group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02ac122203ed5c6a71749ace599b13ac594", null ], + [ "CO_EMC401_OUT_VOLT_HIGH", "group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02abf5b6a68120351c2fa52146b45798ed2", null ], + [ "CO_EMC401_OUT_VOLT_LOW", "group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a5f262e622db7482b7230055e5b27c902", null ] + ] ], + [ "CO_EM_errorStatusBits_t", "group__CO__Emergency.html#ga587034df9d350c8e121c253f1d4eeacc", [ + [ "CO_EM_NO_ERROR", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeaccafb7b06b4b1d4fb2f9fa8661fdbaf8b01", null ], + [ "CO_EM_CAN_BUS_WARNING", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca27ebb7f155d4b72618c34dd6aa496aac", null ], + [ "CO_EM_RXMSG_WRONG_LENGTH", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeaccae1e45de61059459a6f1f6e500962f287", null ], + [ "CO_EM_RXMSG_OVERFLOW", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca0b17027ee0097065d92e6c0981e3face", null ], + [ "CO_EM_RPDO_WRONG_LENGTH", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca2a36480d4dd6a24f1f8bb66d79441a8d", null ], + [ "CO_EM_RPDO_OVERFLOW", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca73426de91d49273d023b5084a0cea8e0", null ], + [ "CO_EM_CAN_RX_BUS_PASSIVE", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeaccaab5efa11cefb2cd6125cec3ec1c570e1", null ], + [ "CO_EM_CAN_TX_BUS_PASSIVE", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeaccadb8502da626d80a8c423e94e1c76d0cb", null ], + [ "CO_EM_NMT_WRONG_COMMAND", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeaccac5f82aeeda52c83eee0025c8b387ac5d", null ], + [ "CO_EM_TIME_TIMEOUT", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca5c1a6209ebe6167bbf13f565b6fd994d", null ], + [ "CO_EM_TIME_LENGTH", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca3af179820ed2aa88e2c22b7961de71f8", null ], + [ "CO_EM_0B_unused", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca8bf6fb0db21e29e477b38304279bed5e", null ], + [ "CO_EM_0C_unused", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeaccaa724f4fdeff7043b0d4f454613a96992", null ], + [ "CO_EM_0D_unused", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca933c070fd08c1223462a3a331b016c99", null ], + [ "CO_EM_0E_unused", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca0cabb2e45202f938cfdafe8e7871f4f7", null ], + [ "CO_EM_0F_unused", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeaccac6469cc3e6176136f69e549c4a4f5b71", null ], + [ "CO_EM_10_unused", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca274f46ac0760c4c340f48d1de884f2fe", null ], + [ "CO_EM_11_unused", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca9d8abe2f426ed071febf85a932c1df98", null ], + [ "CO_EM_CAN_TX_BUS_OFF", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeaccae59f8e20795915a0929861809ed42e7c", null ], + [ "CO_EM_CAN_RXB_OVERFLOW", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeaccaf9a86c6c3b87763593dd14be6b0bef29", null ], + [ "CO_EM_CAN_TX_OVERFLOW", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca2dbceee7b6deae231bb40a96f8f748a9", null ], + [ "CO_EM_TPDO_OUTSIDE_WINDOW", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeaccaea141284cd85126a9b3e7b0605a26a94", null ], + [ "CO_EM_16_unused", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca865160ae9fdac8fcba1e5335b31c2f9f", null ], + [ "CO_EM_17_unused", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeaccad5bb584bb3c85ca0ba0313367aa75a9b", null ], + [ "CO_EM_SYNC_TIME_OUT", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeaccafd760392f4d4d6358896486c5b5d7d82", null ], + [ "CO_EM_SYNC_LENGTH", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca09a521bfc9ea08ed340cfa29952a471c", null ], + [ "CO_EM_PDO_WRONG_MAPPING", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca7308b487766b8feca60ef0c1b873f167", null ], + [ "CO_EM_HEARTBEAT_CONSUMER", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca6478d414ea45f6a9129e68a9d57e11b7", null ], + [ "CO_EM_HB_CONSUMER_REMOTE_RESET", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca0b6698662476cc622661fb5a5a75ec31", null ], + [ "CO_EM_1D_unused", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca6d0bf9c926241ec8f67c477928300761", null ], + [ "CO_EM_1E_unused", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeaccab2f3562c4e1f8e25a7837627dc1721db", null ], + [ "CO_EM_1F_unused", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeaccadba4afb9dac78f8eb0c5f494926568b1", null ], + [ "CO_EM_EMERGENCY_BUFFER_FULL", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeaccabd1935c51679f70f509ffd60e28c02b1", null ], + [ "CO_EM_21_unused", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeaccadbce7bd9d5a0ee681104914092b21d8d", null ], + [ "CO_EM_MICROCONTROLLER_RESET", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeaccacb69eecc08e72c56aec215fa55e27e16", null ], + [ "CO_EM_23_unused", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeaccaa0c8857afdd8455b30fd0179e98599fb", null ], + [ "CO_EM_24_unused", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca9cf88b48355b3cc43fe9a8360b8470df", null ], + [ "CO_EM_25_unused", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca0a8abc6fcd7b0d5469b469c2cf370a82", null ], + [ "CO_EM_26_unused", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca0398195eafec5f8d60a76f677ce2a714", null ], + [ "CO_EM_27_unused", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca95ca6848349affc579fff2c2a62e87d7", null ], + [ "CO_EM_WRONG_ERROR_REPORT", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca2d7776243205bc75e6c448e13e697480", null ], + [ "CO_EM_ISR_TIMER_OVERFLOW", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeaccadbf7382f537c9f59f965ce38be464e46", null ], + [ "CO_EM_MEMORY_ALLOCATION_ERROR", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca2575dac21ff9ac9c4c4e5ca63d34fdbc", null ], + [ "CO_EM_GENERIC_ERROR", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca21648a2863590d3cccb469f8ef759267", null ], + [ "CO_EM_GENERIC_SOFTWARE_ERROR", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca6c3e7fff310443f05815ea2b7ac6b289", null ], + [ "CO_EM_INCONSISTENT_OBJECT_DICT", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca2c6a108cabca6f03b1400065f2ad4887", null ], + [ "CO_EM_CALCULATION_OF_PARAMETERS", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca5544a90d3047bc08186ea7412528dc93", null ], + [ "CO_EM_NON_VOLATILE_MEMORY", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeaccac019532cacaa8310f9ca413a2f599af3", null ], + [ "CO_EM_MANUFACTURER_START", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeaccaf850a661aadde65b10b22715cf24942c", null ], + [ "CO_EM_MANUFACTURER_END", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca5d3c6fdb77551b3f4aaf993ae1dfb414", null ] + ] ], + [ "CO_EM_init", "group__CO__Emergency.html#ga5b80f59df00b71dca7a5c18c139aa71e", null ], + [ "CO_EM_initCallbackPre", "group__CO__Emergency.html#ga94efd78032de3667e2a89780b08aabed", null ], + [ "CO_EM_initCallbackRx", "group__CO__Emergency.html#ga583245c954327c3cf7f9fdb97854e76b", null ], + [ "CO_EM_process", "group__CO__Emergency.html#ga93ae7be6ef966192f5761ce343345d3b", null ], + [ "CO_error", "group__CO__Emergency.html#ga9221f9f631ead4b6f66cfcff8614ba46", null ], + [ "CO_isError", "group__CO__Emergency.html#ga8e9bae71814a3e7bbd8d59d721174c2b", null ], + [ "CO_getErrorRegister", "group__CO__Emergency.html#gaf0c47186d9e51fb91d48385a9f6bad6b", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/CO__Emergency_8h_source.html b/Devices/Libraries/Systems/CANopenSocket/docs/CO__Emergency_8h_source.html new file mode 100755 index 0000000..3e19534 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/CO__Emergency_8h_source.html @@ -0,0 +1,541 @@ + + + + + + + +CANopenNode: 301/CO_Emergency.h Source File + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+
CO_Emergency.h
+
+
+Go to the documentation of this file.
1 
+
26 #ifndef CO_EMERGENCY_H
+
27 #define CO_EMERGENCY_H
+
28 
+
29 #include "301/CO_driver.h"
+
30 #include "301/CO_ODinterface.h"
+
31 
+
32 /* default configuration, see CO_config.h */
+
33 #ifndef CO_CONFIG_EM
+
34 #define CO_CONFIG_EM (CO_CONFIG_EM_PRODUCER | \
+
35  CO_CONFIG_EM_HISTORY)
+
36 #endif
+
37 #ifndef CO_CONFIG_EM_ERR_STATUS_BITS_COUNT
+
38 #define CO_CONFIG_EM_ERR_STATUS_BITS_COUNT (10*8)
+
39 #endif
+
40 #ifndef CO_CONFIG_EM_BUFFER_SIZE
+
41 #define CO_CONFIG_EM_BUFFER_SIZE 16
+
42 #endif
+
43 #ifndef CO_CONFIG_ERR_CONDITION_GENERIC
+
44 #define CO_CONFIG_ERR_CONDITION_GENERIC (em->errorStatusBits[5] != 0)
+
45 #endif
+
46 #ifndef CO_CONFIG_ERR_CONDITION_COMMUNICATION
+
47 #define CO_CONFIG_ERR_CONDITION_COMMUNICATION (em->errorStatusBits[2] \
+
48  || em->errorStatusBits[3])
+
49 #endif
+
50 #ifndef CO_CONFIG_ERR_CONDITION_MANUFACTURER
+
51 #define CO_CONFIG_ERR_CONDITION_MANUFACTURER (em->errorStatusBits[8] \
+
52  || em->errorStatusBits[9])
+
53 #endif
+
54 
+
55 #ifdef __cplusplus
+
56 extern "C" {
+
57 #endif
+
58 
+
119 typedef enum {
+
120  CO_ERR_REG_GENERIC_ERR = 0x01U,
+
121  CO_ERR_REG_CURRENT = 0x02U,
+
122  CO_ERR_REG_VOLTAGE = 0x04U,
+
123  CO_ERR_REG_TEMPERATURE = 0x08U,
+
124  CO_ERR_REG_COMMUNICATION = 0x10U,
+
125  CO_ERR_REG_DEV_PROFILE = 0x20U,
+
126  CO_ERR_REG_RESERVED = 0x40U,
+
127  CO_ERR_REG_MANUFACTURER = 0x80U
+
128 } CO_errorRegister_t;
+
129 
+
130 
+
136 typedef enum {
+
138  CO_EMC_NO_ERROR = 0x0000U,
+
140  CO_EMC_GENERIC = 0x1000U,
+
142  CO_EMC_CURRENT = 0x2000U,
+
144  CO_EMC_CURRENT_INPUT = 0x2100U,
+
146  CO_EMC_CURRENT_INSIDE = 0x2200U,
+
148  CO_EMC_CURRENT_OUTPUT = 0x2300U,
+
150  CO_EMC_VOLTAGE = 0x3000U,
+
152  CO_EMC_VOLTAGE_MAINS = 0x3100U,
+
154  CO_EMC_VOLTAGE_INSIDE = 0x3200U,
+
156  CO_EMC_VOLTAGE_OUTPUT = 0x3300U,
+
158  CO_EMC_TEMPERATURE = 0x4000U,
+
160  CO_EMC_TEMP_AMBIENT = 0x4100U,
+
162  CO_EMC_TEMP_DEVICE = 0x4200U,
+
164  CO_EMC_HARDWARE = 0x5000U,
+
166  CO_EMC_SOFTWARE_DEVICE = 0x6000U,
+
168  CO_EMC_SOFTWARE_INTERNAL = 0x6100U,
+
170  CO_EMC_SOFTWARE_USER = 0x6200U,
+
172  CO_EMC_DATA_SET = 0x6300U,
+
174  CO_EMC_ADDITIONAL_MODUL = 0x7000U,
+
176  CO_EMC_MONITORING = 0x8000U,
+
178  CO_EMC_COMMUNICATION = 0x8100U,
+
180  CO_EMC_CAN_OVERRUN = 0x8110U,
+
182  CO_EMC_CAN_PASSIVE = 0x8120U,
+
184  CO_EMC_HEARTBEAT = 0x8130U,
+
186  CO_EMC_BUS_OFF_RECOVERED = 0x8140U,
+
188  CO_EMC_CAN_ID_COLLISION = 0x8150U,
+
190  CO_EMC_PROTOCOL_ERROR = 0x8200U,
+
192  CO_EMC_PDO_LENGTH = 0x8210U,
+
194  CO_EMC_PDO_LENGTH_EXC = 0x8220U,
+
196  CO_EMC_DAM_MPDO = 0x8230U,
+
198  CO_EMC_SYNC_DATA_LENGTH = 0x8240U,
+
200  CO_EMC_RPDO_TIMEOUT = 0x8250U,
+
202  CO_EMC_TIME_DATA_LENGTH = 0x8260U,
+
204  CO_EMC_EXTERNAL_ERROR = 0x9000U,
+
206  CO_EMC_ADDITIONAL_FUNC = 0xF000U,
+
208  CO_EMC_DEVICE_SPECIFIC = 0xFF00U,
+
209 
+
211  CO_EMC401_OUT_CUR_HI = 0x2310U,
+
213  CO_EMC401_OUT_SHORTED = 0x2320U,
+
215  CO_EMC401_OUT_LOAD_DUMP = 0x2330U,
+
217  CO_EMC401_IN_VOLT_HI = 0x3110U,
+
219  CO_EMC401_IN_VOLT_LOW = 0x3120U,
+
221  CO_EMC401_INTERN_VOLT_HI = 0x3210U,
+
223  CO_EMC401_INTERN_VOLT_LO = 0x3220U,
+
225  CO_EMC401_OUT_VOLT_HIGH = 0x3310U,
+
227  CO_EMC401_OUT_VOLT_LOW = 0x3320U,
+
228 } CO_EM_errorCode_t;
+
229 
+
230 
+
247 typedef enum {
+
249  CO_EM_NO_ERROR = 0x00U,
+
251  CO_EM_CAN_BUS_WARNING = 0x01U,
+
254  CO_EM_RXMSG_WRONG_LENGTH = 0x02U,
+
257  CO_EM_RXMSG_OVERFLOW = 0x03U,
+
259  CO_EM_RPDO_WRONG_LENGTH = 0x04U,
+
261  CO_EM_RPDO_OVERFLOW = 0x05U,
+
263  CO_EM_CAN_RX_BUS_PASSIVE = 0x06U,
+
265  CO_EM_CAN_TX_BUS_PASSIVE = 0x07U,
+
267  CO_EM_NMT_WRONG_COMMAND = 0x08U,
+
269  CO_EM_TIME_TIMEOUT = 0x09U,
+
271  CO_EM_TIME_LENGTH = 0x0AU,
+
273  CO_EM_0B_unused = 0x0BU,
+
275  CO_EM_0C_unused = 0x0CU,
+
277  CO_EM_0D_unused = 0x0DU,
+
279  CO_EM_0E_unused = 0x0EU,
+
281  CO_EM_0F_unused = 0x0FU,
+
282 
+
284  CO_EM_10_unused = 0x10U,
+
286  CO_EM_11_unused = 0x11U,
+
288  CO_EM_CAN_TX_BUS_OFF = 0x12U,
+
291  CO_EM_CAN_RXB_OVERFLOW = 0x13U,
+
293  CO_EM_CAN_TX_OVERFLOW = 0x14U,
+
295  CO_EM_TPDO_OUTSIDE_WINDOW = 0x15U,
+
297  CO_EM_16_unused = 0x16U,
+
299  CO_EM_17_unused = 0x17U,
+
301  CO_EM_SYNC_TIME_OUT = 0x18U,
+
303  CO_EM_SYNC_LENGTH = 0x19U,
+
305  CO_EM_PDO_WRONG_MAPPING = 0x1AU,
+
307  CO_EM_HEARTBEAT_CONSUMER = 0x1BU,
+
310  CO_EM_HB_CONSUMER_REMOTE_RESET = 0x1CU,
+
312  CO_EM_1D_unused = 0x1DU,
+
314  CO_EM_1E_unused = 0x1EU,
+
316  CO_EM_1F_unused = 0x1FU,
+
317 
+
320  CO_EM_EMERGENCY_BUFFER_FULL = 0x20U,
+
322  CO_EM_21_unused = 0x21U,
+
324  CO_EM_MICROCONTROLLER_RESET = 0x22U,
+
326  CO_EM_23_unused = 0x23U,
+
328  CO_EM_24_unused = 0x24U,
+
330  CO_EM_25_unused = 0x25U,
+
332  CO_EM_26_unused = 0x26U,
+
334  CO_EM_27_unused = 0x27U,
+
335 
+
337  CO_EM_WRONG_ERROR_REPORT = 0x28U,
+
339  CO_EM_ISR_TIMER_OVERFLOW = 0x29U,
+
341  CO_EM_MEMORY_ALLOCATION_ERROR = 0x2AU,
+
343  CO_EM_GENERIC_ERROR = 0x2BU,
+
345  CO_EM_GENERIC_SOFTWARE_ERROR = 0x2CU,
+
347  CO_EM_INCONSISTENT_OBJECT_DICT = 0x2DU,
+
349  CO_EM_CALCULATION_OF_PARAMETERS = 0x2EU,
+
352  CO_EM_NON_VOLATILE_MEMORY = 0x2FU,
+
353 
+
359  CO_EM_MANUFACTURER_START = 0x30U,
+
362  CO_EM_MANUFACTURER_END = CO_CONFIG_EM_ERR_STATUS_BITS_COUNT - 1
+
363 } CO_EM_errorStatusBits_t;
+
364 
+
365 
+
369 typedef struct {
+
371  uint8_t errorStatusBits[CO_CONFIG_EM_ERR_STATUS_BITS_COUNT / 8];
+
373  uint8_t *errorRegister;
+
375  uint16_t CANerrorStatusOld;
+
376 
+
377 #if ((CO_CONFIG_EM) & (CO_CONFIG_EM_PRODUCER | CO_CONFIG_EM_HISTORY)) \
+
378  || defined CO_DOXYGEN
+
379 
+
383 #if ((CO_CONFIG_EM) & CO_CONFIG_EM_PRODUCER) || defined CO_DOXYGEN
+
384  uint32_t fifo[CO_CONFIG_EM_BUFFER_SIZE + 1][2];
+
385 #else
+
386  uint32_t fifo[CO_CONFIG_EM_BUFFER_SIZE + 1][1];
+
387 #endif
+
388 
+
390  uint8_t fifoWrPtr;
+
394  uint8_t fifoPpPtr;
+
396  uint8_t fifoOverflow;
+
398  uint8_t fifoCount;
+
399 #endif /* (CO_CONFIG_EM) & (CO_CONFIG_EM_PRODUCER | CO_CONFIG_EM_HISTORY) */
+
400 
+
401 #if ((CO_CONFIG_EM) & CO_CONFIG_EM_PRODUCER) || defined CO_DOXYGEN
+
402 
+
403  bool_t producerEnabled;
+
405  uint8_t nodeId;
+
407  CO_CANmodule_t *CANdevTx;
+
409  CO_CANtx_t *CANtxBuff;
+
410 #if ((CO_CONFIG_EM) & CO_CONFIG_EM_PROD_CONFIGURABLE) || defined CO_DOXYGEN
+
411 
+
412  uint16_t producerCanId;
+
414  uint16_t CANdevTxIdx;
+
415 #endif
+
416 #if ((CO_CONFIG_EM) & CO_CONFIG_EM_PROD_INHIBIT) || defined CO_DOXYGEN
+
417 
+
418  uint32_t inhibitEmTime_us;
+
420  uint32_t inhibitEmTimer;
+
421 #endif
+
422 #endif /* (CO_CONFIG_EM) & CO_CONFIG_EM_PRODUCER */
+
423 
+
424 #if ((CO_CONFIG_EM) & CO_CONFIG_EM_CONSUMER) || defined CO_DOXYGEN
+
425 
+
426  void (*pFunctSignalRx)(const uint16_t ident,
+
427  const uint16_t errorCode,
+
428  const uint8_t errorRegister,
+
429  const uint8_t errorBit,
+
430  const uint32_t infoCode);
+
431 #endif
+
432 
+
433 #if ((CO_CONFIG_EM) & CO_CONFIG_FLAG_CALLBACK_PRE) || defined CO_DOXYGEN
+
434 
+
435  void (*pFunctSignalPre)(void *object);
+
437  void *functSignalObjectPre;
+
438 #endif
+
439 } CO_EM_t;
+
440 
+
441 
+
470 CO_ReturnError_t CO_EM_init(CO_EM_t *em,
+
471  const OD_entry_t *OD_1001_errReg,
+
472 #if ((CO_CONFIG_EM) & CO_CONFIG_EM_PRODUCER) || defined CO_DOXYGEN
+
473  const OD_entry_t *OD_1014_cobIdEm,
+
474  CO_CANmodule_t *CANdevTx,
+
475  uint16_t CANdevTxIdx,
+
476  #if ((CO_CONFIG_EM) & CO_CONFIG_EM_PROD_INHIBIT) || defined CO_DOXYGEN
+
477  const OD_entry_t *OD_1015_InhTime,
+
478  #endif
+
479 #endif
+
480 #if ((CO_CONFIG_EM) & CO_CONFIG_EM_HISTORY) || defined CO_DOXYGEN
+
481  const OD_entry_t *OD_1003_preDefErr,
+
482 #endif
+
483 #if ((CO_CONFIG_EM) & CO_CONFIG_EM_STATUS_BITS) || defined CO_DOXYGEN
+
484  const OD_entry_t *OD_statusBits,
+
485 #endif
+
486 #if ((CO_CONFIG_EM) & CO_CONFIG_EM_CONSUMER) || defined CO_DOXYGEN
+
487  CO_CANmodule_t *CANdevRx,
+
488  uint16_t CANdevRxIdx,
+
489 #endif
+
490  const uint8_t nodeId);
+
491 
+
492 
+
493 #if ((CO_CONFIG_EM) & CO_CONFIG_FLAG_CALLBACK_PRE) || defined CO_DOXYGEN
+
494 
+
508 void CO_EM_initCallbackPre(CO_EM_t *em,
+
509  void *object,
+
510  void (*pFunctSignal)(void *object));
+
511 #endif
+
512 
+
513 
+
514 #if ((CO_CONFIG_EM) & CO_CONFIG_EM_CONSUMER) || defined CO_DOXYGEN
+
515 
+
530 void CO_EM_initCallbackRx(CO_EM_t *em,
+
531  void (*pFunctSignalRx)(const uint16_t ident,
+
532  const uint16_t errorCode,
+
533  const uint8_t errorRegister,
+
534  const uint8_t errorBit,
+
535  const uint32_t infoCode));
+
536 #endif
+
537 
+
538 
+
553 void CO_EM_process(CO_EM_t *em,
+
554  bool_t NMTisPreOrOperational,
+
555  uint32_t timeDifference_us,
+
556  uint32_t *timerNext_us);
+
557 
+
558 
+
577 void CO_error(CO_EM_t *em, bool_t setError, const uint8_t errorBit,
+
578  uint16_t errorCode, uint32_t infoCode);
+
579 
+
580 
+
584 #define CO_errorReport(em, errorBit, errorCode, infoCode) \
+
585  CO_error(em, true, errorBit, errorCode, infoCode)
+
586 
+
587 
+
591 #define CO_errorReset(em, errorBit, infoCode) \
+
592  CO_error(em, false, errorBit, CO_EMC_NO_ERROR, infoCode)
+
593 
+
594 
+
605 static inline bool_t CO_isError(CO_EM_t *em, const uint8_t errorBit) {
+
606  uint8_t index = errorBit >> 3;
+
607  uint8_t bitmask = 1 << (errorBit & 0x7);
+
608 
+
609  return (em == NULL || index >= (CO_CONFIG_EM_ERR_STATUS_BITS_COUNT / 8)
+
610  || (em->errorStatusBits[index] & bitmask) != 0) ? true : false;
+
611 }
+
612 
+
620 static inline uint8_t CO_getErrorRegister(CO_EM_t *em) {
+
621  return (em == NULL || em->errorRegister == NULL) ? 0 : *em->errorRegister;
+
622 }
+
623  /* CO_Emergency */
+
625 
+
626 #ifdef __cplusplus
+
627 }
+
628 #endif /*__cplusplus*/
+
629 
+
630 #endif /* CO_EMERGENCY_H */
+
+
+
uint32_t
unsigned long int uint32_t
UNSIGNED32 in CANopen (0007h), 32-bit unsigned integer.
Definition: CO_driver.h:155
+
CO_EM_CALCULATION_OF_PARAMETERS
@ CO_EM_CALCULATION_OF_PARAMETERS
0x2E, generic, critical, Error in calculation of device parameters
Definition: CO_Emergency.h:349
+
CO_EM_t::producerCanId
uint16_t producerCanId
COB ID of emergency message, from Object dictionary.
Definition: CO_Emergency.h:412
+
CO_CONFIG_EM_ERR_STATUS_BITS_COUNT
#define CO_CONFIG_EM_ERR_STATUS_BITS_COUNT
Maximum number of CO_EM_errorStatusBits_t.
Definition: CO_config.h:214
+
CO_driver.h
Interface between CAN hardware and CANopenNode.
+
CO_EMC_HEARTBEAT
@ CO_EMC_HEARTBEAT
0x8130, Life Guard Error or Heartbeat Error
Definition: CO_Emergency.h:184
+
CO_EM_CAN_RX_BUS_PASSIVE
@ CO_EM_CAN_RX_BUS_PASSIVE
0x06, communication, info, CAN receive bus is passive
Definition: CO_Emergency.h:263
+
CO_EM_21_unused
@ CO_EM_21_unused
0x21, generic, info, (unused)
Definition: CO_Emergency.h:322
+
CO_EMC_MONITORING
@ CO_EMC_MONITORING
0x80xx, Monitoring
Definition: CO_Emergency.h:176
+
CO_EM_errorStatusBits_t
CO_EM_errorStatusBits_t
Error status bits.
Definition: CO_Emergency.h:247
+
CO_EM_GENERIC_ERROR
@ CO_EM_GENERIC_ERROR
0x2B, generic, critical, Generic error, test usage
Definition: CO_Emergency.h:343
+
CO_EMC401_OUT_LOAD_DUMP
@ CO_EMC401_OUT_LOAD_DUMP
0x2330, DS401, Load dump at outputs
Definition: CO_Emergency.h:215
+
CO_ERR_REG_VOLTAGE
@ CO_ERR_REG_VOLTAGE
bit 2, voltage
Definition: CO_Emergency.h:122
+
true
#define true
Logical true, for general use.
Definition: CO_driver.h:137
+
CO_EM_24_unused
@ CO_EM_24_unused
0x24, generic, info, (unused)
Definition: CO_Emergency.h:328
+
CO_EMC_ADDITIONAL_FUNC
@ CO_EMC_ADDITIONAL_FUNC
0xF0xx, Additional Functions
Definition: CO_Emergency.h:206
+
CO_error
void CO_error(CO_EM_t *em, bool_t setError, const uint8_t errorBit, uint16_t errorCode, uint32_t infoCode)
Set or reset error condition.
Definition: CO_Emergency.c:686
+
uint16_t
unsigned int uint16_t
UNSIGNED16 in CANopen (0006h), 16-bit unsigned integer.
Definition: CO_driver.h:153
+
CO_EM_TPDO_OUTSIDE_WINDOW
@ CO_EM_TPDO_OUTSIDE_WINDOW
0x15, communication, critical, TPDO is outside SYNC window
Definition: CO_Emergency.h:295
+
CO_EM_t::CANdevTxIdx
uint16_t CANdevTxIdx
From CO_EM_init()
Definition: CO_Emergency.h:414
+
CO_EM_t::CANerrorStatusOld
uint16_t CANerrorStatusOld
Old CAN error status bitfield.
Definition: CO_Emergency.h:375
+
CO_EM_t::producerEnabled
bool_t producerEnabled
True, if emergency producer is enabled, from Object dictionary.
Definition: CO_Emergency.h:403
+
CO_EMC_CAN_ID_COLLISION
@ CO_EMC_CAN_ID_COLLISION
0x8150, CAN-ID collision
Definition: CO_Emergency.h:188
+
CO_EM_t::CANtxBuff
CO_CANtx_t * CANtxBuff
CAN transmit buffer.
Definition: CO_Emergency.h:409
+
CO_ReturnError_t
CO_ReturnError_t
Return values of some CANopen functions.
Definition: CO_driver.h:488
+
CO_EMC_GENERIC
@ CO_EMC_GENERIC
0x10xx, Generic Error
Definition: CO_Emergency.h:140
+
CO_EM_RXMSG_WRONG_LENGTH
@ CO_EM_RXMSG_WRONG_LENGTH
0x02, communication, info, Wrong data length of the received CAN message
Definition: CO_Emergency.h:254
+
CO_EM_SYNC_TIME_OUT
@ CO_EM_SYNC_TIME_OUT
0x18, communication, critical, SYNC message timeout
Definition: CO_Emergency.h:301
+
CO_ERR_REG_COMMUNICATION
@ CO_ERR_REG_COMMUNICATION
bit 4, communication error
Definition: CO_Emergency.h:124
+
CO_EMC401_INTERN_VOLT_HI
@ CO_EMC401_INTERN_VOLT_HI
0x3210, DS401, Internal voltage too high
Definition: CO_Emergency.h:221
+
CO_EMC_COMMUNICATION
@ CO_EMC_COMMUNICATION
0x81xx, Communication
Definition: CO_Emergency.h:178
+
bool_t
unsigned char bool_t
Boolean data type for general use.
Definition: CO_driver.h:141
+
CO_ERR_REG_RESERVED
@ CO_ERR_REG_RESERVED
bit 6, reserved (always 0)
Definition: CO_Emergency.h:126
+
CO_EMC_PDO_LENGTH
@ CO_EMC_PDO_LENGTH
0x8210, PDO not processed due to length error
Definition: CO_Emergency.h:192
+
CO_ERR_REG_GENERIC_ERR
@ CO_ERR_REG_GENERIC_ERR
bit 0, generic error
Definition: CO_Emergency.h:120
+
CO_EM_0F_unused
@ CO_EM_0F_unused
0x0F, communication, info, (unused)
Definition: CO_Emergency.h:281
+
CO_EM_1D_unused
@ CO_EM_1D_unused
0x1D, communication, critical, (unused)
Definition: CO_Emergency.h:312
+
CO_EMC_DAM_MPDO
@ CO_EMC_DAM_MPDO
0x8230, DAM MPDO not processed, destination object not available
Definition: CO_Emergency.h:196
+
CO_EM_MICROCONTROLLER_RESET
@ CO_EM_MICROCONTROLLER_RESET
0x22, generic, info, Microcontroller has just started
Definition: CO_Emergency.h:324
+
CO_ERR_REG_CURRENT
@ CO_ERR_REG_CURRENT
bit 1, current
Definition: CO_Emergency.h:121
+
CO_EMC_ADDITIONAL_MODUL
@ CO_EMC_ADDITIONAL_MODUL
0x70xx, Additional Modules
Definition: CO_Emergency.h:174
+
CO_EM_WRONG_ERROR_REPORT
@ CO_EM_WRONG_ERROR_REPORT
0x28, generic, critical, Wrong parameters to CO_errorReport() function
Definition: CO_Emergency.h:337
+
CO_EMC_PROTOCOL_ERROR
@ CO_EMC_PROTOCOL_ERROR
0x82xx, Protocol Error
Definition: CO_Emergency.h:190
+
CO_EM_27_unused
@ CO_EM_27_unused
0x27, generic, info, (unused)
Definition: CO_Emergency.h:334
+
CO_EMC_BUS_OFF_RECOVERED
@ CO_EMC_BUS_OFF_RECOVERED
0x8140, recovered from bus off
Definition: CO_Emergency.h:186
+
CO_EM_NON_VOLATILE_MEMORY
@ CO_EM_NON_VOLATILE_MEMORY
0x2F, generic, critical, Error with access to non volatile device memory
Definition: CO_Emergency.h:352
+
CO_EM_SYNC_LENGTH
@ CO_EM_SYNC_LENGTH
0x19, communication, critical, Unexpected SYNC data length
Definition: CO_Emergency.h:303
+
CO_EM_GENERIC_SOFTWARE_ERROR
@ CO_EM_GENERIC_SOFTWARE_ERROR
0x2C, generic, critical, Software error
Definition: CO_Emergency.h:345
+
CO_ERR_REG_DEV_PROFILE
@ CO_ERR_REG_DEV_PROFILE
bit 5, device profile specific
Definition: CO_Emergency.h:125
+
CO_EM_10_unused
@ CO_EM_10_unused
0x10, communication, critical, (unused)
Definition: CO_Emergency.h:284
+
CO_EM_EMERGENCY_BUFFER_FULL
@ CO_EM_EMERGENCY_BUFFER_FULL
0x20, generic, info, Emergency buffer is full, Emergency message wasn't sent
Definition: CO_Emergency.h:320
+
CO_EMC_DATA_SET
@ CO_EMC_DATA_SET
0x63xx, Data Set
Definition: CO_Emergency.h:172
+
CO_EM_0B_unused
@ CO_EM_0B_unused
0x0B, communication, info, (unused)
Definition: CO_Emergency.h:273
+
CO_EMC401_OUT_CUR_HI
@ CO_EMC401_OUT_CUR_HI
0x2310, DS401, Current at outputs too high (overload)
Definition: CO_Emergency.h:211
+
CO_EMC_SOFTWARE_INTERNAL
@ CO_EMC_SOFTWARE_INTERNAL
0x61xx, Internal Software
Definition: CO_Emergency.h:168
+
CO_EM_MANUFACTURER_END
@ CO_EM_MANUFACTURER_END
(CO_CONFIG_EM_ERR_STATUS_BITS_COUNT - 1), largest value of the Error status bit.
Definition: CO_Emergency.h:362
+
CO_EMC_CURRENT
@ CO_EMC_CURRENT
0x20xx, Current
Definition: CO_Emergency.h:142
+
CO_EM_1E_unused
@ CO_EM_1E_unused
0x1E, communication, critical, (unused)
Definition: CO_Emergency.h:314
+
CO_EM_CAN_RXB_OVERFLOW
@ CO_EM_CAN_RXB_OVERFLOW
0x13, communication, critical, CAN module receive buffer has overflowed
Definition: CO_Emergency.h:291
+
CO_EMC_SOFTWARE_DEVICE
@ CO_EMC_SOFTWARE_DEVICE
0x60xx, Device Software
Definition: CO_Emergency.h:166
+
CO_EM_NMT_WRONG_COMMAND
@ CO_EM_NMT_WRONG_COMMAND
0x08, communication, info, Wrong NMT command received
Definition: CO_Emergency.h:267
+
CO_EM_INCONSISTENT_OBJECT_DICT
@ CO_EM_INCONSISTENT_OBJECT_DICT
0x2D, generic, critical, Object dictionary does not match the software
Definition: CO_Emergency.h:347
+
CO_EMC401_OUT_SHORTED
@ CO_EMC401_OUT_SHORTED
0x2320, DS401, Short circuit at outputs
Definition: CO_Emergency.h:213
+
CO_EMC_CURRENT_OUTPUT
@ CO_EMC_CURRENT_OUTPUT
0x23xx, Current, device output side
Definition: CO_Emergency.h:148
+
CO_EMC_VOLTAGE_OUTPUT
@ CO_EMC_VOLTAGE_OUTPUT
0x33xx, Output Voltage
Definition: CO_Emergency.h:156
+
CO_EM_t::CANdevTx
CO_CANmodule_t * CANdevTx
From CO_EM_init()
Definition: CO_Emergency.h:407
+
CO_EM_CAN_TX_BUS_PASSIVE
@ CO_EM_CAN_TX_BUS_PASSIVE
0x07, communication, info, CAN transmit bus is passive
Definition: CO_Emergency.h:265
+
CO_EM_CAN_TX_BUS_OFF
@ CO_EM_CAN_TX_BUS_OFF
0x12, communication, critical, CAN transmit bus is off
Definition: CO_Emergency.h:288
+
CO_EM_CAN_TX_OVERFLOW
@ CO_EM_CAN_TX_OVERFLOW
0x14, communication, critical, CAN transmit buffer has overflowed
Definition: CO_Emergency.h:293
+
CO_EM_MANUFACTURER_START
@ CO_EM_MANUFACTURER_START
0x30+, manufacturer, info or critical, Error status buts, free to use by manufacturer.
Definition: CO_Emergency.h:359
+
CO_EM_RPDO_OVERFLOW
@ CO_EM_RPDO_OVERFLOW
0x05, communication, info, Previous received PDO wasn't processed yet
Definition: CO_Emergency.h:261
+
CO_EMC_DEVICE_SPECIFIC
@ CO_EMC_DEVICE_SPECIFIC
0xFFxx, Device specific
Definition: CO_Emergency.h:208
+
CO_EMC401_OUT_VOLT_LOW
@ CO_EMC401_OUT_VOLT_LOW
0x3320, DS401, Output voltage too low
Definition: CO_Emergency.h:227
+
CO_EM_errorCode_t
CO_EM_errorCode_t
CANopen Error code.
Definition: CO_Emergency.h:136
+
CO_EMC_VOLTAGE_INSIDE
@ CO_EMC_VOLTAGE_INSIDE
0x32xx, Voltage inside the device
Definition: CO_Emergency.h:154
+
CO_EMC401_INTERN_VOLT_LO
@ CO_EMC401_INTERN_VOLT_LO
0x3220, DS401, Internal voltage too low
Definition: CO_Emergency.h:223
+
CO_EMC_HARDWARE
@ CO_EMC_HARDWARE
0x50xx, Device Hardware
Definition: CO_Emergency.h:164
+
CO_EM_t::nodeId
uint8_t nodeId
Copy of CANopen node ID, from CO_EM_init()
Definition: CO_Emergency.h:405
+
CO_EM_t::fifoCount
uint8_t fifoCount
Count of emergency messages in fifo, used for OD object 0x1003.
Definition: CO_Emergency.h:398
+
CO_EM_NO_ERROR
@ CO_EM_NO_ERROR
0x00, Error Reset or No Error
Definition: CO_Emergency.h:249
+
CO_CONFIG_EM_BUFFER_SIZE
#define CO_CONFIG_EM_BUFFER_SIZE
Size of the internal buffer, where emergencies are stored after error indication with CO_error() func...
Definition: CO_config.h:228
+
CO_errorRegister_t
CO_errorRegister_t
CANopen Error register.
Definition: CO_Emergency.h:119
+
CO_EM_HB_CONSUMER_REMOTE_RESET
@ CO_EM_HB_CONSUMER_REMOTE_RESET
0x1C, communication, critical, Heartbeat consumer detected remote node reset
Definition: CO_Emergency.h:310
+
CO_EM_t::errorRegister
uint8_t * errorRegister
Pointer to error register in object dictionary at 0x1001,00.
Definition: CO_Emergency.h:373
+
CO_EM_initCallbackPre
void CO_EM_initCallbackPre(CO_EM_t *em, void *object, void(*pFunctSignal)(void *object))
Initialize Emergency callback function.
+
CO_EM_t::fifoOverflow
uint8_t fifoOverflow
Indication of overflow - messages in buffer are not post-processed.
Definition: CO_Emergency.h:396
+
CO_EM_HEARTBEAT_CONSUMER
@ CO_EM_HEARTBEAT_CONSUMER
0x1B, communication, critical, Heartbeat consumer timeout
Definition: CO_Emergency.h:307
+
CO_EMC_TEMP_AMBIENT
@ CO_EMC_TEMP_AMBIENT
0x41xx, Ambient Temperature
Definition: CO_Emergency.h:160
+
CO_EM_CAN_BUS_WARNING
@ CO_EM_CAN_BUS_WARNING
0x01, communication, info, CAN bus warning limit reached
Definition: CO_Emergency.h:251
+
CO_getErrorRegister
static uint8_t CO_getErrorRegister(CO_EM_t *em)
Get error register.
Definition: CO_Emergency.h:620
+
CO_EM_t::fifoPpPtr
uint8_t fifoPpPtr
Pointer for the fifo, where next emergency message has to be post-processed by CO_EM_process() functi...
Definition: CO_Emergency.h:394
+
CO_EM_11_unused
@ CO_EM_11_unused
0x11, communication, critical, (unused)
Definition: CO_Emergency.h:286
+
CO_EM_t
Emergency object.
Definition: CO_Emergency.h:369
+
CO_EM_23_unused
@ CO_EM_23_unused
0x23, generic, info, (unused)
Definition: CO_Emergency.h:326
+
CO_EMC401_IN_VOLT_HI
@ CO_EMC401_IN_VOLT_HI
0x3110, DS401, Input voltage too high
Definition: CO_Emergency.h:217
+
CO_EM_t::fifoWrPtr
uint8_t fifoWrPtr
Pointer for the fifo buffer, where next emergency message will be written by CO_error() function.
Definition: CO_Emergency.h:390
+
CO_CONFIG_EM
#define CO_CONFIG_EM
Configuration of Emergency.
Definition: CO_config.h:197
+
CO_EMC_CAN_PASSIVE
@ CO_EMC_CAN_PASSIVE
0x8120, CAN in Error Passive Mode
Definition: CO_Emergency.h:182
+
CO_EM_1F_unused
@ CO_EM_1F_unused
0x1F, communication, critical, (unused)
Definition: CO_Emergency.h:316
+
CO_EMC_SYNC_DATA_LENGTH
@ CO_EMC_SYNC_DATA_LENGTH
0x8240, Unexpected SYNC data length
Definition: CO_Emergency.h:198
+
CO_EM_t::errorStatusBits
uint8_t errorStatusBits[CO_CONFIG_EM_ERR_STATUS_BITS_COUNT/8]
Bitfield for the internal indication of the error condition.
Definition: CO_Emergency.h:371
+
CO_ODinterface.h
CANopen Object Dictionary interface.
+
CO_EM_t::functSignalObjectPre
void * functSignalObjectPre
From CO_EM_initCallbackPre() or NULL.
Definition: CO_Emergency.h:437
+
CO_EM_25_unused
@ CO_EM_25_unused
0x25, generic, info, (unused)
Definition: CO_Emergency.h:330
+
CO_EMC_TIME_DATA_LENGTH
@ CO_EMC_TIME_DATA_LENGTH
0x8260, Unexpected TIME data length
Definition: CO_Emergency.h:202
+
CO_EMC_PDO_LENGTH_EXC
@ CO_EMC_PDO_LENGTH_EXC
0x8220, PDO length exceeded
Definition: CO_Emergency.h:194
+
CO_EM_init
CO_ReturnError_t CO_EM_init(CO_EM_t *em, const OD_entry_t *OD_1001_errReg, const OD_entry_t *OD_1014_cobIdEm, CO_CANmodule_t *CANdevTx, uint16_t CANdevTxIdx, const OD_entry_t *OD_1015_InhTime, const OD_entry_t *OD_1003_preDefErr, const OD_entry_t *OD_statusBits, CO_CANmodule_t *CANdevRx, uint16_t CANdevRxIdx, const uint8_t nodeId)
Initialize Emergency object.
+
CO_EMC_EXTERNAL_ERROR
@ CO_EMC_EXTERNAL_ERROR
0x90xx, External Error
Definition: CO_Emergency.h:204
+
CO_EM_0C_unused
@ CO_EM_0C_unused
0x0C, communication, info, (unused)
Definition: CO_Emergency.h:275
+
CO_EM_TIME_LENGTH
@ CO_EM_TIME_LENGTH
0x0A, communication, info, Unexpected TIME data length
Definition: CO_Emergency.h:271
+
CO_EMC401_OUT_VOLT_HIGH
@ CO_EMC401_OUT_VOLT_HIGH
0x3310, DS401, Output voltage too high
Definition: CO_Emergency.h:225
+
CO_EMC_VOLTAGE_MAINS
@ CO_EMC_VOLTAGE_MAINS
0x31xx, Mains Voltage
Definition: CO_Emergency.h:152
+
CO_EM_17_unused
@ CO_EM_17_unused
0x17, communication, critical, (unused)
Definition: CO_Emergency.h:299
+
CO_EMC_TEMP_DEVICE
@ CO_EMC_TEMP_DEVICE
0x42xx, Device Temperature
Definition: CO_Emergency.h:162
+
CO_EM_0E_unused
@ CO_EM_0E_unused
0x0E, communication, info, (unused)
Definition: CO_Emergency.h:279
+
CO_EM_initCallbackRx
void CO_EM_initCallbackRx(CO_EM_t *em, void(*pFunctSignalRx)(const uint16_t ident, const uint16_t errorCode, const uint8_t errorRegister, const uint8_t errorBit, const uint32_t infoCode))
Initialize Emergency received callback function.
+
CO_isError
static bool_t CO_isError(CO_EM_t *em, const uint8_t errorBit)
Check specific error condition.
Definition: CO_Emergency.h:605
+
CO_EM_0D_unused
@ CO_EM_0D_unused
0x0D, communication, info, (unused)
Definition: CO_Emergency.h:277
+
CO_ERR_REG_MANUFACTURER
@ CO_ERR_REG_MANUFACTURER
bit 7, manufacturer specific
Definition: CO_Emergency.h:127
+
CO_EMC_RPDO_TIMEOUT
@ CO_EMC_RPDO_TIMEOUT
0x8250, RPDO timeout
Definition: CO_Emergency.h:200
+
NULL
#define NULL
NULL, for general usage.
Definition: CO_driver.h:135
+
CO_EM_RXMSG_OVERFLOW
@ CO_EM_RXMSG_OVERFLOW
0x03, communication, info, Previous received CAN message wasn't processed yet
Definition: CO_Emergency.h:257
+
CO_EMC_CAN_OVERRUN
@ CO_EMC_CAN_OVERRUN
0x8110, CAN Overrun (Objects lost)
Definition: CO_Emergency.h:180
+
CO_CANmodule_t
Complete CAN module object.
Definition: CO_driver.h:319
+
CO_EM_ISR_TIMER_OVERFLOW
@ CO_EM_ISR_TIMER_OVERFLOW
0x29, generic, critical, Timer task has overflowed
Definition: CO_Emergency.h:339
+
CO_EM_process
void CO_EM_process(CO_EM_t *em, bool_t NMTisPreOrOperational, uint32_t timeDifference_us, uint32_t *timerNext_us)
Process Error control and Emergency object.
Definition: CO_Emergency.c:538
+
CO_EMC401_IN_VOLT_LOW
@ CO_EMC401_IN_VOLT_LOW
0x3120, DS401, Input voltage too low
Definition: CO_Emergency.h:219
+
CO_EM_TIME_TIMEOUT
@ CO_EM_TIME_TIMEOUT
0x09, communication, info, TIME message timeout
Definition: CO_Emergency.h:269
+
OD_entry_t
Object Dictionary entry for one OD object.
Definition: CO_ODinterface.h:336
+
CO_EMC_SOFTWARE_USER
@ CO_EMC_SOFTWARE_USER
0x62xx, User Software
Definition: CO_Emergency.h:170
+
CO_EM_MEMORY_ALLOCATION_ERROR
@ CO_EM_MEMORY_ALLOCATION_ERROR
0x2A, generic, critical, Unable to allocate memory for objects
Definition: CO_Emergency.h:341
+
CO_EMC_VOLTAGE
@ CO_EMC_VOLTAGE
0x30xx, Voltage
Definition: CO_Emergency.h:150
+
CO_EM_16_unused
@ CO_EM_16_unused
0x16, communication, critical, (unused)
Definition: CO_Emergency.h:297
+
CO_EM_PDO_WRONG_MAPPING
@ CO_EM_PDO_WRONG_MAPPING
0x1A, communication, critical, Error with PDO mapping
Definition: CO_Emergency.h:305
+
CO_EMC_TEMPERATURE
@ CO_EMC_TEMPERATURE
0x40xx, Temperature
Definition: CO_Emergency.h:158
+
CO_ERR_REG_TEMPERATURE
@ CO_ERR_REG_TEMPERATURE
bit 3, temperature
Definition: CO_Emergency.h:123
+
CO_EMC_CURRENT_INSIDE
@ CO_EMC_CURRENT_INSIDE
0x22xx, Current inside the device
Definition: CO_Emergency.h:146
+
CO_EMC_CURRENT_INPUT
@ CO_EMC_CURRENT_INPUT
0x21xx, Current, device input side
Definition: CO_Emergency.h:144
+
CO_CANtx_t
Configuration object for CAN transmit message for specific CANopenNode Object.
Definition: CO_driver.h:299
+
CO_EMC_NO_ERROR
@ CO_EMC_NO_ERROR
0x00xx, error Reset or No Error
Definition: CO_Emergency.h:138
+
CO_EM_t::inhibitEmTime_us
uint32_t inhibitEmTime_us
Inhibit time for emergency message, from Object dictionary.
Definition: CO_Emergency.h:418
+
CO_EM_26_unused
@ CO_EM_26_unused
0x26, generic, info, (unused)
Definition: CO_Emergency.h:332
+
uint8_t
unsigned char uint8_t
UNSIGNED8 in CANopen (0005h), 8-bit unsigned integer.
Definition: CO_driver.h:151
+
CO_EM_RPDO_WRONG_LENGTH
@ CO_EM_RPDO_WRONG_LENGTH
0x04, communication, info, Wrong data length of received PDO
Definition: CO_Emergency.h:259
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/CO__GFC_8c.html b/Devices/Libraries/Systems/CANopenSocket/docs/CO__GFC_8c.html new file mode 100755 index 0000000..e7073c0 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/CO__GFC_8c.html @@ -0,0 +1,121 @@ + + + + + + + +CANopenNode: 304/CO_GFC.c File Reference + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+
CO_GFC.c File Reference
CANopen_304 » GFC
+
+
+ +

CANopen Global fail-safe command protocol. +More...

+
#include "304/CO_GFC.h"
+

Detailed Description

+

CANopen Global fail-safe command protocol.

+
Author
Robert Grüning
+
Copyright
2020 - 2020 Robert Grüning
+

This file is part of CANopenNode, an opensource CANopen Stack. Project home page is https://github.com/CANopenNode/CANopenNode. For more information on CANopen see http://www.can-cia.org/.

+

Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. You may obtain a copy of the License at 

http://www.apache.org/licenses/LICENSE-2.0
+

Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License.

+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/CO__GFC_8h.html b/Devices/Libraries/Systems/CANopenSocket/docs/CO__GFC_8h.html new file mode 100755 index 0000000..a52ae30 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/CO__GFC_8h.html @@ -0,0 +1,145 @@ + + + + + + + +CANopenNode: 304/CO_GFC.h File Reference + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Data Structures | +Functions
+
+
CO_GFC.h File Reference
CANopen_304 » GFC
+
+
+ +

CANopen Global fail-safe command protocol. +More...

+
#include "301/CO_driver.h"
+
+

Go to the source code of this file.

+ + + + + +

+Data Structures

struct  CO_GFC_t
 GFC object. More...
 
+ + + + + + + + + + +

+Functions

CO_ReturnError_t CO_GFC_init (CO_GFC_t *GFC, uint8_t *valid, CO_CANmodule_t *GFC_CANdevRx, uint16_t GFC_rxIdx, uint16_t CANidRxGFC, CO_CANmodule_t *GFC_CANdevTx, uint16_t GFC_txIdx, uint16_t CANidTxGFC)
 Initialize GFC object. More...
 
void CO_GFC_initCallbackEnterSafeState (CO_GFC_t *GFC, void *object, void(*pFunctSignalSafe)(void *object))
 Initialize GFC callback function. More...
 
CO_ReturnError_t CO_GFCsend (CO_GFC_t *GFC)
 Send GFC message. More...
 
+

Detailed Description

+

CANopen Global fail-safe command protocol.

+
Author
Robert Grüning
+
Copyright
2020 - 2020 Robert Grüning
+

This file is part of CANopenNode, an opensource CANopen Stack. Project home page is https://github.com/CANopenNode/CANopenNode. For more information on CANopen see http://www.can-cia.org/.

+

Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. You may obtain a copy of the License at 

http://www.apache.org/licenses/LICENSE-2.0
+

Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License.

+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/CO__GFC_8h.js b/Devices/Libraries/Systems/CANopenSocket/docs/CO__GFC_8h.js new file mode 100755 index 0000000..535254f --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/CO__GFC_8h.js @@ -0,0 +1,6 @@ +var CO__GFC_8h = +[ + [ "CO_GFC_init", "group__CO__GFC.html#ga23d83d03ef1b9ad5ffe68103a627026c", null ], + [ "CO_GFC_initCallbackEnterSafeState", "group__CO__GFC.html#gaa7cf845381bf150a5816bc068ab9218f", null ], + [ "CO_GFCsend", "group__CO__GFC.html#ga64a30ac6c275d166a2f4117050b12c8c", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/CO__GFC_8h_source.html b/Devices/Libraries/Systems/CANopenSocket/docs/CO__GFC_8h_source.html new file mode 100755 index 0000000..62610e0 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/CO__GFC_8h_source.html @@ -0,0 +1,183 @@ + + + + + + + +CANopenNode: 304/CO_GFC.h Source File + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+
CO_GFC.h
+
+
+Go to the documentation of this file.
1 
+
26 #ifndef CO_GFC_H
+
27 #define CO_GFC_H
+
28 
+
29 #include "301/CO_driver.h"
+
30 
+
31 /* default configuration, see CO_config.h */
+
32 #ifndef CO_CONFIG_GFC
+
33 #define CO_CONFIG_GFC (0)
+
34 #endif
+
35 
+
36 #if ((CO_CONFIG_GFC) & CO_CONFIG_GFC_ENABLE) || defined CO_DOXYGEN
+
37 
+
38 #ifdef __cplusplus
+
39 extern "C" {
+
40 #endif
+
41 
+
60 typedef struct {
+
61  uint8_t *valid;
+
62 #if ((CO_CONFIG_GFC)&CO_CONFIG_GFC_PRODUCER) || defined CO_DOXYGEN
+
63  CO_CANmodule_t *CANdevTx;
+
64  CO_CANtx_t *CANtxBuff;
+
65 #endif
+
66 #if ((CO_CONFIG_GFC)&CO_CONFIG_GFC_CONSUMER) || defined CO_DOXYGEN
+
67 
+
68  void (*pFunctSignalSafe)(void *object);
+
70  void *functSignalObjectSafe;
+
71 #endif
+
72 } CO_GFC_t;
+
73 
+
90 CO_ReturnError_t CO_GFC_init(CO_GFC_t *GFC,
+
91  uint8_t *valid,
+
92  CO_CANmodule_t *GFC_CANdevRx,
+
93  uint16_t GFC_rxIdx,
+
94  uint16_t CANidRxGFC,
+
95  CO_CANmodule_t *GFC_CANdevTx,
+
96  uint16_t GFC_txIdx,
+
97  uint16_t CANidTxGFC);
+
98 
+
99 #if ((CO_CONFIG_GFC)&CO_CONFIG_GFC_CONSUMER) || defined CO_DOXYGEN
+
100 
+
111 void CO_GFC_initCallbackEnterSafeState(CO_GFC_t *GFC,
+
112  void *object,
+
113  void (*pFunctSignalSafe)(void *object));
+
114 #endif
+
115 
+
116 #if ((CO_CONFIG_GFC)&CO_CONFIG_GFC_PRODUCER) || defined CO_DOXYGEN
+
117 
+
127 CO_ReturnError_t CO_GFCsend(CO_GFC_t *GFC);
+
128 #endif
+
129  /* CO_GFC */
+
131 
+
132 #ifdef __cplusplus
+
133 }
+
134 #endif /*__cplusplus*/
+
135 
+
136 #endif /* (CO_CONFIG_GFC) & CO_CONFIG_GFC_ENABLE */
+
137 
+
138 #endif /* CO_GFC_H */
+
+
+
CO_driver.h
Interface between CAN hardware and CANopenNode.
+
CO_GFC_t::CANtxBuff
CO_CANtx_t * CANtxBuff
CAN transmit buffer inside CANdevTx.
Definition: CO_GFC.h:64
+
CO_GFC_t::valid
uint8_t * valid
From CO_GFC_init()
Definition: CO_GFC.h:61
+
uint16_t
unsigned int uint16_t
UNSIGNED16 in CANopen (0006h), 16-bit unsigned integer.
Definition: CO_driver.h:153
+
CO_GFC_t
GFC object.
Definition: CO_GFC.h:60
+
CO_ReturnError_t
CO_ReturnError_t
Return values of some CANopen functions.
Definition: CO_driver.h:488
+
CO_GFC_init
CO_ReturnError_t CO_GFC_init(CO_GFC_t *GFC, uint8_t *valid, CO_CANmodule_t *GFC_CANdevRx, uint16_t GFC_rxIdx, uint16_t CANidRxGFC, CO_CANmodule_t *GFC_CANdevTx, uint16_t GFC_txIdx, uint16_t CANidTxGFC)
Initialize GFC object.
+
CO_GFCsend
CO_ReturnError_t CO_GFCsend(CO_GFC_t *GFC)
Send GFC message.
+
CO_GFC_t::functSignalObjectSafe
void * functSignalObjectSafe
From CO_GFC_initCallbackEnterSafeState() or NULL.
Definition: CO_GFC.h:70
+
CO_GFC_initCallbackEnterSafeState
void CO_GFC_initCallbackEnterSafeState(CO_GFC_t *GFC, void *object, void(*pFunctSignalSafe)(void *object))
Initialize GFC callback function.
+
CO_CANmodule_t
Complete CAN module object.
Definition: CO_driver.h:319
+
CO_GFC_t::CANdevTx
CO_CANmodule_t * CANdevTx
From CO_GFC_init()
Definition: CO_GFC.h:63
+
CO_CANtx_t
Configuration object for CAN transmit message for specific CANopenNode Object.
Definition: CO_driver.h:299
+
uint8_t
unsigned char uint8_t
UNSIGNED8 in CANopen (0005h), 8-bit unsigned integer.
Definition: CO_driver.h:151
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/CO__HBconsumer_8h.html b/Devices/Libraries/Systems/CANopenSocket/docs/CO__HBconsumer_8h.html new file mode 100755 index 0000000..8328e0d --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/CO__HBconsumer_8h.html @@ -0,0 +1,184 @@ + + + + + + + +CANopenNode: 301/CO_HBconsumer.h File Reference + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Data Structures | +Enumerations | +Functions
+
+
CO_HBconsumer.h File Reference
CANopen_301 » Heartbeat consumer
+
+
+ +

CANopen Heartbeat consumer protocol. +More...

+
#include "301/CO_driver.h"
+#include "301/CO_SDOserver.h"
+
+

Go to the source code of this file.

+ + + + + + + + +

+Data Structures

struct  CO_HBconsNode_t
 One monitored node inside CO_HBconsumer_t. More...
 
struct  CO_HBconsumer_t
 Heartbeat consumer object. More...
 
+ + + + +

+Enumerations

enum  CO_HBconsumer_state_t { CO_HBconsumer_UNCONFIGURED = 0x00U, +CO_HBconsumer_UNKNOWN = 0x01U, +CO_HBconsumer_ACTIVE = 0x02U, +CO_HBconsumer_TIMEOUT = 0x03U + }
 Heartbeat state of a node. More...
 
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

+Functions

CO_ReturnError_t CO_HBconsumer_init (CO_HBconsumer_t *HBcons, CO_EM_t *em, CO_SDO_t *SDO, const uint32_t HBconsTime[], CO_HBconsNode_t monitoredNodes[], uint8_t numberOfMonitoredNodes, CO_CANmodule_t *CANdevRx, uint16_t CANdevRxIdxStart)
 Initialize Heartbeat consumer object. More...
 
CO_ReturnError_t CO_HBconsumer_initEntry (CO_HBconsumer_t *HBcons, uint8_t idx, uint8_t nodeId, uint16_t consumerTime_ms)
 Initialize one Heartbeat consumer entry. More...
 
void CO_HBconsumer_initCallbackPre (CO_HBconsumer_t *HBcons, void *object, void(*pFunctSignal)(void *object))
 Initialize Heartbeat consumer callback function. More...
 
void CO_HBconsumer_initCallbackNmtChanged (CO_HBconsumer_t *HBcons, uint8_t idx, void *object, void(*pFunctSignal)(uint8_t nodeId, uint8_t idx, CO_NMT_internalState_t state, void *object))
 Initialize Heartbeat consumer NMT changed callback function. More...
 
void CO_HBconsumer_initCallbackHeartbeatStarted (CO_HBconsumer_t *HBcons, uint8_t idx, void *object, void(*pFunctSignal)(uint8_t nodeId, uint8_t idx, void *object))
 Initialize Heartbeat consumer started callback function. More...
 
void CO_HBconsumer_initCallbackTimeout (CO_HBconsumer_t *HBcons, uint8_t idx, void *object, void(*pFunctSignal)(uint8_t nodeId, uint8_t idx, void *object))
 Initialize Heartbeat consumer timeout callback function. More...
 
void CO_HBconsumer_initCallbackRemoteReset (CO_HBconsumer_t *HBcons, uint8_t idx, void *object, void(*pFunctSignal)(uint8_t nodeId, uint8_t idx, void *object))
 Initialize Heartbeat consumer remote reset detected callback function. More...
 
void CO_HBconsumer_process (CO_HBconsumer_t *HBcons, bool_t NMTisPreOrOperational, uint32_t timeDifference_us, uint32_t *timerNext_us)
 Process Heartbeat consumer object. More...
 
int8_t CO_HBconsumer_getIdxByNodeId (CO_HBconsumer_t *HBcons, uint8_t nodeId)
 Get the heartbeat producer object index by node ID. More...
 
CO_HBconsumer_state_t CO_HBconsumer_getState (CO_HBconsumer_t *HBcons, uint8_t idx)
 Get the current state of a heartbeat producer by the index in OD 0x1016. More...
 
int8_t CO_HBconsumer_getNmtState (CO_HBconsumer_t *HBcons, uint8_t idx, CO_NMT_internalState_t *nmtState)
 Get the current NMT state of a heartbeat producer by the index in OD 0x1016. More...
 
+

Detailed Description

+

CANopen Heartbeat consumer protocol.

+
Author
Janez Paternoster
+
Copyright
2004 - 2020 Janez Paternoster
+

This file is part of CANopenNode, an opensource CANopen Stack. Project home page is https://github.com/CANopenNode/CANopenNode. For more information on CANopen see http://www.can-cia.org/.

+

Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. You may obtain a copy of the License at 

http://www.apache.org/licenses/LICENSE-2.0
+

Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License.

+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/CO__HBconsumer_8h.js b/Devices/Libraries/Systems/CANopenSocket/docs/CO__HBconsumer_8h.js new file mode 100755 index 0000000..2439831 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/CO__HBconsumer_8h.js @@ -0,0 +1,20 @@ +var CO__HBconsumer_8h = +[ + [ "CO_HBconsumer_state_t", "group__CO__HBconsumer.html#ga7658e41b7c045b7b612e4ef8a2b663f3", [ + [ "CO_HBconsumer_UNCONFIGURED", "group__CO__HBconsumer.html#gga7658e41b7c045b7b612e4ef8a2b663f3a4c481c1ba58fb71b9870e8b355351211", null ], + [ "CO_HBconsumer_UNKNOWN", "group__CO__HBconsumer.html#gga7658e41b7c045b7b612e4ef8a2b663f3af36bc5f46fd11044dc2d1d995ad8f28b", null ], + [ "CO_HBconsumer_ACTIVE", "group__CO__HBconsumer.html#gga7658e41b7c045b7b612e4ef8a2b663f3ad80c78b38e6d28927bf3d1b1464b36e9", null ], + [ "CO_HBconsumer_TIMEOUT", "group__CO__HBconsumer.html#gga7658e41b7c045b7b612e4ef8a2b663f3a8a7ac49e5c809994ee65f365a7a75f22", null ] + ] ], + [ "CO_HBconsumer_init", "group__CO__HBconsumer.html#gacc31c4848a14c9c367505b20e4a6a496", null ], + [ "CO_HBconsumer_initEntry", "group__CO__HBconsumer.html#gaf4bfa2bbd2b7d70f25b6bf173932170a", null ], + [ "CO_HBconsumer_initCallbackPre", "group__CO__HBconsumer.html#ga2faa596dcebbec8f486788a791d638be", null ], + [ "CO_HBconsumer_initCallbackNmtChanged", "group__CO__HBconsumer.html#gabab4b2dd74f6e341fe8b683f7a6d56f3", null ], + [ "CO_HBconsumer_initCallbackHeartbeatStarted", "group__CO__HBconsumer.html#ga6c9bd0df815428719b9f9429ed4476a9", null ], + [ "CO_HBconsumer_initCallbackTimeout", "group__CO__HBconsumer.html#gaef359610a0cdd1331da266be9c55c2d2", null ], + [ "CO_HBconsumer_initCallbackRemoteReset", "group__CO__HBconsumer.html#ga8758a7bd92aa458b90d5da9221cc694f", null ], + [ "CO_HBconsumer_process", "group__CO__HBconsumer.html#ga29e01b5fe6392ce688e8ac57d966258f", null ], + [ "CO_HBconsumer_getIdxByNodeId", "group__CO__HBconsumer.html#ga041b92d6feb1774cb7eb87fba842fdf2", null ], + [ "CO_HBconsumer_getState", "group__CO__HBconsumer.html#ga7c5d4eccbcb0f1f8965a336fde04e765", null ], + [ "CO_HBconsumer_getNmtState", "group__CO__HBconsumer.html#ga1731e3860fce5ca5d341d9b7fc32d8d6", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/CO__HBconsumer_8h_source.html b/Devices/Libraries/Systems/CANopenSocket/docs/CO__HBconsumer_8h_source.html new file mode 100755 index 0000000..80588c9 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/CO__HBconsumer_8h_source.html @@ -0,0 +1,338 @@ + + + + + + + +CANopenNode: 301/CO_HBconsumer.h Source File + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+
CO_HBconsumer.h
+
+
+Go to the documentation of this file.
1 
+
26 #ifndef CO_HB_CONS_H
+
27 #define CO_HB_CONS_H
+
28 
+
29 #include "301/CO_driver.h"
+
30 
+
31 /* default configuration, see CO_config.h */
+
32 #ifndef CO_CONFIG_HB_CONS
+
33 #define CO_CONFIG_HB_CONS (CO_CONFIG_HB_CONS_ENABLE)
+
34 #endif
+
35 #ifndef CO_CONFIG_HB_CONS_SIZE
+
36 #define CO_CONFIG_HB_CONS_SIZE 8
+
37 #endif
+
38 
+
39 #if ((CO_CONFIG_HB_CONS) & CO_CONFIG_HB_CONS_ENABLE) || defined CO_DOXYGEN
+
40 
+
41 #include "301/CO_SDOserver.h"
+
42 
+
43 #ifdef __cplusplus
+
44 extern "C" {
+
45 #endif
+
46 
+
69 typedef enum {
+
70  CO_HBconsumer_UNCONFIGURED = 0x00U,
+
71  CO_HBconsumer_UNKNOWN = 0x01U,
+
72  CO_HBconsumer_ACTIVE = 0x02U,
+
73  CO_HBconsumer_TIMEOUT = 0x03U,
+
74 } CO_HBconsumer_state_t;
+
75 
+
76 
+
80 typedef struct {
+
82  uint8_t nodeId;
+
84  CO_NMT_internalState_t NMTstate;
+
86  CO_HBconsumer_state_t HBstate;
+
88  uint32_t timeoutTimer;
+
90  uint32_t time_us;
+
92  volatile void *CANrxNew;
+
93 #if ((CO_CONFIG_HB_CONS) & CO_CONFIG_FLAG_CALLBACK_PRE) || defined CO_DOXYGEN
+
94 
+
95  void (*pFunctSignalPre)(void *object);
+
97  void *functSignalObjectPre;
+
98 #endif
+
99 #if ((CO_CONFIG_HB_CONS) & CO_CONFIG_HB_CONS_CALLBACK_CHANGE) \
+
100  || ((CO_CONFIG_HB_CONS) & CO_CONFIG_HB_CONS_CALLBACK_MULTI) \
+
101  || defined CO_DOXYGEN
+
102 
+
103  CO_NMT_internalState_t NMTstatePrev;
+
104 #endif
+
105 #if ((CO_CONFIG_HB_CONS) & CO_CONFIG_HB_CONS_CALLBACK_MULTI) || defined CO_DOXYGEN
+
106 
+
108  void (*pFunctSignalNmtChanged)(uint8_t nodeId, uint8_t idx,
+
109  CO_NMT_internalState_t state,
+
110  void *object);
+
112  void *pFunctSignalObjectNmtChanged;
+
115  void (*pFunctSignalHbStarted)(uint8_t nodeId, uint8_t idx, void *object);
+
117  void *functSignalObjectHbStarted;
+
120  void (*pFunctSignalTimeout)(uint8_t nodeId, uint8_t idx, void *object);
+
122  void *functSignalObjectTimeout;
+
125  void (*pFunctSignalRemoteReset)(uint8_t nodeId, uint8_t idx, void *object);
+
127  void *functSignalObjectRemoteReset;
+
128 #endif
+
129 } CO_HBconsNode_t;
+
130 
+
131 
+
138 typedef struct{
+
139  CO_EM_t *em;
+
140  const uint32_t *HBconsTime;
+
141  CO_HBconsNode_t *monitoredNodes;
+
142  uint8_t numberOfMonitoredNodes;
+
145  bool_t allMonitoredActive;
+
148  uint8_t allMonitoredOperational;
+
149  bool_t NMTisPreOrOperationalPrev;
+
150  CO_CANmodule_t *CANdevRx;
+
151  uint16_t CANdevRxIdxStart;
+
152 #if ((CO_CONFIG_HB_CONS) & CO_CONFIG_HB_CONS_CALLBACK_CHANGE) || defined CO_DOXYGEN
+
153 
+
155  void (*pFunctSignalNmtChanged)(uint8_t nodeId, uint8_t idx,
+
156  CO_NMT_internalState_t state,
+
157  void *object);
+
159  void *pFunctSignalObjectNmtChanged;
+
160 #endif
+
161 }CO_HBconsumer_t;
+
162 
+
163 
+
183 CO_ReturnError_t CO_HBconsumer_init(
+
184  CO_HBconsumer_t *HBcons,
+
185  CO_EM_t *em,
+
186  CO_SDO_t *SDO,
+
187  const uint32_t HBconsTime[],
+
188  CO_HBconsNode_t monitoredNodes[],
+
189  uint8_t numberOfMonitoredNodes,
+
190  CO_CANmodule_t *CANdevRx,
+
191  uint16_t CANdevRxIdxStart);
+
192 
+
207 CO_ReturnError_t CO_HBconsumer_initEntry(
+
208  CO_HBconsumer_t *HBcons,
+
209  uint8_t idx,
+
210  uint8_t nodeId,
+
211  uint16_t consumerTime_ms);
+
212 
+
213 #if ((CO_CONFIG_HB_CONS) & CO_CONFIG_FLAG_CALLBACK_PRE) || defined CO_DOXYGEN
+
214 
+
225 void CO_HBconsumer_initCallbackPre(
+
226  CO_HBconsumer_t *HBcons,
+
227  void *object,
+
228  void (*pFunctSignal)(void *object));
+
229 #endif
+
230 
+
231 #if ((CO_CONFIG_HB_CONS) & CO_CONFIG_HB_CONS_CALLBACK_CHANGE) \
+
232  || ((CO_CONFIG_HB_CONS) & CO_CONFIG_HB_CONS_CALLBACK_MULTI) \
+
233  || defined CO_DOXYGEN
+
234 
+
247 void CO_HBconsumer_initCallbackNmtChanged(
+
248  CO_HBconsumer_t *HBcons,
+
249 #if ((CO_CONFIG_HB_CONS) & CO_CONFIG_HB_CONS_CALLBACK_MULTI) || defined CO_DOXYGEN
+
250  uint8_t idx,
+
251 #endif
+
252  void *object,
+
253  void (*pFunctSignal)(uint8_t nodeId, uint8_t idx,
+
254  CO_NMT_internalState_t state,
+
255  void *object));
+
256 #endif
+
257 
+
258 #if ((CO_CONFIG_HB_CONS) & CO_CONFIG_HB_CONS_CALLBACK_MULTI) || defined CO_DOXYGEN
+
259 
+
271 void CO_HBconsumer_initCallbackHeartbeatStarted(
+
272  CO_HBconsumer_t *HBcons,
+
273  uint8_t idx,
+
274  void *object,
+
275  void (*pFunctSignal)(uint8_t nodeId, uint8_t idx, void *object));
+
276 
+
289 void CO_HBconsumer_initCallbackTimeout(
+
290  CO_HBconsumer_t *HBcons,
+
291  uint8_t idx,
+
292  void *object,
+
293  void (*pFunctSignal)(uint8_t nodeId, uint8_t idx, void *object));
+
294 
+
307 void CO_HBconsumer_initCallbackRemoteReset(
+
308  CO_HBconsumer_t *HBcons,
+
309  uint8_t idx,
+
310  void *object,
+
311  void (*pFunctSignal)(uint8_t nodeId, uint8_t idx, void *object));
+
312 #endif /* (CO_CONFIG_HB_CONS) & CO_CONFIG_HB_CONS_CALLBACK_MULTI */
+
313 
+
324 void CO_HBconsumer_process(
+
325  CO_HBconsumer_t *HBcons,
+
326  bool_t NMTisPreOrOperational,
+
327  uint32_t timeDifference_us,
+
328  uint32_t *timerNext_us);
+
329 
+
330 
+
331 #if ((CO_CONFIG_HB_CONS) & CO_CONFIG_HB_CONS_QUERY_FUNCT) || defined CO_DOXYGEN
+
332 
+
339 int8_t CO_HBconsumer_getIdxByNodeId(
+
340  CO_HBconsumer_t *HBcons,
+
341  uint8_t nodeId);
+
342 
+
350 CO_HBconsumer_state_t CO_HBconsumer_getState(
+
351  CO_HBconsumer_t *HBcons,
+
352  uint8_t idx);
+
353 
+
365 int8_t CO_HBconsumer_getNmtState(
+
366  CO_HBconsumer_t *HBcons,
+
367  uint8_t idx,
+
368  CO_NMT_internalState_t *nmtState);
+
369 
+
370 #endif /* (CO_CONFIG_HB_CONS) & CO_CONFIG_HB_CONS_QUERY_FUNCT */
+
371  /* CO_HBconsumer */
+
373 
+
374 #ifdef __cplusplus
+
375 }
+
376 #endif /*__cplusplus*/
+
377 
+
378 #endif /* (CO_CONFIG_HB_CONS) & CO_CONFIG_HB_CONS_ENABLE */
+
379 
+
380 #endif /* CO_HB_CONS_H */
+
+
+
CO_HBconsNode_t::functSignalObjectPre
void * functSignalObjectPre
From CO_HBconsumer_initCallbackPre() or NULL.
Definition: CO_HBconsumer.h:97
+
uint32_t
unsigned long int uint32_t
UNSIGNED32 in CANopen (0007h), 32-bit unsigned integer.
Definition: CO_driver.h:155
+
CO_driver.h
Interface between CAN hardware and CANopenNode.
+
CO_HBconsumer_initCallbackPre
void CO_HBconsumer_initCallbackPre(CO_HBconsumer_t *HBcons, void *object, void(*pFunctSignal)(void *object))
Initialize Heartbeat consumer callback function.
+
CO_HBconsumer_getNmtState
int8_t CO_HBconsumer_getNmtState(CO_HBconsumer_t *HBcons, uint8_t idx, CO_NMT_internalState_t *nmtState)
Get the current NMT state of a heartbeat producer by the index in OD 0x1016.
+
CO_HBconsumer_initEntry
CO_ReturnError_t CO_HBconsumer_initEntry(CO_HBconsumer_t *HBcons, uint8_t idx, uint8_t nodeId, uint16_t consumerTime_ms)
Initialize one Heartbeat consumer entry.
+
CO_HBconsumer_t::CANdevRxIdxStart
uint16_t CANdevRxIdxStart
From CO_HBconsumer_init()
Definition: CO_HBconsumer.h:151
+
uint16_t
unsigned int uint16_t
UNSIGNED16 in CANopen (0006h), 16-bit unsigned integer.
Definition: CO_driver.h:153
+
CO_HBconsumer_t::monitoredNodes
CO_HBconsNode_t * monitoredNodes
From CO_HBconsumer_init()
Definition: CO_HBconsumer.h:141
+
CO_ReturnError_t
CO_ReturnError_t
Return values of some CANopen functions.
Definition: CO_driver.h:488
+
bool_t
unsigned char bool_t
Boolean data type for general use.
Definition: CO_driver.h:141
+
CO_HBconsumer_UNKNOWN
@ CO_HBconsumer_UNKNOWN
Consumer enabled, but no heartbeat received yet.
Definition: CO_HBconsumer.h:71
+
CO_HBconsumer_initCallbackRemoteReset
void CO_HBconsumer_initCallbackRemoteReset(CO_HBconsumer_t *HBcons, uint8_t idx, void *object, void(*pFunctSignal)(uint8_t nodeId, uint8_t idx, void *object))
Initialize Heartbeat consumer remote reset detected callback function.
+
CO_HBconsumer_state_t
CO_HBconsumer_state_t
Heartbeat state of a node.
Definition: CO_HBconsumer.h:69
+
CO_HBconsumer_TIMEOUT
@ CO_HBconsumer_TIMEOUT
No heatbeat received for set time.
Definition: CO_HBconsumer.h:73
+
CO_HBconsumer_initCallbackTimeout
void CO_HBconsumer_initCallbackTimeout(CO_HBconsumer_t *HBcons, uint8_t idx, void *object, void(*pFunctSignal)(uint8_t nodeId, uint8_t idx, void *object))
Initialize Heartbeat consumer timeout callback function.
+
int8_t
signed char int8_t
INTEGER8 in CANopen (0002h), 8-bit signed integer.
Definition: CO_driver.h:143
+
CO_HBconsumer_getIdxByNodeId
int8_t CO_HBconsumer_getIdxByNodeId(CO_HBconsumer_t *HBcons, uint8_t nodeId)
Get the heartbeat producer object index by node ID.
+
CO_SDOserver.h
CANopen Service Data Object - server protocol.
+
CO_HBconsumer_t::pFunctSignalObjectNmtChanged
void * pFunctSignalObjectNmtChanged
Pointer to object.
Definition: CO_HBconsumer.h:159
+
CO_HBconsNode_t::functSignalObjectRemoteReset
void * functSignalObjectRemoteReset
Pointer to object.
Definition: CO_HBconsumer.h:127
+
CO_HBconsumer_UNCONFIGURED
@ CO_HBconsumer_UNCONFIGURED
Consumer entry inactive.
Definition: CO_HBconsumer.h:70
+
CO_HBconsNode_t::time_us
uint32_t time_us
Consumer heartbeat time from OD.
Definition: CO_HBconsumer.h:90
+
CO_HBconsNode_t::functSignalObjectHbStarted
void * functSignalObjectHbStarted
Pointer to object.
Definition: CO_HBconsumer.h:117
+
CO_HBconsumer_t
Heartbeat consumer object.
Definition: CO_HBconsumer.h:138
+
CO_HBconsumer_t::em
CO_EM_t * em
From CO_HBconsumer_init()
Definition: CO_HBconsumer.h:139
+
CO_HBconsumer_t::allMonitoredActive
bool_t allMonitoredActive
True, if all monitored nodes are active or no node is monitored.
Definition: CO_HBconsumer.h:145
+
CO_HBconsNode_t::timeoutTimer
uint32_t timeoutTimer
Time since last heartbeat received.
Definition: CO_HBconsumer.h:88
+
CO_HBconsumer_init
CO_ReturnError_t CO_HBconsumer_init(CO_HBconsumer_t *HBcons, CO_EM_t *em, CO_SDO_t *SDO, const uint32_t HBconsTime[], CO_HBconsNode_t monitoredNodes[], uint8_t numberOfMonitoredNodes, CO_CANmodule_t *CANdevRx, uint16_t CANdevRxIdxStart)
Initialize Heartbeat consumer object.
+
CO_HBconsumer_process
void CO_HBconsumer_process(CO_HBconsumer_t *HBcons, bool_t NMTisPreOrOperational, uint32_t timeDifference_us, uint32_t *timerNext_us)
Process Heartbeat consumer object.
+
CO_HBconsumer_t::NMTisPreOrOperationalPrev
bool_t NMTisPreOrOperationalPrev
previous state of var
Definition: CO_HBconsumer.h:149
+
CO_CONFIG_HB_CONS
#define CO_CONFIG_HB_CONS
Configuration of Heartbeat consumer.
Definition: CO_config.h:153
+
CO_EM_t
Emergency object.
Definition: CO_Emergency.h:369
+
CO_HBconsNode_t::NMTstatePrev
CO_NMT_internalState_t NMTstatePrev
Previous value of the remote node (Heartbeat payload)
Definition: CO_HBconsumer.h:103
+
CO_NMT_internalState_t
CO_NMT_internalState_t
Internal network state of the CANopen node.
Definition: CO_NMT_Heartbeat.h:77
+
CO_HBconsumer_initCallbackHeartbeatStarted
void CO_HBconsumer_initCallbackHeartbeatStarted(CO_HBconsumer_t *HBcons, uint8_t idx, void *object, void(*pFunctSignal)(uint8_t nodeId, uint8_t idx, void *object))
Initialize Heartbeat consumer started callback function.
+
CO_HBconsumer_t::CANdevRx
CO_CANmodule_t * CANdevRx
From CO_HBconsumer_init()
Definition: CO_HBconsumer.h:150
+
CO_HBconsumer_getState
CO_HBconsumer_state_t CO_HBconsumer_getState(CO_HBconsumer_t *HBcons, uint8_t idx)
Get the current state of a heartbeat producer by the index in OD 0x1016.
+
CO_HBconsumer_t::allMonitoredOperational
uint8_t allMonitoredOperational
True, if all monitored nodes are NMT operational or no node is monitored.
Definition: CO_HBconsumer.h:148
+
CO_HBconsNode_t::pFunctSignalObjectNmtChanged
void * pFunctSignalObjectNmtChanged
Pointer to object.
Definition: CO_HBconsumer.h:112
+
CO_HBconsNode_t::functSignalObjectTimeout
void * functSignalObjectTimeout
Pointer to object.
Definition: CO_HBconsumer.h:122
+
CO_HBconsNode_t::nodeId
uint8_t nodeId
Node Id of the monitored node.
Definition: CO_HBconsumer.h:82
+
CO_HBconsumer_t::numberOfMonitoredNodes
uint8_t numberOfMonitoredNodes
From CO_HBconsumer_init()
Definition: CO_HBconsumer.h:142
+
CO_CANmodule_t
Complete CAN module object.
Definition: CO_driver.h:319
+
CO_HBconsNode_t::NMTstate
CO_NMT_internalState_t NMTstate
Of the remote node (Heartbeat payload)
Definition: CO_HBconsumer.h:84
+
CO_HBconsumer_t::HBconsTime
const uint32_t * HBconsTime
From CO_HBconsumer_init()
Definition: CO_HBconsumer.h:140
+
CO_HBconsumer_initCallbackNmtChanged
void CO_HBconsumer_initCallbackNmtChanged(CO_HBconsumer_t *HBcons, uint8_t idx, void *object, void(*pFunctSignal)(uint8_t nodeId, uint8_t idx, CO_NMT_internalState_t state, void *object))
Initialize Heartbeat consumer NMT changed callback function.
+
CO_HBconsNode_t::CANrxNew
volatile void * CANrxNew
Indication if new Heartbeat message received from the CAN bus.
Definition: CO_HBconsumer.h:92
+
CO_HBconsNode_t
One monitored node inside CO_HBconsumer_t.
Definition: CO_HBconsumer.h:80
+
CO_HBconsNode_t::HBstate
CO_HBconsumer_state_t HBstate
Current heartbeat state.
Definition: CO_HBconsumer.h:86
+
CO_HBconsumer_ACTIVE
@ CO_HBconsumer_ACTIVE
Heartbeat received within set time.
Definition: CO_HBconsumer.h:72
+
uint8_t
unsigned char uint8_t
UNSIGNED8 in CANopen (0005h), 8-bit unsigned integer.
Definition: CO_driver.h:151
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/CO__LEDs_8h.html b/Devices/Libraries/Systems/CANopenSocket/docs/CO__LEDs_8h.html new file mode 100755 index 0000000..c9c4ae0 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/CO__LEDs_8h.html @@ -0,0 +1,172 @@ + + + + + + + +CANopenNode: 303/CO_LEDs.h File Reference + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Data Structures | +Macros | +Enumerations | +Functions
+
+
CO_LEDs.h File Reference
CANopen_303 » LED indicators
+
+
+ +

CANopen Indicator specification (CiA 303-3 v1.4.0) +More...

+
#include "301/CO_driver.h"
+#include "301/CO_NMT_Heartbeat.h"
+
+

Go to the source code of this file.

+ + + + + +

+Data Structures

struct  CO_LEDs_t
 LEDs object, initialized by CO_LEDs_init() More...
 
+ + + + + + + +

+Macros

+#define CO_LED_RED(LEDs, BITFIELD)   (((LEDs)->LEDred & BITFIELD) ? 1 : 0)
 Get on/off state for green led for specified bitfield.
 
+#define CO_LED_GREEN(LEDs, BITFIELD)   (((LEDs)->LEDgreen & BITFIELD) ? 1 : 0)
 Get on/off state for green led for specified bitfield.
 
+ + + + +

+Enumerations

enum  CO_LED_BITFIELD_t {
+  CO_LED_flicker = 0x01, +CO_LED_blink = 0x02, +CO_LED_flash_1 = 0x04, +CO_LED_flash_2 = 0x08, +
+  CO_LED_flash_3 = 0x10, +CO_LED_flash_4 = 0x20, +CO_LED_CANopen = 0x80 +
+ }
 Bitfield for combining with red or green led. More...
 
+ + + + + + + +

+Functions

CO_ReturnError_t CO_LEDs_init (CO_LEDs_t *LEDs)
 Initialize LEDs object. More...
 
void CO_LEDs_process (CO_LEDs_t *LEDs, uint32_t timeDifference_us, CO_NMT_internalState_t NMTstate, bool_t LSSconfig, bool_t ErrCANbusOff, bool_t ErrCANbusWarn, bool_t ErrRpdo, bool_t ErrSync, bool_t ErrHbCons, bool_t ErrOther, bool_t firmwareDownload, uint32_t *timerNext_us)
 Process indicator states. More...
 
+

Detailed Description

+

CANopen Indicator specification (CiA 303-3 v1.4.0)

+
Author
Janez Paternoster
+
Copyright
2020 Janez Paternoster
+

This file is part of CANopenNode, an opensource CANopen Stack. Project home page is https://github.com/CANopenNode/CANopenNode. For more information on CANopen see http://www.can-cia.org/.

+

Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. You may obtain a copy of the License at 

http://www.apache.org/licenses/LICENSE-2.0
+

Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License.

+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/CO__LEDs_8h.js b/Devices/Libraries/Systems/CANopenSocket/docs/CO__LEDs_8h.js new file mode 100755 index 0000000..8c2ade5 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/CO__LEDs_8h.js @@ -0,0 +1,16 @@ +var CO__LEDs_8h = +[ + [ "CO_LED_RED", "group__CO__LEDs.html#ga38a415372f20b9444f254a205aa511e8", null ], + [ "CO_LED_GREEN", "group__CO__LEDs.html#ga3e01e6ec590d6d5c7b887b48557498f4", null ], + [ "CO_LED_BITFIELD_t", "group__CO__LEDs.html#ga366de3822a3da8478e97b248bed641fb", [ + [ "CO_LED_flicker", "group__CO__LEDs.html#gga366de3822a3da8478e97b248bed641fba9838518b974c263a401a089901cdcf54", null ], + [ "CO_LED_blink", "group__CO__LEDs.html#gga366de3822a3da8478e97b248bed641fbaa24d4647d37adc17e1d3c242a42f6b68", null ], + [ "CO_LED_flash_1", "group__CO__LEDs.html#gga366de3822a3da8478e97b248bed641fba88e905d94927b3c626b50a48537c7b73", null ], + [ "CO_LED_flash_2", "group__CO__LEDs.html#gga366de3822a3da8478e97b248bed641fbae751e14d72d829b2b7f9a9c1e98e0612", null ], + [ "CO_LED_flash_3", "group__CO__LEDs.html#gga366de3822a3da8478e97b248bed641fba250a08a71c4bbca9761f0dfa54d37938", null ], + [ "CO_LED_flash_4", "group__CO__LEDs.html#gga366de3822a3da8478e97b248bed641fbadd3ec0da4d999e5ffc107d7891c26667", null ], + [ "CO_LED_CANopen", "group__CO__LEDs.html#gga366de3822a3da8478e97b248bed641fbaea338dad48dda75ef1ebac0948093148", null ] + ] ], + [ "CO_LEDs_init", "group__CO__LEDs.html#gadafdaf5de4c227a3df17cbcf1d81be69", null ], + [ "CO_LEDs_process", "group__CO__LEDs.html#gac008ef501f913c9df1ee79c4b071ad80", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/CO__LEDs_8h_source.html b/Devices/Libraries/Systems/CANopenSocket/docs/CO__LEDs_8h_source.html new file mode 100755 index 0000000..b8e5872 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/CO__LEDs_8h_source.html @@ -0,0 +1,205 @@ + + + + + + + +CANopenNode: 303/CO_LEDs.h Source File + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+
CO_LEDs.h
+
+
+Go to the documentation of this file.
1 
+
26 #ifndef CO_LEDS_H
+
27 #define CO_LEDS_H
+
28 
+
29 #include "301/CO_driver.h"
+
30 #include "301/CO_NMT_Heartbeat.h"
+
31 
+
32 /* default configuration, see CO_config.h */
+
33 #ifndef CO_CONFIG_LEDS
+
34 #define CO_CONFIG_LEDS (CO_CONFIG_LEDS_ENABLE)
+
35 #endif
+
36 
+
37 #if ((CO_CONFIG_LEDS) & CO_CONFIG_LEDS_ENABLE) || defined CO_DOXYGEN
+
38 
+
39 #ifdef __cplusplus
+
40 extern "C" {
+
41 #endif
+
42 
+
74 typedef enum {
+
75  CO_LED_flicker = 0x01,
+
76  CO_LED_blink = 0x02,
+
77  CO_LED_flash_1 = 0x04,
+
78  CO_LED_flash_2 = 0x08,
+
79  CO_LED_flash_3 = 0x10,
+
80  CO_LED_flash_4 = 0x20,
+
81  CO_LED_CANopen = 0x80
+
82 } CO_LED_BITFIELD_t;
+
83 
+
85 #define CO_LED_RED(LEDs, BITFIELD) (((LEDs)->LEDred & BITFIELD) ? 1 : 0)
+
86 
+
87 #define CO_LED_GREEN(LEDs, BITFIELD) (((LEDs)->LEDgreen & BITFIELD) ? 1 : 0)
+
88 
+
89 
+
93 typedef struct{
+
94  uint32_t LEDtmr50ms;
+
95  uint8_t LEDtmr200ms;
+
96  uint8_t LEDtmrflash_1;
+
97  uint8_t LEDtmrflash_2;
+
98  uint8_t LEDtmrflash_3;
+
99  uint8_t LEDtmrflash_4;
+
100  uint8_t LEDred;
+
101  uint8_t LEDgreen;
+
102 } CO_LEDs_t;
+
103 
+
104 
+
114 CO_ReturnError_t CO_LEDs_init(CO_LEDs_t *LEDs);
+
115 
+
116 
+
136 void CO_LEDs_process(CO_LEDs_t *LEDs,
+
137  uint32_t timeDifference_us,
+
138  CO_NMT_internalState_t NMTstate,
+
139  bool_t LSSconfig,
+
140  bool_t ErrCANbusOff,
+
141  bool_t ErrCANbusWarn,
+
142  bool_t ErrRpdo,
+
143  bool_t ErrSync,
+
144  bool_t ErrHbCons,
+
145  bool_t ErrOther,
+
146  bool_t firmwareDownload,
+
147  uint32_t *timerNext_us);
+
148  /* CO_LEDs */
+
150 
+
151 #ifdef __cplusplus
+
152 }
+
153 #endif /*__cplusplus*/
+
154 
+
155 #endif /* (CO_CONFIG_LEDS) & CO_CONFIG_LEDS_ENABLE */
+
156 
+
157 #endif /* CO_LEDS_H */
+
+
+
uint32_t
unsigned long int uint32_t
UNSIGNED32 in CANopen (0007h), 32-bit unsigned integer.
Definition: CO_driver.h:155
+
CO_driver.h
Interface between CAN hardware and CANopenNode.
+
CO_LEDs_t::LEDred
uint8_t LEDred
red led CO_LED_BITFIELD_t
Definition: CO_LEDs.h:100
+
CO_LEDs_init
CO_ReturnError_t CO_LEDs_init(CO_LEDs_t *LEDs)
Initialize LEDs object.
+
CO_LED_BITFIELD_t
CO_LED_BITFIELD_t
Bitfield for combining with red or green led.
Definition: CO_LEDs.h:74
+
CO_LED_flash_3
@ CO_LED_flash_3
LED triple flash.
Definition: CO_LEDs.h:79
+
CO_LEDs_t
LEDs object, initialized by CO_LEDs_init()
Definition: CO_LEDs.h:93
+
CO_ReturnError_t
CO_ReturnError_t
Return values of some CANopen functions.
Definition: CO_driver.h:488
+
CO_LED_blink
@ CO_LED_blink
LED blinking 2,5Hz.
Definition: CO_LEDs.h:76
+
bool_t
unsigned char bool_t
Boolean data type for general use.
Definition: CO_driver.h:141
+
CO_LEDs_t::LEDtmrflash_1
uint8_t LEDtmrflash_1
single flash led timer
Definition: CO_LEDs.h:96
+
CO_LEDs_t::LEDgreen
uint8_t LEDgreen
green led CO_LED_BITFIELD_t
Definition: CO_LEDs.h:101
+
CO_LEDs_t::LEDtmrflash_3
uint8_t LEDtmrflash_3
triple flash led timer
Definition: CO_LEDs.h:98
+
CO_LED_CANopen
@ CO_LED_CANopen
LED CANopen according to CiA 303-3.
Definition: CO_LEDs.h:81
+
CO_LEDs_process
void CO_LEDs_process(CO_LEDs_t *LEDs, uint32_t timeDifference_us, CO_NMT_internalState_t NMTstate, bool_t LSSconfig, bool_t ErrCANbusOff, bool_t ErrCANbusWarn, bool_t ErrRpdo, bool_t ErrSync, bool_t ErrHbCons, bool_t ErrOther, bool_t firmwareDownload, uint32_t *timerNext_us)
Process indicator states.
+
CO_LED_flash_4
@ CO_LED_flash_4
LED quadruple flash.
Definition: CO_LEDs.h:80
+
CO_LEDs_t::LEDtmrflash_2
uint8_t LEDtmrflash_2
double flash led timer
Definition: CO_LEDs.h:97
+
CO_LEDs_t::LEDtmrflash_4
uint8_t LEDtmrflash_4
quadruple flash led timer
Definition: CO_LEDs.h:99
+
CO_NMT_internalState_t
CO_NMT_internalState_t
Internal network state of the CANopen node.
Definition: CO_NMT_Heartbeat.h:77
+
CO_NMT_Heartbeat.h
CANopen Network management and Heartbeat producer protocol.
+
CO_LEDs_t::LEDtmr50ms
uint32_t LEDtmr50ms
50ms led timer
Definition: CO_LEDs.h:94
+
CO_LED_flicker
@ CO_LED_flicker
LED flickering 10Hz.
Definition: CO_LEDs.h:75
+
CO_LEDs_t::LEDtmr200ms
uint8_t LEDtmr200ms
200ms led timer
Definition: CO_LEDs.h:95
+
CO_LED_flash_2
@ CO_LED_flash_2
LED double flash.
Definition: CO_LEDs.h:78
+
CO_LED_flash_1
@ CO_LED_flash_1
LED single flash.
Definition: CO_LEDs.h:77
+
uint8_t
unsigned char uint8_t
UNSIGNED8 in CANopen (0005h), 8-bit unsigned integer.
Definition: CO_driver.h:151
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/CO__LSS_8h.html b/Devices/Libraries/Systems/CANopenSocket/docs/CO__LSS_8h.html new file mode 100755 index 0000000..cb218b1 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/CO__LSS_8h.html @@ -0,0 +1,243 @@ + + + + + + + +CANopenNode: 305/CO_LSS.h File Reference + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Data Structures | +Macros | +Enumerations | +Variables
+
+
CO_LSS.h File Reference
CANopen_305 » LSS
+
+
+ +

CANopen Layer Setting Services protocol (common). +More...

+
#include "301/CO_driver.h"
+
+

Go to the source code of this file.

+ + + + + +

+Data Structures

union  CO_LSS_address_t
 The LSS address is a 128 bit number, uniquely identifying each node. More...
 
+ + + + + + + + + + + + + +

+Macros

+#define CO_LSS_BIT_TIMING_VALID(index)   (index != 5 && (index >= CO_LSS_BIT_TIMING_1000 && index <= CO_LSS_BIT_TIMING_AUTO))
 Macro to check if index contains valid bit timing.
 
+#define CO_LSS_NODE_ID_ASSIGNMENT   0xFFU
 Invalid node ID triggers node ID assignment.
 
+#define CO_LSS_NODE_ID_VALID(nid)   ((nid >= 1 && nid <= 0x7F) || nid == CO_LSS_NODE_ID_ASSIGNMENT)
 Macro to check if node id is valid.
 
+#define CO_LSS_ADDRESS_EQUAL(a1, a2)
 Macro to check if two LSS addresses are equal.
 
+ + + + + + + + + + + + + + + + + + + + + + + + + +

+Enumerations

enum  CO_LSS_cs_t {
+  CO_LSS_SWITCH_STATE_GLOBAL = 0x04U, +CO_LSS_SWITCH_STATE_SEL_VENDOR = 0x40U, +CO_LSS_SWITCH_STATE_SEL_PRODUCT = 0x41U, +CO_LSS_SWITCH_STATE_SEL_REV = 0x42U, +
+  CO_LSS_SWITCH_STATE_SEL_SERIAL = 0x43U, +CO_LSS_SWITCH_STATE_SEL = 0x44U, +CO_LSS_CFG_NODE_ID = 0x11U, +CO_LSS_CFG_BIT_TIMING = 0x13U, +
+  CO_LSS_CFG_ACTIVATE_BIT_TIMING = 0x15U, +CO_LSS_CFG_STORE = 0x17U, +CO_LSS_IDENT_SLAVE = 0x4FU, +CO_LSS_IDENT_FASTSCAN = 0x51U, +
+  CO_LSS_INQUIRE_VENDOR = 0x5AU, +CO_LSS_INQUIRE_PRODUCT = 0x5BU, +CO_LSS_INQUIRE_REV = 0x5CU, +CO_LSS_INQUIRE_SERIAL = 0x5DU, +
+  CO_LSS_INQUIRE_NODE_ID = 0x5EU +
+ }
 LSS protocol command specifiers. More...
 
enum  CO_LSS_cfgNodeId_t { CO_LSS_CFG_NODE_ID_OK = 0x00U, +CO_LSS_CFG_NODE_ID_OUT_OF_RANGE = 0x01U, +CO_LSS_CFG_NODE_ID_MANUFACTURER = 0xFFU + }
 Error codes for Configure node ID protocol. More...
 
enum  CO_LSS_cfgBitTiming_t { CO_LSS_CFG_BIT_TIMING_OK = 0x00U, +CO_LSS_CFG_BIT_TIMING_OUT_OF_RANGE = 0x01U, +CO_LSS_CFG_BIT_TIMING_MANUFACTURER = 0xFFU + }
 Error codes for Configure bit timing parameters protocol. More...
 
enum  CO_LSS_cfgStore_t { CO_LSS_CFG_STORE_OK = 0x00U, +CO_LSS_CFG_STORE_NOT_SUPPORTED = 0x01U, +CO_LSS_CFG_STORE_FAILED = 0x02U, +CO_LSS_CFG_STORE_MANUFACTURER = 0xFFU + }
 Error codes for Store configuration protocol. More...
 
enum  CO_LSS_fastscan_bitcheck { CO_LSS_FASTSCAN_BIT0 = 0x00U, +CO_LSS_FASTSCAN_BIT31 = 0x1FU, +CO_LSS_FASTSCAN_CONFIRM = 0x80U + }
 Fastscan BitCheck. More...
 
enum  CO_LSS_fastscan_lss_sub_next { CO_LSS_FASTSCAN_VENDOR_ID = 0, +CO_LSS_FASTSCAN_PRODUCT = 1, +CO_LSS_FASTSCAN_REV = 2, +CO_LSS_FASTSCAN_SERIAL = 3 + }
 Fastscan LSSsub, LSSnext. More...
 
enum  CO_LSS_state_t { CO_LSS_STATE_WAITING = 0, +CO_LSS_STATE_CONFIGURATION = 1 + }
 LSS finite state automaton. More...
 
enum  CO_LSS_bitTimingTable_t {
+  CO_LSS_BIT_TIMING_1000 = 0, +CO_LSS_BIT_TIMING_800 = 1, +CO_LSS_BIT_TIMING_500 = 2, +CO_LSS_BIT_TIMING_250 = 3, +
+  CO_LSS_BIT_TIMING_125 = 4, +CO_LSS_BIT_TIMING_50 = 6, +CO_LSS_BIT_TIMING_20 = 7, +CO_LSS_BIT_TIMING_10 = 8, +
+  CO_LSS_BIT_TIMING_AUTO = 9 +
+ }
 Definition of table_index for /CiA301/ bit timing table. More...
 
+ + + + +

+Variables

+static const uint16_t CO_LSS_bitTimingTableLookup []
 Lookup table for conversion between bit timing table and numerical bit rate.
 
+

Detailed Description

+

CANopen Layer Setting Services protocol (common).

+
Author
Martin Wagner
+
Copyright
2017 - 2020 Neuberger Gebaeudeautomation GmbH
+

This file is part of CANopenNode, an opensource CANopen Stack. Project home page is https://github.com/CANopenNode/CANopenNode. For more information on CANopen see http://www.can-cia.org/.

+

Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. You may obtain a copy of the License at 

http://www.apache.org/licenses/LICENSE-2.0
+

Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License.

+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/CO__LSS_8h.js b/Devices/Libraries/Systems/CANopenSocket/docs/CO__LSS_8h.js new file mode 100755 index 0000000..e34b7b8 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/CO__LSS_8h.js @@ -0,0 +1,69 @@ +var CO__LSS_8h = +[ + [ "CO_LSS_BIT_TIMING_VALID", "group__CO__LSS.html#gaf1a3d7df8dcd93e4a0e0b29aa6b003a1", null ], + [ "CO_LSS_NODE_ID_ASSIGNMENT", "group__CO__LSS.html#ga02771497ab59dd86f2dbe59cd1fb04b1", null ], + [ "CO_LSS_NODE_ID_VALID", "group__CO__LSS.html#ga939b17fdd44126fb758db46b9cadf79c", null ], + [ "CO_LSS_ADDRESS_EQUAL", "group__CO__LSS.html#ga341d156c334f2a3c2523f03eb24f4710", null ], + [ "CO_LSS_cs_t", "group__CO__LSS.html#gacc7cba1fb1f1f595506751d6af385964", [ + [ "CO_LSS_SWITCH_STATE_GLOBAL", "group__CO__LSS.html#ggacc7cba1fb1f1f595506751d6af385964a6c9f8aaef024d11e50c007b881208113", null ], + [ "CO_LSS_SWITCH_STATE_SEL_VENDOR", "group__CO__LSS.html#ggacc7cba1fb1f1f595506751d6af385964a620895a76069780eb5df8188b6c8a2de", null ], + [ "CO_LSS_SWITCH_STATE_SEL_PRODUCT", "group__CO__LSS.html#ggacc7cba1fb1f1f595506751d6af385964a9caf25d2d4a28e279b1bc364d303ee7d", null ], + [ "CO_LSS_SWITCH_STATE_SEL_REV", "group__CO__LSS.html#ggacc7cba1fb1f1f595506751d6af385964ae02d526a2c1170babeccffd00d477db5", null ], + [ "CO_LSS_SWITCH_STATE_SEL_SERIAL", "group__CO__LSS.html#ggacc7cba1fb1f1f595506751d6af385964add363bec1d5ff239c847425f8b94718d", null ], + [ "CO_LSS_SWITCH_STATE_SEL", "group__CO__LSS.html#ggacc7cba1fb1f1f595506751d6af385964a97495fe55645d498550e0c05417e2c22", null ], + [ "CO_LSS_CFG_NODE_ID", "group__CO__LSS.html#ggacc7cba1fb1f1f595506751d6af385964a777432f5b616a250a9db8bf7328b0a59", null ], + [ "CO_LSS_CFG_BIT_TIMING", "group__CO__LSS.html#ggacc7cba1fb1f1f595506751d6af385964afd4b839204b66547e5ebb0e4ff9c4481", null ], + [ "CO_LSS_CFG_ACTIVATE_BIT_TIMING", "group__CO__LSS.html#ggacc7cba1fb1f1f595506751d6af385964a1046863405c6de85d0d86088d9c034cc", null ], + [ "CO_LSS_CFG_STORE", "group__CO__LSS.html#ggacc7cba1fb1f1f595506751d6af385964ab0a136e255e8c2c32984881487b414d9", null ], + [ "CO_LSS_IDENT_SLAVE", "group__CO__LSS.html#ggacc7cba1fb1f1f595506751d6af385964ad0ddd8e3d472f4d85de8613e7f35902a", null ], + [ "CO_LSS_IDENT_FASTSCAN", "group__CO__LSS.html#ggacc7cba1fb1f1f595506751d6af385964a9e4bd7f4a726aee66157ac9aac446ddc", null ], + [ "CO_LSS_INQUIRE_VENDOR", "group__CO__LSS.html#ggacc7cba1fb1f1f595506751d6af385964ae28351cd1b60bbdd045a9f79cb506023", null ], + [ "CO_LSS_INQUIRE_PRODUCT", "group__CO__LSS.html#ggacc7cba1fb1f1f595506751d6af385964a5fba5f8bd0f6a91b45fda117556b994c", null ], + [ "CO_LSS_INQUIRE_REV", "group__CO__LSS.html#ggacc7cba1fb1f1f595506751d6af385964a6f412845cd8cd4ab62a54a988ccc384c", null ], + [ "CO_LSS_INQUIRE_SERIAL", "group__CO__LSS.html#ggacc7cba1fb1f1f595506751d6af385964a55f0698aa15abf17c41cd344df055184", null ], + [ "CO_LSS_INQUIRE_NODE_ID", "group__CO__LSS.html#ggacc7cba1fb1f1f595506751d6af385964af5d369fc4d3d860dc43de041b9fd59f5", null ] + ] ], + [ "CO_LSS_cfgNodeId_t", "group__CO__LSS.html#gaf8a13f567f8f405e4aae68268ba5d0a5", [ + [ "CO_LSS_CFG_NODE_ID_OK", "group__CO__LSS.html#ggaf8a13f567f8f405e4aae68268ba5d0a5abe91f1d0e99fa890fe69a3e60aab6c2b", null ], + [ "CO_LSS_CFG_NODE_ID_OUT_OF_RANGE", "group__CO__LSS.html#ggaf8a13f567f8f405e4aae68268ba5d0a5a79c95a7e63e6ff09fcbe5494ef59eed5", null ], + [ "CO_LSS_CFG_NODE_ID_MANUFACTURER", "group__CO__LSS.html#ggaf8a13f567f8f405e4aae68268ba5d0a5aa23e0ca77dfb47ff4a1d48ddfaebc98e", null ] + ] ], + [ "CO_LSS_cfgBitTiming_t", "group__CO__LSS.html#ga28b8651550d1719c38cd307f4ef0a8ac", [ + [ "CO_LSS_CFG_BIT_TIMING_OK", "group__CO__LSS.html#gga28b8651550d1719c38cd307f4ef0a8aca028ded96599022c4416e3d8c0798456a", null ], + [ "CO_LSS_CFG_BIT_TIMING_OUT_OF_RANGE", "group__CO__LSS.html#gga28b8651550d1719c38cd307f4ef0a8acac089dd862b289dfc3f6bae0f30409625", null ], + [ "CO_LSS_CFG_BIT_TIMING_MANUFACTURER", "group__CO__LSS.html#gga28b8651550d1719c38cd307f4ef0a8aca3f5cbbebba617a9c12a7ed919a541255", null ] + ] ], + [ "CO_LSS_cfgStore_t", "group__CO__LSS.html#ga1e4e8c43143125ebe8912de81464bd9f", [ + [ "CO_LSS_CFG_STORE_OK", "group__CO__LSS.html#gga1e4e8c43143125ebe8912de81464bd9fa0f0407dee97a1e5e5d26cc4c717103cc", null ], + [ "CO_LSS_CFG_STORE_NOT_SUPPORTED", "group__CO__LSS.html#gga1e4e8c43143125ebe8912de81464bd9faf78b03384da05bedcc45016d10dc0c3b", null ], + [ "CO_LSS_CFG_STORE_FAILED", "group__CO__LSS.html#gga1e4e8c43143125ebe8912de81464bd9fad7f12cd5d1125e97d7b9bacac4b80d69", null ], + [ "CO_LSS_CFG_STORE_MANUFACTURER", "group__CO__LSS.html#gga1e4e8c43143125ebe8912de81464bd9fa7ece37a5aabe812068efe6a2780f31cc", null ] + ] ], + [ "CO_LSS_fastscan_bitcheck", "group__CO__LSS.html#ga65751e78ae5f2674cc7205e13967f7c0", [ + [ "CO_LSS_FASTSCAN_BIT0", "group__CO__LSS.html#gga65751e78ae5f2674cc7205e13967f7c0a2db8b358b4954e5989a347e1e308eb20", null ], + [ "CO_LSS_FASTSCAN_BIT31", "group__CO__LSS.html#gga65751e78ae5f2674cc7205e13967f7c0a48f5e26f67114198d945f37f8f713979", null ], + [ "CO_LSS_FASTSCAN_CONFIRM", "group__CO__LSS.html#gga65751e78ae5f2674cc7205e13967f7c0a72f67194903c03b688373ef859b66a0f", null ] + ] ], + [ "CO_LSS_fastscan_lss_sub_next", "group__CO__LSS.html#ga1ce707d287b285e7d148f37f93e0f02a", [ + [ "CO_LSS_FASTSCAN_VENDOR_ID", "group__CO__LSS.html#gga1ce707d287b285e7d148f37f93e0f02aa4eb5786c488953cdb2d5ffbb25c15298", null ], + [ "CO_LSS_FASTSCAN_PRODUCT", "group__CO__LSS.html#gga1ce707d287b285e7d148f37f93e0f02aa6514ca82752d5496904388a0589da209", null ], + [ "CO_LSS_FASTSCAN_REV", "group__CO__LSS.html#gga1ce707d287b285e7d148f37f93e0f02aa0e153eebb470156f5d0a27caac2bc71f", null ], + [ "CO_LSS_FASTSCAN_SERIAL", "group__CO__LSS.html#gga1ce707d287b285e7d148f37f93e0f02aabe98c25d444fa5971bc81f775cd6bb35", null ] + ] ], + [ "CO_LSS_state_t", "group__CO__LSS.html#gaaa9a270e40ea09850e1661e5aeb080ad", [ + [ "CO_LSS_STATE_WAITING", "group__CO__LSS.html#ggaaa9a270e40ea09850e1661e5aeb080adac688ea4e90b7dfac437a44536f2af8db", null ], + [ "CO_LSS_STATE_CONFIGURATION", "group__CO__LSS.html#ggaaa9a270e40ea09850e1661e5aeb080ada69cc3fe20e50dcd6f7bad8c0b887ff89", null ] + ] ], + [ "CO_LSS_bitTimingTable_t", "group__CO__LSS.html#gacb4c13e75306153eafd535e55ba0ca2c", [ + [ "CO_LSS_BIT_TIMING_1000", "group__CO__LSS.html#ggacb4c13e75306153eafd535e55ba0ca2ca4cc0aa1f074ddaa5db4fdc0ed437a0b7", null ], + [ "CO_LSS_BIT_TIMING_800", "group__CO__LSS.html#ggacb4c13e75306153eafd535e55ba0ca2ca51413e9c20e1eb60f5cc160a4d30cffa", null ], + [ "CO_LSS_BIT_TIMING_500", "group__CO__LSS.html#ggacb4c13e75306153eafd535e55ba0ca2caaa4d4f1a766516c6d97a81483001e8ef", null ], + [ "CO_LSS_BIT_TIMING_250", "group__CO__LSS.html#ggacb4c13e75306153eafd535e55ba0ca2ca3fadf15163c4f45300045e3645bba3ea", null ], + [ "CO_LSS_BIT_TIMING_125", "group__CO__LSS.html#ggacb4c13e75306153eafd535e55ba0ca2ca1d93a0699e4269d809404c40565133cc", null ], + [ "CO_LSS_BIT_TIMING_50", "group__CO__LSS.html#ggacb4c13e75306153eafd535e55ba0ca2caf0e90aeacc6461bd2ace73be51fd5383", null ], + [ "CO_LSS_BIT_TIMING_20", "group__CO__LSS.html#ggacb4c13e75306153eafd535e55ba0ca2ca08fb8c70b5d185521959415ef731de82", null ], + [ "CO_LSS_BIT_TIMING_10", "group__CO__LSS.html#ggacb4c13e75306153eafd535e55ba0ca2ca7a3c4e05623e7f75fe3b622a9b0185c8", null ], + [ "CO_LSS_BIT_TIMING_AUTO", "group__CO__LSS.html#ggacb4c13e75306153eafd535e55ba0ca2ca12ca7fd4a3604cb8281fa7d58d1137e2", null ] + ] ], + [ "CO_LSS_bitTimingTableLookup", "group__CO__LSS.html#ga61bf679482f7d425ceb521ea80a0cc18", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/CO__LSS_8h_source.html b/Devices/Libraries/Systems/CANopenSocket/docs/CO__LSS_8h_source.html new file mode 100755 index 0000000..4b40461 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/CO__LSS_8h_source.html @@ -0,0 +1,303 @@ + + + + + + + +CANopenNode: 305/CO_LSS.h Source File + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+
CO_LSS.h
+
+
+Go to the documentation of this file.
1 
+
27 #ifndef CO_LSS_H
+
28 #define CO_LSS_H
+
29 
+
30 #include "301/CO_driver.h"
+
31 
+
32 /* default configuration, see CO_config.h */
+
33 #ifndef CO_CONFIG_LSS
+
34 #define CO_CONFIG_LSS (CO_CONFIG_LSS_SLAVE)
+
35 #endif
+
36 
+
37 #if ((CO_CONFIG_LSS) & (CO_CONFIG_LSS_SLAVE | CO_CONFIG_LSS_MASTER)) || defined CO_DOXYGEN
+
38 
+
39 #ifdef __cplusplus
+
40 extern "C" {
+
41 #endif
+
42 
+
86 typedef enum {
+
87  CO_LSS_SWITCH_STATE_GLOBAL = 0x04U,
+
88  CO_LSS_SWITCH_STATE_SEL_VENDOR = 0x40U,
+
89  CO_LSS_SWITCH_STATE_SEL_PRODUCT = 0x41U,
+
90  CO_LSS_SWITCH_STATE_SEL_REV = 0x42U,
+
91  CO_LSS_SWITCH_STATE_SEL_SERIAL = 0x43U,
+
92  CO_LSS_SWITCH_STATE_SEL = 0x44U,
+
93  CO_LSS_CFG_NODE_ID = 0x11U,
+
94  CO_LSS_CFG_BIT_TIMING = 0x13U,
+
95  CO_LSS_CFG_ACTIVATE_BIT_TIMING = 0x15U,
+
96  CO_LSS_CFG_STORE = 0x17U,
+
97  CO_LSS_IDENT_SLAVE = 0x4FU,
+
98  CO_LSS_IDENT_FASTSCAN = 0x51U,
+
99  CO_LSS_INQUIRE_VENDOR = 0x5AU,
+
100  CO_LSS_INQUIRE_PRODUCT = 0x5BU,
+
101  CO_LSS_INQUIRE_REV = 0x5CU,
+
102  CO_LSS_INQUIRE_SERIAL = 0x5DU,
+
103  CO_LSS_INQUIRE_NODE_ID = 0x5EU,
+
104 } CO_LSS_cs_t;
+
105 
+
109 typedef enum {
+
110  CO_LSS_CFG_NODE_ID_OK = 0x00U,
+
111  CO_LSS_CFG_NODE_ID_OUT_OF_RANGE = 0x01U,
+
112  CO_LSS_CFG_NODE_ID_MANUFACTURER = 0xFFU
+
113 } CO_LSS_cfgNodeId_t;
+
114 
+
118 typedef enum {
+
119  CO_LSS_CFG_BIT_TIMING_OK = 0x00U,
+
120  CO_LSS_CFG_BIT_TIMING_OUT_OF_RANGE = 0x01U,
+
121  CO_LSS_CFG_BIT_TIMING_MANUFACTURER = 0xFFU
+
122 } CO_LSS_cfgBitTiming_t;
+
123 
+
127 typedef enum {
+
128  CO_LSS_CFG_STORE_OK = 0x00U,
+
129  CO_LSS_CFG_STORE_NOT_SUPPORTED = 0x01U,
+
130  CO_LSS_CFG_STORE_FAILED = 0x02U,
+
131  CO_LSS_CFG_STORE_MANUFACTURER = 0xFFU
+
132 } CO_LSS_cfgStore_t;
+
133 
+
137 typedef enum {
+
138  CO_LSS_FASTSCAN_BIT0 = 0x00U,
+
139  /* ... */
+
140  CO_LSS_FASTSCAN_BIT31 = 0x1FU,
+
141  CO_LSS_FASTSCAN_CONFIRM = 0x80U
+
142 } CO_LSS_fastscan_bitcheck;
+
143 
+
144 #define CO_LSS_FASTSCAN_BITCHECK_VALID(bit) ((bit>=CO_LSS_FASTSCAN_BIT0 && bit<=CO_LSS_FASTSCAN_BIT31) || bit==CO_LSS_FASTSCAN_CONFIRM)
+
145 
+
149 typedef enum {
+
150  CO_LSS_FASTSCAN_VENDOR_ID = 0,
+
151  CO_LSS_FASTSCAN_PRODUCT = 1,
+
152  CO_LSS_FASTSCAN_REV = 2,
+
153  CO_LSS_FASTSCAN_SERIAL = 3
+
154 } CO_LSS_fastscan_lss_sub_next;
+
155 
+
156 #define CO_LSS_FASTSCAN_LSS_SUB_NEXT_VALID(index) (index>=CO_LSS_FASTSCAN_VENDOR_ID && index<=CO_LSS_FASTSCAN_SERIAL)
+
157 
+
162 typedef union {
+
163  uint32_t addr[4];
+
164  struct {
+
165  uint32_t vendorID;
+
166  uint32_t productCode;
+
167  uint32_t revisionNumber;
+
168  uint32_t serialNumber;
+
169  } identity;
+
170 } CO_LSS_address_t;
+
171 
+
181 typedef enum {
+
182  CO_LSS_STATE_WAITING = 0,
+
183  CO_LSS_STATE_CONFIGURATION = 1,
+
184 } CO_LSS_state_t;
+
185 
+
189 typedef enum {
+
190  CO_LSS_BIT_TIMING_1000 = 0,
+
191  CO_LSS_BIT_TIMING_800 = 1,
+
192  CO_LSS_BIT_TIMING_500 = 2,
+
193  CO_LSS_BIT_TIMING_250 = 3,
+
194  CO_LSS_BIT_TIMING_125 = 4,
+
195  /* reserved = 5 */
+
196  CO_LSS_BIT_TIMING_50 = 6,
+
197  CO_LSS_BIT_TIMING_20 = 7,
+
198  CO_LSS_BIT_TIMING_10 = 8,
+
199  CO_LSS_BIT_TIMING_AUTO = 9,
+
200 } CO_LSS_bitTimingTable_t;
+
201 
+
206 static const uint16_t CO_LSS_bitTimingTableLookup[] = {
+
207  1000,
+
208  800,
+
209  500,
+
210  250,
+
211  125,
+
212  0,
+
213  50,
+
214  20,
+
215  10,
+
216  0
+
217 };
+
218 
+
222 #define CO_LSS_BIT_TIMING_VALID(index) (index != 5 && (index >= CO_LSS_BIT_TIMING_1000 && index <= CO_LSS_BIT_TIMING_AUTO))
+
223 
+
227 #define CO_LSS_NODE_ID_ASSIGNMENT 0xFFU
+
228 
+
232 #define CO_LSS_NODE_ID_VALID(nid) ((nid >= 1 && nid <= 0x7F) || nid == CO_LSS_NODE_ID_ASSIGNMENT)
+
233 
+
237 #define CO_LSS_ADDRESS_EQUAL(/*CO_LSS_address_t*/ a1, /*CO_LSS_address_t*/ a2) \
+
238  (a1.identity.productCode == a2.identity.productCode && \
+
239  a1.identity.revisionNumber == a2.identity.revisionNumber && \
+
240  a1.identity.serialNumber == a2.identity.serialNumber && \
+
241  a1.identity.vendorID == a2.identity.vendorID)
+
242  /*@defgroup CO_LSS*/
+
244 
+
245 #ifdef __cplusplus
+
246 }
+
247 #endif /*__cplusplus*/
+
248 
+
249 #endif /* (CO_CONFIG_LSS) & (CO_CONFIG_LSS_SLAVE | CO_CONFIG_LSS_MASTER) */
+
250 
+
251 #endif /*CO_LSS_H*/
+
+
+
uint32_t
unsigned long int uint32_t
UNSIGNED32 in CANopen (0007h), 32-bit unsigned integer.
Definition: CO_driver.h:155
+
CO_driver.h
Interface between CAN hardware and CANopenNode.
+
CO_LSS_FASTSCAN_BIT31
@ CO_LSS_FASTSCAN_BIT31
dito
Definition: CO_LSS.h:140
+
CO_LSS_INQUIRE_PRODUCT
@ CO_LSS_INQUIRE_PRODUCT
Inquire identity product-code protocol.
Definition: CO_LSS.h:100
+
CO_LSS_CFG_BIT_TIMING
@ CO_LSS_CFG_BIT_TIMING
Configure bit timing parameter protocol.
Definition: CO_LSS.h:94
+
CO_LSS_cfgBitTiming_t
CO_LSS_cfgBitTiming_t
Error codes for Configure bit timing parameters protocol.
Definition: CO_LSS.h:118
+
CO_LSS_BIT_TIMING_800
@ CO_LSS_BIT_TIMING_800
800kbit/s
Definition: CO_LSS.h:191
+
uint16_t
unsigned int uint16_t
UNSIGNED16 in CANopen (0006h), 16-bit unsigned integer.
Definition: CO_driver.h:153
+
CO_LSS_INQUIRE_SERIAL
@ CO_LSS_INQUIRE_SERIAL
Inquire identity serial-number protocol.
Definition: CO_LSS.h:102
+
CO_LSS_BIT_TIMING_50
@ CO_LSS_BIT_TIMING_50
50kbit/s
Definition: CO_LSS.h:196
+
CO_LSS_cs_t
CO_LSS_cs_t
LSS protocol command specifiers.
Definition: CO_LSS.h:86
+
CO_LSS_SWITCH_STATE_SEL_PRODUCT
@ CO_LSS_SWITCH_STATE_SEL_PRODUCT
Switch state selective protocol - Product code.
Definition: CO_LSS.h:89
+
CO_LSS_fastscan_lss_sub_next
CO_LSS_fastscan_lss_sub_next
Fastscan LSSsub, LSSnext.
Definition: CO_LSS.h:149
+
CO_LSS_CFG_STORE_FAILED
@ CO_LSS_CFG_STORE_FAILED
Storage media access error.
Definition: CO_LSS.h:130
+
CO_LSS_CFG_BIT_TIMING_MANUFACTURER
@ CO_LSS_CFG_BIT_TIMING_MANUFACTURER
Manufacturer specific error.
Definition: CO_LSS.h:121
+
CO_LSS_FASTSCAN_CONFIRM
@ CO_LSS_FASTSCAN_CONFIRM
All LSS slaves waiting for scan respond and previous scan is reset.
Definition: CO_LSS.h:141
+
CO_LSS_SWITCH_STATE_SEL_SERIAL
@ CO_LSS_SWITCH_STATE_SEL_SERIAL
Switch state selective protocol - Serial number.
Definition: CO_LSS.h:91
+
CO_LSS_STATE_WAITING
@ CO_LSS_STATE_WAITING
LSS FSA waiting for requests.
Definition: CO_LSS.h:182
+
CO_LSS_CFG_ACTIVATE_BIT_TIMING
@ CO_LSS_CFG_ACTIVATE_BIT_TIMING
Activate bit timing parameter protocol.
Definition: CO_LSS.h:95
+
CO_LSS_STATE_CONFIGURATION
@ CO_LSS_STATE_CONFIGURATION
LSS FSA waiting for configuration.
Definition: CO_LSS.h:183
+
CO_LSS_CFG_NODE_ID
@ CO_LSS_CFG_NODE_ID
Configure node ID protocol.
Definition: CO_LSS.h:93
+
CO_LSS_CFG_NODE_ID_OK
@ CO_LSS_CFG_NODE_ID_OK
Protocol successfully completed.
Definition: CO_LSS.h:110
+
CO_LSS_CFG_STORE_OK
@ CO_LSS_CFG_STORE_OK
Protocol successfully completed.
Definition: CO_LSS.h:128
+
CO_LSS_SWITCH_STATE_GLOBAL
@ CO_LSS_SWITCH_STATE_GLOBAL
Switch state global protocol.
Definition: CO_LSS.h:87
+
CO_LSS_state_t
CO_LSS_state_t
LSS finite state automaton.
Definition: CO_LSS.h:181
+
CO_LSS_cfgStore_t
CO_LSS_cfgStore_t
Error codes for Store configuration protocol.
Definition: CO_LSS.h:127
+
CO_LSS_SWITCH_STATE_SEL_REV
@ CO_LSS_SWITCH_STATE_SEL_REV
Switch state selective protocol - Revision number.
Definition: CO_LSS.h:90
+
CO_LSS_FASTSCAN_BIT0
@ CO_LSS_FASTSCAN_BIT0
Least significant bit of IDnumbners bit area to be checked.
Definition: CO_LSS.h:138
+
CO_LSS_INQUIRE_REV
@ CO_LSS_INQUIRE_REV
Inquire identity revision-number protocol.
Definition: CO_LSS.h:101
+
CO_LSS_BIT_TIMING_500
@ CO_LSS_BIT_TIMING_500
500kbit/s
Definition: CO_LSS.h:192
+
CO_LSS_FASTSCAN_SERIAL
@ CO_LSS_FASTSCAN_SERIAL
Serial number.
Definition: CO_LSS.h:153
+
CO_LSS_BIT_TIMING_125
@ CO_LSS_BIT_TIMING_125
125kbit/s
Definition: CO_LSS.h:194
+
CO_LSS_INQUIRE_VENDOR
@ CO_LSS_INQUIRE_VENDOR
Inquire identity vendor-ID protocol.
Definition: CO_LSS.h:99
+
CO_LSS_bitTimingTable_t
CO_LSS_bitTimingTable_t
Definition of table_index for /CiA301/ bit timing table.
Definition: CO_LSS.h:189
+
CO_LSS_CFG_BIT_TIMING_OK
@ CO_LSS_CFG_BIT_TIMING_OK
Protocol successfully completed.
Definition: CO_LSS.h:119
+
CO_LSS_BIT_TIMING_20
@ CO_LSS_BIT_TIMING_20
20kbit/s
Definition: CO_LSS.h:197
+
CO_LSS_CFG_STORE_NOT_SUPPORTED
@ CO_LSS_CFG_STORE_NOT_SUPPORTED
Store configuration not supported.
Definition: CO_LSS.h:129
+
CO_LSS_INQUIRE_NODE_ID
@ CO_LSS_INQUIRE_NODE_ID
Inquire node-ID protocol.
Definition: CO_LSS.h:103
+
CO_LSS_FASTSCAN_REV
@ CO_LSS_FASTSCAN_REV
Revision number.
Definition: CO_LSS.h:152
+
CO_LSS_BIT_TIMING_10
@ CO_LSS_BIT_TIMING_10
10kbit/s
Definition: CO_LSS.h:198
+
CO_LSS_CFG_STORE_MANUFACTURER
@ CO_LSS_CFG_STORE_MANUFACTURER
Manufacturer specific error.
Definition: CO_LSS.h:131
+
CO_LSS_IDENT_FASTSCAN
@ CO_LSS_IDENT_FASTSCAN
LSS Fastscan protocol.
Definition: CO_LSS.h:98
+
CO_LSS_CFG_NODE_ID_OUT_OF_RANGE
@ CO_LSS_CFG_NODE_ID_OUT_OF_RANGE
NID out of range.
Definition: CO_LSS.h:111
+
CO_LSS_FASTSCAN_PRODUCT
@ CO_LSS_FASTSCAN_PRODUCT
Product code.
Definition: CO_LSS.h:151
+
CO_LSS_BIT_TIMING_AUTO
@ CO_LSS_BIT_TIMING_AUTO
Automatic bit rate detection.
Definition: CO_LSS.h:199
+
CO_LSS_BIT_TIMING_1000
@ CO_LSS_BIT_TIMING_1000
1000kbit/s
Definition: CO_LSS.h:190
+
CO_LSS_BIT_TIMING_250
@ CO_LSS_BIT_TIMING_250
250kbit/s
Definition: CO_LSS.h:193
+
CO_LSS_cfgNodeId_t
CO_LSS_cfgNodeId_t
Error codes for Configure node ID protocol.
Definition: CO_LSS.h:109
+
CO_LSS_CFG_BIT_TIMING_OUT_OF_RANGE
@ CO_LSS_CFG_BIT_TIMING_OUT_OF_RANGE
Bit timing / Bit rate not supported.
Definition: CO_LSS.h:120
+
CO_LSS_CFG_STORE
@ CO_LSS_CFG_STORE
Store configuration protocol.
Definition: CO_LSS.h:96
+
CO_LSS_SWITCH_STATE_SEL_VENDOR
@ CO_LSS_SWITCH_STATE_SEL_VENDOR
Switch state selective protocol - Vendor ID.
Definition: CO_LSS.h:88
+
CO_LSS_fastscan_bitcheck
CO_LSS_fastscan_bitcheck
Fastscan BitCheck.
Definition: CO_LSS.h:137
+
CO_LSS_bitTimingTableLookup
static const uint16_t CO_LSS_bitTimingTableLookup[]
Lookup table for conversion between bit timing table and numerical bit rate.
Definition: CO_LSS.h:206
+
CO_LSS_CFG_NODE_ID_MANUFACTURER
@ CO_LSS_CFG_NODE_ID_MANUFACTURER
Manufacturer specific error.
Definition: CO_LSS.h:112
+
CO_LSS_address_t
The LSS address is a 128 bit number, uniquely identifying each node.
Definition: CO_LSS.h:162
+
CO_LSS_SWITCH_STATE_SEL
@ CO_LSS_SWITCH_STATE_SEL
Switch state selective protocol - Slave response.
Definition: CO_LSS.h:92
+
CO_LSS_IDENT_SLAVE
@ CO_LSS_IDENT_SLAVE
LSS Fastscan response.
Definition: CO_LSS.h:97
+
CO_LSS_FASTSCAN_VENDOR_ID
@ CO_LSS_FASTSCAN_VENDOR_ID
Vendor ID.
Definition: CO_LSS.h:150
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/CO__LSSmaster_8h.html b/Devices/Libraries/Systems/CANopenSocket/docs/CO__LSSmaster_8h.html new file mode 100755 index 0000000..b2e488f --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/CO__LSSmaster_8h.html @@ -0,0 +1,209 @@ + + + + + + + +CANopenNode: 305/CO_LSSmaster.h File Reference + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Data Structures | +Macros | +Enumerations | +Functions
+
+
CO_LSSmaster.h File Reference
CANopen_305 » LSS
+
+
+ +

CANopen Layer Setting Service - master protocol. +More...

+
#include "305/CO_LSS.h"
+
+

Go to the source code of this file.

+ + + + + + + + +

+Data Structures

struct  CO_LSSmaster_t
 LSS master object. More...
 
struct  CO_LSSmaster_fastscan_t
 Parameters for LSS fastscan CO_LSSmaster_IdentifyFastscan. More...
 
+ + + + +

+Macros

#define CO_LSSmaster_DEFAULT_TIMEOUT   1000U /* ms */
 Default timeout for LSS slave in ms. More...
 
+ + + + + + + +

+Enumerations

enum  CO_LSSmaster_return_t {
+  CO_LSSmaster_SCAN_FINISHED = 2, +CO_LSSmaster_WAIT_SLAVE = 1, +CO_LSSmaster_OK = 0, +CO_LSSmaster_TIMEOUT = -1, +
+  CO_LSSmaster_ILLEGAL_ARGUMENT = -2, +CO_LSSmaster_INVALID_STATE = -3, +CO_LSSmaster_SCAN_NOACK = -4, +CO_LSSmaster_SCAN_FAILED = -5, +
+  CO_LSSmaster_OK_ILLEGAL_ARGUMENT = -101, +CO_LSSmaster_OK_MANUFACTURER = -102 +
+ }
 Return values of LSS master functions. More...
 
enum  CO_LSSmaster_scantype_t { CO_LSSmaster_FS_SCAN = 0, +CO_LSSmaster_FS_SKIP = 1, +CO_LSSmaster_FS_MATCH = 2 + }
 Scan type for CO_LSSmaster_fastscan_t scan. More...
 
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

+Functions

CO_ReturnError_t CO_LSSmaster_init (CO_LSSmaster_t *LSSmaster, uint16_t timeout_ms, CO_CANmodule_t *CANdevRx, uint16_t CANdevRxIdx, uint32_t CANidLssSlave, CO_CANmodule_t *CANdevTx, uint16_t CANdevTxIdx, uint32_t CANidLssMaster)
 Initialize LSS object. More...
 
void CO_LSSmaster_changeTimeout (CO_LSSmaster_t *LSSmaster, uint16_t timeout_ms)
 Change LSS master timeout. More...
 
void CO_LSSmaster_initCallbackPre (CO_LSSmaster_t *LSSmaster, void *object, void(*pFunctSignal)(void *object))
 Initialize LSSmasterRx callback function. More...
 
CO_LSSmaster_return_t CO_LSSmaster_switchStateSelect (CO_LSSmaster_t *LSSmaster, uint32_t timeDifference_us, CO_LSS_address_t *lssAddress)
 Request LSS switch state select. More...
 
CO_LSSmaster_return_t CO_LSSmaster_switchStateDeselect (CO_LSSmaster_t *LSSmaster)
 Request LSS switch state deselect. More...
 
CO_LSSmaster_return_t CO_LSSmaster_configureBitTiming (CO_LSSmaster_t *LSSmaster, uint32_t timeDifference_us, uint16_t bit)
 Request LSS configure Bit Timing. More...
 
CO_LSSmaster_return_t CO_LSSmaster_configureNodeId (CO_LSSmaster_t *LSSmaster, uint32_t timeDifference_us, uint8_t nodeId)
 Request LSS configure node ID. More...
 
CO_LSSmaster_return_t CO_LSSmaster_configureStore (CO_LSSmaster_t *LSSmaster, uint32_t timeDifference_us)
 Request LSS store configuration. More...
 
CO_LSSmaster_return_t CO_LSSmaster_ActivateBit (CO_LSSmaster_t *LSSmaster, uint16_t switchDelay_ms)
 Request LSS activate bit timing. More...
 
CO_LSSmaster_return_t CO_LSSmaster_InquireLssAddress (CO_LSSmaster_t *LSSmaster, uint32_t timeDifference_us, CO_LSS_address_t *lssAddress)
 Request LSS inquire LSS address. More...
 
CO_LSSmaster_return_t CO_LSSmaster_Inquire (CO_LSSmaster_t *LSSmaster, uint32_t timeDifference_us, CO_LSS_cs_t lssInquireCs, uint32_t *value)
 Request LSS inquire node ID or part of LSS address. More...
 
CO_LSSmaster_return_t CO_LSSmaster_IdentifyFastscan (CO_LSSmaster_t *LSSmaster, uint32_t timeDifference_us, CO_LSSmaster_fastscan_t *fastscan)
 Select a node by LSS identify fastscan. More...
 
+

Detailed Description

+

CANopen Layer Setting Service - master protocol.

+
Author
Martin Wagner
+
Copyright
2017 - 2020 Neuberger Gebaeudeautomation GmbH
+

This file is part of CANopenNode, an opensource CANopen Stack. Project home page is https://github.com/CANopenNode/CANopenNode. For more information on CANopen see http://www.can-cia.org/.

+

Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. You may obtain a copy of the License at 

http://www.apache.org/licenses/LICENSE-2.0
+

Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License.

+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/CO__LSSmaster_8h.js b/Devices/Libraries/Systems/CANopenSocket/docs/CO__LSSmaster_8h.js new file mode 100755 index 0000000..f1eecb9 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/CO__LSSmaster_8h.js @@ -0,0 +1,33 @@ +var CO__LSSmaster_8h = +[ + [ "CO_LSSmaster_DEFAULT_TIMEOUT", "group__CO__LSSmaster.html#ga76adbab914fff5b18982dad449e0a00a", null ], + [ "CO_LSSmaster_return_t", "group__CO__LSSmaster.html#gae848ff3ff649c8a23b96053efaea4985", [ + [ "CO_LSSmaster_SCAN_FINISHED", "group__CO__LSSmaster.html#ggae848ff3ff649c8a23b96053efaea4985a0f527d1ce01820fa184ccae2510505c7", null ], + [ "CO_LSSmaster_WAIT_SLAVE", "group__CO__LSSmaster.html#ggae848ff3ff649c8a23b96053efaea4985a20fbe514e36bd92534141bb75e68eb34", null ], + [ "CO_LSSmaster_OK", "group__CO__LSSmaster.html#ggae848ff3ff649c8a23b96053efaea4985a1be85610f29d8e6cecebac9db2da3099", null ], + [ "CO_LSSmaster_TIMEOUT", "group__CO__LSSmaster.html#ggae848ff3ff649c8a23b96053efaea4985a3a61073ef2c8ef7c5be946618b95d42d", null ], + [ "CO_LSSmaster_ILLEGAL_ARGUMENT", "group__CO__LSSmaster.html#ggae848ff3ff649c8a23b96053efaea4985a9bd04e1c9923416d1d1ecb1ded6bc7b9", null ], + [ "CO_LSSmaster_INVALID_STATE", "group__CO__LSSmaster.html#ggae848ff3ff649c8a23b96053efaea4985a1e99145e94c3a6fb8cc7c48150ed5b60", null ], + [ "CO_LSSmaster_SCAN_NOACK", "group__CO__LSSmaster.html#ggae848ff3ff649c8a23b96053efaea4985ae9eb103f25dbe694143a64e7bb2c29d9", null ], + [ "CO_LSSmaster_SCAN_FAILED", "group__CO__LSSmaster.html#ggae848ff3ff649c8a23b96053efaea4985af84e0db6bd9aeebffc4266618145a8ea", null ], + [ "CO_LSSmaster_OK_ILLEGAL_ARGUMENT", "group__CO__LSSmaster.html#ggae848ff3ff649c8a23b96053efaea4985ab6985b2cbcb24653ec1f2ae46d3c09cd", null ], + [ "CO_LSSmaster_OK_MANUFACTURER", "group__CO__LSSmaster.html#ggae848ff3ff649c8a23b96053efaea4985a51083a5cf04e03a2f623eddb1d7324c7", null ] + ] ], + [ "CO_LSSmaster_scantype_t", "group__CO__LSSmaster.html#ga6e3f0d07f0712c371fb81cbf4a3dbcb1", [ + [ "CO_LSSmaster_FS_SCAN", "group__CO__LSSmaster.html#gga6e3f0d07f0712c371fb81cbf4a3dbcb1a5666c53e2fc02e214294bd3210146c90", null ], + [ "CO_LSSmaster_FS_SKIP", "group__CO__LSSmaster.html#gga6e3f0d07f0712c371fb81cbf4a3dbcb1ae0e6e92e760129fc173a0a4f19a0cf07", null ], + [ "CO_LSSmaster_FS_MATCH", "group__CO__LSSmaster.html#gga6e3f0d07f0712c371fb81cbf4a3dbcb1ad5a936fd00345aea75e8917f99df4ab3", null ] + ] ], + [ "CO_LSSmaster_init", "group__CO__LSSmaster.html#ga0675297a7e7e1f472ad2e88d6b7408e7", null ], + [ "CO_LSSmaster_changeTimeout", "group__CO__LSSmaster.html#gae22758aff11b796cfaed979c5f2808c0", null ], + [ "CO_LSSmaster_initCallbackPre", "group__CO__LSSmaster.html#gabfeb7e75d88b76bb00c1740381c7b53f", null ], + [ "CO_LSSmaster_switchStateSelect", "group__CO__LSSmaster.html#ga41b4288c03af394261541b9a8395e8f3", null ], + [ "CO_LSSmaster_switchStateDeselect", "group__CO__LSSmaster.html#gac0e13ec42e1fd85da5ddef6f654ef1a4", null ], + [ "CO_LSSmaster_configureBitTiming", "group__CO__LSSmaster.html#ga71a5d90e569ee7e88763a541c286e240", null ], + [ "CO_LSSmaster_configureNodeId", "group__CO__LSSmaster.html#ga2cdba08d9a564c4a61ebbcd0d10342fd", null ], + [ "CO_LSSmaster_configureStore", "group__CO__LSSmaster.html#gacea091d379a5338f13963eb745b25b16", null ], + [ "CO_LSSmaster_ActivateBit", "group__CO__LSSmaster.html#gaa016c0f3dc4dd021801b6139765657ab", null ], + [ "CO_LSSmaster_InquireLssAddress", "group__CO__LSSmaster.html#ga1b0a5c9e27e046736c6ec55a0256ed77", null ], + [ "CO_LSSmaster_Inquire", "group__CO__LSSmaster.html#ga22414a7184ca0c9d371dd67e9990d820", null ], + [ "CO_LSSmaster_IdentifyFastscan", "group__CO__LSSmaster.html#gad01ce178ea43b1843f541d4dd488f90e", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/CO__LSSmaster_8h_source.html b/Devices/Libraries/Systems/CANopenSocket/docs/CO__LSSmaster_8h_source.html new file mode 100755 index 0000000..2d928dd --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/CO__LSSmaster_8h_source.html @@ -0,0 +1,307 @@ + + + + + + + +CANopenNode: 305/CO_LSSmaster.h Source File + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+
CO_LSSmaster.h
+
+
+Go to the documentation of this file.
1 
+
27 #ifndef CO_LSSmaster_H
+
28 #define CO_LSSmaster_H
+
29 
+
30 #include "305/CO_LSS.h"
+
31 
+
32 #if ((CO_CONFIG_LSS) & CO_CONFIG_LSS_MASTER) || defined CO_DOXYGEN
+
33 
+
34 #ifdef __cplusplus
+
35 extern "C" {
+
36 #endif
+
37 
+
91 typedef enum {
+
92  CO_LSSmaster_SCAN_FINISHED = 2,
+
93  CO_LSSmaster_WAIT_SLAVE = 1,
+
94  CO_LSSmaster_OK = 0,
+
95  CO_LSSmaster_TIMEOUT = -1,
+
96  CO_LSSmaster_ILLEGAL_ARGUMENT = -2,
+
97  CO_LSSmaster_INVALID_STATE = -3,
+
98  CO_LSSmaster_SCAN_NOACK = -4,
+
99  CO_LSSmaster_SCAN_FAILED = -5,
+
100  CO_LSSmaster_OK_ILLEGAL_ARGUMENT = -101,
+
101  CO_LSSmaster_OK_MANUFACTURER = -102,
+
102 } CO_LSSmaster_return_t;
+
103 
+
104 
+
108 typedef struct{
+
109  uint32_t timeout_us;
+
111  uint8_t state;
+
112  uint8_t command;
+
113  uint32_t timeoutTimer;
+
115  uint8_t fsState;
+
116  uint8_t fsLssSub;
+
117  uint8_t fsBitChecked;
+
118  uint32_t fsIdNumber;
+
120  volatile void *CANrxNew;
+
121  uint8_t CANrxData[8];
+
122 #if ((CO_CONFIG_LSS) & CO_CONFIG_FLAG_CALLBACK_PRE) || defined CO_DOXYGEN
+
123  void (*pFunctSignal)(void *object);
+
124  void *functSignalObject;
+
125 #endif
+
126  CO_CANmodule_t *CANdevTx;
+
127  CO_CANtx_t *TXbuff;
+
128 }CO_LSSmaster_t;
+
129 
+
130 
+
135 #ifndef CO_LSSmaster_DEFAULT_TIMEOUT
+
136 #define CO_LSSmaster_DEFAULT_TIMEOUT 1000U /* ms */
+
137 #endif
+
138 
+
139 
+
156 CO_ReturnError_t CO_LSSmaster_init(
+
157  CO_LSSmaster_t *LSSmaster,
+
158  uint16_t timeout_ms,
+
159  CO_CANmodule_t *CANdevRx,
+
160  uint16_t CANdevRxIdx,
+
161  uint32_t CANidLssSlave,
+
162  CO_CANmodule_t *CANdevTx,
+
163  uint16_t CANdevTxIdx,
+
164  uint32_t CANidLssMaster);
+
165 
+
185 void CO_LSSmaster_changeTimeout(
+
186  CO_LSSmaster_t *LSSmaster,
+
187  uint16_t timeout_ms);
+
188 
+
189 
+
190 #if ((CO_CONFIG_LSS) & CO_CONFIG_FLAG_CALLBACK_PRE) || defined CO_DOXYGEN
+
191 
+
203 void CO_LSSmaster_initCallbackPre(
+
204  CO_LSSmaster_t *LSSmaster,
+
205  void *object,
+
206  void (*pFunctSignal)(void *object));
+
207 #endif
+
208 
+
209 
+
227 CO_LSSmaster_return_t CO_LSSmaster_switchStateSelect(
+
228  CO_LSSmaster_t *LSSmaster,
+
229  uint32_t timeDifference_us,
+
230  CO_LSS_address_t *lssAddress);
+
231 
+
232 
+
245 CO_LSSmaster_return_t CO_LSSmaster_switchStateDeselect(
+
246  CO_LSSmaster_t *LSSmaster);
+
247 
+
248 
+
267 CO_LSSmaster_return_t CO_LSSmaster_configureBitTiming(
+
268  CO_LSSmaster_t *LSSmaster,
+
269  uint32_t timeDifference_us,
+
270  uint16_t bit);
+
271 
+
272 
+
292 CO_LSSmaster_return_t CO_LSSmaster_configureNodeId(
+
293  CO_LSSmaster_t *LSSmaster,
+
294  uint32_t timeDifference_us,
+
295  uint8_t nodeId);
+
296 
+
297 
+
316 CO_LSSmaster_return_t CO_LSSmaster_configureStore(
+
317  CO_LSSmaster_t *LSSmaster,
+
318  uint32_t timeDifference_us);
+
319 
+
320 
+
340 CO_LSSmaster_return_t CO_LSSmaster_ActivateBit(
+
341  CO_LSSmaster_t *LSSmaster,
+
342  uint16_t switchDelay_ms);
+
343 
+
344 
+
363 CO_LSSmaster_return_t CO_LSSmaster_InquireLssAddress(
+
364  CO_LSSmaster_t *LSSmaster,
+
365  uint32_t timeDifference_us,
+
366  CO_LSS_address_t *lssAddress);
+
367 
+
368 
+
388 CO_LSSmaster_return_t CO_LSSmaster_Inquire(
+
389  CO_LSSmaster_t *LSSmaster,
+
390  uint32_t timeDifference_us,
+
391  CO_LSS_cs_t lssInquireCs,
+
392  uint32_t *value);
+
393 
+
394 
+
398 typedef enum {
+
399  CO_LSSmaster_FS_SCAN = 0,
+
400  CO_LSSmaster_FS_SKIP = 1,
+
401  CO_LSSmaster_FS_MATCH = 2,
+
402 } CO_LSSmaster_scantype_t;
+
403 
+
407 typedef struct{
+
408  CO_LSSmaster_scantype_t scan[4];
+
409  CO_LSS_address_t match;
+
410  CO_LSS_address_t found;
+
411 } CO_LSSmaster_fastscan_t;
+
412 
+
467 CO_LSSmaster_return_t CO_LSSmaster_IdentifyFastscan(
+
468  CO_LSSmaster_t *LSSmaster,
+
469  uint32_t timeDifference_us,
+
470  CO_LSSmaster_fastscan_t *fastscan);
+
471  /*@defgroup CO_LSSmaster*/
+
473 
+
474 #ifdef __cplusplus
+
475 }
+
476 #endif /*__cplusplus*/
+
477 
+
478 #endif /* (CO_CONFIG_LSS) & CO_CONFIG_LSS_MASTER */
+
479 
+
480 #endif /*CO_LSSmaster_H*/
+
+
+
CO_LSSmaster_OK_MANUFACTURER
@ CO_LSSmaster_OK_MANUFACTURER
LSS success, node rejected argument with manufacturer error code.
Definition: CO_LSSmaster.h:101
+
uint32_t
unsigned long int uint32_t
UNSIGNED32 in CANopen (0007h), 32-bit unsigned integer.
Definition: CO_driver.h:155
+
CO_LSSmaster_t::fsLssSub
uint8_t fsLssSub
Current state of node state machine.
Definition: CO_LSSmaster.h:116
+
CO_LSSmaster_SCAN_NOACK
@ CO_LSSmaster_SCAN_NOACK
No node found that matches scan request.
Definition: CO_LSSmaster.h:98
+
CO_LSSmaster_fastscan_t::found
CO_LSS_address_t found
Scan result.
Definition: CO_LSSmaster.h:410
+
CO_LSS.h
CANopen Layer Setting Services protocol (common).
+
uint16_t
unsigned int uint16_t
UNSIGNED16 in CANopen (0006h), 16-bit unsigned integer.
Definition: CO_driver.h:153
+
CO_LSSmaster_InquireLssAddress
CO_LSSmaster_return_t CO_LSSmaster_InquireLssAddress(CO_LSSmaster_t *LSSmaster, uint32_t timeDifference_us, CO_LSS_address_t *lssAddress)
Request LSS inquire LSS address.
+
CO_LSSmaster_t::fsBitChecked
uint8_t fsBitChecked
Current scan bit position.
Definition: CO_LSSmaster.h:117
+
CO_LSSmaster_ActivateBit
CO_LSSmaster_return_t CO_LSSmaster_ActivateBit(CO_LSSmaster_t *LSSmaster, uint16_t switchDelay_ms)
Request LSS activate bit timing.
+
CO_ReturnError_t
CO_ReturnError_t
Return values of some CANopen functions.
Definition: CO_driver.h:488
+
CO_LSSmaster_t::CANdevTx
CO_CANmodule_t * CANdevTx
From CO_LSSmaster_init()
Definition: CO_LSSmaster.h:126
+
CO_LSSmaster_IdentifyFastscan
CO_LSSmaster_return_t CO_LSSmaster_IdentifyFastscan(CO_LSSmaster_t *LSSmaster, uint32_t timeDifference_us, CO_LSSmaster_fastscan_t *fastscan)
Select a node by LSS identify fastscan.
+
CO_LSS_cs_t
CO_LSS_cs_t
LSS protocol command specifiers.
Definition: CO_LSS.h:86
+
CO_LSSmaster_switchStateDeselect
CO_LSSmaster_return_t CO_LSSmaster_switchStateDeselect(CO_LSSmaster_t *LSSmaster)
Request LSS switch state deselect.
+
CO_LSSmaster_switchStateSelect
CO_LSSmaster_return_t CO_LSSmaster_switchStateSelect(CO_LSSmaster_t *LSSmaster, uint32_t timeDifference_us, CO_LSS_address_t *lssAddress)
Request LSS switch state select.
+
CO_LSSmaster_t::functSignalObject
void * functSignalObject
Pointer to object.
Definition: CO_LSSmaster.h:124
+
CO_LSSmaster_OK
@ CO_LSSmaster_OK
Success, end of communication.
Definition: CO_LSSmaster.h:94
+
CO_LSSmaster_initCallbackPre
void CO_LSSmaster_initCallbackPre(CO_LSSmaster_t *LSSmaster, void *object, void(*pFunctSignal)(void *object))
Initialize LSSmasterRx callback function.
+
CO_LSSmaster_configureNodeId
CO_LSSmaster_return_t CO_LSSmaster_configureNodeId(CO_LSSmaster_t *LSSmaster, uint32_t timeDifference_us, uint8_t nodeId)
Request LSS configure node ID.
+
CO_LSSmaster_SCAN_FAILED
@ CO_LSSmaster_SCAN_FAILED
An error occurred while scanning.
Definition: CO_LSSmaster.h:99
+
CO_LSSmaster_t::fsState
uint8_t fsState
Current state of fastscan master state machine.
Definition: CO_LSSmaster.h:115
+
CO_LSSmaster_Inquire
CO_LSSmaster_return_t CO_LSSmaster_Inquire(CO_LSSmaster_t *LSSmaster, uint32_t timeDifference_us, CO_LSS_cs_t lssInquireCs, uint32_t *value)
Request LSS inquire node ID or part of LSS address.
+
CO_LSSmaster_WAIT_SLAVE
@ CO_LSSmaster_WAIT_SLAVE
No response arrived from slave yet.
Definition: CO_LSSmaster.h:93
+
CO_LSSmaster_t::timeout_us
uint32_t timeout_us
LSS response timeout in us.
Definition: CO_LSSmaster.h:109
+
CO_LSSmaster_t::state
uint8_t state
Node is currently selected.
Definition: CO_LSSmaster.h:111
+
CO_LSSmaster_t::fsIdNumber
uint32_t fsIdNumber
Current scan result.
Definition: CO_LSSmaster.h:118
+
CO_LSSmaster_scantype_t
CO_LSSmaster_scantype_t
Scan type for CO_LSSmaster_fastscan_t scan.
Definition: CO_LSSmaster.h:398
+
CO_LSSmaster_fastscan_t
Parameters for LSS fastscan CO_LSSmaster_IdentifyFastscan.
Definition: CO_LSSmaster.h:407
+
CO_LSSmaster_t
LSS master object.
Definition: CO_LSSmaster.h:108
+
CO_LSSmaster_return_t
CO_LSSmaster_return_t
Return values of LSS master functions.
Definition: CO_LSSmaster.h:91
+
CO_LSSmaster_FS_SKIP
@ CO_LSSmaster_FS_SKIP
Skip this value.
Definition: CO_LSSmaster.h:400
+
CO_LSSmaster_changeTimeout
void CO_LSSmaster_changeTimeout(CO_LSSmaster_t *LSSmaster, uint16_t timeout_ms)
Change LSS master timeout.
+
CO_LSSmaster_ILLEGAL_ARGUMENT
@ CO_LSSmaster_ILLEGAL_ARGUMENT
Invalid argument.
Definition: CO_LSSmaster.h:96
+
CO_LSSmaster_t::TXbuff
CO_CANtx_t * TXbuff
CAN transmit buffer.
Definition: CO_LSSmaster.h:127
+
CO_LSSmaster_FS_MATCH
@ CO_LSSmaster_FS_MATCH
Full 32 bit value is given as argument, just verify.
Definition: CO_LSSmaster.h:401
+
CO_LSSmaster_FS_SCAN
@ CO_LSSmaster_FS_SCAN
Do full 32 bit scan.
Definition: CO_LSSmaster.h:399
+
CO_LSSmaster_SCAN_FINISHED
@ CO_LSSmaster_SCAN_FINISHED
Scanning finished successful.
Definition: CO_LSSmaster.h:92
+
CO_LSSmaster_TIMEOUT
@ CO_LSSmaster_TIMEOUT
No reply received.
Definition: CO_LSSmaster.h:95
+
CO_LSSmaster_fastscan_t::match
CO_LSS_address_t match
Value to match in case of CO_LSSmaster_FS_MATCH.
Definition: CO_LSSmaster.h:409
+
CO_CANmodule_t
Complete CAN module object.
Definition: CO_driver.h:319
+
CO_LSSmaster_OK_ILLEGAL_ARGUMENT
@ CO_LSSmaster_OK_ILLEGAL_ARGUMENT
LSS success, node rejected argument because of non-supported value.
Definition: CO_LSSmaster.h:100
+
CO_LSSmaster_configureStore
CO_LSSmaster_return_t CO_LSSmaster_configureStore(CO_LSSmaster_t *LSSmaster, uint32_t timeDifference_us)
Request LSS store configuration.
+
CO_LSSmaster_t::timeoutTimer
uint32_t timeoutTimer
Timeout timer for LSS communication.
Definition: CO_LSSmaster.h:113
+
CO_LSSmaster_INVALID_STATE
@ CO_LSSmaster_INVALID_STATE
State machine not ready or already processing a request.
Definition: CO_LSSmaster.h:97
+
CO_LSS_address_t
The LSS address is a 128 bit number, uniquely identifying each node.
Definition: CO_LSS.h:162
+
CO_LSSmaster_configureBitTiming
CO_LSSmaster_return_t CO_LSSmaster_configureBitTiming(CO_LSSmaster_t *LSSmaster, uint32_t timeDifference_us, uint16_t bit)
Request LSS configure Bit Timing.
+
CO_CANtx_t
Configuration object for CAN transmit message for specific CANopenNode Object.
Definition: CO_driver.h:299
+
CO_LSSmaster_t::command
uint8_t command
Active command.
Definition: CO_LSSmaster.h:112
+
CO_LSSmaster_t::CANrxNew
volatile void * CANrxNew
Indication if new LSS message is received from CAN bus.
Definition: CO_LSSmaster.h:120
+
CO_LSSmaster_init
CO_ReturnError_t CO_LSSmaster_init(CO_LSSmaster_t *LSSmaster, uint16_t timeout_ms, CO_CANmodule_t *CANdevRx, uint16_t CANdevRxIdx, uint32_t CANidLssSlave, CO_CANmodule_t *CANdevTx, uint16_t CANdevTxIdx, uint32_t CANidLssMaster)
Initialize LSS object.
+
uint8_t
unsigned char uint8_t
UNSIGNED8 in CANopen (0005h), 8-bit unsigned integer.
Definition: CO_driver.h:151
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/CO__LSSslave_8h.html b/Devices/Libraries/Systems/CANopenSocket/docs/CO__LSSslave_8h.html new file mode 100755 index 0000000..740dd70 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/CO__LSSslave_8h.html @@ -0,0 +1,159 @@ + + + + + + + +CANopenNode: 305/CO_LSSslave.h File Reference + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Data Structures | +Functions
+
+
CO_LSSslave.h File Reference
CANopen_305 » LSS
+
+
+ +

CANopen Layer Setting Service - slave protocol. +More...

+
#include "305/CO_LSS.h"
+
+

Go to the source code of this file.

+ + + + + +

+Data Structures

struct  CO_LSSslave_t
 LSS slave object. More...
 
+ + + + + + + + + + + + + + + + + + + + + + +

+Functions

CO_ReturnError_t CO_LSSslave_init (CO_LSSslave_t *LSSslave, CO_LSS_address_t *lssAddress, uint16_t *pendingBitRate, uint8_t *pendingNodeID, CO_CANmodule_t *CANdevRx, uint16_t CANdevRxIdx, uint32_t CANidLssMaster, CO_CANmodule_t *CANdevTx, uint16_t CANdevTxIdx, uint32_t CANidLssSlave)
 Initialize LSS object. More...
 
bool_t CO_LSSslave_process (CO_LSSslave_t *LSSslave)
 Process LSS communication. More...
 
static CO_LSS_state_t CO_LSSslave_getState (CO_LSSslave_t *LSSslave)
 Get current LSS state. More...
 
void CO_LSSslave_initCallbackPre (CO_LSSslave_t *LSSslave, void *object, void(*pFunctSignalPre)(void *object))
 Initialize LSSslaveRx callback function. More...
 
void CO_LSSslave_initCheckBitRateCallback (CO_LSSslave_t *LSSslave, void *object, bool_t(*pFunctLSScheckBitRate)(void *object, uint16_t bitRate))
 Initialize verify bit rate callback. More...
 
void CO_LSSslave_initActivateBitRateCallback (CO_LSSslave_t *LSSslave, void *object, void(*pFunctLSSactivateBitRate)(void *object, uint16_t delay))
 Initialize activate bit rate callback. More...
 
void CO_LSSslave_initCfgStoreCallback (CO_LSSslave_t *LSSslave, void *object, bool_t(*pFunctLSScfgStore)(void *object, uint8_t id, uint16_t bitRate))
 Store configuration callback. More...
 
+

Detailed Description

+

CANopen Layer Setting Service - slave protocol.

+
Author
Martin Wagner
+
+Janez Paternoster
+
Copyright
2017 - 2020 Neuberger Gebaeudeautomation GmbH
+

This file is part of CANopenNode, an opensource CANopen Stack. Project home page is https://github.com/CANopenNode/CANopenNode. For more information on CANopen see http://www.can-cia.org/.

+

Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. You may obtain a copy of the License at 

http://www.apache.org/licenses/LICENSE-2.0
+

Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License.

+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/CO__LSSslave_8h.js b/Devices/Libraries/Systems/CANopenSocket/docs/CO__LSSslave_8h.js new file mode 100755 index 0000000..701a153 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/CO__LSSslave_8h.js @@ -0,0 +1,10 @@ +var CO__LSSslave_8h = +[ + [ "CO_LSSslave_init", "group__CO__LSSslave.html#gaaba1fafcd0024609f8a72be4810baf66", null ], + [ "CO_LSSslave_process", "group__CO__LSSslave.html#gae19d7ad84333f1a3f40ecbdbf639e017", null ], + [ "CO_LSSslave_getState", "group__CO__LSSslave.html#ga2692ff0d6837db494c029a3bef735ee7", null ], + [ "CO_LSSslave_initCallbackPre", "group__CO__LSSslave.html#ga64c23178117a707046eb15ebc6506429", null ], + [ "CO_LSSslave_initCheckBitRateCallback", "group__CO__LSSslave.html#ga665f147d6fae6db71173c4a8d602495c", null ], + [ "CO_LSSslave_initActivateBitRateCallback", "group__CO__LSSslave.html#ga0253fffcb36ab6b850563328784a8a5a", null ], + [ "CO_LSSslave_initCfgStoreCallback", "group__CO__LSSslave.html#gadc6187357904293da0a35317f15d0666", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/CO__LSSslave_8h_source.html b/Devices/Libraries/Systems/CANopenSocket/docs/CO__LSSslave_8h_source.html new file mode 100755 index 0000000..2508eb4 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/CO__LSSslave_8h_source.html @@ -0,0 +1,233 @@ + + + + + + + +CANopenNode: 305/CO_LSSslave.h Source File + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+
CO_LSSslave.h
+
+
+Go to the documentation of this file.
1 
+
28 #ifndef CO_LSSslave_H
+
29 #define CO_LSSslave_H
+
30 
+
31 #include "305/CO_LSS.h"
+
32 
+
33 #if ((CO_CONFIG_LSS) & CO_CONFIG_LSS_SLAVE) || defined CO_DOXYGEN
+
34 
+
35 #ifdef __cplusplus
+
36 extern "C" {
+
37 #endif
+
38 
+
90 typedef struct{
+
91  CO_LSS_address_t lssAddress;
+
92  CO_LSS_state_t lssState;
+
93  CO_LSS_address_t lssSelect;
+
95  CO_LSS_address_t lssFastscan;
+
96  uint8_t fastscanPos;
+
98  uint16_t *pendingBitRate;
+
99  uint8_t *pendingNodeID;
+
100  uint8_t activeNodeID;
+
102  volatile void *sendResponse;
+
103  CO_LSS_cs_t service;
+
104  uint8_t CANdata[8];
+
106 #if ((CO_CONFIG_LSS) & CO_CONFIG_FLAG_CALLBACK_PRE) || defined CO_DOXYGEN
+
107  void (*pFunctSignalPre)(void *object);
+
108  void *functSignalObjectPre;
+
109 #endif
+
110  bool_t (*pFunctLSScheckBitRate)(void *object, uint16_t bitRate);
+
111  void *functLSScheckBitRateObject;
+
112  void (*pFunctLSSactivateBitRate)(void *object, uint16_t delay);
+
113  void *functLSSactivateBitRateObject;
+
114  bool_t (*pFunctLSScfgStore)(void *object, uint8_t id, uint16_t bitRate);
+
115  void *functLSScfgStoreObject;
+
117  CO_CANmodule_t *CANdevTx;
+
118  CO_CANtx_t *TXbuff;
+
119 }CO_LSSslave_t;
+
120 
+
160 CO_ReturnError_t CO_LSSslave_init(
+
161  CO_LSSslave_t *LSSslave,
+
162  CO_LSS_address_t *lssAddress,
+
163  uint16_t *pendingBitRate,
+
164  uint8_t *pendingNodeID,
+
165  CO_CANmodule_t *CANdevRx,
+
166  uint16_t CANdevRxIdx,
+
167  uint32_t CANidLssMaster,
+
168  CO_CANmodule_t *CANdevTx,
+
169  uint16_t CANdevTxIdx,
+
170  uint32_t CANidLssSlave);
+
171 
+
185 bool_t CO_LSSslave_process(CO_LSSslave_t *LSSslave);
+
186 
+
193 static inline CO_LSS_state_t CO_LSSslave_getState(CO_LSSslave_t *LSSslave) {
+
194  return (LSSslave == NULL) ? CO_LSS_STATE_WAITING : LSSslave->lssState;
+
195 }
+
196 
+
197 
+
198 #if ((CO_CONFIG_LSS) & CO_CONFIG_FLAG_CALLBACK_PRE) || defined CO_DOXYGEN
+
199 
+
211 void CO_LSSslave_initCallbackPre(
+
212  CO_LSSslave_t *LSSslave,
+
213  void *object,
+
214  void (*pFunctSignalPre)(void *object));
+
215 #endif
+
216 
+
217 
+
231 void CO_LSSslave_initCheckBitRateCallback(
+
232  CO_LSSslave_t *LSSslave,
+
233  void *object,
+
234  bool_t (*pFunctLSScheckBitRate)(void *object, uint16_t bitRate));
+
235 
+
250 void CO_LSSslave_initActivateBitRateCallback(
+
251  CO_LSSslave_t *LSSslave,
+
252  void *object,
+
253  void (*pFunctLSSactivateBitRate)(void *object, uint16_t delay));
+
254 
+
269 void CO_LSSslave_initCfgStoreCallback(
+
270  CO_LSSslave_t *LSSslave,
+
271  void *object,
+
272  bool_t (*pFunctLSScfgStore)(void *object, uint8_t id, uint16_t bitRate));
+
273  /*@defgroup CO_LSSslave*/
+
275 
+
276 #ifdef __cplusplus
+
277 }
+
278 #endif /*__cplusplus*/
+
279 
+
280 #endif /* (CO_CONFIG_LSS) & CO_CONFIG_LSS_SLAVE */
+
281 
+
282 #endif /*CO_LSSslave_H*/
+
+
+
CO_LSSslave_t::lssFastscan
CO_LSS_address_t lssFastscan
Received LSS Address by fastscan.
Definition: CO_LSSslave.h:95
+
uint32_t
unsigned long int uint32_t
UNSIGNED32 in CANopen (0007h), 32-bit unsigned integer.
Definition: CO_driver.h:155
+
CO_LSSslave_init
CO_ReturnError_t CO_LSSslave_init(CO_LSSslave_t *LSSslave, CO_LSS_address_t *lssAddress, uint16_t *pendingBitRate, uint8_t *pendingNodeID, CO_CANmodule_t *CANdevRx, uint16_t CANdevRxIdx, uint32_t CANidLssMaster, CO_CANmodule_t *CANdevTx, uint16_t CANdevTxIdx, uint32_t CANidLssSlave)
Initialize LSS object.
+
CO_LSS.h
CANopen Layer Setting Services protocol (common).
+
CO_LSSslave_initActivateBitRateCallback
void CO_LSSslave_initActivateBitRateCallback(CO_LSSslave_t *LSSslave, void *object, void(*pFunctLSSactivateBitRate)(void *object, uint16_t delay))
Initialize activate bit rate callback.
+
uint16_t
unsigned int uint16_t
UNSIGNED16 in CANopen (0006h), 16-bit unsigned integer.
Definition: CO_driver.h:153
+
CO_ReturnError_t
CO_ReturnError_t
Return values of some CANopen functions.
Definition: CO_driver.h:488
+
CO_LSSslave_initCfgStoreCallback
void CO_LSSslave_initCfgStoreCallback(CO_LSSslave_t *LSSslave, void *object, bool_t(*pFunctLSScfgStore)(void *object, uint8_t id, uint16_t bitRate))
Store configuration callback.
+
CO_LSSslave_t::CANdevTx
CO_CANmodule_t * CANdevTx
Pointer to object.
Definition: CO_LSSslave.h:117
+
bool_t
unsigned char bool_t
Boolean data type for general use.
Definition: CO_driver.h:141
+
CO_LSS_cs_t
CO_LSS_cs_t
LSS protocol command specifiers.
Definition: CO_LSS.h:86
+
CO_LSSslave_t::service
CO_LSS_cs_t service
Service, which will have to be processed by mainline processing function.
Definition: CO_LSSslave.h:103
+
CO_LSSslave_process
bool_t CO_LSSslave_process(CO_LSSslave_t *LSSslave)
Process LSS communication.
+
CO_LSSslave_initCallbackPre
void CO_LSSslave_initCallbackPre(CO_LSSslave_t *LSSslave, void *object, void(*pFunctSignalPre)(void *object))
Initialize LSSslaveRx callback function.
+
CO_LSSslave_initCheckBitRateCallback
void CO_LSSslave_initCheckBitRateCallback(CO_LSSslave_t *LSSslave, void *object, bool_t(*pFunctLSScheckBitRate)(void *object, uint16_t bitRate))
Initialize verify bit rate callback.
+
CO_LSS_STATE_WAITING
@ CO_LSS_STATE_WAITING
LSS FSA waiting for requests.
Definition: CO_LSS.h:182
+
CO_LSSslave_t::lssState
CO_LSS_state_t lssState
CO_LSS_state_t
Definition: CO_LSSslave.h:92
+
CO_LSSslave_t
LSS slave object.
Definition: CO_LSSslave.h:90
+
CO_LSSslave_t::fastscanPos
uint8_t fastscanPos
Current state of fastscan.
Definition: CO_LSSslave.h:96
+
CO_LSS_state_t
CO_LSS_state_t
LSS finite state automaton.
Definition: CO_LSS.h:181
+
CO_LSSslave_t::lssSelect
CO_LSS_address_t lssSelect
Received LSS Address by select.
Definition: CO_LSSslave.h:93
+
CO_LSSslave_t::pendingNodeID
uint8_t * pendingNodeID
Node ID that is temporarily configured.
Definition: CO_LSSslave.h:99
+
CO_LSSslave_t::activeNodeID
uint8_t activeNodeID
Node ID used at the CAN interface.
Definition: CO_LSSslave.h:100
+
CO_LSSslave_t::functSignalObjectPre
void * functSignalObjectPre
Pointer to object.
Definition: CO_LSSslave.h:108
+
NULL
#define NULL
NULL, for general usage.
Definition: CO_driver.h:135
+
CO_CANmodule_t
Complete CAN module object.
Definition: CO_driver.h:319
+
CO_LSSslave_t::lssAddress
CO_LSS_address_t lssAddress
From CO_LSSslave_init.
Definition: CO_LSSslave.h:91
+
CO_LSSslave_t::sendResponse
volatile void * sendResponse
Variable indicates, if LSS response has to be sent by mainline processing function.
Definition: CO_LSSslave.h:102
+
CO_LSSslave_getState
static CO_LSS_state_t CO_LSSslave_getState(CO_LSSslave_t *LSSslave)
Get current LSS state.
Definition: CO_LSSslave.h:193
+
CO_LSS_address_t
The LSS address is a 128 bit number, uniquely identifying each node.
Definition: CO_LSS.h:162
+
CO_LSSslave_t::pendingBitRate
uint16_t * pendingBitRate
Bit rate value that is temporarily configured.
Definition: CO_LSSslave.h:98
+
CO_CANtx_t
Configuration object for CAN transmit message for specific CANopenNode Object.
Definition: CO_driver.h:299
+
uint8_t
unsigned char uint8_t
UNSIGNED8 in CANopen (0005h), 8-bit unsigned integer.
Definition: CO_driver.h:151
+
CO_LSSslave_t::TXbuff
CO_CANtx_t * TXbuff
CAN transmit buffer.
Definition: CO_LSSslave.h:118
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/CO__NMT__Heartbeat_8h.html b/Devices/Libraries/Systems/CANopenSocket/docs/CO__NMT__Heartbeat_8h.html new file mode 100755 index 0000000..0fb03b5 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/CO__NMT__Heartbeat_8h.html @@ -0,0 +1,204 @@ + + + + + + + +CANopenNode: 301/CO_NMT_Heartbeat.h File Reference + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Data Structures | +Enumerations | +Functions
+
+
CO_NMT_Heartbeat.h File Reference
CANopen_301 » NMT and Heartbeat
+
+
+ +

CANopen Network management and Heartbeat producer protocol. +More...

+
#include "301/CO_driver.h"
+#include "301/CO_ODinterface.h"
+#include "301/CO_Emergency.h"
+
+

Go to the source code of this file.

+ + + + + +

+Data Structures

struct  CO_NMT_t
 NMT consumer and Heartbeat producer object. More...
 
+ + + + + + + + + + + + + +

+Enumerations

enum  CO_NMT_internalState_t {
+  CO_NMT_UNKNOWN = -1, +CO_NMT_INITIALIZING = 0, +CO_NMT_PRE_OPERATIONAL = 127, +CO_NMT_OPERATIONAL = 5, +
+  CO_NMT_STOPPED = 4 +
+ }
 Internal network state of the CANopen node. More...
 
enum  CO_NMT_command_t {
+  CO_NMT_ENTER_OPERATIONAL = 1, +CO_NMT_ENTER_STOPPED = 2, +CO_NMT_ENTER_PRE_OPERATIONAL = 128, +CO_NMT_RESET_NODE = 129, +
+  CO_NMT_RESET_COMMUNICATION = 130 +
+ }
 Commands from NMT master. More...
 
enum  CO_NMT_reset_cmd_t { CO_RESET_NOT = 0, +CO_RESET_COMM = 1, +CO_RESET_APP = 2, +CO_RESET_QUIT = 3 + }
 Return code from CO_NMT_process() that tells application code what to reset. More...
 
enum  CO_NMT_control_t {
+  CO_NMT_ERR_REG_MASK = 0x00FFU, +CO_NMT_STARTUP_TO_OPERATIONAL = 0x0100U, +CO_NMT_ERR_ON_BUSOFF_HB = 0x1000U, +CO_NMT_ERR_ON_ERR_REG = 0x2000U, +
+  CO_NMT_ERR_TO_STOPPED = 0x4000U, +CO_NMT_ERR_FREE_TO_OPERATIONAL = 0x8000U +
+ }
 NMT control bitfield for NMT internal state. More...
 
+ + + + + + + + + + + + + + + + + + + + + + +

+Functions

CO_ReturnError_t CO_NMT_init (CO_NMT_t *NMT, const OD_entry_t *OD_1017_ProducerHbTime, CO_EM_t *em, uint8_t nodeId, CO_NMT_control_t NMTcontrol, uint16_t firstHBTime_ms, CO_CANmodule_t *NMT_CANdevRx, uint16_t NMT_rxIdx, uint16_t CANidRxNMT, CO_CANmodule_t *NMT_CANdevTx, uint16_t NMT_txIdx, uint16_t CANidTxNMT, CO_CANmodule_t *HB_CANdevTx, uint16_t HB_txIdx, uint16_t CANidTxHB)
 Initialize NMT and Heartbeat producer object. More...
 
void CO_NMT_initCallbackPre (CO_NMT_t *NMT, void *object, void(*pFunctSignal)(void *object))
 Initialize NMT callback function after message preprocessed. More...
 
void CO_NMT_initCallbackChanged (CO_NMT_t *NMT, void(*pFunctNMT)(CO_NMT_internalState_t state))
 Initialize NMT callback function. More...
 
CO_NMT_reset_cmd_t CO_NMT_process (CO_NMT_t *NMT, CO_NMT_internalState_t *NMTstate, uint32_t timeDifference_us, uint32_t *timerNext_us)
 Process received NMT and produce Heartbeat messages. More...
 
static CO_NMT_internalState_t CO_NMT_getInternalState (CO_NMT_t *NMT)
 Query current NMT state. More...
 
static void CO_NMT_setInternalState (CO_NMT_t *NMT, CO_NMT_internalState_t state)
 Set internal NMT state. More...
 
CO_ReturnError_t CO_NMT_sendCommand (CO_NMT_t *NMT, CO_NMT_command_t command, uint8_t nodeID)
 Send NMT master command. More...
 
+

Detailed Description

+

CANopen Network management and Heartbeat producer protocol.

+
Author
Janez Paternoster
+
Copyright
2004 - 2020 Janez Paternoster
+

This file is part of CANopenNode, an opensource CANopen Stack. Project home page is https://github.com/CANopenNode/CANopenNode. For more information on CANopen see http://www.can-cia.org/.

+

Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. You may obtain a copy of the License at 

http://www.apache.org/licenses/LICENSE-2.0
+

Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License.

+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/CO__NMT__Heartbeat_8h.js b/Devices/Libraries/Systems/CANopenSocket/docs/CO__NMT__Heartbeat_8h.js new file mode 100755 index 0000000..0904fc3 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/CO__NMT__Heartbeat_8h.js @@ -0,0 +1,38 @@ +var CO__NMT__Heartbeat_8h = +[ + [ "CO_NMT_internalState_t", "group__CO__NMT__Heartbeat.html#ga1e8c2a6c0fd4a33183503d25a7c6d744", [ + [ "CO_NMT_UNKNOWN", "group__CO__NMT__Heartbeat.html#gga1e8c2a6c0fd4a33183503d25a7c6d744a52d111d4262b514df4433ae07969cc3c", null ], + [ "CO_NMT_INITIALIZING", "group__CO__NMT__Heartbeat.html#gga1e8c2a6c0fd4a33183503d25a7c6d744a672c48c0e9bd5cb070bd86e1dcdc085c", null ], + [ "CO_NMT_PRE_OPERATIONAL", "group__CO__NMT__Heartbeat.html#gga1e8c2a6c0fd4a33183503d25a7c6d744ae41d4ebcb7e26e165c8685b5809871a9", null ], + [ "CO_NMT_OPERATIONAL", "group__CO__NMT__Heartbeat.html#gga1e8c2a6c0fd4a33183503d25a7c6d744a3f9d07a10f2781201b85e9de9ba1c9a5", null ], + [ "CO_NMT_STOPPED", "group__CO__NMT__Heartbeat.html#gga1e8c2a6c0fd4a33183503d25a7c6d744aba34c0b4b2c6a0c11b01236cbe949ed5", null ] + ] ], + [ "CO_NMT_command_t", "group__CO__NMT__Heartbeat.html#gac396242e2e12ef6b0b22ff48636bc4eb", [ + [ "CO_NMT_ENTER_OPERATIONAL", "group__CO__NMT__Heartbeat.html#ggac396242e2e12ef6b0b22ff48636bc4eba6c7cdae79e5939cf321cdf82aa6b4146", null ], + [ "CO_NMT_ENTER_STOPPED", "group__CO__NMT__Heartbeat.html#ggac396242e2e12ef6b0b22ff48636bc4eba8a2911a2a576a1bd83b7f4a247cd77c9", null ], + [ "CO_NMT_ENTER_PRE_OPERATIONAL", "group__CO__NMT__Heartbeat.html#ggac396242e2e12ef6b0b22ff48636bc4eba575ed4f3386e8a63498921e659aa7ba4", null ], + [ "CO_NMT_RESET_NODE", "group__CO__NMT__Heartbeat.html#ggac396242e2e12ef6b0b22ff48636bc4ebaa8558d18a80148a598e6295ef41cdf97", null ], + [ "CO_NMT_RESET_COMMUNICATION", "group__CO__NMT__Heartbeat.html#ggac396242e2e12ef6b0b22ff48636bc4ebabc6aef9d84c816eecaea38f9fe2cbd9c", null ] + ] ], + [ "CO_NMT_reset_cmd_t", "group__CO__NMT__Heartbeat.html#gaf42f056a571b8e17a2d74428d1a49674", [ + [ "CO_RESET_NOT", "group__CO__NMT__Heartbeat.html#ggaf42f056a571b8e17a2d74428d1a49674a1e75deb437b2e06b2c1f353bf76d8807", null ], + [ "CO_RESET_COMM", "group__CO__NMT__Heartbeat.html#ggaf42f056a571b8e17a2d74428d1a49674a2a48dc2be7c9ffa7874eb08a1ba39f6f", null ], + [ "CO_RESET_APP", "group__CO__NMT__Heartbeat.html#ggaf42f056a571b8e17a2d74428d1a49674a2ab187bf1daccd4bc31685c08ca93bf1", null ], + [ "CO_RESET_QUIT", "group__CO__NMT__Heartbeat.html#ggaf42f056a571b8e17a2d74428d1a49674ae2c7805e42ce9b7c893018922b24a102", null ] + ] ], + [ "CO_NMT_control_t", "group__CO__NMT__Heartbeat.html#gaf92cf5943801e5dda84654345cc3d67f", [ + [ "CO_NMT_ERR_REG_MASK", "group__CO__NMT__Heartbeat.html#ggaf92cf5943801e5dda84654345cc3d67faaa5ab06e2047e51ac7d2767e8a821d80", null ], + [ "CO_NMT_STARTUP_TO_OPERATIONAL", "group__CO__NMT__Heartbeat.html#ggaf92cf5943801e5dda84654345cc3d67faf4da9fa5d805e0f535abf95a175312e8", null ], + [ "CO_NMT_ERR_ON_BUSOFF_HB", "group__CO__NMT__Heartbeat.html#ggaf92cf5943801e5dda84654345cc3d67faf151918ecf07fc43ba30489d31ca275a", null ], + [ "CO_NMT_ERR_ON_ERR_REG", "group__CO__NMT__Heartbeat.html#ggaf92cf5943801e5dda84654345cc3d67faab1cd82001a087114a5361910e206c74", null ], + [ "CO_NMT_ERR_TO_STOPPED", "group__CO__NMT__Heartbeat.html#ggaf92cf5943801e5dda84654345cc3d67fa9498a5af4e0557a2c0284c04b26b3eb5", null ], + [ "CO_NMT_ERR_FREE_TO_OPERATIONAL", "group__CO__NMT__Heartbeat.html#ggaf92cf5943801e5dda84654345cc3d67fa6e79d137c725417786035982cd0e2687", null ] + ] ], + [ "CO_NMT_init", "group__CO__NMT__Heartbeat.html#ga53c92521fff0fab405f8a372702c7259", null ], + [ "CO_NMT_initCallbackPre", "group__CO__NMT__Heartbeat.html#ga58b6123938e950b5e8c38ec0b9caeec4", null ], + [ "CO_NMT_initCallbackChanged", "group__CO__NMT__Heartbeat.html#ga87d153e39b32586b67ee6dd47dfce787", null ], + [ "CO_NMT_process", "group__CO__NMT__Heartbeat.html#ga724ff5b1d9cfee955914f365514deda0", null ], + [ "CO_NMT_getInternalState", "group__CO__NMT__Heartbeat.html#ga2e3f0d579e321d73b53bf469d49b0ce3", null ], + [ "CO_NMT_setInternalState", "group__CO__NMT__Heartbeat.html#ga91b7041eda0487bbc0588d3200e0e868", null ], + [ "CO_NMT_sendCommand", "group__CO__NMT__Heartbeat.html#ga9d557708be1bfb9b169718abea90b663", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/CO__NMT__Heartbeat_8h_source.html b/Devices/Libraries/Systems/CANopenSocket/docs/CO__NMT__Heartbeat_8h_source.html new file mode 100755 index 0000000..280c840 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/CO__NMT__Heartbeat_8h_source.html @@ -0,0 +1,316 @@ + + + + + + + +CANopenNode: 301/CO_NMT_Heartbeat.h Source File + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+
CO_NMT_Heartbeat.h
+
+
+Go to the documentation of this file.
1 
+
26 #ifndef CO_NMT_HEARTBEAT_H
+
27 #define CO_NMT_HEARTBEAT_H
+
28 
+
29 #include "301/CO_driver.h"
+
30 #include "301/CO_ODinterface.h"
+
31 #include "301/CO_Emergency.h"
+
32 
+
33 /* default configuration, see CO_config.h */
+
34 #ifndef CO_CONFIG_NMT
+
35 #define CO_CONFIG_NMT (0)
+
36 #endif
+
37 
+
38 #ifdef __cplusplus
+
39 extern "C" {
+
40 #endif
+
41 
+
77 typedef enum {
+
79  CO_NMT_UNKNOWN = -1,
+
81  CO_NMT_INITIALIZING = 0,
+
83  CO_NMT_PRE_OPERATIONAL = 127,
+
85  CO_NMT_OPERATIONAL = 5,
+
87  CO_NMT_STOPPED = 4
+
88 } CO_NMT_internalState_t;
+
89 
+
90 
+
94 typedef enum {
+
96  CO_NMT_ENTER_OPERATIONAL = 1,
+
98  CO_NMT_ENTER_STOPPED = 2,
+
100  CO_NMT_ENTER_PRE_OPERATIONAL = 128,
+
102  CO_NMT_RESET_NODE = 129,
+
104  CO_NMT_RESET_COMMUNICATION = 130
+
105 } CO_NMT_command_t;
+
106 
+
107 
+
112 typedef enum {
+
114  CO_RESET_NOT = 0,
+
116  CO_RESET_COMM = 1,
+
118  CO_RESET_APP = 2,
+
121  CO_RESET_QUIT = 3
+
122 } CO_NMT_reset_cmd_t;
+
123 
+
124 
+
135 typedef enum {
+
138  CO_NMT_ERR_REG_MASK = 0x00FFU,
+
141  CO_NMT_STARTUP_TO_OPERATIONAL = 0x0100U,
+
145  CO_NMT_ERR_ON_BUSOFF_HB = 0x1000U,
+
149  CO_NMT_ERR_ON_ERR_REG = 0x2000U,
+
152  CO_NMT_ERR_TO_STOPPED = 0x4000U,
+
155  CO_NMT_ERR_FREE_TO_OPERATIONAL = 0x8000U
+
156 } CO_NMT_control_t;
+
157 
+
158 
+
162 typedef struct {
+
164  uint8_t operatingState;
+
166  uint8_t operatingStatePrev;
+
169  uint8_t internalCommand;
+
171  uint8_t nodeId;
+
173  CO_NMT_control_t NMTcontrol;
+
175  uint32_t HBproducerTime_us;
+
177  uint32_t HBproducerTimer;
+
179  CO_EM_t *em;
+
180 #if ((CO_CONFIG_NMT) & CO_CONFIG_NMT_MASTER) || defined CO_DOXYGEN
+
181 
+
182  CO_CANmodule_t *NMT_CANdevTx;
+
184  CO_CANtx_t *NMT_TXbuff;
+
185 #endif
+
186 
+
187  CO_CANmodule_t *HB_CANdevTx;
+
189  CO_CANtx_t *HB_TXbuff;
+
190 #if ((CO_CONFIG_NMT) & CO_CONFIG_FLAG_CALLBACK_PRE) || defined CO_DOXYGEN
+
191 
+
192  void (*pFunctSignalPre)(void *object);
+
194  void *functSignalObjectPre;
+
195 #endif
+
196 #if ((CO_CONFIG_NMT) & CO_CONFIG_NMT_CALLBACK_CHANGE) || defined CO_DOXYGEN
+
197 
+
198  void (*pFunctNMT)(CO_NMT_internalState_t state);
+
199 #endif
+
200 } CO_NMT_t;
+
201 
+
202 
+
231 CO_ReturnError_t CO_NMT_init(CO_NMT_t *NMT,
+
232  const OD_entry_t *OD_1017_ProducerHbTime,
+
233  CO_EM_t *em,
+
234  uint8_t nodeId,
+
235  CO_NMT_control_t NMTcontrol,
+
236  uint16_t firstHBTime_ms,
+
237  CO_CANmodule_t *NMT_CANdevRx,
+
238  uint16_t NMT_rxIdx,
+
239  uint16_t CANidRxNMT,
+
240 #if ((CO_CONFIG_NMT) & CO_CONFIG_NMT_MASTER) || defined CO_DOXYGEN
+
241  CO_CANmodule_t *NMT_CANdevTx,
+
242  uint16_t NMT_txIdx,
+
243  uint16_t CANidTxNMT,
+
244 #endif
+
245  CO_CANmodule_t *HB_CANdevTx,
+
246  uint16_t HB_txIdx,
+
247  uint16_t CANidTxHB);
+
248 
+
249 
+
250 #if ((CO_CONFIG_NMT) & CO_CONFIG_FLAG_CALLBACK_PRE) || defined CO_DOXYGEN
+
251 
+
263 void CO_NMT_initCallbackPre(CO_NMT_t *NMT,
+
264  void *object,
+
265  void (*pFunctSignal)(void *object));
+
266 #endif
+
267 
+
268 
+
269 #if ((CO_CONFIG_NMT) & CO_CONFIG_NMT_CALLBACK_CHANGE) || defined CO_DOXYGEN
+
270 
+
281 void CO_NMT_initCallbackChanged(CO_NMT_t *NMT,
+
282  void (*pFunctNMT)(CO_NMT_internalState_t state));
+
283 #endif
+
284 
+
285 
+
299 CO_NMT_reset_cmd_t CO_NMT_process(CO_NMT_t *NMT,
+
300  CO_NMT_internalState_t *NMTstate,
+
301  uint32_t timeDifference_us,
+
302  uint32_t *timerNext_us);
+
303 
+
304 
+
312 static inline CO_NMT_internalState_t CO_NMT_getInternalState(CO_NMT_t *NMT) {
+
313  return (NMT == NULL) ? CO_NMT_INITIALIZING : NMT->operatingState;
+
314 }
+
315 
+
316 
+
326 static inline void CO_NMT_setInternalState(CO_NMT_t *NMT,
+
327  CO_NMT_internalState_t state)
+
328 {
+
329  if (NMT != NULL) NMT->operatingState = state;
+
330 }
+
331 
+
332 
+
333 #if ((CO_CONFIG_NMT) & CO_CONFIG_NMT_MASTER) || defined CO_DOXYGEN
+
334 
+
349 CO_ReturnError_t CO_NMT_sendCommand(CO_NMT_t *NMT,
+
350  CO_NMT_command_t command,
+
351  uint8_t nodeID);
+
352 
+
353 #endif /* (CO_CONFIG_NMT) & CO_CONFIG_NMT_MASTER */
+
354  /* CO_NMT_Heartbeat */
+
356 
+
357 #ifdef __cplusplus
+
358 }
+
359 #endif /*__cplusplus*/
+
360 
+
361 #endif /* CO_NMT_HEARTBEAT_H */
+
+
+
CO_RESET_APP
@ CO_RESET_APP
2, Application must provide complete device reset
Definition: CO_NMT_Heartbeat.h:118
+
CO_NMT_PRE_OPERATIONAL
@ CO_NMT_PRE_OPERATIONAL
127, Device is in pre-operational state
Definition: CO_NMT_Heartbeat.h:83
+
uint32_t
unsigned long int uint32_t
UNSIGNED32 in CANopen (0007h), 32-bit unsigned integer.
Definition: CO_driver.h:155
+
CO_driver.h
Interface between CAN hardware and CANopenNode.
+
CO_NMT_t::functSignalObjectPre
void * functSignalObjectPre
From CO_NMT_initCallbackPre() or NULL.
Definition: CO_NMT_Heartbeat.h:194
+
CO_NMT_t::HBproducerTime_us
uint32_t HBproducerTime_us
Producer heartbeat time, calculated from OD 0x1017.
Definition: CO_NMT_Heartbeat.h:175
+
CO_NMT_init
CO_ReturnError_t CO_NMT_init(CO_NMT_t *NMT, const OD_entry_t *OD_1017_ProducerHbTime, CO_EM_t *em, uint8_t nodeId, CO_NMT_control_t NMTcontrol, uint16_t firstHBTime_ms, CO_CANmodule_t *NMT_CANdevRx, uint16_t NMT_rxIdx, uint16_t CANidRxNMT, CO_CANmodule_t *NMT_CANdevTx, uint16_t NMT_txIdx, uint16_t CANidTxNMT, CO_CANmodule_t *HB_CANdevTx, uint16_t HB_txIdx, uint16_t CANidTxHB)
Initialize NMT and Heartbeat producer object.
+
CO_CONFIG_NMT
#define CO_CONFIG_NMT
Configuration of NMT and Heartbeat.
Definition: CO_config.h:123
+
CO_NMT_setInternalState
static void CO_NMT_setInternalState(CO_NMT_t *NMT, CO_NMT_internalState_t state)
Set internal NMT state.
Definition: CO_NMT_Heartbeat.h:326
+
CO_RESET_QUIT
@ CO_RESET_QUIT
3, Application must quit, no reset of microcontroller (command is not requested by the stack....
Definition: CO_NMT_Heartbeat.h:121
+
CO_NMT_INITIALIZING
@ CO_NMT_INITIALIZING
0, Device is initializing
Definition: CO_NMT_Heartbeat.h:81
+
CO_NMT_sendCommand
CO_ReturnError_t CO_NMT_sendCommand(CO_NMT_t *NMT, CO_NMT_command_t command, uint8_t nodeID)
Send NMT master command.
+
uint16_t
unsigned int uint16_t
UNSIGNED16 in CANopen (0006h), 16-bit unsigned integer.
Definition: CO_driver.h:153
+
CO_NMT_RESET_NODE
@ CO_NMT_RESET_NODE
129, Reset device
Definition: CO_NMT_Heartbeat.h:102
+
CO_ReturnError_t
CO_ReturnError_t
Return values of some CANopen functions.
Definition: CO_driver.h:488
+
CO_NMT_getInternalState
static CO_NMT_internalState_t CO_NMT_getInternalState(CO_NMT_t *NMT)
Query current NMT state.
Definition: CO_NMT_Heartbeat.h:312
+
CO_NMT_process
CO_NMT_reset_cmd_t CO_NMT_process(CO_NMT_t *NMT, CO_NMT_internalState_t *NMTstate, uint32_t timeDifference_us, uint32_t *timerNext_us)
Process received NMT and produce Heartbeat messages.
Definition: CO_NMT_Heartbeat.c:222
+
CO_NMT_t::HBproducerTimer
uint32_t HBproducerTimer
Internal timer for HB producer.
Definition: CO_NMT_Heartbeat.h:177
+
CO_NMT_reset_cmd_t
CO_NMT_reset_cmd_t
Return code from CO_NMT_process() that tells application code what to reset.
Definition: CO_NMT_Heartbeat.h:112
+
CO_NMT_t::internalCommand
uint8_t internalCommand
NMT internal command from CO_NMT_receive() or CO_NMT_sendCommand(), processed in CO_NMT_process().
Definition: CO_NMT_Heartbeat.h:169
+
CO_NMT_STARTUP_TO_OPERATIONAL
@ CO_NMT_STARTUP_TO_OPERATIONAL
If bit is set, then device enters NMT operational state after the initialization phase,...
Definition: CO_NMT_Heartbeat.h:141
+
CO_NMT_initCallbackPre
void CO_NMT_initCallbackPre(CO_NMT_t *NMT, void *object, void(*pFunctSignal)(void *object))
Initialize NMT callback function after message preprocessed.
+
CO_NMT_ERR_TO_STOPPED
@ CO_NMT_ERR_TO_STOPPED
If bit is set and CO_NMT_ERR_ON_xx condition is met, then device will enter NMT stopped state,...
Definition: CO_NMT_Heartbeat.h:152
+
CO_NMT_t
NMT consumer and Heartbeat producer object.
Definition: CO_NMT_Heartbeat.h:162
+
CO_NMT_t::HB_TXbuff
CO_CANtx_t * HB_TXbuff
CAN transmit buffer for heartbeat message.
Definition: CO_NMT_Heartbeat.h:189
+
CO_NMT_ERR_FREE_TO_OPERATIONAL
@ CO_NMT_ERR_FREE_TO_OPERATIONAL
If bit is set and device is pre-operational, it enters NMT operational state automatically,...
Definition: CO_NMT_Heartbeat.h:155
+
CO_NMT_t::NMT_TXbuff
CO_CANtx_t * NMT_TXbuff
CAN transmit buffer for NMT master message.
Definition: CO_NMT_Heartbeat.h:184
+
CO_RESET_COMM
@ CO_RESET_COMM
1, Application must provide communication reset.
Definition: CO_NMT_Heartbeat.h:116
+
CO_NMT_ERR_REG_MASK
@ CO_NMT_ERR_REG_MASK
First 8 bits can be used to specify bitmask for the CO_errorRegister_t, to get relevant bits for the ...
Definition: CO_NMT_Heartbeat.h:138
+
CO_NMT_t::HB_CANdevTx
CO_CANmodule_t * HB_CANdevTx
From CO_NMT_init()
Definition: CO_NMT_Heartbeat.h:187
+
CO_NMT_ENTER_STOPPED
@ CO_NMT_ENTER_STOPPED
2, Stop device
Definition: CO_NMT_Heartbeat.h:98
+
CO_NMT_t::em
CO_EM_t * em
From CO_NMT_init()
Definition: CO_NMT_Heartbeat.h:179
+
CO_NMT_OPERATIONAL
@ CO_NMT_OPERATIONAL
5, Device is in operational state
Definition: CO_NMT_Heartbeat.h:85
+
CO_NMT_t::operatingStatePrev
uint8_t operatingStatePrev
Previous NMT operating state.
Definition: CO_NMT_Heartbeat.h:166
+
CO_NMT_command_t
CO_NMT_command_t
Commands from NMT master.
Definition: CO_NMT_Heartbeat.h:94
+
CO_NMT_UNKNOWN
@ CO_NMT_UNKNOWN
-1, Device state is unknown (for heartbeat consumer)
Definition: CO_NMT_Heartbeat.h:79
+
CO_NMT_ERR_ON_ERR_REG
@ CO_NMT_ERR_ON_ERR_REG
If bit is set and device is operational, it enters NMT pre-operational or stopped state,...
Definition: CO_NMT_Heartbeat.h:149
+
CO_RESET_NOT
@ CO_RESET_NOT
0, Normal return, no action
Definition: CO_NMT_Heartbeat.h:114
+
CO_NMT_t::NMT_CANdevTx
CO_CANmodule_t * NMT_CANdevTx
From CO_NMT_init()
Definition: CO_NMT_Heartbeat.h:182
+
CO_EM_t
Emergency object.
Definition: CO_Emergency.h:369
+
CO_NMT_internalState_t
CO_NMT_internalState_t
Internal network state of the CANopen node.
Definition: CO_NMT_Heartbeat.h:77
+
CO_ODinterface.h
CANopen Object Dictionary interface.
+
CO_NMT_initCallbackChanged
void CO_NMT_initCallbackChanged(CO_NMT_t *NMT, void(*pFunctNMT)(CO_NMT_internalState_t state))
Initialize NMT callback function.
+
CO_NMT_STOPPED
@ CO_NMT_STOPPED
4, Device is stopped
Definition: CO_NMT_Heartbeat.h:87
+
CO_Emergency.h
CANopen Emergency protocol.
+
CO_NMT_t::operatingState
uint8_t operatingState
Current NMT operating state.
Definition: CO_NMT_Heartbeat.h:164
+
NULL
#define NULL
NULL, for general usage.
Definition: CO_driver.h:135
+
CO_CANmodule_t
Complete CAN module object.
Definition: CO_driver.h:319
+
CO_NMT_ENTER_PRE_OPERATIONAL
@ CO_NMT_ENTER_PRE_OPERATIONAL
128, Put device into pre-operational
Definition: CO_NMT_Heartbeat.h:100
+
CO_NMT_t::NMTcontrol
CO_NMT_control_t NMTcontrol
From CO_NMT_init()
Definition: CO_NMT_Heartbeat.h:173
+
CO_NMT_ENTER_OPERATIONAL
@ CO_NMT_ENTER_OPERATIONAL
1, Start device
Definition: CO_NMT_Heartbeat.h:96
+
OD_entry_t
Object Dictionary entry for one OD object.
Definition: CO_ODinterface.h:336
+
CO_NMT_control_t
CO_NMT_control_t
NMT control bitfield for NMT internal state.
Definition: CO_NMT_Heartbeat.h:135
+
CO_NMT_t::nodeId
uint8_t nodeId
From CO_NMT_init()
Definition: CO_NMT_Heartbeat.h:171
+
CO_NMT_ERR_ON_BUSOFF_HB
@ CO_NMT_ERR_ON_BUSOFF_HB
If bit is set and device is operational, it enters NMT pre-operational or stopped state,...
Definition: CO_NMT_Heartbeat.h:145
+
CO_NMT_RESET_COMMUNICATION
@ CO_NMT_RESET_COMMUNICATION
130, Reset CANopen communication on device
Definition: CO_NMT_Heartbeat.h:104
+
CO_CANtx_t
Configuration object for CAN transmit message for specific CANopenNode Object.
Definition: CO_driver.h:299
+
uint8_t
unsigned char uint8_t
UNSIGNED8 in CANopen (0005h), 8-bit unsigned integer.
Definition: CO_driver.h:151
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/CO__OD__storage_8h.html b/Devices/Libraries/Systems/CANopenSocket/docs/CO__OD__storage_8h.html new file mode 100755 index 0000000..b51918e --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/CO__OD__storage_8h.html @@ -0,0 +1,161 @@ + + + + + + + +CANopenNode: socketCAN/CO_OD_storage.h File Reference + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Data Structures | +Functions
+
+
CO_OD_storage.h File Reference
socketCAN » OD storage
+
+
+ +

CANopen Object Dictionary storage object for Linux SocketCAN. +More...

+
#include "301/CO_driver.h"
+#include "301/CO_SDOserver.h"
+#include <stdio.h>
+
+

Go to the source code of this file.

+ + + + + +

+Data Structures

struct  CO_OD_storage_t
 Object Dictionary storage object. More...
 
+ + + + + + + + + + + + + + + + + + + + + + +

+Functions

+CO_SDO_abortCode_t CO_ODF_1010 (CO_ODF_arg_t *ODF_arg)
 Callback for use inside CO_OD_configure() function for OD object 1010.
 
+CO_SDO_abortCode_t CO_ODF_1011 (CO_ODF_arg_t *ODF_arg)
 Callback for use inside CO_OD_configure() function for OD object 1011.
 
int CO_OD_storage_saveSecure (uint8_t *odAddress, uint32_t odSize, char *filename)
 Save memory block to a file. More...
 
int CO_OD_storage_restoreSecure (char *filename)
 Remove OD storage file. More...
 
CO_ReturnError_t CO_OD_storage_init (CO_OD_storage_t *odStor, uint8_t *odAddress, uint32_t odSize, char *filename)
 Initialize OD storage object and load data from file. More...
 
CO_ReturnError_t CO_OD_storage_autoSave (CO_OD_storage_t *odStor, uint32_t timer1usDiff, uint32_t delay_us)
 Automatically save memory block if differs from file. More...
 
void CO_OD_storage_autoSaveClose (CO_OD_storage_t *odStor)
 Closes file opened by CO_OD_storage_autoSave. More...
 
+

Detailed Description

+

CANopen Object Dictionary storage object for Linux SocketCAN.

+
Author
Janez Paternoster
+
Copyright
2015 - 2020 Janez Paternoster
+

This file is part of CANopenNode, an opensource CANopen Stack. Project home page is https://github.com/CANopenNode/CANopenNode. For more information on CANopen see http://www.can-cia.org/.

+

Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. You may obtain a copy of the License at 

http://www.apache.org/licenses/LICENSE-2.0
+

Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License.

+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/CO__OD__storage_8h.js b/Devices/Libraries/Systems/CANopenSocket/docs/CO__OD__storage_8h.js new file mode 100755 index 0000000..840e20b --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/CO__OD__storage_8h.js @@ -0,0 +1,10 @@ +var CO__OD__storage_8h = +[ + [ "CO_ODF_1010", "group__CO__socketCAN__OD__storage.html#ga4a5e807a83eeab172bb3b0aeb6fa92c2", null ], + [ "CO_ODF_1011", "group__CO__socketCAN__OD__storage.html#ga059fcd46d8b15caf86c57d541a09576a", null ], + [ "CO_OD_storage_saveSecure", "group__CO__socketCAN__OD__storage.html#ga7f6124c9079807bc2f8f3d860571ccec", null ], + [ "CO_OD_storage_restoreSecure", "group__CO__socketCAN__OD__storage.html#ga63b392fa7eb2bdc92ecd2f1ff6f4ced0", null ], + [ "CO_OD_storage_init", "group__CO__socketCAN__OD__storage.html#ga5a26b63e7222b058c6024e54c6e0d5cd", null ], + [ "CO_OD_storage_autoSave", "group__CO__socketCAN__OD__storage.html#ga9381d4f670cef9068efaf7d42097de2f", null ], + [ "CO_OD_storage_autoSaveClose", "group__CO__socketCAN__OD__storage.html#ga6c51a86516ff7e128873f6cf1fdef5eb", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/CO__OD__storage_8h_source.html b/Devices/Libraries/Systems/CANopenSocket/docs/CO__OD__storage_8h_source.html new file mode 100755 index 0000000..0e862e3 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/CO__OD__storage_8h_source.html @@ -0,0 +1,186 @@ + + + + + + + +CANopenNode: socketCAN/CO_OD_storage.h Source File + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+
CO_OD_storage.h
+
+
+Go to the documentation of this file.
1 
+
27 #ifndef CO_OD_STORAGE_H
+
28 #define CO_OD_STORAGE_H
+
29 
+
30 
+
31 #include "301/CO_driver.h"
+
32 #include "301/CO_SDOserver.h"
+
33 
+
34 #include <stdio.h>
+
35 
+
36 #ifdef __cplusplus
+
37 extern "C" {
+
38 #endif
+
39 
+
52 CO_SDO_abortCode_t CO_ODF_1010(CO_ODF_arg_t *ODF_arg);
+
53 
+
57 CO_SDO_abortCode_t CO_ODF_1011(CO_ODF_arg_t *ODF_arg);
+
58 
+
59 
+
75 int CO_OD_storage_saveSecure(
+
76  uint8_t *odAddress,
+
77  uint32_t odSize,
+
78  char *filename);
+
79 
+
80 
+
97 int CO_OD_storage_restoreSecure(char *filename);
+
98 
+
99 
+
105 typedef struct {
+
106  uint8_t *odAddress;
+
107  uint32_t odSize;
+
108  char *filename;
+
110  FILE *fp;
+
111  uint32_t lastSavedUs;
+
112 } CO_OD_storage_t;
+
113 
+
114 
+
130 CO_ReturnError_t CO_OD_storage_init(
+
131  CO_OD_storage_t *odStor,
+
132  uint8_t *odAddress,
+
133  uint32_t odSize,
+
134  char *filename);
+
135 
+
136 
+
152 CO_ReturnError_t CO_OD_storage_autoSave(
+
153  CO_OD_storage_t *odStor,
+
154  uint32_t timer1usDiff,
+
155  uint32_t delay_us);
+
156 
+
157 
+
163 void CO_OD_storage_autoSaveClose(CO_OD_storage_t *odStor);
+
164 
+
167 #ifdef __cplusplus
+
168 }
+
169 #endif /* __cplusplus */
+
170 
+
171 #endif /* CO_OD_STORAGE_H */
+
+
+
uint32_t
unsigned long int uint32_t
UNSIGNED32 in CANopen (0007h), 32-bit unsigned integer.
Definition: CO_driver.h:155
+
CO_driver.h
Interface between CAN hardware and CANopenNode.
+
CO_OD_storage_restoreSecure
int CO_OD_storage_restoreSecure(char *filename)
Remove OD storage file.
Definition: CO_OD_storage.c:181
+
CO_OD_storage_autoSave
CO_ReturnError_t CO_OD_storage_autoSave(CO_OD_storage_t *odStor, uint32_t timer1usDiff, uint32_t delay_us)
Automatically save memory block if differs from file.
Definition: CO_OD_storage.c:292
+
CO_OD_storage_t
Object Dictionary storage object.
Definition: CO_OD_storage.h:105
+
CO_ODF_1010
CO_SDO_abortCode_t CO_ODF_1010(CO_ODF_arg_t *ODF_arg)
Callback for use inside CO_OD_configure() function for OD object 1010.
Definition: CO_OD_storage.c:42
+
CO_ReturnError_t
CO_ReturnError_t
Return values of some CANopen functions.
Definition: CO_driver.h:488
+
CO_OD_storage_t::odAddress
uint8_t * odAddress
From CO_OD_storage_init()
Definition: CO_OD_storage.h:106
+
CO_OD_storage_saveSecure
int CO_OD_storage_saveSecure(uint8_t *odAddress, uint32_t odSize, char *filename)
Save memory block to a file.
Definition: CO_OD_storage.c:102
+
CO_OD_storage_autoSaveClose
void CO_OD_storage_autoSaveClose(CO_OD_storage_t *odStor)
Closes file opened by CO_OD_storage_autoSave.
Definition: CO_OD_storage.c:369
+
CO_SDOserver.h
CANopen Service Data Object - server protocol.
+
CO_OD_storage_init
CO_ReturnError_t CO_OD_storage_init(CO_OD_storage_t *odStor, uint8_t *odAddress, uint32_t odSize, char *filename)
Initialize OD storage object and load data from file.
Definition: CO_OD_storage.c:224
+
CO_ODF_1011
CO_SDO_abortCode_t CO_ODF_1011(CO_ODF_arg_t *ODF_arg)
Callback for use inside CO_OD_configure() function for OD object 1011.
Definition: CO_OD_storage.c:72
+
CO_SDO_abortCode_t
CO_SDO_abortCode_t
SDO abort codes.
Definition: CO_SDOserver.h:333
+
CO_OD_storage_t::fp
FILE * fp
If CO_OD_storage_autoSave() is used, file stays opened and fp is stored here.
Definition: CO_OD_storage.h:110
+
CO_OD_storage_t::lastSavedUs
uint32_t lastSavedUs
used with CO_OD_storage_autoSave.
Definition: CO_OD_storage.h:111
+
CO_OD_storage_t::odSize
uint32_t odSize
From CO_OD_storage_init()
Definition: CO_OD_storage.h:107
+
CO_OD_storage_t::filename
char * filename
From CO_OD_storage_init()
Definition: CO_OD_storage.h:108
+
uint8_t
unsigned char uint8_t
UNSIGNED8 in CANopen (0005h), 8-bit unsigned integer.
Definition: CO_driver.h:151
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/CO__ODinterface_8h.html b/Devices/Libraries/Systems/CANopenSocket/docs/CO__ODinterface_8h.html new file mode 100755 index 0000000..349e5ae --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/CO__ODinterface_8h.html @@ -0,0 +1,469 @@ + + + + + + + +CANopenNode: 301/CO_ODinterface.h File Reference + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Data Structures | +Macros | +Enumerations | +Functions
+
+
CO_ODinterface.h File Reference
CANopen_301 » OD interface
+
+
+ +

CANopen Object Dictionary interface. +More...

+
#include "301/CO_driver.h"
+
+

Go to the source code of this file.

+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

+Data Structures

struct  OD_subEntry_t
 Structure describing properties of a variable, located in specific index and sub-index inside the Object Dictionary. More...
 
struct  OD_stream_t
 IO stream structure, used for read/write access to OD variable, part of OD_IO_t. More...
 
struct  OD_IO_t
 Structure for input / output on the OD variable. More...
 
struct  OD_entry_t
 Object Dictionary entry for one OD object. More...
 
struct  OD_t
 Object Dictionary. More...
 
struct  OD_obj_var_t
 Object for single OD variable, used for "VAR" type OD objects. More...
 
struct  OD_obj_array_t
 Object for OD array of variables, used for "ARRAY" type OD objects. More...
 
struct  OD_obj_record_t
 Object for OD sub-elements, used in "RECORD" type OD objects. More...
 
struct  OD_extensionIO_t
 Object pointed by OD_obj_extended_t contains application specified parameters for extended OD object. More...
 
struct  OD_obj_extended_t
 Object for extended type of OD variable, configurable by OD_extensionIO_init() function. More...
 
+ + + + + + + + + + +

+Macros

+#define OD_size_t   uint32_t
 Variable of type OD_size_t contains data length in bytes of OD variable.
 
+#define OD_flagsPDO_t   uint32_t
 Type of flagsPDO variable from OD_subEntry_t.
 
+#define OD_attr_t   uint8_t
 Size of Object Dictionary attribute.
 
+ + + + + + + + + + + + + +

+Enumerations

enum  OD_ObjDicId_30x_t {
+  OD_H1000_DEV_TYPE = 0x1000U, +OD_H1001_ERR_REG = 0x1001U, +OD_H1002_MANUF_STATUS_REG = 0x1002U, +OD_H1003_PREDEF_ERR_FIELD = 0x1003U, +
+  OD_H1004_RSV = 0x1004U, +OD_H1005_COBID_SYNC = 0x1005U, +OD_H1006_COMM_CYCL_PERIOD = 0x1006U, +OD_H1007_SYNC_WINDOW_LEN = 0x1007U, +
+  OD_H1008_MANUF_DEV_NAME = 0x1008U, +OD_H1009_MANUF_HW_VERSION = 0x1009U, +OD_H100A_MANUF_SW_VERSION = 0x100AU, +OD_H100B_RSV = 0x100BU, +
+  OD_H100C_GUARD_TIME = 0x100CU, +OD_H100D_LIFETIME_FACTOR = 0x100DU, +OD_H100E_RSV = 0x100EU, +OD_H100F_RSV = 0x100FU, +
+  OD_H1010_STORE_PARAM_FUNC = 0x1010U, +OD_H1011_REST_PARAM_FUNC = 0x1011U, +OD_H1012_COBID_TIME = 0x1012U, +OD_H1013_HIGH_RES_TIMESTAMP = 0x1013U, +
+  OD_H1014_COBID_EMERGENCY = 0x1014U, +OD_H1015_INHIBIT_TIME_EMCY = 0x1015U, +OD_H1016_CONSUMER_HB_TIME = 0x1016U, +OD_H1017_PRODUCER_HB_TIME = 0x1017U, +
+  OD_H1018_IDENTITY_OBJECT = 0x1018U, +OD_H1019_SYNC_CNT_OVERFLOW = 0x1019U, +OD_H1020_VERIFY_CONFIG = 0x1020U, +OD_H1021_STORE_EDS = 0x1021U, +
+  OD_H1022_STORE_FORMAT = 0x1022U, +OD_H1023_OS_CMD = 0x1023U, +OD_H1024_OS_CMD_MODE = 0x1024U, +OD_H1025_OS_DBG_INTERFACE = 0x1025U, +
+  OD_H1026_OS_PROMPT = 0x1026U, +OD_H1027_MODULE_LIST = 0x1027U, +OD_H1028_EMCY_CONSUMER = 0x1028U, +OD_H1029_ERR_BEHAVIOR = 0x1029U, +
+  OD_H1200_SDO_SERVER_1_PARAM = 0x1200U, +OD_H1280_SDO_CLIENT_1_PARAM = 0x1280U, +OD_H1300_GFC_PARAM = 0x1300U, +OD_H1301_SRDO_1_PARAM = 0x1301U, +
+  OD_H1381_SRDO_1_MAPPING = 0x1381U, +OD_H13FE_SRDO_VALID = 0x13FEU, +OD_H13FF_SRDO_CHECKSUM = 0x13FFU, +OD_H1400_RXPDO_1_PARAM = 0x1400U, +
+  OD_H1600_RXPDO_1_MAPPING = 0x1600U, +OD_H1800_TXPDO_1_PARAM = 0x1800U, +OD_H1A00_TXPDO_1_MAPPING = 0x1A00U +
+ }
 Common DS301 object dictionary entries. More...
 
enum  OD_attributes_t {
+  ODA_SDO_R = 0x01, +ODA_SDO_W = 0x02, +ODA_SDO_RW = 0x03, +ODA_TPDO = 0x04, +
+  ODA_RPDO = 0x08, +ODA_TRPDO = 0x0C, +ODA_TSRDO = 0x10, +ODA_RSRDO = 0x20, +
+  ODA_TRSRDO = 0x30, +ODA_MB = 0x40, +ODA_STR = 0x80 +
+ }
 Attributes (bit masks) for OD sub-object. More...
 
enum  ODR_t {
+  ODR_PARTIAL = -1, +ODR_OK = 0, +ODR_OUT_OF_MEM = 1, +ODR_UNSUPP_ACCESS = 2, +
+  ODR_WRITEONLY = 3, +ODR_READONLY = 4, +ODR_IDX_NOT_EXIST = 5, +ODR_NO_MAP = 6, +
+  ODR_MAP_LEN = 7, +ODR_PAR_INCOMPAT = 8, +ODR_DEV_INCOMPAT = 9, +ODR_HW = 10, +
+  ODR_TYPE_MISMATCH = 11, +ODR_DATA_LONG = 12, +ODR_DATA_SHORT = 13, +ODR_SUB_NOT_EXIST = 14, +
+  ODR_INVALID_VALUE = 15, +ODR_VALUE_HIGH = 16, +ODR_VALUE_LOW = 17, +ODR_MAX_LESS_MIN = 18, +
+  ODR_NO_RESOURCE = 19, +ODR_GENERAL = 20, +ODR_DATA_TRANSF = 21, +ODR_DATA_LOC_CTRL = 22, +
+  ODR_DATA_DEV_STATE = 23, +ODR_OD_MISSING = 23, +ODR_NO_DATA = 25, +ODR_COUNT = 26 +
+ }
 Return codes from OD access functions. More...
 
enum  OD_objectTypes_t {
+  ODT_VAR = 0x01, +ODT_ARR = 0x02, +ODT_REC = 0x03, +ODT_EVAR = 0x11, +
+  ODT_EARR = 0x12, +ODT_EREC = 0x13, +ODT_TYPE_MASK = 0x0F, +ODT_EXTENSION_MASK = 0x10 +
+ }
 Types for OD object. More...
 
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

+Functions

OD_size_t OD_readOriginal (OD_stream_t *stream, uint8_t subIndex, void *buf, OD_size_t count, ODR_t *returnCode)
 Read value from original OD location. More...
 
OD_size_t OD_writeOriginal (OD_stream_t *stream, uint8_t subIndex, const void *buf, OD_size_t count, ODR_t *returnCode)
 Write value to original OD location. More...
 
const OD_entry_t * OD_find (const OD_t *od, uint16_t index)
 Find OD entry in Object Dictionary. More...
 
ODR_t OD_getSub (const OD_entry_t *entry, uint8_t subIndex, OD_subEntry_t *subEntry, OD_IO_t *io, bool_t odOrig)
 Find sub-object with specified sub-index on OD entry returned by OD_find. More...
 
static uint16_t OD_getIndex (const OD_entry_t *entry)
 Return index from OD entry. More...
 
static void OD_rwRestart (OD_stream_t *stream)
 Restart read or write operation on OD variable. More...
 
uint32_t OD_getSDOabCode (ODR_t returnCode)
 Get SDO abort code from returnCode. More...
 
ODR_t OD_extensionIO_init (const OD_entry_t *entry, void *object, OD_size_t(*read)(OD_stream_t *stream, uint8_t subIndex, void *buf, OD_size_t count, ODR_t *returnCode), OD_size_t(*write)(OD_stream_t *stream, uint8_t subIndex, const void *buf, OD_size_t count, ODR_t *returnCode))
 Initialise extended OD object with own read/write functions. More...
 
ODR_t OD_get_i8 (const OD_entry_t *entry, uint8_t subIndex, int8_t *val, bool_t odOrig)
 Get int8_t variable from Object Dictionary. More...
 
+ODR_t OD_get_i16 (const OD_entry_t *entry, uint8_t subIndex, int16_t *val, bool_t odOrig)
 Get int16_t variable from Object Dictionary, see OD_get_i8.
 
+ODR_t OD_get_i32 (const OD_entry_t *entry, uint8_t subIndex, int32_t *val, bool_t odOrig)
 Get int32_t variable from Object Dictionary, see OD_get_i8.
 
+ODR_t OD_get_i64 (const OD_entry_t *entry, uint8_t subIndex, int64_t *val, bool_t odOrig)
 Get int64_t variable from Object Dictionary, see OD_get_i8.
 
+ODR_t OD_get_u8 (const OD_entry_t *entry, uint8_t subIndex, uint8_t *val, bool_t odOrig)
 Get uint8_t variable from Object Dictionary, see OD_get_i8.
 
+ODR_t OD_get_u16 (const OD_entry_t *entry, uint8_t subIndex, uint16_t *val, bool_t odOrig)
 Get uint16_t variable from Object Dictionary, see OD_get_i8.
 
+ODR_t OD_get_u32 (const OD_entry_t *entry, uint8_t subIndex, uint32_t *val, bool_t odOrig)
 Get uint32_t variable from Object Dictionary, see OD_get_i8.
 
+ODR_t OD_get_u64 (const OD_entry_t *entry, uint8_t subIndex, uint64_t *val, bool_t odOrig)
 Get uint64_t variable from Object Dictionary, see OD_get_i8.
 
+ODR_t OD_get_r32 (const OD_entry_t *entry, uint8_t subIndex, float32_t *val, bool_t odOrig)
 Get float32_t variable from Object Dictionary, see OD_get_i8.
 
+ODR_t OD_get_r64 (const OD_entry_t *entry, uint8_t subIndex, float64_t *val, bool_t odOrig)
 Get float64_t variable from Object Dictionary, see OD_get_i8.
 
ODR_t OD_set_i8 (const OD_entry_t *entry, uint8_t subIndex, int8_t val, bool_t odOrig)
 Set int8_t variable in Object Dictionary. More...
 
+ODR_t OD_set_i16 (const OD_entry_t *entry, uint8_t subIndex, int16_t val, bool_t odOrig)
 Set int16_t variable in Object Dictionary, see OD_set_i8.
 
+ODR_t OD_set_i32 (const OD_entry_t *entry, uint8_t subIndex, int32_t val, bool_t odOrig)
 Set int16_t variable in Object Dictionary, see OD_set_i8.
 
+ODR_t OD_set_i64 (const OD_entry_t *entry, uint8_t subIndex, int64_t val, bool_t odOrig)
 Set int16_t variable in Object Dictionary, see OD_set_i8.
 
+ODR_t OD_set_u8 (const OD_entry_t *entry, uint8_t subIndex, uint8_t val, bool_t odOrig)
 Set uint8_t variable in Object Dictionary, see OD_set_i8.
 
+ODR_t OD_set_u16 (const OD_entry_t *entry, uint8_t subIndex, uint16_t val, bool_t odOrig)
 Set uint16_t variable in Object Dictionary, see OD_set_i8.
 
+ODR_t OD_set_u32 (const OD_entry_t *entry, uint8_t subIndex, uint32_t val, bool_t odOrig)
 Set uint32_t variable in Object Dictionary, see OD_set_i8.
 
+ODR_t OD_set_u64 (const OD_entry_t *entry, uint8_t subIndex, uint64_t val, bool_t odOrig)
 Set uint64_t variable in Object Dictionary, see OD_set_i8.
 
+ODR_t OD_set_r32 (const OD_entry_t *entry, uint8_t subIndex, float32_t val, bool_t odOrig)
 Set float32_t variable in Object Dictionary, see OD_set_i8.
 
+ODR_t OD_set_r64 (const OD_entry_t *entry, uint8_t subIndex, float64_t val, bool_t odOrig)
 Set float64_t variable in Object Dictionary, see OD_set_i8.
 
ODR_t OD_getPtr_i8 (const OD_entry_t *entry, uint8_t subIndex, int8_t **val)
 Get pointer to int8_t variable from Object Dictionary. More...
 
+ODR_t OD_getPtr_i16 (const OD_entry_t *entry, uint8_t subIndex, int16_t **val)
 Get pointer to int16_t variable from OD, see OD_getPtr_i8.
 
+ODR_t OD_getPtr_i32 (const OD_entry_t *entry, uint8_t subIndex, int32_t **val)
 Get pointer to int32_t variable from OD, see OD_getPtr_i8.
 
+ODR_t OD_getPtr_i64 (const OD_entry_t *entry, uint8_t subIndex, int64_t **val)
 Get pointer to int64_t variable from OD, see OD_getPtr_i8.
 
+ODR_t OD_getPtr_u8 (const OD_entry_t *entry, uint8_t subIndex, uint8_t **val)
 Get pointer to uint8_t variable from OD, see OD_getPtr_i8.
 
+ODR_t OD_getPtr_u16 (const OD_entry_t *entry, uint8_t subIndex, uint16_t **val)
 Get pointer to uint16_t variable from OD, see OD_getPtr_i8.
 
+ODR_t OD_getPtr_u32 (const OD_entry_t *entry, uint8_t subIndex, uint32_t **val)
 Get pointer to uint32_t variable from OD, see OD_getPtr_i8.
 
+ODR_t OD_getPtr_u64 (const OD_entry_t *entry, uint8_t subIndex, uint64_t **val)
 Get pointer to uint64_t variable from OD, see OD_getPtr_i8.
 
+ODR_t OD_getPtr_r32 (const OD_entry_t *entry, uint8_t subIndex, float32_t **val)
 Get pointer to float32_t variable from OD, see OD_getPtr_i8.
 
+ODR_t OD_getPtr_r64 (const OD_entry_t *entry, uint8_t subIndex, float64_t **val)
 Get pointer to float64_t variable from OD, see OD_getPtr_i8.
 
ODR_t OD_getPtr_vs (const OD_entry_t *entry, uint8_t subIndex, char **val, OD_size_t *dataLength)
 Get pointer to "visible string" variable from Object Dictionary. More...
 
+ODR_t OD_getPtr_os (const OD_entry_t *entry, uint8_t subIndex, uint8_t **val, OD_size_t *dataLength)
 Get pointer to "octet string" variable from OD, see OD_getPtr_vs.
 
+ODR_t OD_getPtr_us (const OD_entry_t *entry, uint8_t subIndex, uint16_t **val, OD_size_t *dataLength)
 Get pointer to "unicode string" variable from OD, see OD_getPtr_vs.
 
+

Detailed Description

+

CANopen Object Dictionary interface.

+
Author
Janez Paternoster
+
Copyright
2020 Janez Paternoster
+

This file is part of CANopenNode, an opensource CANopen Stack. Project home page is https://github.com/CANopenNode/CANopenNode. For more information on CANopen see http://www.can-cia.org/.

+

Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. You may obtain a copy of the License at 

http://www.apache.org/licenses/LICENSE-2.0
+

Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License.

+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/CO__ODinterface_8h.js b/Devices/Libraries/Systems/CANopenSocket/docs/CO__ODinterface_8h.js new file mode 100755 index 0000000..efa4feb --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/CO__ODinterface_8h.js @@ -0,0 +1,149 @@ +var CO__ODinterface_8h = +[ + [ "OD_size_t", "group__CO__ODinterface.html#gaef984c993ddbf6a0500391e97f05d08e", null ], + [ "OD_flagsPDO_t", "group__CO__ODinterface.html#ga69f6e1121545e5669098f49e95ce4e47", null ], + [ "OD_attr_t", "group__CO__ODinterface.html#ga8d459f95307815637e41edc4df71a725", null ], + [ "OD_ObjDicId_30x_t", "group__CO__ODinterface.html#gade8960f241ee3b728eac09288a694886", [ + [ "OD_H1000_DEV_TYPE", "group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886af1e65ef6eb730b9302540e0ba44852b1", null ], + [ "OD_H1001_ERR_REG", "group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886af4eb4e0204ae9696f935af5d4fdcff7e", null ], + [ "OD_H1002_MANUF_STATUS_REG", "group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a2cca52f61d70db5ca2aaa168b32f3aaf", null ], + [ "OD_H1003_PREDEF_ERR_FIELD", "group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886adfc213af5af80cf037231621132013fb", null ], + [ "OD_H1004_RSV", "group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886ad7d53fa95504566811bdf0683f645ccd", null ], + [ "OD_H1005_COBID_SYNC", "group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886adcbc9ec0c547b00db2b0403708becb97", null ], + [ "OD_H1006_COMM_CYCL_PERIOD", "group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a463dab19c27811dd6de51fcc082b565b", null ], + [ "OD_H1007_SYNC_WINDOW_LEN", "group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a41f7f0f96dbea4fb0d6bd2bbbd2d59dc", null ], + [ "OD_H1008_MANUF_DEV_NAME", "group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886aa282ef78e8a64ff527c79218d23168f0", null ], + [ "OD_H1009_MANUF_HW_VERSION", "group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886ad91534dd5cc5f382287b9a392c744948", null ], + [ "OD_H100A_MANUF_SW_VERSION", "group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a8bd48d9bb18d1c291249050818c82a57", null ], + [ "OD_H100B_RSV", "group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a561260a506655cf0b7df9d684a08b5be", null ], + [ "OD_H100C_GUARD_TIME", "group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886adf00d5b448274a91940cac15b8e22fc5", null ], + [ "OD_H100D_LIFETIME_FACTOR", "group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a4bfbb36a82606125d52fbe4daff6b5fb", null ], + [ "OD_H100E_RSV", "group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a4d831b2a36d679d31982e35ca38f8f6e", null ], + [ "OD_H100F_RSV", "group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a7d6a3f1ca8f72bf808ee5fe341f2acca", null ], + [ "OD_H1010_STORE_PARAM_FUNC", "group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a0f8c3db5a62d5e4df59d83253b69b0f2", null ], + [ "OD_H1011_REST_PARAM_FUNC", "group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886adad8c0ea18f674f3eb61b43e8259395c", null ], + [ "OD_H1012_COBID_TIME", "group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a5781f519d9ec08fd4389c4761754a4e6", null ], + [ "OD_H1013_HIGH_RES_TIMESTAMP", "group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a7aa32b2b6df7c4d4354599ef2fd2ca29", null ], + [ "OD_H1014_COBID_EMERGENCY", "group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a5cae036f3dd0bc1861dcea7c9a83c6d5", null ], + [ "OD_H1015_INHIBIT_TIME_EMCY", "group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886adbcd0d36ab781fc60b05a94288b8f019", null ], + [ "OD_H1016_CONSUMER_HB_TIME", "group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a996952cb963ce6e2783a6fa915d85612", null ], + [ "OD_H1017_PRODUCER_HB_TIME", "group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886ab2f428825c1127b286f5b8ace5e881b2", null ], + [ "OD_H1018_IDENTITY_OBJECT", "group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886aabb7a688852e453c5535f663be6298d2", null ], + [ "OD_H1019_SYNC_CNT_OVERFLOW", "group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886aa966d2e020222331b18c5b08261acbf0", null ], + [ "OD_H1020_VERIFY_CONFIG", "group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a8c740bbdd0cb98200d402ec6272d7e8b", null ], + [ "OD_H1021_STORE_EDS", "group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a259737f7e9ef239d85cb9e7bdeda550b", null ], + [ "OD_H1022_STORE_FORMAT", "group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a4061d54d1c1583fd178566a3915bcefe", null ], + [ "OD_H1023_OS_CMD", "group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a12ba9d8cdfc20b9ff66167a8d1e5b21c", null ], + [ "OD_H1024_OS_CMD_MODE", "group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886ad7e64256615fcda5b531063eeaa346de", null ], + [ "OD_H1025_OS_DBG_INTERFACE", "group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886aaf890d86468408b0dbe8353a3b270156", null ], + [ "OD_H1026_OS_PROMPT", "group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a6e3b80d148d22f129ed388fad9aaf398", null ], + [ "OD_H1027_MODULE_LIST", "group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a668cdf3b0102b753858b9bfeb7efdc1c", null ], + [ "OD_H1028_EMCY_CONSUMER", "group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a6f093f4fdeaac7b723305fd8d2ce40c1", null ], + [ "OD_H1029_ERR_BEHAVIOR", "group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a67d50722cc629ee8c2a90a123ee41fa3", null ], + [ "OD_H1200_SDO_SERVER_1_PARAM", "group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a7e351a37cb53541b510e97711e167450", null ], + [ "OD_H1280_SDO_CLIENT_1_PARAM", "group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a3e0e0989da485b0511d7e7cadcdb6dbb", null ], + [ "OD_H1300_GFC_PARAM", "group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886afa584e69f73b7a80cd708010fc0ae64f", null ], + [ "OD_H1301_SRDO_1_PARAM", "group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886afbb7d48d240c7584d461712685e62945", null ], + [ "OD_H1381_SRDO_1_MAPPING", "group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a8effa6834289b482e5ac3319ccf2c17b", null ], + [ "OD_H13FE_SRDO_VALID", "group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886aa66d7204dd265d19276113ef7177ce68", null ], + [ "OD_H13FF_SRDO_CHECKSUM", "group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a92c6bed78bc6bbbd182c16fe8890dcbf", null ], + [ "OD_H1400_RXPDO_1_PARAM", "group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886ae64bf5b7166b6adf46b8e965d43150a0", null ], + [ "OD_H1600_RXPDO_1_MAPPING", "group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a7ed53d283e4719920b233b9094b18f9c", null ], + [ "OD_H1800_TXPDO_1_PARAM", "group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886aac35e48e4b717eef309ebb57876d30f1", null ], + [ "OD_H1A00_TXPDO_1_MAPPING", "group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a2a2f1c4cc58d29ccf43e105afd57bc14", null ] + ] ], + [ "OD_attributes_t", "group__CO__ODinterface.html#ga47b0d204aaf1ea64b4f826aaf8f5c151", [ + [ "ODA_SDO_R", "group__CO__ODinterface.html#gga47b0d204aaf1ea64b4f826aaf8f5c151ada3eb985961ffdd9cf655d1a7a7d9485", null ], + [ "ODA_SDO_W", "group__CO__ODinterface.html#gga47b0d204aaf1ea64b4f826aaf8f5c151a9245e7a557f32ab863aef41412df9eb5", null ], + [ "ODA_SDO_RW", "group__CO__ODinterface.html#gga47b0d204aaf1ea64b4f826aaf8f5c151a2c60ba85cbe4f25d5511ffab3dcd7486", null ], + [ "ODA_TPDO", "group__CO__ODinterface.html#gga47b0d204aaf1ea64b4f826aaf8f5c151ad106aacb6b181ab7dac0f6dbc8c50321", null ], + [ "ODA_RPDO", "group__CO__ODinterface.html#gga47b0d204aaf1ea64b4f826aaf8f5c151ae4930aa0efbc2249563613b5107bb107", null ], + [ "ODA_TRPDO", "group__CO__ODinterface.html#gga47b0d204aaf1ea64b4f826aaf8f5c151af750832681daa8d5e44ed8908c4ec552", null ], + [ "ODA_TSRDO", "group__CO__ODinterface.html#gga47b0d204aaf1ea64b4f826aaf8f5c151a3a1b8c2ed54565e89d6e6d3a043bdcfe", null ], + [ "ODA_RSRDO", "group__CO__ODinterface.html#gga47b0d204aaf1ea64b4f826aaf8f5c151a6c6261d5bea91588b2851f5c11faae02", null ], + [ "ODA_TRSRDO", "group__CO__ODinterface.html#gga47b0d204aaf1ea64b4f826aaf8f5c151af887f38d83f35c28d478f1a4b08d1be9", null ], + [ "ODA_MB", "group__CO__ODinterface.html#gga47b0d204aaf1ea64b4f826aaf8f5c151ae02b665e7e8d8bd84f341c9ad040d367", null ], + [ "ODA_STR", "group__CO__ODinterface.html#gga47b0d204aaf1ea64b4f826aaf8f5c151a9527d9c21ba4159d653771e1dedad81d", null ] + ] ], + [ "ODR_t", "group__CO__ODinterface.html#ga0e9afd8ad27de0920d1fe0738834869c", [ + [ "ODR_PARTIAL", "group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869caf7595749473065bfc81cfa6709370fee", null ], + [ "ODR_OK", "group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869ca15f7f20e27f1c5f174bdeecfeef45cc2", null ], + [ "ODR_OUT_OF_MEM", "group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869ca118a63a81ef2fd802c925bf4c79975fa", null ], + [ "ODR_UNSUPP_ACCESS", "group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869ca17d694adc9471112cbb2740f7f45a2d0", null ], + [ "ODR_WRITEONLY", "group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869ca44ab94bfc7547122b96498c781291df6", null ], + [ "ODR_READONLY", "group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869caa119384effe499c9bffb874219c6433a", null ], + [ "ODR_IDX_NOT_EXIST", "group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869ca64ea80d1baebf136382d53d5580fbc85", null ], + [ "ODR_NO_MAP", "group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869ca853242f74446c58773099bdef9835a94", null ], + [ "ODR_MAP_LEN", "group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869cad6c92203fa86ee8ff7d7271bf81e7d9e", null ], + [ "ODR_PAR_INCOMPAT", "group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869ca36edb0ad1c8c5c0d804bb88274bfe165", null ], + [ "ODR_DEV_INCOMPAT", "group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869ca3d259ef030c2a10afecb253b532a0323", null ], + [ "ODR_HW", "group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869caeaac26c680e626185429468dda9c2433", null ], + [ "ODR_TYPE_MISMATCH", "group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869cad197c1462f472a21be2e3ed5c5880aa4", null ], + [ "ODR_DATA_LONG", "group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869ca3f93f54b7f130bb7b266b2d36c24caec", null ], + [ "ODR_DATA_SHORT", "group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869cae0b103160b23ff40047fcf85225121d2", null ], + [ "ODR_SUB_NOT_EXIST", "group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869ca4f76fa87ea446616ff2f6195e7bec67c", null ], + [ "ODR_INVALID_VALUE", "group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869ca0646f592124a9b4d2835c9c0296c6a0c", null ], + [ "ODR_VALUE_HIGH", "group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869ca95291309267a732c380e13ad6c17a986", null ], + [ "ODR_VALUE_LOW", "group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869ca4db02b8575a8a10786959a5472f1c0f4", null ], + [ "ODR_MAX_LESS_MIN", "group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869cac9c8eda20bfc7dfffe17f170c377d646", null ], + [ "ODR_NO_RESOURCE", "group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869ca15fe8d8b791b90373e59bc5bc5d3f8c8", null ], + [ "ODR_GENERAL", "group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869ca968d2a28cb866cacf7ef8b8cd0b76e2c", null ], + [ "ODR_DATA_TRANSF", "group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869ca341104b9ac43168d0c668586b8f750bb", null ], + [ "ODR_DATA_LOC_CTRL", "group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869ca28b28d8f091eefb7e8fac92bbdae82bb", null ], + [ "ODR_DATA_DEV_STATE", "group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869ca16bfe0c712aaea3841ae2b250331b276", null ], + [ "ODR_OD_MISSING", "group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869caeae1e00129ff22708ffdcb2c8b3f083b", null ], + [ "ODR_NO_DATA", "group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869caff503d4f6cc55429913680d071ca3c4d", null ], + [ "ODR_COUNT", "group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869cac43414be729ea2b701380c4400658c37", null ] + ] ], + [ "OD_objectTypes_t", "group__CO__ODdefinition.html#gaae426e9d66ec1bacfef2d93f096d7805", [ + [ "ODT_VAR", "group__CO__ODdefinition.html#ggaae426e9d66ec1bacfef2d93f096d7805a829f1df882410efc0ea0e05b3435c820", null ], + [ "ODT_ARR", "group__CO__ODdefinition.html#ggaae426e9d66ec1bacfef2d93f096d7805a1ad5763beafe79c42ca223297c832ff4", null ], + [ "ODT_REC", "group__CO__ODdefinition.html#ggaae426e9d66ec1bacfef2d93f096d7805a9376cface357f03bec8a651a307f33b9", null ], + [ "ODT_EVAR", "group__CO__ODdefinition.html#ggaae426e9d66ec1bacfef2d93f096d7805adb58a7faa735918d39b8bbcd3a6ad594", null ], + [ "ODT_EARR", "group__CO__ODdefinition.html#ggaae426e9d66ec1bacfef2d93f096d7805a1ae954b4709b24d93bdcac69957c8e40", null ], + [ "ODT_EREC", "group__CO__ODdefinition.html#ggaae426e9d66ec1bacfef2d93f096d7805ae176b06a942e47815d2e4c51a8f9b7f8", null ], + [ "ODT_TYPE_MASK", "group__CO__ODdefinition.html#ggaae426e9d66ec1bacfef2d93f096d7805ac9e24bde0d37c35c3065dbaa541e1acb", null ], + [ "ODT_EXTENSION_MASK", "group__CO__ODdefinition.html#ggaae426e9d66ec1bacfef2d93f096d7805a3dd8bc41ec11c475d487b877fdd0a879", null ] + ] ], + [ "OD_readOriginal", "group__CO__ODinterface.html#gadf9ac60f94e1f9fc21b7f10a0d254503", null ], + [ "OD_writeOriginal", "group__CO__ODinterface.html#ga648f0b0bfabde2d377149bf84e937422", null ], + [ "OD_find", "group__CO__ODinterface.html#gaaacaadfc28bfaf485cefc8bff64310f4", null ], + [ "OD_getSub", "group__CO__ODinterface.html#gaf1f736d4b4d6754d971f0c0a63655bcf", null ], + [ "OD_getIndex", "group__CO__ODinterface.html#gac84e7390f50e7e5c5e8ba42714e51aaf", null ], + [ "OD_rwRestart", "group__CO__ODinterface.html#ga3715e0a6b15bdf45659e1e01f9fc4e65", null ], + [ "OD_getSDOabCode", "group__CO__ODinterface.html#ga7c24b06bb9365d41b8e60acb4eaecc6c", null ], + [ "OD_extensionIO_init", "group__CO__ODinterface.html#gac07bbe54fbfecc6bc8da2e10b2c0f7e8", null ], + [ "OD_get_i8", "group__CO__ODgetSetters.html#gac174f05be716b7d169e0d7b7393e512c", null ], + [ "OD_get_i16", "group__CO__ODgetSetters.html#ga4bcc220e0cc4f8c6bfaf4d5cf31da448", null ], + [ "OD_get_i32", "group__CO__ODgetSetters.html#ga042737cecbf248d2cc6c874022d06e22", null ], + [ "OD_get_i64", "group__CO__ODgetSetters.html#gabfb67bc3602d9b5d901ac13e5271ccd0", null ], + [ "OD_get_u8", "group__CO__ODgetSetters.html#gad8fd318804b9f1ded265bfd07c6cdcf2", null ], + [ "OD_get_u16", "group__CO__ODgetSetters.html#gaa9cba6642facf33cdbe2e0155a90d571", null ], + [ "OD_get_u32", "group__CO__ODgetSetters.html#ga10e1975b6177b92e8da4b2b6a19533be", null ], + [ "OD_get_u64", "group__CO__ODgetSetters.html#ga2ef84e594a6733f7efeaeff55bfc9c9a", null ], + [ "OD_get_r32", "group__CO__ODgetSetters.html#gabbba108fa7a92cb48d31833bb804baa6", null ], + [ "OD_get_r64", "group__CO__ODgetSetters.html#gadf78189c35343bcd2ca1737491b55684", null ], + [ "OD_set_i8", "group__CO__ODgetSetters.html#ga46df632d54b48714cf50e4a3a92b4e98", null ], + [ "OD_set_i16", "group__CO__ODgetSetters.html#gaf207e188609f742be3c12f248f45138c", null ], + [ "OD_set_i32", "group__CO__ODgetSetters.html#ga101037295ea6af8488b499097f0d1ef1", null ], + [ "OD_set_i64", "group__CO__ODgetSetters.html#ga5c19e8e101d06a0203d13f8d3e997d16", null ], + [ "OD_set_u8", "group__CO__ODgetSetters.html#ga3f56347af1f0f9b8bf46463777df87d0", null ], + [ "OD_set_u16", "group__CO__ODgetSetters.html#gac63795511b7decfbbc46ddb7c2b59cfd", null ], + [ "OD_set_u32", "group__CO__ODgetSetters.html#ga5e5e943b89a2385f41a075131f47b5d5", null ], + [ "OD_set_u64", "group__CO__ODgetSetters.html#ga85570b44cc9d0af5b1084f42aeaf5e9f", null ], + [ "OD_set_r32", "group__CO__ODgetSetters.html#ga032219231c5e488969d02787265ad7ab", null ], + [ "OD_set_r64", "group__CO__ODgetSetters.html#ga75c5dd2daa6f0352bf079d18c9e90708", null ], + [ "OD_getPtr_i8", "group__CO__ODgetSetters.html#gaa4e6671855056e5ae13d198acdfab664", null ], + [ "OD_getPtr_i16", "group__CO__ODgetSetters.html#gaddbad04c274a68f3fabfbbdd13e83cfc", null ], + [ "OD_getPtr_i32", "group__CO__ODgetSetters.html#ga085db308dd11ea7047c815d214f12a32", null ], + [ "OD_getPtr_i64", "group__CO__ODgetSetters.html#gae7e11e1bdf04006dcc6a9f91287c459b", null ], + [ "OD_getPtr_u8", "group__CO__ODgetSetters.html#gaaa9f06494001462d32a74d11438d3157", null ], + [ "OD_getPtr_u16", "group__CO__ODgetSetters.html#gad1e334a640dc30553ece27e32e270bbb", null ], + [ "OD_getPtr_u32", "group__CO__ODgetSetters.html#ga92f57ed14ed69ac00c78d48f1e479bb0", null ], + [ "OD_getPtr_u64", "group__CO__ODgetSetters.html#gad192b1efde6a5bd6fa62fc5a4c484d84", null ], + [ "OD_getPtr_r32", "group__CO__ODgetSetters.html#gaf4e3f15fc0fbe2b24e88aeb53c9daa61", null ], + [ "OD_getPtr_r64", "group__CO__ODgetSetters.html#ga87a34e3e04dd2faa851d2d9c0a24e945", null ], + [ "OD_getPtr_vs", "group__CO__ODgetSetters.html#gaad4d755515e8dccf3616f14fe3914bde", null ], + [ "OD_getPtr_os", "group__CO__ODgetSetters.html#ga06361976dce86f180de76e1fced9a212", null ], + [ "OD_getPtr_us", "group__CO__ODgetSetters.html#gaf3efb5f4a50ff7ab17b37bf9c0fcf030", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/CO__ODinterface_8h_source.html b/Devices/Libraries/Systems/CANopenSocket/docs/CO__ODinterface_8h_source.html new file mode 100755 index 0000000..6b6567f --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/CO__ODinterface_8h_source.html @@ -0,0 +1,630 @@ + + + + + + + +CANopenNode: 301/CO_ODinterface.h Source File + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+
CO_ODinterface.h
+
+
+Go to the documentation of this file.
1 
+
26 #ifndef CO_OD_INTERFACE_H
+
27 #define CO_OD_INTERFACE_H
+
28 
+
29 #include "301/CO_driver.h"
+
30 
+
31 #ifdef __cplusplus
+
32 extern "C" {
+
33 #endif
+
34 
+
42 #ifndef OD_size_t
+
43 
+
44 #define OD_size_t uint32_t
+
45 
+
46 #define OD_flagsPDO_t uint32_t
+
47 #endif
+
48 
+
50 #define OD_attr_t uint8_t
+
51 
+
52 
+
56 typedef enum {
+
57  OD_H1000_DEV_TYPE = 0x1000U,
+
58  OD_H1001_ERR_REG = 0x1001U,
+
59  OD_H1002_MANUF_STATUS_REG = 0x1002U,
+
60  OD_H1003_PREDEF_ERR_FIELD = 0x1003U,
+
61  OD_H1004_RSV = 0x1004U,
+
62  OD_H1005_COBID_SYNC = 0x1005U,
+
63  OD_H1006_COMM_CYCL_PERIOD = 0x1006U,
+
64  OD_H1007_SYNC_WINDOW_LEN = 0x1007U,
+
65  OD_H1008_MANUF_DEV_NAME = 0x1008U,
+
66  OD_H1009_MANUF_HW_VERSION = 0x1009U,
+
67  OD_H100A_MANUF_SW_VERSION = 0x100AU,
+
68  OD_H100B_RSV = 0x100BU,
+
69  OD_H100C_GUARD_TIME = 0x100CU,
+
70  OD_H100D_LIFETIME_FACTOR = 0x100DU,
+
71  OD_H100E_RSV = 0x100EU,
+
72  OD_H100F_RSV = 0x100FU,
+
73  OD_H1010_STORE_PARAM_FUNC = 0x1010U,
+
74  OD_H1011_REST_PARAM_FUNC = 0x1011U,
+
75  OD_H1012_COBID_TIME = 0x1012U,
+
76  OD_H1013_HIGH_RES_TIMESTAMP = 0x1013U,
+
77  OD_H1014_COBID_EMERGENCY = 0x1014U,
+
78  OD_H1015_INHIBIT_TIME_EMCY = 0x1015U,
+
79  OD_H1016_CONSUMER_HB_TIME = 0x1016U,
+
80  OD_H1017_PRODUCER_HB_TIME = 0x1017U,
+
81  OD_H1018_IDENTITY_OBJECT = 0x1018U,
+
82  OD_H1019_SYNC_CNT_OVERFLOW = 0x1019U,
+
83  OD_H1020_VERIFY_CONFIG = 0x1020U,
+
84  OD_H1021_STORE_EDS = 0x1021U,
+
85  OD_H1022_STORE_FORMAT = 0x1022U,
+
86  OD_H1023_OS_CMD = 0x1023U,
+
87  OD_H1024_OS_CMD_MODE = 0x1024U,
+
88  OD_H1025_OS_DBG_INTERFACE = 0x1025U,
+
89  OD_H1026_OS_PROMPT = 0x1026U,
+
90  OD_H1027_MODULE_LIST = 0x1027U,
+
91  OD_H1028_EMCY_CONSUMER = 0x1028U,
+
92  OD_H1029_ERR_BEHAVIOR = 0x1029U,
+
93  OD_H1200_SDO_SERVER_1_PARAM = 0x1200U,
+
94  OD_H1280_SDO_CLIENT_1_PARAM = 0x1280U,
+
95  OD_H1300_GFC_PARAM = 0x1300U,
+
96  OD_H1301_SRDO_1_PARAM = 0x1301U,
+
97  OD_H1381_SRDO_1_MAPPING = 0x1381U,
+
98  OD_H13FE_SRDO_VALID = 0x13FEU,
+
99  OD_H13FF_SRDO_CHECKSUM = 0x13FFU,
+
100  OD_H1400_RXPDO_1_PARAM = 0x1400U,
+
101  OD_H1600_RXPDO_1_MAPPING = 0x1600U,
+
102  OD_H1800_TXPDO_1_PARAM = 0x1800U,
+
103  OD_H1A00_TXPDO_1_MAPPING = 0x1A00U,
+
104 } OD_ObjDicId_30x_t;
+
105 
+
106 
+
110 typedef enum {
+
111  ODA_SDO_R = 0x01,
+
112  ODA_SDO_W = 0x02,
+
113  ODA_SDO_RW = 0x03,
+
114  ODA_TPDO = 0x04,
+
115  ODA_RPDO = 0x08,
+
116  ODA_TRPDO = 0x0C,
+
117  ODA_TSRDO = 0x10,
+
118  ODA_RSRDO = 0x20,
+
119  ODA_TRSRDO = 0x30,
+
120  ODA_MB = 0x40,
+
121  ODA_STR = 0x80
+
124 } OD_attributes_t;
+
125 
+
126 
+
132 typedef enum {
+
133 /* !!!! WARNING !!!!
+
134  * If changing these values, change also OD_getSDOabCode() function!
+
135  */
+
137  ODR_PARTIAL = -1,
+
139  ODR_OK = 0,
+
141  ODR_OUT_OF_MEM = 1,
+
143  ODR_UNSUPP_ACCESS = 2,
+
145  ODR_WRITEONLY = 3,
+
147  ODR_READONLY = 4,
+
149  ODR_IDX_NOT_EXIST = 5,
+
151  ODR_NO_MAP = 6,
+
153  ODR_MAP_LEN = 7,
+
155  ODR_PAR_INCOMPAT = 8,
+
157  ODR_DEV_INCOMPAT = 9,
+
159  ODR_HW = 10,
+
161  ODR_TYPE_MISMATCH = 11,
+
163  ODR_DATA_LONG = 12,
+
165  ODR_DATA_SHORT = 13,
+
167  ODR_SUB_NOT_EXIST = 14,
+
169  ODR_INVALID_VALUE = 15,
+
171  ODR_VALUE_HIGH = 16,
+
173  ODR_VALUE_LOW = 17,
+
175  ODR_MAX_LESS_MIN = 18,
+
177  ODR_NO_RESOURCE = 19,
+
179  ODR_GENERAL = 20,
+
181  ODR_DATA_TRANSF = 21,
+
183  ODR_DATA_LOC_CTRL = 22,
+
185  ODR_DATA_DEV_STATE = 23,
+
187  ODR_OD_MISSING = 23,
+
189  ODR_NO_DATA = 25,
+
191  ODR_COUNT = 26
+
192 } ODR_t;
+
193 
+
194 
+
201 typedef struct {
+
203  uint16_t index;
+
205  uint8_t subIndex;
+
209  uint8_t subEntriesCount;
+
211  OD_attr_t attribute;
+
232  OD_flagsPDO_t *flagsPDO;
+
233 } OD_subEntry_t;
+
234 
+
235 
+
240 typedef struct {
+
245  void *dataObjectOriginal;
+
249  void *object;
+
251  OD_size_t dataLength;
+
254  OD_size_t dataOffset;
+
255 } OD_stream_t;
+
256 
+
257 
+
263 typedef struct {
+
265  OD_stream_t stream;
+
296  OD_size_t (*read)(OD_stream_t *stream, uint8_t subIndex,
+
297  void *buf, OD_size_t count, ODR_t *returnCode);
+
324  OD_size_t (*write)(OD_stream_t *stream, uint8_t subIndex,
+
325  const void *buf, OD_size_t count, ODR_t *returnCode);
+
326 } OD_IO_t;
+
327 
+
328 
+
336 typedef struct {
+
338  uint16_t index;
+
340  uint8_t subEntriesCount;
+
342  uint8_t odObjectType;
+
345  const void *odObject;
+
346 } OD_entry_t;
+
347 
+
348 
+
352 typedef struct {
+
354  uint16_t size;
+
356  const OD_entry_t *list;
+
357 } OD_t;
+
358 
+
359 
+
369 OD_size_t OD_readOriginal(OD_stream_t *stream, uint8_t subIndex,
+
370  void *buf, OD_size_t count, ODR_t *returnCode);
+
371 
+
372 
+
382 OD_size_t OD_writeOriginal(OD_stream_t *stream, uint8_t subIndex,
+
383  const void *buf, OD_size_t count, ODR_t *returnCode);
+
384 
+
385 
+
394 const OD_entry_t *OD_find(const OD_t *od, uint16_t index);
+
395 
+
396 
+
410 ODR_t OD_getSub(const OD_entry_t *entry, uint8_t subIndex,
+
411  OD_subEntry_t *subEntry, OD_IO_t *io, bool_t odOrig);
+
412 
+
413 
+
421 static inline uint16_t OD_getIndex(const OD_entry_t *entry) {
+
422  return entry->index;
+
423 }
+
424 
+
425 
+
435 static inline void OD_rwRestart(OD_stream_t *stream) {
+
436  stream->dataOffset = 0;
+
437 }
+
438 
+
439 
+
447 uint32_t OD_getSDOabCode(ODR_t returnCode);
+
448 
+
449 
+
485 ODR_t OD_extensionIO_init(const OD_entry_t *entry,
+
486  void *object,
+
487  OD_size_t (*read)(OD_stream_t *stream,
+
488  uint8_t subIndex,
+
489  void *buf,
+
490  OD_size_t count,
+
491  ODR_t *returnCode),
+
492  OD_size_t (*write)(OD_stream_t *stream,
+
493  uint8_t subIndex,
+
494  const void *buf,
+
495  OD_size_t count,
+
496  ODR_t *returnCode));
+
497 
+
498 
+
519 ODR_t OD_get_i8(const OD_entry_t *entry, uint8_t subIndex,
+
520  int8_t *val, bool_t odOrig);
+
522 ODR_t OD_get_i16(const OD_entry_t *entry, uint8_t subIndex,
+
523  int16_t *val, bool_t odOrig);
+
525 ODR_t OD_get_i32(const OD_entry_t *entry, uint8_t subIndex,
+
526  int32_t *val, bool_t odOrig);
+
528 ODR_t OD_get_i64(const OD_entry_t *entry, uint8_t subIndex,
+
529  int64_t *val, bool_t odOrig);
+
531 ODR_t OD_get_u8(const OD_entry_t *entry, uint8_t subIndex,
+
532  uint8_t *val, bool_t odOrig);
+
534 ODR_t OD_get_u16(const OD_entry_t *entry, uint8_t subIndex,
+
535  uint16_t *val, bool_t odOrig);
+
537 ODR_t OD_get_u32(const OD_entry_t *entry, uint8_t subIndex,
+
538  uint32_t *val, bool_t odOrig);
+
540 ODR_t OD_get_u64(const OD_entry_t *entry, uint8_t subIndex,
+
541  uint64_t *val, bool_t odOrig);
+
543 ODR_t OD_get_r32(const OD_entry_t *entry, uint8_t subIndex,
+
544  float32_t *val, bool_t odOrig);
+
546 ODR_t OD_get_r64(const OD_entry_t *entry, uint8_t subIndex,
+
547  float64_t *val, bool_t odOrig);
+
548 
+
562 ODR_t OD_set_i8(const OD_entry_t *entry, uint8_t subIndex,
+
563  int8_t val, bool_t odOrig);
+
565 ODR_t OD_set_i16(const OD_entry_t *entry, uint8_t subIndex,
+
566  int16_t val, bool_t odOrig);
+
568 ODR_t OD_set_i32(const OD_entry_t *entry, uint8_t subIndex,
+
569  int32_t val, bool_t odOrig);
+
571 ODR_t OD_set_i64(const OD_entry_t *entry, uint8_t subIndex,
+
572  int64_t val, bool_t odOrig);
+
574 ODR_t OD_set_u8(const OD_entry_t *entry, uint8_t subIndex,
+
575  uint8_t val, bool_t odOrig);
+
577 ODR_t OD_set_u16(const OD_entry_t *entry, uint8_t subIndex,
+
578  uint16_t val, bool_t odOrig);
+
580 ODR_t OD_set_u32(const OD_entry_t *entry, uint8_t subIndex,
+
581  uint32_t val, bool_t odOrig);
+
583 ODR_t OD_set_u64(const OD_entry_t *entry, uint8_t subIndex,
+
584  uint64_t val, bool_t odOrig);
+
586 ODR_t OD_set_r32(const OD_entry_t *entry, uint8_t subIndex,
+
587  float32_t val, bool_t odOrig);
+
589 ODR_t OD_set_r64(const OD_entry_t *entry, uint8_t subIndex,
+
590  float64_t val, bool_t odOrig);
+
591 
+
607 ODR_t OD_getPtr_i8(const OD_entry_t *entry, uint8_t subIndex, int8_t **val);
+
609 ODR_t OD_getPtr_i16(const OD_entry_t *entry, uint8_t subIndex, int16_t **val);
+
611 ODR_t OD_getPtr_i32(const OD_entry_t *entry, uint8_t subIndex, int32_t **val);
+
613 ODR_t OD_getPtr_i64(const OD_entry_t *entry, uint8_t subIndex, int64_t **val);
+
615 ODR_t OD_getPtr_u8(const OD_entry_t *entry, uint8_t subIndex, uint8_t **val);
+
617 ODR_t OD_getPtr_u16(const OD_entry_t *entry, uint8_t subIndex, uint16_t **val);
+
619 ODR_t OD_getPtr_u32(const OD_entry_t *entry, uint8_t subIndex, uint32_t **val);
+
621 ODR_t OD_getPtr_u64(const OD_entry_t *entry, uint8_t subIndex, uint64_t **val);
+
623 ODR_t OD_getPtr_r32(const OD_entry_t *entry, uint8_t subIndex, float32_t **val);
+
625 ODR_t OD_getPtr_r64(const OD_entry_t *entry, uint8_t subIndex, float64_t **val);
+
643 ODR_t OD_getPtr_vs(const OD_entry_t *entry, uint8_t subIndex,
+
644  char **val, OD_size_t *dataLength);
+
646 ODR_t OD_getPtr_os(const OD_entry_t *entry, uint8_t subIndex,
+
647  uint8_t **val, OD_size_t *dataLength);
+
649 ODR_t OD_getPtr_us(const OD_entry_t *entry, uint8_t subIndex,
+
650  uint16_t **val, OD_size_t *dataLength); /* CO_ODgetSetters */
+
652 
+
653 
+
654 #if defined OD_DEFINITION || defined CO_DOXYGEN
+
655 
+
664 typedef enum {
+
669  ODT_VAR = 0x01,
+
675  ODT_ARR = 0x02,
+
682  ODT_REC = 0x03,
+
683 
+
686  ODT_EVAR = 0x11,
+
688  ODT_EARR = 0x12,
+
690  ODT_EREC = 0x13,
+
691 
+
693  ODT_TYPE_MASK = 0x0F,
+
695  ODT_EXTENSION_MASK = 0x10
+
696 } OD_objectTypes_t;
+
697 
+
701 typedef struct {
+
702  void *data;
+
703  OD_attr_t attribute;
+
704  OD_size_t dataLength;
+
705 } OD_obj_var_t;
+
706 
+
710 typedef struct {
+
711  uint8_t *data0;
+
712  void *data;
+
713  OD_attr_t attribute0;
+
715  OD_attr_t attribute;
+
716  OD_size_t dataElementLength;
+
717  OD_size_t dataElementSizeof;
+
718 } OD_obj_array_t;
+
719 
+
723 typedef struct {
+
724  void *data;
+
725  uint8_t subIndex;
+
726  OD_attr_t attribute;
+
727  OD_size_t dataLength;
+
728 } OD_obj_record_t;
+
729 
+
734 typedef struct {
+
736  void *object;
+
738  OD_size_t (*read)(OD_stream_t *stream, uint8_t subIndex,
+
739  void *buf, OD_size_t count, ODR_t *returnCode);
+
741  OD_size_t (*write)(OD_stream_t *stream, uint8_t subIndex,
+
742  const void *buf, OD_size_t count, ODR_t *returnCode);
+
743 } OD_extensionIO_t;
+
744 
+
749 typedef struct {
+
751  OD_extensionIO_t *extIO;
+
753  OD_flagsPDO_t *flagsPDO;
+
755  const void *odObjectOriginal;
+
756 } OD_obj_extended_t;
+
757  /* CO_ODdefinition */
+
759 
+
760 #endif /* defined OD_DEFINITION */
+
761  /* CO_ODinterface */
+
763 
+
764 #ifdef __cplusplus
+
765 }
+
766 #endif /*__cplusplus*/
+
767 
+
768 #endif /* CO_OD_INTERFACE_H */
+
+
+
OD_stream_t::dataLength
OD_size_t dataLength
Data length in bytes or 0, if length is not specified.
Definition: CO_ODinterface.h:251
+
uint32_t
unsigned long int uint32_t
UNSIGNED32 in CANopen (0007h), 32-bit unsigned integer.
Definition: CO_driver.h:155
+
OD_rwRestart
static void OD_rwRestart(OD_stream_t *stream)
Restart read or write operation on OD variable.
Definition: CO_ODinterface.h:435
+
CO_driver.h
Interface between CAN hardware and CANopenNode.
+
OD_get_i64
ODR_t OD_get_i64(const OD_entry_t *entry, uint8_t subIndex, int64_t *val, bool_t odOrig)
Get int64_t variable from Object Dictionary, see OD_get_i8.
Definition: CO_ODinterface.c:400
+
ODR_DATA_DEV_STATE
@ ODR_DATA_DEV_STATE
SDO abort 0x08000022 - Data can't be transf.
Definition: CO_ODinterface.h:185
+
OD_H13FE_SRDO_VALID
@ OD_H13FE_SRDO_VALID
SRDO Configuration valid.
Definition: CO_ODinterface.h:98
+
ODR_READONLY
@ ODR_READONLY
SDO abort 0x06010002 - Attempt to write a read only object.
Definition: CO_ODinterface.h:147
+
OD_entry_t::odObject
const void * odObject
OD object of type indicated by odObjectType, from which OD_getSub() fetches the information.
Definition: CO_ODinterface.h:345
+
ODA_TRPDO
@ ODA_TRPDO
Variable is mappable into TPDO or RPDO.
Definition: CO_ODinterface.h:116
+
OD_H100F_RSV
@ OD_H100F_RSV
Reserved.
Definition: CO_ODinterface.h:72
+
OD_get_r32
ODR_t OD_get_r32(const OD_entry_t *entry, uint8_t subIndex, float32_t *val, bool_t odOrig)
Get float32_t variable from Object Dictionary, see OD_get_i8.
Definition: CO_ODinterface.c:460
+
ODR_OUT_OF_MEM
@ ODR_OUT_OF_MEM
SDO abort 0x05040005 - Out of memory.
Definition: CO_ODinterface.h:141
+
ODT_EXTENSION_MASK
@ ODT_EXTENSION_MASK
Mask for extension.
Definition: CO_ODinterface.h:695
+
ODR_VALUE_HIGH
@ ODR_VALUE_HIGH
SDO abort 0x06090031 - Value range of parameter written too high.
Definition: CO_ODinterface.h:171
+
OD_getPtr_r64
ODR_t OD_getPtr_r64(const OD_entry_t *entry, uint8_t subIndex, float64_t **val)
Get pointer to float64_t variable from OD, see OD_getPtr_i8.
Definition: CO_ODinterface.c:723
+
ODR_VALUE_LOW
@ ODR_VALUE_LOW
SDO abort 0x06090032 - Value range of parameter written too low.
Definition: CO_ODinterface.h:173
+
OD_H1301_SRDO_1_PARAM
@ OD_H1301_SRDO_1_PARAM
SRDO communication parameter.
Definition: CO_ODinterface.h:96
+
OD_stream_t::object
void * object
Pointer to object, passed by OD_extensionIO_init().
Definition: CO_ODinterface.h:249
+
OD_IO_t::stream
OD_stream_t stream
Object Dictionary stream object, passed to read or write.
Definition: CO_ODinterface.h:265
+
OD_H1200_SDO_SERVER_1_PARAM
@ OD_H1200_SDO_SERVER_1_PARAM
SDO server parameter.
Definition: CO_ODinterface.h:93
+
OD_getPtr_i8
ODR_t OD_getPtr_i8(const OD_entry_t *entry, uint8_t subIndex, int8_t **val)
Get pointer to int8_t variable from Object Dictionary.
Definition: CO_ODinterface.c:606
+
OD_set_r32
ODR_t OD_set_r32(const OD_entry_t *entry, uint8_t subIndex, float32_t val, bool_t odOrig)
Set float32_t variable in Object Dictionary, see OD_set_i8.
Definition: CO_ODinterface.c:581
+
OD_subEntry_t::flagsPDO
OD_flagsPDO_t * flagsPDO
Pointer to PDO flags bit-field.
Definition: CO_ODinterface.h:232
+
ODR_PAR_INCOMPAT
@ ODR_PAR_INCOMPAT
SDO abort 0x06040043 - General parameter incompatibility reasons.
Definition: CO_ODinterface.h:155
+
OD_get_i8
ODR_t OD_get_i8(const OD_entry_t *entry, uint8_t subIndex, int8_t *val, bool_t odOrig)
Get int8_t variable from Object Dictionary.
Definition: CO_ODinterface.c:364
+
OD_subEntry_t::subEntriesCount
uint8_t subEntriesCount
Number of sub-entries in OD object.
Definition: CO_ODinterface.h:209
+
ODR_DEV_INCOMPAT
@ ODR_DEV_INCOMPAT
SDO abort 0x06040047 - General internal incompatibility in device.
Definition: CO_ODinterface.h:157
+
OD_H1017_PRODUCER_HB_TIME
@ OD_H1017_PRODUCER_HB_TIME
Producer heartbeat time.
Definition: CO_ODinterface.h:80
+
OD_H100B_RSV
@ OD_H100B_RSV
Reserved.
Definition: CO_ODinterface.h:68
+
ODT_TYPE_MASK
@ ODT_TYPE_MASK
Mask for basic type.
Definition: CO_ODinterface.h:693
+
OD_H1014_COBID_EMERGENCY
@ OD_H1014_COBID_EMERGENCY
Emergency message cob-id.
Definition: CO_ODinterface.h:77
+
OD_getPtr_u64
ODR_t OD_getPtr_u64(const OD_entry_t *entry, uint8_t subIndex, uint64_t **val)
Get pointer to uint64_t variable from OD, see OD_getPtr_i8.
Definition: CO_ODinterface.c:697
+
OD_set_i64
ODR_t OD_set_i64(const OD_entry_t *entry, uint8_t subIndex, int64_t val, bool_t odOrig)
Set int16_t variable in Object Dictionary, see OD_set_i8.
Definition: CO_ODinterface.c:521
+
OD_H1003_PREDEF_ERR_FIELD
@ OD_H1003_PREDEF_ERR_FIELD
Predefined error field.
Definition: CO_ODinterface.h:60
+
int64_t
signed long long int int64_t
INTEGER64 in CANopen (0015h), 64-bit signed integer.
Definition: CO_driver.h:149
+
uint16_t
unsigned int uint16_t
UNSIGNED16 in CANopen (0006h), 16-bit unsigned integer.
Definition: CO_driver.h:153
+
OD_getPtr_us
ODR_t OD_getPtr_us(const OD_entry_t *entry, uint8_t subIndex, uint16_t **val, OD_size_t *dataLength)
Get pointer to "unicode string" variable from OD, see OD_getPtr_vs.
Definition: CO_ODinterface.c:772
+
ODA_TRSRDO
@ ODA_TRSRDO
Variable is mappable into tx or rx SRDO.
Definition: CO_ODinterface.h:119
+
ODR_NO_MAP
@ ODR_NO_MAP
SDO abort 0x06040041 - Object cannot be mapped to the PDO.
Definition: CO_ODinterface.h:151
+
ODA_RSRDO
@ ODA_RSRDO
Variable is mappable into receiving SRDO.
Definition: CO_ODinterface.h:118
+
ODR_NO_RESOURCE
@ ODR_NO_RESOURCE
SDO abort 0x060A0023 - Resource not available: SDO connection.
Definition: CO_ODinterface.h:177
+
OD_extensionIO_t
Object pointed by OD_obj_extended_t contains application specified parameters for extended OD object.
Definition: CO_ODinterface.h:734
+
OD_subEntry_t::attribute
OD_attr_t attribute
Attribute bit-field of the OD sub-object, see OD_attributes_t.
Definition: CO_ODinterface.h:211
+
OD_H1015_INHIBIT_TIME_EMCY
@ OD_H1015_INHIBIT_TIME_EMCY
Inhibit time emergency message.
Definition: CO_ODinterface.h:78
+
OD_getPtr_u16
ODR_t OD_getPtr_u16(const OD_entry_t *entry, uint8_t subIndex, uint16_t **val)
Get pointer to uint16_t variable from OD, see OD_getPtr_i8.
Definition: CO_ODinterface.c:671
+
OD_t::list
const OD_entry_t * list
List OD entries (table of contents), ordered by index.
Definition: CO_ODinterface.h:356
+
OD_H1002_MANUF_STATUS_REG
@ OD_H1002_MANUF_STATUS_REG
Manufacturer status register.
Definition: CO_ODinterface.h:59
+
OD_obj_array_t::dataElementSizeof
OD_size_t dataElementSizeof
Sizeof one array element in bytes.
Definition: CO_ODinterface.h:717
+
OD_H1020_VERIFY_CONFIG
@ OD_H1020_VERIFY_CONFIG
Verify configuration.
Definition: CO_ODinterface.h:83
+
OD_H1280_SDO_CLIENT_1_PARAM
@ OD_H1280_SDO_CLIENT_1_PARAM
SDO client parameter.
Definition: CO_ODinterface.h:94
+
bool_t
unsigned char bool_t
Boolean data type for general use.
Definition: CO_driver.h:141
+
OD_getPtr_i64
ODR_t OD_getPtr_i64(const OD_entry_t *entry, uint8_t subIndex, int64_t **val)
Get pointer to int64_t variable from OD, see OD_getPtr_i8.
Definition: CO_ODinterface.c:645
+
OD_getPtr_os
ODR_t OD_getPtr_os(const OD_entry_t *entry, uint8_t subIndex, uint8_t **val, OD_size_t *dataLength)
Get pointer to "octet string" variable from OD, see OD_getPtr_vs.
Definition: CO_ODinterface.c:754
+
ODT_REC
@ ODT_REC
This type corresponds to CANopen Object Dictionary object with object code equal to RECORD.
Definition: CO_ODinterface.h:682
+
ODT_VAR
@ ODT_VAR
This type corresponds to CANopen Object Dictionary object with object code equal to VAR.
Definition: CO_ODinterface.h:669
+
OD_set_u64
ODR_t OD_set_u64(const OD_entry_t *entry, uint8_t subIndex, uint64_t val, bool_t odOrig)
Set uint64_t variable in Object Dictionary, see OD_set_i8.
Definition: CO_ODinterface.c:569
+
int32_t
signed long int int32_t
INTEGER32 in CANopen (0004h), 32-bit signed integer.
Definition: CO_driver.h:147
+
OD_stream_t::dataObjectOriginal
void * dataObjectOriginal
Pointer to original data object, defined by Object Dictionary.
Definition: CO_ODinterface.h:245
+
uint64_t
unsigned long long int uint64_t
UNSIGNED64 in CANopen (001Bh), 64-bit unsigned integer.
Definition: CO_driver.h:157
+
OD_obj_array_t::dataElementLength
OD_size_t dataElementLength
Data length of array elements in bytes.
Definition: CO_ODinterface.h:716
+
ODR_INVALID_VALUE
@ ODR_INVALID_VALUE
SDO abort 0x06090030 - Invalid value for parameter (download only)
Definition: CO_ODinterface.h:169
+
ODA_SDO_W
@ ODA_SDO_W
SDO server may write to the variable.
Definition: CO_ODinterface.h:112
+
OD_obj_record_t::data
void * data
Pointer to data.
Definition: CO_ODinterface.h:724
+
OD_H100A_MANUF_SW_VERSION
@ OD_H100A_MANUF_SW_VERSION
Manufacturer software version.
Definition: CO_ODinterface.h:67
+
OD_H1005_COBID_SYNC
@ OD_H1005_COBID_SYNC
Sync message cob-id.
Definition: CO_ODinterface.h:62
+
OD_H1000_DEV_TYPE
@ OD_H1000_DEV_TYPE
Device type.
Definition: CO_ODinterface.h:57
+
ODT_EVAR
@ ODT_EVAR
Same as ODT_VAR, but extended with OD_obj_extended_t type.
Definition: CO_ODinterface.h:686
+
OD_set_u8
ODR_t OD_set_u8(const OD_entry_t *entry, uint8_t subIndex, uint8_t val, bool_t odOrig)
Set uint8_t variable in Object Dictionary, see OD_set_i8.
Definition: CO_ODinterface.c:533
+
int8_t
signed char int8_t
INTEGER8 in CANopen (0002h), 8-bit signed integer.
Definition: CO_driver.h:143
+
OD_stream_t::dataOffset
OD_size_t dataOffset
In case of large data, dataOffset indicates position of already transferred data.
Definition: CO_ODinterface.h:254
+
ODR_IDX_NOT_EXIST
@ ODR_IDX_NOT_EXIST
SDO abort 0x06020000 - Object does not exist in the object dict.
Definition: CO_ODinterface.h:149
+
ODA_MB
@ ODA_MB
Variable is multi-byte ((u)int16_t to (u)int64_t)
Definition: CO_ODinterface.h:120
+
OD_extensionIO_init
ODR_t OD_extensionIO_init(const OD_entry_t *entry, void *object, OD_size_t(*read)(OD_stream_t *stream, uint8_t subIndex, void *buf, OD_size_t count, ODR_t *returnCode), OD_size_t(*write)(OD_stream_t *stream, uint8_t subIndex, const void *buf, OD_size_t count, ODR_t *returnCode))
Initialise extended OD object with own read/write functions.
Definition: CO_ODinterface.c:332
+
OD_ObjDicId_30x_t
OD_ObjDicId_30x_t
Common DS301 object dictionary entries.
Definition: CO_ODinterface.h:56
+
OD_getPtr_i32
ODR_t OD_getPtr_i32(const OD_entry_t *entry, uint8_t subIndex, int32_t **val)
Get pointer to int32_t variable from OD, see OD_getPtr_i8.
Definition: CO_ODinterface.c:632
+
ODR_GENERAL
@ ODR_GENERAL
SDO abort 0x08000000 - General error.
Definition: CO_ODinterface.h:179
+
OD_H1026_OS_PROMPT
@ OD_H1026_OS_PROMPT
OS prompt.
Definition: CO_ODinterface.h:89
+
OD_find
const OD_entry_t * OD_find(const OD_t *od, uint16_t index)
Find OD entry in Object Dictionary.
Definition: CO_ODinterface.c:160
+
OD_set_i32
ODR_t OD_set_i32(const OD_entry_t *entry, uint8_t subIndex, int32_t val, bool_t odOrig)
Set int16_t variable in Object Dictionary, see OD_set_i8.
Definition: CO_ODinterface.c:509
+
OD_H1010_STORE_PARAM_FUNC
@ OD_H1010_STORE_PARAM_FUNC
Store params in persistent mem.
Definition: CO_ODinterface.h:73
+
OD_get_u16
ODR_t OD_get_u16(const OD_entry_t *entry, uint8_t subIndex, uint16_t *val, bool_t odOrig)
Get uint16_t variable from Object Dictionary, see OD_get_i8.
Definition: CO_ODinterface.c:424
+
OD_obj_record_t::attribute
OD_attr_t attribute
Attribute bitfield, see OD_attributes_t.
Definition: CO_ODinterface.h:726
+
ODR_PARTIAL
@ ODR_PARTIAL
Read/write is only partial, make more calls.
Definition: CO_ODinterface.h:137
+
OD_obj_var_t
Object for single OD variable, used for "VAR" type OD objects.
Definition: CO_ODinterface.h:701
+
float64_t
double float64_t
REAL64 in CANopen (0011h), double precision floating point value, 64-bit.
Definition: CO_driver.h:161
+
ODR_MAX_LESS_MIN
@ ODR_MAX_LESS_MIN
SDO abort 0x06090036 - Maximum value is less than minimum value.
Definition: CO_ODinterface.h:175
+
OD_getSDOabCode
uint32_t OD_getSDOabCode(ODR_t returnCode)
Get SDO abort code from returnCode.
Definition: CO_ODinterface.c:297
+
OD_entry_t::index
uint16_t index
Object Dictionary index.
Definition: CO_ODinterface.h:338
+
OD_H1007_SYNC_WINDOW_LEN
@ OD_H1007_SYNC_WINDOW_LEN
Sync windows length.
Definition: CO_ODinterface.h:64
+
OD_getIndex
static uint16_t OD_getIndex(const OD_entry_t *entry)
Return index from OD entry.
Definition: CO_ODinterface.h:421
+
OD_get_i32
ODR_t OD_get_i32(const OD_entry_t *entry, uint8_t subIndex, int32_t *val, bool_t odOrig)
Get int32_t variable from Object Dictionary, see OD_get_i8.
Definition: CO_ODinterface.c:388
+
float32_t
float float32_t
REAL32 in CANopen (0008h), single precision floating point value, 32-bit.
Definition: CO_driver.h:159
+
OD_H1600_RXPDO_1_MAPPING
@ OD_H1600_RXPDO_1_MAPPING
RXPDO mapping parameters.
Definition: CO_ODinterface.h:101
+
ODT_EREC
@ ODT_EREC
Same as ODT_REC, but extended with OD_obj_extended_t type.
Definition: CO_ODinterface.h:690
+
ODR_OD_MISSING
@ ODR_OD_MISSING
SDO abort 0x08000023 - Object dictionary not present.
Definition: CO_ODinterface.h:187
+
OD_getPtr_vs
ODR_t OD_getPtr_vs(const OD_entry_t *entry, uint8_t subIndex, char **val, OD_size_t *dataLength)
Get pointer to "visible string" variable from Object Dictionary.
Definition: CO_ODinterface.c:736
+
OD_get_u64
ODR_t OD_get_u64(const OD_entry_t *entry, uint8_t subIndex, uint64_t *val, bool_t odOrig)
Get uint64_t variable from Object Dictionary, see OD_get_i8.
Definition: CO_ODinterface.c:448
+
ODR_DATA_LONG
@ ODR_DATA_LONG
SDO abort 0x06070012 - Data type does not match, length too high.
Definition: CO_ODinterface.h:163
+
OD_objectTypes_t
OD_objectTypes_t
Types for OD object.
Definition: CO_ODinterface.h:664
+
ODR_SUB_NOT_EXIST
@ ODR_SUB_NOT_EXIST
SDO abort 0x06090011 - Sub index does not exist.
Definition: CO_ODinterface.h:167
+
OD_H1016_CONSUMER_HB_TIME
@ OD_H1016_CONSUMER_HB_TIME
Consumer heartbeat time.
Definition: CO_ODinterface.h:79
+
OD_writeOriginal
OD_size_t OD_writeOriginal(OD_stream_t *stream, uint8_t subIndex, const void *buf, OD_size_t count, ODR_t *returnCode)
Write value to original OD location.
Definition: CO_ODinterface.c:80
+
OD_entry_t::subEntriesCount
uint8_t subEntriesCount
Maximum sub-index in the OD object.
Definition: CO_ODinterface.h:340
+
OD_extensionIO_t::object
void * object
Object on which read and write will operate.
Definition: CO_ODinterface.h:736
+
OD_H1009_MANUF_HW_VERSION
@ OD_H1009_MANUF_HW_VERSION
Manufacturer hardware version.
Definition: CO_ODinterface.h:66
+
OD_H1011_REST_PARAM_FUNC
@ OD_H1011_REST_PARAM_FUNC
Restore default parameters.
Definition: CO_ODinterface.h:74
+
OD_H1381_SRDO_1_MAPPING
@ OD_H1381_SRDO_1_MAPPING
SRDO mapping parameter.
Definition: CO_ODinterface.h:97
+
OD_H1001_ERR_REG
@ OD_H1001_ERR_REG
Error register.
Definition: CO_ODinterface.h:58
+
ODR_UNSUPP_ACCESS
@ ODR_UNSUPP_ACCESS
SDO abort 0x06010000 - Unsupported access to an object.
Definition: CO_ODinterface.h:143
+
OD_getPtr_i16
ODR_t OD_getPtr_i16(const OD_entry_t *entry, uint8_t subIndex, int16_t **val)
Get pointer to int16_t variable from OD, see OD_getPtr_i8.
Definition: CO_ODinterface.c:619
+
OD_H1008_MANUF_DEV_NAME
@ OD_H1008_MANUF_DEV_NAME
Manufacturer device name.
Definition: CO_ODinterface.h:65
+
OD_get_u32
ODR_t OD_get_u32(const OD_entry_t *entry, uint8_t subIndex, uint32_t *val, bool_t odOrig)
Get uint32_t variable from Object Dictionary, see OD_get_i8.
Definition: CO_ODinterface.c:436
+
OD_subEntry_t::subIndex
uint8_t subIndex
Object Dictionary sub-index.
Definition: CO_ODinterface.h:205
+
OD_entry_t::odObjectType
uint8_t odObjectType
Type of the odObject, indicated by OD_objectTypes_t enumerator.
Definition: CO_ODinterface.h:342
+
OD_H100E_RSV
@ OD_H100E_RSV
Reserved.
Definition: CO_ODinterface.h:71
+
OD_obj_array_t::data0
uint8_t * data0
Pointer to data for sub-index 0.
Definition: CO_ODinterface.h:711
+
OD_H1400_RXPDO_1_PARAM
@ OD_H1400_RXPDO_1_PARAM
RXPDO communication parameter.
Definition: CO_ODinterface.h:100
+
ODR_DATA_TRANSF
@ ODR_DATA_TRANSF
SDO abort 0x08000020 - Data cannot be transferred or stored to app.
Definition: CO_ODinterface.h:181
+
OD_t::size
uint16_t size
Number of elements in the list, without last element, which is blank.
Definition: CO_ODinterface.h:354
+
OD_H1029_ERR_BEHAVIOR
@ OD_H1029_ERR_BEHAVIOR
Error behaviour.
Definition: CO_ODinterface.h:92
+
OD_getSub
ODR_t OD_getSub(const OD_entry_t *entry, uint8_t subIndex, OD_subEntry_t *subEntry, OD_IO_t *io, bool_t odOrig)
Find sub-object with specified sub-index on OD entry returned by OD_find.
Definition: CO_ODinterface.c:200
+
OD_H100D_LIFETIME_FACTOR
@ OD_H100D_LIFETIME_FACTOR
Life time factor.
Definition: CO_ODinterface.h:70
+
ODT_EARR
@ ODT_EARR
Same as ODT_ARR, but extended with OD_obj_extended_t type.
Definition: CO_ODinterface.h:688
+
OD_H1018_IDENTITY_OBJECT
@ OD_H1018_IDENTITY_OBJECT
Identity object.
Definition: CO_ODinterface.h:81
+
OD_flagsPDO_t
#define OD_flagsPDO_t
Type of flagsPDO variable from OD_subEntry_t.
Definition: CO_ODinterface.h:46
+
ODT_ARR
@ ODT_ARR
This type corresponds to CANopen Object Dictionary object with object code equal to ARRAY.
Definition: CO_ODinterface.h:675
+
OD_obj_record_t::dataLength
OD_size_t dataLength
Data length in bytes.
Definition: CO_ODinterface.h:727
+
OD_H1006_COMM_CYCL_PERIOD
@ OD_H1006_COMM_CYCL_PERIOD
Communication cycle period.
Definition: CO_ODinterface.h:63
+
OD_H13FF_SRDO_CHECKSUM
@ OD_H13FF_SRDO_CHECKSUM
SRDO configuration checksum.
Definition: CO_ODinterface.h:99
+
ODA_SDO_R
@ ODA_SDO_R
SDO server may read from the variable.
Definition: CO_ODinterface.h:111
+
ODR_MAP_LEN
@ ODR_MAP_LEN
SDO abort 0x06040042 - PDO length exceeded.
Definition: CO_ODinterface.h:153
+
OD_set_i16
ODR_t OD_set_i16(const OD_entry_t *entry, uint8_t subIndex, int16_t val, bool_t odOrig)
Set int16_t variable in Object Dictionary, see OD_set_i8.
Definition: CO_ODinterface.c:497
+
OD_H1013_HIGH_RES_TIMESTAMP
@ OD_H1013_HIGH_RES_TIMESTAMP
High resolution timestamp.
Definition: CO_ODinterface.h:76
+
OD_get_u8
ODR_t OD_get_u8(const OD_entry_t *entry, uint8_t subIndex, uint8_t *val, bool_t odOrig)
Get uint8_t variable from Object Dictionary, see OD_get_i8.
Definition: CO_ODinterface.c:412
+
OD_H1019_SYNC_CNT_OVERFLOW
@ OD_H1019_SYNC_CNT_OVERFLOW
Sync counter overflow value.
Definition: CO_ODinterface.h:82
+
OD_subEntry_t
Structure describing properties of a variable, located in specific index and sub-index inside the Obj...
Definition: CO_ODinterface.h:201
+
OD_H1023_OS_CMD
@ OD_H1023_OS_CMD
OS command.
Definition: CO_ODinterface.h:86
+
ODR_TYPE_MISMATCH
@ ODR_TYPE_MISMATCH
SDO abort 0x06070010 - Data type does not match.
Definition: CO_ODinterface.h:161
+
OD_obj_record_t::subIndex
uint8_t subIndex
Sub index of element.
Definition: CO_ODinterface.h:725
+
OD_obj_array_t
Object for OD array of variables, used for "ARRAY" type OD objects.
Definition: CO_ODinterface.h:710
+
OD_H1025_OS_DBG_INTERFACE
@ OD_H1025_OS_DBG_INTERFACE
OS debug interface.
Definition: CO_ODinterface.h:88
+
ODR_HW
@ ODR_HW
SDO abort 0x06060000 - Access failed due to hardware error.
Definition: CO_ODinterface.h:159
+
OD_H1800_TXPDO_1_PARAM
@ OD_H1800_TXPDO_1_PARAM
TXPDO communication parameter.
Definition: CO_ODinterface.h:102
+
OD_obj_extended_t
Object for extended type of OD variable, configurable by OD_extensionIO_init() function.
Definition: CO_ODinterface.h:749
+
int16_t
signed int int16_t
INTEGER16 in CANopen (0003h), 16-bit signed integer.
Definition: CO_driver.h:145
+
OD_stream_t
IO stream structure, used for read/write access to OD variable, part of OD_IO_t.
Definition: CO_ODinterface.h:240
+
ODA_TSRDO
@ ODA_TSRDO
Variable is mappable into transmitting SRDO.
Definition: CO_ODinterface.h:117
+
OD_H1300_GFC_PARAM
@ OD_H1300_GFC_PARAM
Global fail-safe command param.
Definition: CO_ODinterface.h:95
+
OD_get_r64
ODR_t OD_get_r64(const OD_entry_t *entry, uint8_t subIndex, float64_t *val, bool_t odOrig)
Get float64_t variable from Object Dictionary, see OD_get_i8.
Definition: CO_ODinterface.c:472
+
ODR_OK
@ ODR_OK
SDO abort 0x00000000 - Read/write successfully finished.
Definition: CO_ODinterface.h:139
+
ODR_t
ODR_t
Return codes from OD access functions.
Definition: CO_ODinterface.h:132
+
OD_t
Object Dictionary.
Definition: CO_ODinterface.h:352
+
OD_obj_var_t::dataLength
OD_size_t dataLength
Data length in bytes.
Definition: CO_ODinterface.h:704
+
ODA_TPDO
@ ODA_TPDO
Variable is mappable into TPDO (can be read)
Definition: CO_ODinterface.h:114
+
OD_attributes_t
OD_attributes_t
Attributes (bit masks) for OD sub-object.
Definition: CO_ODinterface.h:110
+
ODR_WRITEONLY
@ ODR_WRITEONLY
SDO abort 0x06010001 - Attempt to read a write only object.
Definition: CO_ODinterface.h:145
+
OD_getPtr_u8
ODR_t OD_getPtr_u8(const OD_entry_t *entry, uint8_t subIndex, uint8_t **val)
Get pointer to uint8_t variable from OD, see OD_getPtr_i8.
Definition: CO_ODinterface.c:658
+
OD_set_u16
ODR_t OD_set_u16(const OD_entry_t *entry, uint8_t subIndex, uint16_t val, bool_t odOrig)
Set uint16_t variable in Object Dictionary, see OD_set_i8.
Definition: CO_ODinterface.c:545
+
OD_H1A00_TXPDO_1_MAPPING
@ OD_H1A00_TXPDO_1_MAPPING
TXPDO mapping parameters.
Definition: CO_ODinterface.h:103
+
OD_set_u32
ODR_t OD_set_u32(const OD_entry_t *entry, uint8_t subIndex, uint32_t val, bool_t odOrig)
Set uint32_t variable in Object Dictionary, see OD_set_i8.
Definition: CO_ODinterface.c:557
+
ODA_STR
@ ODA_STR
Shorter value, than specified variable size, may be written to the variable.
Definition: CO_ODinterface.h:121
+
ODA_SDO_RW
@ ODA_SDO_RW
SDO server may read from or write to the variable.
Definition: CO_ODinterface.h:113
+
OD_IO_t
Structure for input / output on the OD variable.
Definition: CO_ODinterface.h:263
+
OD_H1028_EMCY_CONSUMER
@ OD_H1028_EMCY_CONSUMER
Emergency consumer object.
Definition: CO_ODinterface.h:91
+
OD_obj_record_t
Object for OD sub-elements, used in "RECORD" type OD objects.
Definition: CO_ODinterface.h:723
+
OD_set_i8
ODR_t OD_set_i8(const OD_entry_t *entry, uint8_t subIndex, int8_t val, bool_t odOrig)
Set int8_t variable in Object Dictionary.
Definition: CO_ODinterface.c:485
+
OD_H1012_COBID_TIME
@ OD_H1012_COBID_TIME
Timestamp message cob-id.
Definition: CO_ODinterface.h:75
+
OD_obj_extended_t::extIO
OD_extensionIO_t * extIO
Pointer to application specified IO extension, may be NULL.
Definition: CO_ODinterface.h:751
+
ODR_COUNT
@ ODR_COUNT
Last element, number of responses.
Definition: CO_ODinterface.h:191
+
OD_obj_array_t::attribute
OD_attr_t attribute
Attribute bitfield for array elements.
Definition: CO_ODinterface.h:715
+
OD_entry_t
Object Dictionary entry for one OD object.
Definition: CO_ODinterface.h:336
+
OD_subEntry_t::index
uint16_t index
Object Dictionary index.
Definition: CO_ODinterface.h:203
+
OD_H1024_OS_CMD_MODE
@ OD_H1024_OS_CMD_MODE
OS command mode.
Definition: CO_ODinterface.h:87
+
OD_H1027_MODULE_LIST
@ OD_H1027_MODULE_LIST
Module list.
Definition: CO_ODinterface.h:90
+
OD_obj_extended_t::flagsPDO
OD_flagsPDO_t * flagsPDO
Pointer to PDO flags bit-field, see OD_subEntry_t, may be NULL.
Definition: CO_ODinterface.h:753
+
ODR_DATA_LOC_CTRL
@ ODR_DATA_LOC_CTRL
SDO abort 0x08000021 - Data can't be transferred (local control)
Definition: CO_ODinterface.h:183
+
OD_get_i16
ODR_t OD_get_i16(const OD_entry_t *entry, uint8_t subIndex, int16_t *val, bool_t odOrig)
Get int16_t variable from Object Dictionary, see OD_get_i8.
Definition: CO_ODinterface.c:376
+
OD_getPtr_u32
ODR_t OD_getPtr_u32(const OD_entry_t *entry, uint8_t subIndex, uint32_t **val)
Get pointer to uint32_t variable from OD, see OD_getPtr_i8.
Definition: CO_ODinterface.c:684
+
OD_obj_array_t::data
void * data
Pointer to array of data.
Definition: CO_ODinterface.h:712
+
OD_obj_extended_t::odObjectOriginal
const void * odObjectOriginal
Pointer to original odObject, see OD_entry_t.
Definition: CO_ODinterface.h:755
+
OD_getPtr_r32
ODR_t OD_getPtr_r32(const OD_entry_t *entry, uint8_t subIndex, float32_t **val)
Get pointer to float32_t variable from OD, see OD_getPtr_i8.
Definition: CO_ODinterface.c:710
+
ODA_RPDO
@ ODA_RPDO
Variable is mappable into RPDO (can be written)
Definition: CO_ODinterface.h:115
+
OD_obj_var_t::attribute
OD_attr_t attribute
Attribute bitfield, see OD_attributes_t.
Definition: CO_ODinterface.h:703
+
OD_H1022_STORE_FORMAT
@ OD_H1022_STORE_FORMAT
Store format.
Definition: CO_ODinterface.h:85
+
OD_H1004_RSV
@ OD_H1004_RSV
Reserved.
Definition: CO_ODinterface.h:61
+
ODR_NO_DATA
@ ODR_NO_DATA
SDO abort 0x08000024 - No data available.
Definition: CO_ODinterface.h:189
+
OD_readOriginal
OD_size_t OD_readOriginal(OD_stream_t *stream, uint8_t subIndex, void *buf, OD_size_t count, ODR_t *returnCode)
Read value from original OD location.
Definition: CO_ODinterface.c:31
+
OD_obj_array_t::attribute0
OD_attr_t attribute0
Attribute bitfield for sub-index 0, see OD_attributes_t.
Definition: CO_ODinterface.h:713
+
ODR_DATA_SHORT
@ ODR_DATA_SHORT
SDO abort 0x06070013 - Data type does not match, length too short.
Definition: CO_ODinterface.h:165
+
OD_H1021_STORE_EDS
@ OD_H1021_STORE_EDS
Store EDS.
Definition: CO_ODinterface.h:84
+
uint8_t
unsigned char uint8_t
UNSIGNED8 in CANopen (0005h), 8-bit unsigned integer.
Definition: CO_driver.h:151
+
OD_size_t
#define OD_size_t
Variable of type OD_size_t contains data length in bytes of OD variable.
Definition: CO_ODinterface.h:44
+
OD_set_r64
ODR_t OD_set_r64(const OD_entry_t *entry, uint8_t subIndex, float64_t val, bool_t odOrig)
Set float64_t variable in Object Dictionary, see OD_set_i8.
Definition: CO_ODinterface.c:593
+
OD_H100C_GUARD_TIME
@ OD_H100C_GUARD_TIME
Guard time.
Definition: CO_ODinterface.h:69
+
OD_attr_t
#define OD_attr_t
Size of Object Dictionary attribute.
Definition: CO_ODinterface.h:50
+
OD_obj_var_t::data
void * data
Pointer to data.
Definition: CO_ODinterface.h:702
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/CO__PDO_8h.html b/Devices/Libraries/Systems/CANopenSocket/docs/CO__PDO_8h.html new file mode 100755 index 0000000..18d4fe2 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/CO__PDO_8h.html @@ -0,0 +1,176 @@ + + + + + + + +CANopenNode: 301/CO_PDO.h File Reference + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Data Structures | +Functions
+
+
CO_PDO.h File Reference
CANopen_301 » PDO
+
+
+ +

CANopen Process Data Object protocol. +More...

+
#include "301/CO_driver.h"
+#include "301/CO_SDOserver.h"
+#include "301/CO_Emergency.h"
+#include "301/CO_NMT_Heartbeat.h"
+#include "301/CO_SYNC.h"
+
+

Go to the source code of this file.

+ + + + + + + + + + + + + + + + + + + + +

+Data Structures

struct  CO_RPDOCommPar_t
 RPDO communication parameter. More...
 
struct  CO_RPDOMapPar_t
 RPDO mapping parameter. More...
 
struct  CO_TPDOCommPar_t
 TPDO communication parameter. More...
 
struct  CO_TPDOMapPar_t
 TPDO mapping parameter. More...
 
struct  CO_RPDO_t
 RPDO object. More...
 
struct  CO_TPDO_t
 TPDO object. More...
 
+ + + + + + + + + + + + + + + + + + + + + + +

+Functions

CO_ReturnError_t CO_RPDO_init (CO_RPDO_t *RPDO, CO_EM_t *em, CO_SDO_t *SDO, CO_SYNC_t *SYNC, CO_NMT_internalState_t *operatingState, uint8_t nodeId, uint16_t defaultCOB_ID, uint8_t restrictionFlags, const CO_RPDOCommPar_t *RPDOCommPar, const CO_RPDOMapPar_t *RPDOMapPar, uint16_t idx_RPDOCommPar, uint16_t idx_RPDOMapPar, CO_CANmodule_t *CANdevRx, uint16_t CANdevRxIdx)
 Initialize RPDO object. More...
 
void CO_RPDO_initCallbackPre (CO_RPDO_t *RPDO, void *object, void(*pFunctSignalPre)(void *object))
 Initialize RPDO callback function. More...
 
CO_ReturnError_t CO_TPDO_init (CO_TPDO_t *TPDO, CO_EM_t *em, CO_SDO_t *SDO, CO_SYNC_t *SYNC, CO_NMT_internalState_t *operatingState, uint8_t nodeId, uint16_t defaultCOB_ID, uint8_t restrictionFlags, const CO_TPDOCommPar_t *TPDOCommPar, const CO_TPDOMapPar_t *TPDOMapPar, uint16_t idx_TPDOCommPar, uint16_t idx_TPDOMapPar, CO_CANmodule_t *CANdevTx, uint16_t CANdevTxIdx)
 Initialize TPDO object. More...
 
uint8_t CO_TPDOisCOS (CO_TPDO_t *TPDO)
 Verify Change of State of the PDO. More...
 
CO_ReturnError_t CO_TPDOsend (CO_TPDO_t *TPDO)
 Send TPDO message. More...
 
void CO_RPDO_process (CO_RPDO_t *RPDO, bool_t syncWas)
 Process received PDO messages. More...
 
void CO_TPDO_process (CO_TPDO_t *TPDO, bool_t syncWas, uint32_t timeDifference_us, uint32_t *timerNext_us)
 Process transmitting PDO messages. More...
 
+

Detailed Description

+

CANopen Process Data Object protocol.

+
Author
Janez Paternoster
+
Copyright
2004 - 2020 Janez Paternoster
+

This file is part of CANopenNode, an opensource CANopen Stack. Project home page is https://github.com/CANopenNode/CANopenNode. For more information on CANopen see http://www.can-cia.org/.

+

Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. You may obtain a copy of the License at 

http://www.apache.org/licenses/LICENSE-2.0
+

Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License.

+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/CO__PDO_8h.js b/Devices/Libraries/Systems/CANopenSocket/docs/CO__PDO_8h.js new file mode 100755 index 0000000..1028ad4 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/CO__PDO_8h.js @@ -0,0 +1,10 @@ +var CO__PDO_8h = +[ + [ "CO_RPDO_init", "group__CO__PDO.html#ga92c484ada2ad240c1b8c891c88d56901", null ], + [ "CO_RPDO_initCallbackPre", "group__CO__PDO.html#ga34532746ccf88ccfa835716e89369478", null ], + [ "CO_TPDO_init", "group__CO__PDO.html#ga8fb100744dc91f84b236c55ee37200a1", null ], + [ "CO_TPDOisCOS", "group__CO__PDO.html#gafec3eb12b93146a3706cbf03d3770a8d", null ], + [ "CO_TPDOsend", "group__CO__PDO.html#ga9b2c8692f74f6a6a389ef88bf9c682a5", null ], + [ "CO_RPDO_process", "group__CO__PDO.html#gad77bfd4c7f64e75e7ddee5c926477e66", null ], + [ "CO_TPDO_process", "group__CO__PDO.html#ga0bb0d1b09d37ca19e01d47d8d0004f6b", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/CO__PDO_8h_source.html b/Devices/Libraries/Systems/CANopenSocket/docs/CO__PDO_8h_source.html new file mode 100755 index 0000000..5b52bf1 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/CO__PDO_8h_source.html @@ -0,0 +1,396 @@ + + + + + + + +CANopenNode: 301/CO_PDO.h Source File + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+
CO_PDO.h
+
+
+Go to the documentation of this file.
1 
+
26 #ifndef CO_PDO_H
+
27 #define CO_PDO_H
+
28 
+
29 #include "301/CO_driver.h"
+
30 #include "301/CO_SDOserver.h"
+
31 #include "301/CO_Emergency.h"
+
32 #include "301/CO_NMT_Heartbeat.h"
+
33 #include "301/CO_SYNC.h"
+
34 
+
35 /* default configuration, see CO_config.h */
+
36 #ifndef CO_CONFIG_PDO
+
37 #define CO_CONFIG_PDO (CO_CONFIG_RPDO_ENABLE | \
+
38  CO_CONFIG_TPDO_ENABLE | \
+
39  CO_CONFIG_PDO_SYNC_ENABLE)
+
40 #endif
+
41 
+
42 #if ((CO_CONFIG_PDO) & (CO_CONFIG_RPDO_ENABLE | CO_CONFIG_TPDO_ENABLE)) || defined CO_DOXYGEN
+
43 
+
44 #ifdef __cplusplus
+
45 extern "C" {
+
46 #endif
+
47 
+
80 typedef struct{
+
81  uint8_t maxSubIndex;
+
87  uint32_t COB_IDUsedByRPDO;
+
93  uint8_t transmissionType;
+
94 }CO_RPDOCommPar_t;
+
95 
+
96 
+
100 typedef struct{
+
103  uint8_t numberOfMappedObjects;
+
108  uint32_t mappedObject1;
+
109  uint32_t mappedObject2;
+
110  uint32_t mappedObject3;
+
111  uint32_t mappedObject4;
+
112  uint32_t mappedObject5;
+
113  uint32_t mappedObject6;
+
114  uint32_t mappedObject7;
+
115  uint32_t mappedObject8;
+
116 }CO_RPDOMapPar_t;
+
117 
+
118 
+
122 typedef struct{
+
123  uint8_t maxSubIndex;
+
129  uint32_t COB_IDUsedByTPDO;
+
137  uint8_t transmissionType;
+
140  uint16_t inhibitTime;
+
142  uint8_t compatibilityEntry;
+
145  uint16_t eventTimer;
+
150  uint8_t SYNCStartValue;
+
151 }CO_TPDOCommPar_t;
+
152 
+
153 
+
157 typedef struct{
+
160  uint8_t numberOfMappedObjects;
+
165  uint32_t mappedObject1;
+
166  uint32_t mappedObject2;
+
167  uint32_t mappedObject3;
+
168  uint32_t mappedObject4;
+
169  uint32_t mappedObject5;
+
170  uint32_t mappedObject6;
+
171  uint32_t mappedObject7;
+
172  uint32_t mappedObject8;
+
173 }CO_TPDOMapPar_t;
+
174 
+
175 
+
179 typedef struct{
+
180  CO_EM_t *em;
+
181  CO_SDO_t *SDO;
+
182  const CO_RPDOCommPar_t *RPDOCommPar;
+
183  const CO_RPDOMapPar_t *RPDOMapPar;
+
184  CO_NMT_internalState_t *operatingState;
+
185  uint8_t nodeId;
+
186  uint16_t defaultCOB_ID;
+
187  uint8_t restrictionFlags;
+
189  bool_t valid;
+
191  uint8_t dataLength;
+
193  uint8_t *mapPointer[8];
+
194 #if ((CO_CONFIG_PDO) & CO_CONFIG_PDO_SYNC_ENABLE) || defined CO_DOXYGEN
+
195  CO_SYNC_t *SYNC;
+
197  bool_t synchronous;
+
199  volatile void *CANrxNew[2];
+
201  uint8_t CANrxData[2][8];
+
202 #else
+
203  volatile void *CANrxNew[1];
+
204  uint8_t CANrxData[1][8];
+
205 #endif
+
206 #if ((CO_CONFIG_PDO) & CO_CONFIG_FLAG_CALLBACK_PRE) || defined CO_DOXYGEN
+
207 
+
208  void (*pFunctSignalPre)(void *object);
+
210  void *functSignalObjectPre;
+
211 #endif
+
212  CO_CANmodule_t *CANdevRx;
+
213  uint16_t CANdevRxIdx;
+
214 }CO_RPDO_t;
+
215 
+
216 
+
220 typedef struct{
+
221  CO_EM_t *em;
+
222  CO_SDO_t *SDO;
+
223  const CO_TPDOCommPar_t *TPDOCommPar;
+
224  const CO_TPDOMapPar_t *TPDOMapPar;
+
225  CO_NMT_internalState_t *operatingState;
+
226  uint8_t nodeId;
+
227  uint16_t defaultCOB_ID;
+
228  uint8_t restrictionFlags;
+
229  bool_t valid;
+
231  uint8_t dataLength;
+
234  uint8_t sendRequest;
+
236  uint8_t *mapPointer[8];
+
238  uint32_t inhibitTimer;
+
240  uint32_t eventTimer;
+
244  uint8_t sendIfCOSFlags;
+
245 #if ((CO_CONFIG_PDO) & CO_CONFIG_PDO_SYNC_ENABLE) || defined CO_DOXYGEN
+
246 
+
247  uint8_t syncCounter;
+
248  CO_SYNC_t *SYNC;
+
249 #endif
+
250  CO_CANmodule_t *CANdevTx;
+
251  CO_CANtx_t *CANtxBuff;
+
252  uint16_t CANdevTxIdx;
+
253 }CO_TPDO_t;
+
254 
+
255 
+
286 CO_ReturnError_t CO_RPDO_init(
+
287  CO_RPDO_t *RPDO,
+
288  CO_EM_t *em,
+
289  CO_SDO_t *SDO,
+
290 #if ((CO_CONFIG_PDO) & CO_CONFIG_PDO_SYNC_ENABLE) || defined CO_DOXYGEN
+
291  CO_SYNC_t *SYNC,
+
292 #endif
+
293  CO_NMT_internalState_t *operatingState,
+
294  uint8_t nodeId,
+
295  uint16_t defaultCOB_ID,
+
296  uint8_t restrictionFlags,
+
297  const CO_RPDOCommPar_t *RPDOCommPar,
+
298  const CO_RPDOMapPar_t *RPDOMapPar,
+
299  uint16_t idx_RPDOCommPar,
+
300  uint16_t idx_RPDOMapPar,
+
301  CO_CANmodule_t *CANdevRx,
+
302  uint16_t CANdevRxIdx);
+
303 
+
304 
+
305 #if ((CO_CONFIG_PDO) & CO_CONFIG_FLAG_CALLBACK_PRE) || defined CO_DOXYGEN
+
306 
+
317 void CO_RPDO_initCallbackPre(
+
318  CO_RPDO_t *RPDO,
+
319  void *object,
+
320  void (*pFunctSignalPre)(void *object));
+
321 #endif
+
322 
+
323 
+
354 CO_ReturnError_t CO_TPDO_init(
+
355  CO_TPDO_t *TPDO,
+
356  CO_EM_t *em,
+
357  CO_SDO_t *SDO,
+
358 #if ((CO_CONFIG_PDO) & CO_CONFIG_PDO_SYNC_ENABLE) || defined CO_DOXYGEN
+
359  CO_SYNC_t *SYNC,
+
360 #endif
+
361  CO_NMT_internalState_t *operatingState,
+
362  uint8_t nodeId,
+
363  uint16_t defaultCOB_ID,
+
364  uint8_t restrictionFlags,
+
365  const CO_TPDOCommPar_t *TPDOCommPar,
+
366  const CO_TPDOMapPar_t *TPDOMapPar,
+
367  uint16_t idx_TPDOCommPar,
+
368  uint16_t idx_TPDOMapPar,
+
369  CO_CANmodule_t *CANdevTx,
+
370  uint16_t CANdevTxIdx);
+
371 
+
372 
+
387 uint8_t CO_TPDOisCOS(CO_TPDO_t *TPDO);
+
388 
+
389 
+
401 CO_ReturnError_t CO_TPDOsend(CO_TPDO_t *TPDO);
+
402 
+
403 
+
414 void CO_RPDO_process(CO_RPDO_t *RPDO, bool_t syncWas);
+
415 
+
416 
+
429 void CO_TPDO_process(
+
430  CO_TPDO_t *TPDO,
+
431  bool_t syncWas,
+
432  uint32_t timeDifference_us,
+
433  uint32_t *timerNext_us);
+
434  /* CO_PDO */
+
436 
+
437 #ifdef __cplusplus
+
438 }
+
439 #endif /*__cplusplus*/
+
440 
+
441 #endif /* (CO_CONFIG_PDO) & (CO_CONFIG_RPDO_ENABLE | CO_CONFIG_TPDO_ENABLE) */
+
442 
+
443 #endif /* CO_PDO_H */
+
+
+
uint32_t
unsigned long int uint32_t
UNSIGNED32 in CANopen (0007h), 32-bit unsigned integer.
Definition: CO_driver.h:155
+
CO_driver.h
Interface between CAN hardware and CANopenNode.
+
CO_RPDOCommPar_t::COB_IDUsedByRPDO
uint32_t COB_IDUsedByRPDO
Communication object identifier for message received.
Definition: CO_PDO.h:87
+
CO_TPDOCommPar_t::compatibilityEntry
uint8_t compatibilityEntry
Not used.
Definition: CO_PDO.h:142
+
CO_RPDOMapPar_t::mappedObject4
uint32_t mappedObject4
Same.
Definition: CO_PDO.h:111
+
CO_TPDOMapPar_t::mappedObject5
uint32_t mappedObject5
Same.
Definition: CO_PDO.h:169
+
CO_RPDO_t::functSignalObjectPre
void * functSignalObjectPre
From CO_RPDO_initCallbackPre() or NULL.
Definition: CO_PDO.h:210
+
CO_TPDOMapPar_t::mappedObject8
uint32_t mappedObject8
Same.
Definition: CO_PDO.h:172
+
CO_TPDOCommPar_t::SYNCStartValue
uint8_t SYNCStartValue
Used with numbered SYNC messages.
Definition: CO_PDO.h:150
+
CO_RPDO_t
RPDO object.
Definition: CO_PDO.h:179
+
CO_RPDO_t::em
CO_EM_t * em
From CO_RPDO_init()
Definition: CO_PDO.h:180
+
CO_RPDO_t::nodeId
uint8_t nodeId
From CO_RPDO_init()
Definition: CO_PDO.h:185
+
CO_TPDOCommPar_t::inhibitTime
uint16_t inhibitTime
Minimum time between transmissions of the PDO in 100micro seconds.
Definition: CO_PDO.h:140
+
CO_TPDO_t::sendRequest
uint8_t sendRequest
If application set this flag, PDO will be later sent by function CO_TPDO_process().
Definition: CO_PDO.h:234
+
CO_TPDO_t::defaultCOB_ID
uint16_t defaultCOB_ID
From CO_TPDO_init()
Definition: CO_PDO.h:227
+
CO_TPDO_t::CANdevTxIdx
uint16_t CANdevTxIdx
From CO_TPDO_init()
Definition: CO_PDO.h:252
+
uint16_t
unsigned int uint16_t
UNSIGNED16 in CANopen (0006h), 16-bit unsigned integer.
Definition: CO_driver.h:153
+
CO_RPDO_t::SDO
CO_SDO_t * SDO
From CO_RPDO_init()
Definition: CO_PDO.h:181
+
CO_RPDO_t::operatingState
CO_NMT_internalState_t * operatingState
From CO_RPDO_init()
Definition: CO_PDO.h:184
+
CO_ReturnError_t
CO_ReturnError_t
Return values of some CANopen functions.
Definition: CO_driver.h:488
+
CO_TPDO_process
void CO_TPDO_process(CO_TPDO_t *TPDO, bool_t syncWas, uint32_t timeDifference_us, uint32_t *timerNext_us)
Process transmitting PDO messages.
+
CO_RPDOMapPar_t
RPDO mapping parameter.
Definition: CO_PDO.h:100
+
CO_RPDOMapPar_t::mappedObject1
uint32_t mappedObject1
Location and size of the mapped object.
Definition: CO_PDO.h:108
+
CO_TPDO_t::TPDOMapPar
const CO_TPDOMapPar_t * TPDOMapPar
From CO_TPDO_init()
Definition: CO_PDO.h:224
+
bool_t
unsigned char bool_t
Boolean data type for general use.
Definition: CO_driver.h:141
+
CO_TPDOMapPar_t::mappedObject6
uint32_t mappedObject6
Same.
Definition: CO_PDO.h:170
+
CO_RPDO_t::restrictionFlags
uint8_t restrictionFlags
From CO_RPDO_init()
Definition: CO_PDO.h:187
+
CO_RPDOMapPar_t::mappedObject6
uint32_t mappedObject6
Same.
Definition: CO_PDO.h:113
+
CO_RPDOCommPar_t::transmissionType
uint8_t transmissionType
Transmission type.
Definition: CO_PDO.h:93
+
CO_TPDOCommPar_t::eventTimer
uint16_t eventTimer
Time between periodic transmissions of the PDO in milliseconds.
Definition: CO_PDO.h:145
+
CO_RPDOCommPar_t::maxSubIndex
uint8_t maxSubIndex
Equal to 2.
Definition: CO_PDO.h:81
+
CO_RPDO_process
void CO_RPDO_process(CO_RPDO_t *RPDO, bool_t syncWas)
Process received PDO messages.
+
CO_TPDO_t::SDO
CO_SDO_t * SDO
From CO_TPDO_init()
Definition: CO_PDO.h:222
+
CO_TPDO_t::operatingState
CO_NMT_internalState_t * operatingState
From CO_TPDO_init()
Definition: CO_PDO.h:225
+
CO_TPDOCommPar_t::COB_IDUsedByTPDO
uint32_t COB_IDUsedByTPDO
Communication object identifier for transmitting message.
Definition: CO_PDO.h:129
+
CO_TPDOsend
CO_ReturnError_t CO_TPDOsend(CO_TPDO_t *TPDO)
Send TPDO message.
+
CO_TPDO_t::SYNC
CO_SYNC_t * SYNC
From CO_TPDO_init()
Definition: CO_PDO.h:248
+
CO_RPDO_t::synchronous
bool_t synchronous
True, if PDO synchronous (transmissionType <= 240)
Definition: CO_PDO.h:197
+
CO_RPDOMapPar_t::mappedObject3
uint32_t mappedObject3
Same.
Definition: CO_PDO.h:110
+
CO_TPDOisCOS
uint8_t CO_TPDOisCOS(CO_TPDO_t *TPDO)
Verify Change of State of the PDO.
+
CO_SDOserver.h
CANopen Service Data Object - server protocol.
+
CO_SYNC.h
CANopen Synchronisation protocol.
+
CO_RPDOMapPar_t::numberOfMappedObjects
uint8_t numberOfMappedObjects
Actual number of mapped objects from 0 to 8.
Definition: CO_PDO.h:103
+
CO_RPDOMapPar_t::mappedObject2
uint32_t mappedObject2
Same.
Definition: CO_PDO.h:109
+
CO_TPDOMapPar_t::mappedObject2
uint32_t mappedObject2
Same.
Definition: CO_PDO.h:166
+
CO_TPDO_t::inhibitTimer
uint32_t inhibitTimer
Inhibit timer used for inhibit PDO sending translated to microseconds.
Definition: CO_PDO.h:238
+
CO_TPDOMapPar_t::mappedObject1
uint32_t mappedObject1
Location and size of the mapped object.
Definition: CO_PDO.h:165
+
CO_RPDO_t::defaultCOB_ID
uint16_t defaultCOB_ID
From CO_RPDO_init()
Definition: CO_PDO.h:186
+
CO_TPDOMapPar_t::numberOfMappedObjects
uint8_t numberOfMappedObjects
Actual number of mapped objects from 0 to 8.
Definition: CO_PDO.h:160
+
CO_TPDOMapPar_t::mappedObject7
uint32_t mappedObject7
Same.
Definition: CO_PDO.h:171
+
CO_TPDO_t::nodeId
uint8_t nodeId
From CO_TPDO_init()
Definition: CO_PDO.h:226
+
CO_RPDOMapPar_t::mappedObject5
uint32_t mappedObject5
Same.
Definition: CO_PDO.h:112
+
CO_TPDO_t::restrictionFlags
uint8_t restrictionFlags
From CO_TPDO_init()
Definition: CO_PDO.h:228
+
CO_RPDO_t::dataLength
uint8_t dataLength
Data length of the received PDO message.
Definition: CO_PDO.h:191
+
CO_RPDO_t::CANdevRx
CO_CANmodule_t * CANdevRx
From CO_RPDO_init()
Definition: CO_PDO.h:212
+
CO_TPDO_t::CANtxBuff
CO_CANtx_t * CANtxBuff
CAN transmit buffer inside CANdev.
Definition: CO_PDO.h:251
+
CO_RPDOCommPar_t
RPDO communication parameter.
Definition: CO_PDO.h:80
+
CO_TPDO_t::em
CO_EM_t * em
From CO_TPDO_init()
Definition: CO_PDO.h:221
+
CO_CONFIG_PDO
#define CO_CONFIG_PDO
Configuration of PDO.
Definition: CO_config.h:453
+
CO_RPDOMapPar_t::mappedObject8
uint32_t mappedObject8
Same.
Definition: CO_PDO.h:115
+
CO_RPDO_t::SYNC
CO_SYNC_t * SYNC
From CO_RPDO_init()
Definition: CO_PDO.h:195
+
CO_TPDO_t::TPDOCommPar
const CO_TPDOCommPar_t * TPDOCommPar
From CO_TPDO_init()
Definition: CO_PDO.h:223
+
CO_TPDO_t::syncCounter
uint8_t syncCounter
SYNC counter used for PDO sending.
Definition: CO_PDO.h:247
+
CO_EM_t
Emergency object.
Definition: CO_Emergency.h:369
+
CO_TPDOCommPar_t
TPDO communication parameter.
Definition: CO_PDO.h:122
+
CO_TPDOMapPar_t::mappedObject3
uint32_t mappedObject3
Same.
Definition: CO_PDO.h:167
+
CO_NMT_internalState_t
CO_NMT_internalState_t
Internal network state of the CANopen node.
Definition: CO_NMT_Heartbeat.h:77
+
CO_NMT_Heartbeat.h
CANopen Network management and Heartbeat producer protocol.
+
CO_TPDOCommPar_t::transmissionType
uint8_t transmissionType
Transmission type.
Definition: CO_PDO.h:137
+
CO_TPDO_t::valid
bool_t valid
True, if PDO is enabled and valid.
Definition: CO_PDO.h:229
+
CO_TPDOMapPar_t::mappedObject4
uint32_t mappedObject4
Same.
Definition: CO_PDO.h:168
+
CO_Emergency.h
CANopen Emergency protocol.
+
CO_RPDO_initCallbackPre
void CO_RPDO_initCallbackPre(CO_RPDO_t *RPDO, void *object, void(*pFunctSignalPre)(void *object))
Initialize RPDO callback function.
+
CO_TPDOMapPar_t
TPDO mapping parameter.
Definition: CO_PDO.h:157
+
CO_CANmodule_t
Complete CAN module object.
Definition: CO_driver.h:319
+
CO_RPDO_init
CO_ReturnError_t CO_RPDO_init(CO_RPDO_t *RPDO, CO_EM_t *em, CO_SDO_t *SDO, CO_SYNC_t *SYNC, CO_NMT_internalState_t *operatingState, uint8_t nodeId, uint16_t defaultCOB_ID, uint8_t restrictionFlags, const CO_RPDOCommPar_t *RPDOCommPar, const CO_RPDOMapPar_t *RPDOMapPar, uint16_t idx_RPDOCommPar, uint16_t idx_RPDOMapPar, CO_CANmodule_t *CANdevRx, uint16_t CANdevRxIdx)
Initialize RPDO object.
+
CO_TPDO_t::eventTimer
uint32_t eventTimer
Event timer used for PDO sending translated to microseconds.
Definition: CO_PDO.h:240
+
CO_TPDOCommPar_t::maxSubIndex
uint8_t maxSubIndex
Equal to 6.
Definition: CO_PDO.h:123
+
CO_RPDO_t::RPDOMapPar
const CO_RPDOMapPar_t * RPDOMapPar
From CO_RPDO_init()
Definition: CO_PDO.h:183
+
CO_SYNC_t
SYNC producer and consumer object.
Definition: CO_SYNC.h:75
+
CO_TPDO_t::dataLength
uint8_t dataLength
Data length of the transmitting PDO message.
Definition: CO_PDO.h:231
+
CO_RPDO_t::CANdevRxIdx
uint16_t CANdevRxIdx
From CO_RPDO_init()
Definition: CO_PDO.h:213
+
CO_RPDO_t::valid
bool_t valid
True, if PDO is enabled and valid.
Definition: CO_PDO.h:189
+
CO_TPDO_init
CO_ReturnError_t CO_TPDO_init(CO_TPDO_t *TPDO, CO_EM_t *em, CO_SDO_t *SDO, CO_SYNC_t *SYNC, CO_NMT_internalState_t *operatingState, uint8_t nodeId, uint16_t defaultCOB_ID, uint8_t restrictionFlags, const CO_TPDOCommPar_t *TPDOCommPar, const CO_TPDOMapPar_t *TPDOMapPar, uint16_t idx_TPDOCommPar, uint16_t idx_TPDOMapPar, CO_CANmodule_t *CANdevTx, uint16_t CANdevTxIdx)
Initialize TPDO object.
+
CO_TPDO_t::CANdevTx
CO_CANmodule_t * CANdevTx
From CO_TPDO_init()
Definition: CO_PDO.h:250
+
CO_RPDO_t::RPDOCommPar
const CO_RPDOCommPar_t * RPDOCommPar
From CO_RPDO_init()
Definition: CO_PDO.h:182
+
CO_CANtx_t
Configuration object for CAN transmit message for specific CANopenNode Object.
Definition: CO_driver.h:299
+
CO_TPDO_t
TPDO object.
Definition: CO_PDO.h:220
+
CO_TPDO_t::sendIfCOSFlags
uint8_t sendIfCOSFlags
Each flag bit is connected with one mapPointer.
Definition: CO_PDO.h:244
+
uint8_t
unsigned char uint8_t
UNSIGNED8 in CANopen (0005h), 8-bit unsigned integer.
Definition: CO_driver.h:151
+
CO_RPDOMapPar_t::mappedObject7
uint32_t mappedObject7
Same.
Definition: CO_PDO.h:114
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/CO__SDOclient_8h.html b/Devices/Libraries/Systems/CANopenSocket/docs/CO__SDOclient_8h.html new file mode 100755 index 0000000..a1de14f --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/CO__SDOclient_8h.html @@ -0,0 +1,174 @@ + + + + + + + +CANopenNode: 301/CO_SDOclient.h File Reference + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Data Structures | +Functions
+
+
CO_SDOclient.h File Reference
CANopen_301 » SDO client
+
+
+ +

CANopen Service Data Object - client protocol. +More...

+
#include "301/CO_driver.h"
+#include "301/CO_ODinterface.h"
+#include "301/CO_SDOserver.h"
+#include "301/CO_fifo.h"
+
+

Go to the source code of this file.

+ + + + + +

+Data Structures

struct  CO_SDOclient_t
 SDO client object. More...
 
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

+Functions

CO_ReturnError_t CO_SDOclient_init (CO_SDOclient_t *SDO_C, const OD_t *OD, const OD_entry_t *OD_1280_SDOcliPar, uint8_t nodeId, CO_CANmodule_t *CANdevRx, uint16_t CANdevRxIdx, CO_CANmodule_t *CANdevTx, uint16_t CANdevTxIdx)
 Initialize SDO client object. More...
 
void CO_SDOclient_initCallbackPre (CO_SDOclient_t *SDOclient, void *object, void(*pFunctSignal)(void *object))
 Initialize SDOclient callback function. More...
 
CO_SDO_return_t CO_SDOclient_setup (CO_SDOclient_t *SDO_C, uint32_t COB_IDClientToServer, uint32_t COB_IDServerToClient, uint8_t nodeIDOfTheSDOServer)
 Setup SDO client object. More...
 
CO_SDO_return_t CO_SDOclientDownloadInitiate (CO_SDOclient_t *SDO_C, uint16_t index, uint8_t subIndex, size_t sizeIndicated, uint16_t SDOtimeoutTime_ms, bool_t blockEnable)
 Initiate SDO download communication. More...
 
void CO_SDOclientDownloadInitiateSize (CO_SDOclient_t *SDO_C, size_t sizeIndicated)
 Initiate SDO download communication - update size. More...
 
size_t CO_SDOclientDownloadBufWrite (CO_SDOclient_t *SDO_C, const char *buf, size_t count)
 Write data into SDO client buffer. More...
 
CO_SDO_return_t CO_SDOclientDownload (CO_SDOclient_t *SDO_C, uint32_t timeDifference_us, bool_t abort, bool_t bufferPartial, CO_SDO_abortCode_t *SDOabortCode, size_t *sizeTransferred, uint32_t *timerNext_us)
 Process SDO download communication. More...
 
CO_SDO_return_t CO_SDOclientUploadInitiate (CO_SDOclient_t *SDO_C, uint16_t index, uint8_t subIndex, uint16_t SDOtimeoutTime_ms, bool_t blockEnable)
 Initiate SDO upload communication. More...
 
CO_SDO_return_t CO_SDOclientUpload (CO_SDOclient_t *SDO_C, uint32_t timeDifference_us, bool_t abort, CO_SDO_abortCode_t *SDOabortCode, size_t *sizeIndicated, size_t *sizeTransferred, uint32_t *timerNext_us)
 Process SDO upload communication. More...
 
size_t CO_SDOclientUploadBufRead (CO_SDOclient_t *SDO_C, char *buf, size_t count)
 Read data from SDO client buffer. More...
 
void CO_SDOclientClose (CO_SDOclient_t *SDO_C)
 Close SDO communication temporary. More...
 
+

Detailed Description

+

CANopen Service Data Object - client protocol.

+
Author
Janez Paternoster
+
+Matej Severkar
+
Copyright
2020 Janez Paternoster
+

This file is part of CANopenNode, an opensource CANopen Stack. Project home page is https://github.com/CANopenNode/CANopenNode. For more information on CANopen see http://www.can-cia.org/.

+

Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. You may obtain a copy of the License at 

http://www.apache.org/licenses/LICENSE-2.0
+

Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License.

+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/CO__SDOclient_8h.js b/Devices/Libraries/Systems/CANopenSocket/docs/CO__SDOclient_8h.js new file mode 100755 index 0000000..08dfbc4 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/CO__SDOclient_8h.js @@ -0,0 +1,14 @@ +var CO__SDOclient_8h = +[ + [ "CO_SDOclient_init", "group__CO__SDOclient.html#ga754160e34089aea70f84d22b06eaff9e", null ], + [ "CO_SDOclient_initCallbackPre", "group__CO__SDOclient.html#ga4377eaecc3bd0a8320a2bbe1ef0ef776", null ], + [ "CO_SDOclient_setup", "group__CO__SDOclient.html#gade3bf4e3249aa4c611570ec43563a08d", null ], + [ "CO_SDOclientDownloadInitiate", "group__CO__SDOclient.html#ga40f6e79592e1d587d02bbb4eaefa9489", null ], + [ "CO_SDOclientDownloadInitiateSize", "group__CO__SDOclient.html#gaf58b7731b4285538c26a0c7c49ab24b6", null ], + [ "CO_SDOclientDownloadBufWrite", "group__CO__SDOclient.html#ga958d0568bd47d9a3152f9ea8d104b5f5", null ], + [ "CO_SDOclientDownload", "group__CO__SDOclient.html#gaab262f0a8d08ba023639a2c197d0943a", null ], + [ "CO_SDOclientUploadInitiate", "group__CO__SDOclient.html#ga3180f82563b3ed768fe7d3bd34fe1886", null ], + [ "CO_SDOclientUpload", "group__CO__SDOclient.html#gabd3a3be7e3d1649adfdd253c979ec21f", null ], + [ "CO_SDOclientUploadBufRead", "group__CO__SDOclient.html#gaf5cd4e009476b15a2cd995a9841fb175", null ], + [ "CO_SDOclientClose", "group__CO__SDOclient.html#ga9b98ea2c36f864f1a589c842528b12ab", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/CO__SDOclient_8h_source.html b/Devices/Libraries/Systems/CANopenSocket/docs/CO__SDOclient_8h_source.html new file mode 100755 index 0000000..4b9b152 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/CO__SDOclient_8h_source.html @@ -0,0 +1,339 @@ + + + + + + + +CANopenNode: 301/CO_SDOclient.h Source File + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+
CO_SDOclient.h
+
+
+Go to the documentation of this file.
1 
+
27 #ifndef CO_SDO_CLIENT_H
+
28 #define CO_SDO_CLIENT_H
+
29 
+
30 #include "301/CO_driver.h"
+
31 #include "301/CO_ODinterface.h"
+
32 #include "301/CO_SDOserver.h"
+
33 #include "301/CO_fifo.h"
+
34 
+
35 /* default configuration, see CO_config.h */
+
36 #ifndef CO_CONFIG_SDO_CLI
+
37 #define CO_CONFIG_SDO_CLI (0)
+
38 #endif
+
39 #ifndef CO_CONFIG_SDO_CLI_BUFFER_SIZE
+
40  #if (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_BLOCK
+
41  #define CO_CONFIG_SDO_CLI_BUFFER_SIZE 1000
+
42  #else
+
43  #define CO_CONFIG_SDO_CLI_BUFFER_SIZE 32
+
44  #endif
+
45 #endif
+
46 
+
47 #if ((CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_ENABLE) || defined CO_DOXYGEN
+
48 
+
49 #ifdef __cplusplus
+
50 extern "C" {
+
51 #endif
+
52 
+
67 typedef struct {
+
68 #if ((CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_LOCAL) || defined CO_DOXYGEN
+
69 
+
70  const OD_t *OD;
+
72  uint8_t nodeId;
+
74  OD_IO_t OD_IO;
+
76  OD_attr_t attribute;
+
77 #endif
+
78 
+
79  CO_CANmodule_t *CANdevRx;
+
81  uint16_t CANdevRxIdx;
+
83  CO_CANmodule_t *CANdevTx;
+
85  uint16_t CANdevTxIdx;
+
87  CO_CANtx_t *CANtxBuff;
+
88 #if ((CO_CONFIG_SDO_CLI) & CO_CONFIG_FLAG_OD_DYNAMIC) || defined CO_DOXYGEN
+
89 
+
93  uint32_t COB_IDClientToServer;
+
95  uint32_t COB_IDServerToClient;
+
96 #endif
+
97 
+
98  uint8_t nodeIDOfTheSDOServer;
+
99  /* If true, SDO channel is valid */
+
100  bool_t valid;
+
102  uint16_t index;
+
104  uint8_t subIndex;
+
105  /* If true, then data transfer is finished */
+
106  bool_t finished;
+
109  size_t sizeInd;
+
111  size_t sizeTran;
+
113  volatile CO_SDO_state_t state;
+
115  uint32_t SDOtimeoutTime_us;
+
117  uint32_t timeoutTimer;
+
119  CO_fifo_t bufFifo;
+
122  char buf[CO_CONFIG_SDO_CLI_BUFFER_SIZE + 1];
+
125  volatile void *CANrxNew;
+
127  uint8_t CANrxData[8];
+
128 #if ((CO_CONFIG_SDO_CLI) & CO_CONFIG_FLAG_CALLBACK_PRE) || defined CO_DOXYGEN
+
129 
+
130  void (*pFunctSignal)(void *object);
+
132  void *functSignalObject;
+
133 #endif
+
134 #if ((CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_SEGMENTED) || defined CO_DOXYGEN
+
135 
+
136  uint8_t toggle;
+
137 #endif
+
138 #if ((CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_BLOCK) || defined CO_DOXYGEN
+
139 
+
140  uint32_t block_SDOtimeoutTime_us;
+
142  uint32_t block_timeoutTimer;
+
144  uint8_t block_seqno;
+
146  uint8_t block_blksize;
+
148  uint8_t block_noData;
+
150  bool_t block_crcEnabled;
+
152  uint8_t block_dataUploadLast[7];
+
154  uint16_t block_crc;
+
155 #endif
+
156 } CO_SDOclient_t;
+
157 
+
158 
+
179 CO_ReturnError_t CO_SDOclient_init(CO_SDOclient_t *SDO_C,
+
180  const OD_t *OD,
+
181  const OD_entry_t *OD_1280_SDOcliPar,
+
182  uint8_t nodeId,
+
183  CO_CANmodule_t *CANdevRx,
+
184  uint16_t CANdevRxIdx,
+
185  CO_CANmodule_t *CANdevTx,
+
186  uint16_t CANdevTxIdx);
+
187 
+
188 
+
189 #if ((CO_CONFIG_SDO_CLI) & CO_CONFIG_FLAG_CALLBACK_PRE) || defined CO_DOXYGEN
+
190 
+
204 void CO_SDOclient_initCallbackPre(CO_SDOclient_t *SDOclient,
+
205  void *object,
+
206  void (*pFunctSignal)(void *object));
+
207 #endif
+
208 
+
209 
+
228 CO_SDO_return_t CO_SDOclient_setup(CO_SDOclient_t *SDO_C,
+
229  uint32_t COB_IDClientToServer,
+
230  uint32_t COB_IDServerToClient,
+
231  uint8_t nodeIDOfTheSDOServer);
+
232 
+
233 
+
258 CO_SDO_return_t CO_SDOclientDownloadInitiate(CO_SDOclient_t *SDO_C,
+
259  uint16_t index,
+
260  uint8_t subIndex,
+
261  size_t sizeIndicated,
+
262  uint16_t SDOtimeoutTime_ms,
+
263  bool_t blockEnable);
+
264 
+
265 
+
277 void CO_SDOclientDownloadInitiateSize(CO_SDOclient_t *SDO_C,
+
278  size_t sizeIndicated);
+
279 
+
280 
+
300 size_t CO_SDOclientDownloadBufWrite(CO_SDOclient_t *SDO_C,
+
301  const char *buf,
+
302  size_t count);
+
303 
+
304 
+
333 CO_SDO_return_t CO_SDOclientDownload(CO_SDOclient_t *SDO_C,
+
334  uint32_t timeDifference_us,
+
335  bool_t abort,
+
336  bool_t bufferPartial,
+
337  CO_SDO_abortCode_t *SDOabortCode,
+
338  size_t *sizeTransferred,
+
339  uint32_t *timerNext_us);
+
340 
+
341 
+
357 CO_SDO_return_t CO_SDOclientUploadInitiate(CO_SDOclient_t *SDO_C,
+
358  uint16_t index,
+
359  uint8_t subIndex,
+
360  uint16_t SDOtimeoutTime_ms,
+
361  bool_t blockEnable);
+
362 
+
363 
+
394 CO_SDO_return_t CO_SDOclientUpload(CO_SDOclient_t *SDO_C,
+
395  uint32_t timeDifference_us,
+
396  bool_t abort,
+
397  CO_SDO_abortCode_t *SDOabortCode,
+
398  size_t *sizeIndicated,
+
399  size_t *sizeTransferred,
+
400  uint32_t *timerNext_us);
+
401 
+
402 
+
423 size_t CO_SDOclientUploadBufRead(CO_SDOclient_t *SDO_C,
+
424  char *buf,
+
425  size_t count);
+
426 
+
427 
+
437 void CO_SDOclientClose(CO_SDOclient_t *SDO_C);
+
438  /* CO_SDOclient */
+
440 
+
441 #ifdef __cplusplus
+
442 }
+
443 #endif /*__cplusplus*/
+
444 
+
445 #endif /* (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_ENABLE */
+
446 
+
447 #endif /* CO_SDO_CLIENT_H */
+
+
+
uint32_t
unsigned long int uint32_t
UNSIGNED32 in CANopen (0007h), 32-bit unsigned integer.
Definition: CO_driver.h:155
+
CO_driver.h
Interface between CAN hardware and CANopenNode.
+
CO_SDOclient_t::CANrxNew
volatile void * CANrxNew
Indicates, if new SDO message received from CAN bus.
Definition: CO_SDOclient.h:125
+
CO_SDOclient_t::attribute
OD_attr_t attribute
Attribute for locally transferred OD sub-object, see OD_attributes_t.
Definition: CO_SDOclient.h:76
+
CO_SDO_return_t
CO_SDO_return_t
Return values from SDO server or client functions.
Definition: CO_SDOserver.h:410
+
CO_SDOclient_t::block_crcEnabled
bool_t block_crcEnabled
Server CRC support in block transfer.
Definition: CO_SDOclient.h:150
+
CO_SDOclient_t
SDO client object.
Definition: CO_SDOclient.h:67
+
CO_SDOclient_setup
CO_SDO_return_t CO_SDOclient_setup(CO_SDOclient_t *SDO_C, uint32_t COB_IDClientToServer, uint32_t COB_IDServerToClient, uint8_t nodeIDOfTheSDOServer)
Setup SDO client object.
+
uint16_t
unsigned int uint16_t
UNSIGNED16 in CANopen (0006h), 16-bit unsigned integer.
Definition: CO_driver.h:153
+
CO_SDOclient_t::COB_IDClientToServer
uint32_t COB_IDClientToServer
Copy of CANopen COB_ID Client -> Server, meaning of the specific bits:
Definition: CO_SDOclient.h:93
+
CO_CONFIG_SDO_CLI_BUFFER_SIZE
#define CO_CONFIG_SDO_CLI_BUFFER_SIZE
Size of the internal data buffer for the SDO client.
Definition: CO_config.h:386
+
CO_SDOclient_t::sizeInd
size_t sizeInd
Size of data, which will be transferred.
Definition: CO_SDOclient.h:109
+
CO_ReturnError_t
CO_ReturnError_t
Return values of some CANopen functions.
Definition: CO_driver.h:488
+
CO_SDOclient_t::OD_IO
OD_IO_t OD_IO
Object dictionary interface for locally transferred object.
Definition: CO_SDOclient.h:74
+
CO_SDOclientUpload
CO_SDO_return_t CO_SDOclientUpload(CO_SDOclient_t *SDO_C, uint32_t timeDifference_us, bool_t abort, CO_SDO_abortCode_t *SDOabortCode, size_t *sizeIndicated, size_t *sizeTransferred, uint32_t *timerNext_us)
Process SDO upload communication.
+
CO_SDOclient_t::bufFifo
CO_fifo_t bufFifo
CO_fifo_t object for data buffer (not pointer)
Definition: CO_SDOclient.h:119
+
bool_t
unsigned char bool_t
Boolean data type for general use.
Definition: CO_driver.h:141
+
CO_SDOclient_init
CO_ReturnError_t CO_SDOclient_init(CO_SDOclient_t *SDO_C, const OD_t *OD, const OD_entry_t *OD_1280_SDOcliPar, uint8_t nodeId, CO_CANmodule_t *CANdevRx, uint16_t CANdevRxIdx, CO_CANmodule_t *CANdevTx, uint16_t CANdevTxIdx)
Initialize SDO client object.
+
CO_SDOclient_t::block_timeoutTimer
uint32_t block_timeoutTimer
Timeout timer for SDO sub-block upload.
Definition: CO_SDOclient.h:142
+
CO_SDOclient_t::subIndex
uint8_t subIndex
Subindex of current object in Object Dictionary.
Definition: CO_SDOclient.h:104
+
CO_SDOclient_t::OD
const OD_t * OD
From CO_SDOclient_init()
Definition: CO_SDOclient.h:70
+
CO_SDOclient_t::index
uint16_t index
Index of current object in Object Dictionary.
Definition: CO_SDOclient.h:102
+
CO_SDOclient_initCallbackPre
void CO_SDOclient_initCallbackPre(CO_SDOclient_t *SDOclient, void *object, void(*pFunctSignal)(void *object))
Initialize SDOclient callback function.
+
CO_SDOclient_t::nodeIDOfTheSDOServer
uint8_t nodeIDOfTheSDOServer
Node-ID of the SDO server.
Definition: CO_SDOclient.h:98
+
CO_SDOserver.h
CANopen Service Data Object - server protocol.
+
CO_SDOclient_t::CANdevRx
CO_CANmodule_t * CANdevRx
From CO_SDOclient_init()
Definition: CO_SDOclient.h:79
+
CO_SDOclientDownloadInitiateSize
void CO_SDOclientDownloadInitiateSize(CO_SDOclient_t *SDO_C, size_t sizeIndicated)
Initiate SDO download communication - update size.
+
CO_SDOclientDownload
CO_SDO_return_t CO_SDOclientDownload(CO_SDOclient_t *SDO_C, uint32_t timeDifference_us, bool_t abort, bool_t bufferPartial, CO_SDO_abortCode_t *SDOabortCode, size_t *sizeTransferred, uint32_t *timerNext_us)
Process SDO download communication.
+
CO_SDOclient_t::COB_IDServerToClient
uint32_t COB_IDServerToClient
Copy of CANopen COB_ID Server -> Client, similar as above.
Definition: CO_SDOclient.h:95
+
CO_SDOclient_t::CANdevTxIdx
uint16_t CANdevTxIdx
From CO_SDOclient_init()
Definition: CO_SDOclient.h:85
+
CO_SDOclient_t::toggle
uint8_t toggle
Toggle bit toggled in each segment in segmented transfer.
Definition: CO_SDOclient.h:136
+
CO_SDOclient_t::CANtxBuff
CO_CANtx_t * CANtxBuff
CAN transmit buffer inside CANdevTx for CAN tx message.
Definition: CO_SDOclient.h:87
+
CO_SDOclient_t::block_SDOtimeoutTime_us
uint32_t block_SDOtimeoutTime_us
Timeout time for SDO sub-block upload, half of SDOtimeoutTime_us.
Definition: CO_SDOclient.h:140
+
CO_SDOclient_t::block_seqno
uint8_t block_seqno
Sequence number of segment in block, 1..127.
Definition: CO_SDOclient.h:144
+
CO_SDOclientUploadInitiate
CO_SDO_return_t CO_SDOclientUploadInitiate(CO_SDOclient_t *SDO_C, uint16_t index, uint8_t subIndex, uint16_t SDOtimeoutTime_ms, bool_t blockEnable)
Initiate SDO upload communication.
+
CO_SDO_abortCode_t
CO_SDO_abortCode_t
SDO abort codes.
Definition: CO_SDOserver.h:333
+
CO_SDOclientDownloadInitiate
CO_SDO_return_t CO_SDOclientDownloadInitiate(CO_SDOclient_t *SDO_C, uint16_t index, uint8_t subIndex, size_t sizeIndicated, uint16_t SDOtimeoutTime_ms, bool_t blockEnable)
Initiate SDO download communication.
+
CO_SDOclient_t::block_crc
uint16_t block_crc
Calculated CRC checksum.
Definition: CO_SDOclient.h:154
+
CO_SDO_state_t
CO_SDO_state_t
Internal states of the SDO state machine.
Definition: CO_SDOserver.h:102
+
CO_fifo_t
Fifo object.
Definition: CO_fifo.h:60
+
CO_SDOclientUploadBufRead
size_t CO_SDOclientUploadBufRead(CO_SDOclient_t *SDO_C, char *buf, size_t count)
Read data from SDO client buffer.
+
CO_SDOclient_t::nodeId
uint8_t nodeId
From CO_SDOclient_init()
Definition: CO_SDOclient.h:72
+
CO_SDOclient_t::state
volatile CO_SDO_state_t state
Internal state of the SDO client.
Definition: CO_SDOclient.h:113
+
CO_ODinterface.h
CANopen Object Dictionary interface.
+
CO_SDOclientDownloadBufWrite
size_t CO_SDOclientDownloadBufWrite(CO_SDOclient_t *SDO_C, const char *buf, size_t count)
Write data into SDO client buffer.
+
CO_SDOclient_t::sizeTran
size_t sizeTran
Size of data which is actually transferred.
Definition: CO_SDOclient.h:111
+
OD_t
Object Dictionary.
Definition: CO_ODinterface.h:352
+
CO_CANmodule_t
Complete CAN module object.
Definition: CO_driver.h:319
+
CO_SDOclient_t::CANdevTx
CO_CANmodule_t * CANdevTx
From CO_SDOclient_init()
Definition: CO_SDOclient.h:83
+
OD_IO_t
Structure for input / output on the OD variable.
Definition: CO_ODinterface.h:263
+
OD_entry_t
Object Dictionary entry for one OD object.
Definition: CO_ODinterface.h:336
+
CO_fifo.h
FIFO circular buffer.
+
CO_SDOclientClose
void CO_SDOclientClose(CO_SDOclient_t *SDO_C)
Close SDO communication temporary.
+
CO_SDOclient_t::block_blksize
uint8_t block_blksize
Number of segments per block, 1..127.
Definition: CO_SDOclient.h:146
+
CO_SDOclient_t::block_noData
uint8_t block_noData
Number of bytes in last segment that do not contain data.
Definition: CO_SDOclient.h:148
+
CO_SDOclient_t::functSignalObject
void * functSignalObject
From CO_SDOclient_initCallbackPre() or NULL.
Definition: CO_SDOclient.h:132
+
CO_SDOclient_t::SDOtimeoutTime_us
uint32_t SDOtimeoutTime_us
Maximum timeout time between request and response in microseconds.
Definition: CO_SDOclient.h:115
+
CO_SDOclient_t::timeoutTimer
uint32_t timeoutTimer
Timeout timer for SDO communication.
Definition: CO_SDOclient.h:117
+
CO_CANtx_t
Configuration object for CAN transmit message for specific CANopenNode Object.
Definition: CO_driver.h:299
+
uint8_t
unsigned char uint8_t
UNSIGNED8 in CANopen (0005h), 8-bit unsigned integer.
Definition: CO_driver.h:151
+
CO_SDOclient_t::CANdevRxIdx
uint16_t CANdevRxIdx
From CO_SDOclient_init()
Definition: CO_SDOclient.h:81
+
OD_attr_t
#define OD_attr_t
Size of Object Dictionary attribute.
Definition: CO_ODinterface.h:50
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/CO__SDOserver_8h.html b/Devices/Libraries/Systems/CANopenSocket/docs/CO__SDOserver_8h.html new file mode 100755 index 0000000..afe5dfe --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/CO__SDOserver_8h.html @@ -0,0 +1,254 @@ + + + + + + + +CANopenNode: 301/CO_SDOserver.h File Reference + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Data Structures | +Macros | +Enumerations | +Functions
+
+
CO_SDOserver.h File Reference
CANopen_301 » SDO server
+
+
+ +

CANopen Service Data Object - server protocol. +More...

+
#include "301/CO_driver.h"
+#include "301/CO_ODinterface.h"
+
+

Go to the source code of this file.

+ + + + + +

+Data Structures

struct  CO_SDOserver_t
 SDO server object. More...
 
+ + + + +

+Macros

#define CO_SDO_ST_FLAG_DOWNLOAD   0x10U
 Internal state flags indicate type of transfer. More...
 
+ + + + + + + + + + +

+Enumerations

enum  CO_SDO_state_t {
+  CO_SDO_ST_IDLE = 0x00U, +CO_SDO_ST_ABORT = 0x01U, +CO_SDO_ST_DOWNLOAD_LOCAL_TRANSFER = 0x10U, +CO_SDO_ST_DOWNLOAD_INITIATE_REQ = 0x11U, +
+  CO_SDO_ST_DOWNLOAD_INITIATE_RSP = 0x12U, +CO_SDO_ST_DOWNLOAD_SEGMENT_REQ = 0x13U, +CO_SDO_ST_DOWNLOAD_SEGMENT_RSP = 0x14U, +CO_SDO_ST_UPLOAD_LOCAL_TRANSFER = 0x20U, +
+  CO_SDO_ST_UPLOAD_INITIATE_REQ = 0x21U, +CO_SDO_ST_UPLOAD_INITIATE_RSP = 0x22U, +CO_SDO_ST_UPLOAD_SEGMENT_REQ = 0x23U, +CO_SDO_ST_UPLOAD_SEGMENT_RSP = 0x24U, +
+  CO_SDO_ST_DOWNLOAD_BLK_INITIATE_REQ = 0x51U, +CO_SDO_ST_DOWNLOAD_BLK_INITIATE_RSP = 0x52U, +CO_SDO_ST_DOWNLOAD_BLK_SUBBLOCK_REQ = 0x53U, +CO_SDO_ST_DOWNLOAD_BLK_SUBBLOCK_RSP = 0x54U, +
+  CO_SDO_ST_DOWNLOAD_BLK_END_REQ = 0x55U, +CO_SDO_ST_DOWNLOAD_BLK_END_RSP = 0x56U, +CO_SDO_ST_UPLOAD_BLK_INITIATE_REQ = 0x61U, +CO_SDO_ST_UPLOAD_BLK_INITIATE_RSP = 0x62U, +
+  CO_SDO_ST_UPLOAD_BLK_INITIATE_REQ2 = 0x63U, +CO_SDO_ST_UPLOAD_BLK_SUBBLOCK_SREQ = 0x64U, +CO_SDO_ST_UPLOAD_BLK_SUBBLOCK_CRSP = 0x65U, +CO_SDO_ST_UPLOAD_BLK_END_SREQ = 0x66U, +
+  CO_SDO_ST_UPLOAD_BLK_END_CRSP = 0x67U +
+ }
 Internal states of the SDO state machine. More...
 
enum  CO_SDO_abortCode_t {
+  CO_SDO_AB_NONE = 0x00000000UL, +CO_SDO_AB_TOGGLE_BIT = 0x05030000UL, +CO_SDO_AB_TIMEOUT = 0x05040000UL, +CO_SDO_AB_CMD = 0x05040001UL, +
+  CO_SDO_AB_BLOCK_SIZE = 0x05040002UL, +CO_SDO_AB_SEQ_NUM = 0x05040003UL, +CO_SDO_AB_CRC = 0x05040004UL, +CO_SDO_AB_OUT_OF_MEM = 0x05040005UL, +
+  CO_SDO_AB_UNSUPPORTED_ACCESS = 0x06010000UL, +CO_SDO_AB_WRITEONLY = 0x06010001UL, +CO_SDO_AB_READONLY = 0x06010002UL, +CO_SDO_AB_NOT_EXIST = 0x06020000UL, +
+  CO_SDO_AB_NO_MAP = 0x06040041UL, +CO_SDO_AB_MAP_LEN = 0x06040042UL, +CO_SDO_AB_PRAM_INCOMPAT = 0x06040043UL, +CO_SDO_AB_DEVICE_INCOMPAT = 0x06040047UL, +
+  CO_SDO_AB_HW = 0x06060000UL, +CO_SDO_AB_TYPE_MISMATCH = 0x06070010UL, +CO_SDO_AB_DATA_LONG = 0x06070012UL, +CO_SDO_AB_DATA_SHORT = 0x06070013UL, +
+  CO_SDO_AB_SUB_UNKNOWN = 0x06090011UL, +CO_SDO_AB_INVALID_VALUE = 0x06090030UL, +CO_SDO_AB_VALUE_HIGH = 0x06090031UL, +CO_SDO_AB_VALUE_LOW = 0x06090032UL, +
+  CO_SDO_AB_MAX_LESS_MIN = 0x06090036UL, +CO_SDO_AB_NO_RESOURCE = 0x060A0023UL, +CO_SDO_AB_GENERAL = 0x08000000UL, +CO_SDO_AB_DATA_TRANSF = 0x08000020UL, +
+  CO_SDO_AB_DATA_LOC_CTRL = 0x08000021UL, +CO_SDO_AB_DATA_DEV_STATE = 0x08000022UL, +CO_SDO_AB_DATA_OD = 0x08000023UL, +CO_SDO_AB_NO_DATA = 0x08000024UL +
+ }
 SDO abort codes. More...
 
enum  CO_SDO_return_t {
+  CO_SDO_RT_waitingLocalTransfer = 6, +CO_SDO_RT_uploadDataBufferFull = 5, +CO_SDO_RT_transmittBufferFull = 4, +CO_SDO_RT_blockDownldInProgress = 3, +
+  CO_SDO_RT_blockUploadInProgress = 2, +CO_SDO_RT_waitingResponse = 1, +CO_SDO_RT_ok_communicationEnd = 0, +CO_SDO_RT_wrongArguments = -2, +
+  CO_SDO_RT_endedWithClientAbort = -9, +CO_SDO_RT_endedWithServerAbort = -10 +
+ }
 Return values from SDO server or client functions. More...
 
+ + + + + + + + + + +

+Functions

CO_ReturnError_t CO_SDOserver_init (CO_SDOserver_t *SDO, const OD_t *OD, const OD_entry_t *OD_1200_SDOsrvPar, uint8_t nodeId, uint16_t SDOtimeoutTime_ms, CO_CANmodule_t *CANdevRx, uint16_t CANdevRxIdx, CO_CANmodule_t *CANdevTx, uint16_t CANdevTxIdx)
 Initialize SDO object. More...
 
void CO_SDOserver_initCallbackPre (CO_SDOserver_t *SDO, void *object, void(*pFunctSignalPre)(void *object))
 Initialize SDOrx callback function. More...
 
CO_SDO_return_t CO_SDOserver_process (CO_SDOserver_t *SDO, bool_t NMTisPreOrOperational, uint32_t timeDifference_us, uint32_t *timerNext_us)
 Process SDO communication. More...
 
+

Detailed Description

+

CANopen Service Data Object - server protocol.

+
Author
Janez Paternoster
+
Copyright
2020 Janez Paternoster
+

This file is part of CANopenNode, an opensource CANopen Stack. Project home page is https://github.com/CANopenNode/CANopenNode. For more information on CANopen see http://www.can-cia.org/.

+

Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. You may obtain a copy of the License at 

http://www.apache.org/licenses/LICENSE-2.0
+

Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License.

+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/CO__SDOserver_8h.js b/Devices/Libraries/Systems/CANopenSocket/docs/CO__SDOserver_8h.js new file mode 100755 index 0000000..dbbe37b --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/CO__SDOserver_8h.js @@ -0,0 +1,80 @@ +var CO__SDOserver_8h = +[ + [ "CO_SDO_ST_FLAG_DOWNLOAD", "group__CO__SDOserver.html#ga84d9afbba1769aada5c52c81b7f5c3f4", null ], + [ "CO_SDO_state_t", "group__CO__SDOserver.html#ga0b0e614dadcc1c005185b8bc9a7fec11", [ + [ "CO_SDO_ST_IDLE", "group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11a2eee38ba2a2d52890281ae54b12d50b3", null ], + [ "CO_SDO_ST_ABORT", "group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11ac40cb6c0b2f2eb1877aee3963dc1927d", null ], + [ "CO_SDO_ST_DOWNLOAD_LOCAL_TRANSFER", "group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11a8f685c4d233c35defb423fda8ff5544c", null ], + [ "CO_SDO_ST_DOWNLOAD_INITIATE_REQ", "group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11ac07432ccfaa6be8730cc8c306b3e42bb", null ], + [ "CO_SDO_ST_DOWNLOAD_INITIATE_RSP", "group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11a49b060ebf39c4bfb498b8691c16bb882", null ], + [ "CO_SDO_ST_DOWNLOAD_SEGMENT_REQ", "group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11a6b44777e7e209313612baab5f83745ff", null ], + [ "CO_SDO_ST_DOWNLOAD_SEGMENT_RSP", "group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11ae5b55aec51372cbc2a6e32ce1456c11c", null ], + [ "CO_SDO_ST_UPLOAD_LOCAL_TRANSFER", "group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11aa07fe53d69ec7e0d56db39111867f8ce", null ], + [ "CO_SDO_ST_UPLOAD_INITIATE_REQ", "group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11aa8a8b5050c6528fdaa19bbb429d8e4f4", null ], + [ "CO_SDO_ST_UPLOAD_INITIATE_RSP", "group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11aa096d10c9eb891cfedddc16276f58aaf", null ], + [ "CO_SDO_ST_UPLOAD_SEGMENT_REQ", "group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11ad610c289b85192d70c835b033b49b3fb", null ], + [ "CO_SDO_ST_UPLOAD_SEGMENT_RSP", "group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11a210a3eb6acfdb055bb72a59d8e24a6b6", null ], + [ "CO_SDO_ST_DOWNLOAD_BLK_INITIATE_REQ", "group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11a5d4ead9d3f06962987b6af8c073b6a2e", null ], + [ "CO_SDO_ST_DOWNLOAD_BLK_INITIATE_RSP", "group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11af25ee4e636a98dd72fe4c5bef9bcecf2", null ], + [ "CO_SDO_ST_DOWNLOAD_BLK_SUBBLOCK_REQ", "group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11a320cc9749db35473265b5203c547bbf8", null ], + [ "CO_SDO_ST_DOWNLOAD_BLK_SUBBLOCK_RSP", "group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11a70e97f34a6a98014bef1d2eeb3b5247c", null ], + [ "CO_SDO_ST_DOWNLOAD_BLK_END_REQ", "group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11af955593bb966b324bfda361b0364d15b", null ], + [ "CO_SDO_ST_DOWNLOAD_BLK_END_RSP", "group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11af511c26db1fb7ba18d6054255b560be7", null ], + [ "CO_SDO_ST_UPLOAD_BLK_INITIATE_REQ", "group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11a49b5c39c9e5d025c85eedffa28aa22ed", null ], + [ "CO_SDO_ST_UPLOAD_BLK_INITIATE_RSP", "group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11ae9be0eeb0711890d1b9c5cbfbd204ed8", null ], + [ "CO_SDO_ST_UPLOAD_BLK_INITIATE_REQ2", "group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11acc4e87ad1ad20eddd19a60d9592bbada", null ], + [ "CO_SDO_ST_UPLOAD_BLK_SUBBLOCK_SREQ", "group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11a39f1cb5426ee3c3689ed833cb66e231c", null ], + [ "CO_SDO_ST_UPLOAD_BLK_SUBBLOCK_CRSP", "group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11af762eb5a985cf79a3e7423a39b29b328", null ], + [ "CO_SDO_ST_UPLOAD_BLK_END_SREQ", "group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11a98896138e97542e659051fff33b1a692", null ], + [ "CO_SDO_ST_UPLOAD_BLK_END_CRSP", "group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11ab75a573a45778a0c4bea2c50402be03e", null ] + ] ], + [ "CO_SDO_abortCode_t", "group__CO__SDOserver.html#ga7587ddcf798747fe6d97d03bf1bf2979", [ + [ "CO_SDO_AB_NONE", "group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979a5fc84558a4ca47e067189a14543691b6", null ], + [ "CO_SDO_AB_TOGGLE_BIT", "group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979ad4e9214eab1d034e9c10eb6c7638e592", null ], + [ "CO_SDO_AB_TIMEOUT", "group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979a036d0be874d10f66aa6601d76a9aa2f0", null ], + [ "CO_SDO_AB_CMD", "group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979a26b4e2680c16ce6a09d3e3a8293472ce", null ], + [ "CO_SDO_AB_BLOCK_SIZE", "group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979ac86b70b71d601658c93a1dd270a902b0", null ], + [ "CO_SDO_AB_SEQ_NUM", "group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979a778ef6b5751cb8ba10b67436409c3fd2", null ], + [ "CO_SDO_AB_CRC", "group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979aee7fcab60a6fde6e41d999f5a2b10aa5", null ], + [ "CO_SDO_AB_OUT_OF_MEM", "group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979adc021e79ace03edbd279a3c492853c7f", null ], + [ "CO_SDO_AB_UNSUPPORTED_ACCESS", "group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979a370ff72a5bddee5760ba0930c3b13ba0", null ], + [ "CO_SDO_AB_WRITEONLY", "group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979a457e80af0f952c272fa90ebd45cdb8cd", null ], + [ "CO_SDO_AB_READONLY", "group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979a48c8a5f4939372564a17b31f992b82a4", null ], + [ "CO_SDO_AB_NOT_EXIST", "group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979a6ef5b921ac0f299f34e9860eb82e332e", null ], + [ "CO_SDO_AB_NO_MAP", "group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979a729452df9557e4acbda8691efb4da310", null ], + [ "CO_SDO_AB_MAP_LEN", "group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979a07edee9ce8ec5cd01cfd3cfbff48b96c", null ], + [ "CO_SDO_AB_PRAM_INCOMPAT", "group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979acaedcf71c4638efb40fc6debfa9dba67", null ], + [ "CO_SDO_AB_DEVICE_INCOMPAT", "group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979ad07acc06f76122627412a71f2f2e39fc", null ], + [ "CO_SDO_AB_HW", "group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979a070f096bb09f5a6235643702b5a40759", null ], + [ "CO_SDO_AB_TYPE_MISMATCH", "group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979a838c274eaa14626514da8f7a8ac043c3", null ], + [ "CO_SDO_AB_DATA_LONG", "group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979a50d373f7a7ba976dc2277a2111cf56c3", null ], + [ "CO_SDO_AB_DATA_SHORT", "group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979a481537e4c170066ca31b167fa598bb54", null ], + [ "CO_SDO_AB_SUB_UNKNOWN", "group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979a3e48e535fddeaa78a4059c2f91f9bb8e", null ], + [ "CO_SDO_AB_INVALID_VALUE", "group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979afff1ec491c628031e65672383f3e3c76", null ], + [ "CO_SDO_AB_VALUE_HIGH", "group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979a4983bce8e9503f9e7a720a44528036ad", null ], + [ "CO_SDO_AB_VALUE_LOW", "group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979ab402816165086fbad21a130e9f488d52", null ], + [ "CO_SDO_AB_MAX_LESS_MIN", "group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979a15d49829c0d15f8cb9995f07617d874f", null ], + [ "CO_SDO_AB_NO_RESOURCE", "group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979ab54dd042727804cd8f310a04fd4575f7", null ], + [ "CO_SDO_AB_GENERAL", "group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979a58d6be7d156bbe576b8438a6fd5b446d", null ], + [ "CO_SDO_AB_DATA_TRANSF", "group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979a631a043a79c7eef4ddb2f874365c6660", null ], + [ "CO_SDO_AB_DATA_LOC_CTRL", "group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979ac489bb77a98f65008932861924bc4bbf", null ], + [ "CO_SDO_AB_DATA_DEV_STATE", "group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979ac92ccaa16d833cac6d2f6d8c2836d886", null ], + [ "CO_SDO_AB_DATA_OD", "group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979aec1840b00621e92f27da2d0705ddab63", null ], + [ "CO_SDO_AB_NO_DATA", "group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979a3e007eeec7538b5dbe7e78240632b415", null ] + ] ], + [ "CO_SDO_return_t", "group__CO__SDOserver.html#ga7f729ab203285c7623df493916f22a73", [ + [ "CO_SDO_RT_waitingLocalTransfer", "group__CO__SDOserver.html#gga7f729ab203285c7623df493916f22a73ab9191f8a57b840a81457591f0fbd8a76", null ], + [ "CO_SDO_RT_uploadDataBufferFull", "group__CO__SDOserver.html#gga7f729ab203285c7623df493916f22a73ada069dad6b1e0bec180600b1d34758d2", null ], + [ "CO_SDO_RT_transmittBufferFull", "group__CO__SDOserver.html#gga7f729ab203285c7623df493916f22a73ad62e2421dcee78ba0477fb379a6e7e4e", null ], + [ "CO_SDO_RT_blockDownldInProgress", "group__CO__SDOserver.html#gga7f729ab203285c7623df493916f22a73aa8036db7f41e8958c057da0d4ab24f8f", null ], + [ "CO_SDO_RT_blockUploadInProgress", "group__CO__SDOserver.html#gga7f729ab203285c7623df493916f22a73ad73a50f4a1d7ef69797cbf7c930293f2", null ], + [ "CO_SDO_RT_waitingResponse", "group__CO__SDOserver.html#gga7f729ab203285c7623df493916f22a73a15d85fc411d0c6e69888c2ec9d641eb5", null ], + [ "CO_SDO_RT_ok_communicationEnd", "group__CO__SDOserver.html#gga7f729ab203285c7623df493916f22a73a2d0d1d8d1bc297205b3e87174642199c", null ], + [ "CO_SDO_RT_wrongArguments", "group__CO__SDOserver.html#gga7f729ab203285c7623df493916f22a73af1dc6a56b2b38fb5f4c878661173decc", null ], + [ "CO_SDO_RT_endedWithClientAbort", "group__CO__SDOserver.html#gga7f729ab203285c7623df493916f22a73a9aafefd96d032c1b65cb6c23bc53f0aa", null ], + [ "CO_SDO_RT_endedWithServerAbort", "group__CO__SDOserver.html#gga7f729ab203285c7623df493916f22a73ae2fce3f477766eb188502886705dc177", null ] + ] ], + [ "CO_SDOserver_init", "group__CO__SDOserver.html#gac989ba60f25fd2bc48bca6df0c0c1dde", null ], + [ "CO_SDOserver_initCallbackPre", "group__CO__SDOserver.html#ga3eeea49e2fb36da22dc754c62b03a423", null ], + [ "CO_SDOserver_process", "group__CO__SDOserver.html#ga360bc6aa1659a5572d4d1077d787433a", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/CO__SDOserver_8h_source.html b/Devices/Libraries/Systems/CANopenSocket/docs/CO__SDOserver_8h_source.html new file mode 100755 index 0000000..760d09f --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/CO__SDOserver_8h_source.html @@ -0,0 +1,418 @@ + + + + + + + +CANopenNode: 301/CO_SDOserver.h Source File + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+
CO_SDOserver.h
+
+
+Go to the documentation of this file.
1 
+
26 #ifndef CO_SDO_SERVER_H
+
27 #define CO_SDO_SERVER_H
+
28 
+
29 #include "301/CO_driver.h"
+
30 #include "301/CO_ODinterface.h"
+
31 
+
32 /* default configuration, see CO_config.h */
+
33 #ifndef CO_CONFIG_SDO_SRV
+
34 #define CO_CONFIG_SDO_SRV (CO_CONFIG_SDO_SRV_SEGMENTED)
+
35 #endif
+
36 #ifndef CO_CONFIG_SDO_SRV_BUFFER_SIZE
+
37 #define CO_CONFIG_SDO_SRV_BUFFER_SIZE 32
+
38 #endif
+
39 
+
40 #ifdef __cplusplus
+
41 extern "C" {
+
42 #endif
+
43 
+
88 #define CO_SDO_ST_FLAG_DOWNLOAD 0x10U
+
89 #define CO_SDO_ST_FLAG_UPLOAD 0x20U
+
90 #define CO_SDO_ST_FLAG_BLOCK 0x40U
+
91 
+
102 typedef enum {
+
108 CO_SDO_ST_IDLE = 0x00U,
+
114 CO_SDO_ST_ABORT = 0x01U,
+
115 
+
121 CO_SDO_ST_DOWNLOAD_LOCAL_TRANSFER = 0x10U,
+
131 CO_SDO_ST_DOWNLOAD_INITIATE_REQ = 0x11U,
+
139 CO_SDO_ST_DOWNLOAD_INITIATE_RSP = 0x12U,
+
147 CO_SDO_ST_DOWNLOAD_SEGMENT_REQ = 0x13U,
+
154 CO_SDO_ST_DOWNLOAD_SEGMENT_RSP = 0x14U,
+
155 
+
161 CO_SDO_ST_UPLOAD_LOCAL_TRANSFER = 0x20U,
+
168 CO_SDO_ST_UPLOAD_INITIATE_REQ = 0x21U,
+
179 CO_SDO_ST_UPLOAD_INITIATE_RSP = 0x22U,
+
185 CO_SDO_ST_UPLOAD_SEGMENT_REQ = 0x23U,
+
194 CO_SDO_ST_UPLOAD_SEGMENT_RSP = 0x24U,
+
195 
+
203 CO_SDO_ST_DOWNLOAD_BLK_INITIATE_REQ = 0x51U,
+
213 CO_SDO_ST_DOWNLOAD_BLK_INITIATE_RSP = 0x52U,
+
221 CO_SDO_ST_DOWNLOAD_BLK_SUBBLOCK_REQ = 0x53U,
+
236 CO_SDO_ST_DOWNLOAD_BLK_SUBBLOCK_RSP = 0x54U,
+
244 CO_SDO_ST_DOWNLOAD_BLK_END_REQ = 0x55U,
+
252 CO_SDO_ST_DOWNLOAD_BLK_END_RSP = 0x56U,
+
253 
+
265 CO_SDO_ST_UPLOAD_BLK_INITIATE_REQ = 0x61U,
+
275 CO_SDO_ST_UPLOAD_BLK_INITIATE_RSP = 0x62U,
+
281 CO_SDO_ST_UPLOAD_BLK_INITIATE_REQ2 = 0x63U,
+
289 CO_SDO_ST_UPLOAD_BLK_SUBBLOCK_SREQ = 0x64U,
+
304 CO_SDO_ST_UPLOAD_BLK_SUBBLOCK_CRSP = 0x65U,
+
312 CO_SDO_ST_UPLOAD_BLK_END_SREQ = 0x66U,
+
322 CO_SDO_ST_UPLOAD_BLK_END_CRSP = 0x67U,
+
323 } CO_SDO_state_t;
+
324 
+
325 
+
333 typedef enum {
+
335  CO_SDO_AB_NONE = 0x00000000UL,
+
337  CO_SDO_AB_TOGGLE_BIT = 0x05030000UL,
+
339  CO_SDO_AB_TIMEOUT = 0x05040000UL,
+
341  CO_SDO_AB_CMD = 0x05040001UL,
+
343  CO_SDO_AB_BLOCK_SIZE = 0x05040002UL,
+
345  CO_SDO_AB_SEQ_NUM = 0x05040003UL,
+
347  CO_SDO_AB_CRC = 0x05040004UL,
+
349  CO_SDO_AB_OUT_OF_MEM = 0x05040005UL,
+
351  CO_SDO_AB_UNSUPPORTED_ACCESS = 0x06010000UL,
+
353  CO_SDO_AB_WRITEONLY = 0x06010001UL,
+
355  CO_SDO_AB_READONLY = 0x06010002UL,
+
357  CO_SDO_AB_NOT_EXIST = 0x06020000UL,
+
359  CO_SDO_AB_NO_MAP = 0x06040041UL,
+
362  CO_SDO_AB_MAP_LEN = 0x06040042UL,
+
364  CO_SDO_AB_PRAM_INCOMPAT = 0x06040043UL,
+
366  CO_SDO_AB_DEVICE_INCOMPAT = 0x06040047UL,
+
368  CO_SDO_AB_HW = 0x06060000UL,
+
371  CO_SDO_AB_TYPE_MISMATCH = 0x06070010UL,
+
374  CO_SDO_AB_DATA_LONG = 0x06070012UL,
+
377  CO_SDO_AB_DATA_SHORT = 0x06070013UL,
+
379  CO_SDO_AB_SUB_UNKNOWN = 0x06090011UL,
+
381  CO_SDO_AB_INVALID_VALUE = 0x06090030UL,
+
383  CO_SDO_AB_VALUE_HIGH = 0x06090031UL,
+
385  CO_SDO_AB_VALUE_LOW = 0x06090032UL,
+
387  CO_SDO_AB_MAX_LESS_MIN = 0x06090036UL,
+
389  CO_SDO_AB_NO_RESOURCE = 0x060A0023UL,
+
391  CO_SDO_AB_GENERAL = 0x08000000UL,
+
393  CO_SDO_AB_DATA_TRANSF = 0x08000020UL,
+
396  CO_SDO_AB_DATA_LOC_CTRL = 0x08000021UL,
+
399  CO_SDO_AB_DATA_DEV_STATE = 0x08000022UL,
+
401  CO_SDO_AB_DATA_OD = 0x08000023UL,
+
403  CO_SDO_AB_NO_DATA = 0x08000024UL
+
404 } CO_SDO_abortCode_t;
+
405 
+
406 
+
410 typedef enum {
+
412  CO_SDO_RT_waitingLocalTransfer = 6,
+
415  CO_SDO_RT_uploadDataBufferFull = 5,
+
417  CO_SDO_RT_transmittBufferFull = 4,
+
419  CO_SDO_RT_blockDownldInProgress = 3,
+
422  CO_SDO_RT_blockUploadInProgress = 2,
+
424  CO_SDO_RT_waitingResponse = 1,
+
426  CO_SDO_RT_ok_communicationEnd = 0,
+
428  CO_SDO_RT_wrongArguments = -2,
+
430  CO_SDO_RT_endedWithClientAbort = -9,
+
432  CO_SDO_RT_endedWithServerAbort = -10,
+
433 } CO_SDO_return_t;
+
434 
+
435 
+
439 typedef struct {
+
441  CO_CANmodule_t *CANdevTx;
+
443  CO_CANtx_t *CANtxBuff;
+
445  const OD_t *OD;
+
447  uint8_t nodeId;
+
448  /* If true, SDO channel is valid */
+
449  bool_t valid;
+
451  volatile CO_SDO_state_t state;
+
453  OD_IO_t OD_IO;
+
455  uint16_t index;
+
457  uint8_t subIndex;
+
459  OD_attr_t attribute;
+
462  volatile void *CANrxNew;
+
464  uint8_t CANrxData[8];
+
465 #if ((CO_CONFIG_SDO_SRV) & CO_CONFIG_FLAG_OD_DYNAMIC) || defined CO_DOXYGEN
+
466 
+
467  CO_CANmodule_t *CANdevRx;
+
469  uint16_t CANdevRxIdx;
+
471  uint16_t CANdevTxIdx;
+
476  uint32_t COB_IDClientToServer;
+
478  uint32_t COB_IDServerToClient;
+
479 #endif
+
480 #if ((CO_CONFIG_SDO_SRV) & CO_CONFIG_SDO_SRV_SEGMENTED) || defined CO_DOXYGEN
+
481 
+
483  OD_size_t sizeInd;
+
485  OD_size_t sizeTran;
+
487  uint8_t toggle;
+
490  bool_t finished;
+
492  uint32_t SDOtimeoutTime_us;
+
494  uint32_t timeoutTimer;
+
496  char buf[CO_CONFIG_SDO_SRV_BUFFER_SIZE + 1];
+
498  OD_size_t bufOffsetWr;
+
500  OD_size_t bufOffsetRd;
+
501 #endif
+
502 #if ((CO_CONFIG_SDO_SRV) & CO_CONFIG_SDO_SRV_BLOCK) || defined CO_DOXYGEN
+
503 
+
504  uint32_t block_SDOtimeoutTime_us;
+
506  uint32_t block_timeoutTimer;
+
508  uint8_t block_seqno;
+
510  uint8_t block_blksize;
+
512  uint8_t block_noData;
+
514  bool_t block_crcEnabled;
+
516  uint16_t block_crc;
+
517 #endif
+
518 #if ((CO_CONFIG_SDO_SRV) & CO_CONFIG_FLAG_CALLBACK_PRE) || defined CO_DOXYGEN
+
519 
+
520  void (*pFunctSignalPre)(void *object);
+
522  void *functSignalObjectPre;
+
523 #endif
+
524 } CO_SDOserver_t;
+
525 
+
526 
+
548 CO_ReturnError_t CO_SDOserver_init(CO_SDOserver_t *SDO,
+
549  const OD_t *OD,
+
550  const OD_entry_t *OD_1200_SDOsrvPar,
+
551  uint8_t nodeId,
+
552  uint16_t SDOtimeoutTime_ms,
+
553  CO_CANmodule_t *CANdevRx,
+
554  uint16_t CANdevRxIdx,
+
555  CO_CANmodule_t *CANdevTx,
+
556  uint16_t CANdevTxIdx);
+
557 
+
558 
+
559 #if ((CO_CONFIG_SDO_SRV) & CO_CONFIG_FLAG_CALLBACK_PRE) || defined CO_DOXYGEN
+
560 
+
573 void CO_SDOserver_initCallbackPre(CO_SDOserver_t *SDO,
+
574  void *object,
+
575  void (*pFunctSignalPre)(void *object));
+
576 #endif
+
577 
+
578 
+
593 CO_SDO_return_t CO_SDOserver_process(CO_SDOserver_t *SDO,
+
594  bool_t NMTisPreOrOperational,
+
595  uint32_t timeDifference_us,
+
596  uint32_t *timerNext_us);
+
597  /* CO_SDOserver */
+
599 
+
600 #ifdef __cplusplus
+
601 }
+
602 #endif /*__cplusplus*/
+
603 
+
604 #endif /* CO_SDO_SERVER_H */
+
+
+
uint32_t
unsigned long int uint32_t
UNSIGNED32 in CANopen (0007h), 32-bit unsigned integer.
Definition: CO_driver.h:155
+
CO_SDOserver_t::COB_IDClientToServer
uint32_t COB_IDClientToServer
Copy of CANopen COB_ID Client -> Server, meaning of the specific bits:
Definition: CO_SDOserver.h:476
+
CO_SDOserver_t::block_noData
uint8_t block_noData
Number of bytes in last segment that do not contain data.
Definition: CO_SDOserver.h:512
+
CO_SDO_AB_CRC
@ CO_SDO_AB_CRC
0x05040004, CRC error (block mode only)
Definition: CO_SDOserver.h:347
+
CO_driver.h
Interface between CAN hardware and CANopenNode.
+
CO_SDO_AB_CMD
@ CO_SDO_AB_CMD
0x05040001, Command specifier not valid or unknown
Definition: CO_SDOserver.h:341
+
CO_SDO_ST_DOWNLOAD_BLK_END_RSP
@ CO_SDO_ST_DOWNLOAD_BLK_END_RSP
Definition: CO_SDOserver.h:252
+
CO_SDOserver_initCallbackPre
void CO_SDOserver_initCallbackPre(CO_SDOserver_t *SDO, void *object, void(*pFunctSignalPre)(void *object))
Initialize SDOrx callback function.
+
CO_SDOserver_t::functSignalObjectPre
void * functSignalObjectPre
From CO_SDOserver_initCallbackPre() or NULL.
Definition: CO_SDOserver.h:522
+
CO_SDOserver_t::timeoutTimer
uint32_t timeoutTimer
Timeout timer for SDO communication.
Definition: CO_SDOserver.h:494
+
CO_SDO_AB_NONE
@ CO_SDO_AB_NONE
0x00000000, No abort
Definition: CO_SDOserver.h:335
+
CO_SDO_RT_ok_communicationEnd
@ CO_SDO_RT_ok_communicationEnd
Success, end of communication.
Definition: CO_SDOserver.h:426
+
CO_CONFIG_SDO_SRV_BUFFER_SIZE
#define CO_CONFIG_SDO_SRV_BUFFER_SIZE
Size of the internal data buffer for the SDO server.
Definition: CO_config.h:342
+
CO_SDO_AB_MAX_LESS_MIN
@ CO_SDO_AB_MAX_LESS_MIN
0x06090036, Maximum value is less than minimum value.
Definition: CO_SDOserver.h:387
+
CO_SDOserver_t
SDO server object.
Definition: CO_SDOserver.h:439
+
CO_SDO_ST_DOWNLOAD_BLK_SUBBLOCK_RSP
@ CO_SDO_ST_DOWNLOAD_BLK_SUBBLOCK_RSP
Definition: CO_SDOserver.h:236
+
CO_SDO_AB_SEQ_NUM
@ CO_SDO_AB_SEQ_NUM
0x05040003, Invalid sequence number in block mode
Definition: CO_SDOserver.h:345
+
CO_SDO_RT_waitingResponse
@ CO_SDO_RT_waitingResponse
Waiting server or client response.
Definition: CO_SDOserver.h:424
+
CO_SDO_return_t
CO_SDO_return_t
Return values from SDO server or client functions.
Definition: CO_SDOserver.h:410
+
CO_SDO_AB_DATA_LOC_CTRL
@ CO_SDO_AB_DATA_LOC_CTRL
0x08000021, Data cannot be transferred or stored to application because of local control
Definition: CO_SDOserver.h:396
+
CO_SDO_AB_NO_DATA
@ CO_SDO_AB_NO_DATA
0x08000024, No data available
Definition: CO_SDOserver.h:403
+
CO_SDOserver_t::block_seqno
uint8_t block_seqno
Sequence number of segment in block, 1..127.
Definition: CO_SDOserver.h:508
+
uint16_t
unsigned int uint16_t
UNSIGNED16 in CANopen (0006h), 16-bit unsigned integer.
Definition: CO_driver.h:153
+
CO_SDOserver_t::index
uint16_t index
Index of the current object in Object Dictionary.
Definition: CO_SDOserver.h:455
+
CO_ReturnError_t
CO_ReturnError_t
Return values of some CANopen functions.
Definition: CO_driver.h:488
+
CO_SDO_AB_GENERAL
@ CO_SDO_AB_GENERAL
0x08000000, General error
Definition: CO_SDOserver.h:391
+
CO_SDO_AB_VALUE_HIGH
@ CO_SDO_AB_VALUE_HIGH
0x06090031, Value range of parameter written too high
Definition: CO_SDOserver.h:383
+
CO_SDOserver_t::block_blksize
uint8_t block_blksize
Number of segments per block, 1..127.
Definition: CO_SDOserver.h:510
+
CO_SDO_ST_DOWNLOAD_BLK_INITIATE_RSP
@ CO_SDO_ST_DOWNLOAD_BLK_INITIATE_RSP
Definition: CO_SDOserver.h:213
+
CO_SDO_AB_TIMEOUT
@ CO_SDO_AB_TIMEOUT
0x05040000, SDO protocol timed out
Definition: CO_SDOserver.h:339
+
CO_SDO_ST_UPLOAD_BLK_SUBBLOCK_SREQ
@ CO_SDO_ST_UPLOAD_BLK_SUBBLOCK_SREQ
Definition: CO_SDOserver.h:289
+
bool_t
unsigned char bool_t
Boolean data type for general use.
Definition: CO_driver.h:141
+
CO_SDO_ST_DOWNLOAD_BLK_SUBBLOCK_REQ
@ CO_SDO_ST_DOWNLOAD_BLK_SUBBLOCK_REQ
Definition: CO_SDOserver.h:221
+
CO_SDO_RT_wrongArguments
@ CO_SDO_RT_wrongArguments
Error in arguments.
Definition: CO_SDOserver.h:428
+
CO_SDO_AB_UNSUPPORTED_ACCESS
@ CO_SDO_AB_UNSUPPORTED_ACCESS
0x06010000, Unsupported access to an object
Definition: CO_SDOserver.h:351
+
CO_SDOserver_t::finished
bool_t finished
If true, then: data transfer is finished (by download) or read from OD variable is finished (by uploa...
Definition: CO_SDOserver.h:490
+
CO_SDO_ST_UPLOAD_BLK_INITIATE_RSP
@ CO_SDO_ST_UPLOAD_BLK_INITIATE_RSP
Definition: CO_SDOserver.h:275
+
CO_SDO_AB_READONLY
@ CO_SDO_AB_READONLY
0x06010002, Attempt to write a read only object
Definition: CO_SDOserver.h:355
+
CO_SDO_ST_DOWNLOAD_SEGMENT_REQ
@ CO_SDO_ST_DOWNLOAD_SEGMENT_REQ
Definition: CO_SDOserver.h:147
+
CO_SDO_AB_DATA_DEV_STATE
@ CO_SDO_AB_DATA_DEV_STATE
0x08000022, Data cannot be transferred or stored to application because of present device state
Definition: CO_SDOserver.h:399
+
CO_SDOserver_t::CANrxNew
volatile void * CANrxNew
Indicates, if new SDO message received from CAN bus.
Definition: CO_SDOserver.h:462
+
CO_SDOserver_t::OD_IO
OD_IO_t OD_IO
Object dictionary interface for current object.
Definition: CO_SDOserver.h:453
+
CO_SDO_ST_DOWNLOAD_INITIATE_REQ
@ CO_SDO_ST_DOWNLOAD_INITIATE_REQ
Definition: CO_SDOserver.h:131
+
CO_SDOserver_t::toggle
uint8_t toggle
Toggle bit toggled in each segment in segmented transfer.
Definition: CO_SDOserver.h:487
+
CO_SDO_ST_DOWNLOAD_BLK_END_REQ
@ CO_SDO_ST_DOWNLOAD_BLK_END_REQ
Definition: CO_SDOserver.h:244
+
CO_SDO_AB_TYPE_MISMATCH
@ CO_SDO_AB_TYPE_MISMATCH
0x06070010, Data type does not match, length of service parameter does not match
Definition: CO_SDOserver.h:371
+
CO_SDOserver_t::sizeTran
OD_size_t sizeTran
Size of data which is actually transferred.
Definition: CO_SDOserver.h:485
+
CO_SDOserver_t::OD
const OD_t * OD
From CO_SDOserver_init()
Definition: CO_SDOserver.h:445
+
CO_SDO_AB_DEVICE_INCOMPAT
@ CO_SDO_AB_DEVICE_INCOMPAT
0x06040047, General internal incompatibility in device
Definition: CO_SDOserver.h:366
+
CO_SDO_ST_DOWNLOAD_SEGMENT_RSP
@ CO_SDO_ST_DOWNLOAD_SEGMENT_RSP
Definition: CO_SDOserver.h:154
+
CO_SDO_AB_NO_RESOURCE
@ CO_SDO_AB_NO_RESOURCE
0x060A0023, Resource not available: SDO connection
Definition: CO_SDOserver.h:389
+
CO_SDO_AB_NOT_EXIST
@ CO_SDO_AB_NOT_EXIST
0x06020000, Object does not exist in the object dictionary
Definition: CO_SDOserver.h:357
+
CO_SDO_AB_PRAM_INCOMPAT
@ CO_SDO_AB_PRAM_INCOMPAT
0x06040043, General parameter incompatibility reasons
Definition: CO_SDOserver.h:364
+
CO_SDO_AB_INVALID_VALUE
@ CO_SDO_AB_INVALID_VALUE
0x06090030, Invalid value for parameter (download only).
Definition: CO_SDOserver.h:381
+
CO_SDO_AB_DATA_SHORT
@ CO_SDO_AB_DATA_SHORT
0x06070013, Data type does not match, length of service parameter too short
Definition: CO_SDOserver.h:377
+
CO_SDO_RT_blockDownldInProgress
@ CO_SDO_RT_blockDownldInProgress
Block download is in progress.
Definition: CO_SDOserver.h:419
+
CO_SDO_AB_VALUE_LOW
@ CO_SDO_AB_VALUE_LOW
0x06090032, Value range of parameter written too low
Definition: CO_SDOserver.h:385
+
CO_SDO_RT_uploadDataBufferFull
@ CO_SDO_RT_uploadDataBufferFull
Data buffer is full.
Definition: CO_SDOserver.h:415
+
CO_SDOserver_t::bufOffsetRd
OD_size_t bufOffsetRd
Offset of first data available for read in the buffer.
Definition: CO_SDOserver.h:500
+
CO_SDOserver_t::state
volatile CO_SDO_state_t state
Internal state of the SDO server.
Definition: CO_SDOserver.h:451
+
CO_SDOserver_t::attribute
OD_attr_t attribute
Attribute bit-field of the current OD sub-object, see OD_attributes_t.
Definition: CO_SDOserver.h:459
+
CO_SDO_AB_BLOCK_SIZE
@ CO_SDO_AB_BLOCK_SIZE
0x05040002, Invalid block size in block mode
Definition: CO_SDOserver.h:343
+
CO_SDOserver_init
CO_ReturnError_t CO_SDOserver_init(CO_SDOserver_t *SDO, const OD_t *OD, const OD_entry_t *OD_1200_SDOsrvPar, uint8_t nodeId, uint16_t SDOtimeoutTime_ms, CO_CANmodule_t *CANdevRx, uint16_t CANdevRxIdx, CO_CANmodule_t *CANdevTx, uint16_t CANdevTxIdx)
Initialize SDO object.
Definition: CO_SDOserver.c:357
+
CO_SDO_RT_blockUploadInProgress
@ CO_SDO_RT_blockUploadInProgress
Block upload is in progress.
Definition: CO_SDOserver.h:422
+
CO_SDOserver_t::CANdevTxIdx
uint16_t CANdevTxIdx
From CO_SDOserver_init()
Definition: CO_SDOserver.h:471
+
CO_SDO_AB_DATA_LONG
@ CO_SDO_AB_DATA_LONG
0x06070012, Data type does not match, length of service parameter too high
Definition: CO_SDOserver.h:374
+
CO_SDO_AB_WRITEONLY
@ CO_SDO_AB_WRITEONLY
0x06010001, Attempt to read a write only object
Definition: CO_SDOserver.h:353
+
CO_SDO_ST_UPLOAD_BLK_END_CRSP
@ CO_SDO_ST_UPLOAD_BLK_END_CRSP
Definition: CO_SDOserver.h:322
+
CO_SDO_ST_UPLOAD_BLK_INITIATE_REQ2
@ CO_SDO_ST_UPLOAD_BLK_INITIATE_REQ2
Definition: CO_SDOserver.h:281
+
CO_SDO_ST_UPLOAD_BLK_INITIATE_REQ
@ CO_SDO_ST_UPLOAD_BLK_INITIATE_REQ
Definition: CO_SDOserver.h:265
+
CO_SDOserver_t::CANdevTx
CO_CANmodule_t * CANdevTx
From CO_SDOserver_init()
Definition: CO_SDOserver.h:441
+
CO_SDO_ST_UPLOAD_INITIATE_RSP
@ CO_SDO_ST_UPLOAD_INITIATE_RSP
Definition: CO_SDOserver.h:179
+
CO_SDO_abortCode_t
CO_SDO_abortCode_t
SDO abort codes.
Definition: CO_SDOserver.h:333
+
CO_SDO_AB_NO_MAP
@ CO_SDO_AB_NO_MAP
0x06040041, Object cannot be mapped to the PDO
Definition: CO_SDOserver.h:359
+
CO_SDO_RT_endedWithClientAbort
@ CO_SDO_RT_endedWithClientAbort
Communication ended with client abort.
Definition: CO_SDOserver.h:430
+
CO_SDOserver_t::CANdevRx
CO_CANmodule_t * CANdevRx
From CO_SDOserver_init()
Definition: CO_SDOserver.h:467
+
CO_SDO_ST_IDLE
@ CO_SDO_ST_IDLE
Definition: CO_SDOserver.h:108
+
CO_SDO_RT_endedWithServerAbort
@ CO_SDO_RT_endedWithServerAbort
Communication ended with server abort.
Definition: CO_SDOserver.h:432
+
CO_SDO_state_t
CO_SDO_state_t
Internal states of the SDO state machine.
Definition: CO_SDOserver.h:102
+
CO_SDOserver_t::SDOtimeoutTime_us
uint32_t SDOtimeoutTime_us
Maximum timeout time between request and response in microseconds.
Definition: CO_SDOserver.h:492
+
CO_SDO_AB_HW
@ CO_SDO_AB_HW
0x06060000, Access failed due to hardware error
Definition: CO_SDOserver.h:368
+
CO_SDO_ST_UPLOAD_SEGMENT_REQ
@ CO_SDO_ST_UPLOAD_SEGMENT_REQ
Definition: CO_SDOserver.h:185
+
CO_SDO_ST_DOWNLOAD_LOCAL_TRANSFER
@ CO_SDO_ST_DOWNLOAD_LOCAL_TRANSFER
Definition: CO_SDOserver.h:121
+
CO_SDOserver_t::CANdevRxIdx
uint16_t CANdevRxIdx
From CO_SDOserver_init()
Definition: CO_SDOserver.h:469
+
CO_SDO_ST_UPLOAD_INITIATE_REQ
@ CO_SDO_ST_UPLOAD_INITIATE_REQ
Definition: CO_SDOserver.h:168
+
CO_ODinterface.h
CANopen Object Dictionary interface.
+
CO_SDOserver_t::block_crc
uint16_t block_crc
Calculated CRC checksum.
Definition: CO_SDOserver.h:516
+
CO_SDO_ST_UPLOAD_SEGMENT_RSP
@ CO_SDO_ST_UPLOAD_SEGMENT_RSP
Definition: CO_SDOserver.h:194
+
CO_SDO_AB_OUT_OF_MEM
@ CO_SDO_AB_OUT_OF_MEM
0x05040005, Out of memory
Definition: CO_SDOserver.h:349
+
CO_SDOserver_t::nodeId
uint8_t nodeId
From CO_SDOserver_init()
Definition: CO_SDOserver.h:447
+
CO_SDO_AB_DATA_OD
@ CO_SDO_AB_DATA_OD
0x08000023, Object dictionary not present or dynamic generation fails
Definition: CO_SDOserver.h:401
+
OD_t
Object Dictionary.
Definition: CO_ODinterface.h:352
+
CO_SDO_ST_UPLOAD_BLK_END_SREQ
@ CO_SDO_ST_UPLOAD_BLK_END_SREQ
Definition: CO_SDOserver.h:312
+
CO_CANmodule_t
Complete CAN module object.
Definition: CO_driver.h:319
+
CO_SDO_AB_DATA_TRANSF
@ CO_SDO_AB_DATA_TRANSF
0x08000020, Data cannot be transferred or stored to application
Definition: CO_SDOserver.h:393
+
CO_SDOserver_t::COB_IDServerToClient
uint32_t COB_IDServerToClient
Copy of CANopen COB_ID Server -> Client, similar as above.
Definition: CO_SDOserver.h:478
+
CO_SDOserver_t::block_SDOtimeoutTime_us
uint32_t block_SDOtimeoutTime_us
Timeout time for SDO sub-block download, half of SDOtimeoutTime_us.
Definition: CO_SDOserver.h:504
+
OD_IO_t
Structure for input / output on the OD variable.
Definition: CO_ODinterface.h:263
+
CO_SDO_ST_UPLOAD_LOCAL_TRANSFER
@ CO_SDO_ST_UPLOAD_LOCAL_TRANSFER
Definition: CO_SDOserver.h:161
+
CO_SDOserver_t::subIndex
uint8_t subIndex
Subindex of the current object in Object Dictionary.
Definition: CO_SDOserver.h:457
+
CO_SDOserver_t::sizeInd
OD_size_t sizeInd
Size of data, which will be transferred.
Definition: CO_SDOserver.h:483
+
OD_entry_t
Object Dictionary entry for one OD object.
Definition: CO_ODinterface.h:336
+
CO_SDO_ST_UPLOAD_BLK_SUBBLOCK_CRSP
@ CO_SDO_ST_UPLOAD_BLK_SUBBLOCK_CRSP
Definition: CO_SDOserver.h:304
+
CO_SDO_ST_DOWNLOAD_INITIATE_RSP
@ CO_SDO_ST_DOWNLOAD_INITIATE_RSP
Definition: CO_SDOserver.h:139
+
CO_SDOserver_t::block_crcEnabled
bool_t block_crcEnabled
Client CRC support in block transfer.
Definition: CO_SDOserver.h:514
+
CO_SDO_AB_SUB_UNKNOWN
@ CO_SDO_AB_SUB_UNKNOWN
0x06090011, Sub index does not exist
Definition: CO_SDOserver.h:379
+
CO_SDO_RT_waitingLocalTransfer
@ CO_SDO_RT_waitingLocalTransfer
Waiting in client local transfer.
Definition: CO_SDOserver.h:412
+
CO_SDOserver_process
CO_SDO_return_t CO_SDOserver_process(CO_SDOserver_t *SDO, bool_t NMTisPreOrOperational, uint32_t timeDifference_us, uint32_t *timerNext_us)
Process SDO communication.
Definition: CO_SDOserver.c:723
+
CO_SDO_AB_TOGGLE_BIT
@ CO_SDO_AB_TOGGLE_BIT
0x05030000, Toggle bit not altered
Definition: CO_SDOserver.h:337
+
CO_SDOserver_t::block_timeoutTimer
uint32_t block_timeoutTimer
Timeout timer for SDO sub-block download.
Definition: CO_SDOserver.h:506
+
CO_SDOserver_t::CANtxBuff
CO_CANtx_t * CANtxBuff
CAN transmit buffer inside CANdevTx for CAN tx message.
Definition: CO_SDOserver.h:443
+
CO_SDOserver_t::bufOffsetWr
OD_size_t bufOffsetWr
Offset of next free data byte available for write in the buffer.
Definition: CO_SDOserver.h:498
+
CO_SDO_ST_ABORT
@ CO_SDO_ST_ABORT
Definition: CO_SDOserver.h:114
+
CO_CANtx_t
Configuration object for CAN transmit message for specific CANopenNode Object.
Definition: CO_driver.h:299
+
CO_SDO_ST_DOWNLOAD_BLK_INITIATE_REQ
@ CO_SDO_ST_DOWNLOAD_BLK_INITIATE_REQ
Definition: CO_SDOserver.h:203
+
uint8_t
unsigned char uint8_t
UNSIGNED8 in CANopen (0005h), 8-bit unsigned integer.
Definition: CO_driver.h:151
+
OD_size_t
#define OD_size_t
Variable of type OD_size_t contains data length in bytes of OD variable.
Definition: CO_ODinterface.h:44
+
CO_SDO_RT_transmittBufferFull
@ CO_SDO_RT_transmittBufferFull
CAN transmit buffer is full.
Definition: CO_SDOserver.h:417
+
OD_attr_t
#define OD_attr_t
Size of Object Dictionary attribute.
Definition: CO_ODinterface.h:50
+
CO_SDO_AB_MAP_LEN
@ CO_SDO_AB_MAP_LEN
0x06040042, Number and length of object to be mapped exceeds PDO length
Definition: CO_SDOserver.h:362
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/CO__SRDO_8c.html b/Devices/Libraries/Systems/CANopenSocket/docs/CO__SRDO_8c.html new file mode 100755 index 0000000..6aa2b29 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/CO__SRDO_8c.html @@ -0,0 +1,121 @@ + + + + + + + +CANopenNode: 304/CO_SRDO.c File Reference + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+
CO_SRDO.c File Reference
CANopen_304 » SRDO
+
+
+ +

CANopen Safety Related Data Object protocol. +More...

+
#include "304/CO_SRDO.h"
+

Detailed Description

+

CANopen Safety Related Data Object protocol.

+
Author
Robert Grüning
+
Copyright
2020 - 2020 Robert Grüning
+

This file is part of CANopenNode, an opensource CANopen Stack. Project home page is https://github.com/CANopenNode/CANopenNode. For more information on CANopen see http://www.can-cia.org/.

+

Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. You may obtain a copy of the License at 

http://www.apache.org/licenses/LICENSE-2.0
+

Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License.

+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/CO__SRDO_8h.html b/Devices/Libraries/Systems/CANopenSocket/docs/CO__SRDO_8h.html new file mode 100755 index 0000000..bd1bf4b --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/CO__SRDO_8h.html @@ -0,0 +1,166 @@ + + + + + + + +CANopenNode: 304/CO_SRDO.h File Reference + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Data Structures | +Functions
+
+
CO_SRDO.h File Reference
CANopen_304 » SRDO
+
+
+ +

CANopen Safety Related Data Object protocol. +More...

+
#include "301/CO_driver.h"
+#include "301/CO_SDOserver.h"
+#include "301/CO_Emergency.h"
+#include "301/CO_NMT_Heartbeat.h"
+
+

Go to the source code of this file.

+ + + + + + + + + + + +

+Data Structures

struct  CO_SRDOCommPar_t
 SRDO communication parameter. More...
 
struct  CO_SRDOGuard_t
 Gurad Object for SRDO monitors: More...
 
struct  CO_SRDO_t
 SRDO object. More...
 
+ + + + + + + + + + + + + + + + + + + + + + +

+Functions

CO_ReturnError_t CO_SRDOGuard_init (CO_SRDOGuard_t *SRDOGuard, CO_SDO_t *SDO, CO_NMT_internalState_t *operatingState, uint8_t *configurationValid, uint16_t idx_SRDOvalid, uint16_t idx_SRDOcrc)
 Initialize SRDOGuard object. More...
 
uint8_t CO_SRDOGuard_process (CO_SRDOGuard_t *SRDOGuard)
 Process operation and valid state changes. More...
 
CO_ReturnError_t CO_SRDO_init (CO_SRDO_t *SRDO, CO_SRDOGuard_t *SRDOGuard, CO_EM_t *em, CO_SDO_t *SDO, uint8_t nodeId, uint16_t defaultCOB_ID, const CO_SRDOCommPar_t *SRDOCommPar, const CO_SRDOMapPar_t *SRDOMapPar, const uint16_t *checksum, uint16_t idx_SRDOCommPar, uint16_t idx_SRDOMapPar, CO_CANmodule_t *CANdevRx, uint16_t CANdevRxIdxNormal, uint16_t CANdevRxIdxInverted, CO_CANmodule_t *CANdevTx, uint16_t CANdevTxIdxNormal, uint16_t CANdevTxIdxInverted)
 Initialize SRDO object. More...
 
void CO_SRDO_initCallbackPre (CO_SRDO_t *SRDO, void *object, void(*pFunctSignalPre)(void *object))
 Initialize SRDO callback function. More...
 
void CO_SRDO_initCallbackEnterSafeState (CO_SRDO_t *SRDO, void *object, void(*pFunctSignalSafe)(void *object))
 Initialize SRDO callback function. More...
 
CO_ReturnError_t CO_SRDO_requestSend (CO_SRDO_t *SRDO)
 Send SRDO on event. More...
 
void CO_SRDO_process (CO_SRDO_t *SRDO, uint8_t commands, uint32_t timeDifference_us, uint32_t *timerNext_us)
 Process transmitting/receiving SRDO messages. More...
 
+

Detailed Description

+

CANopen Safety Related Data Object protocol.

+
Author
Robert Grüning
+
Copyright
2020 - 2020 Robert Grüning
+

This file is part of CANopenNode, an opensource CANopen Stack. Project home page is https://github.com/CANopenNode/CANopenNode. For more information on CANopen see http://www.can-cia.org/.

+

Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. You may obtain a copy of the License at 

http://www.apache.org/licenses/LICENSE-2.0
+

Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License.

+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/CO__SRDO_8h.js b/Devices/Libraries/Systems/CANopenSocket/docs/CO__SRDO_8h.js new file mode 100755 index 0000000..46c6330 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/CO__SRDO_8h.js @@ -0,0 +1,10 @@ +var CO__SRDO_8h = +[ + [ "CO_SRDOGuard_init", "group__CO__SRDO.html#gacd578e8a5a4af024f8e6f8aef87cbd14", null ], + [ "CO_SRDOGuard_process", "group__CO__SRDO.html#ga0201fa4da8b37a18f864a9fd7c826a6c", null ], + [ "CO_SRDO_init", "group__CO__SRDO.html#ga608e6d48e97f1b4da316b36f10e389c6", null ], + [ "CO_SRDO_initCallbackPre", "group__CO__SRDO.html#gac066166b35bcb3d0b01adfc0e2eb9bb1", null ], + [ "CO_SRDO_initCallbackEnterSafeState", "group__CO__SRDO.html#ga5303601f6f94c83530b5e165f54b54bb", null ], + [ "CO_SRDO_requestSend", "group__CO__SRDO.html#gac9a21725c4bb0373ea18a414019cf339", null ], + [ "CO_SRDO_process", "group__CO__SRDO.html#gacb94aa4f279a4476c193ee50c408dfbb", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/CO__SRDO_8h_source.html b/Devices/Libraries/Systems/CANopenSocket/docs/CO__SRDO_8h_source.html new file mode 100755 index 0000000..9be9f12 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/CO__SRDO_8h_source.html @@ -0,0 +1,295 @@ + + + + + + + +CANopenNode: 304/CO_SRDO.h Source File + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+
CO_SRDO.h
+
+
+Go to the documentation of this file.
1 
+
26 #ifndef CO_SRDO_H
+
27 #define CO_SRDO_H
+
28 
+
29 #include "301/CO_driver.h"
+
30 #include "301/CO_SDOserver.h"
+
31 #include "301/CO_Emergency.h"
+
32 #include "301/CO_NMT_Heartbeat.h"
+
33 
+
34 /* default configuration, see CO_config.h */
+
35 #ifndef CO_CONFIG_SRDO
+
36 #define CO_CONFIG_SRDO (0)
+
37 #endif
+
38 #ifndef CO_CONFIG_SRDO_MINIMUM_DELAY
+
39 #define CO_CONFIG_SRDO_MINIMUM_DELAY 0
+
40 #endif
+
41 
+
42 #if ((CO_CONFIG_SRDO) & CO_CONFIG_SRDO_ENABLE) || defined CO_DOXYGEN
+
43 
+
44 #ifdef __cplusplus
+
45 extern "C" {
+
46 #endif
+
47 
+
66 typedef struct{
+
67  uint8_t maxSubIndex;
+
72  uint8_t informationDirection;
+
76  uint16_t safetyCycleTime;
+
80  uint8_t safetyRelatedValidationTime;
+
83  uint8_t transmissionType;
+
87  uint32_t COB_ID1_normal;
+
91  uint32_t COB_ID2_inverted;
+
92 }CO_SRDOCommPar_t;
+
93 
+
94 
+
95 typedef struct{
+
98  uint8_t numberOfMappedObjects;
+
104  uint32_t mappedObjects[16];
+
105 }CO_SRDOMapPar_t;
+
106 
+
114 typedef struct{
+
115  CO_NMT_internalState_t *operatingState;
+
116  CO_NMT_internalState_t operatingStatePrev;
+
117  uint8_t *configurationValid;
+
118  uint8_t checkCRC;
+
119 }CO_SRDOGuard_t;
+
120 
+
124 typedef struct{
+
125  CO_EM_t *em;
+
126  CO_SDO_t *SDO;
+
127  CO_SRDOGuard_t *SRDOGuard;
+
129  uint8_t *mapPointer[2][8];
+
131  uint8_t dataLength;
+
132  uint8_t nodeId;
+
133  uint16_t defaultCOB_ID[2];
+
135  uint8_t valid;
+
136  const CO_SRDOCommPar_t *SRDOCommPar;
+
137  const CO_SRDOMapPar_t *SRDOMapPar;
+
138  const uint16_t *checksum;
+
139  CO_CANmodule_t *CANdevRx;
+
140  CO_CANmodule_t *CANdevTx;
+
141  CO_CANtx_t *CANtxBuff[2];
+
142  uint16_t CANdevRxIdx[2];
+
143  uint16_t CANdevTxIdx[2];
+
144  uint8_t toogle;
+
145  uint32_t timer;
+
147  volatile void *CANrxNew[2];
+
149  uint8_t CANrxData[2][8];
+
151  void (*pFunctSignalSafe)(void *object);
+
153  void *functSignalObjectSafe;
+
154 #if ((CO_CONFIG_SRDO) & CO_CONFIG_FLAG_CALLBACK_PRE) || defined CO_DOXYGEN
+
155 
+
156  void (*pFunctSignalPre)(void *object);
+
158  void *functSignalObjectPre;
+
159 #endif
+
160 }CO_SRDO_t;
+
161 
+
176 CO_ReturnError_t CO_SRDOGuard_init(
+
177  CO_SRDOGuard_t *SRDOGuard,
+
178  CO_SDO_t *SDO,
+
179  CO_NMT_internalState_t *operatingState,
+
180  uint8_t *configurationValid,
+
181  uint16_t idx_SRDOvalid,
+
182  uint16_t idx_SRDOcrc);
+
183 
+
192 uint8_t CO_SRDOGuard_process(
+
193  CO_SRDOGuard_t *SRDOGuard);
+
194 
+
222 CO_ReturnError_t CO_SRDO_init(
+
223  CO_SRDO_t *SRDO,
+
224  CO_SRDOGuard_t *SRDOGuard,
+
225  CO_EM_t *em,
+
226  CO_SDO_t *SDO,
+
227  uint8_t nodeId,
+
228  uint16_t defaultCOB_ID,
+
229  const CO_SRDOCommPar_t *SRDOCommPar,
+
230  const CO_SRDOMapPar_t *SRDOMapPar,
+
231  const uint16_t *checksum,
+
232  uint16_t idx_SRDOCommPar,
+
233  uint16_t idx_SRDOMapPar,
+
234  CO_CANmodule_t *CANdevRx,
+
235  uint16_t CANdevRxIdxNormal,
+
236  uint16_t CANdevRxIdxInverted,
+
237  CO_CANmodule_t *CANdevTx,
+
238  uint16_t CANdevTxIdxNormal,
+
239  uint16_t CANdevTxIdxInverted);
+
240 
+
241 #if ((CO_CONFIG_SRDO) & CO_CONFIG_FLAG_CALLBACK_PRE) || defined CO_DOXYGEN
+
242 
+
253 void CO_SRDO_initCallbackPre(
+
254  CO_SRDO_t *SRDO,
+
255  void *object,
+
256  void (*pFunctSignalPre)(void *object));
+
257 #endif
+
258 
+
271 void CO_SRDO_initCallbackEnterSafeState(
+
272  CO_SRDO_t *SRDO,
+
273  void *object,
+
274  void (*pFunctSignalSafe)(void *object));
+
275 
+
276 
+
286 CO_ReturnError_t CO_SRDO_requestSend(
+
287  CO_SRDO_t *SRDO);
+
288 
+
300 void CO_SRDO_process(
+
301  CO_SRDO_t *SRDO,
+
302  uint8_t commands,
+
303  uint32_t timeDifference_us,
+
304  uint32_t *timerNext_us);
+
305  /* CO_SRDO */
+
307 
+
308 #ifdef __cplusplus
+
309 }
+
310 #endif /*__cplusplus*/
+
311 
+
312 #endif /* (CO_CONFIG_SRDO) & CO_CONFIG_SRDO_ENABLE */
+
313 
+
314 #endif /* CO_SRDO_H */
+
+
+
CO_SRDOGuard_init
CO_ReturnError_t CO_SRDOGuard_init(CO_SRDOGuard_t *SRDOGuard, CO_SDO_t *SDO, CO_NMT_internalState_t *operatingState, uint8_t *configurationValid, uint16_t idx_SRDOvalid, uint16_t idx_SRDOcrc)
Initialize SRDOGuard object.
+
uint32_t
unsigned long int uint32_t
UNSIGNED32 in CANopen (0007h), 32-bit unsigned integer.
Definition: CO_driver.h:155
+
CO_driver.h
Interface between CAN hardware and CANopenNode.
+
CO_SRDO_t::dataLength
uint8_t dataLength
Data length of the received SRDO message.
Definition: CO_SRDO.h:131
+
CO_SRDO_t::nodeId
uint8_t nodeId
From CO_SRDO_init()
Definition: CO_SRDO.h:132
+
CO_SRDO_t::SDO
CO_SDO_t * SDO
From CO_SRDO_init()
Definition: CO_SRDO.h:126
+
uint16_t
unsigned int uint16_t
UNSIGNED16 in CANopen (0006h), 16-bit unsigned integer.
Definition: CO_driver.h:153
+
CO_SRDOGuard_t::operatingStatePrev
CO_NMT_internalState_t operatingStatePrev
last operation state
Definition: CO_SRDO.h:116
+
CO_ReturnError_t
CO_ReturnError_t
Return values of some CANopen functions.
Definition: CO_driver.h:488
+
CO_SRDO_t::functSignalObjectPre
void * functSignalObjectPre
From CO_SRDO_initCallbackPre() or NULL.
Definition: CO_SRDO.h:158
+
CO_SRDOCommPar_t
SRDO communication parameter.
Definition: CO_SRDO.h:66
+
CO_SRDOCommPar_t::informationDirection
uint8_t informationDirection
Direction of the SRDO.
Definition: CO_SRDO.h:72
+
CO_SRDO_t::functSignalObjectSafe
void * functSignalObjectSafe
From CO_SRDO_initCallbackEnterSafeState() or NULL.
Definition: CO_SRDO.h:153
+
CO_SDOserver.h
CANopen Service Data Object - server protocol.
+
CO_SRDO_t::SRDOGuard
CO_SRDOGuard_t * SRDOGuard
From CO_SRDO_init()
Definition: CO_SRDO.h:127
+
CO_SRDO_t::CANdevTx
CO_CANmodule_t * CANdevTx
From CO_SRDO_init()
Definition: CO_SRDO.h:140
+
CO_SRDOCommPar_t::safetyRelatedValidationTime
uint8_t safetyRelatedValidationTime
SRVT.
Definition: CO_SRDO.h:80
+
CO_SRDO_t::timer
uint32_t timer
transmit timer and receive timeout
Definition: CO_SRDO.h:145
+
CO_SRDO_t::em
CO_EM_t * em
From CO_SRDO_init()
Definition: CO_SRDO.h:125
+
CO_SRDO_t::valid
uint8_t valid
0 - invalid, 1 - tx, 2 - rx
Definition: CO_SRDO.h:135
+
CO_EM_t
Emergency object.
Definition: CO_Emergency.h:369
+
CO_SRDOCommPar_t::safetyCycleTime
uint16_t safetyCycleTime
Refresh-time / SCT.
Definition: CO_SRDO.h:76
+
CO_SRDO_init
CO_ReturnError_t CO_SRDO_init(CO_SRDO_t *SRDO, CO_SRDOGuard_t *SRDOGuard, CO_EM_t *em, CO_SDO_t *SDO, uint8_t nodeId, uint16_t defaultCOB_ID, const CO_SRDOCommPar_t *SRDOCommPar, const CO_SRDOMapPar_t *SRDOMapPar, const uint16_t *checksum, uint16_t idx_SRDOCommPar, uint16_t idx_SRDOMapPar, CO_CANmodule_t *CANdevRx, uint16_t CANdevRxIdxNormal, uint16_t CANdevRxIdxInverted, CO_CANmodule_t *CANdevTx, uint16_t CANdevTxIdxNormal, uint16_t CANdevTxIdxInverted)
Initialize SRDO object.
+
CO_NMT_internalState_t
CO_NMT_internalState_t
Internal network state of the CANopen node.
Definition: CO_NMT_Heartbeat.h:77
+
CO_SRDO_process
void CO_SRDO_process(CO_SRDO_t *SRDO, uint8_t commands, uint32_t timeDifference_us, uint32_t *timerNext_us)
Process transmitting/receiving SRDO messages.
+
CO_NMT_Heartbeat.h
CANopen Network management and Heartbeat producer protocol.
+
CO_SRDOCommPar_t::maxSubIndex
uint8_t maxSubIndex
Equal to 6.
Definition: CO_SRDO.h:67
+
CO_SRDOCommPar_t::transmissionType
uint8_t transmissionType
Transmission type.
Definition: CO_SRDO.h:83
+
CO_SRDO_t::toogle
uint8_t toogle
defines the current state
Definition: CO_SRDO.h:144
+
CO_Emergency.h
CANopen Emergency protocol.
+
CO_SRDOCommPar_t::COB_ID2_inverted
uint32_t COB_ID2_inverted
Communication object identifier for message received.
Definition: CO_SRDO.h:91
+
CO_SRDO_t::SRDOMapPar
const CO_SRDOMapPar_t * SRDOMapPar
From CO_SRDO_init()
Definition: CO_SRDO.h:137
+
CO_SRDO_t::CANdevRx
CO_CANmodule_t * CANdevRx
From CO_SRDO_init()
Definition: CO_SRDO.h:139
+
CO_SRDO_t::SRDOCommPar
const CO_SRDOCommPar_t * SRDOCommPar
From CO_SRDO_init()
Definition: CO_SRDO.h:136
+
CO_CANmodule_t
Complete CAN module object.
Definition: CO_driver.h:319
+
CO_SRDO_initCallbackPre
void CO_SRDO_initCallbackPre(CO_SRDO_t *SRDO, void *object, void(*pFunctSignalPre)(void *object))
Initialize SRDO callback function.
+
CO_SRDOGuard_process
uint8_t CO_SRDOGuard_process(CO_SRDOGuard_t *SRDOGuard)
Process operation and valid state changes.
+
CO_SRDOGuard_t::operatingState
CO_NMT_internalState_t * operatingState
pointer to current operation state
Definition: CO_SRDO.h:115
+
CO_SRDO_t
SRDO object.
Definition: CO_SRDO.h:124
+
CO_SRDOCommPar_t::COB_ID1_normal
uint32_t COB_ID1_normal
Communication object identifier for message received.
Definition: CO_SRDO.h:87
+
CO_SRDOGuard_t
Gurad Object for SRDO monitors:
Definition: CO_SRDO.h:114
+
CO_SRDO_t::checksum
const uint16_t * checksum
From CO_SRDO_init()
Definition: CO_SRDO.h:138
+
CO_SRDO_initCallbackEnterSafeState
void CO_SRDO_initCallbackEnterSafeState(CO_SRDO_t *SRDO, void *object, void(*pFunctSignalSafe)(void *object))
Initialize SRDO callback function.
+
CO_SRDO_requestSend
CO_ReturnError_t CO_SRDO_requestSend(CO_SRDO_t *SRDO)
Send SRDO on event.
+
CO_SRDOGuard_t::checkCRC
uint8_t checkCRC
specifies whether a CRC check should be performed
Definition: CO_SRDO.h:118
+
CO_CANtx_t
Configuration object for CAN transmit message for specific CANopenNode Object.
Definition: CO_driver.h:299
+
CO_SRDOGuard_t::configurationValid
uint8_t * configurationValid
pointer to the configuration valid flag in OD
Definition: CO_SRDO.h:117
+
uint8_t
unsigned char uint8_t
UNSIGNED8 in CANopen (0005h), 8-bit unsigned integer.
Definition: CO_driver.h:151
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/CO__SYNC_8h.html b/Devices/Libraries/Systems/CANopenSocket/docs/CO__SYNC_8h.html new file mode 100755 index 0000000..30e6326 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/CO__SYNC_8h.html @@ -0,0 +1,158 @@ + + + + + + + +CANopenNode: 301/CO_SYNC.h File Reference + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Data Structures | +Enumerations | +Functions
+
+
CO_SYNC.h File Reference
CANopen_301 » SYNC
+
+
+ +

CANopen Synchronisation protocol. +More...

+
#include "301/CO_driver.h"
+
+

Go to the source code of this file.

+ + + + + +

+Data Structures

struct  CO_SYNC_t
 SYNC producer and consumer object. More...
 
+ + + + +

+Enumerations

enum  CO_SYNC_status_t { CO_SYNC_NONE = 0, +CO_SYNC_RECEIVED = 1, +CO_SYNC_OUTSIDE_WINDOW = 2 + }
 Return value for CO_SYNC_process. More...
 
+ + + + + + + + + + + + + +

+Functions

CO_ReturnError_t CO_SYNC_init (CO_SYNC_t *SYNC, CO_EM_t *em, CO_SDO_t *SDO, CO_NMT_internalState_t *operatingState, uint32_t COB_ID_SYNCMessage, uint32_t communicationCyclePeriod, uint8_t synchronousCounterOverflowValue, CO_CANmodule_t *CANdevRx, uint16_t CANdevRxIdx, CO_CANmodule_t *CANdevTx, uint16_t CANdevTxIdx)
 Initialize SYNC object. More...
 
void CO_SYNC_initCallbackPre (CO_SYNC_t *SYNC, void *object, void(*pFunctSignalPre)(void *object))
 Initialize SYNC callback function. More...
 
CO_ReturnError_t CO_SYNCsend (CO_SYNC_t *SYNC)
 Send SYNC message. More...
 
CO_SYNC_status_t CO_SYNC_process (CO_SYNC_t *SYNC, uint32_t timeDifference_us, uint32_t ObjDict_synchronousWindowLength, uint32_t *timerNext_us)
 Process SYNC communication. More...
 
+

Detailed Description

+

CANopen Synchronisation protocol.

+
Author
Janez Paternoster
+
Copyright
2004 - 2020 Janez Paternoster
+

This file is part of CANopenNode, an opensource CANopen Stack. Project home page is https://github.com/CANopenNode/CANopenNode. For more information on CANopen see http://www.can-cia.org/.

+

Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. You may obtain a copy of the License at 

http://www.apache.org/licenses/LICENSE-2.0
+

Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License.

+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/CO__SYNC_8h.js b/Devices/Libraries/Systems/CANopenSocket/docs/CO__SYNC_8h.js new file mode 100755 index 0000000..d6666ba --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/CO__SYNC_8h.js @@ -0,0 +1,12 @@ +var CO__SYNC_8h = +[ + [ "CO_SYNC_status_t", "group__CO__SYNC.html#ga121ede6e0c90c66076a7ed950db38517", [ + [ "CO_SYNC_NONE", "group__CO__SYNC.html#gga121ede6e0c90c66076a7ed950db38517aeff7846423b9eb92cd2c69df745ea429", null ], + [ "CO_SYNC_RECEIVED", "group__CO__SYNC.html#gga121ede6e0c90c66076a7ed950db38517aa03c21a78b503a4adebb2d9d7aa655bf", null ], + [ "CO_SYNC_OUTSIDE_WINDOW", "group__CO__SYNC.html#gga121ede6e0c90c66076a7ed950db38517a5b112bec6cb10119879703a6313de41e", null ] + ] ], + [ "CO_SYNC_init", "group__CO__SYNC.html#ga3047b679d5e3261eedf1980ea87b5135", null ], + [ "CO_SYNC_initCallbackPre", "group__CO__SYNC.html#gaf1005766c7f1588262b018fe04960777", null ], + [ "CO_SYNCsend", "group__CO__SYNC.html#gabbc8625d068d19d8b632d034f396a1ff", null ], + [ "CO_SYNC_process", "group__CO__SYNC.html#ga66b8f42fd430daa2a57ff15aa49c814c", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/CO__SYNC_8h_source.html b/Devices/Libraries/Systems/CANopenSocket/docs/CO__SYNC_8h_source.html new file mode 100755 index 0000000..f0dffdd --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/CO__SYNC_8h_source.html @@ -0,0 +1,239 @@ + + + + + + + +CANopenNode: 301/CO_SYNC.h Source File + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+
CO_SYNC.h
+
+
+Go to the documentation of this file.
1 
+
26 #ifndef CO_SYNC_H
+
27 #define CO_SYNC_H
+
28 
+
29 #include "301/CO_driver.h"
+
30 
+
31 /* default configuration, see CO_config.h */
+
32 #ifndef CO_CONFIG_SYNC
+
33 #define CO_CONFIG_SYNC (CO_CONFIG_SYNC_ENABLE | CO_CONFIG_SYNC_PRODUCER)
+
34 #endif
+
35 
+
36 #if ((CO_CONFIG_SYNC) & CO_CONFIG_SYNC_ENABLE) || defined CO_DOXYGEN
+
37 
+
38 #ifdef __cplusplus
+
39 extern "C" {
+
40 #endif
+
41 
+
75 typedef struct{
+
76  CO_EM_t *em;
+
77  CO_NMT_internalState_t *operatingState;
+
80  bool_t isProducer;
+
83  uint16_t COB_ID;
+
86  uint32_t periodTime;
+
89  uint32_t periodTimeoutTime;
+
92  uint8_t counterOverflowValue;
+
95  bool_t curentSyncTimeIsInsideWindow;
+
97  volatile void *CANrxNew;
+
99  bool_t CANrxToggle;
+
101  uint8_t counter;
+
104  uint32_t timer;
+
106  uint16_t receiveError;
+
107 #if ((CO_CONFIG_SYNC) & CO_CONFIG_FLAG_CALLBACK_PRE) || defined CO_DOXYGEN
+
108 
+
109  void (*pFunctSignalPre)(void *object);
+
111  void *functSignalObjectPre;
+
112 #endif
+
113  CO_CANmodule_t *CANdevRx;
+
114  uint16_t CANdevRxIdx;
+
115  CO_CANmodule_t *CANdevTx;
+
116  CO_CANtx_t *CANtxBuff;
+
117  uint16_t CANdevTxIdx;
+
118 }CO_SYNC_t;
+
119 
+
120 
+
122 typedef enum {
+
123  CO_SYNC_NONE = 0,
+
124  CO_SYNC_RECEIVED = 1,
+
125  CO_SYNC_OUTSIDE_WINDOW = 2
+
126 } CO_SYNC_status_t;
+
127 
+
128 
+
148 CO_ReturnError_t CO_SYNC_init(
+
149  CO_SYNC_t *SYNC,
+
150  CO_EM_t *em,
+
151  CO_SDO_t *SDO,
+
152  CO_NMT_internalState_t *operatingState,
+
153  uint32_t COB_ID_SYNCMessage,
+
154  uint32_t communicationCyclePeriod,
+
155  uint8_t synchronousCounterOverflowValue,
+
156  CO_CANmodule_t *CANdevRx,
+
157  uint16_t CANdevRxIdx,
+
158  CO_CANmodule_t *CANdevTx,
+
159  uint16_t CANdevTxIdx);
+
160 
+
161 
+
162 #if ((CO_CONFIG_SYNC) & CO_CONFIG_FLAG_CALLBACK_PRE) || defined CO_DOXYGEN
+
163 
+
174 void CO_SYNC_initCallbackPre(
+
175  CO_SYNC_t *SYNC,
+
176  void *object,
+
177  void (*pFunctSignalPre)(void *object));
+
178 #endif
+
179 
+
180 
+
193 CO_ReturnError_t CO_SYNCsend(CO_SYNC_t *SYNC);
+
194 
+
195 
+
209 CO_SYNC_status_t CO_SYNC_process(
+
210  CO_SYNC_t *SYNC,
+
211  uint32_t timeDifference_us,
+
212  uint32_t ObjDict_synchronousWindowLength,
+
213  uint32_t *timerNext_us);
+
214  /* CO_SYNC */
+
216 
+
217 #ifdef __cplusplus
+
218 }
+
219 #endif /*__cplusplus*/
+
220 
+
221 #endif /* (CO_CONFIG_SYNC) & CO_CONFIG_SYNC_ENABLE */
+
222 
+
223 #endif /* CO_SYNC_H */
+
+
+
uint32_t
unsigned long int uint32_t
UNSIGNED32 in CANopen (0007h), 32-bit unsigned integer.
Definition: CO_driver.h:155
+
CO_driver.h
Interface between CAN hardware and CANopenNode.
+
CO_SYNC_t::periodTime
uint32_t periodTime
Sync period time in [microseconds].
Definition: CO_SYNC.h:86
+
CO_SYNC_process
CO_SYNC_status_t CO_SYNC_process(CO_SYNC_t *SYNC, uint32_t timeDifference_us, uint32_t ObjDict_synchronousWindowLength, uint32_t *timerNext_us)
Process SYNC communication.
+
CO_SYNC_NONE
@ CO_SYNC_NONE
SYNC not received.
Definition: CO_SYNC.h:123
+
uint16_t
unsigned int uint16_t
UNSIGNED16 in CANopen (0006h), 16-bit unsigned integer.
Definition: CO_driver.h:153
+
CO_SYNC_t::counter
uint8_t counter
Counter of the SYNC message if counterOverflowValue is different than zero.
Definition: CO_SYNC.h:101
+
CO_SYNC_OUTSIDE_WINDOW
@ CO_SYNC_OUTSIDE_WINDOW
SYNC received outside SYNC window.
Definition: CO_SYNC.h:125
+
CO_ReturnError_t
CO_ReturnError_t
Return values of some CANopen functions.
Definition: CO_driver.h:488
+
CO_SYNC_t::CANdevRx
CO_CANmodule_t * CANdevRx
From CO_SYNC_init()
Definition: CO_SYNC.h:113
+
bool_t
unsigned char bool_t
Boolean data type for general use.
Definition: CO_driver.h:141
+
CO_SYNC_status_t
CO_SYNC_status_t
Return value for CO_SYNC_process.
Definition: CO_SYNC.h:122
+
CO_SYNC_t::periodTimeoutTime
uint32_t periodTimeoutTime
Sync period timeout time in [microseconds].
Definition: CO_SYNC.h:89
+
CO_SYNC_t::CANtxBuff
CO_CANtx_t * CANtxBuff
CAN transmit buffer inside CANdevTx.
Definition: CO_SYNC.h:116
+
CO_SYNC_initCallbackPre
void CO_SYNC_initCallbackPre(CO_SYNC_t *SYNC, void *object, void(*pFunctSignalPre)(void *object))
Initialize SYNC callback function.
+
CO_SYNC_RECEIVED
@ CO_SYNC_RECEIVED
SYNC received.
Definition: CO_SYNC.h:124
+
CO_SYNC_t::counterOverflowValue
uint8_t counterOverflowValue
Value from Synchronous counter overflow value variable from Object dictionary (index 0x1019)
Definition: CO_SYNC.h:92
+
CO_SYNC_t::receiveError
uint16_t receiveError
Set to nonzero value, if SYNC with wrong data length is received from CAN.
Definition: CO_SYNC.h:106
+
CO_SYNC_t::CANrxToggle
bool_t CANrxToggle
Variable toggles, if new SYNC message received from CAN bus.
Definition: CO_SYNC.h:99
+
CO_SYNCsend
CO_ReturnError_t CO_SYNCsend(CO_SYNC_t *SYNC)
Send SYNC message.
+
CO_SYNC_t::timer
uint32_t timer
Timer for the SYNC message in [microseconds].
Definition: CO_SYNC.h:104
+
CO_SYNC_t::COB_ID
uint16_t COB_ID
COB_ID of SYNC message.
Definition: CO_SYNC.h:83
+
CO_SYNC_t::curentSyncTimeIsInsideWindow
bool_t curentSyncTimeIsInsideWindow
True, if current time is inside synchronous window.
Definition: CO_SYNC.h:95
+
CO_EM_t
Emergency object.
Definition: CO_Emergency.h:369
+
CO_NMT_internalState_t
CO_NMT_internalState_t
Internal network state of the CANopen node.
Definition: CO_NMT_Heartbeat.h:77
+
CO_SYNC_t::isProducer
bool_t isProducer
True, if device is SYNC producer.
Definition: CO_SYNC.h:80
+
CO_SYNC_t::CANrxNew
volatile void * CANrxNew
Indicates, if new SYNC message received from CAN bus.
Definition: CO_SYNC.h:97
+
CO_SYNC_t::functSignalObjectPre
void * functSignalObjectPre
From CO_SYNC_initCallbackPre() or NULL.
Definition: CO_SYNC.h:111
+
CO_SYNC_init
CO_ReturnError_t CO_SYNC_init(CO_SYNC_t *SYNC, CO_EM_t *em, CO_SDO_t *SDO, CO_NMT_internalState_t *operatingState, uint32_t COB_ID_SYNCMessage, uint32_t communicationCyclePeriod, uint8_t synchronousCounterOverflowValue, CO_CANmodule_t *CANdevRx, uint16_t CANdevRxIdx, CO_CANmodule_t *CANdevTx, uint16_t CANdevTxIdx)
Initialize SYNC object.
+
CO_CANmodule_t
Complete CAN module object.
Definition: CO_driver.h:319
+
CO_SYNC_t::em
CO_EM_t * em
From CO_SYNC_init()
Definition: CO_SYNC.h:76
+
CO_SYNC_t::CANdevTx
CO_CANmodule_t * CANdevTx
From CO_SYNC_init()
Definition: CO_SYNC.h:115
+
CO_SYNC_t
SYNC producer and consumer object.
Definition: CO_SYNC.h:75
+
CO_SYNC_t::CANdevRxIdx
uint16_t CANdevRxIdx
From CO_SYNC_init()
Definition: CO_SYNC.h:114
+
CO_SYNC_t::operatingState
CO_NMT_internalState_t * operatingState
From CO_SYNC_init()
Definition: CO_SYNC.h:77
+
CO_CANtx_t
Configuration object for CAN transmit message for specific CANopenNode Object.
Definition: CO_driver.h:299
+
uint8_t
unsigned char uint8_t
UNSIGNED8 in CANopen (0005h), 8-bit unsigned integer.
Definition: CO_driver.h:151
+
CO_SYNC_t::CANdevTxIdx
uint16_t CANdevTxIdx
From CO_SYNC_init()
Definition: CO_SYNC.h:117
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/CO__TIME_8h.html b/Devices/Libraries/Systems/CANopenSocket/docs/CO__TIME_8h.html new file mode 100755 index 0000000..394b453 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/CO__TIME_8h.html @@ -0,0 +1,145 @@ + + + + + + + +CANopenNode: 301/CO_TIME.h File Reference + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Data Structures | +Functions
+
+
CO_TIME.h File Reference
CANopen_301 » TIME
+
+
+ +

CANopen Time-stamp protocol. +More...

+
#include "301/CO_driver.h"
+
+

Go to the source code of this file.

+ + + + + +

+Data Structures

struct  CO_TIME_t
 TIME producer and consumer object. More...
 
+ + + + + + + + + + +

+Functions

CO_ReturnError_t CO_TIME_init (CO_TIME_t *TIME, CO_EM_t *em, CO_SDO_t *SDO, CO_NMT_internalState_t *operatingState, uint32_t COB_ID_TIMEMessage, uint32_t TIMECyclePeriod, CO_CANmodule_t *CANdevRx, uint16_t CANdevRxIdx, CO_CANmodule_t *CANdevTx, uint16_t CANdevTxIdx)
 Initialize TIME object. More...
 
void CO_TIME_initCallbackPre (CO_TIME_t *TIME, void *object, void(*pFunctSignalPre)(void *object))
 Initialize TIME callback function. More...
 
uint8_t CO_TIME_process (CO_TIME_t *TIME, uint32_t timeDifference_us)
 Process TIME communication. More...
 
+

Detailed Description

+

CANopen Time-stamp protocol.

+
Author
Julien PEYREGNE
+
Copyright
2019 - 2020 Janez Paternoster
+

This file is part of CANopenNode, an opensource CANopen Stack. Project home page is https://github.com/CANopenNode/CANopenNode. For more information on CANopen see http://www.can-cia.org/.

+

Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. You may obtain a copy of the License at 

http://www.apache.org/licenses/LICENSE-2.0
+

Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License.

+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/CO__TIME_8h.js b/Devices/Libraries/Systems/CANopenSocket/docs/CO__TIME_8h.js new file mode 100755 index 0000000..c8666aa --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/CO__TIME_8h.js @@ -0,0 +1,6 @@ +var CO__TIME_8h = +[ + [ "CO_TIME_init", "group__CO__TIME.html#ga9e02651c1662d3da13a9a071c73347bb", null ], + [ "CO_TIME_initCallbackPre", "group__CO__TIME.html#gae2f03663f1477cdc551b61cf5689cd6b", null ], + [ "CO_TIME_process", "group__CO__TIME.html#ga39f71192db2b40da6dcf8f4fceac9bb6", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/CO__TIME_8h_source.html b/Devices/Libraries/Systems/CANopenSocket/docs/CO__TIME_8h_source.html new file mode 100755 index 0000000..d0b3faa --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/CO__TIME_8h_source.html @@ -0,0 +1,230 @@ + + + + + + + +CANopenNode: 301/CO_TIME.h Source File + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+
CO_TIME.h
+
+
+Go to the documentation of this file.
1 
+
26 #ifndef CO_TIME_H
+
27 #define CO_TIME_H
+
28 
+
29 #include "301/CO_driver.h"
+
30 
+
31 /* default configuration, see CO_config.h */
+
32 #ifndef CO_CONFIG_TIME
+
33 #define CO_CONFIG_TIME (0)
+
34 #endif
+
35 
+
36 #if ((CO_CONFIG_TIME) & CO_CONFIG_TIME_ENABLE) || defined CO_DOXYGEN
+
37 
+
38 #ifdef __cplusplus
+
39 extern "C" {
+
40 #endif
+
41 
+
79 #define TIME_MSG_LENGTH 6U
+
80 
+
81 #ifndef timeOfDay_t
+
82 typedef union {
+
83  unsigned long long ullValue;
+
84  struct {
+
85  unsigned long ms:28;
+
86  unsigned reserved:4;
+
87  unsigned days:16;
+
88  unsigned reserved2:16;
+
89  };
+
90 } timeOfDay_t;
+
91 #endif
+
92 
+
93 typedef timeOfDay_t TIME_OF_DAY;
+
94 typedef timeOfDay_t TIME_DIFFERENCE;
+
95 
+
99 typedef struct{
+
100  CO_EM_t *em;
+
101  CO_NMT_internalState_t *operatingState;
+
104  bool_t isConsumer;
+
107  bool_t isProducer;
+
108  uint16_t COB_ID;
+
111  uint32_t periodTime;
+
114  uint32_t periodTimeoutTime;
+
116  volatile void *CANrxNew;
+
119  uint32_t timer;
+
121  uint16_t receiveError;
+
122 #if ((CO_CONFIG_TIME) & CO_CONFIG_FLAG_CALLBACK_PRE) || defined CO_DOXYGEN
+
123 
+
124  void (*pFunctSignalPre)(void *object);
+
126  void *functSignalObjectPre;
+
127 #endif
+
128  CO_CANmodule_t *CANdevRx;
+
129  uint16_t CANdevRxIdx;
+
130  CO_CANmodule_t *CANdevTx;
+
131  uint16_t CANdevTxIdx;
+
132  CO_CANtx_t *TXbuff;
+
133  TIME_OF_DAY Time;
+
134 }CO_TIME_t;
+
135 
+
154 CO_ReturnError_t CO_TIME_init(
+
155  CO_TIME_t *TIME,
+
156  CO_EM_t *em,
+
157  CO_SDO_t *SDO,
+
158  CO_NMT_internalState_t *operatingState,
+
159  uint32_t COB_ID_TIMEMessage,
+
160  uint32_t TIMECyclePeriod,
+
161  CO_CANmodule_t *CANdevRx,
+
162  uint16_t CANdevRxIdx,
+
163  CO_CANmodule_t *CANdevTx,
+
164  uint16_t CANdevTxIdx);
+
165 
+
166 #if ((CO_CONFIG_TIME) & CO_CONFIG_FLAG_CALLBACK_PRE) || defined CO_DOXYGEN
+
167 
+
178 void CO_TIME_initCallbackPre(
+
179  CO_TIME_t *TIME,
+
180  void *object,
+
181  void (*pFunctSignalPre)(void *object));
+
182 #endif
+
183 
+
195 uint8_t CO_TIME_process(
+
196  CO_TIME_t *TIME,
+
197  uint32_t timeDifference_us);
+
198  /* CO_TIME */
+
200 
+
201 #ifdef __cplusplus
+
202 }
+
203 #endif /*__cplusplus*/
+
204 
+
205 #endif /* (CO_CONFIG_TIME) & CO_CONFIG_TIME_ENABLE */
+
206 
+
207 #endif /* CO_TIME_H */
+
+
+
uint32_t
unsigned long int uint32_t
UNSIGNED32 in CANopen (0007h), 32-bit unsigned integer.
Definition: CO_driver.h:155
+
CO_driver.h
Interface between CAN hardware and CANopenNode.
+
CO_TIME_t::functSignalObjectPre
void * functSignalObjectPre
From CO_TIME_initCallbackPre() or NULL.
Definition: CO_TIME.h:126
+
CO_TIME_t::TXbuff
CO_CANtx_t * TXbuff
CAN transmit buffer.
Definition: CO_TIME.h:132
+
CO_TIME_t::isProducer
bool_t isProducer
True, if device is TIME producer.
Definition: CO_TIME.h:107
+
uint16_t
unsigned int uint16_t
UNSIGNED16 in CANopen (0006h), 16-bit unsigned integer.
Definition: CO_driver.h:153
+
CO_TIME_t::CANdevTx
CO_CANmodule_t * CANdevTx
From CO_TIME_init()
Definition: CO_TIME.h:130
+
CO_TIME_initCallbackPre
void CO_TIME_initCallbackPre(CO_TIME_t *TIME, void *object, void(*pFunctSignalPre)(void *object))
Initialize TIME callback function.
+
CO_ReturnError_t
CO_ReturnError_t
Return values of some CANopen functions.
Definition: CO_driver.h:488
+
bool_t
unsigned char bool_t
Boolean data type for general use.
Definition: CO_driver.h:141
+
CO_TIME_t::periodTime
uint32_t periodTime
TIME period time in [milliseconds].
Definition: CO_TIME.h:111
+
CO_TIME_t::em
CO_EM_t * em
From CO_TIME_init()
Definition: CO_TIME.h:100
+
CO_TIME_t::COB_ID
uint16_t COB_ID
From CO_TIME_init()
Definition: CO_TIME.h:108
+
CO_TIME_t::CANdevRxIdx
uint16_t CANdevRxIdx
From CO_TIME_init()
Definition: CO_TIME.h:129
+
CO_TIME_t
TIME producer and consumer object.
Definition: CO_TIME.h:99
+
CO_TIME_t::CANrxNew
volatile void * CANrxNew
Variable indicates, if new TIME message received from CAN bus.
Definition: CO_TIME.h:116
+
CO_TIME_init
CO_ReturnError_t CO_TIME_init(CO_TIME_t *TIME, CO_EM_t *em, CO_SDO_t *SDO, CO_NMT_internalState_t *operatingState, uint32_t COB_ID_TIMEMessage, uint32_t TIMECyclePeriod, CO_CANmodule_t *CANdevRx, uint16_t CANdevRxIdx, CO_CANmodule_t *CANdevTx, uint16_t CANdevTxIdx)
Initialize TIME object.
+
CO_TIME_process
uint8_t CO_TIME_process(CO_TIME_t *TIME, uint32_t timeDifference_us)
Process TIME communication.
+
CO_EM_t
Emergency object.
Definition: CO_Emergency.h:369
+
CO_NMT_internalState_t
CO_NMT_internalState_t
Internal network state of the CANopen node.
Definition: CO_NMT_Heartbeat.h:77
+
CO_TIME_t::periodTimeoutTime
uint32_t periodTimeoutTime
TIME period timeout time in [milliseconds].
Definition: CO_TIME.h:114
+
CO_TIME_t::isConsumer
bool_t isConsumer
True, if device is TIME consumer.
Definition: CO_TIME.h:104
+
CO_CANmodule_t
Complete CAN module object.
Definition: CO_driver.h:319
+
CO_TIME_t::operatingState
CO_NMT_internalState_t * operatingState
From CO_TIME_init()
Definition: CO_TIME.h:101
+
CO_TIME_t::CANdevTxIdx
uint16_t CANdevTxIdx
From CO_TIME_init()
Definition: CO_TIME.h:131
+
CO_TIME_t::receiveError
uint16_t receiveError
Set to nonzero value, if TIME with wrong data length is received from CAN.
Definition: CO_TIME.h:121
+
CO_TIME_t::CANdevRx
CO_CANmodule_t * CANdevRx
From CO_TIME_init()
Definition: CO_TIME.h:128
+
CO_CANtx_t
Configuration object for CAN transmit message for specific CANopenNode Object.
Definition: CO_driver.h:299
+
uint8_t
unsigned char uint8_t
UNSIGNED8 in CANopen (0005h), 8-bit unsigned integer.
Definition: CO_driver.h:151
+
CO_TIME_t::timer
uint32_t timer
Timer for the TIME message in [microseconds].
Definition: CO_TIME.h:119
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/CO__config_8h.html b/Devices/Libraries/Systems/CANopenSocket/docs/CO__config_8h.html new file mode 100755 index 0000000..fec2cf1 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/CO__config_8h.html @@ -0,0 +1,237 @@ + + + + + + + +CANopenNode: 301/CO_config.h File Reference + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Macros
+
+
CO_config.h File Reference
CANopen_301 » Driver
+
+
+ +

Configuration macros for CANopenNode. +More...

+ +

Go to the source code of this file.

+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

+Macros

#define CO_CONFIG_FLAG_CALLBACK_PRE   0x1000
 Enable custom callback after CAN receive. More...
 
#define CO_CONFIG_FLAG_TIMERNEXT   0x2000
 Enable calculation of timerNext_us variable. More...
 
#define CO_CONFIG_FLAG_OD_DYNAMIC   0x4000
 Enable dynamic behaviour of Object Dictionary variables. More...
 
#define CO_CONFIG_NMT   (0)
 Configuration of NMT and Heartbeat. More...
 
#define CO_CONFIG_HB_CONS   (CO_CONFIG_HB_CONS_ENABLE)
 Configuration of Heartbeat consumer. More...
 
#define CO_CONFIG_HB_CONS_SIZE   8
 Number of heartbeat consumer objects, where each object corresponds to one sub-index in OD object 0x1016, "Consumer heartbeat time". More...
 
#define CO_CONFIG_EM   (CO_CONFIG_EM_PRODUCER | CO_CONFIG_EM_HISTORY)
 Configuration of Emergency. More...
 
#define CO_CONFIG_EM_ERR_STATUS_BITS_COUNT   (10*8)
 Maximum number of CO_EM_errorStatusBits_t. More...
 
#define CO_CONFIG_EM_BUFFER_SIZE   16
 Size of the internal buffer, where emergencies are stored after error indication with CO_error() function. More...
 
#define CO_CONFIG_ERR_CONDITION_GENERIC   (em->errorStatusBits[5] != 0)
 Condition for calculating CANopen Error register, "generic" error bit. More...
 
#define CO_CONFIG_ERR_CONDITION_CURRENT
 Condition for calculating CANopen Error register, "current" error bit. More...
 
#define CO_CONFIG_ERR_CONDITION_VOLTAGE
 Condition for calculating CANopen Error register, "voltage" error bit. More...
 
#define CO_CONFIG_ERR_CONDITION_TEMPERATURE
 Condition for calculating CANopen Error register, "temperature" error bit. More...
 
#define CO_CONFIG_ERR_CONDITION_COMMUNICATION   (em->errorStatusBits[2] || em->errorStatusBits[3])
 Condition for calculating CANopen Error register, "communication" error bit. More...
 
#define CO_CONFIG_ERR_CONDITION_DEV_PROFILE
 Condition for calculating CANopen Error register, "device profile" error bit. More...
 
#define CO_CONFIG_ERR_CONDITION_MANUFACTURER   (em->errorStatusBits[8] || em->errorStatusBits[9])
 Condition for calculating CANopen Error register, "manufacturer" error bit. More...
 
#define CO_CONFIG_SDO_SRV   (CO_CONFIG_SDO_SRV_SEGMENTED)
 Configuration of SDO server. More...
 
#define CO_CONFIG_SDO_SRV_BUFFER_SIZE   32
 Size of the internal data buffer for the SDO server. More...
 
#define CO_CONFIG_SDO_CLI   (0)
 Configuration of SDO client. More...
 
#define CO_CONFIG_SDO_CLI_BUFFER_SIZE   32
 Size of the internal data buffer for the SDO client. More...
 
#define CO_CONFIG_TIME   (0)
 Configuration of TIME. More...
 
#define CO_CONFIG_SYNC   (CO_CONFIG_SYNC_ENABLE | CO_CONFIG_SYNC_PRODUCER)
 Configuration of SYNC. More...
 
#define CO_CONFIG_PDO   (CO_CONFIG_RPDO_ENABLE | CO_CONFIG_TPDO_ENABLE | CO_CONFIG_PDO_SYNC_ENABLE)
 Configuration of PDO. More...
 
#define CO_CONFIG_LEDS   (CO_CONFIG_LEDS_ENABLE)
 Configuration of LED indicators. More...
 
#define CO_CONFIG_GFC   (0)
 Configuration of GFC. More...
 
#define CO_CONFIG_SRDO   (0)
 Configuration of SRDO. More...
 
#define CO_CONFIG_SRDO_MINIMUM_DELAY   0
 SRDO Tx time delay. More...
 
#define CO_CONFIG_LSS   (CO_CONFIG_LSS_SLAVE)
 Configuration of LSS. More...
 
#define CO_CONFIG_GTW   (0)
 Configuration of Gateway ASCII mapping. More...
 
#define CO_CONFIG_GTW_BLOCK_DL_LOOP   1
 Number of loops of CO_SDOclientDownload() in case of block download. More...
 
#define CO_CONFIG_GTWA_COMM_BUF_SIZE   200
 Size of command buffer in ASCII gateway object. More...
 
+#define CO_CONFIG_GTWA_LOG_BUF_SIZE   2000
 Size of message log buffer in ASCII gateway object.
 
#define CO_CONFIG_CRC16   (0)
 Configuration of CRC 16 CCITT calculation. More...
 
#define CO_CONFIG_FIFO   (0)
 Configuration of FIFO circular buffer. More...
 
#define CO_CONFIG_TRACE   (0)
 Configuration of Trace for recording variables over time. More...
 
#define CO_CONFIG_DEBUG   (0)
 Configuration of debug messages from different parts of the stack, which can be logged according to target specific function. More...
 
+

Detailed Description

+

Configuration macros for CANopenNode.

+
Author
Janez Paternoster
+
Copyright
2020 Janez Paternoster
+

This file is part of CANopenNode, an opensource CANopen Stack. Project home page is https://github.com/CANopenNode/CANopenNode. For more information on CANopen see http://www.can-cia.org/.

+

Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. You may obtain a copy of the License at 

http://www.apache.org/licenses/LICENSE-2.0
+

Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License.

+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/CO__config_8h.js b/Devices/Libraries/Systems/CANopenSocket/docs/CO__config_8h.js new file mode 100755 index 0000000..f5e1d37 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/CO__config_8h.js @@ -0,0 +1,39 @@ +var CO__config_8h = +[ + [ "CO_CONFIG_FLAG_CALLBACK_PRE", "group__CO__STACK__CONFIG__COMMON.html#gab55099df45bed12f182ef7c0c779dc14", null ], + [ "CO_CONFIG_FLAG_TIMERNEXT", "group__CO__STACK__CONFIG__COMMON.html#ga9e84c3a9256f15246be7766a61096c2d", null ], + [ "CO_CONFIG_FLAG_OD_DYNAMIC", "group__CO__STACK__CONFIG__COMMON.html#gaf0f46ccffdd156cc7c2d8774ecb2060d", null ], + [ "CO_CONFIG_NMT", "group__CO__STACK__CONFIG__NMT__HB.html#gafa3b1f1b4931175bf9c67a5d45633e76", null ], + [ "CO_CONFIG_HB_CONS", "group__CO__STACK__CONFIG__NMT__HB.html#ga7368d68cb039983bc8cc164410877098", null ], + [ "CO_CONFIG_HB_CONS_SIZE", "group__CO__STACK__CONFIG__NMT__HB.html#ga0cbe9ab929ff9d122ab6727d66fe7752", null ], + [ "CO_CONFIG_EM", "group__CO__STACK__CONFIG__EMERGENCY.html#ga16aa1479ffd52a627d1053c20f844b62", null ], + [ "CO_CONFIG_EM_ERR_STATUS_BITS_COUNT", "group__CO__STACK__CONFIG__EMERGENCY.html#gab87776d4802748671b234112263760af", null ], + [ "CO_CONFIG_EM_BUFFER_SIZE", "group__CO__STACK__CONFIG__EMERGENCY.html#ga3c35cf4947c82a0b15afdbfa43a10d67", null ], + [ "CO_CONFIG_ERR_CONDITION_GENERIC", "group__CO__STACK__CONFIG__EMERGENCY.html#gad6270eb7887b22c0365c304d7cf2c633", null ], + [ "CO_CONFIG_ERR_CONDITION_CURRENT", "group__CO__STACK__CONFIG__EMERGENCY.html#ga63af1aaa73297df53b555cb89cd0c07f", null ], + [ "CO_CONFIG_ERR_CONDITION_VOLTAGE", "group__CO__STACK__CONFIG__EMERGENCY.html#ga2b1c3c4f106a8a5d7efda475b469a727", null ], + [ "CO_CONFIG_ERR_CONDITION_TEMPERATURE", "group__CO__STACK__CONFIG__EMERGENCY.html#gaeb96443d9ea2142c346638612fd5c717", null ], + [ "CO_CONFIG_ERR_CONDITION_COMMUNICATION", "group__CO__STACK__CONFIG__EMERGENCY.html#gae47daba892331857e65df82272ed4152", null ], + [ "CO_CONFIG_ERR_CONDITION_DEV_PROFILE", "group__CO__STACK__CONFIG__EMERGENCY.html#gaec2f6161e439dba5376808dcb0cdc36a", null ], + [ "CO_CONFIG_ERR_CONDITION_MANUFACTURER", "group__CO__STACK__CONFIG__EMERGENCY.html#ga3717ce44b5db3189757d874f440adce1", null ], + [ "CO_CONFIG_SDO_SRV", "group__CO__STACK__CONFIG__SDO.html#ga2928cc23dd27138821d48c2fb3e24222", null ], + [ "CO_CONFIG_SDO_SRV_BUFFER_SIZE", "group__CO__STACK__CONFIG__SDO.html#gacad3d0d9060469aedcb9e058c1883375", null ], + [ "CO_CONFIG_SDO_CLI", "group__CO__STACK__CONFIG__SDO.html#gac8ee65cd62dbee2982f5304513402a57", null ], + [ "CO_CONFIG_SDO_CLI_BUFFER_SIZE", "group__CO__STACK__CONFIG__SDO.html#ga763b09ab827365e46f10234bd9c0acfd", null ], + [ "CO_CONFIG_TIME", "group__CO__STACK__CONFIG__TIME.html#gaba4a59929bbd8512ca954ba8fcf1dfe6", null ], + [ "CO_CONFIG_SYNC", "group__CO__STACK__CONFIG__SYNC__PDO.html#ga7d1d2210fdf2b916ca1d82c4933856bc", null ], + [ "CO_CONFIG_PDO", "group__CO__STACK__CONFIG__SYNC__PDO.html#gaa20d1b49249b7f5a15963cc1a4611be9", null ], + [ "CO_CONFIG_LEDS", "group__CO__STACK__CONFIG__LEDS.html#ga423160131d618b5d57bc7c016ee369fd", null ], + [ "CO_CONFIG_GFC", "group__CO__STACK__CONFIG__SRDO.html#ga71d11e8460a5410be21863a0f99cbab2", null ], + [ "CO_CONFIG_SRDO", "group__CO__STACK__CONFIG__SRDO.html#ga61645e6ad8a02e356abde012434bedf9", null ], + [ "CO_CONFIG_SRDO_MINIMUM_DELAY", "group__CO__STACK__CONFIG__SRDO.html#gaebb5427a155133b50622e60acdd0e650", null ], + [ "CO_CONFIG_LSS", "group__CO__STACK__CONFIG__LSS.html#gafeb75d750efb0879fe11a5482b6629f3", null ], + [ "CO_CONFIG_GTW", "group__CO__STACK__CONFIG__GATEWAY.html#ga9af15f76cd14fece499764499c6bc2d3", null ], + [ "CO_CONFIG_GTW_BLOCK_DL_LOOP", "group__CO__STACK__CONFIG__GATEWAY.html#gaa864e7c6e7ebd3fc7ce424dc3c94db9d", null ], + [ "CO_CONFIG_GTWA_COMM_BUF_SIZE", "group__CO__STACK__CONFIG__GATEWAY.html#ga7903ae4ca7939fc32bd747224e868a38", null ], + [ "CO_CONFIG_GTWA_LOG_BUF_SIZE", "group__CO__STACK__CONFIG__GATEWAY.html#ga4f471dca1341879dc56c2e0a2c73cb29", null ], + [ "CO_CONFIG_CRC16", "group__CO__STACK__CONFIG__CRC16.html#ga15737bc0ede4bcd56968e5f96b2e8c9b", null ], + [ "CO_CONFIG_FIFO", "group__CO__STACK__CONFIG__FIFO.html#ga055654eb6f93ba05e3534b31626eec3a", null ], + [ "CO_CONFIG_TRACE", "group__CO__STACK__CONFIG__TRACE.html#ga9d4e333d0b599c2369366defc6ce5e62", null ], + [ "CO_CONFIG_DEBUG", "group__CO__STACK__CONFIG__DEBUG.html#ga1ce7b96c60a5ab9349b66b77f6a6e2a7", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/CO__config_8h_source.html b/Devices/Libraries/Systems/CANopenSocket/docs/CO__config_8h_source.html new file mode 100755 index 0000000..3d2cb22 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/CO__config_8h_source.html @@ -0,0 +1,348 @@ + + + + + + + +CANopenNode: 301/CO_config.h Source File + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+
CO_config.h
+
+
+Go to the documentation of this file.
1 
+
27 #ifndef CO_CONFIG_FLAGS_H
+
28 #define CO_CONFIG_FLAGS_H
+
29 
+
30 #ifdef __cplusplus
+
31 extern "C" {
+
32 #endif
+
33 
+
78 #define CO_CONFIG_FLAG_CALLBACK_PRE 0x1000
+
79 
+
88 #define CO_CONFIG_FLAG_TIMERNEXT 0x2000
+
89 
+
100 #define CO_CONFIG_FLAG_OD_DYNAMIC 0x4000
+
101  /* CO_STACK_CONFIG_COMMON */
+
102 
+
103 
+
122 #ifdef CO_DOXYGEN
+
123 #define CO_CONFIG_NMT (0)
+
124 #endif
+
125 #define CO_CONFIG_NMT_CALLBACK_CHANGE 0x01
+
126 #define CO_CONFIG_NMT_MASTER 0x02
+
127 
+
152 #ifdef CO_DOXYGEN
+
153 #define CO_CONFIG_HB_CONS (CO_CONFIG_HB_CONS_ENABLE)
+
154 #endif
+
155 #define CO_CONFIG_HB_CONS_ENABLE 0x01
+
156 #define CO_CONFIG_HB_CONS_CALLBACK_CHANGE 0x02
+
157 #define CO_CONFIG_HB_CONS_CALLBACK_MULTI 0x04
+
158 #define CO_CONFIG_HB_CONS_QUERY_FUNCT 0x08
+
159 
+
166 #ifdef CO_DOXYGEN
+
167 #define CO_CONFIG_HB_CONS_SIZE 8
+
168 #endif
+
169  /* CO_STACK_CONFIG_NMT_HB */
+
170 
+
171 
+
196 #ifdef CO_DOXYGEN
+
197 #define CO_CONFIG_EM (CO_CONFIG_EM_PRODUCER | CO_CONFIG_EM_HISTORY)
+
198 #endif
+
199 #define CO_CONFIG_EM_PRODUCER 0x01
+
200 #define CO_CONFIG_EM_PROD_CONFIGURABLE 0x02
+
201 #define CO_CONFIG_EM_PROD_INHIBIT 0x04
+
202 #define CO_CONFIG_EM_HISTORY 0x08
+
203 #define CO_CONFIG_EM_STATUS_BITS 0x10
+
204 #define CO_CONFIG_EM_CONSUMER 0x20
+
205 
+
213 #ifdef CO_DOXYGEN
+
214 #define CO_CONFIG_EM_ERR_STATUS_BITS_COUNT (10*8)
+
215 #endif
+
216 
+
227 #ifdef CO_DOXYGEN
+
228 #define CO_CONFIG_EM_BUFFER_SIZE 16
+
229 #endif
+
230 
+
245 #ifdef CO_DOXYGEN
+
246 #define CO_CONFIG_ERR_CONDITION_GENERIC (em->errorStatusBits[5] != 0)
+
247 #endif
+
248 
+
254 #ifdef CO_DOXYGEN
+
255 #define CO_CONFIG_ERR_CONDITION_CURRENT
+
256 #endif
+
257 
+
263 #ifdef CO_DOXYGEN
+
264 #define CO_CONFIG_ERR_CONDITION_VOLTAGE
+
265 #endif
+
266 
+
272 #ifdef CO_DOXYGEN
+
273 #define CO_CONFIG_ERR_CONDITION_TEMPERATURE
+
274 #endif
+
275 
+
283 #ifdef CO_DOXYGEN
+
284 #define CO_CONFIG_ERR_CONDITION_COMMUNICATION (em->errorStatusBits[2] || em->errorStatusBits[3])
+
285 #endif
+
286 
+
292 #ifdef CO_DOXYGEN
+
293 #define CO_CONFIG_ERR_CONDITION_DEV_PROFILE
+
294 #endif
+
295 
+
303 #ifdef CO_DOXYGEN
+
304 #define CO_CONFIG_ERR_CONDITION_MANUFACTURER (em->errorStatusBits[8] || em->errorStatusBits[9])
+
305 #endif
+
306  /* CO_STACK_CONFIG_EMERGENCY */
+
307 
+
308 
+
328 #ifdef CO_DOXYGEN
+
329 #define CO_CONFIG_SDO_SRV (CO_CONFIG_SDO_SRV_SEGMENTED)
+
330 #endif
+
331 #define CO_CONFIG_SDO_SRV_SEGMENTED 0x02
+
332 #define CO_CONFIG_SDO_SRV_BLOCK 0x04
+
333 
+
341 #ifdef CO_DOXYGEN
+
342 #define CO_CONFIG_SDO_SRV_BUFFER_SIZE 32
+
343 #endif
+
344 
+
366 #ifdef CO_DOXYGEN
+
367 #define CO_CONFIG_SDO_CLI (0)
+
368 #endif
+
369 #define CO_CONFIG_SDO_CLI_ENABLE 0x01
+
370 #define CO_CONFIG_SDO_CLI_SEGMENTED 0x02
+
371 #define CO_CONFIG_SDO_CLI_BLOCK 0x04
+
372 #define CO_CONFIG_SDO_CLI_LOCAL 0x08
+
373 
+
385 #ifdef CO_DOXYGEN
+
386 #define CO_CONFIG_SDO_CLI_BUFFER_SIZE 32
+
387 #endif
+
388  /* CO_STACK_CONFIG_SDO */
+
389 
+
390 
+
405 #ifdef CO_DOXYGEN
+
406 #define CO_CONFIG_TIME (0)
+
407 #endif
+
408 #define CO_CONFIG_TIME_ENABLE 0x01
+
409 #define CO_CONFIG_TIME_PRODUCER 0x02
+
410  /* CO_STACK_CONFIG_TIME */
+
411 
+
412 
+
429 #ifdef CO_DOXYGEN
+
430 #define CO_CONFIG_SYNC (CO_CONFIG_SYNC_ENABLE | CO_CONFIG_SYNC_PRODUCER)
+
431 #endif
+
432 #define CO_CONFIG_SYNC_ENABLE 0x01
+
433 #define CO_CONFIG_SYNC_PRODUCER 0x02
+
434 
+
452 #ifdef CO_DOXYGEN
+
453 #define CO_CONFIG_PDO (CO_CONFIG_RPDO_ENABLE | CO_CONFIG_TPDO_ENABLE | CO_CONFIG_PDO_SYNC_ENABLE)
+
454 #endif
+
455 #define CO_CONFIG_RPDO_ENABLE 0x01
+
456 #define CO_CONFIG_TPDO_ENABLE 0x02
+
457 #define CO_CONFIG_PDO_SYNC_ENABLE 0x04
+
458 #define CO_CONFIG_RPDO_CALLS_EXTENSION 0x08
+
459 #define CO_CONFIG_TPDO_CALLS_EXTENSION 0x10
+
460  /* CO_STACK_CONFIG_SYNC_PDO */
+
461 
+
462 
+
476 #ifdef CO_DOXYGEN
+
477 #define CO_CONFIG_LEDS (CO_CONFIG_LEDS_ENABLE)
+
478 #endif
+
479 #define CO_CONFIG_LEDS_ENABLE 0x01
+
480  /* CO_STACK_CONFIG_LEDS */
+
481 
+
482 
+
496 #ifdef CO_DOXYGEN
+
497 #define CO_CONFIG_GFC (0)
+
498 #endif
+
499 #define CO_CONFIG_GFC_ENABLE 0x01
+
500 #define CO_CONFIG_GFC_CONSUMER 0x02
+
501 #define CO_CONFIG_GFC_PRODUCER 0x04
+
502 
+
519 #ifdef CO_DOXYGEN
+
520 #define CO_CONFIG_SRDO (0)
+
521 #endif
+
522 #define CO_CONFIG_SRDO_ENABLE 0x01
+
523 #define CO_CONFIG_SRDO_CHECK_TX 0x02
+
524 #define CO_CONFIG_RSRDO_CALLS_EXTENSION 0x04
+
525 #define CO_CONFIG_TSRDO_CALLS_EXTENSION 0x08
+
526 
+
533 #ifdef CO_DOXYGEN
+
534 #define CO_CONFIG_SRDO_MINIMUM_DELAY 0
+
535 #endif
+
536  /* CO_STACK_CONFIG_SRDO */
+
537 
+
538 
+
556 #ifdef CO_DOXYGEN
+
557 #define CO_CONFIG_LSS (CO_CONFIG_LSS_SLAVE)
+
558 #endif
+
559 #define CO_CONFIG_LSS_SLAVE 0x01
+
560 #define CO_CONFIG_LSS_SLAVE_FASTSCAN_DIRECT_RESPOND 0x02
+
561 #define CO_CONFIG_LSS_MASTER 0x10
+
562  /* CO_STACK_CONFIG_LSS */
+
563 
+
564 
+
594 #ifdef CO_DOXYGEN
+
595 #define CO_CONFIG_GTW (0)
+
596 #endif
+
597 #define CO_CONFIG_GTW_MULTI_NET 0x01
+
598 #define CO_CONFIG_GTW_ASCII 0x02
+
599 #define CO_CONFIG_GTW_ASCII_SDO 0x04
+
600 #define CO_CONFIG_GTW_ASCII_NMT 0x08
+
601 #define CO_CONFIG_GTW_ASCII_LSS 0x10
+
602 #define CO_CONFIG_GTW_ASCII_LOG 0x20
+
603 #define CO_CONFIG_GTW_ASCII_ERROR_DESC 0x40
+
604 #define CO_CONFIG_GTW_ASCII_PRINT_HELP 0x80
+
605 #define CO_CONFIG_GTW_ASCII_PRINT_LEDS 0x100
+
606 
+
614 #ifdef CO_DOXYGEN
+
615 #define CO_CONFIG_GTW_BLOCK_DL_LOOP 1
+
616 #endif
+
617 
+
624 #ifdef CO_DOXYGEN
+
625 #define CO_CONFIG_GTWA_COMM_BUF_SIZE 200
+
626 #endif
+
627 
+
631 #ifdef CO_DOXYGEN
+
632 #define CO_CONFIG_GTWA_LOG_BUF_SIZE 2000
+
633 #endif
+
634  /* CO_STACK_CONFIG_GATEWAY */
+
635 
+
636 
+
649 #ifdef CO_DOXYGEN
+
650 #define CO_CONFIG_CRC16 (0)
+
651 #endif
+
652 #define CO_CONFIG_CRC16_ENABLE 0x01
+
653 #define CO_CONFIG_CRC16_EXTERNAL 0x02
+
654  /* CO_STACK_CONFIG_CRC16 */
+
655 
+
656 
+
683 #ifdef CO_DOXYGEN
+
684 #define CO_CONFIG_FIFO (0)
+
685 #endif
+
686 #define CO_CONFIG_FIFO_ENABLE 0x01
+
687 #define CO_CONFIG_FIFO_ALT_READ 0x02
+
688 #define CO_CONFIG_FIFO_CRC16_CCITT 0x04
+
689 #define CO_CONFIG_FIFO_ASCII_COMMANDS 0x08
+
690 #define CO_CONFIG_FIFO_ASCII_DATATYPES 0x10
+
691  /* CO_STACK_CONFIG_FIFO */
+
692 
+
693 
+
707 #ifdef CO_DOXYGEN
+
708 #define CO_CONFIG_TRACE (0)
+
709 #endif
+
710 #define CO_CONFIG_TRACE_ENABLE 0x01
+
711 #define CO_CONFIG_TRACE_OWN_INTTYPES 0x02
+
712  /* CO_STACK_CONFIG_TRACE */
+
713 
+
714 
+
731 #ifdef CO_DOXYGEN
+
732 #define CO_CONFIG_DEBUG (0)
+
733 #endif
+
734 #define CO_CONFIG_DEBUG_COMMON 0x01
+
735 #define CO_CONFIG_DEBUG_SDO_CLIENT 0x02
+
736 #define CO_CONFIG_DEBUG_SDO_SERVER 0x04
+
737  /* CO_STACK_CONFIG_DEBUG */
+
738  /* CO_STACK_CONFIG */
+
740 
+
741 #ifdef __cplusplus
+
742 }
+
743 #endif /* __cplusplus */
+
744 
+
745 #endif /* CO_CONFIG_FLAGS_H */
+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/CO__driver_8h.html b/Devices/Libraries/Systems/CANopenSocket/docs/CO__driver_8h.html new file mode 100755 index 0000000..1e76b2e --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/CO__driver_8h.html @@ -0,0 +1,421 @@ + + + + + + + +CANopenNode: 301/CO_driver.h File Reference + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Data Structures | +Macros | +Typedefs | +Enumerations | +Functions
+
+
CO_driver.h File Reference
CANopen_301 » Driver
+
+
+ +

Interface between CAN hardware and CANopenNode. +More...

+
#include <string.h>
+#include "CO_config.h"
+#include "CO_driver_target.h"
+
+

Go to the source code of this file.

+ + + + + + + + + + + +

+Data Structures

struct  CO_CANrx_t
 Configuration object for CAN received message for specific CANopenNode Object. More...
 
struct  CO_CANtx_t
 Configuration object for CAN transmit message for specific CANopenNode Object. More...
 
struct  CO_CANmodule_t
 Complete CAN module object. More...
 
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

+Macros

+#define CO_VERSION_MAJOR   4
 Major version number of CANopenNode.
 
+#define CO_VERSION_MINOR   0
 Minor version number of CANopenNode.
 
+#define CO_LITTLE_ENDIAN
 CO_LITTLE_ENDIAN or CO_BIG_ENDIAN must be defined.
 
+#define CO_SWAP_16(x)   x
 Macro must swap bytes, if CO_BIG_ENDIAN is defined.
 
+#define CO_SWAP_32(x)   x
 Macro must swap bytes, if CO_BIG_ENDIAN is defined.
 
+#define CO_SWAP_64(x)   x
 Macro must swap bytes, if CO_BIG_ENDIAN is defined.
 
+#define NULL   (0)
 NULL, for general usage.
 
+#define true   1
 Logical true, for general use.
 
+#define false   0
 Logical false, for general use.
 
+#define CO_LOCK_CAN_SEND()
 Lock critical section in CO_CANsend()
 
+#define CO_UNLOCK_CAN_SEND()
 Unlock critical section in CO_CANsend()
 
+#define CO_LOCK_EMCY()
 Lock critical section in CO_errorReport() or CO_errorReset()
 
+#define CO_UNLOCK_EMCY()
 Unlock critical section in CO_errorReport() or CO_errorReset()
 
+#define CO_LOCK_OD()
 Lock critical section when accessing Object Dictionary.
 
+#define CO_UNLOCK_OD()
 Unock critical section when accessing Object Dictionary.
 
+#define CO_FLAG_READ(rxNew)   ((rxNew) != NULL)
 Check if new message has arrived.
 
+#define CO_FLAG_SET(rxNew)   { __sync_synchronize(); rxNew = (void *)1L; }
 Set new message flag.
 
+#define CO_FLAG_CLEAR(rxNew)   { __sync_synchronize(); rxNew = NULL; }
 Clear new message flag.
 
#define CO_errinfo(CANmodule, err)
 Macro for passing additional information about error. More...
 
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

+Typedefs

+typedef unsigned char bool_t
 Boolean data type for general use.
 
+typedef signed char int8_t
 INTEGER8 in CANopen (0002h), 8-bit signed integer.
 
+typedef signed int int16_t
 INTEGER16 in CANopen (0003h), 16-bit signed integer.
 
+typedef signed long int int32_t
 INTEGER32 in CANopen (0004h), 32-bit signed integer.
 
+typedef signed long long int int64_t
 INTEGER64 in CANopen (0015h), 64-bit signed integer.
 
+typedef unsigned char uint8_t
 UNSIGNED8 in CANopen (0005h), 8-bit unsigned integer.
 
+typedef unsigned int uint16_t
 UNSIGNED16 in CANopen (0006h), 16-bit unsigned integer.
 
+typedef unsigned long int uint32_t
 UNSIGNED32 in CANopen (0007h), 32-bit unsigned integer.
 
+typedef unsigned long long int uint64_t
 UNSIGNED64 in CANopen (001Bh), 64-bit unsigned integer.
 
+typedef float float32_t
 REAL32 in CANopen (0008h), single precision floating point value, 32-bit.
 
+typedef double float64_t
 REAL64 in CANopen (0011h), double precision floating point value, 64-bit.
 
+typedef char char_t
 VISIBLE_STRING in CANopen (0009h), string of signed 8-bit values.
 
+typedef unsigned char oChar_t
 OCTET_STRING in CANopen (000Ah), string of unsigned 8-bit values.
 
+typedef unsigned char domain_t
 DOMAIN in CANopen (000Fh), used to transfer a large block of data.
 
+ + + + + + + + + + +

+Enumerations

enum  CO_Default_CAN_ID_t {
+  CO_CAN_ID_NMT_SERVICE = 0x000, +CO_CAN_ID_GFC = 0x001, +CO_CAN_ID_SYNC = 0x080, +CO_CAN_ID_EMERGENCY = 0x080, +
+  CO_CAN_ID_TIME = 0x100, +CO_CAN_ID_SRDO_1 = 0x0FF, +CO_CAN_ID_TPDO_1 = 0x180, +CO_CAN_ID_RPDO_1 = 0x200, +
+  CO_CAN_ID_TPDO_2 = 0x280, +CO_CAN_ID_RPDO_2 = 0x300, +CO_CAN_ID_TPDO_3 = 0x380, +CO_CAN_ID_RPDO_3 = 0x400, +
+  CO_CAN_ID_TPDO_4 = 0x480, +CO_CAN_ID_RPDO_4 = 0x500, +CO_CAN_ID_SDO_SRV = 0x580, +CO_CAN_ID_SDO_CLI = 0x600, +
+  CO_CAN_ID_HEARTBEAT = 0x700, +CO_CAN_ID_LSS_SLV = 0x7E4, +CO_CAN_ID_LSS_MST = 0x7E5 +
+ }
 Default CANopen identifiers. More...
 
enum  CO_CAN_ERR_status_t {
+  CO_CAN_ERRTX_WARNING = 0x0001, +CO_CAN_ERRTX_PASSIVE = 0x0002, +CO_CAN_ERRTX_BUS_OFF = 0x0004, +CO_CAN_ERRTX_OVERFLOW = 0x0008, +
+  CO_CAN_ERRTX_PDO_LATE = 0x0080, +CO_CAN_ERRRX_WARNING = 0x0100, +CO_CAN_ERRRX_PASSIVE = 0x0200, +CO_CAN_ERRRX_OVERFLOW = 0x0800, +
+  CO_CAN_ERR_WARN_PASSIVE = 0x0303 +
+ }
 CAN error status bitmasks. More...
 
enum  CO_ReturnError_t {
+  CO_ERROR_NO = 0, +CO_ERROR_ILLEGAL_ARGUMENT = -1, +CO_ERROR_OUT_OF_MEMORY = -2, +CO_ERROR_TIMEOUT = -3, +
+  CO_ERROR_ILLEGAL_BAUDRATE = -4, +CO_ERROR_RX_OVERFLOW = -5, +CO_ERROR_RX_PDO_OVERFLOW = -6, +CO_ERROR_RX_MSG_LENGTH = -7, +
+  CO_ERROR_RX_PDO_LENGTH = -8, +CO_ERROR_TX_OVERFLOW = -9, +CO_ERROR_TX_PDO_WINDOW = -10, +CO_ERROR_TX_UNCONFIGURED = -11, +
+  CO_ERROR_OD_PARAMETERS = -12, +CO_ERROR_DATA_CORRUPT = -13, +CO_ERROR_CRC = -14, +CO_ERROR_TX_BUSY = -15, +
+  CO_ERROR_WRONG_NMT_STATE = -16, +CO_ERROR_SYSCALL = -17, +CO_ERROR_INVALID_STATE = -18, +CO_ERROR_NODE_ID_UNCONFIGURED_LSS = -19 +
+ }
 Return values of some CANopen functions. More...
 
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

+Functions

void CANrx_callback (void *object, void *rxMsg)
 CAN receive callback function which pre-processes received CAN message. More...
 
static uint16_t CO_CANrxMsg_readIdent (void *rxMsg)
 CANrx_callback() can read CAN identifier from received CAN message. More...
 
static uint8_t CO_CANrxMsg_readDLC (void *rxMsg)
 CANrx_callback() can read Data Length Code from received CAN message. More...
 
static uint8_t * CO_CANrxMsg_readData (void *rxMsg)
 CANrx_callback() can read pointer to data from received CAN message. More...
 
void CO_CANsetConfigurationMode (void *CANptr)
 Request CAN configuration (stopped) mode and wait until it is set. More...
 
void CO_CANsetNormalMode (CO_CANmodule_t *CANmodule)
 Request CAN normal (operational) mode and wait until it is set. More...
 
CO_ReturnError_t CO_CANmodule_init (CO_CANmodule_t *CANmodule, void *CANptr, CO_CANrx_t rxArray[], uint16_t rxSize, CO_CANtx_t txArray[], uint16_t txSize, uint16_t CANbitRate)
 Initialize CAN module object. More...
 
void CO_CANmodule_disable (CO_CANmodule_t *CANmodule)
 Switch off CANmodule. More...
 
CO_ReturnError_t CO_CANrxBufferInit (CO_CANmodule_t *CANmodule, uint16_t index, uint16_t ident, uint16_t mask, bool_t rtr, void *object, void(*CANrx_callback)(void *object, void *message))
 Configure CAN message receive buffer. More...
 
CO_CANtx_t * CO_CANtxBufferInit (CO_CANmodule_t *CANmodule, uint16_t index, uint16_t ident, bool_t rtr, uint8_t noOfBytes, bool_t syncFlag)
 Configure CAN message transmit buffer. More...
 
CO_ReturnError_t CO_CANsend (CO_CANmodule_t *CANmodule, CO_CANtx_t *buffer)
 Send CAN message. More...
 
void CO_CANclearPendingSyncPDOs (CO_CANmodule_t *CANmodule)
 Clear all synchronous TPDOs from CAN module transmit buffers. More...
 
void CO_CANmodule_process (CO_CANmodule_t *CANmodule)
 Process can module - verify CAN errors. More...
 
static uint8_t CO_getUint8 (const void *buf)
 Get uint8_t value from memory buffer. More...
 
+static uint16_t CO_getUint16 (const void *buf)
 Get uint16_t value from memory buffer, see CO_getUint8.
 
+static uint32_t CO_getUint32 (const void *buf)
 Get uint32_t value from memory buffer, see CO_getUint8.
 
static uint8_t CO_setUint8 (void *buf, uint8_t value)
 Write uint8_t value into memory buffer. More...
 
+static uint8_t CO_setUint16 (void *buf, uint16_t value)
 Write uint16_t value into memory buffer, see CO_setUint8.
 
+static uint8_t CO_setUint32 (void *buf, uint32_t value)
 Write uint32_t value into memory buffer, see CO_setUint8.
 
+

Detailed Description

+

Interface between CAN hardware and CANopenNode.

+
Author
Janez Paternoster
+
Copyright
2004 - 2020 Janez Paternoster
+

This file is part of CANopenNode, an opensource CANopen Stack. Project home page is https://github.com/CANopenNode/CANopenNode. For more information on CANopen see http://www.can-cia.org/.

+

Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. You may obtain a copy of the License at 

http://www.apache.org/licenses/LICENSE-2.0
+

Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License.

+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/CO__driver_8h.js b/Devices/Libraries/Systems/CANopenSocket/docs/CO__driver_8h.js new file mode 100755 index 0000000..de72d42 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/CO__driver_8h.js @@ -0,0 +1,109 @@ +var CO__driver_8h = +[ + [ "CO_VERSION_MAJOR", "group__CO__driver.html#ga0e351c2972f6d8f2e08fb5ac21a833b8", null ], + [ "CO_VERSION_MINOR", "group__CO__driver.html#gaac3f110c1dd3cfc2b994b5c20d1c6ace", null ], + [ "CO_LITTLE_ENDIAN", "group__CO__dataTypes.html#gaed3e1bffaf912485092fc20193705f35", null ], + [ "CO_SWAP_16", "group__CO__dataTypes.html#ga1717fcaabbe2cd6cd9fc0bc0cb917a6c", null ], + [ "CO_SWAP_32", "group__CO__dataTypes.html#gadf87da54942e3a0ff159688c5e0e267b", null ], + [ "CO_SWAP_64", "group__CO__dataTypes.html#gaec0fc209357883f42d66cd2cdaa7236f", null ], + [ "NULL", "group__CO__dataTypes.html#ga070d2ce7b6bb7e5c05602aa8c308d0c4", null ], + [ "true", "group__CO__dataTypes.html#ga41f9c5fb8b08eb5dc3edce4dcb37fee7", null ], + [ "false", "group__CO__dataTypes.html#ga65e9886d74aaee76545e83dd09011727", null ], + [ "CO_LOCK_CAN_SEND", "group__CO__critical__sections.html#ga7566ee901bbf1a0d76d771d72d2f826f", null ], + [ "CO_UNLOCK_CAN_SEND", "group__CO__critical__sections.html#ga511a5a0bf905c2207d5c9e26d35fe3cc", null ], + [ "CO_LOCK_EMCY", "group__CO__critical__sections.html#ga3052a84235f56d535a14705e0cfda799", null ], + [ "CO_UNLOCK_EMCY", "group__CO__critical__sections.html#ga720a798f2bf7fe20d9c95a212b4df417", null ], + [ "CO_LOCK_OD", "group__CO__critical__sections.html#ga3850830931ced2bd3d7e15821572bbcc", null ], + [ "CO_UNLOCK_OD", "group__CO__critical__sections.html#ga2477f5d24fd31a9f4052cf451b87809f", null ], + [ "CO_FLAG_READ", "group__CO__critical__sections.html#ga577a6ebcf246087f084c75d9ae25eeb7", null ], + [ "CO_FLAG_SET", "group__CO__critical__sections.html#gac54b5e4f680aa8b0177f0df5d5be2e88", null ], + [ "CO_FLAG_CLEAR", "group__CO__critical__sections.html#ga044da4253aeed15c3e0bb7fce13664af", null ], + [ "CO_errinfo", "group__CO__driver.html#gaaa84189910b720ce18c8d83aab405d86", null ], + [ "bool_t", "group__CO__dataTypes.html#ga449976458a084f880dc8e3d29e7eb6f5", null ], + [ "int8_t", "group__CO__dataTypes.html#gaef44329758059c91c76d334e8fc09700", null ], + [ "int16_t", "group__CO__dataTypes.html#ga932e6ccc3d54c58f761c1aead83bd6d7", null ], + [ "int32_t", "group__CO__dataTypes.html#gadb828ef50c2dbb783109824e94cf6c47", null ], + [ "int64_t", "group__CO__dataTypes.html#ga831d6234342279926bb11bad3a37add9", null ], + [ "uint8_t", "group__CO__dataTypes.html#gaba7bc1797add20fe3efdf37ced1182c5", null ], + [ "uint16_t", "group__CO__dataTypes.html#ga1f1825b69244eb3ad2c7165ddc99c956", null ], + [ "uint32_t", "group__CO__dataTypes.html#ga33594304e786b158f3fb30289278f5af", null ], + [ "uint64_t", "group__CO__dataTypes.html#gad27ed092432b64ff558d2254c278720f", null ], + [ "float32_t", "group__CO__dataTypes.html#ga4611b605e45ab401f02cab15c5e38715", null ], + [ "float64_t", "group__CO__dataTypes.html#gac55f3ae81b5bc9053760baacf57e47f4", null ], + [ "char_t", "group__CO__dataTypes.html#ga40bb5262bf908c328fbcfbe5d29d0201", null ], + [ "oChar_t", "group__CO__dataTypes.html#ga00f664c467579d7b2839d6926b6f33a6", null ], + [ "domain_t", "group__CO__dataTypes.html#gadc433a2a90dacd3b2b3801dd9431c254", null ], + [ "CO_Default_CAN_ID_t", "group__CO__driver.html#ga01dd35ae53fd2209ceccabdc8bf8dd06", [ + [ "CO_CAN_ID_NMT_SERVICE", "group__CO__driver.html#gga01dd35ae53fd2209ceccabdc8bf8dd06a335d0f6204819d267ba396b715f66ead", null ], + [ "CO_CAN_ID_GFC", "group__CO__driver.html#gga01dd35ae53fd2209ceccabdc8bf8dd06a0ba8a628aa1a873a21820070261c2783", null ], + [ "CO_CAN_ID_SYNC", "group__CO__driver.html#gga01dd35ae53fd2209ceccabdc8bf8dd06a7a8486aaf2f35eb83c6ca690d0cdce06", null ], + [ "CO_CAN_ID_EMERGENCY", "group__CO__driver.html#gga01dd35ae53fd2209ceccabdc8bf8dd06a94ffef8babcef5b807c5f8c865ef7666", null ], + [ "CO_CAN_ID_TIME", "group__CO__driver.html#gga01dd35ae53fd2209ceccabdc8bf8dd06ab2e20e54189f5cb565e80b05eb8c4931", null ], + [ "CO_CAN_ID_SRDO_1", "group__CO__driver.html#gga01dd35ae53fd2209ceccabdc8bf8dd06aefe4dd6630902d36173b81c106a813bc", null ], + [ "CO_CAN_ID_TPDO_1", "group__CO__driver.html#gga01dd35ae53fd2209ceccabdc8bf8dd06a848f2bdb085bc3a342400a6b43c37f82", null ], + [ "CO_CAN_ID_RPDO_1", "group__CO__driver.html#gga01dd35ae53fd2209ceccabdc8bf8dd06a0ab4be02961987ad817a99a4ef379517", null ], + [ "CO_CAN_ID_TPDO_2", "group__CO__driver.html#gga01dd35ae53fd2209ceccabdc8bf8dd06acb90f0dab2e31982df1bebae6dd02e4b", null ], + [ "CO_CAN_ID_RPDO_2", "group__CO__driver.html#gga01dd35ae53fd2209ceccabdc8bf8dd06a60081a7b09921c6bfce3762a3dd4e49f", null ], + [ "CO_CAN_ID_TPDO_3", "group__CO__driver.html#gga01dd35ae53fd2209ceccabdc8bf8dd06a83b73730655607582d1dabc8f78f7ca4", null ], + [ "CO_CAN_ID_RPDO_3", "group__CO__driver.html#gga01dd35ae53fd2209ceccabdc8bf8dd06ad634b89f227db86bc8c633dda327e5fb", null ], + [ "CO_CAN_ID_TPDO_4", "group__CO__driver.html#gga01dd35ae53fd2209ceccabdc8bf8dd06abe92c9f7938ad6566e8aa010ab6f5cae", null ], + [ "CO_CAN_ID_RPDO_4", "group__CO__driver.html#gga01dd35ae53fd2209ceccabdc8bf8dd06afec9dfa33a34beef50c434e5cde68c6b", null ], + [ "CO_CAN_ID_SDO_SRV", "group__CO__driver.html#gga01dd35ae53fd2209ceccabdc8bf8dd06a3c84d92ad004cfc04e398193b742d30c", null ], + [ "CO_CAN_ID_SDO_CLI", "group__CO__driver.html#gga01dd35ae53fd2209ceccabdc8bf8dd06acfe8640033d9668fafc63aa81d68ede5", null ], + [ "CO_CAN_ID_HEARTBEAT", "group__CO__driver.html#gga01dd35ae53fd2209ceccabdc8bf8dd06a0cfd21623475a1a8522b30b8b16d9874", null ], + [ "CO_CAN_ID_LSS_SLV", "group__CO__driver.html#gga01dd35ae53fd2209ceccabdc8bf8dd06a4a62af7fb0b8768e57945a558a0ceee4", null ], + [ "CO_CAN_ID_LSS_MST", "group__CO__driver.html#gga01dd35ae53fd2209ceccabdc8bf8dd06ad7d16ed89e513b035104e4b2634ce287", null ] + ] ], + [ "CO_CAN_ERR_status_t", "group__CO__driver.html#ga6c5539afb3a95af43f5477d904607426", [ + [ "CO_CAN_ERRTX_WARNING", "group__CO__driver.html#gga6c5539afb3a95af43f5477d904607426a725e4fe057c2f9d222850686a76c724d", null ], + [ "CO_CAN_ERRTX_PASSIVE", "group__CO__driver.html#gga6c5539afb3a95af43f5477d904607426a85d05e861dc03e256dccf977ae16ad6e", null ], + [ "CO_CAN_ERRTX_BUS_OFF", "group__CO__driver.html#gga6c5539afb3a95af43f5477d904607426ae58aa7d0eb5d13630f858a3869f0ee7d", null ], + [ "CO_CAN_ERRTX_OVERFLOW", "group__CO__driver.html#gga6c5539afb3a95af43f5477d904607426ad6fba8a27d82774f53812d3a49655d12", null ], + [ "CO_CAN_ERRTX_PDO_LATE", "group__CO__driver.html#gga6c5539afb3a95af43f5477d904607426ac0b993c7786f41a8c73ad0339124881b", null ], + [ "CO_CAN_ERRRX_WARNING", "group__CO__driver.html#gga6c5539afb3a95af43f5477d904607426a6df88a8a296eb4addc12d9136c0566b0", null ], + [ "CO_CAN_ERRRX_PASSIVE", "group__CO__driver.html#gga6c5539afb3a95af43f5477d904607426a51494ad0df1e2de6d9395f1803c4b233", null ], + [ "CO_CAN_ERRRX_OVERFLOW", "group__CO__driver.html#gga6c5539afb3a95af43f5477d904607426a714a0b9c0978ffde5f138a81880a1fdd", null ], + [ "CO_CAN_ERR_WARN_PASSIVE", "group__CO__driver.html#gga6c5539afb3a95af43f5477d904607426a5ad44f86d5691f2bc809f722364516e0", null ] + ] ], + [ "CO_ReturnError_t", "group__CO__driver.html#ga1cb2d3466eb0c6d267f3b5ff1a0d9532", [ + [ "CO_ERROR_NO", "group__CO__driver.html#gga1cb2d3466eb0c6d267f3b5ff1a0d9532aff1c3e9fd4bf65e6757b020f752cdac8", null ], + [ "CO_ERROR_ILLEGAL_ARGUMENT", "group__CO__driver.html#gga1cb2d3466eb0c6d267f3b5ff1a0d9532a3e78d588c0e21630c9cb7b43bfda3dae", null ], + [ "CO_ERROR_OUT_OF_MEMORY", "group__CO__driver.html#gga1cb2d3466eb0c6d267f3b5ff1a0d9532a2f07295c4c6539c2b390db2d7c351267", null ], + [ "CO_ERROR_TIMEOUT", "group__CO__driver.html#gga1cb2d3466eb0c6d267f3b5ff1a0d9532a129c7141c52ae59d37a2fff163fec600", null ], + [ "CO_ERROR_ILLEGAL_BAUDRATE", "group__CO__driver.html#gga1cb2d3466eb0c6d267f3b5ff1a0d9532a0214308865e83b8c21de7317b3070097", null ], + [ "CO_ERROR_RX_OVERFLOW", "group__CO__driver.html#gga1cb2d3466eb0c6d267f3b5ff1a0d9532ab0746d0801a99639077b325594b347b5", null ], + [ "CO_ERROR_RX_PDO_OVERFLOW", "group__CO__driver.html#gga1cb2d3466eb0c6d267f3b5ff1a0d9532a7e4cefeb35775754c87cf6a559f1bbd9", null ], + [ "CO_ERROR_RX_MSG_LENGTH", "group__CO__driver.html#gga1cb2d3466eb0c6d267f3b5ff1a0d9532a3e40e8440a8480c413d3ff724b875de4", null ], + [ "CO_ERROR_RX_PDO_LENGTH", "group__CO__driver.html#gga1cb2d3466eb0c6d267f3b5ff1a0d9532a4829a460cbba557a2ff7f52bd912aa65", null ], + [ "CO_ERROR_TX_OVERFLOW", "group__CO__driver.html#gga1cb2d3466eb0c6d267f3b5ff1a0d9532a757b4d226a3e00a9e2543cf21833d46f", null ], + [ "CO_ERROR_TX_PDO_WINDOW", "group__CO__driver.html#gga1cb2d3466eb0c6d267f3b5ff1a0d9532a48993dc18698738d168071b4f4c3d244", null ], + [ "CO_ERROR_TX_UNCONFIGURED", "group__CO__driver.html#gga1cb2d3466eb0c6d267f3b5ff1a0d9532a6f07e7c21acde035b561edc7f55edb89", null ], + [ "CO_ERROR_OD_PARAMETERS", "group__CO__driver.html#gga1cb2d3466eb0c6d267f3b5ff1a0d9532a737e67c9f8d3ca882a3cba852153c1f3", null ], + [ "CO_ERROR_DATA_CORRUPT", "group__CO__driver.html#gga1cb2d3466eb0c6d267f3b5ff1a0d9532aa053301213d13670e9af7d31abc1ee48", null ], + [ "CO_ERROR_CRC", "group__CO__driver.html#gga1cb2d3466eb0c6d267f3b5ff1a0d9532a25eaff6474c6bc2aae67b1e7c7a35109", null ], + [ "CO_ERROR_TX_BUSY", "group__CO__driver.html#gga1cb2d3466eb0c6d267f3b5ff1a0d9532ab47ecc1b08463eb986fd29c187096343", null ], + [ "CO_ERROR_WRONG_NMT_STATE", "group__CO__driver.html#gga1cb2d3466eb0c6d267f3b5ff1a0d9532a07457c9c6eda89fc0260e6d7e431424c", null ], + [ "CO_ERROR_SYSCALL", "group__CO__driver.html#gga1cb2d3466eb0c6d267f3b5ff1a0d9532a46d81c922ecbaf8f136626725ad8399d", null ], + [ "CO_ERROR_INVALID_STATE", "group__CO__driver.html#gga1cb2d3466eb0c6d267f3b5ff1a0d9532a57f1c4d2d960ab2550669a1c5ebbf4e1", null ], + [ "CO_ERROR_NODE_ID_UNCONFIGURED_LSS", "group__CO__driver.html#gga1cb2d3466eb0c6d267f3b5ff1a0d9532ac7a6f0554ae52997b88a97b46a16e5a3", null ] + ] ], + [ "CANrx_callback", "group__CO__CAN__Message__reception.html#ga23168979123a5ca8a87d49307eb2990e", null ], + [ "CO_CANrxMsg_readIdent", "group__CO__CAN__Message__reception.html#ga018e9159b92165b2506a6673113cdc0e", null ], + [ "CO_CANrxMsg_readDLC", "group__CO__CAN__Message__reception.html#gacec1dcf8b7e66ea2e65905f91321b299", null ], + [ "CO_CANrxMsg_readData", "group__CO__CAN__Message__reception.html#gab02a5fe910acf9aa5021f97e523697f9", null ], + [ "CO_CANsetConfigurationMode", "group__CO__driver.html#ga88ef52baae169a80e4c2f2cb93b33747", null ], + [ "CO_CANsetNormalMode", "group__CO__driver.html#gad654edfa651bf7b68263913786697200", null ], + [ "CO_CANmodule_init", "group__CO__driver.html#ga3a1131813110529494cee5e27c0bf28d", null ], + [ "CO_CANmodule_disable", "group__CO__driver.html#gac6f60f9da27dda0c9b3950c4e96bd687", null ], + [ "CO_CANrxBufferInit", "group__CO__driver.html#ga25ee22cd2e3a2f3bb49990e8bc3076b0", null ], + [ "CO_CANtxBufferInit", "group__CO__driver.html#ga01e2ee79e7e3a8b321dac831e7e00976", null ], + [ "CO_CANsend", "group__CO__driver.html#ga4664a9f5d547cb0605a9e929fb079f2e", null ], + [ "CO_CANclearPendingSyncPDOs", "group__CO__driver.html#gabbeac85cbf513162b11fc3d0717b0753", null ], + [ "CO_CANmodule_process", "group__CO__driver.html#ga066c6742f75b2daac585735ffd6c9928", null ], + [ "CO_getUint8", "group__CO__driver.html#ga27a4052f87c60cf2df472378689c2ef9", null ], + [ "CO_getUint16", "group__CO__driver.html#ga6590fe7a05ecb4b81ee3d7e233274ea4", null ], + [ "CO_getUint32", "group__CO__driver.html#ga21cd9e2391f276b908bcde5769e967ed", null ], + [ "CO_setUint8", "group__CO__driver.html#ga61de0908305223a0a7949e184cc1d644", null ], + [ "CO_setUint16", "group__CO__driver.html#gaf74a6b7343bc9d6efd5ec5d9d9f965fb", null ], + [ "CO_setUint32", "group__CO__driver.html#ga9dbc7193fbd875503e0b3b34ea22434e", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/CO__driver_8h_source.html b/Devices/Libraries/Systems/CANopenSocket/docs/CO__driver_8h_source.html new file mode 100755 index 0000000..c60652c --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/CO__driver_8h_source.html @@ -0,0 +1,480 @@ + + + + + + + +CANopenNode: 301/CO_driver.h Source File + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+
CO_driver.h
+
+
+Go to the documentation of this file.
1 
+
26 #ifndef CO_DRIVER_H
+
27 #define CO_DRIVER_H
+
28 
+
29 #include <string.h>
+
30 
+
31 #include "CO_config.h"
+
32 #include "CO_driver_target.h"
+
33 
+
34 #ifdef __cplusplus
+
35 extern "C" {
+
36 #endif
+
37 
+
38 #ifdef CO_DEBUG_COMMON
+
39 #if (CO_CONFIG_DEBUG) & CO_CONFIG_DEBUG_SDO_CLIENT
+
40  #define CO_DEBUG_SDO_CLIENT(msg) CO_DEBUG_COMMON(msg)
+
41 #endif
+
42 #if (CO_CONFIG_DEBUG) & CO_CONFIG_DEBUG_SDO_SERVER
+
43  #define CO_DEBUG_SDO_SERVER(msg) CO_DEBUG_COMMON(msg)
+
44 #endif
+
45 #endif
+
46 
+
102 #define CO_VERSION_MAJOR 4
+
103 
+
104 #define CO_VERSION_MINOR 0
+
105 
+
106 
+
107 /* Macros and declarations in following part are only used for documentation. */
+
108 #ifdef CO_DOXYGEN
+
109 
+
127 #define CO_LITTLE_ENDIAN
+
128 
+
129 #define CO_SWAP_16(x) x
+
130 
+
131 #define CO_SWAP_32(x) x
+
132 
+
133 #define CO_SWAP_64(x) x
+
134 
+
135 #define NULL (0)
+
136 
+
137 #define true 1
+
138 
+
139 #define false 0
+
140 
+
141 typedef unsigned char bool_t;
+
143 typedef signed char int8_t;
+
145 typedef signed int int16_t;
+
147 typedef signed long int int32_t;
+
149 typedef signed long long int int64_t;
+
151 typedef unsigned char uint8_t;
+
153 typedef unsigned int uint16_t;
+
155 typedef unsigned long int uint32_t;
+
157 typedef unsigned long long int uint64_t;
+
159 typedef float float32_t;
+
161 typedef double float64_t;
+
163 typedef char char_t;
+
165 typedef unsigned char oChar_t;
+
167 typedef unsigned char domain_t;
+
202 void CANrx_callback(void *object, void *rxMsg);
+
203 
+
218 static inline uint16_t CO_CANrxMsg_readIdent(void *rxMsg) {
+
219  return 0;
+
220 }
+
221 
+
230 static inline uint8_t CO_CANrxMsg_readDLC(void *rxMsg) {
+
231  return 0;
+
232 }
+
233 
+
242 static inline uint8_t *CO_CANrxMsg_readData(void *rxMsg) {
+
243  return NULL;
+
244 }
+
245 
+
257 typedef struct {
+
258  uint16_t ident;
+
259  uint16_t mask;
+
261  void *object;
+
263  void (*pCANrx_callback)(
+
264  void *object, void *message);
+
266 } CO_CANrx_t;
+
299 typedef struct {
+
300  uint32_t ident;
+
301  uint8_t DLC;
+
302  uint8_t data[8];
+
303  volatile bool_t bufferFull;
+
305  volatile bool_t syncFlag;
+
307 } CO_CANtx_t;
+
319 typedef struct {
+
320  void *CANptr;
+
321  CO_CANrx_t *rxArray;
+
322  uint16_t rxSize;
+
323  CO_CANtx_t *txArray;
+
324  uint16_t txSize;
+
325  uint16_t CANerrorStatus;
+
327  volatile bool_t CANnormal;
+
328  volatile bool_t useCANrxFilters;
+
332  volatile bool_t bufferInhibitFlag;
+
336  volatile bool_t firstCANtxMessage;
+
338  volatile uint16_t CANtxCount;
+
340  uint32_t errOld;
+
341  int32_t errinfo;
+
342 } CO_CANmodule_t;
+
343 
+
344 
+
388 #define CO_LOCK_CAN_SEND()
+
389 
+
390 #define CO_UNLOCK_CAN_SEND()
+
391 
+
392 #define CO_LOCK_EMCY()
+
393 
+
394 #define CO_UNLOCK_EMCY()
+
395 
+
396 #define CO_LOCK_OD()
+
397 
+
398 #define CO_UNLOCK_OD()
+
399 
+
401 #define CO_FLAG_READ(rxNew) ((rxNew) != NULL)
+
402 
+
403 #define CO_FLAG_SET(rxNew) { __sync_synchronize(); rxNew = (void *)1L; }
+
404 
+
405 #define CO_FLAG_CLEAR(rxNew) { __sync_synchronize(); rxNew = NULL; }
+
406 
+
408 #endif /* CO_DOXYGEN */
+
409 
+
410 
+
424 #ifndef CO_errinfo
+
425 #define CO_errinfo(CANmodule, err)
+
426 #endif
+
427 
+
428 
+
437 typedef enum {
+
438  CO_CAN_ID_NMT_SERVICE = 0x000,
+
439  CO_CAN_ID_GFC = 0x001,
+
440  CO_CAN_ID_SYNC = 0x080,
+
441  CO_CAN_ID_EMERGENCY = 0x080,
+
442  CO_CAN_ID_TIME = 0x100,
+
443  CO_CAN_ID_SRDO_1 = 0x0FF,
+
444  CO_CAN_ID_TPDO_1 = 0x180,
+
445  CO_CAN_ID_RPDO_1 = 0x200,
+
446  CO_CAN_ID_TPDO_2 = 0x280,
+
447  CO_CAN_ID_RPDO_2 = 0x300,
+
448  CO_CAN_ID_TPDO_3 = 0x380,
+
449  CO_CAN_ID_RPDO_3 = 0x400,
+
450  CO_CAN_ID_TPDO_4 = 0x480,
+
451  CO_CAN_ID_RPDO_4 = 0x500,
+
452  CO_CAN_ID_SDO_SRV = 0x580,
+
453  CO_CAN_ID_SDO_CLI = 0x600,
+
454  CO_CAN_ID_HEARTBEAT = 0x700,
+
455  CO_CAN_ID_LSS_SLV = 0x7E4,
+
456  CO_CAN_ID_LSS_MST = 0x7E5
+
457 } CO_Default_CAN_ID_t;
+
458 
+
459 
+
468 typedef enum {
+
469  CO_CAN_ERRTX_WARNING = 0x0001,
+
470  CO_CAN_ERRTX_PASSIVE = 0x0002,
+
471  CO_CAN_ERRTX_BUS_OFF = 0x0004,
+
472  CO_CAN_ERRTX_OVERFLOW = 0x0008,
+
474  CO_CAN_ERRTX_PDO_LATE = 0x0080,
+
476  CO_CAN_ERRRX_WARNING = 0x0100,
+
477  CO_CAN_ERRRX_PASSIVE = 0x0200,
+
478  CO_CAN_ERRRX_OVERFLOW = 0x0800,
+
480  CO_CAN_ERR_WARN_PASSIVE = 0x0303
+
481 } CO_CAN_ERR_status_t;
+
482 
+
483 
+
488 typedef enum {
+
489  CO_ERROR_NO = 0,
+
490  CO_ERROR_ILLEGAL_ARGUMENT = -1,
+
491  CO_ERROR_OUT_OF_MEMORY = -2,
+
492  CO_ERROR_TIMEOUT = -3,
+
493  CO_ERROR_ILLEGAL_BAUDRATE = -4,
+
495  CO_ERROR_RX_OVERFLOW = -5,
+
497  CO_ERROR_RX_PDO_OVERFLOW = -6,
+
498  CO_ERROR_RX_MSG_LENGTH = -7,
+
499  CO_ERROR_RX_PDO_LENGTH = -8,
+
500  CO_ERROR_TX_OVERFLOW = -9,
+
502  CO_ERROR_TX_PDO_WINDOW = -10,
+
503  CO_ERROR_TX_UNCONFIGURED = -11,
+
505  CO_ERROR_OD_PARAMETERS = -12,
+
506  CO_ERROR_DATA_CORRUPT = -13,
+
507  CO_ERROR_CRC = -14,
+
508  CO_ERROR_TX_BUSY = -15,
+
510  CO_ERROR_WRONG_NMT_STATE = -16,
+
512  CO_ERROR_SYSCALL = -17,
+
513  CO_ERROR_INVALID_STATE = -18,
+
514  CO_ERROR_NODE_ID_UNCONFIGURED_LSS = -19
+
517 } CO_ReturnError_t;
+
518 
+
519 
+
525 void CO_CANsetConfigurationMode(void *CANptr);
+
526 
+
527 
+
533 void CO_CANsetNormalMode(CO_CANmodule_t *CANmodule);
+
534 
+
535 
+
555 CO_ReturnError_t CO_CANmodule_init(CO_CANmodule_t *CANmodule,
+
556  void *CANptr,
+
557  CO_CANrx_t rxArray[],
+
558  uint16_t rxSize,
+
559  CO_CANtx_t txArray[],
+
560  uint16_t txSize,
+
561  uint16_t CANbitRate);
+
562 
+
563 
+
569 void CO_CANmodule_disable(CO_CANmodule_t *CANmodule);
+
570 
+
571 
+
597 CO_ReturnError_t CO_CANrxBufferInit(CO_CANmodule_t *CANmodule,
+
598  uint16_t index,
+
599  uint16_t ident,
+
600  uint16_t mask,
+
601  bool_t rtr,
+
602  void *object,
+
603  void (*CANrx_callback)(void *object,
+
604  void *message));
+
605 
+
606 
+
625 CO_CANtx_t *CO_CANtxBufferInit(CO_CANmodule_t *CANmodule,
+
626  uint16_t index,
+
627  uint16_t ident,
+
628  bool_t rtr,
+
629  uint8_t noOfBytes,
+
630  bool_t syncFlag);
+
631 
+
632 
+
643 CO_ReturnError_t CO_CANsend(CO_CANmodule_t *CANmodule, CO_CANtx_t *buffer);
+
644 
+
645 
+
660 void CO_CANclearPendingSyncPDOs(CO_CANmodule_t *CANmodule);
+
661 
+
662 
+
671 void CO_CANmodule_process(CO_CANmodule_t *CANmodule);
+
672 
+
673 
+
681 static inline uint8_t CO_getUint8(const void *buf) {
+
682  uint8_t value; memmove(&value, buf, sizeof(value)); return value;
+
683 }
+
685 static inline uint16_t CO_getUint16(const void *buf) {
+
686  uint16_t value; memmove(&value, buf, sizeof(value)); return value;
+
687 }
+
689 static inline uint32_t CO_getUint32(const void *buf) {
+
690  uint32_t value; memmove(&value, buf, sizeof(value)); return value;
+
691 }
+
692 
+
701 static inline uint8_t CO_setUint8(void *buf, uint8_t value) {
+
702  memmove(buf, &value, sizeof(value)); return sizeof(value);
+
703 }
+
705 static inline uint8_t CO_setUint16(void *buf, uint16_t value) {
+
706  memmove(buf, &value, sizeof(value)); return sizeof(value);
+
707 }
+
709 static inline uint8_t CO_setUint32(void *buf, uint32_t value) {
+
710  memmove(buf, &value, sizeof(value)); return sizeof(value);
+
711 }
+
712  /* CO_driver */
+
714 
+
715 #ifdef __cplusplus
+
716 }
+
717 #endif /* __cplusplus */
+
718 
+
719 #endif /* CO_DRIVER_H */
+
+
+
uint32_t
unsigned long int uint32_t
UNSIGNED32 in CANopen (0007h), 32-bit unsigned integer.
Definition: CO_driver.h:155
+
CO_CANmodule_t::firstCANtxMessage
volatile bool_t firstCANtxMessage
Equal to 1, when the first transmitted message (bootup message) is in CAN TX buffers.
Definition: CO_driver.h:336
+
CO_CAN_ID_TPDO_1
@ CO_CAN_ID_TPDO_1
0x180, Default TPDO1 (+nodeID)
Definition: CO_driver.h:444
+
CO_ERROR_DATA_CORRUPT
@ CO_ERROR_DATA_CORRUPT
Stored data are corrupt.
Definition: CO_driver.h:506
+
CO_CAN_ID_GFC
@ CO_CAN_ID_GFC
0x001, Global fail-safe command
Definition: CO_driver.h:439
+
CO_CANrx_t::mask
uint16_t mask
Standard CAN Identifier mask with the same alignment as ident.
Definition: CO_driver.h:259
+
CO_CANmodule_t::txArray
CO_CANtx_t * txArray
From CO_CANmodule_init()
Definition: CO_driver.h:323
+
CO_CANtxBufferInit
CO_CANtx_t * CO_CANtxBufferInit(CO_CANmodule_t *CANmodule, uint16_t index, uint16_t ident, bool_t rtr, uint8_t noOfBytes, bool_t syncFlag)
Configure CAN message transmit buffer.
Definition: CO_driver.c:519
+
CO_CANclearPendingSyncPDOs
void CO_CANclearPendingSyncPDOs(CO_CANmodule_t *CANmodule)
Clear all synchronous TPDOs from CAN module transmit buffers.
Definition: CO_driver.c:707
+
CO_CAN_ID_NMT_SERVICE
@ CO_CAN_ID_NMT_SERVICE
0x000, Network management
Definition: CO_driver.h:438
+
CO_CANrx_t::object
void * object
CANopenNode Object initialized in from CO_CANrxBufferInit()
Definition: CO_driver.h:261
+
char_t
char char_t
VISIBLE_STRING in CANopen (0009h), string of signed 8-bit values.
Definition: CO_driver.h:163
+
CO_setUint16
static uint8_t CO_setUint16(void *buf, uint16_t value)
Write uint16_t value into memory buffer, see CO_setUint8.
Definition: CO_driver.h:705
+
CO_ERROR_NO
@ CO_ERROR_NO
Operation completed successfully.
Definition: CO_driver.h:489
+
CO_CAN_ID_TPDO_2
@ CO_CAN_ID_TPDO_2
0x280, Default TPDO2 (+nodeID)
Definition: CO_driver.h:446
+
int64_t
signed long long int int64_t
INTEGER64 in CANopen (0015h), 64-bit signed integer.
Definition: CO_driver.h:149
+
uint16_t
unsigned int uint16_t
UNSIGNED16 in CANopen (0006h), 16-bit unsigned integer.
Definition: CO_driver.h:153
+
CO_ERROR_RX_PDO_OVERFLOW
@ CO_ERROR_RX_PDO_OVERFLOW
previous PDO was not processed yet
Definition: CO_driver.h:497
+
CO_CAN_ID_SRDO_1
@ CO_CAN_ID_SRDO_1
0x0FF, Default SRDO1 (+2*nodeID)
Definition: CO_driver.h:443
+
CO_ReturnError_t
CO_ReturnError_t
Return values of some CANopen functions.
Definition: CO_driver.h:488
+
CO_CAN_ERRTX_PASSIVE
@ CO_CAN_ERRTX_PASSIVE
0x0002, CAN transmitter passive
Definition: CO_driver.h:470
+
CO_CANrxMsg_readDLC
static uint8_t CO_CANrxMsg_readDLC(void *rxMsg)
CANrx_callback() can read Data Length Code from received CAN message.
Definition: CO_driver.h:230
+
CO_CAN_ID_EMERGENCY
@ CO_CAN_ID_EMERGENCY
0x080, Emergency messages (+nodeID)
Definition: CO_driver.h:441
+
CO_CAN_ERRTX_BUS_OFF
@ CO_CAN_ERRTX_BUS_OFF
0x0004, CAN transmitter bus off
Definition: CO_driver.h:471
+
CO_CAN_ERRTX_OVERFLOW
@ CO_CAN_ERRTX_OVERFLOW
0x0008, CAN transmitter overflow
Definition: CO_driver.h:472
+
bool_t
unsigned char bool_t
Boolean data type for general use.
Definition: CO_driver.h:141
+
CO_ERROR_TX_UNCONFIGURED
@ CO_ERROR_TX_UNCONFIGURED
Transmit buffer was not configured properly.
Definition: CO_driver.h:503
+
CANrx_callback
void CANrx_callback(void *object, void *rxMsg)
CAN receive callback function which pre-processes received CAN message.
+
CO_CANmodule_init
CO_ReturnError_t CO_CANmodule_init(CO_CANmodule_t *CANmodule, void *CANptr, CO_CANrx_t rxArray[], uint16_t rxSize, CO_CANtx_t txArray[], uint16_t txSize, uint16_t CANbitRate)
Initialize CAN module object.
Definition: CO_driver.c:198
+
CO_setUint8
static uint8_t CO_setUint8(void *buf, uint8_t value)
Write uint8_t value into memory buffer.
Definition: CO_driver.h:701
+
CO_CAN_ERRRX_PASSIVE
@ CO_CAN_ERRRX_PASSIVE
0x0200, CAN receiver passive
Definition: CO_driver.h:477
+
CO_CANrxMsg_readData
static uint8_t * CO_CANrxMsg_readData(void *rxMsg)
CANrx_callback() can read pointer to data from received CAN message.
Definition: CO_driver.h:242
+
CO_CAN_ID_RPDO_2
@ CO_CAN_ID_RPDO_2
0x300, Default RPDO2 (+nodeID)
Definition: CO_driver.h:447
+
int32_t
signed long int int32_t
INTEGER32 in CANopen (0004h), 32-bit signed integer.
Definition: CO_driver.h:147
+
CO_CANrxBufferInit
CO_ReturnError_t CO_CANrxBufferInit(CO_CANmodule_t *CANmodule, uint16_t index, uint16_t ident, uint16_t mask, bool_t rtr, void *object, void(*CANrx_callback)(void *object, void *message))
Configure CAN message receive buffer.
Definition: CO_driver.c:428
+
uint64_t
unsigned long long int uint64_t
UNSIGNED64 in CANopen (001Bh), 64-bit unsigned integer.
Definition: CO_driver.h:157
+
CO_CANmodule_t::useCANrxFilters
volatile bool_t useCANrxFilters
Value different than zero indicates, that CAN module hardware filters are used for CAN reception.
Definition: CO_driver.h:328
+
CO_ERROR_RX_OVERFLOW
@ CO_ERROR_RX_OVERFLOW
Previous message was not processed yet.
Definition: CO_driver.h:495
+
CO_CAN_ERRTX_PDO_LATE
@ CO_CAN_ERRTX_PDO_LATE
0x0080, TPDO is outside sync window
Definition: CO_driver.h:474
+
CO_CAN_ID_TPDO_3
@ CO_CAN_ID_TPDO_3
0x380, Default TPDO3 (+nodeID)
Definition: CO_driver.h:448
+
CO_CAN_ID_RPDO_1
@ CO_CAN_ID_RPDO_1
0x200, Default RPDO1 (+nodeID)
Definition: CO_driver.h:445
+
CO_CAN_ID_HEARTBEAT
@ CO_CAN_ID_HEARTBEAT
0x700, Heartbeat message
Definition: CO_driver.h:454
+
int8_t
signed char int8_t
INTEGER8 in CANopen (0002h), 8-bit signed integer.
Definition: CO_driver.h:143
+
CO_getUint16
static uint16_t CO_getUint16(const void *buf)
Get uint16_t value from memory buffer, see CO_getUint8.
Definition: CO_driver.h:685
+
CO_ERROR_RX_MSG_LENGTH
@ CO_ERROR_RX_MSG_LENGTH
Wrong receive message length.
Definition: CO_driver.h:498
+
CO_CANrxMsg_readIdent
static uint16_t CO_CANrxMsg_readIdent(void *rxMsg)
CANrx_callback() can read CAN identifier from received CAN message.
Definition: CO_driver.h:218
+
CO_CANmodule_t::CANptr
void * CANptr
From CO_CANmodule_init()
Definition: CO_driver.h:320
+
CO_driver_target.h
Linux socketCAN specific definitions for CANopenNode.
+
CO_CAN_ID_TPDO_4
@ CO_CAN_ID_TPDO_4
0x480, Default TPDO4 (+nodeID)
Definition: CO_driver.h:450
+
CO_CAN_ID_LSS_MST
@ CO_CAN_ID_LSS_MST
0x7E5, LSS request from master
Definition: CO_driver.h:456
+
CO_ERROR_TX_OVERFLOW
@ CO_ERROR_TX_OVERFLOW
Previous message is still waiting, buffer full.
Definition: CO_driver.h:500
+
CO_CAN_ID_SYNC
@ CO_CAN_ID_SYNC
0x080, Synchronous message
Definition: CO_driver.h:440
+
float64_t
double float64_t
REAL64 in CANopen (0011h), double precision floating point value, 64-bit.
Definition: CO_driver.h:161
+
CO_CANmodule_disable
void CO_CANmodule_disable(CO_CANmodule_t *CANmodule)
Switch off CANmodule.
Definition: CO_driver.c:392
+
CO_CANtx_t::ident
uint32_t ident
CAN identifier as aligned in CAN module.
Definition: CO_driver.h:300
+
CO_config.h
Configuration macros for CANopenNode.
+
float32_t
float float32_t
REAL32 in CANopen (0008h), single precision floating point value, 32-bit.
Definition: CO_driver.h:159
+
CO_CANtx_t::bufferFull
volatile bool_t bufferFull
True if previous message is still in the buffer.
Definition: CO_driver.h:303
+
CO_ERROR_SYSCALL
@ CO_ERROR_SYSCALL
Syscall failed.
Definition: CO_driver.h:512
+
oChar_t
unsigned char oChar_t
OCTET_STRING in CANopen (000Ah), string of unsigned 8-bit values.
Definition: CO_driver.h:165
+
CO_ERROR_TX_PDO_WINDOW
@ CO_ERROR_TX_PDO_WINDOW
Synchronous TPDO is outside window.
Definition: CO_driver.h:502
+
CO_CAN_ID_SDO_CLI
@ CO_CAN_ID_SDO_CLI
0x600, SDO request from client (+nodeID)
Definition: CO_driver.h:453
+
CO_ERROR_INVALID_STATE
@ CO_ERROR_INVALID_STATE
Driver not ready.
Definition: CO_driver.h:513
+
CO_CANmodule_t::rxArray
CO_CANrx_t * rxArray
From CO_CANmodule_init()
Definition: CO_driver.h:321
+
CO_CANmodule_process
void CO_CANmodule_process(CO_CANmodule_t *CANmodule)
Process can module - verify CAN errors.
Definition: CO_driver.c:715
+
CO_CAN_ID_LSS_SLV
@ CO_CAN_ID_LSS_SLV
0x7E4, LSS response from slave
Definition: CO_driver.h:455
+
CO_CANmodule_t::rxSize
uint16_t rxSize
From CO_CANmodule_init()
Definition: CO_driver.h:322
+
CO_ERROR_CRC
@ CO_ERROR_CRC
CRC does not match.
Definition: CO_driver.h:507
+
CO_ERROR_OD_PARAMETERS
@ CO_ERROR_OD_PARAMETERS
Error in Object Dictionary parameters.
Definition: CO_driver.h:505
+
CO_ERROR_ILLEGAL_BAUDRATE
@ CO_ERROR_ILLEGAL_BAUDRATE
Illegal baudrate passed to function CO_CANmodule_init()
Definition: CO_driver.h:493
+
CO_ERROR_ILLEGAL_ARGUMENT
@ CO_ERROR_ILLEGAL_ARGUMENT
Error in function arguments.
Definition: CO_driver.h:490
+
CO_CANmodule_t::CANerrorStatus
uint16_t CANerrorStatus
CAN error status bitfield, see CO_CAN_ERR_status_t.
Definition: CO_driver.h:325
+
CO_CAN_ERR_WARN_PASSIVE
@ CO_CAN_ERR_WARN_PASSIVE
0x0303, combination
Definition: CO_driver.h:480
+
CO_ERROR_TX_BUSY
@ CO_ERROR_TX_BUSY
Sending rejected because driver is busy.
Definition: CO_driver.h:508
+
CO_ERROR_WRONG_NMT_STATE
@ CO_ERROR_WRONG_NMT_STATE
Command can't be processed in current state.
Definition: CO_driver.h:510
+
CO_CAN_ID_RPDO_4
@ CO_CAN_ID_RPDO_4
0x500, Default RPDO5 (+nodeID)
Definition: CO_driver.h:451
+
CO_ERROR_TIMEOUT
@ CO_ERROR_TIMEOUT
Function timeout.
Definition: CO_driver.h:492
+
CO_ERROR_RX_PDO_LENGTH
@ CO_ERROR_RX_PDO_LENGTH
Wrong receive PDO length.
Definition: CO_driver.h:499
+
int16_t
signed int int16_t
INTEGER16 in CANopen (0003h), 16-bit signed integer.
Definition: CO_driver.h:145
+
CO_CANsetConfigurationMode
void CO_CANsetConfigurationMode(void *CANptr)
Request CAN configuration (stopped) mode and wait until it is set.
Definition: CO_driver.c:173
+
CO_getUint32
static uint32_t CO_getUint32(const void *buf)
Get uint32_t value from memory buffer, see CO_getUint8.
Definition: CO_driver.h:689
+
CO_CANtx_t::DLC
uint8_t DLC
Length of CAN message.
Definition: CO_driver.h:301
+
CO_CANmodule_t::txSize
uint16_t txSize
From CO_CANmodule_init()
Definition: CO_driver.h:324
+
CO_CAN_ERRRX_WARNING
@ CO_CAN_ERRRX_WARNING
0x0100, CAN receiver warning
Definition: CO_driver.h:476
+
CO_CANmodule_t::errinfo
int32_t errinfo
For use with CO_errinfo()
Definition: CO_driver.h:341
+
CO_ERROR_NODE_ID_UNCONFIGURED_LSS
@ CO_ERROR_NODE_ID_UNCONFIGURED_LSS
Node-id is in LSS unconfigured state.
Definition: CO_driver.h:514
+
CO_CANrx_t
Configuration object for CAN received message for specific CANopenNode Object.
Definition: CO_driver.h:257
+
CO_CAN_ERR_status_t
CO_CAN_ERR_status_t
CAN error status bitmasks.
Definition: CO_driver.h:468
+
NULL
#define NULL
NULL, for general usage.
Definition: CO_driver.h:135
+
CO_CAN_ID_RPDO_3
@ CO_CAN_ID_RPDO_3
0x400, Default RPDO3 (+nodeID)
Definition: CO_driver.h:449
+
CO_Default_CAN_ID_t
CO_Default_CAN_ID_t
Default CANopen identifiers.
Definition: CO_driver.h:437
+
CO_CANmodule_t
Complete CAN module object.
Definition: CO_driver.h:319
+
CO_CANrx_t::ident
uint16_t ident
Standard CAN Identifier (bits 0..10) + RTR (bit 11)
Definition: CO_driver.h:258
+
CO_CANmodule_t::CANtxCount
volatile uint16_t CANtxCount
Number of messages in transmit buffer, which are waiting to be copied to the CAN module.
Definition: CO_driver.h:338
+
CO_getUint8
static uint8_t CO_getUint8(const void *buf)
Get uint8_t value from memory buffer.
Definition: CO_driver.h:681
+
CO_CANmodule_t::CANnormal
volatile bool_t CANnormal
CAN module is in normal mode.
Definition: CO_driver.h:327
+
CO_CANsetNormalMode
void CO_CANsetNormalMode(CO_CANmodule_t *CANmodule)
Request CAN normal (operational) mode and wait until it is set.
Definition: CO_driver.c:181
+
CO_ERROR_OUT_OF_MEMORY
@ CO_ERROR_OUT_OF_MEMORY
Memory allocation failed.
Definition: CO_driver.h:491
+
CO_CANmodule_t::errOld
uint32_t errOld
Previous state of CAN errors.
Definition: CO_driver.h:340
+
domain_t
unsigned char domain_t
DOMAIN in CANopen (000Fh), used to transfer a large block of data.
Definition: CO_driver.h:167
+
CO_CANmodule_t::bufferInhibitFlag
volatile bool_t bufferInhibitFlag
If flag is true, then message in transmit buffer is synchronous PDO message, which will be aborted,...
Definition: CO_driver.h:332
+
CO_CAN_ERRRX_OVERFLOW
@ CO_CAN_ERRRX_OVERFLOW
0x0800, CAN receiver overflow
Definition: CO_driver.h:478
+
CO_CAN_ID_TIME
@ CO_CAN_ID_TIME
0x100, Time message
Definition: CO_driver.h:442
+
CO_CAN_ID_SDO_SRV
@ CO_CAN_ID_SDO_SRV
0x580, SDO response from server (+nodeID)
Definition: CO_driver.h:452
+
CO_CAN_ERRTX_WARNING
@ CO_CAN_ERRTX_WARNING
0x0001, CAN transmitter warning
Definition: CO_driver.h:469
+
CO_CANtx_t
Configuration object for CAN transmit message for specific CANopenNode Object.
Definition: CO_driver.h:299
+
CO_CANtx_t::syncFlag
volatile bool_t syncFlag
Synchronous PDO messages has this flag set.
Definition: CO_driver.h:305
+
CO_CANsend
CO_ReturnError_t CO_CANsend(CO_CANmodule_t *CANmodule, CO_CANtx_t *buffer)
Send CAN message.
Definition: CO_driver.c:664
+
CO_setUint32
static uint8_t CO_setUint32(void *buf, uint32_t value)
Write uint32_t value into memory buffer, see CO_setUint8.
Definition: CO_driver.h:709
+
uint8_t
unsigned char uint8_t
UNSIGNED8 in CANopen (0005h), 8-bit unsigned integer.
Definition: CO_driver.h:151
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/CO__driver__target_8h.html b/Devices/Libraries/Systems/CANopenSocket/docs/CO__driver__target_8h.html new file mode 100755 index 0000000..f5d7e97 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/CO__driver__target_8h.html @@ -0,0 +1,163 @@ + + + + + + + +CANopenNode: socketCAN/CO_driver_target.h File Reference + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Macros | +Functions
+
+
CO_driver_target.h File Reference
socketCAN » CO_driver_target.h
+
+
+ +

Linux socketCAN specific definitions for CANopenNode. +More...

+
#include <stddef.h>
+#include <stdbool.h>
+#include <stdint.h>
+#include <endian.h>
+#include <pthread.h>
+#include <linux/can.h>
+#include <net/if.h>
+#include <sys/epoll.h>
+#include "CO_error.h"
+
+

Go to the source code of this file.

+ + + + + + + + +

+Macros

#define CO_DRIVER_MULTI_INTERFACE   0
 Multi interface support. More...
 
#define CO_DRIVER_ERROR_REPORTING   1
 CAN bus error reporting. More...
 
+ + + + + + + + + + + + + +

+Functions

CO_ReturnError_t CO_CANmodule_addInterface (CO_CANmodule_t *CANmodule, int can_ifindex)
 Add socketCAN interface to can driver. More...
 
bool_t CO_CANrxBuffer_getInterface (CO_CANmodule_t *CANmodule, uint16_t ident, const void **const CANptrRx, struct timespec *timestamp)
 Check on which interface the last message for one message buffer was received. More...
 
CO_ReturnError_t CO_CANtxBuffer_setInterface (CO_CANmodule_t *CANmodule, uint16_t ident, const void *CANptrTx)
 Set which interface should be used for message buffer transmission. More...
 
bool_t CO_CANrxFromEpoll (CO_CANmodule_t *CANmodule, struct epoll_event *ev, CO_CANrxMsg_t *buffer, int32_t *msgIndex)
 Receives CAN messages from matching epoll event. More...
 
+

Detailed Description

+

Linux socketCAN specific definitions for CANopenNode.

+
Author
Janez Paternoster
+
+Martin Wagner
+
Copyright
2004 - 2020 Janez Paternoster
+
+2018 - 2020 Neuberger Gebaeudeautomation GmbH
+

This file is part of CANopenNode, an opensource CANopen Stack. Project home page is https://github.com/CANopenNode/CANopenNode. For more information on CANopen see http://www.can-cia.org/.

+

Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. You may obtain a copy of the License at 

http://www.apache.org/licenses/LICENSE-2.0
+

Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License.

+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/CO__driver__target_8h.js b/Devices/Libraries/Systems/CANopenSocket/docs/CO__driver__target_8h.js new file mode 100755 index 0000000..ac144c6 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/CO__driver__target_8h.js @@ -0,0 +1,9 @@ +var CO__driver__target_8h = +[ + [ "CO_DRIVER_MULTI_INTERFACE", "group__CO__socketCAN__driver__target.html#ga63d0056e7f18144c6eee7b66f196377c", null ], + [ "CO_DRIVER_ERROR_REPORTING", "group__CO__socketCAN__driver__target.html#ga88077a1ecc6ae53de0b40ae092d48452", null ], + [ "CO_CANmodule_addInterface", "group__CO__socketCAN__driver__target.html#gaaffa26125993a7a1f6cbfdb468b59333", null ], + [ "CO_CANrxBuffer_getInterface", "group__CO__socketCAN__driver__target.html#gaf266a58e21acf104d9f19cd0da704afe", null ], + [ "CO_CANtxBuffer_setInterface", "group__CO__socketCAN__driver__target.html#ga0d7a8fdf7a2fafd4c6d2f2dd1e1017b0", null ], + [ "CO_CANrxFromEpoll", "group__CO__socketCAN__driver__target.html#ga072c53260a32d7cd30d9ad1b5bb2c359", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/CO__driver__target_8h_source.html b/Devices/Libraries/Systems/CANopenSocket/docs/CO__driver__target_8h_source.html new file mode 100755 index 0000000..b8803fa --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/CO__driver__target_8h_source.html @@ -0,0 +1,503 @@ + + + + + + + +CANopenNode: socketCAN/CO_driver_target.h Source File + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+
CO_driver_target.h
+
+
+Go to the documentation of this file.
1 
+
29 #ifndef CO_DRIVER_TARGET_H
+
30 #define CO_DRIVER_TARGET_H
+
31 
+
32 /* This file contains device and application specific definitions.
+
33  * It is included from CO_driver.h, which contains documentation
+
34  * for common definitions below. */
+
35 
+
36 #include <stddef.h>
+
37 #include <stdbool.h>
+
38 #include <stdint.h>
+
39 #include <endian.h>
+
40 #ifndef CO_SINGLE_THREAD
+
41 #include <pthread.h>
+
42 #endif
+
43 #include <linux/can.h>
+
44 #include <net/if.h>
+
45 #include <sys/epoll.h>
+
46 
+
47 #ifdef CO_DRIVER_CUSTOM
+
48 #include "CO_driver_custom.h"
+
49 #endif
+
50 #include "CO_error.h"
+
51 
+
52 #ifdef __cplusplus
+
53 extern "C" {
+
54 #endif
+
55 
+
56 /* TODO some parts are disabled in non-finished pre-release */
+
57 #define CO_CONFIG_HB_CONS (0)
+
58 #define CO_CONFIG_TIME (0)
+
59 #define CO_CONFIG_SYNC (0)
+
60 #define CO_CONFIG_PDO (0)
+
61 #define CO_CONFIG_TRACE (0)
+
62 
+
63 
+
64 /* Stack configuration override default values.
+
65  * For more information see file CO_config.h. */
+
66 #ifdef CO_SINGLE_THREAD
+
67 #define CO_CONFIG_FLAG_CALLBACK_PRE_USED 0
+
68 #else
+
69 #define CO_CONFIG_FLAG_CALLBACK_PRE_USED CO_CONFIG_FLAG_CALLBACK_PRE
+
70 #endif
+
71 
+
72 #ifndef CO_CONFIG_NMT
+
73 #define CO_CONFIG_NMT (CO_CONFIG_NMT_CALLBACK_CHANGE | \
+
74  CO_CONFIG_NMT_MASTER | \
+
75  CO_CONFIG_FLAG_CALLBACK_PRE_USED | \
+
76  CO_CONFIG_FLAG_TIMERNEXT)
+
77 #endif
+
78 
+
79 #ifndef CO_CONFIG_HB_CONS
+
80 #define CO_CONFIG_HB_CONS (CO_CONFIG_HB_CONS_ENABLE | \
+
81  CO_CONFIG_HB_CONS_CALLBACK_CHANGE | \
+
82  CO_CONFIG_FLAG_CALLBACK_PRE_USED | \
+
83  CO_CONFIG_FLAG_TIMERNEXT)
+
84 #endif
+
85 
+
86 #ifndef CO_CONFIG_EM
+
87 #define CO_CONFIG_EM (CO_CONFIG_EM_PRODUCER | \
+
88  CO_CONFIG_EM_PROD_CONFIGURABLE | \
+
89  CO_CONFIG_EM_PROD_INHIBIT | \
+
90  CO_CONFIG_EM_HISTORY | \
+
91  CO_CONFIG_EM_STATUS_BITS | \
+
92  CO_CONFIG_EM_CONSUMER | \
+
93  CO_CONFIG_FLAG_CALLBACK_PRE_USED | \
+
94  CO_CONFIG_FLAG_TIMERNEXT)
+
95 #endif
+
96 
+
97 #ifndef CO_CONFIG_SDO_SRV
+
98 #define CO_CONFIG_SDO_SRV (CO_CONFIG_SDO_SRV_SEGMENTED | \
+
99  CO_CONFIG_SDO_SRV_BLOCK | \
+
100  CO_CONFIG_FLAG_CALLBACK_PRE_USED | \
+
101  CO_CONFIG_FLAG_TIMERNEXT | \
+
102  CO_CONFIG_FLAG_OD_DYNAMIC)
+
103 #endif
+
104 
+
105 #ifndef CO_CONFIG_SDO_SRV_BUFFER_SIZE
+
106 #define CO_CONFIG_SDO_SRV_BUFFER_SIZE 900
+
107 #endif
+
108 
+
109 #ifndef CO_CONFIG_SDO_CLI
+
110 #define CO_CONFIG_SDO_CLI (CO_CONFIG_SDO_CLI_ENABLE | \
+
111  CO_CONFIG_SDO_CLI_SEGMENTED | \
+
112  CO_CONFIG_SDO_CLI_BLOCK | \
+
113  CO_CONFIG_SDO_CLI_LOCAL | \
+
114  CO_CONFIG_FLAG_CALLBACK_PRE_USED | \
+
115  CO_CONFIG_FLAG_TIMERNEXT | \
+
116  CO_CONFIG_FLAG_OD_DYNAMIC)
+
117 #endif
+
118 
+
119 #ifndef CO_CONFIG_TIME
+
120 #define CO_CONFIG_TIME (CO_CONFIG_TIME_ENABLE | \
+
121  CO_CONFIG_TIME_PRODUCER | \
+
122  CO_CONFIG_FLAG_CALLBACK_PRE_USED)
+
123 #endif
+
124 
+
125 #ifndef CO_CONFIG_SYNC
+
126 #define CO_CONFIG_SYNC (CO_CONFIG_SYNC_ENABLE | \
+
127  CO_CONFIG_SYNC_PRODUCER | \
+
128  CO_CONFIG_FLAG_CALLBACK_PRE_USED | \
+
129  CO_CONFIG_FLAG_TIMERNEXT)
+
130 #endif
+
131 
+
132 #ifndef CO_CONFIG_PDO
+
133 #define CO_CONFIG_PDO (CO_CONFIG_RPDO_ENABLE | \
+
134  CO_CONFIG_TPDO_ENABLE | \
+
135  CO_CONFIG_PDO_SYNC_ENABLE | \
+
136  CO_CONFIG_RPDO_CALLS_EXTENSION | \
+
137  CO_CONFIG_TPDO_CALLS_EXTENSION | \
+
138  CO_CONFIG_FLAG_CALLBACK_PRE_USED | \
+
139  CO_CONFIG_FLAG_TIMERNEXT)
+
140 #endif
+
141 
+
142 #ifndef CO_CONFIG_LEDS
+
143 #define CO_CONFIG_LEDS (CO_CONFIG_LEDS_ENABLE | \
+
144  CO_CONFIG_FLAG_TIMERNEXT)
+
145 #endif
+
146 
+
147 #ifndef CO_CONFIG_LSS
+
148 #define CO_CONFIG_LSS (CO_CONFIG_LSS_SLAVE | \
+
149  CO_CONFIG_LSS_SLAVE_FASTSCAN_DIRECT_RESPOND | \
+
150  CO_CONFIG_LSS_MASTER | \
+
151  CO_CONFIG_FLAG_CALLBACK_PRE_USED)
+
152 #endif
+
153 
+
154 #ifndef CO_CONFIG_GTW
+
155 #define CO_CONFIG_GTW (CO_CONFIG_GTW_ASCII | \
+
156  CO_CONFIG_GTW_ASCII_SDO | \
+
157  CO_CONFIG_GTW_ASCII_NMT | \
+
158  CO_CONFIG_GTW_ASCII_LSS | \
+
159  CO_CONFIG_GTW_ASCII_LOG | \
+
160  CO_CONFIG_GTW_ASCII_ERROR_DESC | \
+
161  CO_CONFIG_GTW_ASCII_PRINT_HELP | \
+
162  CO_CONFIG_GTW_ASCII_PRINT_LEDS)
+
163 #define CO_CONFIG_GTW_BLOCK_DL_LOOP 3
+
164 #define CO_CONFIG_GTWA_COMM_BUF_SIZE 2000
+
165 #define CO_CONFIG_GTWA_LOG_BUF_SIZE 10000
+
166 #endif
+
167 
+
168 #ifndef CO_CONFIG_CRC16
+
169 #define CO_CONFIG_CRC16 (CO_CONFIG_CRC16_ENABLE)
+
170 #endif
+
171 
+
172 #ifndef CO_CONFIG_FIFO
+
173 #define CO_CONFIG_FIFO (CO_CONFIG_FIFO_ENABLE | \
+
174  CO_CONFIG_FIFO_ALT_READ | \
+
175  CO_CONFIG_FIFO_CRC16_CCITT | \
+
176  CO_CONFIG_FIFO_ASCII_COMMANDS | \
+
177  CO_CONFIG_FIFO_ASCII_DATATYPES)
+
178 #endif
+
179 
+
180 #ifndef CO_CONFIG_TRACE
+
181 #define CO_CONFIG_TRACE (CO_CONFIG_TRACE_ENABLE)
+
182 #endif
+
183 
+
184 
+
185 /* Print debug info from some internal parts of the stack */
+
186 #if (CO_CONFIG_DEBUG) & CO_CONFIG_DEBUG_COMMON
+
187 #include <stdio.h>
+
188 #include <syslog.h>
+
189 #define CO_DEBUG_COMMON(msg) log_printf(LOG_DEBUG, DBG_CO_DEBUG, msg);
+
190 #endif
+
191 
+
192 
+
201 /* Macro for passing additional information about error, see CO_driver.h. */
+
202 #define CO_errinfo(CANmodule, err) CANmodule->errinfo = err
+
203 
+
224 #ifndef CO_DRIVER_MULTI_INTERFACE
+
225 #define CO_DRIVER_MULTI_INTERFACE 0
+
226 #endif
+
227 
+
245 #ifndef CO_DRIVER_ERROR_REPORTING
+
246 #define CO_DRIVER_ERROR_REPORTING 1
+
247 #endif
+
248 
+
249 /* skip this section for Doxygen, because it is documented in CO_driver.h */
+
250 #ifndef CO_DOXYGEN
+
251 
+
252 /* Basic definitions */
+
253 #ifdef __BYTE_ORDER
+
254 #if __BYTE_ORDER == __LITTLE_ENDIAN
+
255  #define CO_LITTLE_ENDIAN
+
256  #define CO_SWAP_16(x) x
+
257  #define CO_SWAP_32(x) x
+
258  #define CO_SWAP_64(x) x
+
259 #else
+
260  #define CO_BIG_ENDIAN
+
261  #include <byteswap.h>
+
262  #define CO_SWAP_16(x) bswap_16(x)
+
263  #define CO_SWAP_32(x) bswap_32(x)
+
264  #define CO_SWAP_64(x) bswap_64(x)
+
265 #endif
+
266 #endif
+
267 /* #define CO_USE_LEDS */
+
268 /* NULL is defined in stddef.h */
+
269 /* true and false are defined in stdbool.h */
+
270 /* int8_t to uint64_t are defined in stdint.h */
+
271 typedef unsigned char bool_t;
+
272 typedef float float32_t;
+
273 typedef double float64_t;
+
274 typedef char char_t;
+
275 typedef unsigned char oChar_t;
+
276 typedef unsigned char domain_t;
+
277 
+
278 
+
279 /* CAN receive message structure as aligned in socketCAN. */
+
280 typedef struct {
+
281  uint32_t ident;
+
282  uint8_t DLC;
+
283  uint8_t padding[3];
+
284  uint8_t data[8];
+
285 } CO_CANrxMsg_t;
+
286 
+
287 /* Access to received CAN message */
+
288 static inline uint16_t CO_CANrxMsg_readIdent(void *rxMsg) {
+
289  CO_CANrxMsg_t *rxMsgCasted = (CO_CANrxMsg_t *)rxMsg;
+
290  return (uint16_t) (rxMsgCasted->ident & CAN_SFF_MASK);
+
291 }
+
292 static inline uint8_t CO_CANrxMsg_readDLC(void *rxMsg) {
+
293  CO_CANrxMsg_t *rxMsgCasted = (CO_CANrxMsg_t *)rxMsg;
+
294  return (uint8_t) (rxMsgCasted->DLC);
+
295 }
+
296 static inline uint8_t *CO_CANrxMsg_readData(void *rxMsg) {
+
297  CO_CANrxMsg_t *rxMsgCasted = (CO_CANrxMsg_t *)rxMsg;
+
298  return (uint8_t *) (rxMsgCasted->data);
+
299 }
+
300 
+
301 
+
302 /* Received message object */
+
303 typedef struct {
+
304  uint32_t ident;
+
305  uint32_t mask;
+
306  void *object;
+
307  void (*CANrx_callback)(void *object, void *message);
+
308  int can_ifindex; /* CAN Interface index from last message */
+
309  struct timespec timestamp; /* time of reception of last message */
+
310 } CO_CANrx_t;
+
311 
+
312 /* Transmit message object as aligned in socketCAN. */
+
313 typedef struct {
+
314  uint32_t ident;
+
315  uint8_t DLC;
+
316  uint8_t padding[3]; /* ensure alignment */
+
317  uint8_t data[8];
+
318  volatile bool_t bufferFull; /* not used */
+
319  volatile bool_t syncFlag; /* info about transmit message */
+
320  int can_ifindex; /* CAN Interface index to use */
+
321 } CO_CANtx_t;
+
322 
+
323 
+
324 /* Max COB ID for standard frame format */
+
325 #define CO_CAN_MSG_SFF_MAX_COB_ID (1 << CAN_SFF_ID_BITS)
+
326 
+
327 /* CAN interface object (CANptr), passed to CO_CANinit() */
+
328 typedef struct {
+
329  int can_ifindex; /* CAN Interface index */
+
330  int epoll_fd; /* File descriptor for epoll, which waits for
+
331  CAN receive event */
+
332 } CO_CANptrSocketCan_t;
+
333 
+
334 /* socketCAN interface object */
+
335 typedef struct {
+
336  int can_ifindex; /* CAN Interface index */
+
337  char ifName[IFNAMSIZ]; /* CAN Interface name */
+
338  int fd; /* socketCAN file descriptor */
+
339 #if CO_DRIVER_ERROR_REPORTING > 0 || defined CO_DOXYGEN
+
340  CO_CANinterfaceErrorhandler_t errorhandler;
+
341 #endif
+
342 } CO_CANinterface_t;
+
343 
+
344 /* CAN module object */
+
345 typedef struct {
+
346  /* List of can interfaces. From CO_CANmodule_init() or one per
+
347  * CO_CANmodule_addInterface() call */
+
348  CO_CANinterface_t *CANinterfaces;
+
349  uint32_t CANinterfaceCount; /* interface count */
+
350  CO_CANrx_t *rxArray;
+
351  uint16_t rxSize;
+
352  struct can_filter *rxFilter;/* socketCAN filter list, one per rx buffer */
+
353  uint32_t rxDropCount; /* messages dropped on rx socket queue */
+
354  CO_CANtx_t *txArray;
+
355  uint16_t txSize;
+
356  uint16_t CANerrorStatus;
+
357  int32_t errinfo;
+
358  volatile bool_t CANnormal;
+
359  int epoll_fd; /* File descriptor for epoll, which waits for
+
360  CAN receive event */
+
361 #if CO_DRIVER_MULTI_INTERFACE > 0 || defined CO_DOXYGEN
+
362  /* Lookup tables Cob ID to rx/tx array index.
+
363  * Only feasible for SFF Messages. */
+
364  uint32_t rxIdentToIndex[CO_CAN_MSG_SFF_MAX_COB_ID];
+
365  uint32_t txIdentToIndex[CO_CAN_MSG_SFF_MAX_COB_ID];
+
366 #endif
+
367 } CO_CANmodule_t;
+
368 
+
369 #ifdef CO_SINGLE_THREAD
+
370 #define CO_LOCK_CAN_SEND()
+
371 #define CO_UNLOCK_CAN_SEND()
+
372 #define CO_LOCK_EMCY()
+
373 #define CO_UNLOCK_EMCY()
+
374 #define CO_LOCK_OD()
+
375 #define CO_UNLOCK_OD()
+
376 #define CO_MemoryBarrier()
+
377 #else
+
378 
+
379 /* (un)lock critical section in CO_CANsend() - unused */
+
380 #define CO_LOCK_CAN_SEND()
+
381 #define CO_UNLOCK_CAN_SEND()
+
382 
+
383 /* (un)lock critical section in CO_errorReport() or CO_errorReset() */
+
384 extern pthread_mutex_t CO_EMCY_mutex;
+
385 static inline int CO_LOCK_EMCY() {
+
386  return pthread_mutex_lock(&CO_EMCY_mutex);
+
387 }
+
388 static inline void CO_UNLOCK_EMCY() {
+
389  (void)pthread_mutex_unlock(&CO_EMCY_mutex);
+
390 }
+
391 
+
392 /* (un)lock critical section when accessing Object Dictionary */
+
393 extern pthread_mutex_t CO_OD_mutex;
+
394 static inline int CO_LOCK_OD() {
+
395  return pthread_mutex_lock(&CO_OD_mutex);
+
396 }
+
397 static inline void CO_UNLOCK_OD() {
+
398  (void)pthread_mutex_unlock(&CO_OD_mutex);
+
399 }
+
400 
+
401 /* Synchronization between CAN receive and message processing threads. */
+
402 #define CO_MemoryBarrier() {__sync_synchronize();}
+
403 #endif /* CO_SINGLE_THREAD */
+
404 
+
405 #define CO_FLAG_READ(rxNew) ((rxNew) != NULL)
+
406 #define CO_FLAG_SET(rxNew) {CO_MemoryBarrier(); rxNew = (void*)1L;}
+
407 #define CO_FLAG_CLEAR(rxNew) {CO_MemoryBarrier(); rxNew = NULL;}
+
408 
+
409 #endif /* #ifndef CO_DOXYGEN */
+
410 
+
411 
+
412 #if CO_DRIVER_MULTI_INTERFACE > 0 || defined CO_DOXYGEN
+
413 
+
423 CO_ReturnError_t CO_CANmodule_addInterface(CO_CANmodule_t *CANmodule,
+
424  int can_ifindex);
+
425 
+
441 bool_t CO_CANrxBuffer_getInterface(CO_CANmodule_t *CANmodule,
+
442  uint16_t ident,
+
443  const void **const CANptrRx,
+
444  struct timespec *timestamp);
+
445 
+
461 CO_ReturnError_t CO_CANtxBuffer_setInterface(CO_CANmodule_t *CANmodule,
+
462  uint16_t ident,
+
463  const void *CANptrTx);
+
464 #endif /* CO_DRIVER_MULTI_INTERFACE */
+
465 
+
466 
+
491 bool_t CO_CANrxFromEpoll(CO_CANmodule_t *CANmodule,
+
492  struct epoll_event *ev,
+
493  CO_CANrxMsg_t *buffer,
+
494  int32_t *msgIndex);
+
495 
+
498 #ifdef __cplusplus
+
499 }
+
500 #endif /* __cplusplus */
+
501 
+
502 #endif /* CO_DRIVER_TARGET_H */
+
+
+
uint32_t
unsigned long int uint32_t
UNSIGNED32 in CANopen (0007h), 32-bit unsigned integer.
Definition: CO_driver.h:155
+
CO_CANtxBuffer_setInterface
CO_ReturnError_t CO_CANtxBuffer_setInterface(CO_CANmodule_t *CANmodule, uint16_t ident, const void *CANptrTx)
Set which interface should be used for message buffer transmission.
+
CO_CANmodule_addInterface
CO_ReturnError_t CO_CANmodule_addInterface(CO_CANmodule_t *CANmodule, int can_ifindex)
Add socketCAN interface to can driver.
+
CO_error.h
CANopenNode Linux socketCAN Error handling.
+
char_t
char char_t
VISIBLE_STRING in CANopen (0009h), string of signed 8-bit values.
Definition: CO_driver.h:163
+
CO_UNLOCK_EMCY
#define CO_UNLOCK_EMCY()
Unlock critical section in CO_errorReport() or CO_errorReset()
Definition: CO_driver.h:394
+
uint16_t
unsigned int uint16_t
UNSIGNED16 in CANopen (0006h), 16-bit unsigned integer.
Definition: CO_driver.h:153
+
CO_LOCK_EMCY
#define CO_LOCK_EMCY()
Lock critical section in CO_errorReport() or CO_errorReset()
Definition: CO_driver.h:392
+
CO_ReturnError_t
CO_ReturnError_t
Return values of some CANopen functions.
Definition: CO_driver.h:488
+
CO_CANrxMsg_readDLC
static uint8_t CO_CANrxMsg_readDLC(void *rxMsg)
CANrx_callback() can read Data Length Code from received CAN message.
Definition: CO_driver.h:230
+
bool_t
unsigned char bool_t
Boolean data type for general use.
Definition: CO_driver.h:141
+
CANrx_callback
void CANrx_callback(void *object, void *rxMsg)
CAN receive callback function which pre-processes received CAN message.
+
CO_CANrxMsg_readData
static uint8_t * CO_CANrxMsg_readData(void *rxMsg)
CANrx_callback() can read pointer to data from received CAN message.
Definition: CO_driver.h:242
+
int32_t
signed long int int32_t
INTEGER32 in CANopen (0004h), 32-bit signed integer.
Definition: CO_driver.h:147
+
CO_LOCK_OD
#define CO_LOCK_OD()
Lock critical section when accessing Object Dictionary.
Definition: CO_driver.h:396
+
CO_CANrxMsg_readIdent
static uint16_t CO_CANrxMsg_readIdent(void *rxMsg)
CANrx_callback() can read CAN identifier from received CAN message.
Definition: CO_driver.h:218
+
CO_CANrxFromEpoll
bool_t CO_CANrxFromEpoll(CO_CANmodule_t *CANmodule, struct epoll_event *ev, CO_CANrxMsg_t *buffer, int32_t *msgIndex)
Receives CAN messages from matching epoll event.
Definition: CO_driver.c:842
+
float64_t
double float64_t
REAL64 in CANopen (0011h), double precision floating point value, 64-bit.
Definition: CO_driver.h:161
+
float32_t
float float32_t
REAL32 in CANopen (0008h), single precision floating point value, 32-bit.
Definition: CO_driver.h:159
+
oChar_t
unsigned char oChar_t
OCTET_STRING in CANopen (000Ah), string of unsigned 8-bit values.
Definition: CO_driver.h:165
+
CO_CANrxBuffer_getInterface
bool_t CO_CANrxBuffer_getInterface(CO_CANmodule_t *CANmodule, uint16_t ident, const void **const CANptrRx, struct timespec *timestamp)
Check on which interface the last message for one message buffer was received.
+
CO_UNLOCK_OD
#define CO_UNLOCK_OD()
Unock critical section when accessing Object Dictionary.
Definition: CO_driver.h:398
+
CO_CANrx_t
Configuration object for CAN received message for specific CANopenNode Object.
Definition: CO_driver.h:257
+
CO_CANmodule_t
Complete CAN module object.
Definition: CO_driver.h:319
+
domain_t
unsigned char domain_t
DOMAIN in CANopen (000Fh), used to transfer a large block of data.
Definition: CO_driver.h:167
+
CO_CANtx_t
Configuration object for CAN transmit message for specific CANopenNode Object.
Definition: CO_driver.h:299
+
CO_CANinterfaceErrorhandler_t
socketCAN interface error handling
Definition: CO_error.h:113
+
uint8_t
unsigned char uint8_t
UNSIGNED8 in CANopen (0005h), 8-bit unsigned integer.
Definition: CO_driver.h:151
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/CO__epoll__interface_8h.html b/Devices/Libraries/Systems/CANopenSocket/docs/CO__epoll__interface_8h.html new file mode 100755 index 0000000..9c507dc --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/CO__epoll__interface_8h.html @@ -0,0 +1,186 @@ + + + + + + + +CANopenNode: socketCAN/CO_epoll_interface.h File Reference + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Data Structures | +Enumerations | +Functions
+
+
CO_epoll_interface.h File Reference
socketCAN » Epoll interface
+
+
+ +

Helper functions for Linux epoll interface to CANopenNode. +More...

+
#include "CANopen.h"
+#include <sys/epoll.h>
+#include <sys/eventfd.h>
+#include <sys/timerfd.h>
+
+

Go to the source code of this file.

+ + + + + + + + +

+Data Structures

struct  CO_epoll_t
 Object for epoll, timer and event API. More...
 
struct  CO_epoll_gtw_t
 Object for gateway. More...
 
+ + + + +

+Enumerations

enum  CO_commandInterface_t
 Command interface type for gateway-ascii.
 
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

+Functions

CO_ReturnError_t CO_epoll_create (CO_epoll_t *ep, uint32_t timerInterval_us)
 Create Linux epoll, timerfd and eventfd. More...
 
void CO_epoll_close (CO_epoll_t *ep)
 Close epoll, timerfd and eventfd. More...
 
void CO_epoll_wait (CO_epoll_t *ep)
 Wait for an epoll event. More...
 
void CO_epoll_processLast (CO_epoll_t *ep)
 Closing function for an epoll event. More...
 
void CO_epoll_initCANopenMain (CO_epoll_t *ep, CO_t *co)
 Initialization of functions in CANopen reset-communication section. More...
 
void CO_epoll_processMain (CO_epoll_t *ep, CO_t *co, bool_t enableGateway, CO_NMT_reset_cmd_t *reset)
 Process CANopen mainline functions. More...
 
void CO_epoll_processRT (CO_epoll_t *ep, CO_t *co, bool_t realtime)
 Process CAN receive and realtime functions. More...
 
CO_ReturnError_t CO_epoll_createGtw (CO_epoll_gtw_t *epGtw, int epoll_fd, int32_t commandInterface, uint32_t socketTimeout_ms, char *localSocketPath)
 Create socket for gateway-ascii command interface and add it to epoll. More...
 
void CO_epoll_closeGtw (CO_epoll_gtw_t *epGtw)
 Close gateway-ascii sockets. More...
 
void CO_epoll_initCANopenGtw (CO_epoll_gtw_t *epGtw, CO_t *co)
 Initialization of gateway functions in CANopen reset-communication section. More...
 
void CO_epoll_processGtw (CO_epoll_gtw_t *epGtw, CO_t *co, CO_epoll_t *ep)
 Process CANopen gateway functions. More...
 
+

Detailed Description

+

Helper functions for Linux epoll interface to CANopenNode.

+
Author
Janez Paternoster
+
+Martin Wagner
+
Copyright
2004 - 2020 Janez Paternoster
+
+2018 - 2020 Neuberger Gebaeudeautomation GmbH
+

This file is part of CANopenNode, an opensource CANopen Stack. Project home page is https://github.com/CANopenNode/CANopenNode. For more information on CANopen see http://www.can-cia.org/.

+

Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. You may obtain a copy of the License at 

http://www.apache.org/licenses/LICENSE-2.0
+

Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License.

+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/CO__epoll__interface_8h.js b/Devices/Libraries/Systems/CANopenSocket/docs/CO__epoll__interface_8h.js new file mode 100755 index 0000000..09767ba --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/CO__epoll__interface_8h.js @@ -0,0 +1,15 @@ +var CO__epoll__interface_8h = +[ + [ "CO_commandInterface_t", "group__CO__epoll__interface.html#gad5a4218d5775fd7cda81a8015e1ee6f0", null ], + [ "CO_epoll_create", "group__CO__epoll__interface.html#ga9bb687bf26f711ce0573581984b79443", null ], + [ "CO_epoll_close", "group__CO__epoll__interface.html#ga72c3ebdede1207e55e0915128f5a2c6a", null ], + [ "CO_epoll_wait", "group__CO__epoll__interface.html#ga5cfd1df62494cf9d9e85dbb9fe981a57", null ], + [ "CO_epoll_processLast", "group__CO__epoll__interface.html#ga2aef637d4f2f818a7d95a7bfac251132", null ], + [ "CO_epoll_initCANopenMain", "group__CO__epoll__interface.html#ga3668c8b600022ffecdd3c2fa643b5e7d", null ], + [ "CO_epoll_processMain", "group__CO__epoll__interface.html#gad79d73fbac0e816f81f75507bd164515", null ], + [ "CO_epoll_processRT", "group__CO__epoll__interface.html#ga6ed67073bc96e575bec6fceb627b1245", null ], + [ "CO_epoll_createGtw", "group__CO__epoll__interface.html#ga7165df8b37ca1ed59476773fa075470c", null ], + [ "CO_epoll_closeGtw", "group__CO__epoll__interface.html#gab2f3f7bb7d885799c462e95a45563b69", null ], + [ "CO_epoll_initCANopenGtw", "group__CO__epoll__interface.html#ga07eaf2c954bb09b6420acee62ff207c3", null ], + [ "CO_epoll_processGtw", "group__CO__epoll__interface.html#ga97aa0bc09684b0ed78b708198e663407", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/CO__epoll__interface_8h_source.html b/Devices/Libraries/Systems/CANopenSocket/docs/CO__epoll__interface_8h_source.html new file mode 100755 index 0000000..f124ae7 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/CO__epoll__interface_8h_source.html @@ -0,0 +1,239 @@ + + + + + + + +CANopenNode: socketCAN/CO_epoll_interface.h Source File + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+
CO_epoll_interface.h
+
+
+Go to the documentation of this file.
1 
+
29 #ifndef CO_EPOLL_INTERFACE_H
+
30 #define CO_EPOLL_INTERFACE_H
+
31 
+
32 #include "CANopen.h"
+
33 
+
34 #include <sys/epoll.h>
+
35 #include <sys/eventfd.h>
+
36 #include <sys/timerfd.h>
+
37 
+
38 #ifdef __cplusplus
+
39 extern "C" {
+
40 #endif
+
41 
+
89 typedef struct {
+
91  int epoll_fd;
+
93  int event_fd;
+
95  int timer_fd;
+
97  uint32_t timerInterval_us;
+
100  uint32_t timeDifference_us;
+
103  uint32_t timerNext_us;
+
105  bool_t timerEvent;
+
107  uint64_t previousTime_us;
+
109  struct itimerspec tm;
+
111  struct epoll_event ev;
+
113  bool_t epoll_new;
+
114 } CO_epoll_t;
+
115 
+
130 CO_ReturnError_t CO_epoll_create(CO_epoll_t *ep, uint32_t timerInterval_us);
+
131 
+
137 void CO_epoll_close(CO_epoll_t *ep);
+
138 
+
148 void CO_epoll_wait(CO_epoll_t *ep);
+
149 
+
161 void CO_epoll_processLast(CO_epoll_t *ep);
+
162 
+
171 void CO_epoll_initCANopenMain(CO_epoll_t *ep, CO_t *co);
+
172 
+
185 void CO_epoll_processMain(CO_epoll_t *ep,
+
186  CO_t *co,
+
187  bool_t enableGateway,
+
188  CO_NMT_reset_cmd_t *reset);
+
189 
+
190 
+
211 void CO_epoll_processRT(CO_epoll_t *ep,
+
212  CO_t *co,
+
213  bool_t realtime);
+
214 
+
215 
+
216 #if ((CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII) || defined CO_DOXYGEN
+
217 
+
220 typedef enum {
+
221  CO_COMMAND_IF_DISABLED = -100,
+
222  CO_COMMAND_IF_STDIO = -2,
+
223  CO_COMMAND_IF_LOCAL_SOCKET = -1,
+
224  CO_COMMAND_IF_TCP_SOCKET_MIN = 0,
+
225  CO_COMMAND_IF_TCP_SOCKET_MAX = 0xFFFF
+
226 } CO_commandInterface_t;
+
227 
+
231 typedef struct {
+
233  int epoll_fd;
+
236  int32_t commandInterface;
+
238  uint32_t socketTimeout_us;
+
240  uint32_t socketTimeoutTmr_us;
+
242  char *localSocketPath;
+
244  int gtwa_fdSocket;
+
246  int gtwa_fd;
+
248  bool_t freshCommand;
+
249 } CO_epoll_gtw_t;
+
250 
+
266 CO_ReturnError_t CO_epoll_createGtw(CO_epoll_gtw_t *epGtw,
+
267  int epoll_fd,
+
268  int32_t commandInterface,
+
269  uint32_t socketTimeout_ms,
+
270  char *localSocketPath);
+
271 
+
277 void CO_epoll_closeGtw(CO_epoll_gtw_t *epGtw);
+
278 
+
285 void CO_epoll_initCANopenGtw(CO_epoll_gtw_t *epGtw, CO_t *co);
+
286 
+
298 void CO_epoll_processGtw(CO_epoll_gtw_t *epGtw,
+
299  CO_t *co,
+
300  CO_epoll_t *ep);
+
301 #endif /* (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII */
+
302 
+
305 #ifdef __cplusplus
+
306 }
+
307 #endif /*__cplusplus*/
+
308 
+
309 #endif /* CO_EPOLL_INTERFACE_H */
+
+
+
uint32_t
unsigned long int uint32_t
UNSIGNED32 in CANopen (0007h), 32-bit unsigned integer.
Definition: CO_driver.h:155
+
CO_epoll_gtw_t::epoll_fd
int epoll_fd
Epoll file descriptor, from CO_epoll_createGtw()
Definition: CO_epoll_interface.h:233
+
CO_epoll_processRT
void CO_epoll_processRT(CO_epoll_t *ep, CO_t *co, bool_t realtime)
Process CAN receive and realtime functions.
Definition: CO_epoll_interface.c:304
+
CO_commandInterface_t
CO_commandInterface_t
Command interface type for gateway-ascii.
Definition: CO_epoll_interface.h:220
+
CO_epoll_gtw_t
Object for gateway.
Definition: CO_epoll_interface.h:231
+
CO_ReturnError_t
CO_ReturnError_t
Return values of some CANopen functions.
Definition: CO_driver.h:488
+
CO_epoll_gtw_t::localSocketPath
char * localSocketPath
Path in case of local socket.
Definition: CO_epoll_interface.h:242
+
CO_epoll_gtw_t::gtwa_fd
int gtwa_fd
Gateway io stream file descriptor.
Definition: CO_epoll_interface.h:246
+
bool_t
unsigned char bool_t
Boolean data type for general use.
Definition: CO_driver.h:141
+
CO_epoll_processGtw
void CO_epoll_processGtw(CO_epoll_gtw_t *epGtw, CO_t *co, CO_epoll_t *ep)
Process CANopen gateway functions.
+
int32_t
signed long int int32_t
INTEGER32 in CANopen (0004h), 32-bit signed integer.
Definition: CO_driver.h:147
+
CO_NMT_reset_cmd_t
CO_NMT_reset_cmd_t
Return code from CO_NMT_process() that tells application code what to reset.
Definition: CO_NMT_Heartbeat.h:112
+
uint64_t
unsigned long long int uint64_t
UNSIGNED64 in CANopen (001Bh), 64-bit unsigned integer.
Definition: CO_driver.h:157
+
CO_epoll_wait
void CO_epoll_wait(CO_epoll_t *ep)
Wait for an epoll event.
Definition: CO_epoll_interface.c:138
+
CO_epoll_processMain
void CO_epoll_processMain(CO_epoll_t *ep, CO_t *co, bool_t enableGateway, CO_NMT_reset_cmd_t *reset)
Process CANopen mainline functions.
Definition: CO_epoll_interface.c:286
+
CO_epoll_t::timer_fd
int timer_fd
Interval timer file descriptor.
Definition: CO_epoll_interface.h:95
+
CO_epoll_gtw_t::commandInterface
int32_t commandInterface
Command interface type or tcp port number, see CO_commandInterface_t.
Definition: CO_epoll_interface.h:236
+
CO_epoll_initCANopenMain
void CO_epoll_initCANopenMain(CO_epoll_t *ep, CO_t *co)
Initialization of functions in CANopen reset-communication section.
Definition: CO_epoll_interface.c:232
+
CO_epoll_t::timerNext_us
uint32_t timerNext_us
Timer value in microseconds, which can be changed by application and can shorten time of next CO_epol...
Definition: CO_epoll_interface.h:103
+
CO_epoll_gtw_t::socketTimeoutTmr_us
uint32_t socketTimeoutTmr_us
Socket timeout timer in microseconds.
Definition: CO_epoll_interface.h:240
+
CO_epoll_t::timerEvent
bool_t timerEvent
True,if timer event is inside CO_epoll_wait()
Definition: CO_epoll_interface.h:105
+
CO_epoll_t
Object for epoll, timer and event API.
Definition: CO_epoll_interface.h:89
+
CO_epoll_t::epoll_new
bool_t epoll_new
true, if new epoll event is necessary to process
Definition: CO_epoll_interface.h:113
+
CO_epoll_create
CO_ReturnError_t CO_epoll_create(CO_epoll_t *ep, uint32_t timerInterval_us)
Create Linux epoll, timerfd and eventfd.
Definition: CO_epoll_interface.c:63
+
CANopen.h
Main CANopenNode file.
+
CO_epoll_t::timeDifference_us
uint32_t timeDifference_us
Time difference since last CO_epoll_wait() execution in microseconds.
Definition: CO_epoll_interface.h:100
+
CO_epoll_gtw_t::socketTimeout_us
uint32_t socketTimeout_us
Socket timeout in microseconds.
Definition: CO_epoll_interface.h:238
+
CO_epoll_close
void CO_epoll_close(CO_epoll_t *ep)
Close epoll, timerfd and eventfd.
Definition: CO_epoll_interface.c:122
+
CO_epoll_processLast
void CO_epoll_processLast(CO_epoll_t *ep)
Closing function for an epoll event.
Definition: CO_epoll_interface.c:187
+
CO_epoll_initCANopenGtw
void CO_epoll_initCANopenGtw(CO_epoll_gtw_t *epGtw, CO_t *co)
Initialization of gateway functions in CANopen reset-communication section.
+
CO_epoll_closeGtw
void CO_epoll_closeGtw(CO_epoll_gtw_t *epGtw)
Close gateway-ascii sockets.
+
CO_epoll_t::event_fd
int event_fd
Notification event file descriptor.
Definition: CO_epoll_interface.h:93
+
CO_t
CANopen object - collection of all CANopenNode objects.
Definition: CANopen.h:301
+
CO_epoll_t::epoll_fd
int epoll_fd
Epoll file descriptor.
Definition: CO_epoll_interface.h:91
+
CO_epoll_t::timerInterval_us
uint32_t timerInterval_us
Interval of the timer in microseconds, from CO_epoll_create()
Definition: CO_epoll_interface.h:97
+
CO_epoll_createGtw
CO_ReturnError_t CO_epoll_createGtw(CO_epoll_gtw_t *epGtw, int epoll_fd, int32_t commandInterface, uint32_t socketTimeout_ms, char *localSocketPath)
Create socket for gateway-ascii command interface and add it to epoll.
+
CO_epoll_t::previousTime_us
uint64_t previousTime_us
time value from the last process call in microseconds
Definition: CO_epoll_interface.h:107
+
CO_epoll_gtw_t::freshCommand
bool_t freshCommand
Indication of fresh command.
Definition: CO_epoll_interface.h:248
+
CO_epoll_gtw_t::gtwa_fdSocket
int gtwa_fdSocket
Gateway socket file descriptor.
Definition: CO_epoll_interface.h:244
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/CO__error_8h.html b/Devices/Libraries/Systems/CANopenSocket/docs/CO__error_8h.html new file mode 100755 index 0000000..45e7c55 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/CO__error_8h.html @@ -0,0 +1,180 @@ + + + + + + + +CANopenNode: socketCAN/CO_error.h File Reference + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Data Structures | +Macros | +Enumerations | +Functions
+
+
CO_error.h File Reference
socketCAN » CAN errors & Log
+
+
+ +

CANopenNode Linux socketCAN Error handling. +More...

+
#include <stdint.h>
+#include <errno.h>
+#include <string.h>
+#include <linux/can.h>
+#include <net/if.h>
+#include "CO_error_msgs.h"
+
+

Go to the source code of this file.

+ + + + + +

+Data Structures

struct  CO_CANinterfaceErrorhandler_t
 socketCAN interface error handling More...
 
+ + + + + + + +

+Macros

+#define CO_CANerror_NOACK_MAX   16
 This is how many NO-ACKs need to be received in a row to assume that no other nodes are connected to a bus and therefore are assuming listen-only.
 
#define CO_CANerror_LISTEN_ONLY   10
 This is how long we are going to block transmission if listen-only mode is active. More...
 
+ + + + +

+Enumerations

enum  CO_CANinterfaceState_t { CO_INTERFACE_ACTIVE, +CO_INTERFACE_LISTEN_ONLY, +CO_INTERFACE_BUS_OFF + }
 driver interface state More...
 
+ + + + + + + + + + + + + + + + + + + +

+Functions

void log_printf (int priority, const char *format,...)
 Message logging function. More...
 
void CO_CANerror_init (CO_CANinterfaceErrorhandler_t *CANerrorhandler, int fd, const char *ifname)
 Initialize CAN error handler. More...
 
void CO_CANerror_disable (CO_CANinterfaceErrorhandler_t *CANerrorhandler)
 Reset CAN error handler. More...
 
void CO_CANerror_rxMsg (CO_CANinterfaceErrorhandler_t *CANerrorhandler)
 Message received event. More...
 
CO_CANinterfaceState_t CO_CANerror_txMsg (CO_CANinterfaceErrorhandler_t *CANerrorhandler)
 Check if interface is ready for message transmission. More...
 
CO_CANinterfaceState_t CO_CANerror_rxMsgError (CO_CANinterfaceErrorhandler_t *CANerrorhandler, const struct can_frame *msg)
 Error message received event. More...
 
+

Detailed Description

+

CANopenNode Linux socketCAN Error handling.

+
Author
Martin Wagner
+
Copyright
2018 - 2020 Neuberger Gebaeudeautomation GmbH
+

This file is part of CANopenNode, an opensource CANopen Stack. Project home page is https://github.com/CANopenNode/CANopenNode. For more information on CANopen see http://www.can-cia.org/.

+

Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. You may obtain a copy of the License at 

http://www.apache.org/licenses/LICENSE-2.0
+

Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License.

+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/CO__error_8h.js b/Devices/Libraries/Systems/CANopenSocket/docs/CO__error_8h.js new file mode 100755 index 0000000..e0597fb --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/CO__error_8h.js @@ -0,0 +1,16 @@ +var CO__error_8h = +[ + [ "CO_CANerror_NOACK_MAX", "group__CO__socketCAN__ERROR.html#ga85f1fa06be5e9a8337e1ec86546ea72d", null ], + [ "CO_CANerror_LISTEN_ONLY", "group__CO__socketCAN__ERROR.html#ga02f9c19a953d2bd9005033c35c2cb6de", null ], + [ "CO_CANinterfaceState_t", "group__CO__socketCAN__ERROR.html#ga7bf6a56766c008531d32b4218a5a9061", [ + [ "CO_INTERFACE_ACTIVE", "group__CO__socketCAN__ERROR.html#gga7bf6a56766c008531d32b4218a5a9061ad22fcb069e808548dee28e4aae580c1a", null ], + [ "CO_INTERFACE_LISTEN_ONLY", "group__CO__socketCAN__ERROR.html#gga7bf6a56766c008531d32b4218a5a9061a9b74584a321fe8a89cf74e087b094581", null ], + [ "CO_INTERFACE_BUS_OFF", "group__CO__socketCAN__ERROR.html#gga7bf6a56766c008531d32b4218a5a9061a8e51897ec66a9b37865659bbc348e212", null ] + ] ], + [ "log_printf", "group__CO__socketCAN__ERROR.html#gac9aeec86e89e5525b4e13e3b1e21866d", null ], + [ "CO_CANerror_init", "group__CO__socketCAN__ERROR.html#ga30c3cb98d37aedf45d49643064fee4cd", null ], + [ "CO_CANerror_disable", "group__CO__socketCAN__ERROR.html#ga6a74902a35f8a260cdc3956b83c17c77", null ], + [ "CO_CANerror_rxMsg", "group__CO__socketCAN__ERROR.html#ga5d5ee1aac31cf0334108cd147b9c9038", null ], + [ "CO_CANerror_txMsg", "group__CO__socketCAN__ERROR.html#gac40e1c681a9721c91ed75c49afda913c", null ], + [ "CO_CANerror_rxMsgError", "group__CO__socketCAN__ERROR.html#ga975e329593af25c4467f3ddde5cf5f5c", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/CO__error_8h_source.html b/Devices/Libraries/Systems/CANopenSocket/docs/CO__error_8h_source.html new file mode 100755 index 0000000..cb650d6 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/CO__error_8h_source.html @@ -0,0 +1,201 @@ + + + + + + + +CANopenNode: socketCAN/CO_error.h Source File + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+
CO_error.h
+
+
+Go to the documentation of this file.
1 
+
28 #ifndef CO_ERROR_H
+
29 #define CO_ERROR_H
+
30 
+
31 #include <stdint.h>
+
32 #include <errno.h>
+
33 #include <string.h>
+
34 #include <linux/can.h>
+
35 #include <net/if.h>
+
36 
+
37 #if __has_include("CO_error_custom.h")
+
38  #include "CO_error_custom.h"
+
39 #else
+
40  #include "CO_error_msgs.h"
+
41 #endif
+
42 
+
43 #ifdef __cplusplus
+
44 extern "C" {
+
45 #endif
+
46 
+
75 void log_printf(int priority, const char *format, ...);
+
76 
+
77 
+
86 typedef enum {
+
87  CO_INTERFACE_ACTIVE,
+
88  CO_INTERFACE_LISTEN_ONLY,
+
89  CO_INTERFACE_BUS_OFF
+
90 } CO_CANinterfaceState_t;
+
91 
+
92 
+
98 #define CO_CANerror_NOACK_MAX 16
+
99 
+
100 
+
107 #define CO_CANerror_LISTEN_ONLY 10
+
108 
+
109 
+
113 typedef struct {
+
114  int fd;
+
115  char ifName[IFNAMSIZ];
+
116  uint32_t noackCounter;
+
117  volatile unsigned char listenOnly;
+
118  struct timespec timestamp;
+
119  uint16_t CANerrorStatus;
+
120 } CO_CANinterfaceErrorhandler_t;
+
121 
+
122 
+
132 void CO_CANerror_init(
+
133  CO_CANinterfaceErrorhandler_t *CANerrorhandler,
+
134  int fd,
+
135  const char *ifname);
+
136 
+
137 
+
143 void CO_CANerror_disable(
+
144  CO_CANinterfaceErrorhandler_t *CANerrorhandler);
+
145 
+
146 
+
155 void CO_CANerror_rxMsg(
+
156  CO_CANinterfaceErrorhandler_t *CANerrorhandler);
+
157 
+
158 
+
167 CO_CANinterfaceState_t CO_CANerror_txMsg(
+
168  CO_CANinterfaceErrorhandler_t *CANerrorhandler);
+
169 
+
170 
+
180 CO_CANinterfaceState_t CO_CANerror_rxMsgError(
+
181  CO_CANinterfaceErrorhandler_t *CANerrorhandler,
+
182  const struct can_frame *msg);
+
183 
+
184 
+
187 #ifdef __cplusplus
+
188 }
+
189 #endif /*__cplusplus*/
+
190 
+
191 #endif /* CO_ERROR_H */
+
+
+
uint32_t
unsigned long int uint32_t
UNSIGNED32 in CANopen (0007h), 32-bit unsigned integer.
Definition: CO_driver.h:155
+
log_printf
void log_printf(int priority, const char *format,...)
Message logging function.
Definition: CO_main_basic.c:137
+
CO_CANinterfaceErrorhandler_t::CANerrorStatus
uint16_t CANerrorStatus
CAN error status bitfield, see CO_CAN_ERR_status_t.
Definition: CO_error.h:119
+
uint16_t
unsigned int uint16_t
UNSIGNED16 in CANopen (0006h), 16-bit unsigned integer.
Definition: CO_driver.h:153
+
CO_CANerror_disable
void CO_CANerror_disable(CO_CANinterfaceErrorhandler_t *CANerrorhandler)
Reset CAN error handler.
Definition: CO_error.c:224
+
CO_INTERFACE_LISTEN_ONLY
@ CO_INTERFACE_LISTEN_ONLY
CAN error passive/active, but currently no other device on bus.
Definition: CO_error.h:88
+
CO_INTERFACE_BUS_OFF
@ CO_INTERFACE_BUS_OFF
CAN bus off.
Definition: CO_error.h:89
+
CO_CANinterfaceState_t
CO_CANinterfaceState_t
driver interface state
Definition: CO_error.h:86
+
CO_INTERFACE_ACTIVE
@ CO_INTERFACE_ACTIVE
CAN error passive/active.
Definition: CO_error.h:87
+
CO_CANerror_rxMsgError
CO_CANinterfaceState_t CO_CANerror_rxMsgError(CO_CANinterfaceErrorhandler_t *CANerrorhandler, const struct can_frame *msg)
Error message received event.
Definition: CO_error.c:277
+
CO_CANerror_rxMsg
void CO_CANerror_rxMsg(CO_CANinterfaceErrorhandler_t *CANerrorhandler)
Message received event.
Definition: CO_error.c:237
+
CO_CANinterfaceErrorhandler_t::noackCounter
uint32_t noackCounter
counts no ACK on CAN transmission
Definition: CO_error.h:116
+
CO_CANinterfaceErrorhandler_t::listenOnly
volatile unsigned char listenOnly
set to listen only mode
Definition: CO_error.h:117
+
CO_CANerror_txMsg
CO_CANinterfaceState_t CO_CANerror_txMsg(CO_CANinterfaceErrorhandler_t *CANerrorhandler)
Check if interface is ready for message transmission.
Definition: CO_error.c:253
+
CO_CANinterfaceErrorhandler_t::fd
int fd
interface FD
Definition: CO_error.h:114
+
CO_CANerror_init
void CO_CANerror_init(CO_CANinterfaceErrorhandler_t *CANerrorhandler, int fd, const char *ifname)
Initialize CAN error handler.
Definition: CO_error.c:204
+
CO_CANinterfaceErrorhandler_t
socketCAN interface error handling
Definition: CO_error.h:113
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/CO__error__msgs_8h_source.html b/Devices/Libraries/Systems/CANopenSocket/docs/CO__error__msgs_8h_source.html new file mode 100755 index 0000000..491c83f --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/CO__error__msgs_8h_source.html @@ -0,0 +1,208 @@ + + + + + + + +CANopenNode: socketCAN/CO_error_msgs.h Source File + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+
CO_error_msgs.h
+
+
+
1 /*
+
2  * Definitions for CANopenNode Linux socketCAN Error handling.
+
3  *
+
4  * @file CO_Error_msgs.h
+
5  * @author Martin Wagner
+
6  * @copyright 2020 Neuberger Gebaeudeautomation GmbH
+
7  *
+
8  *
+
9  * This file is part of CANopenNode, an opensource CANopen Stack.
+
10  * Project home page is <https://github.com/CANopenNode/CANopenNode>.
+
11  * For more information on CANopen see <http://www.can-cia.org/>.
+
12  *
+
13  * Licensed under the Apache License, Version 2.0 (the "License");
+
14  * you may not use this file except in compliance with the License.
+
15  * You may obtain a copy of the License at
+
16  *
+
17  * http://www.apache.org/licenses/LICENSE-2.0
+
18  *
+
19  * Unless required by applicable law or agreed to in writing, software
+
20  * distributed under the License is distributed on an "AS IS" BASIS,
+
21  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+
22  * See the License for the specific language governing permissions and
+
23  * limitations under the License.
+
24  */
+
25 
+
26 
+
27 #ifndef CO_ERROR_MSGS_H
+
28 #define CO_ERROR_MSGS_H
+
29 
+
30 #ifdef __cplusplus
+
31 extern "C" {
+
32 #endif
+
33 
+
34 
+
35 /*
+
36  * Message definitions for Linux CANopen socket driver (notice and errors)
+
37  */
+
38 #define CAN_NOT_FOUND "(%s) CAN Interface \"%s\" not found", __func__
+
39 #define CAN_INIT_FAILED "(%s) CAN Interface \"%s\" Init failed", __func__
+
40 #define CAN_BINDING_FAILED "(%s) Binding CAN Interface \"%s\" failed", __func__
+
41 #define CAN_ERROR_FILTER_FAILED "(%s) Setting CAN Interface \"%s\" error filter failed", __func__
+
42 #define CAN_FILTER_FAILED "(%s) Setting CAN Interface \"%s\" message filter failed", __func__
+
43 #define CAN_NAMETOINDEX "CAN Interface \"%s\" -> Index %d"
+
44 #define CAN_SOCKET_BUF_SIZE "CAN Interface \"%s\" Buffer set to %d messages (%d Bytes)"
+
45 #define CAN_RX_SOCKET_QUEUE_OVERFLOW "CAN Interface \"%s\" has lost %d messages"
+
46 #define CAN_BUSOFF "CAN Interface \"%s\" changed to \"Bus Off\". Switching to Listen Only mode..."
+
47 #define CAN_NOACK "CAN Interface \"%s\" no \"ACK\" received. Switching to Listen Only mode..."
+
48 #define CAN_RX_PASSIVE "CAN Interface \"%s\" changed state to \"Rx Passive\""
+
49 #define CAN_TX_PASSIVE "CAN Interface \"%s\" changed state to \"Tx Passive\""
+
50 #define CAN_TX_LEVEL_ACTIVE "CAN Interface \"%s\" changed state to \"Active\""
+
51 #define CAN_RX_BUF_OVERFLOW "CAN Interface \"%s\" Rx buffer overflow. Message dropped"
+
52 #define CAN_TX_BUF_OVERFLOW "CAN Interface \"%s\" Tx buffer overflow. Message dropped"
+
53 #define CAN_RX_LEVEL_WARNING "CAN Interface \"%s\" reached Rx Warning Level"
+
54 #define CAN_TX_LEVEL_WARNING "CAN Interface \"%s\" reached Tx Warning Level"
+
55 
+
56 
+
57 /*
+
58  * Message definitions for debugging
+
59  */
+
60 #define DBG_GENERAL "(%s) Error: %s%d", __func__
+
61 #define DBG_ERRNO "(%s) OS error \"%s\" in %s", __func__, strerror(errno)
+
62 #define DBG_CO_DEBUG "(%s) CO_DEBUG: %s", __func__
+
63 #define DBG_CAN_TX_FAILED "(%s) Transmitting CAN msg OID 0x%08x failed(%s)", __func__
+
64 #define DBG_CAN_RX_PARAM_FAILED "(%s) Setting CAN rx buffer failed (%s)", __func__
+
65 #define DBG_CAN_RX_FAILED "(%s) Receiving CAN msg failed (%s)", __func__
+
66 #define DBG_CAN_ERROR_GENERAL "(%s) Socket error msg ID: 0x%08x, Data[0..7]: 0x%02x, 0x%02x, 0x%02x, 0x%02x, 0x%02x, 0x%02x, 0x%02x, 0x%02x (%s)", __func__
+
67 #define DBG_CAN_RX_EPOLL "(%s) CAN Epoll error (0x%02x - %s)", __func__
+
68 #define DBG_CAN_SET_LISTEN_ONLY "(%s) %s Set Listen Only", __func__
+
69 #define DBG_CAN_CLR_LISTEN_ONLY "(%s) %s Leave Listen Only", __func__
+
70 
+
71 /* mainline */
+
72 #define DBG_EMERGENCY_RX "CANopen Emergency message from node 0x%02X: errorCode=0x%04X, errorRegister=0x%02X, errorBit=0x%02X, infoCode=0x%08X"
+
73 #define DBG_NMT_CHANGE "CANopen NMT state changed to: \"%s\" (%d)"
+
74 #define DBG_HB_CONS_NMT_CHANGE "CANopen Remote node ID = 0x%02X (index = %d): NMT state changed to: \"%s\" (%d)"
+
75 #define DBG_ARGUMENT_UNKNOWN "(%s) Unknown %s argument: \"%s\"", __func__
+
76 #define DBG_NOT_TCP_PORT "(%s) -c argument \"%s\" is not a valid tcp port", __func__
+
77 #define DBG_WRONG_NODE_ID "(%s) Wrong node ID \"%d\"", __func__
+
78 #define DBG_WRONG_PRIORITY "(%s) Wrong RT priority \"%d\"", __func__
+
79 #define DBG_NO_CAN_DEVICE "(%s) Can't find CAN device \"%s\"", __func__
+
80 #define DBG_OBJECT_DICTIONARY "(%s) Error in Object Dictionary \"%s\"", __func__
+
81 #define DBG_OD_ENTRY "(%s) Error in Object Dictionary entry: 0x%X", __func__
+
82 #define DBG_CAN_OPEN "(%s) CANopen error in %s, err=%d", __func__
+
83 #define DBG_CAN_OPEN_INFO "CANopen device, Node ID = 0x%02X, %s"
+
84 
+
85 /* CO_epoll_interface */
+
86 #define DBG_EPOLL_UNKNOWN "(%s) CAN Epoll error, events=0x%02x, fd=%d", __func__
+
87 #define DBG_COMMAND_LOCAL_BIND "(%s) Can't bind local socket to path \"%s\"", __func__
+
88 #define DBG_COMMAND_TCP_BIND "(%s) Can't bind tcp socket to port \"%d\"", __func__
+
89 #define DBG_COMMAND_STDIO_INFO "CANopen command interface on \"standard IO\" started"
+
90 #define DBG_COMMAND_LOCAL_INFO "CANopen command interface on local socket \"%s\" started"
+
91 #define DBG_COMMAND_TCP_INFO "CANopen command interface on tcp port \"%d\" started"
+
92 
+
93 
+
94 #ifdef __cplusplus
+
95 }
+
96 #endif /* __cplusplus */
+
97 
+
98 #endif /* CO_ERROR_MSGS_H */
+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/CO__fifo_8h.html b/Devices/Libraries/Systems/CANopenSocket/docs/CO__fifo_8h.html new file mode 100755 index 0000000..3ffd8cb --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/CO__fifo_8h.html @@ -0,0 +1,317 @@ + + + + + + + +CANopenNode: 301/CO_fifo.h File Reference + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Data Structures | +Enumerations | +Functions
+
+
CO_fifo.h File Reference
CANopen_301 » FIFO circular buffer
+
+
+ +

FIFO circular buffer. +More...

+
#include "301/CO_driver.h"
+
+

Go to the source code of this file.

+ + + + + +

+Data Structures

struct  CO_fifo_t
 Fifo object. More...
 
+ + + + +

+Enumerations

enum  CO_fifo_st {
+  CO_fifo_st_closed = 0x01U, +CO_fifo_st_partial = 0x02U, +CO_fifo_st_errTok = 0x10U, +CO_fifo_st_errVal = 0x20U, +
+  CO_fifo_st_errBuf = 0x40U, +CO_fifo_st_errInt = 0x80U, +CO_fifo_st_errMask = 0xF0U +
+ }
 Bitfields for status argument from CO_fifo_cpyTok2U8 function and similar. More...
 
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

+Functions

void CO_fifo_init (CO_fifo_t *fifo, char *buf, size_t bufSize)
 Initialize fifo object. More...
 
static void CO_fifo_reset (CO_fifo_t *fifo)
 Reset fifo object, make it empty. More...
 
static bool_t CO_fifo_purge (CO_fifo_t *fifo)
 Purge all data in fifo object, keep other properties. More...
 
static size_t CO_fifo_getSpace (CO_fifo_t *fifo)
 Get free buffer space in CO_fifo_t object. More...
 
static size_t CO_fifo_getOccupied (CO_fifo_t *fifo)
 Get size of data inside CO_fifo_t buffer object. More...
 
static bool_t CO_fifo_putc (CO_fifo_t *fifo, const char c)
 Put one character into CO_fifo_t buffer object. More...
 
static void CO_fifo_putc_ov (CO_fifo_t *fifo, const char c)
 Put one character into CO_fifo_t buffer object. More...
 
static bool_t CO_fifo_getc (CO_fifo_t *fifo, char *buf)
 Get one character from CO_fifo_t buffer object. More...
 
size_t CO_fifo_write (CO_fifo_t *fifo, const char *buf, size_t count, uint16_t *crc)
 Write data into CO_fifo_t object. More...
 
size_t CO_fifo_read (CO_fifo_t *fifo, char *buf, size_t count, bool_t *eof)
 Read data from CO_fifo_t object. More...
 
size_t CO_fifo_altBegin (CO_fifo_t *fifo, size_t offset)
 Initializes alternate read with CO_fifo_altRead. More...
 
void CO_fifo_altFinish (CO_fifo_t *fifo, uint16_t *crc)
 Ends alternate read with CO_fifo_altRead and calculate crc checksum. More...
 
static size_t CO_fifo_altGetOccupied (CO_fifo_t *fifo)
 Get alternate size of remaining data, see CO_fifo_altRead. More...
 
size_t CO_fifo_altRead (CO_fifo_t *fifo, char *buf, size_t count)
 Alternate read data from CO_fifo_t object. More...
 
bool_t CO_fifo_CommSearch (CO_fifo_t *fifo, bool_t clear)
 Search command inside FIFO. More...
 
bool_t CO_fifo_trimSpaces (CO_fifo_t *fifo, bool_t *insideComment)
 Trim spaces inside FIFO. More...
 
size_t CO_fifo_readToken (CO_fifo_t *fifo, char *buf, size_t count, char *closed, bool_t *err)
 Get token from FIFO buffer. More...
 
size_t CO_fifo_readU82a (CO_fifo_t *fifo, char *buf, size_t count, bool_t end)
 Read uint8_t variable from fifo and output as ascii string. More...
 
+size_t CO_fifo_readU162a (CO_fifo_t *fifo, char *buf, size_t count, bool_t end)
 Read uint16_t variable from fifo as ascii string, see CO_fifo_readU82a.
 
+size_t CO_fifo_readU322a (CO_fifo_t *fifo, char *buf, size_t count, bool_t end)
 Read uint32_t variable from fifo as ascii string, see CO_fifo_readU82a.
 
+size_t CO_fifo_readU642a (CO_fifo_t *fifo, char *buf, size_t count, bool_t end)
 Read uint64_t variable from fifo as ascii string, see CO_fifo_readU82a.
 
+size_t CO_fifo_readX82a (CO_fifo_t *fifo, char *buf, size_t count, bool_t end)
 Read uint8_t variable from fifo as ascii string, see CO_fifo_readU82a.
 
+size_t CO_fifo_readX162a (CO_fifo_t *fifo, char *buf, size_t count, bool_t end)
 Read uint16_t variable from fifo as ascii string, see CO_fifo_readU82a.
 
+size_t CO_fifo_readX322a (CO_fifo_t *fifo, char *buf, size_t count, bool_t end)
 Read uint32_t variable from fifo as ascii string, see CO_fifo_readU82a.
 
+size_t CO_fifo_readX642a (CO_fifo_t *fifo, char *buf, size_t count, bool_t end)
 Read uint64_t variable from fifo as ascii string, see CO_fifo_readU82a.
 
+size_t CO_fifo_readI82a (CO_fifo_t *fifo, char *buf, size_t count, bool_t end)
 Read int8_t variable from fifo as ascii string, see CO_fifo_readU82a.
 
+size_t CO_fifo_readI162a (CO_fifo_t *fifo, char *buf, size_t count, bool_t end)
 Read int16_t variable from fifo as ascii string, see CO_fifo_readU82a.
 
+size_t CO_fifo_readI322a (CO_fifo_t *fifo, char *buf, size_t count, bool_t end)
 Read int32_t variable from fifo as ascii string, see CO_fifo_readU82a.
 
+size_t CO_fifo_readI642a (CO_fifo_t *fifo, char *buf, size_t count, bool_t end)
 Read int64_t variable from fifo as ascii string, see CO_fifo_readU82a.
 
+size_t CO_fifo_readR322a (CO_fifo_t *fifo, char *buf, size_t count, bool_t end)
 Read float32_t variable from fifo as ascii string, see CO_fifo_readU82a.
 
+size_t CO_fifo_readR642a (CO_fifo_t *fifo, char *buf, size_t count, bool_t end)
 Read float64_t variable from fifo as ascii string, see CO_fifo_readU82a.
 
+size_t CO_fifo_readHex2a (CO_fifo_t *fifo, char *buf, size_t count, bool_t end)
 Read data from fifo and output as space separated two digit ascii string, see also CO_fifo_readU82a.
 
size_t CO_fifo_readVs2a (CO_fifo_t *fifo, char *buf, size_t count, bool_t end)
 Read data from fifo and output as visible string. More...
 
size_t CO_fifo_readB642a (CO_fifo_t *fifo, char *buf, size_t count, bool_t end)
 Read data from fifo and output as mime-base64 encoded string. More...
 
size_t CO_fifo_cpyTok2U8 (CO_fifo_t *dest, CO_fifo_t *src, CO_fifo_st *status)
 Read ascii string from src fifo and copy as uint8_t variable to dest fifo. More...
 
+size_t CO_fifo_cpyTok2U16 (CO_fifo_t *dest, CO_fifo_t *src, CO_fifo_st *status)
 Copy ascii string to uint16_t variable, see CO_fifo_cpyTok2U8.
 
+size_t CO_fifo_cpyTok2U32 (CO_fifo_t *dest, CO_fifo_t *src, CO_fifo_st *status)
 Copy ascii string to uint32_t variable, see CO_fifo_cpyTok2U8.
 
+size_t CO_fifo_cpyTok2U64 (CO_fifo_t *dest, CO_fifo_t *src, CO_fifo_st *status)
 Copy ascii string to uint64_t variable, see CO_fifo_cpyTok2U8.
 
+size_t CO_fifo_cpyTok2I8 (CO_fifo_t *dest, CO_fifo_t *src, CO_fifo_st *status)
 Copy ascii string to int8_t variable, see CO_fifo_cpyTok2U8.
 
+size_t CO_fifo_cpyTok2I16 (CO_fifo_t *dest, CO_fifo_t *src, CO_fifo_st *status)
 Copy ascii string to int16_t variable, see CO_fifo_cpyTok2U8.
 
+size_t CO_fifo_cpyTok2I32 (CO_fifo_t *dest, CO_fifo_t *src, CO_fifo_st *status)
 Copy ascii string to int32_t variable, see CO_fifo_cpyTok2U8.
 
+size_t CO_fifo_cpyTok2I64 (CO_fifo_t *dest, CO_fifo_t *src, CO_fifo_st *status)
 Copy ascii string to int64_t variable, see CO_fifo_cpyTok2U8.
 
+size_t CO_fifo_cpyTok2R32 (CO_fifo_t *dest, CO_fifo_t *src, CO_fifo_st *status)
 Copy ascii string to float32_t variable, see CO_fifo_cpyTok2U8.
 
+size_t CO_fifo_cpyTok2R64 (CO_fifo_t *dest, CO_fifo_t *src, CO_fifo_st *status)
 Copy ascii string to float64_t variable, see CO_fifo_cpyTok2U8.
 
size_t CO_fifo_cpyTok2Hex (CO_fifo_t *dest, CO_fifo_t *src, CO_fifo_st *status)
 Copy bytes written as two hex digits into to data. More...
 
size_t CO_fifo_cpyTok2Vs (CO_fifo_t *dest, CO_fifo_t *src, CO_fifo_st *status)
 Copy visible string to data. More...
 
size_t CO_fifo_cpyTok2B64 (CO_fifo_t *dest, CO_fifo_t *src, CO_fifo_st *status)
 Read ascii mime-base64 encoded string from src fifo and copy as binary data to dest fifo. More...
 
+

Detailed Description

+

FIFO circular buffer.

+
Author
Janez Paternoster
+
Copyright
2020 Janez Paternoster
+

This file is part of CANopenNode, an opensource CANopen Stack. Project home page is https://github.com/CANopenNode/CANopenNode. For more information on CANopen see http://www.can-cia.org/.

+

Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. You may obtain a copy of the License at 

http://www.apache.org/licenses/LICENSE-2.0
+

Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License.

+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/CO__fifo_8h.js b/Devices/Libraries/Systems/CANopenSocket/docs/CO__fifo_8h.js new file mode 100755 index 0000000..4bdf23b --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/CO__fifo_8h.js @@ -0,0 +1,59 @@ +var CO__fifo_8h = +[ + [ "CO_fifo_st", "group__CO__CANopen__301__fifo.html#ga2c7db7d527e4055a5dde62b74dfc2818", [ + [ "CO_fifo_st_closed", "group__CO__CANopen__301__fifo.html#gga2c7db7d527e4055a5dde62b74dfc2818a1f111c3f5a9396da6ced33132a2341f9", null ], + [ "CO_fifo_st_partial", "group__CO__CANopen__301__fifo.html#gga2c7db7d527e4055a5dde62b74dfc2818a7bf88ff5b78ef6a25a4391bc6423c1f7", null ], + [ "CO_fifo_st_errTok", "group__CO__CANopen__301__fifo.html#gga2c7db7d527e4055a5dde62b74dfc2818a70060b411fcbfa0696ca453d7f4348d4", null ], + [ "CO_fifo_st_errVal", "group__CO__CANopen__301__fifo.html#gga2c7db7d527e4055a5dde62b74dfc2818a9c41b295a487450017cc3b1ce0bcedfb", null ], + [ "CO_fifo_st_errBuf", "group__CO__CANopen__301__fifo.html#gga2c7db7d527e4055a5dde62b74dfc2818a51c064e77b0e2bf3741719b57de65141", null ], + [ "CO_fifo_st_errInt", "group__CO__CANopen__301__fifo.html#gga2c7db7d527e4055a5dde62b74dfc2818a07a7d8d483365e3afdcf9dc5b2dd2143", null ], + [ "CO_fifo_st_errMask", "group__CO__CANopen__301__fifo.html#gga2c7db7d527e4055a5dde62b74dfc2818a9e587fa1d802fbe939f6da9d44aba389", null ] + ] ], + [ "CO_fifo_init", "group__CO__CANopen__301__fifo.html#ga44e8f83feb2dd463b2fbb399dcd4de4d", null ], + [ "CO_fifo_reset", "group__CO__CANopen__301__fifo.html#ga6817a4baeaee87a578291e9de820b126", null ], + [ "CO_fifo_purge", "group__CO__CANopen__301__fifo.html#ga15a27a4871e680475f680c574050844a", null ], + [ "CO_fifo_getSpace", "group__CO__CANopen__301__fifo.html#ga0d456e83af18cce9db9157b5a30fac21", null ], + [ "CO_fifo_getOccupied", "group__CO__CANopen__301__fifo.html#ga40ef4fc5dc184ef3c0c2f37eca1bc507", null ], + [ "CO_fifo_putc", "group__CO__CANopen__301__fifo.html#ga7c2deb5abee499ad2f1b5175a205bc5b", null ], + [ "CO_fifo_putc_ov", "group__CO__CANopen__301__fifo.html#gae9351e31e5d74e738fe5e1514f99f2dd", null ], + [ "CO_fifo_getc", "group__CO__CANopen__301__fifo.html#ga4c1237aa63123d0ab8e9e16132a5c40a", null ], + [ "CO_fifo_write", "group__CO__CANopen__301__fifo.html#ga715cb5e1feacd2f3af5bc8195bbe69d3", null ], + [ "CO_fifo_read", "group__CO__CANopen__301__fifo.html#gad0ba8be2a601030a374913e4fa94e6cb", null ], + [ "CO_fifo_altBegin", "group__CO__CANopen__301__fifo.html#ga55c186098b2d860b116ddeb637a27b5a", null ], + [ "CO_fifo_altFinish", "group__CO__CANopen__301__fifo.html#ga7c534dd50959814b40c3027aa85f1c55", null ], + [ "CO_fifo_altGetOccupied", "group__CO__CANopen__301__fifo.html#ga2ee45a0cab19a212022d82987de2a955", null ], + [ "CO_fifo_altRead", "group__CO__CANopen__301__fifo.html#gaa28339101c4ac5a2c44004276075c759", null ], + [ "CO_fifo_CommSearch", "group__CO__CANopen__301__fifo.html#ga0dbef46e369e659bab7e29742971134c", null ], + [ "CO_fifo_trimSpaces", "group__CO__CANopen__301__fifo.html#gac01a59d65d275bd9f9a7fb0ea04fb915", null ], + [ "CO_fifo_readToken", "group__CO__CANopen__301__fifo.html#ga87a199708c95d3ca02e2fcc4ca4a6319", null ], + [ "CO_fifo_readU82a", "group__CO__CANopen__301__fifo.html#ga50b3075a0cfab3ab8608a4ba4977ecd6", null ], + [ "CO_fifo_readU162a", "group__CO__CANopen__301__fifo.html#ga99072bd50ad3b92c291136e458885ca2", null ], + [ "CO_fifo_readU322a", "group__CO__CANopen__301__fifo.html#ga9a2d7d07570a8f552a81e8c72327f43c", null ], + [ "CO_fifo_readU642a", "group__CO__CANopen__301__fifo.html#ga8805e75a32e9f8672259f2274816a0b8", null ], + [ "CO_fifo_readX82a", "group__CO__CANopen__301__fifo.html#ga7ed37d0c4caea872cfe15a0c2e86fe0e", null ], + [ "CO_fifo_readX162a", "group__CO__CANopen__301__fifo.html#ga3202ac32250b599f5a5719c52264578a", null ], + [ "CO_fifo_readX322a", "group__CO__CANopen__301__fifo.html#ga8dcf1f06de7b744d95c6a65721828a5a", null ], + [ "CO_fifo_readX642a", "group__CO__CANopen__301__fifo.html#ga387acd7fdbd61389a42935d452dbd409", null ], + [ "CO_fifo_readI82a", "group__CO__CANopen__301__fifo.html#ga00fe8ce9f04846e8c756a09aad22cf03", null ], + [ "CO_fifo_readI162a", "group__CO__CANopen__301__fifo.html#ga01a1765094d97ca3d0c5bbf98d1b8b1f", null ], + [ "CO_fifo_readI322a", "group__CO__CANopen__301__fifo.html#ga6a7a2ec338f735cdddf7ea7f7c0fd3a5", null ], + [ "CO_fifo_readI642a", "group__CO__CANopen__301__fifo.html#gaf959a6b823cc6db1d2107fd1faa40472", null ], + [ "CO_fifo_readR322a", "group__CO__CANopen__301__fifo.html#ga74aad9b161d3953221a99d4b238f3c1f", null ], + [ "CO_fifo_readR642a", "group__CO__CANopen__301__fifo.html#gadc55fa15874eaf757acf83921d0156c4", null ], + [ "CO_fifo_readHex2a", "group__CO__CANopen__301__fifo.html#gae9bfcb5f9c52de5f7239ab133fe326ec", null ], + [ "CO_fifo_readVs2a", "group__CO__CANopen__301__fifo.html#ga8000ba92f2023a88b5f7d0399373b206", null ], + [ "CO_fifo_readB642a", "group__CO__CANopen__301__fifo.html#ga7b9dcd98906837e82fa914d10028cf33", null ], + [ "CO_fifo_cpyTok2U8", "group__CO__CANopen__301__fifo.html#ga1788f69639bcc6bfa59bf85e57e5e13a", null ], + [ "CO_fifo_cpyTok2U16", "group__CO__CANopen__301__fifo.html#gaf19bdbd45626578afc8a19de43698e0b", null ], + [ "CO_fifo_cpyTok2U32", "group__CO__CANopen__301__fifo.html#gae2816b287cc2091d2382aaa6ac56e422", null ], + [ "CO_fifo_cpyTok2U64", "group__CO__CANopen__301__fifo.html#gacfd3fdfda66e1feb6982f68377a8b7e2", null ], + [ "CO_fifo_cpyTok2I8", "group__CO__CANopen__301__fifo.html#gae038564001c68ab242a6f33756668ac5", null ], + [ "CO_fifo_cpyTok2I16", "group__CO__CANopen__301__fifo.html#ga306913df146db90f7209b89080d51640", null ], + [ "CO_fifo_cpyTok2I32", "group__CO__CANopen__301__fifo.html#ga4f5b25af39c5a78faf04e54cb3cc90f7", null ], + [ "CO_fifo_cpyTok2I64", "group__CO__CANopen__301__fifo.html#ga6a9bd6e8d07d20ded9aeca531f412d50", null ], + [ "CO_fifo_cpyTok2R32", "group__CO__CANopen__301__fifo.html#ga853847c375425a55fc2007880d94c484", null ], + [ "CO_fifo_cpyTok2R64", "group__CO__CANopen__301__fifo.html#ga8b17d4f3c0d272bb73d82a79cd51cd3d", null ], + [ "CO_fifo_cpyTok2Hex", "group__CO__CANopen__301__fifo.html#gab3c4acf458a13bd6c4e5b0bc2adca10b", null ], + [ "CO_fifo_cpyTok2Vs", "group__CO__CANopen__301__fifo.html#ga8cb3d7032cd46dfeb3b6c0bbd5ed1575", null ], + [ "CO_fifo_cpyTok2B64", "group__CO__CANopen__301__fifo.html#gaf11cf0cccc01e86f341e2b31607e90c7", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/CO__fifo_8h_source.html b/Devices/Libraries/Systems/CANopenSocket/docs/CO__fifo_8h_source.html new file mode 100755 index 0000000..074ea83 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/CO__fifo_8h_source.html @@ -0,0 +1,401 @@ + + + + + + + +CANopenNode: 301/CO_fifo.h Source File + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+
CO_fifo.h
+
+
+Go to the documentation of this file.
1 
+
26 #ifndef CO_FIFO_H
+
27 #define CO_FIFO_H
+
28 
+
29 #include "301/CO_driver.h"
+
30 
+
31 /* default configuration, see CO_config.h */
+
32 #ifndef CO_CONFIG_FIFO
+
33 #define CO_CONFIG_FIFO (0)
+
34 #endif
+
35 
+
36 #if ((CO_CONFIG_FIFO) & CO_CONFIG_FIFO_ENABLE) || defined CO_DOXYGEN
+
37 
+
38 #ifdef __cplusplus
+
39 extern "C" {
+
40 #endif
+
41 
+
60 typedef struct {
+
62  char *buf;
+
64  size_t bufSize;
+
66  size_t writePtr;
+
68  size_t readPtr;
+
69 #if ((CO_CONFIG_FIFO) & CO_CONFIG_FIFO_ALT_READ) || defined CO_DOXYGEN
+
70 
+
71  size_t altReadPtr;
+
72 #endif
+
73 #if ((CO_CONFIG_FIFO) & CO_CONFIG_FIFO_ASCII_DATATYPES) || defined CO_DOXYGEN
+
74 
+
76  bool_t started;
+
78  uint32_t aux;
+
79 #endif
+
80 } CO_fifo_t;
+
81 
+
82 
+
83 
+
92 void CO_fifo_init(CO_fifo_t *fifo, char *buf, size_t bufSize);
+
93 
+
94 
+
100 static inline void CO_fifo_reset(CO_fifo_t *fifo) {
+
101  if (fifo != NULL) {
+
102  fifo->readPtr = 0;
+
103  fifo->writePtr = 0;
+
104 #if (CO_CONFIG_FIFO) & CO_CONFIG_FIFO_ASCII_DATATYPES
+
105  fifo->started = false;
+
106 #endif
+
107  }
+
108 
+
109  return;
+
110 }
+
111 
+
112 
+
120 static inline bool_t CO_fifo_purge(CO_fifo_t *fifo) {
+
121  if (fifo != NULL && fifo->readPtr != fifo->writePtr) {
+
122  fifo->readPtr = 0;
+
123  fifo->writePtr = 0;
+
124  return true;
+
125  }
+
126 
+
127  return false;
+
128 }
+
129 
+
130 
+
138 static inline size_t CO_fifo_getSpace(CO_fifo_t *fifo) {
+
139  int sizeLeft = (int)fifo->readPtr - fifo->writePtr - 1;
+
140  if (sizeLeft < 0) {
+
141  sizeLeft += fifo->bufSize;
+
142  }
+
143 
+
144  return (size_t) sizeLeft;
+
145 }
+
146 
+
147 
+
155 static inline size_t CO_fifo_getOccupied(CO_fifo_t *fifo) {
+
156  int sizeOccupied = (int)fifo->writePtr - fifo->readPtr;
+
157  if (sizeOccupied < 0) {
+
158  sizeOccupied += fifo->bufSize;
+
159  }
+
160 
+
161  return (size_t) sizeOccupied;
+
162 }
+
163 
+
164 
+
173 static inline bool_t CO_fifo_putc(CO_fifo_t *fifo, const char c) {
+
174  if (fifo != NULL && fifo->buf != NULL) {
+
175  size_t writePtrNext = fifo->writePtr + 1;
+
176  if (writePtrNext != fifo->readPtr &&
+
177  !(writePtrNext == fifo->bufSize && fifo->readPtr == 0))
+
178  {
+
179  fifo->buf[fifo->writePtr] = c;
+
180  fifo->writePtr = (writePtrNext == fifo->bufSize) ? 0 : writePtrNext;
+
181  return true;
+
182  }
+
183  }
+
184  return false;
+
185 }
+
186 
+
187 
+
196 static inline void CO_fifo_putc_ov(CO_fifo_t *fifo, const char c) {
+
197  if (fifo != NULL && fifo->buf != NULL) {
+
198  fifo->buf[fifo->writePtr] = c;
+
199 
+
200  if (++fifo->writePtr == fifo->bufSize) fifo->writePtr = 0;
+
201  if (fifo->readPtr == fifo->writePtr) {
+
202  if (++fifo->readPtr == fifo->bufSize) fifo->readPtr = 0;
+
203  }
+
204  }
+
205 }
+
206 
+
207 
+
216 static inline bool_t CO_fifo_getc(CO_fifo_t *fifo, char *buf) {
+
217  if (fifo != NULL && buf != NULL && fifo->readPtr != fifo->writePtr) {
+
218  *buf = fifo->buf[fifo->readPtr];
+
219  if (++fifo->readPtr == fifo->bufSize) {
+
220  fifo->readPtr = 0;
+
221  }
+
222  return true;
+
223  }
+
224  return false;
+
225 }
+
226 
+
227 
+
243 size_t CO_fifo_write(CO_fifo_t *fifo,
+
244  const char *buf,
+
245  size_t count,
+
246  uint16_t *crc);
+
247 
+
248 
+
265 size_t CO_fifo_read(CO_fifo_t *fifo, char *buf, size_t count, bool_t *eof);
+
266 
+
267 
+
268 #if ((CO_CONFIG_FIFO) & CO_CONFIG_FIFO_ALT_READ) || defined CO_DOXYGEN
+
269 
+
277 size_t CO_fifo_altBegin(CO_fifo_t *fifo, size_t offset);
+
278 
+
279 
+
287 void CO_fifo_altFinish(CO_fifo_t *fifo, uint16_t *crc);
+
288 
+
289 
+
297 static inline size_t CO_fifo_altGetOccupied(CO_fifo_t *fifo) {
+
298  int sizeOccupied = (int)fifo->writePtr - fifo->altReadPtr;
+
299  if (sizeOccupied < 0) {
+
300  sizeOccupied += fifo->bufSize;
+
301  }
+
302 
+
303  return (size_t) sizeOccupied;
+
304 }
+
305 
+
306 
+
323 size_t CO_fifo_altRead(CO_fifo_t *fifo, char *buf, size_t count);
+
324 #endif /* #if (CO_CONFIG_FIFO) & CO_CONFIG_FIFO_ALT_READ */
+
325 
+
326 
+
327 #if ((CO_CONFIG_FIFO) & CO_CONFIG_FIFO_ASCII_COMMANDS) || defined CO_DOXYGEN
+
328 
+
352 bool_t CO_fifo_CommSearch(CO_fifo_t *fifo, bool_t clear);
+
353 
+
354 
+
369 bool_t CO_fifo_trimSpaces(CO_fifo_t *fifo, bool_t *insideComment);
+
370 
+
371 
+
409 size_t CO_fifo_readToken(CO_fifo_t *fifo,
+
410  char *buf,
+
411  size_t count,
+
412  char *closed,
+
413  bool_t *err);
+
414 #endif /* (CO_CONFIG_FIFO) & CO_CONFIG_FIFO_ASCII_COMMANDS */
+
415 
+
416 
+
417 #if ((CO_CONFIG_FIFO) & CO_CONFIG_FIFO_ASCII_DATATYPES) || defined CO_DOXYGEN
+
418 
+
428 size_t CO_fifo_readU82a (CO_fifo_t *fifo, char *buf, size_t count, bool_t end);
+
430 size_t CO_fifo_readU162a(CO_fifo_t *fifo, char *buf, size_t count, bool_t end);
+
432 size_t CO_fifo_readU322a(CO_fifo_t *fifo, char *buf, size_t count, bool_t end);
+
434 size_t CO_fifo_readU642a(CO_fifo_t *fifo, char *buf, size_t count, bool_t end);
+
436 size_t CO_fifo_readX82a (CO_fifo_t *fifo, char *buf, size_t count, bool_t end);
+
438 size_t CO_fifo_readX162a(CO_fifo_t *fifo, char *buf, size_t count, bool_t end);
+
440 size_t CO_fifo_readX322a(CO_fifo_t *fifo, char *buf, size_t count, bool_t end);
+
442 size_t CO_fifo_readX642a(CO_fifo_t *fifo, char *buf, size_t count, bool_t end);
+
444 size_t CO_fifo_readI82a (CO_fifo_t *fifo, char *buf, size_t count, bool_t end);
+
446 size_t CO_fifo_readI162a(CO_fifo_t *fifo, char *buf, size_t count, bool_t end);
+
448 size_t CO_fifo_readI322a(CO_fifo_t *fifo, char *buf, size_t count, bool_t end);
+
450 size_t CO_fifo_readI642a(CO_fifo_t *fifo, char *buf, size_t count, bool_t end);
+
452 size_t CO_fifo_readR322a(CO_fifo_t *fifo, char *buf, size_t count, bool_t end);
+
454 size_t CO_fifo_readR642a(CO_fifo_t *fifo, char *buf, size_t count, bool_t end);
+
457 size_t CO_fifo_readHex2a(CO_fifo_t *fifo, char *buf, size_t count, bool_t end);
+
464 size_t CO_fifo_readVs2a(CO_fifo_t *fifo, char *buf, size_t count, bool_t end);
+
468 size_t CO_fifo_readB642a(CO_fifo_t *fifo, char *buf, size_t count, bool_t end);
+
469 
+
470 
+
472 typedef enum {
+
474  CO_fifo_st_closed = 0x01U,
+
477  CO_fifo_st_partial = 0x02U,
+
479  CO_fifo_st_errTok = 0x10U,
+
481  CO_fifo_st_errVal = 0x20U,
+
483  CO_fifo_st_errBuf = 0x40U,
+
485  CO_fifo_st_errInt = 0x80U,
+
487  CO_fifo_st_errMask = 0xF0U
+
488 } CO_fifo_st;
+
489 
+
499 size_t CO_fifo_cpyTok2U8 (CO_fifo_t *dest, CO_fifo_t *src, CO_fifo_st *status);
+
501 size_t CO_fifo_cpyTok2U16(CO_fifo_t *dest, CO_fifo_t *src, CO_fifo_st *status);
+
503 size_t CO_fifo_cpyTok2U32(CO_fifo_t *dest, CO_fifo_t *src, CO_fifo_st *status);
+
505 size_t CO_fifo_cpyTok2U64(CO_fifo_t *dest, CO_fifo_t *src, CO_fifo_st *status);
+
507 size_t CO_fifo_cpyTok2I8 (CO_fifo_t *dest, CO_fifo_t *src, CO_fifo_st *status);
+
509 size_t CO_fifo_cpyTok2I16(CO_fifo_t *dest, CO_fifo_t *src, CO_fifo_st *status);
+
511 size_t CO_fifo_cpyTok2I32(CO_fifo_t *dest, CO_fifo_t *src, CO_fifo_st *status);
+
513 size_t CO_fifo_cpyTok2I64(CO_fifo_t *dest, CO_fifo_t *src, CO_fifo_st *status);
+
515 size_t CO_fifo_cpyTok2R32(CO_fifo_t *dest, CO_fifo_t *src, CO_fifo_st *status);
+
517 size_t CO_fifo_cpyTok2R64(CO_fifo_t *dest, CO_fifo_t *src, CO_fifo_st *status);
+
520 size_t CO_fifo_cpyTok2Hex(CO_fifo_t *dest, CO_fifo_t *src, CO_fifo_st *status);
+
525 size_t CO_fifo_cpyTok2Vs(CO_fifo_t *dest, CO_fifo_t *src, CO_fifo_st *status);
+
529 size_t CO_fifo_cpyTok2B64(CO_fifo_t *dest, CO_fifo_t *src, CO_fifo_st *status);
+
530 
+
531 #endif /* (CO_CONFIG_FIFO) & CO_CONFIG_FIFO_ASCII_DATATYPES */
+
532  /* CO_CANopen_301_fifo */
+
534 
+
535 #ifdef __cplusplus
+
536 }
+
537 #endif /*__cplusplus*/
+
538 
+
539 #endif /* (CO_CONFIG_FIFO) & CO_CONFIG_FIFO_ENABLE */
+
540 
+
541 #endif /* CO_FIFO_H */
+
+
+
CO_fifo_readI82a
size_t CO_fifo_readI82a(CO_fifo_t *fifo, char *buf, size_t count, bool_t end)
Read int8_t variable from fifo as ascii string, see CO_fifo_readU82a.
+
CO_fifo_readU642a
size_t CO_fifo_readU642a(CO_fifo_t *fifo, char *buf, size_t count, bool_t end)
Read uint64_t variable from fifo as ascii string, see CO_fifo_readU82a.
+
uint32_t
unsigned long int uint32_t
UNSIGNED32 in CANopen (0007h), 32-bit unsigned integer.
Definition: CO_driver.h:155
+
CO_fifo_readU322a
size_t CO_fifo_readU322a(CO_fifo_t *fifo, char *buf, size_t count, bool_t end)
Read uint32_t variable from fifo as ascii string, see CO_fifo_readU82a.
+
CO_fifo_st_errVal
@ CO_fifo_st_errVal
Bit is set, if value is not valid or out of limits.
Definition: CO_fifo.h:481
+
CO_driver.h
Interface between CAN hardware and CANopenNode.
+
CO_fifo_cpyTok2Hex
size_t CO_fifo_cpyTok2Hex(CO_fifo_t *dest, CO_fifo_t *src, CO_fifo_st *status)
Copy bytes written as two hex digits into to data.
+
CO_fifo_readU82a
size_t CO_fifo_readU82a(CO_fifo_t *fifo, char *buf, size_t count, bool_t end)
Read uint8_t variable from fifo and output as ascii string.
+
CO_fifo_readX82a
size_t CO_fifo_readX82a(CO_fifo_t *fifo, char *buf, size_t count, bool_t end)
Read uint8_t variable from fifo as ascii string, see CO_fifo_readU82a.
+
CO_fifo_t::writePtr
size_t writePtr
Location in the buffer, which will be next written.
Definition: CO_fifo.h:66
+
CO_fifo_cpyTok2B64
size_t CO_fifo_cpyTok2B64(CO_fifo_t *dest, CO_fifo_t *src, CO_fifo_st *status)
Read ascii mime-base64 encoded string from src fifo and copy as binary data to dest fifo.
+
CO_fifo_reset
static void CO_fifo_reset(CO_fifo_t *fifo)
Reset fifo object, make it empty.
Definition: CO_fifo.h:100
+
uint16_t
unsigned int uint16_t
UNSIGNED16 in CANopen (0006h), 16-bit unsigned integer.
Definition: CO_driver.h:153
+
CO_fifo_t::altReadPtr
size_t altReadPtr
Location in the buffer, which will be next read.
Definition: CO_fifo.h:71
+
CO_fifo_altGetOccupied
static size_t CO_fifo_altGetOccupied(CO_fifo_t *fifo)
Get alternate size of remaining data, see CO_fifo_altRead.
Definition: CO_fifo.h:297
+
CO_fifo_cpyTok2U32
size_t CO_fifo_cpyTok2U32(CO_fifo_t *dest, CO_fifo_t *src, CO_fifo_st *status)
Copy ascii string to uint32_t variable, see CO_fifo_cpyTok2U8.
+
bool_t
unsigned char bool_t
Boolean data type for general use.
Definition: CO_driver.h:141
+
CO_fifo_getc
static bool_t CO_fifo_getc(CO_fifo_t *fifo, char *buf)
Get one character from CO_fifo_t buffer object.
Definition: CO_fifo.h:216
+
CO_fifo_t::buf
char * buf
Buffer of size bufSize.
Definition: CO_fifo.h:62
+
CO_fifo_readR642a
size_t CO_fifo_readR642a(CO_fifo_t *fifo, char *buf, size_t count, bool_t end)
Read float64_t variable from fifo as ascii string, see CO_fifo_readU82a.
+
CO_fifo_t::started
bool_t started
helper variable, set to false in CO_fifo_reset(), used in some functions.
Definition: CO_fifo.h:76
+
CO_fifo_st_errInt
@ CO_fifo_st_errInt
Bit is set, if internal error.
Definition: CO_fifo.h:485
+
CO_fifo_readB642a
size_t CO_fifo_readB642a(CO_fifo_t *fifo, char *buf, size_t count, bool_t end)
Read data from fifo and output as mime-base64 encoded string.
+
CO_fifo_readR322a
size_t CO_fifo_readR322a(CO_fifo_t *fifo, char *buf, size_t count, bool_t end)
Read float32_t variable from fifo as ascii string, see CO_fifo_readU82a.
+
CO_fifo_cpyTok2U8
size_t CO_fifo_cpyTok2U8(CO_fifo_t *dest, CO_fifo_t *src, CO_fifo_st *status)
Read ascii string from src fifo and copy as uint8_t variable to dest fifo.
+
CO_fifo_t::bufSize
size_t bufSize
Initialized by CO_fifo_init()
Definition: CO_fifo.h:64
+
CO_fifo_st_errMask
@ CO_fifo_st_errMask
Bitmask for error bits.
Definition: CO_fifo.h:487
+
CO_fifo_st_errBuf
@ CO_fifo_st_errBuf
Bit is set, if destination buffer is to small.
Definition: CO_fifo.h:483
+
CO_fifo_init
void CO_fifo_init(CO_fifo_t *fifo, char *buf, size_t bufSize)
Initialize fifo object.
+
CO_fifo_st
CO_fifo_st
Bitfields for status argument from CO_fifo_cpyTok2U8 function and similar.
Definition: CO_fifo.h:472
+
CO_fifo_readI642a
size_t CO_fifo_readI642a(CO_fifo_t *fifo, char *buf, size_t count, bool_t end)
Read int64_t variable from fifo as ascii string, see CO_fifo_readU82a.
+
CO_fifo_cpyTok2Vs
size_t CO_fifo_cpyTok2Vs(CO_fifo_t *dest, CO_fifo_t *src, CO_fifo_st *status)
Copy visible string to data.
+
CO_fifo_getOccupied
static size_t CO_fifo_getOccupied(CO_fifo_t *fifo)
Get size of data inside CO_fifo_t buffer object.
Definition: CO_fifo.h:155
+
CO_fifo_cpyTok2I32
size_t CO_fifo_cpyTok2I32(CO_fifo_t *dest, CO_fifo_t *src, CO_fifo_st *status)
Copy ascii string to int32_t variable, see CO_fifo_cpyTok2U8.
+
CO_fifo_readU162a
size_t CO_fifo_readU162a(CO_fifo_t *fifo, char *buf, size_t count, bool_t end)
Read uint16_t variable from fifo as ascii string, see CO_fifo_readU82a.
+
CO_fifo_altBegin
size_t CO_fifo_altBegin(CO_fifo_t *fifo, size_t offset)
Initializes alternate read with CO_fifo_altRead.
+
CO_fifo_cpyTok2I16
size_t CO_fifo_cpyTok2I16(CO_fifo_t *dest, CO_fifo_t *src, CO_fifo_st *status)
Copy ascii string to int16_t variable, see CO_fifo_cpyTok2U8.
+
CO_fifo_t::readPtr
size_t readPtr
Location in the buffer, which will be next read.
Definition: CO_fifo.h:68
+
CO_fifo_t
Fifo object.
Definition: CO_fifo.h:60
+
CO_fifo_putc
static bool_t CO_fifo_putc(CO_fifo_t *fifo, const char c)
Put one character into CO_fifo_t buffer object.
Definition: CO_fifo.h:173
+
CO_fifo_readX642a
size_t CO_fifo_readX642a(CO_fifo_t *fifo, char *buf, size_t count, bool_t end)
Read uint64_t variable from fifo as ascii string, see CO_fifo_readU82a.
+
CO_fifo_cpyTok2I8
size_t CO_fifo_cpyTok2I8(CO_fifo_t *dest, CO_fifo_t *src, CO_fifo_st *status)
Copy ascii string to int8_t variable, see CO_fifo_cpyTok2U8.
+
CO_fifo_st_partial
@ CO_fifo_st_partial
Bit is set, if copy was partial and more data are available.
Definition: CO_fifo.h:477
+
CO_fifo_cpyTok2U16
size_t CO_fifo_cpyTok2U16(CO_fifo_t *dest, CO_fifo_t *src, CO_fifo_st *status)
Copy ascii string to uint16_t variable, see CO_fifo_cpyTok2U8.
+
CO_fifo_write
size_t CO_fifo_write(CO_fifo_t *fifo, const char *buf, size_t count, uint16_t *crc)
Write data into CO_fifo_t object.
+
CO_fifo_readX322a
size_t CO_fifo_readX322a(CO_fifo_t *fifo, char *buf, size_t count, bool_t end)
Read uint32_t variable from fifo as ascii string, see CO_fifo_readU82a.
+
CO_fifo_readI322a
size_t CO_fifo_readI322a(CO_fifo_t *fifo, char *buf, size_t count, bool_t end)
Read int32_t variable from fifo as ascii string, see CO_fifo_readU82a.
+
CO_fifo_readVs2a
size_t CO_fifo_readVs2a(CO_fifo_t *fifo, char *buf, size_t count, bool_t end)
Read data from fifo and output as visible string.
+
CO_fifo_purge
static bool_t CO_fifo_purge(CO_fifo_t *fifo)
Purge all data in fifo object, keep other properties.
Definition: CO_fifo.h:120
+
CO_fifo_read
size_t CO_fifo_read(CO_fifo_t *fifo, char *buf, size_t count, bool_t *eof)
Read data from CO_fifo_t object.
+
CO_fifo_altRead
size_t CO_fifo_altRead(CO_fifo_t *fifo, char *buf, size_t count)
Alternate read data from CO_fifo_t object.
+
CO_fifo_altFinish
void CO_fifo_altFinish(CO_fifo_t *fifo, uint16_t *crc)
Ends alternate read with CO_fifo_altRead and calculate crc checksum.
+
CO_fifo_cpyTok2R64
size_t CO_fifo_cpyTok2R64(CO_fifo_t *dest, CO_fifo_t *src, CO_fifo_st *status)
Copy ascii string to float64_t variable, see CO_fifo_cpyTok2U8.
+
NULL
#define NULL
NULL, for general usage.
Definition: CO_driver.h:135
+
CO_fifo_t::aux
uint32_t aux
auxiliary variable, used in some functions.
Definition: CO_fifo.h:78
+
CO_fifo_cpyTok2I64
size_t CO_fifo_cpyTok2I64(CO_fifo_t *dest, CO_fifo_t *src, CO_fifo_st *status)
Copy ascii string to int64_t variable, see CO_fifo_cpyTok2U8.
+
CO_fifo_CommSearch
bool_t CO_fifo_CommSearch(CO_fifo_t *fifo, bool_t clear)
Search command inside FIFO.
+
CO_fifo_putc_ov
static void CO_fifo_putc_ov(CO_fifo_t *fifo, const char c)
Put one character into CO_fifo_t buffer object.
Definition: CO_fifo.h:196
+
CO_fifo_cpyTok2U64
size_t CO_fifo_cpyTok2U64(CO_fifo_t *dest, CO_fifo_t *src, CO_fifo_st *status)
Copy ascii string to uint64_t variable, see CO_fifo_cpyTok2U8.
+
CO_fifo_readI162a
size_t CO_fifo_readI162a(CO_fifo_t *fifo, char *buf, size_t count, bool_t end)
Read int16_t variable from fifo as ascii string, see CO_fifo_readU82a.
+
CO_fifo_cpyTok2R32
size_t CO_fifo_cpyTok2R32(CO_fifo_t *dest, CO_fifo_t *src, CO_fifo_st *status)
Copy ascii string to float32_t variable, see CO_fifo_cpyTok2U8.
+
CO_fifo_readToken
size_t CO_fifo_readToken(CO_fifo_t *fifo, char *buf, size_t count, char *closed, bool_t *err)
Get token from FIFO buffer.
+
CO_fifo_st_errTok
@ CO_fifo_st_errTok
Bit is set, if no valid token found.
Definition: CO_fifo.h:479
+
CO_fifo_readX162a
size_t CO_fifo_readX162a(CO_fifo_t *fifo, char *buf, size_t count, bool_t end)
Read uint16_t variable from fifo as ascii string, see CO_fifo_readU82a.
+
CO_fifo_readHex2a
size_t CO_fifo_readHex2a(CO_fifo_t *fifo, char *buf, size_t count, bool_t end)
Read data from fifo and output as space separated two digit ascii string, see also CO_fifo_readU82a.
+
CO_fifo_st_closed
@ CO_fifo_st_closed
Bit is set, if command delimiter is reached in src.
Definition: CO_fifo.h:474
+
CO_fifo_getSpace
static size_t CO_fifo_getSpace(CO_fifo_t *fifo)
Get free buffer space in CO_fifo_t object.
Definition: CO_fifo.h:138
+
CO_fifo_trimSpaces
bool_t CO_fifo_trimSpaces(CO_fifo_t *fifo, bool_t *insideComment)
Trim spaces inside FIFO.
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/CO__gateway__ascii_8h.html b/Devices/Libraries/Systems/CANopenSocket/docs/CO__gateway__ascii_8h.html new file mode 100755 index 0000000..1aeef7f --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/CO__gateway__ascii_8h.html @@ -0,0 +1,239 @@ + + + + + + + +CANopenNode: 309/CO_gateway_ascii.h File Reference + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Data Structures | +Macros | +Enumerations | +Functions
+
+
CO_gateway_ascii.h File Reference
CANopen_309 » Gateway ASCII mapping
+
+
+ +

CANopen access from other networks - ASCII mapping (CiA 309-3 DS v3.0.0) +More...

+
#include "301/CO_driver.h"
+#include "301/CO_fifo.h"
+#include "301/CO_SDOclient.h"
+#include "301/CO_NMT_Heartbeat.h"
+#include "305/CO_LSSmaster.h"
+#include "303/CO_LEDs.h"
+
+

Go to the source code of this file.

+ + + + + +

+Data Structures

struct  CO_GTWA_t
 CANopen Gateway-ascii object. More...
 
+ + + + + + + +

+Macros

#define CO_GTWA_RESP_BUF_SIZE   200
 Size of response string buffer. More...
 
+#define CO_GTWA_STATE_TIMEOUT_TIME_US   1200000
 Timeout time in microseconds for some internal states.
 
+ + + + + + + +

+Enumerations

enum  CO_GTWA_respErrorCode_t {
+  CO_GTWA_respErrorNone = 0, +CO_GTWA_respErrorReqNotSupported = 100, +CO_GTWA_respErrorSyntax = 101, +CO_GTWA_respErrorInternalState = 102, +
+  CO_GTWA_respErrorTimeOut = 103, +CO_GTWA_respErrorNoDefaultNetSet = 104, +CO_GTWA_respErrorNoDefaultNodeSet = 105, +CO_GTWA_respErrorUnsupportedNet = 106, +
+  CO_GTWA_respErrorUnsupportedNode = 107, +CO_GTWA_respErrorLostGuardingMessage = 200, +CO_GTWA_respErrorLostConnection = 201, +CO_GTWA_respErrorHeartbeatStarted = 202, +
+  CO_GTWA_respErrorHeartbeatLost = 203, +CO_GTWA_respErrorWrongNMTstate = 204, +CO_GTWA_respErrorBootUp = 205, +CO_GTWA_respErrorErrorPassive = 300, +
+  CO_GTWA_respErrorBusOff = 301, +CO_GTWA_respErrorCANbufferOverflow = 303, +CO_GTWA_respErrorCANinit = 304, +CO_GTWA_respErrorCANactive = 305, +
+  CO_GTWA_respErrorPDOalreadyUsed = 400, +CO_GTWA_respErrorPDOlengthExceeded = 401, +CO_GTWA_respErrorLSSmanufacturer = 501, +CO_GTWA_respErrorLSSnodeIdNotSupported = 502, +
+  CO_GTWA_respErrorLSSbitRateNotSupported = 503, +CO_GTWA_respErrorLSSparameterStoringFailed = 504, +CO_GTWA_respErrorLSSmediaError = 505, +CO_GTWA_respErrorRunningOutOfMemory = 600 +
+ }
 Response error codes as specified by CiA 309-3. More...
 
enum  CO_GTWA_state_t {
+  CO_GTWA_ST_IDLE = 0x00U, +CO_GTWA_ST_READ = 0x10U, +CO_GTWA_ST_WRITE = 0x11U, +CO_GTWA_ST_WRITE_ABORTED = 0x12U, +
+  CO_GTWA_ST_LSS_SWITCH_GLOB = 0x20U, +CO_GTWA_ST_LSS_SWITCH_SEL = 0x21U, +CO_GTWA_ST_LSS_SET_NODE = 0x22U, +CO_GTWA_ST_LSS_CONF_BITRATE = 0x23U, +
+  CO_GTWA_ST_LSS_STORE = 0x24U, +CO_GTWA_ST_LSS_INQUIRE = 0x25U, +CO_GTWA_ST_LSS_INQUIRE_ADDR_ALL = 0x26U, +CO_GTWA_ST__LSS_FASTSCAN = 0x30U, +
+  CO_GTWA_ST_LSS_ALLNODES = 0x31U, +CO_GTWA_ST_LOG = 0x80U, +CO_GTWA_ST_HELP = 0x81U, +CO_GTWA_ST_LED = 0x82U +
+ }
 Internal states of the Gateway-ascii state machine. More...
 
+ + + + + + + + + + + + + + + + + + + +

+Functions

CO_ReturnError_t CO_GTWA_init (CO_GTWA_t *gtwa, CO_SDOclient_t *SDO_C, uint16_t SDOclientTimeoutTime_ms, bool_t SDOclientBlockTransfer, CO_NMT_t *NMT, CO_LSSmaster_t *LSSmaster, CO_LEDs_t *LEDs, uint8_t dummy)
 Initialize Gateway-ascii object. More...
 
void CO_GTWA_initRead (CO_GTWA_t *gtwa, size_t(*readCallback)(void *object, const char *buf, size_t count, uint8_t *connectionOK), void *readCallbackObject)
 Initialize read callback in Gateway-ascii object. More...
 
static size_t CO_GTWA_write_getSpace (CO_GTWA_t *gtwa)
 Get free write buffer space. More...
 
static size_t CO_GTWA_write (CO_GTWA_t *gtwa, const char *buf, size_t count)
 Write command into CO_GTWA_t object. More...
 
void CO_GTWA_log_print (CO_GTWA_t *gtwa, const char *message)
 Print message log string into fifo buffer. More...
 
void CO_GTWA_process (CO_GTWA_t *gtwa, bool_t enable, uint32_t timeDifference_us, uint32_t *timerNext_us)
 Process Gateway-ascii object. More...
 
+

Detailed Description

+

CANopen access from other networks - ASCII mapping (CiA 309-3 DS v3.0.0)

+
Author
Janez Paternoster
+
+Martin Wagner
+
Copyright
2020 Janez Paternoster
+

This file is part of CANopenNode, an opensource CANopen Stack. Project home page is https://github.com/CANopenNode/CANopenNode. For more information on CANopen see http://www.can-cia.org/.

+

Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. You may obtain a copy of the License at 

http://www.apache.org/licenses/LICENSE-2.0
+

Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License.

+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/CO__gateway__ascii_8h.js b/Devices/Libraries/Systems/CANopenSocket/docs/CO__gateway__ascii_8h.js new file mode 100755 index 0000000..5d52891 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/CO__gateway__ascii_8h.js @@ -0,0 +1,59 @@ +var CO__gateway__ascii_8h = +[ + [ "CO_GTWA_RESP_BUF_SIZE", "group__CO__CANopen__309__3.html#ga52919223e5f43323f15c6a382913653d", null ], + [ "CO_GTWA_STATE_TIMEOUT_TIME_US", "group__CO__CANopen__309__3.html#gaa631d47f972204f26502d65894694cfb", null ], + [ "CO_GTWA_respErrorCode_t", "group__CO__CANopen__309__3.html#ga92e67dec9b5e29cdd67a28651db237fb", [ + [ "CO_GTWA_respErrorNone", "group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fba46a2114a5c4d9b43babecebb11573c66", null ], + [ "CO_GTWA_respErrorReqNotSupported", "group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fba04540349402c75b73af33951db285904", null ], + [ "CO_GTWA_respErrorSyntax", "group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fba541a3546ac6be179c728272409259c98", null ], + [ "CO_GTWA_respErrorInternalState", "group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fba1d990943991355e276f31533c841cb81", null ], + [ "CO_GTWA_respErrorTimeOut", "group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fba81131bf73d1ad39163a8b28e5ecf92fd", null ], + [ "CO_GTWA_respErrorNoDefaultNetSet", "group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fbad05b7717a0f79d333eed37ced2e758f9", null ], + [ "CO_GTWA_respErrorNoDefaultNodeSet", "group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fbac668fafa372a0cee1d50ceb0f5ed7b4a", null ], + [ "CO_GTWA_respErrorUnsupportedNet", "group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fba291ec5267c935d6277c27c767e2dd178", null ], + [ "CO_GTWA_respErrorUnsupportedNode", "group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fbaf65eee2744df8e528312139a54a853d5", null ], + [ "CO_GTWA_respErrorLostGuardingMessage", "group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fba1111be206d1ebfb62caa217d6231092c", null ], + [ "CO_GTWA_respErrorLostConnection", "group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fba9cc980ac35af4c1f9ad1d96719f05425", null ], + [ "CO_GTWA_respErrorHeartbeatStarted", "group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fbaf4ed1e079359d86cee1ad431ed745886", null ], + [ "CO_GTWA_respErrorHeartbeatLost", "group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fbaba7c053af330ec3ffdc8c2c298db6c93", null ], + [ "CO_GTWA_respErrorWrongNMTstate", "group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fbad007ed28e48b2fd7c287de3e6755a604", null ], + [ "CO_GTWA_respErrorBootUp", "group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fbaa4c5257721f3b64c231f67dccacd2ee7", null ], + [ "CO_GTWA_respErrorErrorPassive", "group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fba6f9a335886d942ef028878832551afe0", null ], + [ "CO_GTWA_respErrorBusOff", "group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fbabb407ab8220e0d3f0ebc06f867ecac9c", null ], + [ "CO_GTWA_respErrorCANbufferOverflow", "group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fba8aa993a6c57d8db7d6355260a091e4a4", null ], + [ "CO_GTWA_respErrorCANinit", "group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fba97d5b22449aadae1226679986d654bbc", null ], + [ "CO_GTWA_respErrorCANactive", "group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fba06e779c446aae0f7c6021dd0b955ffe8", null ], + [ "CO_GTWA_respErrorPDOalreadyUsed", "group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fbace2f0ef57b11e716d7ca58369ff5eede", null ], + [ "CO_GTWA_respErrorPDOlengthExceeded", "group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fbadd3dbd3197e1756102508208e394d72f", null ], + [ "CO_GTWA_respErrorLSSmanufacturer", "group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fba7e3209f49a5ae7bad0b197110aa78512", null ], + [ "CO_GTWA_respErrorLSSnodeIdNotSupported", "group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fba6e164941b6f3a18ef6e366e7c53073b6", null ], + [ "CO_GTWA_respErrorLSSbitRateNotSupported", "group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fbad8ff0b3e8970d010eb084a703fa794db", null ], + [ "CO_GTWA_respErrorLSSparameterStoringFailed", "group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fba315295e9ea4cdaba0955e6d8920df5a5", null ], + [ "CO_GTWA_respErrorLSSmediaError", "group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fbacc7ec1f2040bebb15becb067673e40e5", null ], + [ "CO_GTWA_respErrorRunningOutOfMemory", "group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fba99443a03d7ba443b9beb07c6d475b350", null ] + ] ], + [ "CO_GTWA_state_t", "group__CO__CANopen__309__3.html#gae809d7b5adbc7a4fb1f2fce527b30954", [ + [ "CO_GTWA_ST_IDLE", "group__CO__CANopen__309__3.html#ggae809d7b5adbc7a4fb1f2fce527b30954a98a36dabc8934b9c2d37b13999e3c393", null ], + [ "CO_GTWA_ST_READ", "group__CO__CANopen__309__3.html#ggae809d7b5adbc7a4fb1f2fce527b30954aaca55c8223aa1ba8f18031196178ea58", null ], + [ "CO_GTWA_ST_WRITE", "group__CO__CANopen__309__3.html#ggae809d7b5adbc7a4fb1f2fce527b30954a959c96b4eb3d948a977f66679423baa2", null ], + [ "CO_GTWA_ST_WRITE_ABORTED", "group__CO__CANopen__309__3.html#ggae809d7b5adbc7a4fb1f2fce527b30954ab10b68af4a8bc47f96abf552c7baa3b9", null ], + [ "CO_GTWA_ST_LSS_SWITCH_GLOB", "group__CO__CANopen__309__3.html#ggae809d7b5adbc7a4fb1f2fce527b30954afcc0bdbc08aa70d401f122c73055223e", null ], + [ "CO_GTWA_ST_LSS_SWITCH_SEL", "group__CO__CANopen__309__3.html#ggae809d7b5adbc7a4fb1f2fce527b30954a19bb6359e2efba37226a5dcf27d4a0e3", null ], + [ "CO_GTWA_ST_LSS_SET_NODE", "group__CO__CANopen__309__3.html#ggae809d7b5adbc7a4fb1f2fce527b30954acbc1d88036a086ae9d9dcbed1742330e", null ], + [ "CO_GTWA_ST_LSS_CONF_BITRATE", "group__CO__CANopen__309__3.html#ggae809d7b5adbc7a4fb1f2fce527b30954a6e18980d6fbcbd7dd871af5c98c4755e", null ], + [ "CO_GTWA_ST_LSS_STORE", "group__CO__CANopen__309__3.html#ggae809d7b5adbc7a4fb1f2fce527b30954a63e0705eddd22fb9d476c1b370394fc4", null ], + [ "CO_GTWA_ST_LSS_INQUIRE", "group__CO__CANopen__309__3.html#ggae809d7b5adbc7a4fb1f2fce527b30954abc4877f6d72e218cbd4959279a537b94", null ], + [ "CO_GTWA_ST_LSS_INQUIRE_ADDR_ALL", "group__CO__CANopen__309__3.html#ggae809d7b5adbc7a4fb1f2fce527b30954a17254161b141cc0c424c3649655f4df2", null ], + [ "CO_GTWA_ST__LSS_FASTSCAN", "group__CO__CANopen__309__3.html#ggae809d7b5adbc7a4fb1f2fce527b30954ab8c2946ce5d3581ecf5640bd1f5667b7", null ], + [ "CO_GTWA_ST_LSS_ALLNODES", "group__CO__CANopen__309__3.html#ggae809d7b5adbc7a4fb1f2fce527b30954a338a75317f0bf276f1c786316d6b9ec7", null ], + [ "CO_GTWA_ST_LOG", "group__CO__CANopen__309__3.html#ggae809d7b5adbc7a4fb1f2fce527b30954a829f096eca2ccd152069e97d9c70022f", null ], + [ "CO_GTWA_ST_HELP", "group__CO__CANopen__309__3.html#ggae809d7b5adbc7a4fb1f2fce527b30954af2f0ce738128675b98926aba680884d1", null ], + [ "CO_GTWA_ST_LED", "group__CO__CANopen__309__3.html#ggae809d7b5adbc7a4fb1f2fce527b30954a1a467cf33b8d3a2c0e8e31f87d81f05c", null ] + ] ], + [ "CO_GTWA_init", "group__CO__CANopen__309__3.html#gabc95dab4fb09bcb18948502f922520ee", null ], + [ "CO_GTWA_initRead", "group__CO__CANopen__309__3.html#ga2093c35b83b096e01bd0c65ae9374e30", null ], + [ "CO_GTWA_write_getSpace", "group__CO__CANopen__309__3.html#gad614da9bb171d995c02ffe1940fe7e64", null ], + [ "CO_GTWA_write", "group__CO__CANopen__309__3.html#gaae1f01d444be975aa3cb3b2c9ebded3d", null ], + [ "CO_GTWA_log_print", "group__CO__CANopen__309__3.html#ga20523907b832d55b47b855dd92409996", null ], + [ "CO_GTWA_process", "group__CO__CANopen__309__3.html#ga4a82ef2ebdd5d5f9d8a7efe84048493d", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/CO__gateway__ascii_8h_source.html b/Devices/Libraries/Systems/CANopenSocket/docs/CO__gateway__ascii_8h_source.html new file mode 100755 index 0000000..c7108b3 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/CO__gateway__ascii_8h_source.html @@ -0,0 +1,447 @@ + + + + + + + +CANopenNode: 309/CO_gateway_ascii.h Source File + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+
CO_gateway_ascii.h
+
+
+Go to the documentation of this file.
1 
+
27 #ifndef CO_GATEWAY_ASCII_H
+
28 #define CO_GATEWAY_ASCII_H
+
29 
+
30 #include "301/CO_driver.h"
+
31 #include "301/CO_fifo.h"
+
32 #include "301/CO_SDOclient.h"
+
33 #include "301/CO_NMT_Heartbeat.h"
+
34 #include "305/CO_LSSmaster.h"
+
35 #include "303/CO_LEDs.h"
+
36 
+
37 /* default configuration, see CO_config.h */
+
38 #ifndef CO_CONFIG_GTW
+
39 #define CO_CONFIG_GTW (0)
+
40 #endif
+
41 
+
42 #if ((CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII) || defined CO_DOXYGEN
+
43 
+
44 #ifdef __cplusplus
+
45 extern "C" {
+
46 #endif
+
47 
+
150 #ifndef CO_GTWA_RESP_BUF_SIZE
+
151 #define CO_GTWA_RESP_BUF_SIZE 200
+
152 #endif
+
153 
+
154 
+
156 #ifndef CO_GTWA_STATE_TIMEOUT_TIME_US
+
157 #define CO_GTWA_STATE_TIMEOUT_TIME_US 1200000
+
158 #endif
+
159 
+
160 
+
165 typedef enum {
+
167  CO_GTWA_respErrorNone = 0,
+
169  CO_GTWA_respErrorReqNotSupported = 100,
+
171  CO_GTWA_respErrorSyntax = 101,
+
173  CO_GTWA_respErrorInternalState = 102,
+
175  CO_GTWA_respErrorTimeOut = 103,
+
177  CO_GTWA_respErrorNoDefaultNetSet = 104,
+
179  CO_GTWA_respErrorNoDefaultNodeSet = 105,
+
181  CO_GTWA_respErrorUnsupportedNet = 106,
+
183  CO_GTWA_respErrorUnsupportedNode = 107,
+
185  CO_GTWA_respErrorLostGuardingMessage = 200,
+
187  CO_GTWA_respErrorLostConnection = 201,
+
189  CO_GTWA_respErrorHeartbeatStarted = 202,
+
191  CO_GTWA_respErrorHeartbeatLost = 203,
+
193  CO_GTWA_respErrorWrongNMTstate = 204,
+
195  CO_GTWA_respErrorBootUp = 205,
+
197  CO_GTWA_respErrorErrorPassive = 300,
+
199  CO_GTWA_respErrorBusOff = 301,
+
201  CO_GTWA_respErrorCANbufferOverflow = 303,
+
203  CO_GTWA_respErrorCANinit = 304,
+
205  CO_GTWA_respErrorCANactive = 305,
+
207  CO_GTWA_respErrorPDOalreadyUsed = 400,
+
209  CO_GTWA_respErrorPDOlengthExceeded = 401,
+
211  CO_GTWA_respErrorLSSmanufacturer = 501,
+
213  CO_GTWA_respErrorLSSnodeIdNotSupported = 502,
+
215  CO_GTWA_respErrorLSSbitRateNotSupported = 503,
+
217  CO_GTWA_respErrorLSSparameterStoringFailed = 504,
+
219  CO_GTWA_respErrorLSSmediaError = 505,
+
221  CO_GTWA_respErrorRunningOutOfMemory = 600
+
222 } CO_GTWA_respErrorCode_t;
+
223 
+
224 
+
228 typedef enum {
+
231  CO_GTWA_ST_IDLE = 0x00U,
+
233  CO_GTWA_ST_READ = 0x10U,
+
235  CO_GTWA_ST_WRITE = 0x11U,
+
237  CO_GTWA_ST_WRITE_ABORTED = 0x12U,
+
239  CO_GTWA_ST_LSS_SWITCH_GLOB = 0x20U,
+
241  CO_GTWA_ST_LSS_SWITCH_SEL = 0x21U,
+
243  CO_GTWA_ST_LSS_SET_NODE = 0x22U,
+
245  CO_GTWA_ST_LSS_CONF_BITRATE = 0x23U,
+
247  CO_GTWA_ST_LSS_STORE = 0x24U,
+
249  CO_GTWA_ST_LSS_INQUIRE = 0x25U,
+
251  CO_GTWA_ST_LSS_INQUIRE_ADDR_ALL = 0x26U,
+
253  CO_GTWA_ST__LSS_FASTSCAN = 0x30U,
+
255  CO_GTWA_ST_LSS_ALLNODES = 0x31U,
+
257  CO_GTWA_ST_LOG = 0x80U,
+
259  CO_GTWA_ST_HELP = 0x81U,
+
261  CO_GTWA_ST_LED = 0x82U
+
262 } CO_GTWA_state_t;
+
263 
+
264 
+
265 #if ((CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_SDO) || defined CO_DOXYGEN
+
266 /*
+
267  * CANopen Gateway-ascii data types structure
+
268  */
+
269 typedef struct {
+
271  char* syntax;
+
273  size_t length;
+
278  size_t (*dataTypePrint)(CO_fifo_t *fifo,
+
279  char *buf,
+
280  size_t count,
+
281  bool_t end);
+
286  size_t (*dataTypeScan)(CO_fifo_t *dest,
+
287  CO_fifo_t *src,
+
288  CO_fifo_st *status);
+
289 } CO_GTWA_dataType_t;
+
290 #endif /* (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_SDO */
+
291 
+
292 
+
296 typedef struct {
+
307  size_t (*readCallback)(void *object,
+
308  const char *buf,
+
309  size_t count,
+
310  uint8_t *connectionOK);
+
313  void *readCallbackObject;
+
315  uint32_t sequence;
+
317  int32_t net_default;
+
319  int16_t node_default;
+
321  uint16_t net;
+
323  uint8_t node;
+
325  CO_fifo_t commFifo;
+
327  char commBuf[CO_CONFIG_GTWA_COMM_BUF_SIZE + 1];
+
329  char respBuf[CO_GTWA_RESP_BUF_SIZE];
+
331  size_t respBufCount;
+
336  size_t respBufOffset;
+
338  bool_t respHold;
+
340  uint32_t timeDifference_us_cumulative;
+
342  CO_GTWA_state_t state;
+
344  uint32_t stateTimeoutTmr;
+
345 #if ((CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_SDO) || defined CO_DOXYGEN
+
346 
+
347  CO_SDOclient_t *SDO_C;
+
349  uint16_t SDOtimeoutTime;
+
351  bool_t SDOblockTransferEnable;
+
355  bool_t SDOdataCopyStatus;
+
357  const CO_GTWA_dataType_t *SDOdataType;
+
358 #endif
+
359 #if ((CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_NMT) || defined CO_DOXYGEN
+
360 
+
361  CO_NMT_t *NMT;
+
362 #endif
+
363 #if ((CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_LSS) || defined CO_DOXYGEN
+
364 
+
365  CO_LSSmaster_t *LSSmaster;
+
367  CO_LSS_address_t lssAddress;
+
369  uint8_t lssNID;
+
371  uint16_t lssBitrate;
+
373  CO_LSS_cs_t lssInquireCs;
+
375  CO_LSSmaster_fastscan_t lssFastscan;
+
377  uint8_t lssSubState;
+
379  uint8_t lssNodeCount;
+
381  bool_t lssStore;
+
383  uint16_t lssTimeout_ms;
+
384 #endif
+
385 #if ((CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_LOG) || defined CO_DOXYGEN
+
386 
+
387  char logBuf[CO_CONFIG_GTWA_LOG_BUF_SIZE + 1];
+
389  CO_fifo_t logFifo;
+
390 #endif
+
391 #if ((CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_PRINT_HELP) || defined CO_DOXYGEN
+
392 
+
393  const char *helpString;
+
394  size_t helpStringOffset;
+
395 #endif
+
396 #if ((CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_PRINT_LEDS) || defined CO_DOXYGEN
+
397 
+
398  CO_LEDs_t *LEDs;
+
399  uint8_t ledStringPreviousIndex;
+
400 #endif
+
401 } CO_GTWA_t;
+
402 
+
403 
+
418 CO_ReturnError_t CO_GTWA_init(CO_GTWA_t* gtwa,
+
419 #if ((CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_SDO) || defined CO_DOXYGEN
+
420  CO_SDOclient_t* SDO_C,
+
421  uint16_t SDOclientTimeoutTime_ms,
+
422  bool_t SDOclientBlockTransfer,
+
423 #endif
+
424 #if ((CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_NMT) || defined CO_DOXYGEN
+
425  CO_NMT_t *NMT,
+
426 #endif
+
427 #if ((CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_LSS) || defined CO_DOXYGEN
+
428  CO_LSSmaster_t *LSSmaster,
+
429 #endif
+
430 #if ((CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_PRINT_LEDS) || defined CO_DOXYGEN
+
431  CO_LEDs_t *LEDs,
+
432 #endif
+
433  uint8_t dummy);
+
434 
+
435 
+
450 void CO_GTWA_initRead(CO_GTWA_t* gtwa,
+
451  size_t (*readCallback)(void *object,
+
452  const char *buf,
+
453  size_t count,
+
454  uint8_t *connectionOK),
+
455  void *readCallbackObject);
+
456 
+
457 
+
465 static inline size_t CO_GTWA_write_getSpace(CO_GTWA_t* gtwa) {
+
466  return CO_fifo_getSpace(&gtwa->commFifo);
+
467 }
+
468 
+
469 
+
485 static inline size_t CO_GTWA_write(CO_GTWA_t* gtwa,
+
486  const char *buf,
+
487  size_t count)
+
488 {
+
489  return CO_fifo_write(&gtwa->commFifo, buf, count, NULL);
+
490 }
+
491 
+
492 
+
493 #if ((CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_LOG) || defined CO_DOXYGEN
+
494 
+
506 void CO_GTWA_log_print(CO_GTWA_t* gtwa, const char *message);
+
507 #endif /* (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII_LOG */
+
508 
+
509 
+
525 void CO_GTWA_process(CO_GTWA_t *gtwa,
+
526  bool_t enable,
+
527  uint32_t timeDifference_us,
+
528  uint32_t *timerNext_us);
+
529  /* CO_CANopen_309_3 */
+
531 
+
532 #ifdef __cplusplus
+
533 }
+
534 #endif /*__cplusplus*/
+
535 
+
536 #endif /* (CO_CONFIG_GTW) & CO_CONFIG_GTW_ASCII */
+
537 
+
538 #endif /* CO_GATEWAY_ASCII_H */
+
+
+
uint32_t
unsigned long int uint32_t
UNSIGNED32 in CANopen (0007h), 32-bit unsigned integer.
Definition: CO_driver.h:155
+
CO_GTWA_t::commFifo
CO_fifo_t commFifo
CO_fifo_t object for command (not pointer)
Definition: CO_gateway_ascii.h:325
+
CO_driver.h
Interface between CAN hardware and CANopenNode.
+
CO_GTWA_initRead
void CO_GTWA_initRead(CO_GTWA_t *gtwa, size_t(*readCallback)(void *object, const char *buf, size_t count, uint8_t *connectionOK), void *readCallbackObject)
Initialize read callback in Gateway-ascii object.
+
CO_GTWA_t::NMT
CO_NMT_t * NMT
NMT object from CO_GTWA_init()
Definition: CO_gateway_ascii.h:361
+
CO_GTWA_t::lssBitrate
uint16_t lssBitrate
LSS bitrate parameter.
Definition: CO_gateway_ascii.h:371
+
CO_GTWA_respErrorLSSmanufacturer
@ CO_GTWA_respErrorLSSmanufacturer
501 - LSS implementation- / manufacturer-specific error
Definition: CO_gateway_ascii.h:211
+
CO_LEDs_t
LEDs object, initialized by CO_LEDs_init()
Definition: CO_LEDs.h:93
+
CO_GTWA_t::node
uint8_t node
Current CANopen Node ID.
Definition: CO_gateway_ascii.h:323
+
CO_SDOclient_t
SDO client object.
Definition: CO_SDOclient.h:67
+
CO_GTWA_t::sequence
uint32_t sequence
Sequence number of the command.
Definition: CO_gateway_ascii.h:315
+
CO_GTWA_write
static size_t CO_GTWA_write(CO_GTWA_t *gtwa, const char *buf, size_t count)
Write command into CO_GTWA_t object.
Definition: CO_gateway_ascii.h:485
+
uint16_t
unsigned int uint16_t
UNSIGNED16 in CANopen (0006h), 16-bit unsigned integer.
Definition: CO_driver.h:153
+
CO_GTWA_t::lssNodeCount
uint8_t lssNodeCount
LSS allnodes node count parameter.
Definition: CO_gateway_ascii.h:379
+
CO_SDOclient.h
CANopen Service Data Object - client protocol.
+
CO_GTWA_ST_LSS_INQUIRE
@ CO_GTWA_ST_LSS_INQUIRE
LSS 'lss_inquire_addr' or 'lss_get_node'.
Definition: CO_gateway_ascii.h:249
+
CO_ReturnError_t
CO_ReturnError_t
Return values of some CANopen functions.
Definition: CO_driver.h:488
+
CO_GTWA_ST_IDLE
@ CO_GTWA_ST_IDLE
Gateway is idle, no command is processing.
Definition: CO_gateway_ascii.h:231
+
CO_GTWA_ST__LSS_FASTSCAN
@ CO_GTWA_ST__LSS_FASTSCAN
LSS '_lss_fastscan'.
Definition: CO_gateway_ascii.h:253
+
CO_GTWA_t::helpString
const char * helpString
Offset, when printing help text.
Definition: CO_gateway_ascii.h:393
+
bool_t
unsigned char bool_t
Boolean data type for general use.
Definition: CO_driver.h:141
+
CO_GTWA_t::lssSubState
uint8_t lssSubState
LSS allnodes sub state parameter.
Definition: CO_gateway_ascii.h:377
+
CO_GTWA_t::lssInquireCs
CO_LSS_cs_t lssInquireCs
LSS inquire parameter.
Definition: CO_gateway_ascii.h:373
+
CO_LSS_cs_t
CO_LSS_cs_t
LSS protocol command specifiers.
Definition: CO_LSS.h:86
+
CO_LSSmaster.h
CANopen Layer Setting Service - master protocol.
+
CO_GTWA_t::lssFastscan
CO_LSSmaster_fastscan_t lssFastscan
LSS fastscan parameter.
Definition: CO_gateway_ascii.h:375
+
int32_t
signed long int int32_t
INTEGER32 in CANopen (0004h), 32-bit signed integer.
Definition: CO_driver.h:147
+
CO_GTWA_t::respHold
bool_t respHold
See respBufOffset above.
Definition: CO_gateway_ascii.h:338
+
CO_GTWA_ST_WRITE
@ CO_GTWA_ST_WRITE
SDO 'write' (download)
Definition: CO_gateway_ascii.h:235
+
CO_GTWA_respErrorHeartbeatStarted
@ CO_GTWA_respErrorHeartbeatStarted
202 - Heartbeat started
Definition: CO_gateway_ascii.h:189
+
CO_GTWA_respErrorPDOlengthExceeded
@ CO_GTWA_respErrorPDOlengthExceeded
401 - PDO length exceeded
Definition: CO_gateway_ascii.h:209
+
CO_GTWA_t::readCallbackObject
void * readCallbackObject
Pointer to object, which will be used inside readCallback, from CO_GTWA_init()
Definition: CO_gateway_ascii.h:313
+
CO_GTWA_t::lssAddress
CO_LSS_address_t lssAddress
128 bit number, uniquely identifying each node
Definition: CO_gateway_ascii.h:367
+
CO_GTWA_respErrorBusOff
@ CO_GTWA_respErrorBusOff
301 - Bus off
Definition: CO_gateway_ascii.h:199
+
CO_NMT_t
NMT consumer and Heartbeat producer object.
Definition: CO_NMT_Heartbeat.h:162
+
CO_GTWA_respErrorLSSnodeIdNotSupported
@ CO_GTWA_respErrorLSSnodeIdNotSupported
502 - LSS node-ID not supported
Definition: CO_gateway_ascii.h:213
+
CO_GTWA_t::lssStore
bool_t lssStore
LSS allnodes store parameter.
Definition: CO_gateway_ascii.h:381
+
CO_GTWA_ST_LSS_CONF_BITRATE
@ CO_GTWA_ST_LSS_CONF_BITRATE
LSS 'lss_conf_bitrate'.
Definition: CO_gateway_ascii.h:245
+
CO_GTWA_write_getSpace
static size_t CO_GTWA_write_getSpace(CO_GTWA_t *gtwa)
Get free write buffer space.
Definition: CO_gateway_ascii.h:465
+
CO_GTWA_t::LEDs
CO_LEDs_t * LEDs
CO_LEDs_t object for CANopen status LEDs imitation from CO_GTWA_init()
Definition: CO_gateway_ascii.h:398
+
CO_GTWA_respErrorLostGuardingMessage
@ CO_GTWA_respErrorLostGuardingMessage
200 - Lost guarding message
Definition: CO_gateway_ascii.h:185
+
CO_GTWA_RESP_BUF_SIZE
#define CO_GTWA_RESP_BUF_SIZE
Size of response string buffer.
Definition: CO_gateway_ascii.h:151
+
CO_GTWA_t::SDO_C
CO_SDOclient_t * SDO_C
SDO client object from CO_GTWA_init()
Definition: CO_gateway_ascii.h:347
+
CO_GTWA_respErrorLSSmediaError
@ CO_GTWA_respErrorLSSmediaError
505 - LSS command failed because of media error
Definition: CO_gateway_ascii.h:219
+
CO_GTWA_respErrorWrongNMTstate
@ CO_GTWA_respErrorWrongNMTstate
204 - Wrong NMT state
Definition: CO_gateway_ascii.h:193
+
CO_GTWA_t::net
uint16_t net
Current CANopen Net number.
Definition: CO_gateway_ascii.h:321
+
CO_GTWA_respErrorLostConnection
@ CO_GTWA_respErrorLostConnection
201 - Lost connection
Definition: CO_gateway_ascii.h:187
+
CO_CONFIG_GTW
#define CO_CONFIG_GTW
Configuration of Gateway ASCII mapping.
Definition: CO_config.h:595
+
CO_GTWA_t::LSSmaster
CO_LSSmaster_t * LSSmaster
LSSmaster object from CO_GTWA_init()
Definition: CO_gateway_ascii.h:365
+
CO_GTWA_t::lssNID
uint8_t lssNID
LSS Node-ID parameter.
Definition: CO_gateway_ascii.h:369
+
CO_GTWA_state_t
CO_GTWA_state_t
Internal states of the Gateway-ascii state machine.
Definition: CO_gateway_ascii.h:228
+
CO_GTWA_init
CO_ReturnError_t CO_GTWA_init(CO_GTWA_t *gtwa, CO_SDOclient_t *SDO_C, uint16_t SDOclientTimeoutTime_ms, bool_t SDOclientBlockTransfer, CO_NMT_t *NMT, CO_LSSmaster_t *LSSmaster, CO_LEDs_t *LEDs, uint8_t dummy)
Initialize Gateway-ascii object.
+
CO_GTWA_respErrorNoDefaultNodeSet
@ CO_GTWA_respErrorNoDefaultNodeSet
105 - No default node set
Definition: CO_gateway_ascii.h:179
+
CO_GTWA_t::lssTimeout_ms
uint16_t lssTimeout_ms
LSS allnodes timeout parameter.
Definition: CO_gateway_ascii.h:383
+
CO_GTWA_respErrorCANinit
@ CO_GTWA_respErrorCANinit
304 - CAN init
Definition: CO_gateway_ascii.h:203
+
CO_GTWA_t::SDOdataType
const CO_GTWA_dataType_t * SDOdataType
Data type of variable in current SDO communication.
Definition: CO_gateway_ascii.h:357
+
CO_GTWA_respErrorSyntax
@ CO_GTWA_respErrorSyntax
101 - Syntax error
Definition: CO_gateway_ascii.h:171
+
CO_fifo_st
CO_fifo_st
Bitfields for status argument from CO_fifo_cpyTok2U8 function and similar.
Definition: CO_fifo.h:472
+
CO_LSSmaster_fastscan_t
Parameters for LSS fastscan CO_LSSmaster_IdentifyFastscan.
Definition: CO_LSSmaster.h:407
+
CO_LSSmaster_t
LSS master object.
Definition: CO_LSSmaster.h:108
+
CO_GTWA_respErrorNone
@ CO_GTWA_respErrorNone
0 - No error or idle
Definition: CO_gateway_ascii.h:167
+
CO_GTWA_t::SDOtimeoutTime
uint16_t SDOtimeoutTime
Timeout time for SDO transfer in milliseconds, if no response.
Definition: CO_gateway_ascii.h:349
+
CO_GTWA_respErrorTimeOut
@ CO_GTWA_respErrorTimeOut
103 - Time-out (where applicable)
Definition: CO_gateway_ascii.h:175
+
CO_GTWA_respErrorLSSparameterStoringFailed
@ CO_GTWA_respErrorLSSparameterStoringFailed
504 - LSS parameter storing failed
Definition: CO_gateway_ascii.h:217
+
CO_fifo_t
Fifo object.
Definition: CO_fifo.h:60
+
CO_GTWA_respErrorErrorPassive
@ CO_GTWA_respErrorErrorPassive
300 - Error passive
Definition: CO_gateway_ascii.h:197
+
CO_GTWA_ST_LSS_SWITCH_GLOB
@ CO_GTWA_ST_LSS_SWITCH_GLOB
LSS 'lss_switch_glob'.
Definition: CO_gateway_ascii.h:239
+
CO_GTWA_ST_LSS_STORE
@ CO_GTWA_ST_LSS_STORE
LSS 'lss_store'.
Definition: CO_gateway_ascii.h:247
+
CO_GTWA_t::node_default
int16_t node_default
Default CANopen Node ID number is undefined (-1) at startup.
Definition: CO_gateway_ascii.h:319
+
CO_GTWA_ST_LSS_INQUIRE_ADDR_ALL
@ CO_GTWA_ST_LSS_INQUIRE_ADDR_ALL
LSS 'lss_inquire_addr', all parameters.
Definition: CO_gateway_ascii.h:251
+
CO_GTWA_respErrorUnsupportedNet
@ CO_GTWA_respErrorUnsupportedNet
106 - Unsupported net
Definition: CO_gateway_ascii.h:181
+
CO_NMT_Heartbeat.h
CANopen Network management and Heartbeat producer protocol.
+
CO_fifo_write
size_t CO_fifo_write(CO_fifo_t *fifo, const char *buf, size_t count, uint16_t *crc)
Write data into CO_fifo_t object.
+
int16_t
signed int int16_t
INTEGER16 in CANopen (0003h), 16-bit signed integer.
Definition: CO_driver.h:145
+
CO_GTWA_process
void CO_GTWA_process(CO_GTWA_t *gtwa, bool_t enable, uint32_t timeDifference_us, uint32_t *timerNext_us)
Process Gateway-ascii object.
+
CO_GTWA_ST_LED
@ CO_GTWA_ST_LED
print 'status' of the node
Definition: CO_gateway_ascii.h:261
+
CO_GTWA_respErrorUnsupportedNode
@ CO_GTWA_respErrorUnsupportedNode
107 - Unsupported node
Definition: CO_gateway_ascii.h:183
+
CO_GTWA_t::stateTimeoutTmr
uint32_t stateTimeoutTmr
Timeout timer for the current state.
Definition: CO_gateway_ascii.h:344
+
CO_GTWA_respErrorCode_t
CO_GTWA_respErrorCode_t
Response error codes as specified by CiA 309-3.
Definition: CO_gateway_ascii.h:165
+
CO_GTWA_respErrorReqNotSupported
@ CO_GTWA_respErrorReqNotSupported
100 - Request not supported
Definition: CO_gateway_ascii.h:169
+
CO_GTWA_respErrorPDOalreadyUsed
@ CO_GTWA_respErrorPDOalreadyUsed
400 - PDO already used
Definition: CO_gateway_ascii.h:207
+
CO_GTWA_respErrorCANactive
@ CO_GTWA_respErrorCANactive
305 - CAN active (at init or start-up)
Definition: CO_gateway_ascii.h:205
+
CO_GTWA_respErrorInternalState
@ CO_GTWA_respErrorInternalState
102 - Request not processed due to internal state
Definition: CO_gateway_ascii.h:173
+
NULL
#define NULL
NULL, for general usage.
Definition: CO_driver.h:135
+
CO_GTWA_ST_LOG
@ CO_GTWA_ST_LOG
print message 'log'
Definition: CO_gateway_ascii.h:257
+
CO_CONFIG_GTWA_LOG_BUF_SIZE
#define CO_CONFIG_GTWA_LOG_BUF_SIZE
Size of message log buffer in ASCII gateway object.
Definition: CO_config.h:632
+
CO_CONFIG_GTWA_COMM_BUF_SIZE
#define CO_CONFIG_GTWA_COMM_BUF_SIZE
Size of command buffer in ASCII gateway object.
Definition: CO_config.h:625
+
CO_GTWA_ST_WRITE_ABORTED
@ CO_GTWA_ST_WRITE_ABORTED
SDO 'write' (download) - aborted, purging remaining data.
Definition: CO_gateway_ascii.h:237
+
CO_GTWA_respErrorNoDefaultNetSet
@ CO_GTWA_respErrorNoDefaultNetSet
104 - No default net set
Definition: CO_gateway_ascii.h:177
+
CO_GTWA_t::respBufOffset
size_t respBufOffset
If only part of data has been successfully written into external application (with readCallback()),...
Definition: CO_gateway_ascii.h:336
+
CO_GTWA_t::logFifo
CO_fifo_t logFifo
CO_fifo_t object for message log (not pointer)
Definition: CO_gateway_ascii.h:389
+
CO_GTWA_ST_LSS_SWITCH_SEL
@ CO_GTWA_ST_LSS_SWITCH_SEL
LSS 'lss_switch_sel'.
Definition: CO_gateway_ascii.h:241
+
CO_fifo.h
FIFO circular buffer.
+
CO_GTWA_respErrorRunningOutOfMemory
@ CO_GTWA_respErrorRunningOutOfMemory
600 - Running out of memory
Definition: CO_gateway_ascii.h:221
+
CO_LEDs.h
CANopen Indicator specification (CiA 303-3 v1.4.0)
+
CO_GTWA_ST_LSS_ALLNODES
@ CO_GTWA_ST_LSS_ALLNODES
LSS 'lss_allnodes'.
Definition: CO_gateway_ascii.h:255
+
CO_GTWA_t::net_default
int32_t net_default
Default CANopen Net number is undefined (-1) at startup.
Definition: CO_gateway_ascii.h:317
+
CO_GTWA_t::SDOdataCopyStatus
bool_t SDOdataCopyStatus
Indicate status of data copy from / to SDO buffer.
Definition: CO_gateway_ascii.h:355
+
CO_GTWA_t::SDOblockTransferEnable
bool_t SDOblockTransferEnable
SDO block transfer enabled?
Definition: CO_gateway_ascii.h:351
+
CO_GTWA_t::state
CO_GTWA_state_t state
Current state of the gateway object.
Definition: CO_gateway_ascii.h:342
+
CO_LSS_address_t
The LSS address is a 128 bit number, uniquely identifying each node.
Definition: CO_LSS.h:162
+
CO_GTWA_respErrorCANbufferOverflow
@ CO_GTWA_respErrorCANbufferOverflow
303 - CAN buffer overflow
Definition: CO_gateway_ascii.h:201
+
CO_GTWA_t::timeDifference_us_cumulative
uint32_t timeDifference_us_cumulative
Sum of time difference from CO_GTWA_process() in case of respHold.
Definition: CO_gateway_ascii.h:340
+
CO_GTWA_ST_READ
@ CO_GTWA_ST_READ
SDO 'read' (upload)
Definition: CO_gateway_ascii.h:233
+
CO_GTWA_respErrorHeartbeatLost
@ CO_GTWA_respErrorHeartbeatLost
203 - Heartbeat lost
Definition: CO_gateway_ascii.h:191
+
CO_GTWA_log_print
void CO_GTWA_log_print(CO_GTWA_t *gtwa, const char *message)
Print message log string into fifo buffer.
+
CO_GTWA_ST_HELP
@ CO_GTWA_ST_HELP
print 'help' text
Definition: CO_gateway_ascii.h:259
+
CO_fifo_getSpace
static size_t CO_fifo_getSpace(CO_fifo_t *fifo)
Get free buffer space in CO_fifo_t object.
Definition: CO_fifo.h:138
+
CO_GTWA_t
CANopen Gateway-ascii object.
Definition: CO_gateway_ascii.h:296
+
uint8_t
unsigned char uint8_t
UNSIGNED8 in CANopen (0005h), 8-bit unsigned integer.
Definition: CO_driver.h:151
+
CO_GTWA_ST_LSS_SET_NODE
@ CO_GTWA_ST_LSS_SET_NODE
LSS 'lss_set_node'.
Definition: CO_gateway_ascii.h:243
+
CO_GTWA_t::respBufCount
size_t respBufCount
Actual size of data in respBuf.
Definition: CO_gateway_ascii.h:331
+
CO_GTWA_respErrorBootUp
@ CO_GTWA_respErrorBootUp
205 - Boot-up
Definition: CO_gateway_ascii.h:195
+
CO_GTWA_respErrorLSSbitRateNotSupported
@ CO_GTWA_respErrorLSSbitRateNotSupported
503 - LSS bit-rate not supported
Definition: CO_gateway_ascii.h:215
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/CO__trace_8h.html b/Devices/Libraries/Systems/CANopenSocket/docs/CO__trace_8h.html new file mode 100755 index 0000000..d12f371 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/CO__trace_8h.html @@ -0,0 +1,154 @@ + + + + + + + +CANopenNode: extra/CO_trace.h File Reference + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Data Structures | +Macros | +Functions
+
+
CO_trace.h File Reference
CANopen_extra » Trace
+
+
+ +

CANopen trace object for recording variables over time. +More...

+
#include "301/CO_driver.h"
+#include "301/CO_SDOserver.h"
+
+

Go to the source code of this file.

+ + + + + + + + +

+Data Structures

struct  CO_trace_dataType_t
 structure for reading variables and printing points for specific data type. More...
 
struct  CO_trace_t
 Trace object. More...
 
+ + + + +

+Macros

+#define OD_INDEX_TRACE_CONFIG   0x2301
 Start index of traceConfig and Trace objects in Object Dictionary.
 
+ + + + + + + +

+Functions

void CO_trace_init (CO_trace_t *trace, CO_SDO_t *SDO, uint8_t enabled, uint32_t *timeBuffer, int32_t *valueBuffer, uint32_t bufferSize, uint32_t *map, uint8_t *format, uint8_t *trigger, int32_t *threshold, int32_t *value, int32_t *minValue, int32_t *maxValue, uint32_t *triggerTime, uint16_t idx_OD_traceConfig, uint16_t idx_OD_trace)
 Initialize trace object. More...
 
void CO_trace_process (CO_trace_t *trace, uint32_t timestamp)
 Process trace object. More...
 
+

Detailed Description

+

CANopen trace object for recording variables over time.

+
Author
Janez Paternoster
+
Copyright
2016 - 2020 Janez Paternoster
+

This file is part of CANopenNode, an opensource CANopen Stack. Project home page is https://github.com/CANopenNode/CANopenNode. For more information on CANopen see http://www.can-cia.org/.

+

Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. You may obtain a copy of the License at 

http://www.apache.org/licenses/LICENSE-2.0
+

Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License.

+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/CO__trace_8h.js b/Devices/Libraries/Systems/CANopenSocket/docs/CO__trace_8h.js new file mode 100755 index 0000000..0cf5f24 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/CO__trace_8h.js @@ -0,0 +1,6 @@ +var CO__trace_8h = +[ + [ "OD_INDEX_TRACE_CONFIG", "group__CO__trace.html#ga4edaf4c29180028f5f128f0aed2a417d", null ], + [ "CO_trace_init", "group__CO__trace.html#ga50621e5b8e28349f7caf260b6cf9962e", null ], + [ "CO_trace_process", "group__CO__trace.html#ga5b8d9460d7a42920325cf66eb8b725ec", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/CO__trace_8h_source.html b/Devices/Libraries/Systems/CANopenSocket/docs/CO__trace_8h_source.html new file mode 100755 index 0000000..38aef4d --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/CO__trace_8h_source.html @@ -0,0 +1,224 @@ + + + + + + + +CANopenNode: extra/CO_trace.h Source File + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+
CO_trace.h
+
+
+Go to the documentation of this file.
1 
+
26 #ifndef CO_TRACE_H
+
27 #define CO_TRACE_H
+
28 
+
29 #include "301/CO_driver.h"
+
30 #include "301/CO_SDOserver.h"
+
31 
+
32 /* default configuration, see CO_config.h */
+
33 #ifndef CO_CONFIG_TRACE
+
34 #define CO_CONFIG_TRACE (0)
+
35 #endif
+
36 
+
37 #if ((CO_CONFIG_TRACE) & CO_CONFIG_TRACE_ENABLE) || defined CO_DOXYGEN
+
38 
+
39 #ifdef __cplusplus
+
40 extern "C" {
+
41 #endif
+
42 
+
69 #ifndef OD_INDEX_TRACE_CONFIG
+
70 #define OD_INDEX_TRACE_CONFIG 0x2301
+
71 #define OD_INDEX_TRACE 0x2401
+
72 #endif
+
73 
+
74 
+
78 typedef struct {
+
80  int32_t (*pGetValue) (void *OD_variable);
+
82  uint32_t (*printPointStart)(char *s, uint32_t size, uint32_t timeStamp, int32_t value);
+
84  uint32_t (*printPoint)(char *s, uint32_t size, uint32_t timeStamp, int32_t value);
+
86  uint32_t (*printPointEnd)(char *s, uint32_t size, uint32_t timeStamp, int32_t value);
+
87 } CO_trace_dataType_t;
+
88 
+
89 
+
93 typedef struct {
+
94  bool_t enabled;
+
95  CO_SDO_t *SDO;
+
96  uint32_t *timeBuffer;
+
97  int32_t *valueBuffer;
+
98  uint32_t bufferSize;
+
99  volatile uint32_t writePtr;
+
100  volatile uint32_t readPtr;
+
101  uint32_t lastTimeStamp;
+
102  void *OD_variable;
+
103  const CO_trace_dataType_t *dt;
+
104  int32_t valuePrev;
+
105  uint32_t *map;
+
106  uint8_t *format;
+
107  int32_t *value;
+
108  int32_t *minValue;
+
109  int32_t *maxValue;
+
110  uint32_t *triggerTime;
+
111  uint8_t *trigger;
+
112  int32_t *threshold;
+
113 } CO_trace_t;
+
114 
+
115 
+
140 void CO_trace_init(
+
141  CO_trace_t *trace,
+
142  CO_SDO_t *SDO,
+
143  uint8_t enabled,
+
144  uint32_t *timeBuffer,
+
145  int32_t *valueBuffer,
+
146  uint32_t bufferSize,
+
147  uint32_t *map,
+
148  uint8_t *format,
+
149  uint8_t *trigger,
+
150  int32_t *threshold,
+
151  int32_t *value,
+
152  int32_t *minValue,
+
153  int32_t *maxValue,
+
154  uint32_t *triggerTime,
+
155  uint16_t idx_OD_traceConfig,
+
156  uint16_t idx_OD_trace);
+
157 
+
158 
+
169 void CO_trace_process(CO_trace_t *trace, uint32_t timestamp);
+
170  /* CO_trace */
+
172 
+
173 #ifdef __cplusplus
+
174 }
+
175 #endif /*__cplusplus*/
+
176 
+
177 #endif /* (CO_CONFIG_TRACE) & CO_CONFIG_TRACE_ENABLE */
+
178 
+
179 #endif /* CO_TRACE_H */
+
+
+
uint32_t
unsigned long int uint32_t
UNSIGNED32 in CANopen (0007h), 32-bit unsigned integer.
Definition: CO_driver.h:155
+
CO_driver.h
Interface between CAN hardware and CANopenNode.
+
CO_trace_t
Trace object.
Definition: CO_trace.h:93
+
CO_trace_t::minValue
int32_t * minValue
From CO_trace_init().
Definition: CO_trace.h:108
+
uint16_t
unsigned int uint16_t
UNSIGNED16 in CANopen (0006h), 16-bit unsigned integer.
Definition: CO_driver.h:153
+
CO_trace_t::triggerTime
uint32_t * triggerTime
From CO_trace_init().
Definition: CO_trace.h:110
+
CO_trace_t::valuePrev
int32_t valuePrev
Previous value of value.
Definition: CO_trace.h:104
+
CO_trace_t::timeBuffer
uint32_t * timeBuffer
From CO_trace_init().
Definition: CO_trace.h:96
+
CO_trace_t::bufferSize
uint32_t bufferSize
From CO_trace_init().
Definition: CO_trace.h:98
+
bool_t
unsigned char bool_t
Boolean data type for general use.
Definition: CO_driver.h:141
+
int32_t
signed long int int32_t
INTEGER32 in CANopen (0004h), 32-bit signed integer.
Definition: CO_driver.h:147
+
CO_trace_t::format
uint8_t * format
From CO_trace_init().
Definition: CO_trace.h:106
+
CO_trace_t::threshold
int32_t * threshold
From CO_trace_init().
Definition: CO_trace.h:112
+
CO_trace_t::writePtr
volatile uint32_t writePtr
Location in buffer, which will be next written.
Definition: CO_trace.h:99
+
CO_SDOserver.h
CANopen Service Data Object - server protocol.
+
CO_trace_t::lastTimeStamp
uint32_t lastTimeStamp
Last time stamp.
Definition: CO_trace.h:101
+
CO_trace_t::OD_variable
void * OD_variable
Pointer to variable, which is monitored.
Definition: CO_trace.h:102
+
CO_trace_t::SDO
CO_SDO_t * SDO
From CO_trace_init().
Definition: CO_trace.h:95
+
CO_trace_dataType_t
structure for reading variables and printing points for specific data type.
Definition: CO_trace.h:78
+
CO_trace_t::readPtr
volatile uint32_t readPtr
Location in buffer, which will be next read.
Definition: CO_trace.h:100
+
CO_trace_init
void CO_trace_init(CO_trace_t *trace, CO_SDO_t *SDO, uint8_t enabled, uint32_t *timeBuffer, int32_t *valueBuffer, uint32_t bufferSize, uint32_t *map, uint8_t *format, uint8_t *trigger, int32_t *threshold, int32_t *value, int32_t *minValue, int32_t *maxValue, uint32_t *triggerTime, uint16_t idx_OD_traceConfig, uint16_t idx_OD_trace)
Initialize trace object.
+
CO_trace_t::value
int32_t * value
From CO_trace_init().
Definition: CO_trace.h:107
+
CO_trace_t::enabled
bool_t enabled
True, if trace is enabled.
Definition: CO_trace.h:94
+
CO_trace_t::map
uint32_t * map
From CO_trace_init().
Definition: CO_trace.h:105
+
CO_trace_t::maxValue
int32_t * maxValue
From CO_trace_init().
Definition: CO_trace.h:109
+
CO_trace_t::trigger
uint8_t * trigger
From CO_trace_init().
Definition: CO_trace.h:111
+
CO_trace_process
void CO_trace_process(CO_trace_t *trace, uint32_t timestamp)
Process trace object.
+
CO_trace_t::dt
const CO_trace_dataType_t * dt
Data type specific function pointers.
Definition: CO_trace.h:103
+
uint8_t
unsigned char uint8_t
UNSIGNED8 in CANopen (0005h), 8-bit unsigned integer.
Definition: CO_driver.h:151
+
CO_trace_t::valueBuffer
int32_t * valueBuffer
From CO_trace_init().
Definition: CO_trace.h:97
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/annotated.html b/Devices/Libraries/Systems/CANopenSocket/docs/annotated.html new file mode 100755 index 0000000..28d7152 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/annotated.html @@ -0,0 +1,159 @@ + + + + + + + +CANopenNode: Data Structures + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+
Data Structures
+
+
+
Here are the data structures with brief descriptions:
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
 CCO_CANinterfaceErrorhandler_tSocketCAN interface error handling
 CCO_CANmodule_tComplete CAN module object
 CCO_CANrx_tConfiguration object for CAN received message for specific CANopenNode Object
 CCO_CANtx_tConfiguration object for CAN transmit message for specific CANopenNode Object
 CCO_config_tCANopen configuration, used with CO_new()
 CCO_EM_tEmergency object
 CCO_epoll_gtw_tObject for gateway
 CCO_epoll_tObject for epoll, timer and event API
 CCO_fifo_tFifo object
 CCO_GFC_tGFC object
 CCO_GTWA_tCANopen Gateway-ascii object
 CCO_HBconsNode_tOne monitored node inside CO_HBconsumer_t
 CCO_HBconsumer_tHeartbeat consumer object
 CCO_LEDs_tLEDs object, initialized by CO_LEDs_init()
 CCO_LSS_address_tThe LSS address is a 128 bit number, uniquely identifying each node
 CCO_LSSmaster_fastscan_tParameters for LSS fastscan CO_LSSmaster_IdentifyFastscan
 CCO_LSSmaster_tLSS master object
 CCO_LSSslave_tLSS slave object
 CCO_NMT_tNMT consumer and Heartbeat producer object
 CCO_OD_storage_tObject Dictionary storage object
 CCO_RPDO_tRPDO object
 CCO_RPDOCommPar_tRPDO communication parameter
 CCO_RPDOMapPar_tRPDO mapping parameter
 CCO_SDOclient_tSDO client object
 CCO_SDOserver_tSDO server object
 CCO_SRDO_tSRDO object
 CCO_SRDOCommPar_tSRDO communication parameter
 CCO_SRDOGuard_tGurad Object for SRDO monitors:
 CCO_SYNC_tSYNC producer and consumer object
 CCO_tCANopen object - collection of all CANopenNode objects
 CCO_TIME_tTIME producer and consumer object
 CCO_TPDO_tTPDO object
 CCO_TPDOCommPar_tTPDO communication parameter
 CCO_TPDOMapPar_tTPDO mapping parameter
 CCO_trace_dataType_tStructure for reading variables and printing points for specific data type
 CCO_trace_tTrace object
 COD_entry_tObject Dictionary entry for one OD object
 COD_extensionIO_tObject pointed by OD_obj_extended_t contains application specified parameters for extended OD object
 COD_IO_tStructure for input / output on the OD variable
 COD_obj_array_tObject for OD array of variables, used for "ARRAY" type OD objects
 COD_obj_extended_tObject for extended type of OD variable, configurable by OD_extensionIO_init() function
 COD_obj_record_tObject for OD sub-elements, used in "RECORD" type OD objects
 COD_obj_var_tObject for single OD variable, used for "VAR" type OD objects
 COD_stream_tIO stream structure, used for read/write access to OD variable, part of OD_IO_t
 COD_subEntry_tStructure describing properties of a variable, located in specific index and sub-index inside the Object Dictionary
 COD_tObject Dictionary
+
+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/annotated_dup.js b/Devices/Libraries/Systems/CANopenSocket/docs/annotated_dup.js new file mode 100755 index 0000000..012e162 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/annotated_dup.js @@ -0,0 +1,49 @@ +var annotated_dup = +[ + [ "CO_CANinterfaceErrorhandler_t", "structCO__CANinterfaceErrorhandler__t.html", "structCO__CANinterfaceErrorhandler__t" ], + [ "CO_CANmodule_t", "structCO__CANmodule__t.html", "structCO__CANmodule__t" ], + [ "CO_CANrx_t", "structCO__CANrx__t.html", "structCO__CANrx__t" ], + [ "CO_CANtx_t", "structCO__CANtx__t.html", "structCO__CANtx__t" ], + [ "CO_config_t", "structCO__config__t.html", "structCO__config__t" ], + [ "CO_EM_t", "structCO__EM__t.html", "structCO__EM__t" ], + [ "CO_epoll_gtw_t", "structCO__epoll__gtw__t.html", "structCO__epoll__gtw__t" ], + [ "CO_epoll_t", "structCO__epoll__t.html", "structCO__epoll__t" ], + [ "CO_fifo_t", "structCO__fifo__t.html", "structCO__fifo__t" ], + [ "CO_GFC_t", "structCO__GFC__t.html", "structCO__GFC__t" ], + [ "CO_GTWA_t", "structCO__GTWA__t.html", "structCO__GTWA__t" ], + [ "CO_HBconsNode_t", "structCO__HBconsNode__t.html", "structCO__HBconsNode__t" ], + [ "CO_HBconsumer_t", "structCO__HBconsumer__t.html", "structCO__HBconsumer__t" ], + [ "CO_LEDs_t", "structCO__LEDs__t.html", "structCO__LEDs__t" ], + [ "CO_LSS_address_t", "unionCO__LSS__address__t.html", null ], + [ "CO_LSSmaster_fastscan_t", "structCO__LSSmaster__fastscan__t.html", "structCO__LSSmaster__fastscan__t" ], + [ "CO_LSSmaster_t", "structCO__LSSmaster__t.html", "structCO__LSSmaster__t" ], + [ "CO_LSSslave_t", "structCO__LSSslave__t.html", "structCO__LSSslave__t" ], + [ "CO_NMT_t", "structCO__NMT__t.html", "structCO__NMT__t" ], + [ "CO_OD_storage_t", "structCO__OD__storage__t.html", "structCO__OD__storage__t" ], + [ "CO_RPDO_t", "structCO__RPDO__t.html", "structCO__RPDO__t" ], + [ "CO_RPDOCommPar_t", "structCO__RPDOCommPar__t.html", "structCO__RPDOCommPar__t" ], + [ "CO_RPDOMapPar_t", "structCO__RPDOMapPar__t.html", "structCO__RPDOMapPar__t" ], + [ "CO_SDOclient_t", "structCO__SDOclient__t.html", "structCO__SDOclient__t" ], + [ "CO_SDOserver_t", "structCO__SDOserver__t.html", "structCO__SDOserver__t" ], + [ "CO_SRDO_t", "structCO__SRDO__t.html", "structCO__SRDO__t" ], + [ "CO_SRDOCommPar_t", "structCO__SRDOCommPar__t.html", "structCO__SRDOCommPar__t" ], + [ "CO_SRDOGuard_t", "structCO__SRDOGuard__t.html", "structCO__SRDOGuard__t" ], + [ "CO_SYNC_t", "structCO__SYNC__t.html", "structCO__SYNC__t" ], + [ "CO_t", "structCO__t.html", "structCO__t" ], + [ "CO_TIME_t", "structCO__TIME__t.html", "structCO__TIME__t" ], + [ "CO_TPDO_t", "structCO__TPDO__t.html", "structCO__TPDO__t" ], + [ "CO_TPDOCommPar_t", "structCO__TPDOCommPar__t.html", "structCO__TPDOCommPar__t" ], + [ "CO_TPDOMapPar_t", "structCO__TPDOMapPar__t.html", "structCO__TPDOMapPar__t" ], + [ "CO_trace_dataType_t", "structCO__trace__dataType__t.html", "structCO__trace__dataType__t" ], + [ "CO_trace_t", "structCO__trace__t.html", "structCO__trace__t" ], + [ "OD_entry_t", "structOD__entry__t.html", "structOD__entry__t" ], + [ "OD_extensionIO_t", "structOD__extensionIO__t.html", "structOD__extensionIO__t" ], + [ "OD_IO_t", "structOD__IO__t.html", "structOD__IO__t" ], + [ "OD_obj_array_t", "structOD__obj__array__t.html", "structOD__obj__array__t" ], + [ "OD_obj_extended_t", "structOD__obj__extended__t.html", "structOD__obj__extended__t" ], + [ "OD_obj_record_t", "structOD__obj__record__t.html", "structOD__obj__record__t" ], + [ "OD_obj_var_t", "structOD__obj__var__t.html", "structOD__obj__var__t" ], + [ "OD_stream_t", "structOD__stream__t.html", "structOD__stream__t" ], + [ "OD_subEntry_t", "structOD__subEntry__t.html", "structOD__subEntry__t" ], + [ "OD_t", "structOD__t.html", "structOD__t" ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/bc_s.png b/Devices/Libraries/Systems/CANopenSocket/docs/bc_s.png new file mode 100755 index 0000000..224b29a Binary files /dev/null and b/Devices/Libraries/Systems/CANopenSocket/docs/bc_s.png differ diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/bdwn.png b/Devices/Libraries/Systems/CANopenSocket/docs/bdwn.png new file mode 100755 index 0000000..940a0b9 Binary files /dev/null and b/Devices/Libraries/Systems/CANopenSocket/docs/bdwn.png differ diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/classes.html b/Devices/Libraries/Systems/CANopenSocket/docs/classes.html new file mode 100755 index 0000000..1d1289a --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/classes.html @@ -0,0 +1,175 @@ + + + + + + + +CANopenNode: Data Structure Index + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+
Data Structure Index
+
+
+
c | o
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
  c  
+		CO_fifo_t   CO_NMT_t   CO_SYNC_t   OD_extensionIO_t   
CO_GFC_t   CO_OD_storage_t   CO_t   OD_IO_t   
CO_CANinterfaceErrorhandler_t   CO_GTWA_t   CO_RPDO_t   CO_TIME_t   OD_obj_array_t   
CO_CANmodule_t   CO_HBconsNode_t   CO_RPDOCommPar_t   CO_TPDO_t   OD_obj_extended_t   
CO_CANrx_t   CO_HBconsumer_t   CO_RPDOMapPar_t   CO_TPDOCommPar_t   OD_obj_record_t   
CO_CANtx_t   CO_LEDs_t   CO_SDOclient_t   CO_TPDOMapPar_t   OD_obj_var_t   
CO_config_t   CO_LSS_address_t   CO_SDOserver_t   CO_trace_dataType_t   OD_stream_t   
CO_EM_t   CO_LSSmaster_fastscan_t   CO_SRDO_t   CO_trace_t   OD_subEntry_t   
CO_epoll_gtw_t   CO_LSSmaster_t   CO_SRDOCommPar_t   
  o  
+		OD_t   
CO_epoll_t   CO_LSSslave_t   CO_SRDOGuard_t   
OD_entry_t   
+
c | o
+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/closed.png b/Devices/Libraries/Systems/CANopenSocket/docs/closed.png new file mode 100755 index 0000000..98cc2c9 Binary files /dev/null and b/Devices/Libraries/Systems/CANopenSocket/docs/closed.png differ diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/crc16-ccitt_8h.html b/Devices/Libraries/Systems/CANopenSocket/docs/crc16-ccitt_8h.html new file mode 100755 index 0000000..f411215 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/crc16-ccitt_8h.html @@ -0,0 +1,137 @@ + + + + + + + +CANopenNode: 301/crc16-ccitt.h File Reference + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Functions
+
+
crc16-ccitt.h File Reference
CANopen_301 » CRC 16 CCITT
+
+
+ +

Calculation of CRC 16 CCITT polynomial. +More...

+
#include "301/CO_driver.h"
+
+

Go to the source code of this file.

+ + + + + + + + +

+Functions

void crc16_ccitt_single (uint16_t *crc, const uint8_t chr)
 Update crc16_ccitt variable with one data byte. More...
 
uint16_t crc16_ccitt (const uint8_t block[], size_t blockLength, uint16_t crc)
 Calculate CRC sum on block of data. More...
 
+

Detailed Description

+

Calculation of CRC 16 CCITT polynomial.

+
Author
Lammert Bies
+
+Janez Paternoster
+
Copyright
2012 - 2020 Janez Paternoster
+

This file is part of CANopenNode, an opensource CANopen Stack. Project home page is https://github.com/CANopenNode/CANopenNode. For more information on CANopen see http://www.can-cia.org/.

+

Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. You may obtain a copy of the License at 

http://www.apache.org/licenses/LICENSE-2.0
+

Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License.

+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/crc16-ccitt_8h.js b/Devices/Libraries/Systems/CANopenSocket/docs/crc16-ccitt_8h.js new file mode 100755 index 0000000..7a8109f --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/crc16-ccitt_8h.js @@ -0,0 +1,5 @@ +var crc16_ccitt_8h = +[ + [ "crc16_ccitt_single", "group__CO__crc16__ccitt.html#gad0ef7bb8f47a7eb3ff71d603beba7f92", null ], + [ "crc16_ccitt", "group__CO__crc16__ccitt.html#gab03185ec096eb8792b52d657ed6cbea1", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/crc16-ccitt_8h_source.html b/Devices/Libraries/Systems/CANopenSocket/docs/crc16-ccitt_8h_source.html new file mode 100755 index 0000000..96b19d7 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/crc16-ccitt_8h_source.html @@ -0,0 +1,148 @@ + + + + + + + +CANopenNode: 301/crc16-ccitt.h Source File + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+
crc16-ccitt.h
+
+
+Go to the documentation of this file.
1 
+
27 #ifndef CRC16_CCITT_H
+
28 #define CRC16_CCITT_H
+
29 
+
30 #include "301/CO_driver.h"
+
31 
+
32 /* default configuration, see CO_config.h */
+
33 #ifndef CO_CONFIG_CRC16
+
34 #define CO_CONFIG_CRC16 (0)
+
35 #endif
+
36 
+
37 #if ((CO_CONFIG_CRC16) & CO_CONFIG_CRC16_ENABLE) || defined CO_DOXYGEN
+
38 
+
39 #ifdef __cplusplus
+
40 extern "C" {
+
41 #endif
+
42 
+
66 void crc16_ccitt_single(uint16_t *crc, const uint8_t chr);
+
67 
+
68 
+
79 uint16_t crc16_ccitt(const uint8_t block[],
+
80  size_t blockLength,
+
81  uint16_t crc);
+
82 
+
83  /* CO_crc16_ccitt */
+
85 
+
86 #ifdef __cplusplus
+
87 }
+
88 #endif /*__cplusplus*/
+
89 
+
90 #endif /* (CO_CONFIG_CRC16) & CO_CONFIG_CRC16_ENABLE */
+
91 
+
92 #endif /* CRC16_CCITT_H */
+
+
+
CO_driver.h
Interface between CAN hardware and CANopenNode.
+
crc16_ccitt
uint16_t crc16_ccitt(const uint8_t block[], size_t blockLength, uint16_t crc)
Calculate CRC sum on block of data.
+
uint16_t
unsigned int uint16_t
UNSIGNED16 in CANopen (0006h), 16-bit unsigned integer.
Definition: CO_driver.h:153
+
crc16_ccitt_single
void crc16_ccitt_single(uint16_t *crc, const uint8_t chr)
Update crc16_ccitt variable with one data byte.
+
uint8_t
unsigned char uint8_t
UNSIGNED8 in CANopen (0005h), 8-bit unsigned integer.
Definition: CO_driver.h:151
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/dir_4b0ffcb7ab18f30ccfa85879d0b1134f.html b/Devices/Libraries/Systems/CANopenSocket/docs/dir_4b0ffcb7ab18f30ccfa85879d0b1134f.html new file mode 100755 index 0000000..c6dee5c --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/dir_4b0ffcb7ab18f30ccfa85879d0b1134f.html @@ -0,0 +1,126 @@ + + + + + + + +CANopenNode: socketCAN Directory Reference + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+
socketCAN Directory Reference
+
+
+ + + + + + + + + + + + + + +

+Files

file  CO_driver_target.h [code]
 Linux socketCAN specific definitions for CANopenNode.
 
file  CO_epoll_interface.h [code]
 Helper functions for Linux epoll interface to CANopenNode.
 
file  CO_error.h [code]
 CANopenNode Linux socketCAN Error handling.
 
file  CO_OD_storage.h [code]
 CANopen Object Dictionary storage object for Linux SocketCAN.
 
+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/dir_4b0ffcb7ab18f30ccfa85879d0b1134f.js b/Devices/Libraries/Systems/CANopenSocket/docs/dir_4b0ffcb7ab18f30ccfa85879d0b1134f.js new file mode 100755 index 0000000..2af6bb3 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/dir_4b0ffcb7ab18f30ccfa85879d0b1134f.js @@ -0,0 +1,8 @@ +var dir_4b0ffcb7ab18f30ccfa85879d0b1134f = +[ + [ "CO_driver_target.h", "CO__driver__target_8h.html", "CO__driver__target_8h" ], + [ "CO_epoll_interface.h", "CO__epoll__interface_8h.html", "CO__epoll__interface_8h" ], + [ "CO_error.h", "CO__error_8h.html", "CO__error_8h" ], + [ "CO_error_msgs.h", "CO__error__msgs_8h_source.html", null ], + [ "CO_OD_storage.h", "CO__OD__storage_8h.html", "CO__OD__storage_8h" ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/dir_61725470c0c4ad532abb3cb1e156c123.html b/Devices/Libraries/Systems/CANopenSocket/docs/dir_61725470c0c4ad532abb3cb1e156c123.html new file mode 100755 index 0000000..cb23089 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/dir_61725470c0c4ad532abb3cb1e156c123.html @@ -0,0 +1,117 @@ + + + + + + + +CANopenNode: extra Directory Reference + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+
extra Directory Reference
+
+
+ + + + + +

+Files

file  CO_trace.h [code]
 CANopen trace object for recording variables over time.
 
+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/dir_61725470c0c4ad532abb3cb1e156c123.js b/Devices/Libraries/Systems/CANopenSocket/docs/dir_61725470c0c4ad532abb3cb1e156c123.js new file mode 100755 index 0000000..8d83518 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/dir_61725470c0c4ad532abb3cb1e156c123.js @@ -0,0 +1,4 @@ +var dir_61725470c0c4ad532abb3cb1e156c123 = +[ + [ "CO_trace.h", "CO__trace_8h.html", "CO__trace_8h" ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/dir_6b9d3acb0ed2bca35694b645384363d3.html b/Devices/Libraries/Systems/CANopenSocket/docs/dir_6b9d3acb0ed2bca35694b645384363d3.html new file mode 100755 index 0000000..78a7b6d --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/dir_6b9d3acb0ed2bca35694b645384363d3.html @@ -0,0 +1,117 @@ + + + + + + + +CANopenNode: 303 Directory Reference + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+
303 Directory Reference
+
+
+ + + + + +

+Files

file  CO_LEDs.h [code]
 CANopen Indicator specification (CiA 303-3 v1.4.0)
 
+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/dir_6b9d3acb0ed2bca35694b645384363d3.js b/Devices/Libraries/Systems/CANopenSocket/docs/dir_6b9d3acb0ed2bca35694b645384363d3.js new file mode 100755 index 0000000..1292f00 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/dir_6b9d3acb0ed2bca35694b645384363d3.js @@ -0,0 +1,4 @@ +var dir_6b9d3acb0ed2bca35694b645384363d3 = +[ + [ "CO_LEDs.h", "CO__LEDs_8h.html", "CO__LEDs_8h" ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/dir_77924ae5e158f0fe5749d81e75dc818c.html b/Devices/Libraries/Systems/CANopenSocket/docs/dir_77924ae5e158f0fe5749d81e75dc818c.html new file mode 100755 index 0000000..dec3dda --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/dir_77924ae5e158f0fe5749d81e75dc818c.html @@ -0,0 +1,117 @@ + + + + + + + +CANopenNode: 309 Directory Reference + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+
309 Directory Reference
+
+
+ + + + + +

+Files

file  CO_gateway_ascii.h [code]
 CANopen access from other networks - ASCII mapping (CiA 309-3 DS v3.0.0)
 
+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/dir_77924ae5e158f0fe5749d81e75dc818c.js b/Devices/Libraries/Systems/CANopenSocket/docs/dir_77924ae5e158f0fe5749d81e75dc818c.js new file mode 100755 index 0000000..2b6541c --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/dir_77924ae5e158f0fe5749d81e75dc818c.js @@ -0,0 +1,4 @@ +var dir_77924ae5e158f0fe5749d81e75dc818c = +[ + [ "CO_gateway_ascii.h", "CO__gateway__ascii_8h.html", "CO__gateway__ascii_8h" ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/dir_9d7b210e307d5785c6c2b238c23a336b.html b/Devices/Libraries/Systems/CANopenSocket/docs/dir_9d7b210e307d5785c6c2b238c23a336b.html new file mode 100755 index 0000000..41794ef --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/dir_9d7b210e307d5785c6c2b238c23a336b.html @@ -0,0 +1,123 @@ + + + + + + + +CANopenNode: 305 Directory Reference + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+
305 Directory Reference
+
+
+ + + + + + + + + + + +

+Files

file  CO_LSS.h [code]
 CANopen Layer Setting Services protocol (common).
 
file  CO_LSSmaster.h [code]
 CANopen Layer Setting Service - master protocol.
 
file  CO_LSSslave.h [code]
 CANopen Layer Setting Service - slave protocol.
 
+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/dir_9d7b210e307d5785c6c2b238c23a336b.js b/Devices/Libraries/Systems/CANopenSocket/docs/dir_9d7b210e307d5785c6c2b238c23a336b.js new file mode 100755 index 0000000..cd4e43d --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/dir_9d7b210e307d5785c6c2b238c23a336b.js @@ -0,0 +1,6 @@ +var dir_9d7b210e307d5785c6c2b238c23a336b = +[ + [ "CO_LSS.h", "CO__LSS_8h.html", "CO__LSS_8h" ], + [ "CO_LSSmaster.h", "CO__LSSmaster_8h.html", "CO__LSSmaster_8h" ], + [ "CO_LSSslave.h", "CO__LSSslave_8h.html", "CO__LSSslave_8h" ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/dir_cfafba98a580ce4b62f8a6fa96d7cbb0.html b/Devices/Libraries/Systems/CANopenSocket/docs/dir_cfafba98a580ce4b62f8a6fa96d7cbb0.html new file mode 100755 index 0000000..b4dbe01 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/dir_cfafba98a580ce4b62f8a6fa96d7cbb0.html @@ -0,0 +1,110 @@ + + + + + + + +CANopenNode: example Directory Reference + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+
example Directory Reference
+
+
+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/dir_dedeb03de5db43ead5483ab2f59b049d.html b/Devices/Libraries/Systems/CANopenSocket/docs/dir_dedeb03de5db43ead5483ab2f59b049d.html new file mode 100755 index 0000000..e3250e2 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/dir_dedeb03de5db43ead5483ab2f59b049d.html @@ -0,0 +1,126 @@ + + + + + + + +CANopenNode: 304 Directory Reference + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+
304 Directory Reference
+
+
+ + + + + + + + + + + + + + +

+Files

file  CO_GFC.c
 CANopen Global fail-safe command protocol.
 
file  CO_GFC.h [code]
 CANopen Global fail-safe command protocol.
 
file  CO_SRDO.c
 CANopen Safety Related Data Object protocol.
 
file  CO_SRDO.h [code]
 CANopen Safety Related Data Object protocol.
 
+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/dir_dedeb03de5db43ead5483ab2f59b049d.js b/Devices/Libraries/Systems/CANopenSocket/docs/dir_dedeb03de5db43ead5483ab2f59b049d.js new file mode 100755 index 0000000..752f072 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/dir_dedeb03de5db43ead5483ab2f59b049d.js @@ -0,0 +1,7 @@ +var dir_dedeb03de5db43ead5483ab2f59b049d = +[ + [ "CO_GFC.c", "CO__GFC_8c.html", null ], + [ "CO_GFC.h", "CO__GFC_8h.html", "CO__GFC_8h" ], + [ "CO_SRDO.c", "CO__SRDO_8c.html", null ], + [ "CO_SRDO.h", "CO__SRDO_8h.html", "CO__SRDO_8h" ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/dir_e68e8157741866f444e17edd764ebbae.html b/Devices/Libraries/Systems/CANopenSocket/docs/dir_e68e8157741866f444e17edd764ebbae.html new file mode 100755 index 0000000..7b7b5fb --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/dir_e68e8157741866f444e17edd764ebbae.html @@ -0,0 +1,110 @@ + + + + + + + +CANopenNode: doc Directory Reference + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+
doc Directory Reference
+
+
+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/dir_f17b92000dd778003adbbcb6b5ce4226.html b/Devices/Libraries/Systems/CANopenSocket/docs/dir_f17b92000dd778003adbbcb6b5ce4226.html new file mode 100755 index 0000000..762550c --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/dir_f17b92000dd778003adbbcb6b5ce4226.html @@ -0,0 +1,153 @@ + + + + + + + +CANopenNode: 301 Directory Reference + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+
301 Directory Reference
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

+Files

file  CO_config.h [code]
 Configuration macros for CANopenNode.
 
file  CO_driver.h [code]
 Interface between CAN hardware and CANopenNode.
 
file  CO_Emergency.h [code]
 CANopen Emergency protocol.
 
file  CO_fifo.h [code]
 FIFO circular buffer.
 
file  CO_HBconsumer.h [code]
 CANopen Heartbeat consumer protocol.
 
file  CO_NMT_Heartbeat.h [code]
 CANopen Network management and Heartbeat producer protocol.
 
file  CO_ODinterface.h [code]
 CANopen Object Dictionary interface.
 
file  CO_PDO.h [code]
 CANopen Process Data Object protocol.
 
file  CO_SDOclient.h [code]
 CANopen Service Data Object - client protocol.
 
file  CO_SDOserver.h [code]
 CANopen Service Data Object - server protocol.
 
file  CO_SYNC.h [code]
 CANopen Synchronisation protocol.
 
file  CO_TIME.h [code]
 CANopen Time-stamp protocol.
 
file  crc16-ccitt.h [code]
 Calculation of CRC 16 CCITT polynomial.
 
+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/dir_f17b92000dd778003adbbcb6b5ce4226.js b/Devices/Libraries/Systems/CANopenSocket/docs/dir_f17b92000dd778003adbbcb6b5ce4226.js new file mode 100755 index 0000000..090b66f --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/dir_f17b92000dd778003adbbcb6b5ce4226.js @@ -0,0 +1,16 @@ +var dir_f17b92000dd778003adbbcb6b5ce4226 = +[ + [ "CO_config.h", "CO__config_8h.html", "CO__config_8h" ], + [ "CO_driver.h", "CO__driver_8h.html", "CO__driver_8h" ], + [ "CO_Emergency.h", "CO__Emergency_8h.html", "CO__Emergency_8h" ], + [ "CO_fifo.h", "CO__fifo_8h.html", "CO__fifo_8h" ], + [ "CO_HBconsumer.h", "CO__HBconsumer_8h.html", "CO__HBconsumer_8h" ], + [ "CO_NMT_Heartbeat.h", "CO__NMT__Heartbeat_8h.html", "CO__NMT__Heartbeat_8h" ], + [ "CO_ODinterface.h", "CO__ODinterface_8h.html", "CO__ODinterface_8h" ], + [ "CO_PDO.h", "CO__PDO_8h.html", "CO__PDO_8h" ], + [ "CO_SDOclient.h", "CO__SDOclient_8h.html", "CO__SDOclient_8h" ], + [ "CO_SDOserver.h", "CO__SDOserver_8h.html", "CO__SDOserver_8h" ], + [ "CO_SYNC.h", "CO__SYNC_8h.html", "CO__SYNC_8h" ], + [ "CO_TIME.h", "CO__TIME_8h.html", "CO__TIME_8h" ], + [ "crc16-ccitt.h", "crc16-ccitt_8h.html", "crc16-ccitt_8h" ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/doc.png b/Devices/Libraries/Systems/CANopenSocket/docs/doc.png new file mode 100755 index 0000000..17edabf Binary files /dev/null and b/Devices/Libraries/Systems/CANopenSocket/docs/doc.png differ diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/doxygen.css b/Devices/Libraries/Systems/CANopenSocket/docs/doxygen.css new file mode 100755 index 0000000..73ecbb2 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/doxygen.css @@ -0,0 +1,1771 @@ +/* The standard CSS for doxygen 1.8.17 */ + +body, table, div, p, dl { + font: 400 14px/22px Roboto,sans-serif; +} + +p.reference, p.definition { + font: 400 14px/22px Roboto,sans-serif; +} + +/* @group Heading Levels */ + +h1.groupheader { + font-size: 150%; +} + +.title { + font: 400 14px/28px Roboto,sans-serif; + font-size: 150%; + font-weight: bold; + margin: 10px 2px; +} + +h2.groupheader { + border-bottom: 1px solid #879ECB; + color: #354C7B; + font-size: 150%; + font-weight: normal; + margin-top: 1.75em; + padding-top: 8px; + padding-bottom: 4px; + width: 100%; +} + +h3.groupheader { + font-size: 100%; +} + +h1, h2, h3, h4, h5, h6 { + -webkit-transition: text-shadow 0.5s linear; + -moz-transition: text-shadow 0.5s linear; + -ms-transition: text-shadow 0.5s linear; + -o-transition: text-shadow 0.5s linear; + transition: text-shadow 0.5s linear; + margin-right: 15px; +} + +h1.glow, h2.glow, h3.glow, h4.glow, h5.glow, h6.glow { + text-shadow: 0 0 15px cyan; +} + +dt { + font-weight: bold; +} + +ul.multicol { + -moz-column-gap: 1em; + -webkit-column-gap: 1em; + column-gap: 1em; + -moz-column-count: 3; + -webkit-column-count: 3; + column-count: 3; +} + +p.startli, p.startdd { + margin-top: 2px; +} + +th p.starttd, p.intertd, p.endtd { + font-size: 100%; + font-weight: 700; +} + +p.starttd { + margin-top: 0px; +} + +p.endli { + margin-bottom: 0px; +} + +p.enddd { + margin-bottom: 4px; +} + +p.endtd { + margin-bottom: 2px; +} + +p.interli { +} + +p.interdd { +} + +p.intertd { +} + +/* @end */ + +caption { + font-weight: bold; +} + +span.legend { + font-size: 70%; + text-align: center; +} + +h3.version { + font-size: 90%; + text-align: center; +} + +div.qindex, div.navtab{ + background-color: #EBEFF6; + border: 1px solid #A3B4D7; + text-align: center; +} + +div.qindex, div.navpath { + width: 100%; + line-height: 140%; +} + +div.navtab { + margin-right: 15px; +} + +/* @group Link Styling */ + +a { + color: #3D578C; + font-weight: normal; + text-decoration: none; +} + +.contents a:visited { + color: #4665A2; +} + +a:hover { + text-decoration: underline; +} + +a.qindex { + font-weight: bold; +} + +a.qindexHL { + font-weight: bold; + background-color: #9CAFD4; + color: #FFFFFF; + border: 1px double #869DCA; +} + +.contents a.qindexHL:visited { + color: #FFFFFF; +} + +a.el { + font-weight: bold; +} + +a.elRef { +} + +a.code, a.code:visited, a.line, a.line:visited { + color: #4665A2; +} + +a.codeRef, a.codeRef:visited, a.lineRef, a.lineRef:visited { + color: #4665A2; +} + +/* @end */ + +dl.el { + margin-left: -1cm; +} + +ul { + overflow: hidden; /*Fixed: list item bullets overlap floating elements*/ +} + +#side-nav ul { + overflow: visible; /* reset ul rule for scroll bar in GENERATE_TREEVIEW window */ +} + +#main-nav ul { + overflow: visible; /* reset ul rule for the navigation bar drop down lists */ +} + +.fragment { + text-align: left; + direction: ltr; + overflow-x: auto; /*Fixed: fragment lines overlap floating elements*/ + overflow-y: hidden; +} + +pre.fragment { + border: 1px solid #C4CFE5; + background-color: #FBFCFD; + padding: 4px 6px; + margin: 4px 8px 4px 2px; + overflow: auto; + word-wrap: break-word; + font-size: 9pt; + line-height: 125%; + font-family: monospace, fixed; + font-size: 105%; +} + +div.fragment { + padding: 0 0 1px 0; /*Fixed: last line underline overlap border*/ + margin: 4px 8px 4px 2px; + background-color: #FBFCFD; + border: 1px solid #C4CFE5; +} + +div.line { + font-family: monospace, fixed; + font-size: 13px; + min-height: 13px; + line-height: 1.0; + text-wrap: unrestricted; + white-space: -moz-pre-wrap; /* Moz */ + white-space: -pre-wrap; /* Opera 4-6 */ + white-space: -o-pre-wrap; /* Opera 7 */ + white-space: pre-wrap; /* CSS3 */ + word-wrap: break-word; /* IE 5.5+ */ + text-indent: -53px; + padding-left: 53px; + padding-bottom: 0px; + margin: 0px; + -webkit-transition-property: background-color, box-shadow; + -webkit-transition-duration: 0.5s; + -moz-transition-property: background-color, box-shadow; + -moz-transition-duration: 0.5s; + -ms-transition-property: background-color, box-shadow; + -ms-transition-duration: 0.5s; + -o-transition-property: background-color, box-shadow; + -o-transition-duration: 0.5s; + transition-property: background-color, box-shadow; + transition-duration: 0.5s; +} + +div.line:after { + content:"\000A"; + white-space: pre; +} + +div.line.glow { + background-color: cyan; + box-shadow: 0 0 10px cyan; +} + + +span.lineno { + padding-right: 4px; + text-align: right; + border-right: 2px solid #0F0; + background-color: #E8E8E8; + white-space: pre; +} +span.lineno a { + background-color: #D8D8D8; +} + +span.lineno a:hover { + background-color: #C8C8C8; +} + +.lineno { + -webkit-touch-callout: none; + -webkit-user-select: none; + -khtml-user-select: none; + -moz-user-select: none; + -ms-user-select: none; + user-select: none; +} + +div.ah, span.ah { + background-color: black; + font-weight: bold; + color: #FFFFFF; + margin-bottom: 3px; + margin-top: 3px; + padding: 0.2em; + border: solid thin #333; + border-radius: 0.5em; + -webkit-border-radius: .5em; + -moz-border-radius: .5em; + box-shadow: 2px 2px 3px #999; + -webkit-box-shadow: 2px 2px 3px #999; + -moz-box-shadow: rgba(0, 0, 0, 0.15) 2px 2px 2px; + background-image: -webkit-gradient(linear, left top, left bottom, from(#eee), to(#000),color-stop(0.3, #444)); + background-image: -moz-linear-gradient(center top, #eee 0%, #444 40%, #000 110%); +} + +div.classindex ul { + list-style: none; + padding-left: 0; +} + +div.classindex span.ai { + display: inline-block; +} + +div.groupHeader { + margin-left: 16px; + margin-top: 12px; + font-weight: bold; +} + +div.groupText { + margin-left: 16px; + font-style: italic; +} + +body { + background-color: white; + color: black; + margin: 0; +} + +div.contents { + margin-top: 10px; + margin-left: 12px; + margin-right: 8px; +} + +td.indexkey { + background-color: #EBEFF6; + font-weight: bold; + border: 1px solid #C4CFE5; + margin: 2px 0px 2px 0; + padding: 2px 10px; + white-space: nowrap; + vertical-align: top; +} + +td.indexvalue { + background-color: #EBEFF6; + border: 1px solid #C4CFE5; + padding: 2px 10px; + margin: 2px 0px; +} + +tr.memlist { + background-color: #EEF1F7; +} + +p.formulaDsp { + text-align: center; +} + +img.formulaDsp { + +} + +img.formulaInl, img.inline { + vertical-align: middle; +} + +div.center { + text-align: center; + margin-top: 0px; + margin-bottom: 0px; + padding: 0px; +} + +div.center img { + border: 0px; +} + +address.footer { + text-align: right; + padding-right: 12px; +} + +img.footer { + border: 0px; + vertical-align: middle; +} + +/* @group Code Colorization */ + +span.keyword { + color: #008000 +} + +span.keywordtype { + color: #604020 +} + +span.keywordflow { + color: #e08000 +} + +span.comment { + color: #800000 +} + +span.preprocessor { + color: #806020 +} + +span.stringliteral { + color: #002080 +} + +span.charliteral { + color: #008080 +} + +span.vhdldigit { + color: #ff00ff +} + +span.vhdlchar { + color: #000000 +} + +span.vhdlkeyword { + color: #700070 +} + +span.vhdllogic { + color: #ff0000 +} + +blockquote { + background-color: #F7F8FB; + border-left: 2px solid #9CAFD4; + margin: 0 24px 0 4px; + padding: 0 12px 0 16px; +} + +blockquote.DocNodeRTL { + border-left: 0; + border-right: 2px solid #9CAFD4; + margin: 0 4px 0 24px; + padding: 0 16px 0 12px; +} + +/* @end */ + +/* +.search { + color: #003399; + font-weight: bold; +} + +form.search { + margin-bottom: 0px; + margin-top: 0px; +} + +input.search { + font-size: 75%; + color: #000080; + font-weight: normal; + background-color: #e8eef2; +} +*/ + +td.tiny { + font-size: 75%; +} + +.dirtab { + padding: 4px; + border-collapse: collapse; + border: 1px solid #A3B4D7; +} + +th.dirtab { + background: #EBEFF6; + font-weight: bold; +} + +hr { + height: 0px; + border: none; + border-top: 1px solid #4A6AAA; +} + +hr.footer { + height: 1px; +} + +/* @group Member Descriptions */ + +table.memberdecls { + border-spacing: 0px; + padding: 0px; +} + +.memberdecls td, .fieldtable tr { + -webkit-transition-property: background-color, box-shadow; + -webkit-transition-duration: 0.5s; + -moz-transition-property: background-color, box-shadow; + -moz-transition-duration: 0.5s; + -ms-transition-property: background-color, box-shadow; + -ms-transition-duration: 0.5s; + -o-transition-property: background-color, box-shadow; + -o-transition-duration: 0.5s; + transition-property: background-color, box-shadow; + transition-duration: 0.5s; +} + +.memberdecls td.glow, .fieldtable tr.glow { + background-color: cyan; + box-shadow: 0 0 15px cyan; +} + +.mdescLeft, .mdescRight, +.memItemLeft, .memItemRight, +.memTemplItemLeft, .memTemplItemRight, .memTemplParams { + background-color: #F9FAFC; + border: none; + margin: 4px; + padding: 1px 0 0 8px; +} + +.mdescLeft, .mdescRight { + padding: 0px 8px 4px 8px; + color: #555; +} + +.memSeparator { + border-bottom: 1px solid #DEE4F0; + line-height: 1px; + margin: 0px; + padding: 0px; +} + +.memItemLeft, .memTemplItemLeft { + white-space: nowrap; +} + +.memItemRight, .memTemplItemRight { + width: 100%; +} + +.memTemplParams { + color: #4665A2; + white-space: nowrap; + font-size: 80%; +} + +/* @end */ + +/* @group Member Details */ + +/* Styles for detailed member documentation */ + +.memtitle { + padding: 8px; + border-top: 1px solid #A8B8D9; + border-left: 1px solid #A8B8D9; + border-right: 1px solid #A8B8D9; + border-top-right-radius: 4px; + border-top-left-radius: 4px; + margin-bottom: -1px; + background-image: url('nav_f.png'); + background-repeat: repeat-x; + background-color: #E2E8F2; + line-height: 1.25; + font-weight: 300; + float:left; +} + +.permalink +{ + font-size: 65%; + display: inline-block; + vertical-align: middle; +} + +.memtemplate { + font-size: 80%; + color: #4665A2; + font-weight: normal; + margin-left: 9px; +} + +.memnav { + background-color: #EBEFF6; + border: 1px solid #A3B4D7; + text-align: center; + margin: 2px; + margin-right: 15px; + padding: 2px; +} + +.mempage { + width: 100%; +} + +.memitem { + padding: 0; + margin-bottom: 10px; + margin-right: 5px; + -webkit-transition: box-shadow 0.5s linear; + -moz-transition: box-shadow 0.5s linear; + -ms-transition: box-shadow 0.5s linear; + -o-transition: box-shadow 0.5s linear; + transition: box-shadow 0.5s linear; + display: table !important; + width: 100%; +} + +.memitem.glow { + box-shadow: 0 0 15px cyan; +} + +.memname { + font-weight: 400; + margin-left: 6px; +} + +.memname td { + vertical-align: bottom; +} + +.memproto, dl.reflist dt { + border-top: 1px solid #A8B8D9; + border-left: 1px solid #A8B8D9; + border-right: 1px solid #A8B8D9; + padding: 6px 0px 6px 0px; + color: #253555; + font-weight: bold; + text-shadow: 0px 1px 1px rgba(255, 255, 255, 0.9); + background-color: #DFE5F1; + /* opera specific markup */ + box-shadow: 5px 5px 5px rgba(0, 0, 0, 0.15); + border-top-right-radius: 4px; + /* firefox specific markup */ + -moz-box-shadow: rgba(0, 0, 0, 0.15) 5px 5px 5px; + -moz-border-radius-topright: 4px; + /* webkit specific markup */ + -webkit-box-shadow: 5px 5px 5px rgba(0, 0, 0, 0.15); + -webkit-border-top-right-radius: 4px; + +} + +.overload { + font-family: "courier new",courier,monospace; + font-size: 65%; +} + +.memdoc, dl.reflist dd { + border-bottom: 1px solid #A8B8D9; + border-left: 1px solid #A8B8D9; + border-right: 1px solid #A8B8D9; + padding: 6px 10px 2px 10px; + background-color: #FBFCFD; + border-top-width: 0; + background-image:url('nav_g.png'); + background-repeat:repeat-x; + background-color: #FFFFFF; + /* opera specific markup */ + border-bottom-left-radius: 4px; + border-bottom-right-radius: 4px; + box-shadow: 5px 5px 5px rgba(0, 0, 0, 0.15); + /* firefox specific markup */ + -moz-border-radius-bottomleft: 4px; + -moz-border-radius-bottomright: 4px; + -moz-box-shadow: rgba(0, 0, 0, 0.15) 5px 5px 5px; + /* webkit specific markup */ + -webkit-border-bottom-left-radius: 4px; + -webkit-border-bottom-right-radius: 4px; + -webkit-box-shadow: 5px 5px 5px rgba(0, 0, 0, 0.15); +} + +dl.reflist dt { + padding: 5px; +} + +dl.reflist dd { + margin: 0px 0px 10px 0px; + padding: 5px; +} + +.paramkey { + text-align: right; +} + +.paramtype { + white-space: nowrap; +} + +.paramname { + color: #602020; + white-space: nowrap; +} +.paramname em { + font-style: normal; +} +.paramname code { + line-height: 14px; +} + +.params, .retval, .exception, .tparams { + margin-left: 0px; + padding-left: 0px; +} + +.params .paramname, .retval .paramname, .tparams .paramname, .exception .paramname { + font-weight: bold; + vertical-align: top; +} + +.params .paramtype, .tparams .paramtype { + font-style: italic; + vertical-align: top; +} + +.params .paramdir, .tparams .paramdir { + font-family: "courier new",courier,monospace; + vertical-align: top; +} + +table.mlabels { + border-spacing: 0px; +} + +td.mlabels-left { + width: 100%; + padding: 0px; +} + +td.mlabels-right { + vertical-align: bottom; + padding: 0px; + white-space: nowrap; +} + +span.mlabels { + margin-left: 8px; +} + +span.mlabel { + background-color: #728DC1; + border-top:1px solid #5373B4; + border-left:1px solid #5373B4; + border-right:1px solid #C4CFE5; + border-bottom:1px solid #C4CFE5; + text-shadow: none; + color: white; + margin-right: 4px; + padding: 2px 3px; + border-radius: 3px; + font-size: 7pt; + white-space: nowrap; + vertical-align: middle; +} + + + +/* @end */ + +/* these are for tree view inside a (index) page */ + +div.directory { + margin: 10px 0px; + border-top: 1px solid #9CAFD4; + border-bottom: 1px solid #9CAFD4; + width: 100%; +} + +.directory table { + border-collapse:collapse; +} + +.directory td { + margin: 0px; + padding: 0px; + vertical-align: top; +} + +.directory td.entry { + white-space: nowrap; + padding-right: 6px; + padding-top: 3px; +} + +.directory td.entry a { + outline:none; +} + +.directory td.entry a img { + border: none; +} + +.directory td.desc { + width: 100%; + padding-left: 6px; + padding-right: 6px; + padding-top: 3px; + border-left: 1px solid rgba(0,0,0,0.05); +} + +.directory tr.even { + padding-left: 6px; + background-color: #F7F8FB; +} + +.directory img { + vertical-align: -30%; +} + +.directory .levels { + white-space: nowrap; + width: 100%; + text-align: right; + font-size: 9pt; +} + +.directory .levels span { + cursor: pointer; + padding-left: 2px; + padding-right: 2px; + color: #3D578C; +} + +.arrow { + color: #9CAFD4; + -webkit-user-select: none; + -khtml-user-select: none; + -moz-user-select: none; + -ms-user-select: none; + user-select: none; + cursor: pointer; + font-size: 80%; + display: inline-block; + width: 16px; + height: 22px; +} + +.icon { + font-family: Arial, Helvetica; + font-weight: bold; + font-size: 12px; + height: 14px; + width: 16px; + display: inline-block; + background-color: #728DC1; + color: white; + text-align: center; + border-radius: 4px; + margin-left: 2px; + margin-right: 2px; +} + +.icona { + width: 24px; + height: 22px; + display: inline-block; +} + +.iconfopen { + width: 24px; + height: 18px; + margin-bottom: 4px; + background-image:url('folderopen.png'); + background-position: 0px -4px; + background-repeat: repeat-y; + vertical-align:top; + display: inline-block; +} + +.iconfclosed { + width: 24px; + height: 18px; + margin-bottom: 4px; + background-image:url('folderclosed.png'); + background-position: 0px -4px; + background-repeat: repeat-y; + vertical-align:top; + display: inline-block; +} + +.icondoc { + width: 24px; + height: 18px; + margin-bottom: 4px; + background-image:url('doc.png'); + background-position: 0px -4px; + background-repeat: repeat-y; + vertical-align:top; + display: inline-block; +} + +table.directory { + font: 400 14px Roboto,sans-serif; +} + +/* @end */ + +div.dynheader { + margin-top: 8px; + -webkit-touch-callout: none; + -webkit-user-select: none; + -khtml-user-select: none; + -moz-user-select: none; + -ms-user-select: none; + user-select: none; +} + +address { + font-style: normal; + color: #2A3D61; +} + +table.doxtable caption { + caption-side: top; +} + +table.doxtable { + border-collapse:collapse; + margin-top: 4px; + margin-bottom: 4px; +} + +table.doxtable td, table.doxtable th { + border: 1px solid #2D4068; + padding: 3px 7px 2px; +} + +table.doxtable th { + background-color: #374F7F; + color: #FFFFFF; + font-size: 110%; + padding-bottom: 4px; + padding-top: 5px; +} + +table.fieldtable { + /*width: 100%;*/ + margin-bottom: 10px; + border: 1px solid #A8B8D9; + border-spacing: 0px; + -moz-border-radius: 4px; + -webkit-border-radius: 4px; + border-radius: 4px; + -moz-box-shadow: rgba(0, 0, 0, 0.15) 2px 2px 2px; + -webkit-box-shadow: 2px 2px 2px rgba(0, 0, 0, 0.15); + box-shadow: 2px 2px 2px rgba(0, 0, 0, 0.15); +} + +.fieldtable td, .fieldtable th { + padding: 3px 7px 2px; +} + +.fieldtable td.fieldtype, .fieldtable td.fieldname { + white-space: nowrap; + border-right: 1px solid #A8B8D9; + border-bottom: 1px solid #A8B8D9; + vertical-align: top; +} + +.fieldtable td.fieldname { + padding-top: 3px; +} + +.fieldtable td.fielddoc { + border-bottom: 1px solid #A8B8D9; + /*width: 100%;*/ +} + +.fieldtable td.fielddoc p:first-child { + margin-top: 0px; +} + +.fieldtable td.fielddoc p:last-child { + margin-bottom: 2px; +} + +.fieldtable tr:last-child td { + border-bottom: none; +} + +.fieldtable th { + background-image:url('nav_f.png'); + background-repeat:repeat-x; + background-color: #E2E8F2; + font-size: 90%; + color: #253555; + padding-bottom: 4px; + padding-top: 5px; + text-align:left; + font-weight: 400; + -moz-border-radius-topleft: 4px; + -moz-border-radius-topright: 4px; + -webkit-border-top-left-radius: 4px; + -webkit-border-top-right-radius: 4px; + border-top-left-radius: 4px; + border-top-right-radius: 4px; + border-bottom: 1px solid #A8B8D9; +} + + +.tabsearch { + top: 0px; + left: 10px; + height: 36px; + background-image: url('tab_b.png'); + z-index: 101; + overflow: hidden; + font-size: 13px; +} + +.navpath ul +{ + font-size: 11px; + background-image:url('tab_b.png'); + background-repeat:repeat-x; + background-position: 0 -5px; + height:30px; + line-height:30px; + color:#8AA0CC; + border:solid 1px #C2CDE4; + overflow:hidden; + margin:0px; + padding:0px; +} + +.navpath li +{ + list-style-type:none; + float:left; + padding-left:10px; + padding-right:15px; + background-image:url('bc_s.png'); + background-repeat:no-repeat; + background-position:right; + color:#364D7C; +} + +.navpath li.navelem a +{ + height:32px; + display:block; + text-decoration: none; + outline: none; + color: #283A5D; + font-family: 'Lucida Grande',Geneva,Helvetica,Arial,sans-serif; + text-shadow: 0px 1px 1px rgba(255, 255, 255, 0.9); + text-decoration: none; +} + +.navpath li.navelem a:hover +{ + color:#6884BD; +} + +.navpath li.footer +{ + list-style-type:none; + float:right; + padding-left:10px; + padding-right:15px; + background-image:none; + background-repeat:no-repeat; + background-position:right; + color:#364D7C; + font-size: 8pt; +} + + +div.summary +{ + float: right; + font-size: 8pt; + padding-right: 5px; + width: 50%; + text-align: right; +} + +div.summary a +{ + white-space: nowrap; +} + +table.classindex +{ + margin: 10px; + white-space: nowrap; + margin-left: 3%; + margin-right: 3%; + width: 94%; + border: 0; + border-spacing: 0; + padding: 0; +} + +div.ingroups +{ + font-size: 8pt; + width: 50%; + text-align: left; +} + +div.ingroups a +{ + white-space: nowrap; +} + +div.header +{ + background-image:url('nav_h.png'); + background-repeat:repeat-x; + background-color: #F9FAFC; + margin: 0px; + border-bottom: 1px solid #C4CFE5; +} + +div.headertitle +{ + padding: 5px 5px 5px 10px; +} + +.PageDocRTL-title div.headertitle { + text-align: right; + direction: rtl; +} + +dl { + padding: 0 0 0 0; +} + +/* dl.note, dl.warning, dl.attention, dl.pre, dl.post, dl.invariant, dl.deprecated, dl.todo, dl.test, dl.bug, dl.examples */ +dl.section { + margin-left: 0px; + padding-left: 0px; +} + +dl.section.DocNodeRTL { + margin-right: 0px; + padding-right: 0px; +} + +dl.note { + margin-left: -7px; + padding-left: 3px; + border-left: 4px solid; + border-color: #D0C000; +} + +dl.note.DocNodeRTL { + margin-left: 0; + padding-left: 0; + border-left: 0; + margin-right: -7px; + padding-right: 3px; + border-right: 4px solid; + border-color: #D0C000; +} + +dl.warning, dl.attention { + margin-left: -7px; + padding-left: 3px; + border-left: 4px solid; + border-color: #FF0000; +} + +dl.warning.DocNodeRTL, dl.attention.DocNodeRTL { + margin-left: 0; + padding-left: 0; + border-left: 0; + margin-right: -7px; + padding-right: 3px; + border-right: 4px solid; + border-color: #FF0000; +} + +dl.pre, dl.post, dl.invariant { + margin-left: -7px; + padding-left: 3px; + border-left: 4px solid; + border-color: #00D000; +} + +dl.pre.DocNodeRTL, dl.post.DocNodeRTL, dl.invariant.DocNodeRTL { + margin-left: 0; + padding-left: 0; + border-left: 0; + margin-right: -7px; + padding-right: 3px; + border-right: 4px solid; + border-color: #00D000; +} + +dl.deprecated { + margin-left: -7px; + padding-left: 3px; + border-left: 4px solid; + border-color: #505050; +} + +dl.deprecated.DocNodeRTL { + margin-left: 0; + padding-left: 0; + border-left: 0; + margin-right: -7px; + padding-right: 3px; + border-right: 4px solid; + border-color: #505050; +} + +dl.todo { + margin-left: -7px; + padding-left: 3px; + border-left: 4px solid; + border-color: #00C0E0; +} + +dl.todo.DocNodeRTL { + margin-left: 0; + padding-left: 0; + border-left: 0; + margin-right: -7px; + padding-right: 3px; + border-right: 4px solid; + border-color: #00C0E0; +} + +dl.test { + margin-left: -7px; + padding-left: 3px; + border-left: 4px solid; + border-color: #3030E0; +} + +dl.test.DocNodeRTL { + margin-left: 0; + padding-left: 0; + border-left: 0; + margin-right: -7px; + padding-right: 3px; + border-right: 4px solid; + border-color: #3030E0; +} + +dl.bug { + margin-left: -7px; + padding-left: 3px; + border-left: 4px solid; + border-color: #C08050; +} + +dl.bug.DocNodeRTL { + margin-left: 0; + padding-left: 0; + border-left: 0; + margin-right: -7px; + padding-right: 3px; + border-right: 4px solid; + border-color: #C08050; +} + +dl.section dd { + margin-bottom: 6px; +} + + +#projectlogo +{ + text-align: center; + vertical-align: bottom; + border-collapse: separate; +} + +#projectlogo img +{ + border: 0px none; +} + +#projectalign +{ + vertical-align: middle; +} + +#projectname +{ + font: 300% Tahoma, Arial,sans-serif; + margin: 0px; + padding: 2px 0px; +} + +#projectbrief +{ + font: 120% Tahoma, Arial,sans-serif; + margin: 0px; + padding: 0px; +} + +#projectnumber +{ + font: 50% Tahoma, Arial,sans-serif; + margin: 0px; + padding: 0px; +} + +#titlearea +{ + padding: 0px; + margin: 0px; + width: 100%; + border-bottom: 1px solid #5373B4; +} + +.image +{ + text-align: center; +} + +.dotgraph +{ + text-align: center; +} + +.mscgraph +{ + text-align: center; +} + +.plantumlgraph +{ + text-align: center; +} + +.diagraph +{ + text-align: center; +} + +.caption +{ + font-weight: bold; +} + +div.zoom +{ + border: 1px solid #90A5CE; +} + +dl.citelist { + margin-bottom:50px; +} + +dl.citelist dt { + color:#334975; + float:left; + font-weight:bold; + margin-right:10px; + padding:5px; +} + +dl.citelist dd { + margin:2px 0; + padding:5px 0; +} + +div.toc { + padding: 14px 25px; + background-color: #F4F6FA; + border: 1px solid #D8DFEE; + border-radius: 7px 7px 7px 7px; + float: right; + height: auto; + margin: 0 8px 10px 10px; + width: 200px; +} + +.PageDocRTL-title div.toc { + float: left !important; + text-align: right; +} + +div.toc li { + background: url("bdwn.png") no-repeat scroll 0 5px transparent; + font: 10px/1.2 Verdana,DejaVu Sans,Geneva,sans-serif; + margin-top: 5px; + padding-left: 10px; + padding-top: 2px; +} + +.PageDocRTL-title div.toc li { + background-position-x: right !important; + padding-left: 0 !important; + padding-right: 10px; +} + +div.toc h3 { + font: bold 12px/1.2 Arial,FreeSans,sans-serif; + color: #4665A2; + border-bottom: 0 none; + margin: 0; +} + +div.toc ul { + list-style: none outside none; + border: medium none; + padding: 0px; +} + +div.toc li.level1 { + margin-left: 0px; +} + +div.toc li.level2 { + margin-left: 15px; +} + +div.toc li.level3 { + margin-left: 30px; +} + +div.toc li.level4 { + margin-left: 45px; +} + +.PageDocRTL-title div.toc li.level1 { + margin-left: 0 !important; + margin-right: 0; +} + +.PageDocRTL-title div.toc li.level2 { + margin-left: 0 !important; + margin-right: 15px; +} + +.PageDocRTL-title div.toc li.level3 { + margin-left: 0 !important; + margin-right: 30px; +} + +.PageDocRTL-title div.toc li.level4 { + margin-left: 0 !important; + margin-right: 45px; +} + +.inherit_header { + font-weight: bold; + color: gray; + cursor: pointer; + -webkit-touch-callout: none; + -webkit-user-select: none; + -khtml-user-select: none; + -moz-user-select: none; + -ms-user-select: none; + user-select: none; +} + +.inherit_header td { + padding: 6px 0px 2px 5px; +} + +.inherit { + display: none; +} + +tr.heading h2 { + margin-top: 12px; + margin-bottom: 4px; +} + +/* tooltip related style info */ + +.ttc { + position: absolute; + display: none; +} + +#powerTip { + cursor: default; + white-space: nowrap; + background-color: white; + border: 1px solid gray; + border-radius: 4px 4px 4px 4px; + box-shadow: 1px 1px 7px gray; + display: none; + font-size: smaller; + max-width: 80%; + opacity: 0.9; + padding: 1ex 1em 1em; + position: absolute; + z-index: 2147483647; +} + +#powerTip div.ttdoc { + color: grey; + font-style: italic; +} + +#powerTip div.ttname a { + font-weight: bold; +} + +#powerTip div.ttname { + font-weight: bold; +} + +#powerTip div.ttdeci { + color: #006318; +} + +#powerTip div { + margin: 0px; + padding: 0px; + font: 12px/16px Roboto,sans-serif; +} + +#powerTip:before, #powerTip:after { + content: ""; + position: absolute; + margin: 0px; +} + +#powerTip.n:after, #powerTip.n:before, +#powerTip.s:after, #powerTip.s:before, +#powerTip.w:after, #powerTip.w:before, +#powerTip.e:after, #powerTip.e:before, +#powerTip.ne:after, #powerTip.ne:before, +#powerTip.se:after, #powerTip.se:before, +#powerTip.nw:after, #powerTip.nw:before, +#powerTip.sw:after, #powerTip.sw:before { + border: solid transparent; + content: " "; + height: 0; + width: 0; + position: absolute; +} + +#powerTip.n:after, #powerTip.s:after, +#powerTip.w:after, #powerTip.e:after, +#powerTip.nw:after, #powerTip.ne:after, +#powerTip.sw:after, #powerTip.se:after { + border-color: rgba(255, 255, 255, 0); +} + +#powerTip.n:before, #powerTip.s:before, +#powerTip.w:before, #powerTip.e:before, +#powerTip.nw:before, #powerTip.ne:before, +#powerTip.sw:before, #powerTip.se:before { + border-color: rgba(128, 128, 128, 0); +} + +#powerTip.n:after, #powerTip.n:before, +#powerTip.ne:after, #powerTip.ne:before, +#powerTip.nw:after, #powerTip.nw:before { + top: 100%; +} + +#powerTip.n:after, #powerTip.ne:after, #powerTip.nw:after { + border-top-color: #FFFFFF; + border-width: 10px; + margin: 0px -10px; +} +#powerTip.n:before { + border-top-color: #808080; + border-width: 11px; + margin: 0px -11px; +} +#powerTip.n:after, #powerTip.n:before { + left: 50%; +} + +#powerTip.nw:after, #powerTip.nw:before { + right: 14px; +} + +#powerTip.ne:after, #powerTip.ne:before { + left: 14px; +} + +#powerTip.s:after, #powerTip.s:before, +#powerTip.se:after, #powerTip.se:before, +#powerTip.sw:after, #powerTip.sw:before { + bottom: 100%; +} + +#powerTip.s:after, #powerTip.se:after, #powerTip.sw:after { + border-bottom-color: #FFFFFF; + border-width: 10px; + margin: 0px -10px; +} + +#powerTip.s:before, #powerTip.se:before, #powerTip.sw:before { + border-bottom-color: #808080; + border-width: 11px; + margin: 0px -11px; +} + +#powerTip.s:after, #powerTip.s:before { + left: 50%; +} + +#powerTip.sw:after, #powerTip.sw:before { + right: 14px; +} + +#powerTip.se:after, #powerTip.se:before { + left: 14px; +} + +#powerTip.e:after, #powerTip.e:before { + left: 100%; +} +#powerTip.e:after { + border-left-color: #FFFFFF; + border-width: 10px; + top: 50%; + margin-top: -10px; +} +#powerTip.e:before { + border-left-color: #808080; + border-width: 11px; + top: 50%; + margin-top: -11px; +} + +#powerTip.w:after, #powerTip.w:before { + right: 100%; +} +#powerTip.w:after { + border-right-color: #FFFFFF; + border-width: 10px; + top: 50%; + margin-top: -10px; +} +#powerTip.w:before { + border-right-color: #808080; + border-width: 11px; + top: 50%; + margin-top: -11px; +} + +@media print +{ + #top { display: none; } + #side-nav { display: none; } + #nav-path { display: none; } + body { overflow:visible; } + h1, h2, h3, h4, h5, h6 { page-break-after: avoid; } + .summary { display: none; } + .memitem { page-break-inside: avoid; } + #doc-content + { + margin-left:0 !important; + height:auto !important; + width:auto !important; + overflow:inherit; + display:inline; + } +} + +/* @group Markdown */ + +/* +table.markdownTable { + border-collapse:collapse; + margin-top: 4px; + margin-bottom: 4px; +} + +table.markdownTable td, table.markdownTable th { + border: 1px solid #2D4068; + padding: 3px 7px 2px; +} + +table.markdownTableHead tr { +} + +table.markdownTableBodyLeft td, table.markdownTable th { + border: 1px solid #2D4068; + padding: 3px 7px 2px; +} + +th.markdownTableHeadLeft th.markdownTableHeadRight th.markdownTableHeadCenter th.markdownTableHeadNone { + background-color: #374F7F; + color: #FFFFFF; + font-size: 110%; + padding-bottom: 4px; + padding-top: 5px; +} + +th.markdownTableHeadLeft { + text-align: left +} + +th.markdownTableHeadRight { + text-align: right +} + +th.markdownTableHeadCenter { + text-align: center +} +*/ + +table.markdownTable { + border-collapse:collapse; + margin-top: 4px; + margin-bottom: 4px; +} + +table.markdownTable td, table.markdownTable th { + border: 1px solid #2D4068; + padding: 3px 7px 2px; +} + +table.markdownTable tr { +} + +th.markdownTableHeadLeft, th.markdownTableHeadRight, th.markdownTableHeadCenter, th.markdownTableHeadNone { + background-color: #374F7F; + color: #FFFFFF; + font-size: 110%; + padding-bottom: 4px; + padding-top: 5px; +} + +th.markdownTableHeadLeft, td.markdownTableBodyLeft { + text-align: left +} + +th.markdownTableHeadRight, td.markdownTableBodyRight { + text-align: right +} + +th.markdownTableHeadCenter, td.markdownTableBodyCenter { + text-align: center +} + +.DocNodeRTL { + text-align: right; + direction: rtl; +} + +.DocNodeLTR { + text-align: left; + direction: ltr; +} + +table.DocNodeRTL { + width: auto; + margin-right: 0; + margin-left: auto; +} + +table.DocNodeLTR { + width: auto; + margin-right: auto; + margin-left: 0; +} + +tt, code, kbd, samp +{ + display: inline-block; + direction:ltr; +} +/* @end */ + +u { + text-decoration: underline; +} + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/doxygen.png b/Devices/Libraries/Systems/CANopenSocket/docs/doxygen.png new file mode 100755 index 0000000..3ff17d8 Binary files /dev/null and b/Devices/Libraries/Systems/CANopenSocket/docs/doxygen.png differ diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/dynsections.js b/Devices/Libraries/Systems/CANopenSocket/docs/dynsections.js new file mode 100755 index 0000000..ea0a7b3 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/dynsections.js @@ -0,0 +1,120 @@ +/* + @licstart The following is the entire license notice for the + JavaScript code in this file. + + Copyright (C) 1997-2017 by Dimitri van Heesch + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 2 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License along + with this program; if not, write to the Free Software Foundation, Inc., + 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. + + @licend The above is the entire license notice + for the JavaScript code in this file + */ +function toggleVisibility(linkObj) +{ + var base = $(linkObj).attr('id'); + var summary = $('#'+base+'-summary'); + var content = $('#'+base+'-content'); + var trigger = $('#'+base+'-trigger'); + var src=$(trigger).attr('src'); + if (content.is(':visible')===true) { + content.hide(); + summary.show(); + $(linkObj).addClass('closed').removeClass('opened'); + $(trigger).attr('src',src.substring(0,src.length-8)+'closed.png'); + } else { + content.show(); + summary.hide(); + $(linkObj).removeClass('closed').addClass('opened'); + $(trigger).attr('src',src.substring(0,src.length-10)+'open.png'); + } + return false; +} + +function updateStripes() +{ + $('table.directory tr'). + removeClass('even').filter(':visible:even').addClass('even'); +} + +function toggleLevel(level) +{ + $('table.directory tr').each(function() { + var l = this.id.split('_').length-1; + var i = $('#img'+this.id.substring(3)); + var a = $('#arr'+this.id.substring(3)); + if (l + + + + + + +CANopenNode: File List + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+
File List
+
+
+
Here is a list of all documented files with brief descriptions:
+
[detail level 12]
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
 ▼ 301
 CO_config.hConfiguration macros for CANopenNode
 CO_driver.hInterface between CAN hardware and CANopenNode
 CO_Emergency.hCANopen Emergency protocol
 CO_fifo.hFIFO circular buffer
 CO_HBconsumer.hCANopen Heartbeat consumer protocol
 CO_NMT_Heartbeat.hCANopen Network management and Heartbeat producer protocol
 CO_ODinterface.hCANopen Object Dictionary interface
 CO_PDO.hCANopen Process Data Object protocol
 CO_SDOclient.hCANopen Service Data Object - client protocol
 CO_SDOserver.hCANopen Service Data Object - server protocol
 CO_SYNC.hCANopen Synchronisation protocol
 CO_TIME.hCANopen Time-stamp protocol
 crc16-ccitt.hCalculation of CRC 16 CCITT polynomial
 ▼ 303
 CO_LEDs.hCANopen Indicator specification (CiA 303-3 v1.4.0)
 ▼ 304
 CO_GFC.cCANopen Global fail-safe command protocol
 CO_GFC.hCANopen Global fail-safe command protocol
 CO_SRDO.cCANopen Safety Related Data Object protocol
 CO_SRDO.hCANopen Safety Related Data Object protocol
 ▼ 305
 CO_LSS.hCANopen Layer Setting Services protocol (common)
 CO_LSSmaster.hCANopen Layer Setting Service - master protocol
 CO_LSSslave.hCANopen Layer Setting Service - slave protocol
 ▼ 309
 CO_gateway_ascii.hCANopen access from other networks - ASCII mapping (CiA 309-3 DS v3.0.0)
 ▼ extra
 CO_trace.hCANopen trace object for recording variables over time
 ▼ socketCAN
 CO_driver_target.hLinux socketCAN specific definitions for CANopenNode
 CO_epoll_interface.hHelper functions for Linux epoll interface to CANopenNode
 CO_error.hCANopenNode Linux socketCAN Error handling
 CO_error_msgs.h
 CO_OD_storage.hCANopen Object Dictionary storage object for Linux SocketCAN
 CANopen.hMain CANopenNode file
+
+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/files_dup.js b/Devices/Libraries/Systems/CANopenSocket/docs/files_dup.js new file mode 100755 index 0000000..fa46294 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/files_dup.js @@ -0,0 +1,11 @@ +var files_dup = +[ + [ "301", "dir_f17b92000dd778003adbbcb6b5ce4226.html", "dir_f17b92000dd778003adbbcb6b5ce4226" ], + [ "303", "dir_6b9d3acb0ed2bca35694b645384363d3.html", "dir_6b9d3acb0ed2bca35694b645384363d3" ], + [ "304", "dir_dedeb03de5db43ead5483ab2f59b049d.html", "dir_dedeb03de5db43ead5483ab2f59b049d" ], + [ "305", "dir_9d7b210e307d5785c6c2b238c23a336b.html", "dir_9d7b210e307d5785c6c2b238c23a336b" ], + [ "309", "dir_77924ae5e158f0fe5749d81e75dc818c.html", "dir_77924ae5e158f0fe5749d81e75dc818c" ], + [ "extra", "dir_61725470c0c4ad532abb3cb1e156c123.html", "dir_61725470c0c4ad532abb3cb1e156c123" ], + [ "socketCAN", "dir_4b0ffcb7ab18f30ccfa85879d0b1134f.html", "dir_4b0ffcb7ab18f30ccfa85879d0b1134f" ], + [ "CANopen.h", "CANopen_8h.html", "CANopen_8h" ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/folderclosed.png b/Devices/Libraries/Systems/CANopenSocket/docs/folderclosed.png new file mode 100755 index 0000000..bb8ab35 Binary files /dev/null and b/Devices/Libraries/Systems/CANopenSocket/docs/folderclosed.png differ diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/folderopen.png b/Devices/Libraries/Systems/CANopenSocket/docs/folderopen.png new file mode 100755 index 0000000..d6c7f67 Binary files /dev/null and b/Devices/Libraries/Systems/CANopenSocket/docs/folderopen.png differ diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/functions.html b/Devices/Libraries/Systems/CANopenSocket/docs/functions.html new file mode 100755 index 0000000..96e2134 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/functions.html @@ -0,0 +1,135 @@ + + + + + + + +CANopenNode: Data Fields + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
Here is a list of all documented struct and union fields with links to the struct/union documentation for each field:
+ +

- a -

+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/functions_b.html b/Devices/Libraries/Systems/CANopenSocket/docs/functions_b.html new file mode 100755 index 0000000..2532177 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/functions_b.html @@ -0,0 +1,166 @@ + + + + + + + +CANopenNode: Data Fields + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
Here is a list of all documented struct and union fields with links to the struct/union documentation for each field:
+ +

- b -

+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/functions_c.html b/Devices/Libraries/Systems/CANopenSocket/docs/functions_c.html new file mode 100755 index 0000000..ec4cc42 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/functions_c.html @@ -0,0 +1,321 @@ + + + + + + + +CANopenNode: Data Fields + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
Here is a list of all documented struct and union fields with links to the struct/union documentation for each field:
+ +

- c -

+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/functions_d.html b/Devices/Libraries/Systems/CANopenSocket/docs/functions_d.html new file mode 100755 index 0000000..0398646 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/functions_d.html @@ -0,0 +1,149 @@ + + + + + + + +CANopenNode: Data Fields + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
Here is a list of all documented struct and union fields with links to the struct/union documentation for each field:
+ +

- d -

+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/functions_dup.js b/Devices/Libraries/Systems/CANopenSocket/docs/functions_dup.js new file mode 100755 index 0000000..78575fb --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/functions_dup.js @@ -0,0 +1,23 @@ +var functions_dup = +[ + [ "a", "functions.html", null ], + [ "b", "functions_b.html", null ], + [ "c", "functions_c.html", null ], + [ "d", "functions_d.html", null ], + [ "e", "functions_e.html", null ], + [ "f", "functions_f.html", null ], + [ "g", "functions_g.html", null ], + [ "h", "functions_h.html", null ], + [ "i", "functions_i.html", null ], + [ "l", "functions_l.html", null ], + [ "m", "functions_m.html", null ], + [ "n", "functions_n.html", null ], + [ "o", "functions_o.html", null ], + [ "p", "functions_p.html", null ], + [ "r", "functions_r.html", null ], + [ "s", "functions_s.html", null ], + [ "t", "functions_t.html", null ], + [ "u", "functions_u.html", null ], + [ "v", "functions_v.html", null ], + [ "w", "functions_w.html", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/functions_e.html b/Devices/Libraries/Systems/CANopenSocket/docs/functions_e.html new file mode 100755 index 0000000..96eb4d2 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/functions_e.html @@ -0,0 +1,220 @@ + + + + + + + +CANopenNode: Data Fields + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
Here is a list of all documented struct and union fields with links to the struct/union documentation for each field:
+ +

- e -

+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/functions_f.html b/Devices/Libraries/Systems/CANopenSocket/docs/functions_f.html new file mode 100755 index 0000000..1139bed --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/functions_f.html @@ -0,0 +1,195 @@ + + + + + + + +CANopenNode: Data Fields + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
Here is a list of all documented struct and union fields with links to the struct/union documentation for each field:
+ +

- f -

+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/functions_g.html b/Devices/Libraries/Systems/CANopenSocket/docs/functions_g.html new file mode 100755 index 0000000..d768096 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/functions_g.html @@ -0,0 +1,121 @@ + + + + + + + +CANopenNode: Data Fields + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
Here is a list of all documented struct and union fields with links to the struct/union documentation for each field:
+ +

- g -

+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/functions_h.html b/Devices/Libraries/Systems/CANopenSocket/docs/functions_h.html new file mode 100755 index 0000000..316be03 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/functions_h.html @@ -0,0 +1,133 @@ + + + + + + + +CANopenNode: Data Fields + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
Here is a list of all documented struct and union fields with links to the struct/union documentation for each field:
+ +

- h -

+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/functions_i.html b/Devices/Libraries/Systems/CANopenSocket/docs/functions_i.html new file mode 100755 index 0000000..b72bd11 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/functions_i.html @@ -0,0 +1,144 @@ + + + + + + + +CANopenNode: Data Fields + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
Here is a list of all documented struct and union fields with links to the struct/union documentation for each field:
+ +

- i -

+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/functions_l.html b/Devices/Libraries/Systems/CANopenSocket/docs/functions_l.html new file mode 100755 index 0000000..8bc2152 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/functions_l.html @@ -0,0 +1,200 @@ + + + + + + + +CANopenNode: Data Fields + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
Here is a list of all documented struct and union fields with links to the struct/union documentation for each field:
+ +

- l -

+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/functions_m.html b/Devices/Libraries/Systems/CANopenSocket/docs/functions_m.html new file mode 100755 index 0000000..b92636e --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/functions_m.html @@ -0,0 +1,169 @@ + + + + + + + +CANopenNode: Data Fields + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
Here is a list of all documented struct and union fields with links to the struct/union documentation for each field:
+ +

- m -

+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/functions_n.html b/Devices/Libraries/Systems/CANopenSocket/docs/functions_n.html new file mode 100755 index 0000000..f8e2db5 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/functions_n.html @@ -0,0 +1,169 @@ + + + + + + + +CANopenNode: Data Fields + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
Here is a list of all documented struct and union fields with links to the struct/union documentation for each field:
+ +

- n -

+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/functions_o.html b/Devices/Libraries/Systems/CANopenSocket/docs/functions_o.html new file mode 100755 index 0000000..b3ae708 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/functions_o.html @@ -0,0 +1,152 @@ + + + + + + + +CANopenNode: Data Fields + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
Here is a list of all documented struct and union fields with links to the struct/union documentation for each field:
+ +

- o -

+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/functions_p.html b/Devices/Libraries/Systems/CANopenSocket/docs/functions_p.html new file mode 100755 index 0000000..b52d40d --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/functions_p.html @@ -0,0 +1,198 @@ + + + + + + + +CANopenNode: Data Fields + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
Here is a list of all documented struct and union fields with links to the struct/union documentation for each field:
+ +

- p -

+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/functions_r.html b/Devices/Libraries/Systems/CANopenSocket/docs/functions_r.html new file mode 100755 index 0000000..c0724c7 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/functions_r.html @@ -0,0 +1,194 @@ + + + + + + + +CANopenNode: Data Fields + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
Here is a list of all documented struct and union fields with links to the struct/union documentation for each field:
+ +

- r -

+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/functions_s.html b/Devices/Libraries/Systems/CANopenSocket/docs/functions_s.html new file mode 100755 index 0000000..959e204 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/functions_s.html @@ -0,0 +1,236 @@ + + + + + + + +CANopenNode: Data Fields + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
Here is a list of all documented struct and union fields with links to the struct/union documentation for each field:
+ +

- s -

+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/functions_t.html b/Devices/Libraries/Systems/CANopenSocket/docs/functions_t.html new file mode 100755 index 0000000..cd78586 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/functions_t.html @@ -0,0 +1,236 @@ + + + + + + + +CANopenNode: Data Fields + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
Here is a list of all documented struct and union fields with links to the struct/union documentation for each field:
+ +

- t -

+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/functions_u.html b/Devices/Libraries/Systems/CANopenSocket/docs/functions_u.html new file mode 100755 index 0000000..78c7ee3 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/functions_u.html @@ -0,0 +1,112 @@ + + + + + + + +CANopenNode: Data Fields + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
Here is a list of all documented struct and union fields with links to the struct/union documentation for each field:
+ +

- u -

+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/functions_v.html b/Devices/Libraries/Systems/CANopenSocket/docs/functions_v.html new file mode 100755 index 0000000..7bfb11e --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/functions_v.html @@ -0,0 +1,124 @@ + + + + + + + +CANopenNode: Data Fields + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
Here is a list of all documented struct and union fields with links to the struct/union documentation for each field:
+ +

- v -

+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/functions_vars.html b/Devices/Libraries/Systems/CANopenSocket/docs/functions_vars.html new file mode 100755 index 0000000..5158f8f --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/functions_vars.html @@ -0,0 +1,135 @@ + + + + + + + +CANopenNode: Data Fields - Variables + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+  + +

- a -

+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/functions_vars.js b/Devices/Libraries/Systems/CANopenSocket/docs/functions_vars.js new file mode 100755 index 0000000..f04dd3c --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/functions_vars.js @@ -0,0 +1,23 @@ +var functions_vars = +[ + [ "a", "functions_vars.html", null ], + [ "b", "functions_vars_b.html", null ], + [ "c", "functions_vars_c.html", null ], + [ "d", "functions_vars_d.html", null ], + [ "e", "functions_vars_e.html", null ], + [ "f", "functions_vars_f.html", null ], + [ "g", "functions_vars_g.html", null ], + [ "h", "functions_vars_h.html", null ], + [ "i", "functions_vars_i.html", null ], + [ "l", "functions_vars_l.html", null ], + [ "m", "functions_vars_m.html", null ], + [ "n", "functions_vars_n.html", null ], + [ "o", "functions_vars_o.html", null ], + [ "p", "functions_vars_p.html", null ], + [ "r", "functions_vars_r.html", null ], + [ "s", "functions_vars_s.html", null ], + [ "t", "functions_vars_t.html", null ], + [ "u", "functions_vars_u.html", null ], + [ "v", "functions_vars_v.html", null ], + [ "w", "functions_vars_w.html", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/functions_vars_b.html b/Devices/Libraries/Systems/CANopenSocket/docs/functions_vars_b.html new file mode 100755 index 0000000..fe6ca4b --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/functions_vars_b.html @@ -0,0 +1,166 @@ + + + + + + + +CANopenNode: Data Fields - Variables + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+  + +

- b -

+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/functions_vars_c.html b/Devices/Libraries/Systems/CANopenSocket/docs/functions_vars_c.html new file mode 100755 index 0000000..a7cd294 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/functions_vars_c.html @@ -0,0 +1,321 @@ + + + + + + + +CANopenNode: Data Fields - Variables + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+  + +

- c -

+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/functions_vars_d.html b/Devices/Libraries/Systems/CANopenSocket/docs/functions_vars_d.html new file mode 100755 index 0000000..b722e3b --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/functions_vars_d.html @@ -0,0 +1,149 @@ + + + + + + + +CANopenNode: Data Fields - Variables + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+  + +

- d -

+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/functions_vars_e.html b/Devices/Libraries/Systems/CANopenSocket/docs/functions_vars_e.html new file mode 100755 index 0000000..8277027 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/functions_vars_e.html @@ -0,0 +1,220 @@ + + + + + + + +CANopenNode: Data Fields - Variables + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+  + +

- e -

+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/functions_vars_f.html b/Devices/Libraries/Systems/CANopenSocket/docs/functions_vars_f.html new file mode 100755 index 0000000..0bb3a37 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/functions_vars_f.html @@ -0,0 +1,195 @@ + + + + + + + +CANopenNode: Data Fields - Variables + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+  + +

- f -

+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/functions_vars_g.html b/Devices/Libraries/Systems/CANopenSocket/docs/functions_vars_g.html new file mode 100755 index 0000000..95198a5 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/functions_vars_g.html @@ -0,0 +1,121 @@ + + + + + + + +CANopenNode: Data Fields - Variables + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+  + +

- g -

+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/functions_vars_h.html b/Devices/Libraries/Systems/CANopenSocket/docs/functions_vars_h.html new file mode 100755 index 0000000..2e80b40 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/functions_vars_h.html @@ -0,0 +1,133 @@ + + + + + + + +CANopenNode: Data Fields - Variables + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+  + +

- h -

+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/functions_vars_i.html b/Devices/Libraries/Systems/CANopenSocket/docs/functions_vars_i.html new file mode 100755 index 0000000..831d765 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/functions_vars_i.html @@ -0,0 +1,144 @@ + + + + + + + +CANopenNode: Data Fields - Variables + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+  + +

- i -

+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/functions_vars_l.html b/Devices/Libraries/Systems/CANopenSocket/docs/functions_vars_l.html new file mode 100755 index 0000000..8a4dedf --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/functions_vars_l.html @@ -0,0 +1,200 @@ + + + + + + + +CANopenNode: Data Fields - Variables + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+  + +

- l -

+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/functions_vars_m.html b/Devices/Libraries/Systems/CANopenSocket/docs/functions_vars_m.html new file mode 100755 index 0000000..815bed8 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/functions_vars_m.html @@ -0,0 +1,169 @@ + + + + + + + +CANopenNode: Data Fields - Variables + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+  + +

- m -

+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/functions_vars_n.html b/Devices/Libraries/Systems/CANopenSocket/docs/functions_vars_n.html new file mode 100755 index 0000000..b41419f --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/functions_vars_n.html @@ -0,0 +1,169 @@ + + + + + + + +CANopenNode: Data Fields - Variables + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+  + +

- n -

+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/functions_vars_o.html b/Devices/Libraries/Systems/CANopenSocket/docs/functions_vars_o.html new file mode 100755 index 0000000..b83fe91 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/functions_vars_o.html @@ -0,0 +1,152 @@ + + + + + + + +CANopenNode: Data Fields - Variables + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+  + +

- o -

+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/functions_vars_p.html b/Devices/Libraries/Systems/CANopenSocket/docs/functions_vars_p.html new file mode 100755 index 0000000..d4f53d3 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/functions_vars_p.html @@ -0,0 +1,198 @@ + + + + + + + +CANopenNode: Data Fields - Variables + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+  + +

- p -

+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/functions_vars_r.html b/Devices/Libraries/Systems/CANopenSocket/docs/functions_vars_r.html new file mode 100755 index 0000000..7f426fa --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/functions_vars_r.html @@ -0,0 +1,194 @@ + + + + + + + +CANopenNode: Data Fields - Variables + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+  + +

- r -

+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/functions_vars_s.html b/Devices/Libraries/Systems/CANopenSocket/docs/functions_vars_s.html new file mode 100755 index 0000000..c3df299 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/functions_vars_s.html @@ -0,0 +1,236 @@ + + + + + + + +CANopenNode: Data Fields - Variables + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+  + +

- s -

+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/functions_vars_t.html b/Devices/Libraries/Systems/CANopenSocket/docs/functions_vars_t.html new file mode 100755 index 0000000..902c9d9 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/functions_vars_t.html @@ -0,0 +1,236 @@ + + + + + + + +CANopenNode: Data Fields - Variables + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+  + +

- t -

+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/functions_vars_u.html b/Devices/Libraries/Systems/CANopenSocket/docs/functions_vars_u.html new file mode 100755 index 0000000..336120c --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/functions_vars_u.html @@ -0,0 +1,112 @@ + + + + + + + +CANopenNode: Data Fields - Variables + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+  + +

- u -

+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/functions_vars_v.html b/Devices/Libraries/Systems/CANopenSocket/docs/functions_vars_v.html new file mode 100755 index 0000000..229f83f --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/functions_vars_v.html @@ -0,0 +1,124 @@ + + + + + + + +CANopenNode: Data Fields - Variables + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+  + +

- v -

+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/functions_vars_w.html b/Devices/Libraries/Systems/CANopenSocket/docs/functions_vars_w.html new file mode 100755 index 0000000..89ec45a --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/functions_vars_w.html @@ -0,0 +1,117 @@ + + + + + + + +CANopenNode: Data Fields - Variables + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+  + +

- w -

+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/functions_w.html b/Devices/Libraries/Systems/CANopenSocket/docs/functions_w.html new file mode 100755 index 0000000..6b474c8 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/functions_w.html @@ -0,0 +1,117 @@ + + + + + + + +CANopenNode: Data Fields + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
Here is a list of all documented struct and union fields with links to the struct/union documentation for each field:
+ +

- w -

+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/globals.html b/Devices/Libraries/Systems/CANopenSocket/docs/globals.html new file mode 100755 index 0000000..1e87d98 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/globals.html @@ -0,0 +1,112 @@ + + + + + + + +CANopenNode: Globals + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
Here is a list of all documented functions, variables, defines, enums, and typedefs with links to the documentation:
+ +

- b -

+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/globals_c.html b/Devices/Libraries/Systems/CANopenSocket/docs/globals_c.html new file mode 100755 index 0000000..97dc25d --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/globals_c.html @@ -0,0 +1,2104 @@ + + + + + + + +CANopenNode: Globals + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
Here is a list of all documented functions, variables, defines, enums, and typedefs with links to the documentation:
+ +

- c -

+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/globals_d.html b/Devices/Libraries/Systems/CANopenSocket/docs/globals_d.html new file mode 100755 index 0000000..7590fbf --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/globals_d.html @@ -0,0 +1,112 @@ + + + + + + + +CANopenNode: Globals + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
Here is a list of all documented functions, variables, defines, enums, and typedefs with links to the documentation:
+ +

- d -

+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/globals_defs.html b/Devices/Libraries/Systems/CANopenSocket/docs/globals_defs.html new file mode 100755 index 0000000..3f95cf1 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/globals_defs.html @@ -0,0 +1,356 @@ + + + + + + + +CANopenNode: Globals + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+  + +

- c -

+ + +

- f -

+ + +

- n -

+ + +

- o -

+ + +

- t -

+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/globals_dup.js b/Devices/Libraries/Systems/CANopenSocket/docs/globals_dup.js new file mode 100755 index 0000000..da69b29 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/globals_dup.js @@ -0,0 +1,13 @@ +var globals_dup = +[ + [ "b", "globals.html", null ], + [ "c", "globals_c.html", null ], + [ "d", "globals_d.html", null ], + [ "f", "globals_f.html", null ], + [ "i", "globals_i.html", null ], + [ "l", "globals_l.html", null ], + [ "n", "globals_n.html", null ], + [ "o", "globals_o.html", null ], + [ "t", "globals_t.html", null ], + [ "u", "globals_u.html", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/globals_enum.html b/Devices/Libraries/Systems/CANopenSocket/docs/globals_enum.html new file mode 100755 index 0000000..6c52a09 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/globals_enum.html @@ -0,0 +1,218 @@ + + + + + + + +CANopenNode: Globals + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+  + +

- c -

+ + +

- o -

+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/globals_eval.html b/Devices/Libraries/Systems/CANopenSocket/docs/globals_eval.html new file mode 100755 index 0000000..40efaf5 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/globals_eval.html @@ -0,0 +1,1201 @@ + + + + + + + +CANopenNode: Globals + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+  + +

- c -

+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/globals_eval.js b/Devices/Libraries/Systems/CANopenSocket/docs/globals_eval.js new file mode 100755 index 0000000..6bef011 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/globals_eval.js @@ -0,0 +1,5 @@ +var globals_eval = +[ + [ "c", "globals_eval.html", null ], + [ "o", "globals_eval_o.html", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/globals_eval_o.html b/Devices/Libraries/Systems/CANopenSocket/docs/globals_eval_o.html new file mode 100755 index 0000000..eee0b45 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/globals_eval_o.html @@ -0,0 +1,391 @@ + + + + + + + +CANopenNode: Globals + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+  + +

- o -

+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/globals_f.html b/Devices/Libraries/Systems/CANopenSocket/docs/globals_f.html new file mode 100755 index 0000000..45ddcae --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/globals_f.html @@ -0,0 +1,118 @@ + + + + + + + +CANopenNode: Globals + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
Here is a list of all documented functions, variables, defines, enums, and typedefs with links to the documentation:
+ +

- f -

+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/globals_func.html b/Devices/Libraries/Systems/CANopenSocket/docs/globals_func.html new file mode 100755 index 0000000..eafdee6 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/globals_func.html @@ -0,0 +1,703 @@ + + + + + + + +CANopenNode: Globals + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+  + +

- c -

+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/globals_func.js b/Devices/Libraries/Systems/CANopenSocket/docs/globals_func.js new file mode 100755 index 0000000..e74d184 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/globals_func.js @@ -0,0 +1,6 @@ +var globals_func = +[ + [ "c", "globals_func.html", null ], + [ "l", "globals_func_l.html", null ], + [ "o", "globals_func_o.html", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/globals_func_l.html b/Devices/Libraries/Systems/CANopenSocket/docs/globals_func_l.html new file mode 100755 index 0000000..f637bd2 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/globals_func_l.html @@ -0,0 +1,112 @@ + + + + + + + +CANopenNode: Globals + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+  + +

- l -

+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/globals_func_o.html b/Devices/Libraries/Systems/CANopenSocket/docs/globals_func_o.html new file mode 100755 index 0000000..079b3fc --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/globals_func_o.html @@ -0,0 +1,232 @@ + + + + + + + +CANopenNode: Globals + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+  + +

- o -

+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/globals_i.html b/Devices/Libraries/Systems/CANopenSocket/docs/globals_i.html new file mode 100755 index 0000000..c3b4a56 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/globals_i.html @@ -0,0 +1,121 @@ + + + + + + + +CANopenNode: Globals + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
Here is a list of all documented functions, variables, defines, enums, and typedefs with links to the documentation:
+ +

- i -

+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/globals_l.html b/Devices/Libraries/Systems/CANopenSocket/docs/globals_l.html new file mode 100755 index 0000000..228e3c0 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/globals_l.html @@ -0,0 +1,112 @@ + + + + + + + +CANopenNode: Globals + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
Here is a list of all documented functions, variables, defines, enums, and typedefs with links to the documentation:
+ +

- l -

+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/globals_n.html b/Devices/Libraries/Systems/CANopenSocket/docs/globals_n.html new file mode 100755 index 0000000..d256717 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/globals_n.html @@ -0,0 +1,112 @@ + + + + + + + +CANopenNode: Globals + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
Here is a list of all documented functions, variables, defines, enums, and typedefs with links to the documentation:
+ +

- n -

+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/globals_o.html b/Devices/Libraries/Systems/CANopenSocket/docs/globals_o.html new file mode 100755 index 0000000..fbd2ed8 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/globals_o.html @@ -0,0 +1,541 @@ + + + + + + + +CANopenNode: Globals + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
Here is a list of all documented functions, variables, defines, enums, and typedefs with links to the documentation:
+ +

- o -

+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/globals_t.html b/Devices/Libraries/Systems/CANopenSocket/docs/globals_t.html new file mode 100755 index 0000000..2d5c4fe --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/globals_t.html @@ -0,0 +1,112 @@ + + + + + + + +CANopenNode: Globals + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
Here is a list of all documented functions, variables, defines, enums, and typedefs with links to the documentation:
+ +

- t -

+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/globals_type.html b/Devices/Libraries/Systems/CANopenSocket/docs/globals_type.html new file mode 100755 index 0000000..a117d91 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/globals_type.html @@ -0,0 +1,149 @@ + + + + + + + +CANopenNode: Globals + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+  +
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/globals_u.html b/Devices/Libraries/Systems/CANopenSocket/docs/globals_u.html new file mode 100755 index 0000000..69603b1 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/globals_u.html @@ -0,0 +1,121 @@ + + + + + + + +CANopenNode: Globals + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
Here is a list of all documented functions, variables, defines, enums, and typedefs with links to the documentation:
+ +

- u -

+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/globals_vars.html b/Devices/Libraries/Systems/CANopenSocket/docs/globals_vars.html new file mode 100755 index 0000000..ccbcc10 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/globals_vars.html @@ -0,0 +1,110 @@ + + + + + + + +CANopenNode: Globals + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+ 
    +
  • CO_LSS_bitTimingTableLookup +: CO_LSS.h +
    • +
+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__CAN__Message__reception.html b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__CAN__Message__reception.html new file mode 100755 index 0000000..e73dc99 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__CAN__Message__reception.html @@ -0,0 +1,289 @@ + + + + + + + +CANopenNode: Reception of CAN messages + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Data Structures | +Functions
+
+
Reception of CAN messages
CANopen_301 » Driver
+
+
+ +

Target specific definitions and description of CAN message reception. +More...

+ + + + + +

+Data Structures

struct  CO_CANrx_t
 Configuration object for CAN received message for specific CANopenNode Object. More...
 
+ + + + + + + + + + + + + +

+Functions

void CANrx_callback (void *object, void *rxMsg)
 CAN receive callback function which pre-processes received CAN message. More...
 
static uint16_t CO_CANrxMsg_readIdent (void *rxMsg)
 CANrx_callback() can read CAN identifier from received CAN message. More...
 
static uint8_t CO_CANrxMsg_readDLC (void *rxMsg)
 CANrx_callback() can read Data Length Code from received CAN message. More...
 
static uint8_t * CO_CANrxMsg_readData (void *rxMsg)
 CANrx_callback() can read pointer to data from received CAN message. More...
 
+

Detailed Description

+

Target specific definitions and description of CAN message reception.

+

CAN messages in CANopenNode are usually received by its own thread or higher priority interrupt. Received CAN messages are first filtered by hardware or by software. Thread then examines its 11-bit CAN-id and mask and determines, to which CANopenNode Object it belongs to. After that it calls predefined CANrx_callback() function, which quickly pre-processes the message and fetches the relevant data. CANrx_callback() function is defined by each CANopenNode Object separately. Pre-processed fetched data are later processed in another thread.

+

If CANopenNode Object reception of specific CAN message, it must first configure its own CO_CANrx_t object with the CO_CANrxBufferInit() function.

+

Function Documentation

+ +

◆ CANrx_callback()

+ +
+
+ + + + + + + + + + + + + + + + + + +
void CANrx_callback (void * object,
void * rxMsg 
)
+
+ +

CAN receive callback function which pre-processes received CAN message.

+

It is called by fast CAN receive thread. Each CANopenNodeObject" defines its own and registers it with CO_CANrxBufferInit(), by passing function pointer.

+
Parameters
+ + + +
objectpointer to specific CANopenNode Object, registered with CO_CANrxBufferInit()
rxMsgpointer to received CAN message
+
+
+ +
+
+ +

◆ CO_CANrxMsg_readIdent()

+ +
+
+ + + + + +
+ + + + + + + + +
static uint16_t CO_CANrxMsg_readIdent (void * rxMsg)
+ 		+inlinestatic
+
+ +

CANrx_callback() can read CAN identifier from received CAN message.

+

Must be defined in the CO_driver_target.h file.

+

This is target specific function and is specific for specific microcontroller. It is best to implement it by using inline function or macro. rxMsg parameter should cast to a pointer to structure. For best efficiency structure may have the same alignment as CAN registers inside CAN module.

+
Parameters
+ + +
rxMsgPointer to received message
+
+
+
Returns
11-bit CAN standard identifier.
+ +
+
+ +

◆ CO_CANrxMsg_readDLC()

+ +
+
+ + + + + +
+ + + + + + + + +
static uint8_t CO_CANrxMsg_readDLC (void * rxMsg)
+ 		+inlinestatic
+
+ +

CANrx_callback() can read Data Length Code from received CAN message.

+

See also CO_CANrxMsg_readIdent():

+
Parameters
+ + +
rxMsgPointer to received message
+
+
+
Returns
data length in bytes (0 to 8)
+ +
+
+ +

◆ CO_CANrxMsg_readData()

+ +
+
+ + + + + +
+ + + + + + + + +
static uint8_t* CO_CANrxMsg_readData (void * rxMsg)
+ 		+inlinestatic
+
+ +

CANrx_callback() can read pointer to data from received CAN message.

+

See also CO_CANrxMsg_readIdent():

+
Parameters
+ + +
rxMsgPointer to received message
+
+
+
Returns
pointer to data buffer
+ +
+
+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__CAN__Message__reception.js b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__CAN__Message__reception.js new file mode 100755 index 0000000..5139fdf --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__CAN__Message__reception.js @@ -0,0 +1,13 @@ +var group__CO__CAN__Message__reception = +[ + [ "CO_CANrx_t", "structCO__CANrx__t.html", [ + [ "ident", "structCO__CANrx__t.html#a8595c238cf0364bde995dee97d321909", null ], + [ "mask", "structCO__CANrx__t.html#af7a48dd4ac895a19c4031038e2c1222d", null ], + [ "object", "structCO__CANrx__t.html#a957a1ce67cd1d9010889d557bf0c5770", null ], + [ "pCANrx_callback", "structCO__CANrx__t.html#a8e4668eec8326bb9ac08d67afc3060c7", null ] + ] ], + [ "CANrx_callback", "group__CO__CAN__Message__reception.html#ga23168979123a5ca8a87d49307eb2990e", null ], + [ "CO_CANrxMsg_readIdent", "group__CO__CAN__Message__reception.html#ga018e9159b92165b2506a6673113cdc0e", null ], + [ "CO_CANrxMsg_readDLC", "group__CO__CAN__Message__reception.html#gacec1dcf8b7e66ea2e65905f91321b299", null ], + [ "CO_CANrxMsg_readData", "group__CO__CAN__Message__reception.html#gab02a5fe910acf9aa5021f97e523697f9", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__CAN__Message__transmission.html b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__CAN__Message__transmission.html new file mode 100755 index 0000000..a27c06d --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__CAN__Message__transmission.html @@ -0,0 +1,124 @@ + + + + + + + +CANopenNode: Transmission of CAN messages + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Data Structures
+
+
Transmission of CAN messages
CANopen_301 » Driver
+
+
+ +

Target specific definitions and description of CAN message transmission. +More...

+ + + + + +

+Data Structures

struct  CO_CANtx_t
 Configuration object for CAN transmit message for specific CANopenNode Object. More...
 
+

Detailed Description

+

Target specific definitions and description of CAN message transmission.

+

If CANopenNode Object needs transmitting CAN message, it must first configure its own CO_CANtx_t object with the CO_CANtxBufferInit() function. CAN message can then be sent with CO_CANsend() function. If at that moment CAN transmit buffer inside microcontroller's CAN module is free, message is copied directly to the CAN module. Otherwise CO_CANsend() function sets bufferFull flag to true. Message will be then sent by CAN TX interrupt as soon as CAN module is freed. Until message is not copied to CAN module, its contents must not change. If there are multiple CO_CANtx_t objects with bufferFull flag set to true, then CO_CANtx_t with lower index will be sent first.

+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__CAN__Message__transmission.js b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__CAN__Message__transmission.js new file mode 100755 index 0000000..6477a54 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__CAN__Message__transmission.js @@ -0,0 +1,10 @@ +var group__CO__CAN__Message__transmission = +[ + [ "CO_CANtx_t", "structCO__CANtx__t.html", [ + [ "ident", "structCO__CANtx__t.html#a9cc2687eb11da14d4c0aa167352c635c", null ], + [ "DLC", "structCO__CANtx__t.html#a9bb96d60314283061f7619e36d870fa0", null ], + [ "data", "structCO__CANtx__t.html#aae5bcdc2296a5d1d53a3105e86dbb66d", null ], + [ "bufferFull", "structCO__CANtx__t.html#a305f0687a4ed7cd533e7937d6ff7d31b", null ], + [ "syncFlag", "structCO__CANtx__t.html#a79c19597a51351b9d6ed1c9bdfd051b3", null ] + ] ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__CANopen.html b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__CANopen.html new file mode 100755 index 0000000..235aee8 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__CANopen.html @@ -0,0 +1,752 @@ + + + + + + + +CANopenNode: CANopen + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Files | +Data Structures | +Macros | +Functions
+
+
CANopen
+
+
+ +

CANopenNode is free and open source implementation of CANopen communication protocol. +More...

+ + + + + +

+Files

file  CANopen.h
 Main CANopenNode file.
 
+ + + + + + + +

+Data Structures

struct  CO_config_t
 CANopen configuration, used with CO_new() More...
 
struct  CO_t
 CANopen object - collection of all CANopenNode objects. More...
 
+ + + + + + + +

+Macros

#define CO_MULTIPLE_OD
 If macro is defined externally, then configuration with multiple object dictionaries will be possible. More...
 
#define CO_USE_GLOBALS
 If macro is defined externally, then global variables for CANopen objects will be used instead of heap. More...
 
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

+Functions

CO_t * CO_new (CO_config_t *config, uint32_t *heapMemoryUsed)
 Create new CANopen object. More...
 
void CO_delete (CO_t *co)
 Delete CANopen object and free memory. More...
 
bool_t CO_isLSSslaveEnabled (CO_t *co)
 Test if LSS slave is enabled. More...
 
CO_ReturnError_t CO_CANinit (CO_t *co, void *CANptr, uint16_t bitRate)
 Initialize CAN driver. More...
 
CO_ReturnError_t CO_LSSinit (CO_t *co, CO_LSS_address_t *lssAddress, uint8_t *pendingNodeID, uint16_t *pendingBitRate)
 Initialize CANopen LSS slave. More...
 
CO_ReturnError_t CO_CANopenInit (CO_t *co, CO_NMT_t *NMT, CO_EM_t *em, const OD_t *od, const OD_entry_t *OD_statusBits, CO_NMT_control_t NMTcontrol, uint16_t firstHBTime_ms, uint16_t SDOserverTimeoutTime_ms, uint16_t SDOclientTimeoutTime_ms, bool_t SDOclientBlockTransfer, uint8_t nodeId)
 Initialize CANopenNode. More...
 
CO_NMT_reset_cmd_t CO_process (CO_t *co, bool_t enableGateway, uint32_t timeDifference_us, uint32_t *timerNext_us)
 Process CANopen objects. More...
 
bool_t CO_process_SYNC (CO_t *co, uint32_t timeDifference_us, uint32_t *timerNext_us)
 Process CANopen SYNC objects. More...
 
void CO_process_RPDO (CO_t *co, bool_t syncWas)
 Process CANopen RPDO objects. More...
 
void CO_process_TPDO (CO_t *co, bool_t syncWas, uint32_t timeDifference_us, uint32_t *timerNext_us)
 Process CANopen TPDO objects. More...
 
void CO_process_SRDO (CO_t *co, uint32_t timeDifference_us, uint32_t *timerNext_us)
 Process CANopen SRDO objects. More...
 
+

Detailed Description

+

CANopenNode is free and open source implementation of CANopen communication protocol.

+

CANopen is the internationally standardized (EN 50325-4) (CiA DS-301) CAN-based higher-layer protocol for embedded control system. For more information on CANopen see http://www.can-cia.org/

+

CANopenNode homepage is https://github.com/CANopenNode/CANopenNode

+

CANopen.h file combines all CANopenNode source files. Stack configuration is first defined in "CO_config.h" file. Number of different CANopenNode objects used is configured with CO_config_t structure or is read directly from "OD.h" file, if single object dictionary definition is used. "OD.h" and "OD.c" files defines CANopen Object Dictionary and are generated by external tool.

+

Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. You may obtain a copy of the License at

+

https://www.apache.org/licenses/LICENSE-2.0

+

Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License.

+

Macro Definition Documentation

+ +

◆ CO_MULTIPLE_OD

+ +
+
+ + + + +
#define CO_MULTIPLE_OD
+
+ +

If macro is defined externally, then configuration with multiple object dictionaries will be possible.

+

If macro is not defined, default "OD.h" file with necessary definitions, such as OD_CNT_xxx, will be used, and also memory consumption and startup time will be lower.

+ +
+
+ +

◆ CO_USE_GLOBALS

+ +
+
+ + + + +
#define CO_USE_GLOBALS
+
+ +

If macro is defined externally, then global variables for CANopen objects will be used instead of heap.

+

This is possible only if CO_MULTIPLE_OD is not defined.

+ +
+
+

Function Documentation

+ +

◆ CO_new()

+ +
+
+ + + + + + + + + + + + + + + + + + +
CO_t* CO_new (CO_config_t * config,
uint32_t * heapMemoryUsed 
)
+
+ +

Create new CANopen object.

+

If CO_USE_GLOBALS is defined, then function uses global static variables for all the CANopenNode objects. Otherwise it allocates all objects from heap.

+
Remarks
With some microcontrollers it is necessary to specify Heap size within linker configuration, if heap is used.
+
Parameters
+ + + +
configConfiguration structure, used if CO_MULTIPLE_OD is defined. It must stay in memory permanently. If CO_MULTIPLE_OD is not defined, config should be NULL and parameters are retrieved from default "OD.h" file.
[out]heapMemoryUsedInformation about heap memory used. Ignored if NULL.
+
+
+
Returns
Successfully allocated and configured CO_t object or NULL.
+ +
+
+ +

◆ CO_delete()

+ +
+
+ + + + + + + + +
void CO_delete (CO_t * co)
+
+ +

Delete CANopen object and free memory.

+

Must be called at program exit.

+
Parameters
+ + +
coCANopen object.
+
+
+ +
+
+ +

◆ CO_isLSSslaveEnabled()

+ +
+
+ + + + + + + + +
bool_t CO_isLSSslaveEnabled (CO_t * co)
+
+ +

Test if LSS slave is enabled.

+
Parameters
+ + +
coCANopen object.
+
+
+
Returns
True if enabled
+ +
+
+ +

◆ CO_CANinit()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + +
CO_ReturnError_t CO_CANinit (CO_t * co,
void * CANptr,
uint16_t bitRate 
)
+
+ +

Initialize CAN driver.

+

Function must be called in the communication reset section.

+
Parameters
+ + + + +
coCANopen object.
CANptrPointer to the user-defined CAN base structure, passed to CO_CANmodule_init().
bitRateCAN bit rate.
+
+
+
Returns
CO_ERROR_NO in case of success.
+ +
+
+ +

◆ CO_LSSinit()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
CO_ReturnError_t CO_LSSinit (CO_t * co,
CO_LSS_address_t * lssAddress,
uint8_t * pendingNodeID,
uint16_t * pendingBitRate 
)
+
+ +

Initialize CANopen LSS slave.

+

Function must be called before CO_CANopenInit.

+

See CO_LSSslave_init() for description of parameters.

+
Parameters
+ + + + + +
coCANopen object.
lssAddressLSS slave address, from OD object 0x1018
[in,out]pendingNodeIDPending node ID or 0xFF (unconfigured)
[in,out]pendingBitRatePending bit rate of the CAN interface
+
+
+
Returns
CO_ERROR_NO in case of success.
+ +
+
+ +

◆ CO_CANopenInit()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
CO_ReturnError_t CO_CANopenInit (CO_t * co,
CO_NMT_t * NMT,
CO_EM_t * em,
const OD_t * od,
const OD_entry_t * OD_statusBits,
CO_NMT_control_t NMTcontrol,
uint16_t firstHBTime_ms,
uint16_t SDOserverTimeoutTime_ms,
uint16_t SDOclientTimeoutTime_ms,
bool_t SDOclientBlockTransfer,
uint8_t nodeId 
)
+
+ +

Initialize CANopenNode.

+

Function must be called in the communication reset section.

+
Parameters
+ + + + + + + + + + + + +
coCANopen object.
emEmergency object, which is used inside different CANopen objects, usually for error reporting. If NULL, then 'co->em' will be used. if NULL and 'co->CNT_EM' is 0, then function returns with error.
NMTIf 'co->CNT_NMT' is 0, this object must be specified, If 'co->CNT_NMT' is 1,then it is ignored and can be NULL. NMT object is used for retrieving NMT internal state inside CO_process().
odCANopen Object dictionary
OD_statusBitsArgument passed to CO_EM_init(). May be NULL.
NMTcontrolArgument passed to CO_NMT_init().
firstHBTime_msArgument passed to CO_NMT_init().
SDOserverTimeoutTime_msArgument passed to CO_SDOserver_init().
SDOclientTimeoutTime_msDefault timeout in milliseconds for SDO client, 500 typically. SDO client is configured from CO_GTWA_init().
SDOclientBlockTransferIf true, block transfer will be set in SDO client by default. SDO client is configured from by CO_GTWA_init().
nodeIdCANopen Node ID (1 ... 127) or 0xFF(unconfigured). In the CANopen initialization it is the same as pendingBitRate from CO_LSSinit(). If it is unconfigured, then some CANopen objects will not be initialized nor processed.
+
+
+
Returns
CO_ERROR_NO in case of success.
+ +
+
+ +

◆ CO_process()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
CO_NMT_reset_cmd_t CO_process (CO_t * co,
bool_t enableGateway,
uint32_t timeDifference_us,
uint32_t * timerNext_us 
)
+
+ +

Process CANopen objects.

+

Function must be called cyclically. It processes all "asynchronous" CANopen objects.

+
Parameters
+ + + + + +
coCANopen object.
enableGatewayIf true, gateway to external world will be enabled.
timeDifference_usTime difference from previous function call in microseconds.
[out]timerNext_usinfo to OS - maximum delay time after this function should be called next time in [microseconds]. Value can be used for OS sleep time. Initial value must be set to maximum interval time. Output will be equal or lower to initial value. Calculation is based on various timers which expire in known time. Parameter should be used in combination with callbacks configured with CO_***_initCallbackPre() functions. Those callbacks should also trigger calling of CO_process() function. Parameter is ignored if NULL. See also CO_CONFIG_FLAG_CALLBACK_PRE configuration macro.
+
+
+
Returns
Node or communication reset request, from CO_NMT_process().
+ +
+
+ +

◆ CO_process_SYNC()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + +
bool_t CO_process_SYNC (CO_t * co,
uint32_t timeDifference_us,
uint32_t * timerNext_us 
)
+
+ +

Process CANopen SYNC objects.

+

Function must be called cyclically. For time critical applications it may be called from real time thread with constant interval (1ms typically). It processes SYNC CANopen objects.

+
Parameters
+ + + + +
coCANopen object.
timeDifference_usTime difference from previous function call in microseconds.
[out]timerNext_usinfo to OS - see CO_process().
+
+
+
Returns
True, if CANopen SYNC message was just received or transmitted.
+ +
+
+ +

◆ CO_process_RPDO()

+ +
+
+ + + + + + + + + + + + + + + + + + +
void CO_process_RPDO (CO_t * co,
bool_t syncWas 
)
+
+ +

Process CANopen RPDO objects.

+

Function must be called cyclically. For time critical applications it may be called from real time thread with constant interval (1ms typically). It processes receive PDO CANopen objects.

+
Parameters
+ + + +
coCANopen object.
syncWasTrue, if CANopen SYNC message was just received or transmitted.
+
+
+ +
+
+ +

◆ CO_process_TPDO()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
void CO_process_TPDO (CO_t * co,
bool_t syncWas,
uint32_t timeDifference_us,
uint32_t * timerNext_us 
)
+
+ +

Process CANopen TPDO objects.

+

Function must be called cyclically. For time critical applications it may be called from real time thread with constant interval (1ms typically). It processes transmit PDO CANopen objects.

+
Parameters
+ + + + + +
coCANopen object.
syncWasTrue, if CANopen SYNC message was just received or transmitted.
timeDifference_usTime difference from previous function call in microseconds.
[out]timerNext_usinfo to OS - see CO_process().
+
+
+ +
+
+ +

◆ CO_process_SRDO()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + +
void CO_process_SRDO (CO_t * co,
uint32_t timeDifference_us,
uint32_t * timerNext_us 
)
+
+ +

Process CANopen SRDO objects.

+

Function must be called cyclically. For time critical applications it may be called from real time thread with constant interval (1ms typically). It processes SRDO CANopen objects.

+
Parameters
+ + + + +
coCANopen object.
timeDifference_usTime difference from previous function call in microseconds.
[out]timerNext_usinfo to OS - see CO_process().
+
+
+ +
+
+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__CANopen.js b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__CANopen.js new file mode 100755 index 0000000..c1d511b --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__CANopen.js @@ -0,0 +1,107 @@ +var group__CO__CANopen = +[ + [ "CANopen.h", "CANopen_8h.html", null ], + [ "CO_config_t", "structCO__config__t.html", [ + [ "CNT_NMT", "structCO__config__t.html#aeef814580eb5ece5156e63bfc1b490c9", null ], + [ "ENTRY_H1017", "structCO__config__t.html#ad17f77b55de3d90ec983fcac49eeab6d", null ], + [ "CNT_HB_CONS", "structCO__config__t.html#a0031fc8f80e95f8480c918dbf8289671", null ], + [ "ENTRY_H1016", "structCO__config__t.html#a0af4cf7d0355861e7f60206d794d6a91", null ], + [ "CNT_EM", "structCO__config__t.html#a515e08f68835f71a6f145be8f27b510a", null ], + [ "ENTRY_H1001", "structCO__config__t.html#a6a6c19e816fb76882e85b2c07c0d8f42", null ], + [ "ENTRY_H1014", "structCO__config__t.html#a4827d94f6152cc12d86bd21312ae86e4", null ], + [ "ENTRY_H1015", "structCO__config__t.html#a141f21b4d1730206d1af823fd6b13a01", null ], + [ "ENTRY_H1003", "structCO__config__t.html#a7e320b309714b7f623c2006d45fee929", null ], + [ "CNT_SDO_SRV", "structCO__config__t.html#aac83faf556924515cc2aa8003753ab58", null ], + [ "ENTRY_H1200", "structCO__config__t.html#a05ab8adad4517850e31e5542895f7cc5", null ], + [ "CNT_SDO_CLI", "structCO__config__t.html#a2fc9606643a7fb4d4237f01812d3a6d2", null ], + [ "ENTRY_H1280", "structCO__config__t.html#a9f871c4ec753e8414cdb47eb78c3e09d", null ], + [ "CNT_TIME", "structCO__config__t.html#ada2a43384a544fa2f235de24a874b1e6", null ], + [ "ENTRY_H1012", "structCO__config__t.html#abac6be7122af1a8a4f9ae3ff5912d490", null ], + [ "CNT_SYNC", "structCO__config__t.html#af6dbc7d9f31b4cb050e23af8cff3df33", null ], + [ "ENTRY_H1005", "structCO__config__t.html#a02a4992f47db72816753ff2aa1964318", null ], + [ "ENTRY_H1006", "structCO__config__t.html#aa9befdebbaaa22f309b9a1b115612071", null ], + [ "ENTRY_H1007", "structCO__config__t.html#ad51ab63ca8b5836bf0dd8543f02db544", null ], + [ "ENTRY_H1019", "structCO__config__t.html#a468c82f6a0afd757a6b78ce33532c0d2", null ], + [ "CNT_RPDO", "structCO__config__t.html#a7a75302ac077462b67d767b0a11c9f56", null ], + [ "ENTRY_H1400", "structCO__config__t.html#a5e0984d93183493d587523888465eaa7", null ], + [ "ENTRY_H1600", "structCO__config__t.html#ab2ddc9943fd8c89f3b852d7ac9508d21", null ], + [ "CNT_TPDO", "structCO__config__t.html#a1d830617f50e3235de35a403a1513693", null ], + [ "ENTRY_H1800", "structCO__config__t.html#a29b98c08edfe0fba2e46c7af7a9edf6f", null ], + [ "ENTRY_H1A00", "structCO__config__t.html#a43fd6a448c91910c603f2c7756610432", null ], + [ "CNT_LEDS", "structCO__config__t.html#a642809cc681792bca855906241d891cc", null ], + [ "CNT_GFC", "structCO__config__t.html#ae282bab830810b61c0b0c3223654d674", null ], + [ "ENTRY_H1300", "structCO__config__t.html#a91c9f3ddb67231854af39224a9597e20", null ], + [ "CNT_SRDO", "structCO__config__t.html#ae58a44be57069709af3f6acbd10953e1", null ], + [ "ENTRY_H1301", "structCO__config__t.html#a87076cb1f9282d9720c21d395ff4e541", null ], + [ "ENTRY_H1381", "structCO__config__t.html#a7b3172b29ce8751adcab9e4351dcc31e", null ], + [ "ENTRY_H13FE", "structCO__config__t.html#a03fcaca5a8e0e71b86086908cae75f3d", null ], + [ "ENTRY_H13FF", "structCO__config__t.html#aa4cb9674209b83e7f0e48b01feaa04ef", null ], + [ "CNT_LSS_SLV", "structCO__config__t.html#a00a7a598b946ed13e3af7696e9f92dcc", null ], + [ "CNT_LSS_MST", "structCO__config__t.html#ac253cae7039090a6c04bc1e385f3ec21", null ], + [ "CNT_GTWA", "structCO__config__t.html#a64725014ecce342843f14ffc4b57e2a2", null ], + [ "CNT_TRACE", "structCO__config__t.html#aaafb8ffff236b51cd6d4ab16426d460f", null ] + ] ], + [ "CO_t", "structCO__t.html", [ + [ "nodeIdUnconfigured", "structCO__t.html#a139e71d4b3c9f2c072df13e6ac0dbac4", null ], + [ "config", "structCO__t.html#a522a9a56f1256dac8cd305313b3913f1", null ], + [ "CANmodule", "structCO__t.html#a65bea9a6028917db3b0b3a95a60aea18", null ], + [ "CANrx", "structCO__t.html#a9665dd795f4c5d26910eb1c5b06503e5", null ], + [ "CANtx", "structCO__t.html#a990b6bd828bc23b455887b5a72f79dca", null ], + [ "CNT_ALL_RX_MSGS", "structCO__t.html#a5b9bea68bd5c63af52819c3c4cdb7592", null ], + [ "CNT_ALL_TX_MSGS", "structCO__t.html#a9e9e7548a182ed9d4aad12e6a41be495", null ], + [ "NMT", "structCO__t.html#a6fddf777eec75e0cc8fc510a5c4ec8e5", null ], + [ "RX_IDX_NMT_SLV", "structCO__t.html#af693786fa6c0923d4c934d09a00820d3", null ], + [ "TX_IDX_NMT_MST", "structCO__t.html#a77bf70da8db7ecfe78f3720bfca12101", null ], + [ "TX_IDX_HB_PROD", "structCO__t.html#ad2718c137526676467853885d286fc74", null ], + [ "HBcons", "structCO__t.html#a5eea4e2b8390e1f0ec531248e229cd72", null ], + [ "RX_IDX_HB_CONS", "structCO__t.html#a788f815f4d31a4aa11d171114173573e", null ], + [ "em", "structCO__t.html#a6e4c80d975a5a2207530b72bdbf530b7", null ], + [ "RX_IDX_EM_CONS", "structCO__t.html#a47df99673e8da6e48c6c17cbf204c16d", null ], + [ "TX_IDX_EM_PROD", "structCO__t.html#a3d2f250c5c3bc972ce45418d22e79caa", null ], + [ "SDOserver", "structCO__t.html#aa9fe002954a6b902e594fc47a9f1917b", null ], + [ "RX_IDX_SDO_SRV", "structCO__t.html#a8e63b9c217cf4cd7fc8fbc0900bdb01c", null ], + [ "TX_IDX_SDO_SRV", "structCO__t.html#a9287e4d62f23f4222fc90c4a6ded2ef0", null ], + [ "SDOclient", "structCO__t.html#ae9548e1a85b750b039dcc12bd43bd972", null ], + [ "RX_IDX_SDO_CLI", "structCO__t.html#a85a2007d87b4fb3608859e251cf8653e", null ], + [ "TX_IDX_SDO_CLI", "structCO__t.html#adc86b044f08602394cbb4346b41a9a45", null ], + [ "TIME", "structCO__t.html#a80052c25c47be2460e02cf88ff91bbb7", null ], + [ "RX_IDX_TIME", "structCO__t.html#a039a63c4b10f91814775f9d1cef43aad", null ], + [ "TX_IDX_TIME", "structCO__t.html#ad5cbb43e75d99c5479efb3eddd36ec3d", null ], + [ "SYNC", "structCO__t.html#a9c704cab995029fd216076523c70d283", null ], + [ "RX_IDX_SYNC", "structCO__t.html#a72a378c1e806978f8399bf3f3af7c002", null ], + [ "TX_IDX_SYNC", "structCO__t.html#a2eb3962f37ce7a4b51d8779f96db58bc", null ], + [ "RPDO", "structCO__t.html#a332fa4512032d94a20af69b5a103e017", null ], + [ "RX_IDX_RPDO", "structCO__t.html#a7a01e7792c600f5dc1eb2e7676e1e01d", null ], + [ "TPDO", "structCO__t.html#ab5027ca1447bf64e5e93935c1e5466c2", null ], + [ "TX_IDX_TPDO", "structCO__t.html#a9451781c713680d8bab86cda2a1ca570", null ], + [ "LEDs", "structCO__t.html#a4837a9caa7daa219a8ecff5c64bc08ba", null ], + [ "GFC", "structCO__t.html#a9c6e7b29436b05c8b659502c6fae2a6a", null ], + [ "RX_IDX_GFC", "structCO__t.html#ae689666dc05116ce0694d5ea505a2e79", null ], + [ "TX_IDX_GFC", "structCO__t.html#a709e86d5401484bb7d0c5f17e824636d", null ], + [ "SRDOGuard", "structCO__t.html#a007c96db5b54e91145571feb06e9e683", null ], + [ "SRDO", "structCO__t.html#a2a65c843c89da67fc049aa285e03243c", null ], + [ "RX_IDX_SRDO", "structCO__t.html#a8fec2730008c19f6fbc6192937776739", null ], + [ "TX_IDX_SRDO", "structCO__t.html#a3dbc89e76d1627f2b360ddaefd7a7b51", null ], + [ "LSSslave", "structCO__t.html#a0c6ecabcb18ed2bd9eb881f9156482b6", null ], + [ "RX_IDX_LSS_SLV", "structCO__t.html#a2c230c531fc2a2a09ad33edef582041e", null ], + [ "TX_IDX_LSS_SLV", "structCO__t.html#a7d1159efd4a31d0701c497d25343073d", null ], + [ "LSSmaster", "structCO__t.html#af586261baf229fe624fb72b712208c86", null ], + [ "RX_IDX_LSS_MST", "structCO__t.html#aa0c0c6b387e406c67d3b988a744b3b36", null ], + [ "TX_IDX_LSS_MST", "structCO__t.html#a38e102b879d1dd04f69bacac56266fdf", null ], + [ "gtwa", "structCO__t.html#afa0e937046492a26af9bb5e03c3aab94", null ], + [ "trace", "structCO__t.html#a4f7a05e49ea89178cb61e14c4c4575f1", null ] + ] ], + [ "CO_MULTIPLE_OD", "group__CO__CANopen.html#gae3927c69d6937caf2b67ac49c5e41982", null ], + [ "CO_USE_GLOBALS", "group__CO__CANopen.html#gaab00eab90dbe59885c98831e2d819e56", null ], + [ "CO_new", "group__CO__CANopen.html#gaf6ef29daa2063de90b4799ae795c7027", null ], + [ "CO_delete", "group__CO__CANopen.html#ga7023592c26cf6649aa67bc2c04dfd95d", null ], + [ "CO_isLSSslaveEnabled", "group__CO__CANopen.html#ga4c9143f717e8df279034fb897e39b517", null ], + [ "CO_CANinit", "group__CO__CANopen.html#ga619d9ee70c17464bb819b48b5eddb074", null ], + [ "CO_LSSinit", "group__CO__CANopen.html#ga0e11332fd28af597bc47b21327cdc8a3", null ], + [ "CO_CANopenInit", "group__CO__CANopen.html#ga0b64a860299af6e96f5663419aa6d446", null ], + [ "CO_process", "group__CO__CANopen.html#ga895d7fad40b60aacdac3cb0615729b5e", null ], + [ "CO_process_SYNC", "group__CO__CANopen.html#gaad5c15c3ca475912661f512d37413b12", null ], + [ "CO_process_RPDO", "group__CO__CANopen.html#ga4318848921c35e8bb5a7d97dca5668a0", null ], + [ "CO_process_TPDO", "group__CO__CANopen.html#ga8c62a2afd2762d99e9c9be13a3d9a7a8", null ], + [ "CO_process_SRDO", "group__CO__CANopen.html#gab76d7283fe5190d3a0009b423a9ba8b1", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__CANopen__301.html b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__CANopen__301.html new file mode 100755 index 0000000..d1aff57 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__CANopen__301.html @@ -0,0 +1,145 @@ + + + + + + + +CANopenNode: CANopen_301 + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Modules
+
+
CANopen_301
+
+
+ +

CANopen application layer and communication profile (CiA 301 v4.2.0) +More...

+ + + + + + + + + + + + + + + + + + + + + + + + + + +

+Modules

 Driver
 
 Emergency
 
 FIFO circular buffer
 
 Heartbeat consumer
 
 NMT and Heartbeat
 
 OD interface
 
 PDO
 
 SDO client
 
 SDO server
 
 SYNC
 
 TIME
 
 CRC 16 CCITT
 
+

Detailed Description

+

CANopen application layer and communication profile (CiA 301 v4.2.0)

+

Definitions of data types, encoding rules, object dictionary objects and CANopen communication services and protocols.

+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__CANopen__301.js b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__CANopen__301.js new file mode 100755 index 0000000..5917ad2 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__CANopen__301.js @@ -0,0 +1,15 @@ +var group__CO__CANopen__301 = +[ + [ "Driver", "group__CO__driver.html", "group__CO__driver" ], + [ "Emergency", "group__CO__Emergency.html", "group__CO__Emergency" ], + [ "FIFO circular buffer", "group__CO__CANopen__301__fifo.html", "group__CO__CANopen__301__fifo" ], + [ "Heartbeat consumer", "group__CO__HBconsumer.html", "group__CO__HBconsumer" ], + [ "NMT and Heartbeat", "group__CO__NMT__Heartbeat.html", "group__CO__NMT__Heartbeat" ], + [ "OD interface", "group__CO__ODinterface.html", "group__CO__ODinterface" ], + [ "PDO", "group__CO__PDO.html", "group__CO__PDO" ], + [ "SDO client", "group__CO__SDOclient.html", "group__CO__SDOclient" ], + [ "SDO server", "group__CO__SDOserver.html", "group__CO__SDOserver" ], + [ "SYNC", "group__CO__SYNC.html", "group__CO__SYNC" ], + [ "TIME", "group__CO__TIME.html", "group__CO__TIME" ], + [ "CRC 16 CCITT", "group__CO__crc16__ccitt.html", "group__CO__crc16__ccitt" ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__CANopen__301__fifo.html b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__CANopen__301__fifo.html new file mode 100755 index 0000000..6b48b43 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__CANopen__301__fifo.html @@ -0,0 +1,1379 @@ + + + + + + + +CANopenNode: FIFO circular buffer + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Files | +Data Structures | +Enumerations | +Functions
+
+
FIFO circular buffer
CANopen_301
+
+
+ + + + + +

+Files

file  CO_fifo.h
 FIFO circular buffer.
 
+ + + + +

+Data Structures

struct  CO_fifo_t
 Fifo object. More...
 
+ + + + +

+Enumerations

enum  CO_fifo_st {
+  CO_fifo_st_closed = 0x01U, +CO_fifo_st_partial = 0x02U, +CO_fifo_st_errTok = 0x10U, +CO_fifo_st_errVal = 0x20U, +
+  CO_fifo_st_errBuf = 0x40U, +CO_fifo_st_errInt = 0x80U, +CO_fifo_st_errMask = 0xF0U +
+ }
 Bitfields for status argument from CO_fifo_cpyTok2U8 function and similar. More...
 
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

+Functions

void CO_fifo_init (CO_fifo_t *fifo, char *buf, size_t bufSize)
 Initialize fifo object. More...
 
static void CO_fifo_reset (CO_fifo_t *fifo)
 Reset fifo object, make it empty. More...
 
static bool_t CO_fifo_purge (CO_fifo_t *fifo)
 Purge all data in fifo object, keep other properties. More...
 
static size_t CO_fifo_getSpace (CO_fifo_t *fifo)
 Get free buffer space in CO_fifo_t object. More...
 
static size_t CO_fifo_getOccupied (CO_fifo_t *fifo)
 Get size of data inside CO_fifo_t buffer object. More...
 
static bool_t CO_fifo_putc (CO_fifo_t *fifo, const char c)
 Put one character into CO_fifo_t buffer object. More...
 
static void CO_fifo_putc_ov (CO_fifo_t *fifo, const char c)
 Put one character into CO_fifo_t buffer object. More...
 
static bool_t CO_fifo_getc (CO_fifo_t *fifo, char *buf)
 Get one character from CO_fifo_t buffer object. More...
 
size_t CO_fifo_write (CO_fifo_t *fifo, const char *buf, size_t count, uint16_t *crc)
 Write data into CO_fifo_t object. More...
 
size_t CO_fifo_read (CO_fifo_t *fifo, char *buf, size_t count, bool_t *eof)
 Read data from CO_fifo_t object. More...
 
size_t CO_fifo_altBegin (CO_fifo_t *fifo, size_t offset)
 Initializes alternate read with CO_fifo_altRead. More...
 
void CO_fifo_altFinish (CO_fifo_t *fifo, uint16_t *crc)
 Ends alternate read with CO_fifo_altRead and calculate crc checksum. More...
 
static size_t CO_fifo_altGetOccupied (CO_fifo_t *fifo)
 Get alternate size of remaining data, see CO_fifo_altRead. More...
 
size_t CO_fifo_altRead (CO_fifo_t *fifo, char *buf, size_t count)
 Alternate read data from CO_fifo_t object. More...
 
bool_t CO_fifo_CommSearch (CO_fifo_t *fifo, bool_t clear)
 Search command inside FIFO. More...
 
bool_t CO_fifo_trimSpaces (CO_fifo_t *fifo, bool_t *insideComment)
 Trim spaces inside FIFO. More...
 
size_t CO_fifo_readToken (CO_fifo_t *fifo, char *buf, size_t count, char *closed, bool_t *err)
 Get token from FIFO buffer. More...
 
size_t CO_fifo_readU82a (CO_fifo_t *fifo, char *buf, size_t count, bool_t end)
 Read uint8_t variable from fifo and output as ascii string. More...
 
+size_t CO_fifo_readU162a (CO_fifo_t *fifo, char *buf, size_t count, bool_t end)
 Read uint16_t variable from fifo as ascii string, see CO_fifo_readU82a.
 
+size_t CO_fifo_readU322a (CO_fifo_t *fifo, char *buf, size_t count, bool_t end)
 Read uint32_t variable from fifo as ascii string, see CO_fifo_readU82a.
 
+size_t CO_fifo_readU642a (CO_fifo_t *fifo, char *buf, size_t count, bool_t end)
 Read uint64_t variable from fifo as ascii string, see CO_fifo_readU82a.
 
+size_t CO_fifo_readX82a (CO_fifo_t *fifo, char *buf, size_t count, bool_t end)
 Read uint8_t variable from fifo as ascii string, see CO_fifo_readU82a.
 
+size_t CO_fifo_readX162a (CO_fifo_t *fifo, char *buf, size_t count, bool_t end)
 Read uint16_t variable from fifo as ascii string, see CO_fifo_readU82a.
 
+size_t CO_fifo_readX322a (CO_fifo_t *fifo, char *buf, size_t count, bool_t end)
 Read uint32_t variable from fifo as ascii string, see CO_fifo_readU82a.
 
+size_t CO_fifo_readX642a (CO_fifo_t *fifo, char *buf, size_t count, bool_t end)
 Read uint64_t variable from fifo as ascii string, see CO_fifo_readU82a.
 
+size_t CO_fifo_readI82a (CO_fifo_t *fifo, char *buf, size_t count, bool_t end)
 Read int8_t variable from fifo as ascii string, see CO_fifo_readU82a.
 
+size_t CO_fifo_readI162a (CO_fifo_t *fifo, char *buf, size_t count, bool_t end)
 Read int16_t variable from fifo as ascii string, see CO_fifo_readU82a.
 
+size_t CO_fifo_readI322a (CO_fifo_t *fifo, char *buf, size_t count, bool_t end)
 Read int32_t variable from fifo as ascii string, see CO_fifo_readU82a.
 
+size_t CO_fifo_readI642a (CO_fifo_t *fifo, char *buf, size_t count, bool_t end)
 Read int64_t variable from fifo as ascii string, see CO_fifo_readU82a.
 
+size_t CO_fifo_readR322a (CO_fifo_t *fifo, char *buf, size_t count, bool_t end)
 Read float32_t variable from fifo as ascii string, see CO_fifo_readU82a.
 
+size_t CO_fifo_readR642a (CO_fifo_t *fifo, char *buf, size_t count, bool_t end)
 Read float64_t variable from fifo as ascii string, see CO_fifo_readU82a.
 
+size_t CO_fifo_readHex2a (CO_fifo_t *fifo, char *buf, size_t count, bool_t end)
 Read data from fifo and output as space separated two digit ascii string, see also CO_fifo_readU82a.
 
size_t CO_fifo_readVs2a (CO_fifo_t *fifo, char *buf, size_t count, bool_t end)
 Read data from fifo and output as visible string. More...
 
size_t CO_fifo_readB642a (CO_fifo_t *fifo, char *buf, size_t count, bool_t end)
 Read data from fifo and output as mime-base64 encoded string. More...
 
size_t CO_fifo_cpyTok2U8 (CO_fifo_t *dest, CO_fifo_t *src, CO_fifo_st *status)
 Read ascii string from src fifo and copy as uint8_t variable to dest fifo. More...
 
+size_t CO_fifo_cpyTok2U16 (CO_fifo_t *dest, CO_fifo_t *src, CO_fifo_st *status)
 Copy ascii string to uint16_t variable, see CO_fifo_cpyTok2U8.
 
+size_t CO_fifo_cpyTok2U32 (CO_fifo_t *dest, CO_fifo_t *src, CO_fifo_st *status)
 Copy ascii string to uint32_t variable, see CO_fifo_cpyTok2U8.
 
+size_t CO_fifo_cpyTok2U64 (CO_fifo_t *dest, CO_fifo_t *src, CO_fifo_st *status)
 Copy ascii string to uint64_t variable, see CO_fifo_cpyTok2U8.
 
+size_t CO_fifo_cpyTok2I8 (CO_fifo_t *dest, CO_fifo_t *src, CO_fifo_st *status)
 Copy ascii string to int8_t variable, see CO_fifo_cpyTok2U8.
 
+size_t CO_fifo_cpyTok2I16 (CO_fifo_t *dest, CO_fifo_t *src, CO_fifo_st *status)
 Copy ascii string to int16_t variable, see CO_fifo_cpyTok2U8.
 
+size_t CO_fifo_cpyTok2I32 (CO_fifo_t *dest, CO_fifo_t *src, CO_fifo_st *status)
 Copy ascii string to int32_t variable, see CO_fifo_cpyTok2U8.
 
+size_t CO_fifo_cpyTok2I64 (CO_fifo_t *dest, CO_fifo_t *src, CO_fifo_st *status)
 Copy ascii string to int64_t variable, see CO_fifo_cpyTok2U8.
 
+size_t CO_fifo_cpyTok2R32 (CO_fifo_t *dest, CO_fifo_t *src, CO_fifo_st *status)
 Copy ascii string to float32_t variable, see CO_fifo_cpyTok2U8.
 
+size_t CO_fifo_cpyTok2R64 (CO_fifo_t *dest, CO_fifo_t *src, CO_fifo_st *status)
 Copy ascii string to float64_t variable, see CO_fifo_cpyTok2U8.
 
size_t CO_fifo_cpyTok2Hex (CO_fifo_t *dest, CO_fifo_t *src, CO_fifo_st *status)
 Copy bytes written as two hex digits into to data. More...
 
size_t CO_fifo_cpyTok2Vs (CO_fifo_t *dest, CO_fifo_t *src, CO_fifo_st *status)
 Copy visible string to data. More...
 
size_t CO_fifo_cpyTok2B64 (CO_fifo_t *dest, CO_fifo_t *src, CO_fifo_st *status)
 Read ascii mime-base64 encoded string from src fifo and copy as binary data to dest fifo. More...
 
+

Detailed Description

+

FIFO is organized as circular buffer with predefined capacity. It must be initialized by CO_fifo_init(). Functions are not not thread safe.

+

It can be used as general purpose FIFO circular buffer for any data. Data can be written by CO_fifo_write() and read by CO_fifo_read() functions.

+

Buffer has additional functions for usage with CiA309-3 standard. It acts as circular buffer for storing ascii commands and fetching tokens from them.

+

Enumeration Type Documentation

+ +

◆ CO_fifo_st

+ +
+
+ + + + +
enum CO_fifo_st
+
+ +

Bitfields for status argument from CO_fifo_cpyTok2U8 function and similar.

+ + + + + + + + +
Enumerator
CO_fifo_st_closed 

Bit is set, if command delimiter is reached in src.

+
CO_fifo_st_partial 

Bit is set, if copy was partial and more data are available.

+

If unset and no error, then all data was successfully copied.

+
CO_fifo_st_errTok 

Bit is set, if no valid token found.

+
CO_fifo_st_errVal 

Bit is set, if value is not valid or out of limits.

+
CO_fifo_st_errBuf 

Bit is set, if destination buffer is to small.

+
CO_fifo_st_errInt 

Bit is set, if internal error.

+
CO_fifo_st_errMask 

Bitmask for error bits.

+
+ +
+
+

Function Documentation

+ +

◆ CO_fifo_init()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + +
void CO_fifo_init (CO_fifo_t * fifo,
char * buf,
size_t bufSize 
)
+
+ +

Initialize fifo object.

+
Parameters
+ + + + +
fifoThis object will be initialized
bufPointer to externally defined buffer
bufSizeSize of the above buffer. Usable size of the buffer will be one byte less than bufSize, it is used for operation of the circular buffer.
+
+
+ +
+
+ +

◆ CO_fifo_reset()

+ +
+
+ + + + + +
+ + + + + + + + +
static void CO_fifo_reset (CO_fifo_t * fifo)
+ 		+inlinestatic
+
+ +

Reset fifo object, make it empty.

+
Parameters
+ + +
fifoThis object
+
+
+ +
+
+ +

◆ CO_fifo_purge()

+ +
+
+ + + + + +
+ + + + + + + + +
static bool_t CO_fifo_purge (CO_fifo_t * fifo)
+ 		+inlinestatic
+
+ +

Purge all data in fifo object, keep other properties.

+
Parameters
+ + +
fifoThis object
+
+
+
Returns
true, if data were purged or false, if fifo were already empty
+ +
+
+ +

◆ CO_fifo_getSpace()

+ +
+
+ + + + + +
+ + + + + + + + +
static size_t CO_fifo_getSpace (CO_fifo_t * fifo)
+ 		+inlinestatic
+
+ +

Get free buffer space in CO_fifo_t object.

+
Parameters
+ + +
fifoThis object
+
+
+
Returns
number of available bytes
+ +
+
+ +

◆ CO_fifo_getOccupied()

+ +
+
+ + + + + +
+ + + + + + + + +
static size_t CO_fifo_getOccupied (CO_fifo_t * fifo)
+ 		+inlinestatic
+
+ +

Get size of data inside CO_fifo_t buffer object.

+
Parameters
+ + +
fifoThis object
+
+
+
Returns
number of occupied bytes
+ +
+
+ +

◆ CO_fifo_putc()

+ +
+
+ + + + + +
+ + + + + + + + + + + + + + + + + + +
static bool_t CO_fifo_putc (CO_fifo_t * fifo,
const char c 
)
+ 		+inlinestatic
+
+ +

Put one character into CO_fifo_t buffer object.

+
Parameters
+ + + +
fifoThis object
cCharacter to put
+
+
+
Returns
true, if write was successful (enough space in fifo buffer)
+ +
+
+ +

◆ CO_fifo_putc_ov()

+ +
+
+ + + + + +
+ + + + + + + + + + + + + + + + + + +
static void CO_fifo_putc_ov (CO_fifo_t * fifo,
const char c 
)
+ 		+inlinestatic
+
+ +

Put one character into CO_fifo_t buffer object.

+

Overwrite old characters, if run out of space

+
Parameters
+ + + +
fifoThis object
cCharacter to put
+
+
+ +
+
+ +

◆ CO_fifo_getc()

+ +
+
+ + + + + +
+ + + + + + + + + + + + + + + + + + +
static bool_t CO_fifo_getc (CO_fifo_t * fifo,
char * buf 
)
+ 		+inlinestatic
+
+ +

Get one character from CO_fifo_t buffer object.

+
Parameters
+ + + +
fifoThis object
bufBuffer of length one byte, where character will be copied
+
+
+
Returns
true, if read was successful (non-empty fifo buffer)
+ +
+
+ +

◆ CO_fifo_write()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
size_t CO_fifo_write (CO_fifo_t * fifo,
const char * buf,
size_t count,
uint16_t * crc 
)
+
+ +

Write data into CO_fifo_t object.

+

This function copies data from buf into internal buffer of CO_fifo_t object. Function returns number of bytes successfully copied. If there is not enough space in destination, not all bytes will be copied.

+
Parameters
+ + + + + +
fifoThis object
bufBuffer which will be copied
countNumber of bytes in buf
[in,out]crcExternally defined variable for CRC checksum, ignored if NULL
+
+
+
Returns
number of bytes actually written.
+ +
+
+ +

◆ CO_fifo_read()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
size_t CO_fifo_read (CO_fifo_t * fifo,
char * buf,
size_t count,
bool_t * eof 
)
+
+ +

Read data from CO_fifo_t object.

+

This function copies data from internal buffer of CO_fifo_t object into buf. Function returns number of bytes successfully copied. Function also writes true into eof argument, if command delimiter character is reached.

+
Parameters
+ + + + + +
fifoThis object
bufBuffer into which data will be copied
countCopy up to count bytes into buffer
[out]eofIf different than NULL, then search for delimiter character. If found, then read up to (including) that character and set *eof to true. Otherwise set *eof to false.
+
+
+
Returns
number of bytes actually read.
+ +
+
+ +

◆ CO_fifo_altBegin()

+ +
+
+ + + + + + + + + + + + + + + + + + +
size_t CO_fifo_altBegin (CO_fifo_t * fifo,
size_t offset 
)
+
+ +

Initializes alternate read with CO_fifo_altRead.

+
Parameters
+ + + +
fifoThis object
offsetOffset in bytes from original read pointer
+
+
+
Returns
same as offset or lower, if there is not enough data.
+ +
+
+ +

◆ CO_fifo_altFinish()

+ +
+
+ + + + + + + + + + + + + + + + + + +
void CO_fifo_altFinish (CO_fifo_t * fifo,
uint16_t * crc 
)
+
+ +

Ends alternate read with CO_fifo_altRead and calculate crc checksum.

+
Parameters
+ + + +
fifoThis object
[in,out]crcExternally defined variable for CRC checksum, ignored if NULL
+
+
+ +
+
+ +

◆ CO_fifo_altGetOccupied()

+ +
+
+ + + + + +
+ + + + + + + + +
static size_t CO_fifo_altGetOccupied (CO_fifo_t * fifo)
+ 		+inlinestatic
+
+ +

Get alternate size of remaining data, see CO_fifo_altRead.

+
Parameters
+ + +
fifoThis object
+
+
+
Returns
number of occupied bytes.
+ +
+
+ +

◆ CO_fifo_altRead()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + +
size_t CO_fifo_altRead (CO_fifo_t * fifo,
char * buf,
size_t count 
)
+
+ +

Alternate read data from CO_fifo_t object.

+

This function is similar as CO_fifo_read(), but uses alternate read pointer inside circular buffer. It reads data from the buffer and data remains in it. This function uses alternate read pointer and keeps original read pointer unchanged. Before using this function, alternate read pointer must be initialized with CO_fifo_altBegin(). CO_fifo_altFinish() sets original read pointer to alternate read pointer and so empties the buffer.

+
Parameters
+ + + + +
fifoThis object
bufBuffer into which data will be copied
countCopy up to count bytes into buffer
+
+
+
Returns
number of bytes actually read.
+ +
+
+ +

◆ CO_fifo_CommSearch()

+ +
+
+ + + + + + + + + + + + + + + + + + +
bool_t CO_fifo_CommSearch (CO_fifo_t * fifo,
bool_t clear 
)
+
+ +

Search command inside FIFO.

+

If there are some data inside the FIFO, then search for command delimiter.

+

If command is long, then in the buffer may not be enough space for it. In that case buffer is full and no delimiter is present. Function then returns true and command should be processed for the starting tokens. Buffer can later be refilled multiple times, until command is closed by command delimiter.

+

If this function returns different than 0, then buffer is usually read by multiple CO_fifo_readToken() calls. If reads was successful, then delimiter is reached and fifo->readPtr is set after the command. If any CO_fifo_readToken() returns nonzero *err, then there is an error and command should be cleared. All this procedure must be implemented inside single function call.

+
Parameters
+ + + +
fifoThis object.
clearIf true, then command will be cleared from the buffer. If there is no delimiter, buffer will be cleared entirely.
+
+
+
Returns
true if command with delimiter found or buffer full.
+ +
+
+ +

◆ CO_fifo_trimSpaces()

+ +
+
+ + + + + + + + + + + + + + + + + + +
bool_t CO_fifo_trimSpaces (CO_fifo_t * fifo,
bool_t * insideComment 
)
+
+ +

Trim spaces inside FIFO.

+

Function removes all non graphical characters and comments from fifo buffer. It stops on first graphical character or on command delimiter (later is also removed).

+
Parameters
+ + + +
fifoThis object.
[in,out]insideCommentif set to true as input, it skips all characters and searches only for delimiter. As output it is set to true, if fifo is empty, is inside comment and command delimiter is not found.
+
+
+
Returns
true if command delimiter was found.
+ +
+
+ +

◆ CO_fifo_readToken()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
size_t CO_fifo_readToken (CO_fifo_t * fifo,
char * buf,
size_t count,
char * closed,
bool_t * err 
)
+
+ +

Get token from FIFO buffer.

+

Function search FIFO buffer for token. Token is string of only graphical characters. Graphical character is any printable character except space with acsii code within limits: 0x20 < code < 0x7F (see isgraph() function).

+

If token is found, then copy it to the buf, if count is large enough. On success also set readPtr to point to the next graphical character.

+

Each token must have at least one empty space after it (space, command delimiter, '\0', etc.). Delimiter must not be graphical character.

+

If comment delimiter (delimComment, see CO_fifo_init) character is found, then all string till command delimiter (delimCommand, see CO_fifo_init) will be erased from the buffer.

+

See also CO_fifo_CommSearch().

+
Parameters
+ + + + + + +
fifoThis object.
bufBuffer into which data will be copied. Will be terminated by '\0'.
countCopy up to count bytes into buffer
[in,out]closedThis is input/output variable. Not used if NULL.
    +
  • As output variable it is set to 1, if command delimiter (delimCommand, see CO_fifo_init) is found after the token and set to 0 otherwise.
    • +
  • As input variable it is used for verifying error condition:
      +
    • *closed = 0: Set *err to true if token is empty or command delimiter is found.
      • +
    • *closed = 1: Set *err to true if token is empty or command delimiter is NOT found.
      • +
    • *closed = any other value: No checking of token size or command delimiter.
      • +
    +
    • +
+
[out]errIf not NULL, it is set to true if token is larger than buf or in matching combination in 'closed' argument. If it is already true, then function returns immediately.
+
+
+
Returns
Number of bytes read.
+ +
+
+ +

◆ CO_fifo_readU82a()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
size_t CO_fifo_readU82a (CO_fifo_t * fifo,
char * buf,
size_t count,
bool_t end 
)
+
+ +

Read uint8_t variable from fifo and output as ascii string.

+
Parameters
+ + + + + +
fifoThis object.
bufBuffer into which ascii string will be copied.
countAvailable count of bytes inside the buf.
endTrue indicates, that fifo contains last bytes of data.
+
+
+
Returns
Number of ascii bytes written into buf.
+ +
+
+ +

◆ CO_fifo_readVs2a()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
size_t CO_fifo_readVs2a (CO_fifo_t * fifo,
char * buf,
size_t count,
bool_t end 
)
+
+ +

Read data from fifo and output as visible string.

+

A visible string is enclosed with double quotes. If a double quote is used within the string, the quotes are escaped by a second quotes, e.g. “Hello “”World””, CANopen is great”. UTF-8 characters and also line breaks works with this function. Function removes all NULL and CR characters from output string. See also CO_fifo_readU82a

+ +
+
+ +

◆ CO_fifo_readB642a()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
size_t CO_fifo_readB642a (CO_fifo_t * fifo,
char * buf,
size_t count,
bool_t end 
)
+
+ +

Read data from fifo and output as mime-base64 encoded string.

+

Encoding is as specified in RFC 2045, without CR-LF, but one long string. See also CO_fifo_readU82a

+ +
+
+ +

◆ CO_fifo_cpyTok2U8()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + +
size_t CO_fifo_cpyTok2U8 (CO_fifo_t * dest,
CO_fifo_t * src,
CO_fifo_st * status 
)
+
+ +

Read ascii string from src fifo and copy as uint8_t variable to dest fifo.

+
Parameters
+ + + + +
destdestination fifo buffer object.
srcsource fifo buffer object.
[out]statusbitfield of the CO_fifo_st type.
+
+
+
Returns
Number of bytes written into dest.
+ +
+
+ +

◆ CO_fifo_cpyTok2Hex()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + +
size_t CO_fifo_cpyTok2Hex (CO_fifo_t * dest,
CO_fifo_t * src,
CO_fifo_st * status 
)
+
+ +

Copy bytes written as two hex digits into to data.

+

Bytes may be space separated. See CO_fifo_cpyTok2U8 for parameters.

+ +
+
+ +

◆ CO_fifo_cpyTok2Vs()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + +
size_t CO_fifo_cpyTok2Vs (CO_fifo_t * dest,
CO_fifo_t * src,
CO_fifo_st * status 
)
+
+ +

Copy visible string to data.

+

A visible string must be enclosed with double quotes, if it contains space. If a double quote is used within the string, the quotes are escaped by a second quotes. Input string can not contain newline characters. See CO_fifo_cpyTok2U8

+ +
+
+ +

◆ CO_fifo_cpyTok2B64()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + +
size_t CO_fifo_cpyTok2B64 (CO_fifo_t * dest,
CO_fifo_t * src,
CO_fifo_st * status 
)
+
+ +

Read ascii mime-base64 encoded string from src fifo and copy as binary data to dest fifo.

+

Encoding is as specified in RFC 2045, without CR-LF, but one long string in single line. See also CO_fifo_readU82a

+ +
+
+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__CANopen__301__fifo.js b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__CANopen__301__fifo.js new file mode 100755 index 0000000..5a4d50b --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__CANopen__301__fifo.js @@ -0,0 +1,69 @@ +var group__CO__CANopen__301__fifo = +[ + [ "CO_fifo.h", "CO__fifo_8h.html", null ], + [ "CO_fifo_t", "structCO__fifo__t.html", [ + [ "buf", "structCO__fifo__t.html#aa4a8bae66b107809c099cd1d2de5c966", null ], + [ "bufSize", "structCO__fifo__t.html#a35d9f2fcb8d2ef00794cb305d8344e61", null ], + [ "writePtr", "structCO__fifo__t.html#a540fbc52344d1205de11625cd81f351d", null ], + [ "readPtr", "structCO__fifo__t.html#a4b56b7d217aa2b02eaf1b3592c5c26e6", null ], + [ "altReadPtr", "structCO__fifo__t.html#a4f8eadd2e9b966ce21274cbbceb3adbe", null ], + [ "started", "structCO__fifo__t.html#ab754d03636ff0ef17950d4167429a77f", null ], + [ "aux", "structCO__fifo__t.html#aa255bcb00601a8f4225c97ad6cd854a7", null ] + ] ], + [ "CO_fifo_st", "group__CO__CANopen__301__fifo.html#ga2c7db7d527e4055a5dde62b74dfc2818", [ + [ "CO_fifo_st_closed", "group__CO__CANopen__301__fifo.html#gga2c7db7d527e4055a5dde62b74dfc2818a1f111c3f5a9396da6ced33132a2341f9", null ], + [ "CO_fifo_st_partial", "group__CO__CANopen__301__fifo.html#gga2c7db7d527e4055a5dde62b74dfc2818a7bf88ff5b78ef6a25a4391bc6423c1f7", null ], + [ "CO_fifo_st_errTok", "group__CO__CANopen__301__fifo.html#gga2c7db7d527e4055a5dde62b74dfc2818a70060b411fcbfa0696ca453d7f4348d4", null ], + [ "CO_fifo_st_errVal", "group__CO__CANopen__301__fifo.html#gga2c7db7d527e4055a5dde62b74dfc2818a9c41b295a487450017cc3b1ce0bcedfb", null ], + [ "CO_fifo_st_errBuf", "group__CO__CANopen__301__fifo.html#gga2c7db7d527e4055a5dde62b74dfc2818a51c064e77b0e2bf3741719b57de65141", null ], + [ "CO_fifo_st_errInt", "group__CO__CANopen__301__fifo.html#gga2c7db7d527e4055a5dde62b74dfc2818a07a7d8d483365e3afdcf9dc5b2dd2143", null ], + [ "CO_fifo_st_errMask", "group__CO__CANopen__301__fifo.html#gga2c7db7d527e4055a5dde62b74dfc2818a9e587fa1d802fbe939f6da9d44aba389", null ] + ] ], + [ "CO_fifo_init", "group__CO__CANopen__301__fifo.html#ga44e8f83feb2dd463b2fbb399dcd4de4d", null ], + [ "CO_fifo_reset", "group__CO__CANopen__301__fifo.html#ga6817a4baeaee87a578291e9de820b126", null ], + [ "CO_fifo_purge", "group__CO__CANopen__301__fifo.html#ga15a27a4871e680475f680c574050844a", null ], + [ "CO_fifo_getSpace", "group__CO__CANopen__301__fifo.html#ga0d456e83af18cce9db9157b5a30fac21", null ], + [ "CO_fifo_getOccupied", "group__CO__CANopen__301__fifo.html#ga40ef4fc5dc184ef3c0c2f37eca1bc507", null ], + [ "CO_fifo_putc", "group__CO__CANopen__301__fifo.html#ga7c2deb5abee499ad2f1b5175a205bc5b", null ], + [ "CO_fifo_putc_ov", "group__CO__CANopen__301__fifo.html#gae9351e31e5d74e738fe5e1514f99f2dd", null ], + [ "CO_fifo_getc", "group__CO__CANopen__301__fifo.html#ga4c1237aa63123d0ab8e9e16132a5c40a", null ], + [ "CO_fifo_write", "group__CO__CANopen__301__fifo.html#ga715cb5e1feacd2f3af5bc8195bbe69d3", null ], + [ "CO_fifo_read", "group__CO__CANopen__301__fifo.html#gad0ba8be2a601030a374913e4fa94e6cb", null ], + [ "CO_fifo_altBegin", "group__CO__CANopen__301__fifo.html#ga55c186098b2d860b116ddeb637a27b5a", null ], + [ "CO_fifo_altFinish", "group__CO__CANopen__301__fifo.html#ga7c534dd50959814b40c3027aa85f1c55", null ], + [ "CO_fifo_altGetOccupied", "group__CO__CANopen__301__fifo.html#ga2ee45a0cab19a212022d82987de2a955", null ], + [ "CO_fifo_altRead", "group__CO__CANopen__301__fifo.html#gaa28339101c4ac5a2c44004276075c759", null ], + [ "CO_fifo_CommSearch", "group__CO__CANopen__301__fifo.html#ga0dbef46e369e659bab7e29742971134c", null ], + [ "CO_fifo_trimSpaces", "group__CO__CANopen__301__fifo.html#gac01a59d65d275bd9f9a7fb0ea04fb915", null ], + [ "CO_fifo_readToken", "group__CO__CANopen__301__fifo.html#ga87a199708c95d3ca02e2fcc4ca4a6319", null ], + [ "CO_fifo_readU82a", "group__CO__CANopen__301__fifo.html#ga50b3075a0cfab3ab8608a4ba4977ecd6", null ], + [ "CO_fifo_readU162a", "group__CO__CANopen__301__fifo.html#ga99072bd50ad3b92c291136e458885ca2", null ], + [ "CO_fifo_readU322a", "group__CO__CANopen__301__fifo.html#ga9a2d7d07570a8f552a81e8c72327f43c", null ], + [ "CO_fifo_readU642a", "group__CO__CANopen__301__fifo.html#ga8805e75a32e9f8672259f2274816a0b8", null ], + [ "CO_fifo_readX82a", "group__CO__CANopen__301__fifo.html#ga7ed37d0c4caea872cfe15a0c2e86fe0e", null ], + [ "CO_fifo_readX162a", "group__CO__CANopen__301__fifo.html#ga3202ac32250b599f5a5719c52264578a", null ], + [ "CO_fifo_readX322a", "group__CO__CANopen__301__fifo.html#ga8dcf1f06de7b744d95c6a65721828a5a", null ], + [ "CO_fifo_readX642a", "group__CO__CANopen__301__fifo.html#ga387acd7fdbd61389a42935d452dbd409", null ], + [ "CO_fifo_readI82a", "group__CO__CANopen__301__fifo.html#ga00fe8ce9f04846e8c756a09aad22cf03", null ], + [ "CO_fifo_readI162a", "group__CO__CANopen__301__fifo.html#ga01a1765094d97ca3d0c5bbf98d1b8b1f", null ], + [ "CO_fifo_readI322a", "group__CO__CANopen__301__fifo.html#ga6a7a2ec338f735cdddf7ea7f7c0fd3a5", null ], + [ "CO_fifo_readI642a", "group__CO__CANopen__301__fifo.html#gaf959a6b823cc6db1d2107fd1faa40472", null ], + [ "CO_fifo_readR322a", "group__CO__CANopen__301__fifo.html#ga74aad9b161d3953221a99d4b238f3c1f", null ], + [ "CO_fifo_readR642a", "group__CO__CANopen__301__fifo.html#gadc55fa15874eaf757acf83921d0156c4", null ], + [ "CO_fifo_readHex2a", "group__CO__CANopen__301__fifo.html#gae9bfcb5f9c52de5f7239ab133fe326ec", null ], + [ "CO_fifo_readVs2a", "group__CO__CANopen__301__fifo.html#ga8000ba92f2023a88b5f7d0399373b206", null ], + [ "CO_fifo_readB642a", "group__CO__CANopen__301__fifo.html#ga7b9dcd98906837e82fa914d10028cf33", null ], + [ "CO_fifo_cpyTok2U8", "group__CO__CANopen__301__fifo.html#ga1788f69639bcc6bfa59bf85e57e5e13a", null ], + [ "CO_fifo_cpyTok2U16", "group__CO__CANopen__301__fifo.html#gaf19bdbd45626578afc8a19de43698e0b", null ], + [ "CO_fifo_cpyTok2U32", "group__CO__CANopen__301__fifo.html#gae2816b287cc2091d2382aaa6ac56e422", null ], + [ "CO_fifo_cpyTok2U64", "group__CO__CANopen__301__fifo.html#gacfd3fdfda66e1feb6982f68377a8b7e2", null ], + [ "CO_fifo_cpyTok2I8", "group__CO__CANopen__301__fifo.html#gae038564001c68ab242a6f33756668ac5", null ], + [ "CO_fifo_cpyTok2I16", "group__CO__CANopen__301__fifo.html#ga306913df146db90f7209b89080d51640", null ], + [ "CO_fifo_cpyTok2I32", "group__CO__CANopen__301__fifo.html#ga4f5b25af39c5a78faf04e54cb3cc90f7", null ], + [ "CO_fifo_cpyTok2I64", "group__CO__CANopen__301__fifo.html#ga6a9bd6e8d07d20ded9aeca531f412d50", null ], + [ "CO_fifo_cpyTok2R32", "group__CO__CANopen__301__fifo.html#ga853847c375425a55fc2007880d94c484", null ], + [ "CO_fifo_cpyTok2R64", "group__CO__CANopen__301__fifo.html#ga8b17d4f3c0d272bb73d82a79cd51cd3d", null ], + [ "CO_fifo_cpyTok2Hex", "group__CO__CANopen__301__fifo.html#gab3c4acf458a13bd6c4e5b0bc2adca10b", null ], + [ "CO_fifo_cpyTok2Vs", "group__CO__CANopen__301__fifo.html#ga8cb3d7032cd46dfeb3b6c0bbd5ed1575", null ], + [ "CO_fifo_cpyTok2B64", "group__CO__CANopen__301__fifo.html#gaf11cf0cccc01e86f341e2b31607e90c7", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__CANopen__303.html b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__CANopen__303.html new file mode 100755 index 0000000..32e72be --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__CANopen__303.html @@ -0,0 +1,123 @@ + + + + + + + +CANopenNode: CANopen_303 + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Modules
+
+
CANopen_303
+
+
+ +

CANopen recommendation for indicator specification (CiA 303-3 v1.4.0) +More...

+ + + + +

+Modules

 LED indicators
 
+

Detailed Description

+

CANopen recommendation for indicator specification (CiA 303-3 v1.4.0)

+

Description of communication related indicators - green and red LED diodes.

+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__CANopen__303.js b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__CANopen__303.js new file mode 100755 index 0000000..8a080eb --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__CANopen__303.js @@ -0,0 +1,4 @@ +var group__CO__CANopen__303 = +[ + [ "LED indicators", "group__CO__LEDs.html", "group__CO__LEDs" ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__CANopen__304.html b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__CANopen__304.html new file mode 100755 index 0000000..ad312bd --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__CANopen__304.html @@ -0,0 +1,125 @@ + + + + + + + +CANopenNode: CANopen_304 + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Modules
+
+
CANopen_304
+
+
+ +

CANopen Safety (EN 50325­-5:2010 (formerly CiA 304)) +More...

+ + + + + + +

+Modules

 GFC
 
 SRDO
 
+

Detailed Description

+

CANopen Safety (EN 50325­-5:2010 (formerly CiA 304))

+

Standard defines the usage of Safety Related Data Objects (SRDO) and the GFC. This is an additional protocol (to SDO, PDO) to exchange data. The meaning of "security" here refers not to security (crypto) but to data consistency.

+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__CANopen__304.js b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__CANopen__304.js new file mode 100755 index 0000000..8d45f46 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__CANopen__304.js @@ -0,0 +1,5 @@ +var group__CO__CANopen__304 = +[ + [ "GFC", "group__CO__GFC.html", "group__CO__GFC" ], + [ "SRDO", "group__CO__SRDO.html", "group__CO__SRDO" ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__CANopen__305.html b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__CANopen__305.html new file mode 100755 index 0000000..89b4eb5 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__CANopen__305.html @@ -0,0 +1,127 @@ + + + + + + + +CANopenNode: CANopen_305 + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Modules
+
+
CANopen_305
+
+
+ +

CANopen layer setting services (LSS) and protocols (CiA 305 DSP v3.0.0) +More...

+ + + + + + + + +

+Modules

 LSS
 
 LSS Master
 
 LSS Slave
 
+

Detailed Description

+

CANopen layer setting services (LSS) and protocols (CiA 305 DSP v3.0.0)

+

Inquire or change three parameters on a CANopen device with LSS slave capability by a CANopen device with LSS master capability via the CAN network: the settings of Node-ID of the CANopen device, bit timing parameters of the physical layer (bit rate) or LSS address compliant to the identity object (1018h).

+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__CANopen__305.js b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__CANopen__305.js new file mode 100755 index 0000000..ad851f6 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__CANopen__305.js @@ -0,0 +1,6 @@ +var group__CO__CANopen__305 = +[ + [ "LSS", "group__CO__LSS.html", "group__CO__LSS" ], + [ "LSS Master", "group__CO__LSSmaster.html", "group__CO__LSSmaster" ], + [ "LSS Slave", "group__CO__LSSslave.html", "group__CO__LSSslave" ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__CANopen__309.html b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__CANopen__309.html new file mode 100755 index 0000000..35d5ef9 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__CANopen__309.html @@ -0,0 +1,128 @@ + + + + + + + +CANopenNode: CANopen_309 + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Modules
+
+
CANopen_309
+
+
+ +

CANopen access from other networks (CiA 309) +More...

+ + + + +

+Modules

 Gateway ASCII mapping
 
+

Detailed Description

+

CANopen access from other networks (CiA 309)

+

Standard defines the services and protocols to interface CANopen networks to other networks. Standard is organized as follows:

    +
  • Part 1: General principles and services
    • +
  • Part 2: Modbus/TCP mapping
    • +
  • Part 3: ASCII mapping
    • +
  • Part 4: Amendment 7 to Fieldbus Integration into PROFINET IO
    • +
+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__CANopen__309.js b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__CANopen__309.js new file mode 100755 index 0000000..36dcbf5 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__CANopen__309.js @@ -0,0 +1,4 @@ +var group__CO__CANopen__309 = +[ + [ "Gateway ASCII mapping", "group__CO__CANopen__309__3.html", "group__CO__CANopen__309__3" ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__CANopen__309__3.html b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__CANopen__309__3.html new file mode 100755 index 0000000..17aeb7b --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__CANopen__309__3.html @@ -0,0 +1,688 @@ + + + + + + + +CANopenNode: Gateway ASCII mapping + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Modules | +Files | +Data Structures | +Macros | +Enumerations | +Functions
+
+
Gateway ASCII mapping
CANopen_309
+
+
+ + + + +

+Modules

 Command syntax
 
+ + + + +

+Files

file  CO_gateway_ascii.h
 CANopen access from other networks - ASCII mapping (CiA 309-3 DS v3.0.0)
 
+ + + + +

+Data Structures

struct  CO_GTWA_t
 CANopen Gateway-ascii object. More...
 
+ + + + + + + +

+Macros

#define CO_GTWA_RESP_BUF_SIZE   200
 Size of response string buffer. More...
 
+#define CO_GTWA_STATE_TIMEOUT_TIME_US   1200000
 Timeout time in microseconds for some internal states.
 
+ + + + + + + +

+Enumerations

enum  CO_GTWA_respErrorCode_t {
+  CO_GTWA_respErrorNone = 0, +CO_GTWA_respErrorReqNotSupported = 100, +CO_GTWA_respErrorSyntax = 101, +CO_GTWA_respErrorInternalState = 102, +
+  CO_GTWA_respErrorTimeOut = 103, +CO_GTWA_respErrorNoDefaultNetSet = 104, +CO_GTWA_respErrorNoDefaultNodeSet = 105, +CO_GTWA_respErrorUnsupportedNet = 106, +
+  CO_GTWA_respErrorUnsupportedNode = 107, +CO_GTWA_respErrorLostGuardingMessage = 200, +CO_GTWA_respErrorLostConnection = 201, +CO_GTWA_respErrorHeartbeatStarted = 202, +
+  CO_GTWA_respErrorHeartbeatLost = 203, +CO_GTWA_respErrorWrongNMTstate = 204, +CO_GTWA_respErrorBootUp = 205, +CO_GTWA_respErrorErrorPassive = 300, +
+  CO_GTWA_respErrorBusOff = 301, +CO_GTWA_respErrorCANbufferOverflow = 303, +CO_GTWA_respErrorCANinit = 304, +CO_GTWA_respErrorCANactive = 305, +
+  CO_GTWA_respErrorPDOalreadyUsed = 400, +CO_GTWA_respErrorPDOlengthExceeded = 401, +CO_GTWA_respErrorLSSmanufacturer = 501, +CO_GTWA_respErrorLSSnodeIdNotSupported = 502, +
+  CO_GTWA_respErrorLSSbitRateNotSupported = 503, +CO_GTWA_respErrorLSSparameterStoringFailed = 504, +CO_GTWA_respErrorLSSmediaError = 505, +CO_GTWA_respErrorRunningOutOfMemory = 600 +
+ }
 Response error codes as specified by CiA 309-3. More...
 
enum  CO_GTWA_state_t {
+  CO_GTWA_ST_IDLE = 0x00U, +CO_GTWA_ST_READ = 0x10U, +CO_GTWA_ST_WRITE = 0x11U, +CO_GTWA_ST_WRITE_ABORTED = 0x12U, +
+  CO_GTWA_ST_LSS_SWITCH_GLOB = 0x20U, +CO_GTWA_ST_LSS_SWITCH_SEL = 0x21U, +CO_GTWA_ST_LSS_SET_NODE = 0x22U, +CO_GTWA_ST_LSS_CONF_BITRATE = 0x23U, +
+  CO_GTWA_ST_LSS_STORE = 0x24U, +CO_GTWA_ST_LSS_INQUIRE = 0x25U, +CO_GTWA_ST_LSS_INQUIRE_ADDR_ALL = 0x26U, +CO_GTWA_ST__LSS_FASTSCAN = 0x30U, +
+  CO_GTWA_ST_LSS_ALLNODES = 0x31U, +CO_GTWA_ST_LOG = 0x80U, +CO_GTWA_ST_HELP = 0x81U, +CO_GTWA_ST_LED = 0x82U +
+ }
 Internal states of the Gateway-ascii state machine. More...
 
+ + + + + + + + + + + + + + + + + + + +

+Functions

CO_ReturnError_t CO_GTWA_init (CO_GTWA_t *gtwa, CO_SDOclient_t *SDO_C, uint16_t SDOclientTimeoutTime_ms, bool_t SDOclientBlockTransfer, CO_NMT_t *NMT, CO_LSSmaster_t *LSSmaster, CO_LEDs_t *LEDs, uint8_t dummy)
 Initialize Gateway-ascii object. More...
 
void CO_GTWA_initRead (CO_GTWA_t *gtwa, size_t(*readCallback)(void *object, const char *buf, size_t count, uint8_t *connectionOK), void *readCallbackObject)
 Initialize read callback in Gateway-ascii object. More...
 
static size_t CO_GTWA_write_getSpace (CO_GTWA_t *gtwa)
 Get free write buffer space. More...
 
static size_t CO_GTWA_write (CO_GTWA_t *gtwa, const char *buf, size_t count)
 Write command into CO_GTWA_t object. More...
 
void CO_GTWA_log_print (CO_GTWA_t *gtwa, const char *message)
 Print message log string into fifo buffer. More...
 
void CO_GTWA_process (CO_GTWA_t *gtwa, bool_t enable, uint32_t timeDifference_us, uint32_t *timerNext_us)
 Process Gateway-ascii object. More...
 
+

Detailed Description

+

CANopen access from other networks - ASCII mapping (CiA 309-3 DSP v3.0.0)

+

This module enables ascii command interface (CAN gateway), which can be used for master interaction with CANopen network. Some sort of string input/output stream can be used, for example serial port + terminal on microcontroller or stdio in OS or sockets, etc.

+

For example, one wants to read 'Heartbeat producer time' parameter (0x1017,0) on remote node (with id=4). Parameter is 16-bit integer. He can can enter command string: [1] 4 read 0x1017 0 i16. CANopenNode will use SDO client, send request to remote node via CAN, wait for response via CAN and prints [1] OK to output stream on success.

+

This module is usually initialized and processed in CANopen.c file. Application should register own callback function for reading the output stream. Application writes new commands with CO_GTWA_write().

+

Macro Definition Documentation

+ +

◆ CO_GTWA_RESP_BUF_SIZE

+ +
+
+ + + + +
#define CO_GTWA_RESP_BUF_SIZE   200
+
+ +

Size of response string buffer.

+

This is intermediate buffer. If there is larger amount of data to transfer, then multiple transfers will occur.

+ +
+
+

Enumeration Type Documentation

+ +

◆ CO_GTWA_respErrorCode_t

+ +
+
+ + + + +
enum CO_GTWA_respErrorCode_t
+
+ +

Response error codes as specified by CiA 309-3.

+

Values less or equal to 0 are used for control for some functions and are not part of the standard.

+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Enumerator
CO_GTWA_respErrorNone 

0 - No error or idle

+
CO_GTWA_respErrorReqNotSupported 

100 - Request not supported

+
CO_GTWA_respErrorSyntax 

101 - Syntax error

+
CO_GTWA_respErrorInternalState 

102 - Request not processed due to internal state

+
CO_GTWA_respErrorTimeOut 

103 - Time-out (where applicable)

+
CO_GTWA_respErrorNoDefaultNetSet 

104 - No default net set

+
CO_GTWA_respErrorNoDefaultNodeSet 

105 - No default node set

+
CO_GTWA_respErrorUnsupportedNet 

106 - Unsupported net

+
CO_GTWA_respErrorUnsupportedNode 

107 - Unsupported node

+
CO_GTWA_respErrorLostGuardingMessage 

200 - Lost guarding message

+
CO_GTWA_respErrorLostConnection 

201 - Lost connection

+
CO_GTWA_respErrorHeartbeatStarted 

202 - Heartbeat started

+
CO_GTWA_respErrorHeartbeatLost 

203 - Heartbeat lost

+
CO_GTWA_respErrorWrongNMTstate 

204 - Wrong NMT state

+
CO_GTWA_respErrorBootUp 

205 - Boot-up

+
CO_GTWA_respErrorErrorPassive 

300 - Error passive

+
CO_GTWA_respErrorBusOff 

301 - Bus off

+
CO_GTWA_respErrorCANbufferOverflow 

303 - CAN buffer overflow

+
CO_GTWA_respErrorCANinit 

304 - CAN init

+
CO_GTWA_respErrorCANactive 

305 - CAN active (at init or start-up)

+
CO_GTWA_respErrorPDOalreadyUsed 

400 - PDO already used

+
CO_GTWA_respErrorPDOlengthExceeded 

401 - PDO length exceeded

+
CO_GTWA_respErrorLSSmanufacturer 

501 - LSS implementation- / manufacturer-specific error

+
CO_GTWA_respErrorLSSnodeIdNotSupported 

502 - LSS node-ID not supported

+
CO_GTWA_respErrorLSSbitRateNotSupported 

503 - LSS bit-rate not supported

+
CO_GTWA_respErrorLSSparameterStoringFailed 

504 - LSS parameter storing failed

+
CO_GTWA_respErrorLSSmediaError 

505 - LSS command failed because of media error

+
CO_GTWA_respErrorRunningOutOfMemory 

600 - Running out of memory

+
+ +
+
+ +

◆ CO_GTWA_state_t

+ +
+
+ + + + +
enum CO_GTWA_state_t
+
+ +

Internal states of the Gateway-ascii state machine.

+ + + + + + + + + + + + + + + + + +
Enumerator
CO_GTWA_ST_IDLE 

Gateway is idle, no command is processing.

+

This state is starting point for new commands, which are parsed here.

+
CO_GTWA_ST_READ 

SDO 'read' (upload)

+
CO_GTWA_ST_WRITE 

SDO 'write' (download)

+
CO_GTWA_ST_WRITE_ABORTED 

SDO 'write' (download) - aborted, purging remaining data.

+
CO_GTWA_ST_LSS_SWITCH_GLOB 

LSS 'lss_switch_glob'.

+
CO_GTWA_ST_LSS_SWITCH_SEL 

LSS 'lss_switch_sel'.

+
CO_GTWA_ST_LSS_SET_NODE 

LSS 'lss_set_node'.

+
CO_GTWA_ST_LSS_CONF_BITRATE 

LSS 'lss_conf_bitrate'.

+
CO_GTWA_ST_LSS_STORE 

LSS 'lss_store'.

+
CO_GTWA_ST_LSS_INQUIRE 

LSS 'lss_inquire_addr' or 'lss_get_node'.

+
CO_GTWA_ST_LSS_INQUIRE_ADDR_ALL 

LSS 'lss_inquire_addr', all parameters.

+
CO_GTWA_ST__LSS_FASTSCAN 

LSS '_lss_fastscan'.

+
CO_GTWA_ST_LSS_ALLNODES 

LSS 'lss_allnodes'.

+
CO_GTWA_ST_LOG 

print message 'log'

+
CO_GTWA_ST_HELP 

print 'help' text

+
CO_GTWA_ST_LED 

print 'status' of the node

+
+ +
+
+

Function Documentation

+ +

◆ CO_GTWA_init()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
CO_ReturnError_t CO_GTWA_init (CO_GTWA_t * gtwa,
CO_SDOclient_t * SDO_C,
uint16_t SDOclientTimeoutTime_ms,
bool_t SDOclientBlockTransfer,
CO_NMT_t * NMT,
CO_LSSmaster_t * LSSmaster,
CO_LEDs_t * LEDs,
uint8_t dummy 
)
+
+ +

Initialize Gateway-ascii object.

+
Parameters
+ + + + + + + + + +
gtwaThis object will be initialized
SDO_CSDO client object
SDOclientTimeoutTime_msDefault timeout in milliseconds, 500 typically
SDOclientBlockTransferIf true, block transfer will be set by default
NMTNMT object
LSSmasterLSS master object
LEDsLEDs object
dummydummy argument, set to 0
+
+
+
Returns
CO_ReturnError_t: CO_ERROR_NO or CO_ERROR_ILLEGAL_ARGUMENT
+ +
+
+ +

◆ CO_GTWA_initRead()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + +
void CO_GTWA_initRead (CO_GTWA_t * gtwa,
size_t(*)(void *object, const char *buf, size_t count, uint8_t *connectionOK) readCallback,
void * readCallbackObject 
)
+
+ +

Initialize read callback in Gateway-ascii object.

+

Callback will be used for transfer data to output stream of the application. It will be called from CO_GTWA_process() zero or multiple times, depending on the data available. If readCallback is uninitialized or NULL, then output data will be purged.

+
Parameters
+ + + + +
gtwaThis object will be initialized
readCallbackPointer to external function for reading response from Gateway-ascii object. See CO_GTWA_t for parameters.
readCallbackObjectPointer to object, which will be used inside readCallback
+
+
+ +
+
+ +

◆ CO_GTWA_write_getSpace()

+ +
+
+ + + + + +
+ + + + + + + + +
static size_t CO_GTWA_write_getSpace (CO_GTWA_t * gtwa)
+ 		+inlinestatic
+
+ +

Get free write buffer space.

+
Parameters
+ + +
gtwaThis object
+
+
+
Returns
number of available bytes
+ +
+
+ +

◆ CO_GTWA_write()

+ +
+
+ + + + + +
+ + + + + + + + + + + + + + + + + + + + + + + + +
static size_t CO_GTWA_write (CO_GTWA_t * gtwa,
const char * buf,
size_t count 
)
+ 		+inlinestatic
+
+ +

Write command into CO_GTWA_t object.

+

This function copies ascii command from buf into internal fifo buffer. Command must be closed with '
+' character. Function returns number of bytes successfully copied. If there is not enough space in destination, not all bytes will be copied and data can be refilled later (in case of large SDO download).

+
Parameters
+ + + + +
gtwaThis object
bufBuffer which will be copied
countNumber of bytes in buf
+
+
+
Returns
number of bytes actually written.
+ +
+
+ +

◆ CO_GTWA_log_print()

+ +
+
+ + + + + + + + + + + + + + + + + + +
void CO_GTWA_log_print (CO_GTWA_t * gtwa,
const char * message 
)
+
+ +

Print message log string into fifo buffer.

+

This function enables recording of system log messages including CANopen events. Function can be called by application for recording any message. Message is copied to internal fifo buffer. In case fifo is full, old messages will be owerwritten. Message log fifo can be read with non-standard command "log". After log is read, it is emptied.

+
Parameters
+ + + +
gtwaThis object
messageNull terminated string
+
+
+ +
+
+ +

◆ CO_GTWA_process()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
void CO_GTWA_process (CO_GTWA_t * gtwa,
bool_t enable,
uint32_t timeDifference_us,
uint32_t * timerNext_us 
)
+
+ +

Process Gateway-ascii object.

+

This is non-blocking function and must be called cyclically

+
Parameters
+ + + + + +
gtwaThis object will be initialized.
enableIf true, gateway operates normally. If false, gateway is completely disabled and no command interaction is possible. Can be connected to hardware switch, for example.
timeDifference_usTime difference from previous function call in [microseconds].
[out]timerNext_usinfo to OS - see CO_process().
+
+
+
Returns
CO_ReturnError_t: CO_ERROR_NO on success or CO_ERROR_ILLEGAL_ARGUMENT
+ +
+
+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__CANopen__309__3.js b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__CANopen__309__3.js new file mode 100755 index 0000000..cee0c8f --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__CANopen__309__3.js @@ -0,0 +1,99 @@ +var group__CO__CANopen__309__3 = +[ + [ "Command syntax", "group__CO__CANopen__309__3__Syntax.html", null ], + [ "CO_gateway_ascii.h", "CO__gateway__ascii_8h.html", null ], + [ "CO_GTWA_t", "structCO__GTWA__t.html", [ + [ "readCallback", "structCO__GTWA__t.html#a036c4a3a89b8171baba8039fc50876ba", null ], + [ "readCallbackObject", "structCO__GTWA__t.html#ae1b9a86d7020ac21713a9b658a08495b", null ], + [ "sequence", "structCO__GTWA__t.html#a31b7ae3a5da107dfb5432f5a95f9faee", null ], + [ "net_default", "structCO__GTWA__t.html#aec9a1ffe0ce40572452d3f1e36e51c1b", null ], + [ "node_default", "structCO__GTWA__t.html#a2464fa84713d31811e8872b4557d50d1", null ], + [ "net", "structCO__GTWA__t.html#a8df8a3f47d967e4fb0a56e491db0f9e9", null ], + [ "node", "structCO__GTWA__t.html#a38f5c9325dc69820d831688282a63a10", null ], + [ "commFifo", "structCO__GTWA__t.html#a74241ff1c68a8fc05f0b2be601dcf960", null ], + [ "commBuf", "structCO__GTWA__t.html#a51fd91cf468da15e5f943131fa696266", null ], + [ "respBuf", "structCO__GTWA__t.html#a987d9431f47a10272cf9c81b0d0159d1", null ], + [ "respBufCount", "structCO__GTWA__t.html#a67770af170976d4d904fbc044d347376", null ], + [ "respBufOffset", "structCO__GTWA__t.html#a605bfa0c99f4a0235980de0603a050ca", null ], + [ "respHold", "structCO__GTWA__t.html#aef556bb4c595944ebf3de22a2c9d5007", null ], + [ "timeDifference_us_cumulative", "structCO__GTWA__t.html#a8ba7809acba0f2de26eda4a890e68160", null ], + [ "state", "structCO__GTWA__t.html#a5c37389f4a985950708e2fed036daf1f", null ], + [ "stateTimeoutTmr", "structCO__GTWA__t.html#ac5fc932142ed17d04393ae30819fb021", null ], + [ "SDO_C", "structCO__GTWA__t.html#a274945dbaacfd975f86a58566a884769", null ], + [ "SDOtimeoutTime", "structCO__GTWA__t.html#a9fa3fe5ce1806296cf36ef5d8c3ecae2", null ], + [ "SDOblockTransferEnable", "structCO__GTWA__t.html#a0b05c1b89fe8b104b019d12679d4edcf", null ], + [ "SDOdataCopyStatus", "structCO__GTWA__t.html#aae981f9a446cfaad8a5450e77adc32ec", null ], + [ "SDOdataType", "structCO__GTWA__t.html#a7b9bcb2113286454a273b9b43f4e1548", null ], + [ "NMT", "structCO__GTWA__t.html#a6aa019a1583f8ba56fada7c5ed8ec191", null ], + [ "LSSmaster", "structCO__GTWA__t.html#ae2897d681afe5bd45db8306b52734318", null ], + [ "lssAddress", "structCO__GTWA__t.html#aae702db8c811f7e85f3fd8984bde8f9d", null ], + [ "lssNID", "structCO__GTWA__t.html#af49dacd6548ca791a09e78727aeeabad", null ], + [ "lssBitrate", "structCO__GTWA__t.html#ae404009d2987f589cc52b9958d688917", null ], + [ "lssInquireCs", "structCO__GTWA__t.html#a74892b830cb6064d31190ff6bac6a3c9", null ], + [ "lssFastscan", "structCO__GTWA__t.html#aa39bf29226ecaa52d20a6a92b61cf4fa", null ], + [ "lssSubState", "structCO__GTWA__t.html#a7f55184f433e48dd4d4fb52aebed6da6", null ], + [ "lssNodeCount", "structCO__GTWA__t.html#a16f5893d54bce3d2741d8b732ec6c29e", null ], + [ "lssStore", "structCO__GTWA__t.html#a47ec165dd4d545a6ae52067644dbbaab", null ], + [ "lssTimeout_ms", "structCO__GTWA__t.html#a78c3d1a9ebb44a1db4e6eb100ed6c6a7", null ], + [ "logBuf", "structCO__GTWA__t.html#a7400a0dcf3d8ad25b8ad9820237d4f3c", null ], + [ "logFifo", "structCO__GTWA__t.html#af84217848a2f2e4d3f840f4978d4e2ac", null ], + [ "helpString", "structCO__GTWA__t.html#a5ce9a4cef511904ad4038c0b1443d3f6", null ], + [ "LEDs", "structCO__GTWA__t.html#a90ec5c0f770fd1a3ec2ea4f33059357a", null ] + ] ], + [ "CO_GTWA_RESP_BUF_SIZE", "group__CO__CANopen__309__3.html#ga52919223e5f43323f15c6a382913653d", null ], + [ "CO_GTWA_STATE_TIMEOUT_TIME_US", "group__CO__CANopen__309__3.html#gaa631d47f972204f26502d65894694cfb", null ], + [ "CO_GTWA_respErrorCode_t", "group__CO__CANopen__309__3.html#ga92e67dec9b5e29cdd67a28651db237fb", [ + [ "CO_GTWA_respErrorNone", "group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fba46a2114a5c4d9b43babecebb11573c66", null ], + [ "CO_GTWA_respErrorReqNotSupported", "group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fba04540349402c75b73af33951db285904", null ], + [ "CO_GTWA_respErrorSyntax", "group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fba541a3546ac6be179c728272409259c98", null ], + [ "CO_GTWA_respErrorInternalState", "group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fba1d990943991355e276f31533c841cb81", null ], + [ "CO_GTWA_respErrorTimeOut", "group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fba81131bf73d1ad39163a8b28e5ecf92fd", null ], + [ "CO_GTWA_respErrorNoDefaultNetSet", "group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fbad05b7717a0f79d333eed37ced2e758f9", null ], + [ "CO_GTWA_respErrorNoDefaultNodeSet", "group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fbac668fafa372a0cee1d50ceb0f5ed7b4a", null ], + [ "CO_GTWA_respErrorUnsupportedNet", "group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fba291ec5267c935d6277c27c767e2dd178", null ], + [ "CO_GTWA_respErrorUnsupportedNode", "group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fbaf65eee2744df8e528312139a54a853d5", null ], + [ "CO_GTWA_respErrorLostGuardingMessage", "group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fba1111be206d1ebfb62caa217d6231092c", null ], + [ "CO_GTWA_respErrorLostConnection", "group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fba9cc980ac35af4c1f9ad1d96719f05425", null ], + [ "CO_GTWA_respErrorHeartbeatStarted", "group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fbaf4ed1e079359d86cee1ad431ed745886", null ], + [ "CO_GTWA_respErrorHeartbeatLost", "group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fbaba7c053af330ec3ffdc8c2c298db6c93", null ], + [ "CO_GTWA_respErrorWrongNMTstate", "group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fbad007ed28e48b2fd7c287de3e6755a604", null ], + [ "CO_GTWA_respErrorBootUp", "group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fbaa4c5257721f3b64c231f67dccacd2ee7", null ], + [ "CO_GTWA_respErrorErrorPassive", "group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fba6f9a335886d942ef028878832551afe0", null ], + [ "CO_GTWA_respErrorBusOff", "group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fbabb407ab8220e0d3f0ebc06f867ecac9c", null ], + [ "CO_GTWA_respErrorCANbufferOverflow", "group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fba8aa993a6c57d8db7d6355260a091e4a4", null ], + [ "CO_GTWA_respErrorCANinit", "group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fba97d5b22449aadae1226679986d654bbc", null ], + [ "CO_GTWA_respErrorCANactive", "group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fba06e779c446aae0f7c6021dd0b955ffe8", null ], + [ "CO_GTWA_respErrorPDOalreadyUsed", "group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fbace2f0ef57b11e716d7ca58369ff5eede", null ], + [ "CO_GTWA_respErrorPDOlengthExceeded", "group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fbadd3dbd3197e1756102508208e394d72f", null ], + [ "CO_GTWA_respErrorLSSmanufacturer", "group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fba7e3209f49a5ae7bad0b197110aa78512", null ], + [ "CO_GTWA_respErrorLSSnodeIdNotSupported", "group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fba6e164941b6f3a18ef6e366e7c53073b6", null ], + [ "CO_GTWA_respErrorLSSbitRateNotSupported", "group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fbad8ff0b3e8970d010eb084a703fa794db", null ], + [ "CO_GTWA_respErrorLSSparameterStoringFailed", "group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fba315295e9ea4cdaba0955e6d8920df5a5", null ], + [ "CO_GTWA_respErrorLSSmediaError", "group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fbacc7ec1f2040bebb15becb067673e40e5", null ], + [ "CO_GTWA_respErrorRunningOutOfMemory", "group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fba99443a03d7ba443b9beb07c6d475b350", null ] + ] ], + [ "CO_GTWA_state_t", "group__CO__CANopen__309__3.html#gae809d7b5adbc7a4fb1f2fce527b30954", [ + [ "CO_GTWA_ST_IDLE", "group__CO__CANopen__309__3.html#ggae809d7b5adbc7a4fb1f2fce527b30954a98a36dabc8934b9c2d37b13999e3c393", null ], + [ "CO_GTWA_ST_READ", "group__CO__CANopen__309__3.html#ggae809d7b5adbc7a4fb1f2fce527b30954aaca55c8223aa1ba8f18031196178ea58", null ], + [ "CO_GTWA_ST_WRITE", "group__CO__CANopen__309__3.html#ggae809d7b5adbc7a4fb1f2fce527b30954a959c96b4eb3d948a977f66679423baa2", null ], + [ "CO_GTWA_ST_WRITE_ABORTED", "group__CO__CANopen__309__3.html#ggae809d7b5adbc7a4fb1f2fce527b30954ab10b68af4a8bc47f96abf552c7baa3b9", null ], + [ "CO_GTWA_ST_LSS_SWITCH_GLOB", "group__CO__CANopen__309__3.html#ggae809d7b5adbc7a4fb1f2fce527b30954afcc0bdbc08aa70d401f122c73055223e", null ], + [ "CO_GTWA_ST_LSS_SWITCH_SEL", "group__CO__CANopen__309__3.html#ggae809d7b5adbc7a4fb1f2fce527b30954a19bb6359e2efba37226a5dcf27d4a0e3", null ], + [ "CO_GTWA_ST_LSS_SET_NODE", "group__CO__CANopen__309__3.html#ggae809d7b5adbc7a4fb1f2fce527b30954acbc1d88036a086ae9d9dcbed1742330e", null ], + [ "CO_GTWA_ST_LSS_CONF_BITRATE", "group__CO__CANopen__309__3.html#ggae809d7b5adbc7a4fb1f2fce527b30954a6e18980d6fbcbd7dd871af5c98c4755e", null ], + [ "CO_GTWA_ST_LSS_STORE", "group__CO__CANopen__309__3.html#ggae809d7b5adbc7a4fb1f2fce527b30954a63e0705eddd22fb9d476c1b370394fc4", null ], + [ "CO_GTWA_ST_LSS_INQUIRE", "group__CO__CANopen__309__3.html#ggae809d7b5adbc7a4fb1f2fce527b30954abc4877f6d72e218cbd4959279a537b94", null ], + [ "CO_GTWA_ST_LSS_INQUIRE_ADDR_ALL", "group__CO__CANopen__309__3.html#ggae809d7b5adbc7a4fb1f2fce527b30954a17254161b141cc0c424c3649655f4df2", null ], + [ "CO_GTWA_ST__LSS_FASTSCAN", "group__CO__CANopen__309__3.html#ggae809d7b5adbc7a4fb1f2fce527b30954ab8c2946ce5d3581ecf5640bd1f5667b7", null ], + [ "CO_GTWA_ST_LSS_ALLNODES", "group__CO__CANopen__309__3.html#ggae809d7b5adbc7a4fb1f2fce527b30954a338a75317f0bf276f1c786316d6b9ec7", null ], + [ "CO_GTWA_ST_LOG", "group__CO__CANopen__309__3.html#ggae809d7b5adbc7a4fb1f2fce527b30954a829f096eca2ccd152069e97d9c70022f", null ], + [ "CO_GTWA_ST_HELP", "group__CO__CANopen__309__3.html#ggae809d7b5adbc7a4fb1f2fce527b30954af2f0ce738128675b98926aba680884d1", null ], + [ "CO_GTWA_ST_LED", "group__CO__CANopen__309__3.html#ggae809d7b5adbc7a4fb1f2fce527b30954a1a467cf33b8d3a2c0e8e31f87d81f05c", null ] + ] ], + [ "CO_GTWA_init", "group__CO__CANopen__309__3.html#gabc95dab4fb09bcb18948502f922520ee", null ], + [ "CO_GTWA_initRead", "group__CO__CANopen__309__3.html#ga2093c35b83b096e01bd0c65ae9374e30", null ], + [ "CO_GTWA_write_getSpace", "group__CO__CANopen__309__3.html#gad614da9bb171d995c02ffe1940fe7e64", null ], + [ "CO_GTWA_write", "group__CO__CANopen__309__3.html#gaae1f01d444be975aa3cb3b2c9ebded3d", null ], + [ "CO_GTWA_log_print", "group__CO__CANopen__309__3.html#ga20523907b832d55b47b855dd92409996", null ], + [ "CO_GTWA_process", "group__CO__CANopen__309__3.html#ga4a82ef2ebdd5d5f9d8a7efe84048493d", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__CANopen__309__3__Syntax.html b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__CANopen__309__3__Syntax.html new file mode 100755 index 0000000..df73d72 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__CANopen__309__3__Syntax.html @@ -0,0 +1,175 @@ + + + + + + + +CANopenNode: Command syntax + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+
Command syntax
CANopen_309 » Gateway ASCII mapping
+
+
+
Command strings start with '"["<sequence>"]"' followed by:
+
[[<net>] <node>] r[ead] <index> <subindex> [<datatype>] # SDO upload.
+
[[<net>] <node>] w[rite] <index> <subindex> <datatype> <value> # SDO download.
+
+
[[<net>] <node>] start # NMT Start node.
+
[[<net>] <node>] stop # NMT Stop node.
+
[[<net>] <node>] preop[erational] # NMT Set node to pre-operational.
+
[[<net>] <node>] reset node # NMT Reset node.
+
[[<net>] <node>] reset comm[unication] # NMT Reset communication.
+
+
[<net>] set network <value> # Set default net.
+
[<net>] set node <value> # Set default node.
+
[<net>] set sdo_timeout <value> # Configure SDO time-out.
+
[<net>] set sdo_block <value> # Enable/disable SDO block transfer.
+
+
help [datatype|lss] # Print this or datatype or lss help.
+
led # Print status LED diodes.
+
log # Print message log.
+
+
Response:
+
"["<sequence>"]" OK | <value> |
+
ERROR:<SDO-abort-code> | ERROR:<internal-error-code>
+
+
Every command must be terminated with <CR><LF> ('\\r\\n'). characters. Same
+
is response. String is not null terminated, <CR> is optional in command.
+
Comments started with '#' are ignored. They may be on the beginning of the
+
line or after the command string.
+
'sdo_timeout' is in milliseconds, 500 by default. Block transfer is
+
disabled by default.
+
If '<net>' or '<node>' is not specified within commands, then value defined
+
by 'set network' or 'set node' command is used.
+
+
Datatypes:
+
b # Boolean.
+
i8, i16, i32, i64 # Signed integers.
+
u8, u16, u32, u64 # Unsigned integers.
+
x8, x16, x32, x64 # Unsigned integers, displayed as hexadecimal, non-standard.
+
r32, r64 # Real numbers.
+
t, td # Time of day, time difference.
+
vs # Visible string (between double quotes if multi-word).
+
os, us # Octet, unicode string, (mime-base64 (RFC2045) based, line).
+
d # domain (mime-base64 (RFC2045) based, one line).
+
hex # Hexagonal data, optionally space separated, non-standard.
+
+
LSS commands:
+
lss_switch_glob <0|1> # Switch state global command.
+
lss_switch_sel <vendorID> <product code> \\
+
<revisionNo> <serialNo> #Switch state selective.
+
lss_set_node <node> # Configure node-ID.
+
lss_conf_bitrate <table_selector=0> \\
+
<table_index> # Configure bit-rate.
+
lss_activate_bitrate <switch_delay_ms> # Activate new bit-rate.
+
lss_store # LSS store configuration.
+
lss_inquire_addr [<LSSSUB=0..3>] # Inquire LSS address.
+
lss_get_node # Inquire node-ID.
+
_lss_fastscan [<timeout_ms>] # Identify fastscan, non-standard.
+
lss_allnodes [<timeout_ms> [<nodeStart=1..127> <store=0|1>\\
+
[<scanType0> <vendorId> <scanType1> <productCode>\\
+
<scanType2> <revisionNo> <scanType3> <serialNo>]]]
+
# Node-ID configuration of all nodes.
+
+
All LSS commands start with '\"[\"<sequence>\"]\" [<net>]'.
+
<table_index>: 0=1000 kbit/s, 1=800 kbit/s, 2=500 kbit/s, 3=250 kbit/s,
+
4=125 kbit/s, 6=50 kbit/s, 7=20 kbit/s, 8=10 kbit/s, 9=auto
+
<scanType>: 0=fastscan, 1=ignore, 2=match value in next parameter
+

This help text is the same as variable contents in CO_GTWA_helpString.

+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__CANopen__extra.html b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__CANopen__extra.html new file mode 100755 index 0000000..5be686d --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__CANopen__extra.html @@ -0,0 +1,122 @@ + + + + + + + +CANopenNode: CANopen_extra + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Modules
+
+
CANopen_extra
+
+
+ +

Additional non-standard objects related to CANopenNode. +More...

+ + + + +

+Modules

 Trace
 
+

Detailed Description

+

Additional non-standard objects related to CANopenNode.

+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__CANopen__extra.js b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__CANopen__extra.js new file mode 100755 index 0000000..fc0ef4c --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__CANopen__extra.js @@ -0,0 +1,4 @@ +var group__CO__CANopen__extra = +[ + [ "Trace", "group__CO__trace.html", "group__CO__trace" ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__Emergency.html b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__Emergency.html new file mode 100755 index 0000000..69b57d4 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__Emergency.html @@ -0,0 +1,982 @@ + + + + + + + +CANopenNode: Emergency + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Files | +Data Structures | +Macros | +Enumerations | +Functions
+
+
Emergency
CANopen_301
+
+
+ + + + + +

+Files

file  CO_Emergency.h
 CANopen Emergency protocol.
 
+ + + + +

+Data Structures

struct  CO_EM_t
 Emergency object. More...
 
+ + + + + + + +

+Macros

+#define CO_errorReport(em, errorBit, errorCode, infoCode)   CO_error(em, true, errorBit, errorCode, infoCode)
 Report error condition, for description of parameters see CO_error.
 
+#define CO_errorReset(em, errorBit, infoCode)   CO_error(em, false, errorBit, CO_EMC_NO_ERROR, infoCode)
 Reset error condition, for description of parameters see CO_error.
 
+ + + + + + + + + + +

+Enumerations

enum  CO_errorRegister_t {
+  CO_ERR_REG_GENERIC_ERR = 0x01U, +CO_ERR_REG_CURRENT = 0x02U, +CO_ERR_REG_VOLTAGE = 0x04U, +CO_ERR_REG_TEMPERATURE = 0x08U, +
+  CO_ERR_REG_COMMUNICATION = 0x10U, +CO_ERR_REG_DEV_PROFILE = 0x20U, +CO_ERR_REG_RESERVED = 0x40U, +CO_ERR_REG_MANUFACTURER = 0x80U +
+ }
 CANopen Error register. More...
 
enum  CO_EM_errorCode_t {
+  CO_EMC_NO_ERROR = 0x0000U, +CO_EMC_GENERIC = 0x1000U, +CO_EMC_CURRENT = 0x2000U, +CO_EMC_CURRENT_INPUT = 0x2100U, +
+  CO_EMC_CURRENT_INSIDE = 0x2200U, +CO_EMC_CURRENT_OUTPUT = 0x2300U, +CO_EMC_VOLTAGE = 0x3000U, +CO_EMC_VOLTAGE_MAINS = 0x3100U, +
+  CO_EMC_VOLTAGE_INSIDE = 0x3200U, +CO_EMC_VOLTAGE_OUTPUT = 0x3300U, +CO_EMC_TEMPERATURE = 0x4000U, +CO_EMC_TEMP_AMBIENT = 0x4100U, +
+  CO_EMC_TEMP_DEVICE = 0x4200U, +CO_EMC_HARDWARE = 0x5000U, +CO_EMC_SOFTWARE_DEVICE = 0x6000U, +CO_EMC_SOFTWARE_INTERNAL = 0x6100U, +
+  CO_EMC_SOFTWARE_USER = 0x6200U, +CO_EMC_DATA_SET = 0x6300U, +CO_EMC_ADDITIONAL_MODUL = 0x7000U, +CO_EMC_MONITORING = 0x8000U, +
+  CO_EMC_COMMUNICATION = 0x8100U, +CO_EMC_CAN_OVERRUN = 0x8110U, +CO_EMC_CAN_PASSIVE = 0x8120U, +CO_EMC_HEARTBEAT = 0x8130U, +
+  CO_EMC_BUS_OFF_RECOVERED = 0x8140U, +CO_EMC_CAN_ID_COLLISION = 0x8150U, +CO_EMC_PROTOCOL_ERROR = 0x8200U, +CO_EMC_PDO_LENGTH = 0x8210U, +
+  CO_EMC_PDO_LENGTH_EXC = 0x8220U, +CO_EMC_DAM_MPDO = 0x8230U, +CO_EMC_SYNC_DATA_LENGTH = 0x8240U, +CO_EMC_RPDO_TIMEOUT = 0x8250U, +
+  CO_EMC_TIME_DATA_LENGTH = 0x8260U, +CO_EMC_EXTERNAL_ERROR = 0x9000U, +CO_EMC_ADDITIONAL_FUNC = 0xF000U, +CO_EMC_DEVICE_SPECIFIC = 0xFF00U, +
+  CO_EMC401_OUT_CUR_HI = 0x2310U, +CO_EMC401_OUT_SHORTED = 0x2320U, +CO_EMC401_OUT_LOAD_DUMP = 0x2330U, +CO_EMC401_IN_VOLT_HI = 0x3110U, +
+  CO_EMC401_IN_VOLT_LOW = 0x3120U, +CO_EMC401_INTERN_VOLT_HI = 0x3210U, +CO_EMC401_INTERN_VOLT_LO = 0x3220U, +CO_EMC401_OUT_VOLT_HIGH = 0x3310U, +
+  CO_EMC401_OUT_VOLT_LOW = 0x3320U +
+ }
 CANopen Error code. More...
 
enum  CO_EM_errorStatusBits_t {
+  CO_EM_NO_ERROR = 0x00U, +CO_EM_CAN_BUS_WARNING = 0x01U, +CO_EM_RXMSG_WRONG_LENGTH = 0x02U, +CO_EM_RXMSG_OVERFLOW = 0x03U, +
+  CO_EM_RPDO_WRONG_LENGTH = 0x04U, +CO_EM_RPDO_OVERFLOW = 0x05U, +CO_EM_CAN_RX_BUS_PASSIVE = 0x06U, +CO_EM_CAN_TX_BUS_PASSIVE = 0x07U, +
+  CO_EM_NMT_WRONG_COMMAND = 0x08U, +CO_EM_TIME_TIMEOUT = 0x09U, +CO_EM_TIME_LENGTH = 0x0AU, +CO_EM_0B_unused = 0x0BU, +
+  CO_EM_0C_unused = 0x0CU, +CO_EM_0D_unused = 0x0DU, +CO_EM_0E_unused = 0x0EU, +CO_EM_0F_unused = 0x0FU, +
+  CO_EM_10_unused = 0x10U, +CO_EM_11_unused = 0x11U, +CO_EM_CAN_TX_BUS_OFF = 0x12U, +CO_EM_CAN_RXB_OVERFLOW = 0x13U, +
+  CO_EM_CAN_TX_OVERFLOW = 0x14U, +CO_EM_TPDO_OUTSIDE_WINDOW = 0x15U, +CO_EM_16_unused = 0x16U, +CO_EM_17_unused = 0x17U, +
+  CO_EM_SYNC_TIME_OUT = 0x18U, +CO_EM_SYNC_LENGTH = 0x19U, +CO_EM_PDO_WRONG_MAPPING = 0x1AU, +CO_EM_HEARTBEAT_CONSUMER = 0x1BU, +
+  CO_EM_HB_CONSUMER_REMOTE_RESET = 0x1CU, +CO_EM_1D_unused = 0x1DU, +CO_EM_1E_unused = 0x1EU, +CO_EM_1F_unused = 0x1FU, +
+  CO_EM_EMERGENCY_BUFFER_FULL = 0x20U, +CO_EM_21_unused = 0x21U, +CO_EM_MICROCONTROLLER_RESET = 0x22U, +CO_EM_23_unused = 0x23U, +
+  CO_EM_24_unused = 0x24U, +CO_EM_25_unused = 0x25U, +CO_EM_26_unused = 0x26U, +CO_EM_27_unused = 0x27U, +
+  CO_EM_WRONG_ERROR_REPORT = 0x28U, +CO_EM_ISR_TIMER_OVERFLOW = 0x29U, +CO_EM_MEMORY_ALLOCATION_ERROR = 0x2AU, +CO_EM_GENERIC_ERROR = 0x2BU, +
+  CO_EM_GENERIC_SOFTWARE_ERROR = 0x2CU, +CO_EM_INCONSISTENT_OBJECT_DICT = 0x2DU, +CO_EM_CALCULATION_OF_PARAMETERS = 0x2EU, +CO_EM_NON_VOLATILE_MEMORY = 0x2FU, +
+  CO_EM_MANUFACTURER_START = 0x30U, +CO_EM_MANUFACTURER_END = CO_CONFIG_EM_ERR_STATUS_BITS_COUNT - 1 +
+ }
 Error status bits. More...
 
+ + + + + + + + + + + + + + + + + + + + + + +

+Functions

CO_ReturnError_t CO_EM_init (CO_EM_t *em, const OD_entry_t *OD_1001_errReg, const OD_entry_t *OD_1014_cobIdEm, CO_CANmodule_t *CANdevTx, uint16_t CANdevTxIdx, const OD_entry_t *OD_1015_InhTime, const OD_entry_t *OD_1003_preDefErr, const OD_entry_t *OD_statusBits, CO_CANmodule_t *CANdevRx, uint16_t CANdevRxIdx, const uint8_t nodeId)
 Initialize Emergency object. More...
 
void CO_EM_initCallbackPre (CO_EM_t *em, void *object, void(*pFunctSignal)(void *object))
 Initialize Emergency callback function. More...
 
void CO_EM_initCallbackRx (CO_EM_t *em, void(*pFunctSignalRx)(const uint16_t ident, const uint16_t errorCode, const uint8_t errorRegister, const uint8_t errorBit, const uint32_t infoCode))
 Initialize Emergency received callback function. More...
 
void CO_EM_process (CO_EM_t *em, bool_t NMTisPreOrOperational, uint32_t timeDifference_us, uint32_t *timerNext_us)
 Process Error control and Emergency object. More...
 
void CO_error (CO_EM_t *em, bool_t setError, const uint8_t errorBit, uint16_t errorCode, uint32_t infoCode)
 Set or reset error condition. More...
 
static bool_t CO_isError (CO_EM_t *em, const uint8_t errorBit)
 Check specific error condition. More...
 
static uint8_t CO_getErrorRegister (CO_EM_t *em)
 Get error register. More...
 
+

Detailed Description

+

CANopen Emergency protocol.

+

Error control and Emergency is used for control internal error state and for sending a CANopen Emergency message.

+

In case of error condition stack or application calls CO_errorReport() function with indication of the error. Specific error condition is reported (with CANopen Emergency message) only the first time after it occurs. Internal state of specific error condition is indicated by internal bitfield variable, with space for maximum CO_CONFIG_EM_ERR_STATUS_BITS_COUNT bits. Meaning for each bit is described by CO_EM_errorStatusBits_t. Specific error condition can be reset by CO_errorReset() function. In that case Emergency message is sent with CO_EM_NO_ERROR indication.

+

Some error conditions are informative and some are critical. Critical error conditions set the corresponding bit in CO_errorRegister_t. Critical error conditions for generic error are specified by CO_CONFIG_ERR_CONDITION_GENERIC macro. Similar macros are defined for other error bits in in CO_errorRegister_t.

+

Emergency producer

+

If CO_CONFIG_EM has CO_CONFIG_EM_PRODUCER enabled, then CANopen Emergency message will be sent on each change of any error condition. Emergency message contents are:

+ + + + + + + + + + + +
Byte Description
0..1 CO_EM_errorCode_t
2 CO_errorRegister_t
3 Index of error condition (see CO_EM_errorStatusBits_t).
4..7 Additional informative argument to CO_errorReport() function.
+

Error history

+

If CO_CONFIG_EM has CO_CONFIG_EM_HISTORY enabled, then latest errors can be read from Pre Defined Error Field (object dictionary, index 0x1003). Contents corresponds to bytes 0..3 from the Emergency message.

+

Emergency consumer

+

If CO_CONFIG_EM has CO_CONFIG_EM_CONSUMER enabled, then callback can be registered by CO_EM_initCallbackRx() function.

+

Enumeration Type Documentation

+ +

◆ CO_errorRegister_t

+ +
+
+ + + + +
enum CO_errorRegister_t
+
+ +

CANopen Error register.

+

Mandatory for CANopen, resides in object dictionary, index 0x1001.

+

Error register is calculated from internal bitfield variable, critical bits. See CO_EM_errorStatusBits_t and Emergency producer/consumer for error condition macros.

+

Internal errors may prevent device to stay in NMT Operational state and changes may switch between the states. See CO_NMT_control_t for details.

+ + + + + + + + + +
Enumerator
CO_ERR_REG_GENERIC_ERR 

bit 0, generic error

+
CO_ERR_REG_CURRENT 

bit 1, current

+
CO_ERR_REG_VOLTAGE 

bit 2, voltage

+
CO_ERR_REG_TEMPERATURE 

bit 3, temperature

+
CO_ERR_REG_COMMUNICATION 

bit 4, communication error

+
CO_ERR_REG_DEV_PROFILE 

bit 5, device profile specific

+
CO_ERR_REG_RESERVED 

bit 6, reserved (always 0)

+
CO_ERR_REG_MANUFACTURER 

bit 7, manufacturer specific

+
+ +
+
+ +

◆ CO_EM_errorCode_t

+ +
+
+ + + + +
enum CO_EM_errorCode_t
+
+ +

CANopen Error code.

+

Standard error codes according to CiA DS-301 and DS-401.

+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Enumerator
CO_EMC_NO_ERROR 

0x00xx, error Reset or No Error

+
CO_EMC_GENERIC 

0x10xx, Generic Error

+
CO_EMC_CURRENT 

0x20xx, Current

+
CO_EMC_CURRENT_INPUT 

0x21xx, Current, device input side

+
CO_EMC_CURRENT_INSIDE 

0x22xx, Current inside the device

+
CO_EMC_CURRENT_OUTPUT 

0x23xx, Current, device output side

+
CO_EMC_VOLTAGE 

0x30xx, Voltage

+
CO_EMC_VOLTAGE_MAINS 

0x31xx, Mains Voltage

+
CO_EMC_VOLTAGE_INSIDE 

0x32xx, Voltage inside the device

+
CO_EMC_VOLTAGE_OUTPUT 

0x33xx, Output Voltage

+
CO_EMC_TEMPERATURE 

0x40xx, Temperature

+
CO_EMC_TEMP_AMBIENT 

0x41xx, Ambient Temperature

+
CO_EMC_TEMP_DEVICE 

0x42xx, Device Temperature

+
CO_EMC_HARDWARE 

0x50xx, Device Hardware

+
CO_EMC_SOFTWARE_DEVICE 

0x60xx, Device Software

+
CO_EMC_SOFTWARE_INTERNAL 

0x61xx, Internal Software

+
CO_EMC_SOFTWARE_USER 

0x62xx, User Software

+
CO_EMC_DATA_SET 

0x63xx, Data Set

+
CO_EMC_ADDITIONAL_MODUL 

0x70xx, Additional Modules

+
CO_EMC_MONITORING 

0x80xx, Monitoring

+
CO_EMC_COMMUNICATION 

0x81xx, Communication

+
CO_EMC_CAN_OVERRUN 

0x8110, CAN Overrun (Objects lost)

+
CO_EMC_CAN_PASSIVE 

0x8120, CAN in Error Passive Mode

+
CO_EMC_HEARTBEAT 

0x8130, Life Guard Error or Heartbeat Error

+
CO_EMC_BUS_OFF_RECOVERED 

0x8140, recovered from bus off

+
CO_EMC_CAN_ID_COLLISION 

0x8150, CAN-ID collision

+
CO_EMC_PROTOCOL_ERROR 

0x82xx, Protocol Error

+
CO_EMC_PDO_LENGTH 

0x8210, PDO not processed due to length error

+
CO_EMC_PDO_LENGTH_EXC 

0x8220, PDO length exceeded

+
CO_EMC_DAM_MPDO 

0x8230, DAM MPDO not processed, destination object not available

+
CO_EMC_SYNC_DATA_LENGTH 

0x8240, Unexpected SYNC data length

+
CO_EMC_RPDO_TIMEOUT 

0x8250, RPDO timeout

+
CO_EMC_TIME_DATA_LENGTH 

0x8260, Unexpected TIME data length

+
CO_EMC_EXTERNAL_ERROR 

0x90xx, External Error

+
CO_EMC_ADDITIONAL_FUNC 

0xF0xx, Additional Functions

+
CO_EMC_DEVICE_SPECIFIC 

0xFFxx, Device specific

+
CO_EMC401_OUT_CUR_HI 

0x2310, DS401, Current at outputs too high (overload)

+
CO_EMC401_OUT_SHORTED 

0x2320, DS401, Short circuit at outputs

+
CO_EMC401_OUT_LOAD_DUMP 

0x2330, DS401, Load dump at outputs

+
CO_EMC401_IN_VOLT_HI 

0x3110, DS401, Input voltage too high

+
CO_EMC401_IN_VOLT_LOW 

0x3120, DS401, Input voltage too low

+
CO_EMC401_INTERN_VOLT_HI 

0x3210, DS401, Internal voltage too high

+
CO_EMC401_INTERN_VOLT_LO 

0x3220, DS401, Internal voltage too low

+
CO_EMC401_OUT_VOLT_HIGH 

0x3310, DS401, Output voltage too high

+
CO_EMC401_OUT_VOLT_LOW 

0x3320, DS401, Output voltage too low

+
+ +
+
+ +

◆ CO_EM_errorStatusBits_t

+ +
+
+ + + + +
enum CO_EM_errorStatusBits_t
+
+ +

Error status bits.

+

Bits for internal indication of the error condition. Each error condition is specified by unique index from 0x00 up to 0xFF.

+

If specific error occurs in the stack or in the application, CO_errorReport() sets specific bit in the errorStatusBit variable from CO_EM_t. If bit was already set, function returns without any action. Otherwise it prepares emergency message.

+

Maximum size (in bits) of the errorStatusBit variable is specified by CO_CONFIG_EM_ERR_STATUS_BITS_COUNT (set to 10*8 bits by default). Stack uses first 6 bytes. Additional 4 bytes are pre-defined for manufacturer or device specific error indications, by default.

+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Enumerator
CO_EM_NO_ERROR 

0x00, Error Reset or No Error

+
CO_EM_CAN_BUS_WARNING 

0x01, communication, info, CAN bus warning limit reached

+
CO_EM_RXMSG_WRONG_LENGTH 

0x02, communication, info, Wrong data length of the received CAN message

+
CO_EM_RXMSG_OVERFLOW 

0x03, communication, info, Previous received CAN message wasn't processed yet

+
CO_EM_RPDO_WRONG_LENGTH 

0x04, communication, info, Wrong data length of received PDO

+
CO_EM_RPDO_OVERFLOW 

0x05, communication, info, Previous received PDO wasn't processed yet

+
CO_EM_CAN_RX_BUS_PASSIVE 

0x06, communication, info, CAN receive bus is passive

+
CO_EM_CAN_TX_BUS_PASSIVE 

0x07, communication, info, CAN transmit bus is passive

+
CO_EM_NMT_WRONG_COMMAND 

0x08, communication, info, Wrong NMT command received

+
CO_EM_TIME_TIMEOUT 

0x09, communication, info, TIME message timeout

+
CO_EM_TIME_LENGTH 

0x0A, communication, info, Unexpected TIME data length

+
CO_EM_0B_unused 

0x0B, communication, info, (unused)

+
CO_EM_0C_unused 

0x0C, communication, info, (unused)

+
CO_EM_0D_unused 

0x0D, communication, info, (unused)

+
CO_EM_0E_unused 

0x0E, communication, info, (unused)

+
CO_EM_0F_unused 

0x0F, communication, info, (unused)

+
CO_EM_10_unused 

0x10, communication, critical, (unused)

+
CO_EM_11_unused 

0x11, communication, critical, (unused)

+
CO_EM_CAN_TX_BUS_OFF 

0x12, communication, critical, CAN transmit bus is off

+
CO_EM_CAN_RXB_OVERFLOW 

0x13, communication, critical, CAN module receive buffer has overflowed

+
CO_EM_CAN_TX_OVERFLOW 

0x14, communication, critical, CAN transmit buffer has overflowed

+
CO_EM_TPDO_OUTSIDE_WINDOW 

0x15, communication, critical, TPDO is outside SYNC window

+
CO_EM_16_unused 

0x16, communication, critical, (unused)

+
CO_EM_17_unused 

0x17, communication, critical, (unused)

+
CO_EM_SYNC_TIME_OUT 

0x18, communication, critical, SYNC message timeout

+
CO_EM_SYNC_LENGTH 

0x19, communication, critical, Unexpected SYNC data length

+
CO_EM_PDO_WRONG_MAPPING 

0x1A, communication, critical, Error with PDO mapping

+
CO_EM_HEARTBEAT_CONSUMER 

0x1B, communication, critical, Heartbeat consumer timeout

+
CO_EM_HB_CONSUMER_REMOTE_RESET 

0x1C, communication, critical, Heartbeat consumer detected remote node reset

+
CO_EM_1D_unused 

0x1D, communication, critical, (unused)

+
CO_EM_1E_unused 

0x1E, communication, critical, (unused)

+
CO_EM_1F_unused 

0x1F, communication, critical, (unused)

+
CO_EM_EMERGENCY_BUFFER_FULL 

0x20, generic, info, Emergency buffer is full, Emergency message wasn't sent

+
CO_EM_21_unused 

0x21, generic, info, (unused)

+
CO_EM_MICROCONTROLLER_RESET 

0x22, generic, info, Microcontroller has just started

+
CO_EM_23_unused 

0x23, generic, info, (unused)

+
CO_EM_24_unused 

0x24, generic, info, (unused)

+
CO_EM_25_unused 

0x25, generic, info, (unused)

+
CO_EM_26_unused 

0x26, generic, info, (unused)

+
CO_EM_27_unused 

0x27, generic, info, (unused)

+
CO_EM_WRONG_ERROR_REPORT 

0x28, generic, critical, Wrong parameters to CO_errorReport() function

+
CO_EM_ISR_TIMER_OVERFLOW 

0x29, generic, critical, Timer task has overflowed

+
CO_EM_MEMORY_ALLOCATION_ERROR 

0x2A, generic, critical, Unable to allocate memory for objects

+
CO_EM_GENERIC_ERROR 

0x2B, generic, critical, Generic error, test usage

+
CO_EM_GENERIC_SOFTWARE_ERROR 

0x2C, generic, critical, Software error

+
CO_EM_INCONSISTENT_OBJECT_DICT 

0x2D, generic, critical, Object dictionary does not match the software

+
CO_EM_CALCULATION_OF_PARAMETERS 

0x2E, generic, critical, Error in calculation of device parameters

+
CO_EM_NON_VOLATILE_MEMORY 

0x2F, generic, critical, Error with access to non volatile device memory

+
CO_EM_MANUFACTURER_START 

0x30+, manufacturer, info or critical, Error status buts, free to use by manufacturer.

+

By default bits 0x30..0x3F are set as informational and bits 0x40..0x4F are set as critical. Manufacturer critical bits sets the error register, as specified by CO_CONFIG_ERR_CONDITION_MANUFACTURER

+
CO_EM_MANUFACTURER_END 

(CO_CONFIG_EM_ERR_STATUS_BITS_COUNT - 1), largest value of the Error status bit.

+
+ +
+
+

Function Documentation

+ +

◆ CO_EM_init()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
CO_ReturnError_t CO_EM_init (CO_EM_t * em,
const OD_entry_t * OD_1001_errReg,
const OD_entry_t * OD_1014_cobIdEm,
CO_CANmodule_t * CANdevTx,
uint16_t CANdevTxIdx,
const OD_entry_t * OD_1015_InhTime,
const OD_entry_t * OD_1003_preDefErr,
const OD_entry_t * OD_statusBits,
CO_CANmodule_t * CANdevRx,
uint16_t CANdevRxIdx,
const uint8_t nodeId 
)
+
+ +

Initialize Emergency object.

+

Function must be called in the communication reset section.

+
Parameters
+ + + + + + + + + + + + +
emThis object will be initialized.
OD_1001_errRegOD entry for 0x1001 - "Error register", entry is required, without IO extension.
OD_1014_cobIdEmOD entry for 0x1014 - "COB-ID EMCY", entry is required, IO extension is required.
CANdevTxCAN device for Emergency transmission.
CANdevTxIdxIndex of transmit buffer in the above CAN device.
OD_1015_InhTimeOD entry for 0x1015 - "Inhibit time EMCY", entry is optional (can be NULL), IO extension is optional for runtime configuration.
OD_1003_preDefErrOD entry for 0x1003 - "Pre-defined error field". Emergency object has own memory buffer for this entry. Entry is optional, IO extension is required.
OD_statusBitsCustom OD entry for accessing errorStatusBits from CO_EM_t. Entry must have variable of size (CO_CONFIG_EM_ERR_STATUS_BITS_COUNT/8) bytes available for read/write access on subindex 0. Emergency object has own memory buffer for this entry. Entry is optional, IO extension is required.
CANdevRxCAN device for Emergency consumer reception.
CANdevRxIdxIndex of receive buffer in the above CAN device.
nodeIdCANopen node ID of this device (for default emergency producer)
+
+
+
Returns
CO_ReturnError_t CO_ERROR_NO in case of success.
+ +
+
+ +

◆ CO_EM_initCallbackPre()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + +
void CO_EM_initCallbackPre (CO_EM_t * em,
void * object,
void(*)(void *object) pFunctSignal 
)
+
+ +

Initialize Emergency callback function.

+

Function initializes optional callback function, which should immediately start processing of CO_EM_process() function. Callback is called from CO_errorReport() or CO_errorReset() function. Those functions are fast and may be called from any thread. Callback should immediately start mainline thread, which calls CO_EM_process() function.

+
Parameters
+ + + + +
emThis object.
objectPointer to object, which will be passed to pFunctSignal(). Can be NULL
pFunctSignalPointer to the callback function. Not called if NULL.
+
+
+ +
+
+ +

◆ CO_EM_initCallbackRx()

+ +
+
+ + + + + + + + + + + + + + + + + + +
void CO_EM_initCallbackRx (CO_EM_t * em,
void(*)(const uint16_t ident, const uint16_t errorCode, const uint8_t errorRegister, const uint8_t errorBit, const uint32_t infoCode) pFunctSignalRx 
)
+
+ +

Initialize Emergency received callback function.

+

Function initializes optional callback function, which executes after error condition is received.

+

ident argument from callback contains CAN-ID of the emergency message. If ident == 0, then emergency message was sent from this device.

+
Remarks
Depending on the CAN driver implementation, this function is called inside an ISR or inside a mainline. Must be thread safe.
+
Parameters
+ + + +
emThis object.
pFunctSignalRxPointer to the callback function. Not called if NULL.
+
+
+ +
+
+ +

◆ CO_EM_process()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
void CO_EM_process (CO_EM_t * em,
bool_t NMTisPreOrOperational,
uint32_t timeDifference_us,
uint32_t * timerNext_us 
)
+
+ +

Process Error control and Emergency object.

+

Function must be called cyclically. It verifies some communication errors, calculates OD object 0x1001 - "Error register" and sends emergency message if necessary.

+
Parameters
+ + + + + +
emThis object.
NMTisPreOrOperationalTrue if this node is NMT_PRE_OPERATIONAL or NMT_OPERATIONAL state.
timeDifference_usTime difference from previous function call in [microseconds].
[out]timerNext_usinfo to OS - see CO_process().
+
+
+ +
+
+ +

◆ CO_error()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
void CO_error (CO_EM_t * em,
bool_t setError,
const uint8_t errorBit,
uint16_t errorCode,
uint32_t infoCode 
)
+
+ +

Set or reset error condition.

+

Function can be called on any error condition inside CANopen stack or application. Function first checks change of error condition (setError is true and error bit wasn't set or setError is false and error bit was set before). If changed, then Emergency message is prepared and record in history is added. Emergency message is later sent by CO_EM_process() function.

+

Function is short and thread safe.

+
Parameters
+ + + + + + +
emEmergency object.
setErrorTrue if error occurred or false if error resolved.
errorBitfrom CO_EM_errorStatusBits_t.
errorCodefrom CO_EM_errorCode_t.
infoCode32 bit value is passed to bytes 4...7 of the Emergency message. It contains optional additional information.
+
+
+ +
+
+ +

◆ CO_isError()

+ +
+
+ + + + + +
+ + + + + + + + + + + + + + + + + + +
static bool_t CO_isError (CO_EM_t * em,
const uint8_t errorBit 
)
+ 		+inlinestatic
+
+ +

Check specific error condition.

+

Function returns true, if specific internal error is present.

+
Parameters
+ + + +
emEmergency object.
errorBitfrom CO_EM_errorStatusBits_t.
+
+
+
Returns
true if Error is present.
+ +
+
+ +

◆ CO_getErrorRegister()

+ +
+
+ + + + + +
+ + + + + + + + +
static uint8_t CO_getErrorRegister (CO_EM_t * em)
+ 		+inlinestatic
+
+ +

Get error register.

+
Parameters
+ + +
emEmergency object.
+
+
+
Returns
Error register or 0 if doesn't exist.
+ +
+
+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__Emergency.js b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__Emergency.js new file mode 100755 index 0000000..3f98a41 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__Emergency.js @@ -0,0 +1,142 @@ +var group__CO__Emergency = +[ + [ "CO_Emergency.h", "CO__Emergency_8h.html", null ], + [ "CO_EM_t", "structCO__EM__t.html", [ + [ "errorStatusBits", "structCO__EM__t.html#a377eb478f0af20e6e1e23bd77186dcfd", null ], + [ "errorRegister", "structCO__EM__t.html#ae18bb84c6235afbdf3745d9da337c2d3", null ], + [ "CANerrorStatusOld", "structCO__EM__t.html#a2bbed8454995910f4ac54035a0129b1b", null ], + [ "fifo", "structCO__EM__t.html#ac84d3cf89e04ee48fff85d59bb91f3d9", null ], + [ "fifoWrPtr", "structCO__EM__t.html#aa790e927251322c852c26bdb21853647", null ], + [ "fifoPpPtr", "structCO__EM__t.html#a1cf16a27db5ccf065c68b39e8e2d401f", null ], + [ "fifoOverflow", "structCO__EM__t.html#a52c37c126d1f761b0a9cf608123a5976", null ], + [ "fifoCount", "structCO__EM__t.html#a99ffd8be6baebaf6c598b02c7ce31518", null ], + [ "producerEnabled", "structCO__EM__t.html#ad54685d2f0e7ac934edbe0cf5c0b4baf", null ], + [ "nodeId", "structCO__EM__t.html#ac5522470ed7ea0f5e91520a2563b9abc", null ], + [ "CANdevTx", "structCO__EM__t.html#a5d24b22a05c354937894109a30b1f641", null ], + [ "CANtxBuff", "structCO__EM__t.html#a81e9d98c9384b573a8adefaacec3fdec", null ], + [ "producerCanId", "structCO__EM__t.html#a490ef24607a0b637d9bc3a60f616b41e", null ], + [ "CANdevTxIdx", "structCO__EM__t.html#a9a56cba0d9fada8b489884ec766aab04", null ], + [ "inhibitEmTime_us", "structCO__EM__t.html#a82db41fc720e2f2551207bb0d2ba1ae4", null ], + [ "pFunctSignalRx", "structCO__EM__t.html#a71ba138e5c1814446c210ac7d7f2aa02", null ], + [ "pFunctSignalPre", "structCO__EM__t.html#a124a5d8fb51bb600618a9427b14663c4", null ], + [ "functSignalObjectPre", "structCO__EM__t.html#ac1dec593fd20fbf7ccc5e8287e27b2d8", null ] + ] ], + [ "CO_errorReport", "group__CO__Emergency.html#gab66d4a6daa5f7492704b56a46b135f71", null ], + [ "CO_errorReset", "group__CO__Emergency.html#ga24e2a9311cf704ec6ed43b0ea730c4a3", null ], + [ "CO_errorRegister_t", "group__CO__Emergency.html#ga2cfc261cce03577083ee3f1a31d5e03c", [ + [ "CO_ERR_REG_GENERIC_ERR", "group__CO__Emergency.html#gga2cfc261cce03577083ee3f1a31d5e03ca92a7e121ae04a022fc2fe604eb1c148e", null ], + [ "CO_ERR_REG_CURRENT", "group__CO__Emergency.html#gga2cfc261cce03577083ee3f1a31d5e03ca61eded29fb0fcd95b2f66c2682de0f2b", null ], + [ "CO_ERR_REG_VOLTAGE", "group__CO__Emergency.html#gga2cfc261cce03577083ee3f1a31d5e03ca360c75e04303d1c55e2bc8528407cb87", null ], + [ "CO_ERR_REG_TEMPERATURE", "group__CO__Emergency.html#gga2cfc261cce03577083ee3f1a31d5e03cab12f2b419af0aeb8aae83a13d5c8b7bf", null ], + [ "CO_ERR_REG_COMMUNICATION", "group__CO__Emergency.html#gga2cfc261cce03577083ee3f1a31d5e03ca2f3b7aeac7282281c1d17895406c006a", null ], + [ "CO_ERR_REG_DEV_PROFILE", "group__CO__Emergency.html#gga2cfc261cce03577083ee3f1a31d5e03cab137d1705e9ab20e2caeb22f57dd4716", null ], + [ "CO_ERR_REG_RESERVED", "group__CO__Emergency.html#gga2cfc261cce03577083ee3f1a31d5e03caffdf7f5d9f9ae52fa1bf97a3fb3d848b", null ], + [ "CO_ERR_REG_MANUFACTURER", "group__CO__Emergency.html#gga2cfc261cce03577083ee3f1a31d5e03ca207eee1d9638f61166bc395ee71b84a3", null ] + ] ], + [ "CO_EM_errorCode_t", "group__CO__Emergency.html#ga0653c307fd6bc5238babf48e01c9fa02", [ + [ "CO_EMC_NO_ERROR", "group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02aa60e1333102cbe544eccbaad8e77f6f7", null ], + [ "CO_EMC_GENERIC", "group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a2eaf45ca12b32b7bcc58df91becda767", null ], + [ "CO_EMC_CURRENT", "group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02abad9ca04a37cc43cacabfef9483699cf", null ], + [ "CO_EMC_CURRENT_INPUT", "group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02ab792c971a569d1175666b3fff9ffbe70", null ], + [ "CO_EMC_CURRENT_INSIDE", "group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a3ded1c05cbf37d2d7d286af97e833e65", null ], + [ "CO_EMC_CURRENT_OUTPUT", "group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02ad42e8ab666fd3da75d1fa3a7b8708efc", null ], + [ "CO_EMC_VOLTAGE", "group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a84a8f0dfb97e0ec13be9a4cdb0d71233", null ], + [ "CO_EMC_VOLTAGE_MAINS", "group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02ab4b095d1d9e7e7f5150bc2ecd83bc140", null ], + [ "CO_EMC_VOLTAGE_INSIDE", "group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a78dfa1d496a010ae7ae2e8b6edc1362a", null ], + [ "CO_EMC_VOLTAGE_OUTPUT", "group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a9c5becd591c91bb3e255badf0a308c2d", null ], + [ "CO_EMC_TEMPERATURE", "group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02aa24dfa4c6948187f62d3e8182285d4a3", null ], + [ "CO_EMC_TEMP_AMBIENT", "group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02ae5256d8178374a48750537c3d04c0a30", null ], + [ "CO_EMC_TEMP_DEVICE", "group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a6c29a4b49fae39b45c5c0e553ef6668f", null ], + [ "CO_EMC_HARDWARE", "group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a33344d49b9667151d86aef28a73e6f66", null ], + [ "CO_EMC_SOFTWARE_DEVICE", "group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a6d0b3c0c31228e0bc57fc080c754fefa", null ], + [ "CO_EMC_SOFTWARE_INTERNAL", "group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a62e0949639733e85c2b6d4c8b099d467", null ], + [ "CO_EMC_SOFTWARE_USER", "group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a7b6ae38c015688128890bfe42b0271e5", null ], + [ "CO_EMC_DATA_SET", "group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02ad22329fc3e44867a365401458e691ddc", null ], + [ "CO_EMC_ADDITIONAL_MODUL", "group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02ae210dc1069c7b046527f7d7903ef82cb", null ], + [ "CO_EMC_MONITORING", "group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a8ecd7e45af52d83d986e3de8e957a986", null ], + [ "CO_EMC_COMMUNICATION", "group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02aab2946bf800f551bcae55dd299ff315b", null ], + [ "CO_EMC_CAN_OVERRUN", "group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a1f04b4ffe9cc1d8f2b294261909dec4e", null ], + [ "CO_EMC_CAN_PASSIVE", "group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02aa024c00c21f705474355b9ca7d7ce948", null ], + [ "CO_EMC_HEARTBEAT", "group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02aff47b500e2e760355ca653b247e4b93f", null ], + [ "CO_EMC_BUS_OFF_RECOVERED", "group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a2fd717ed311007b4dd6fe92443f134b0", null ], + [ "CO_EMC_CAN_ID_COLLISION", "group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a683bff5350b0cbab24aef2fc8eac363a", null ], + [ "CO_EMC_PROTOCOL_ERROR", "group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02ab884b23b23af9671d99cca5865549e5a", null ], + [ "CO_EMC_PDO_LENGTH", "group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a80fedd7bbb98ddf1ec26d4b31ed6d749", null ], + [ "CO_EMC_PDO_LENGTH_EXC", "group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a88bac871b7539a579fba73825a2e240a", null ], + [ "CO_EMC_DAM_MPDO", "group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02ab58df03302ab06710f7455d37039dea3", null ], + [ "CO_EMC_SYNC_DATA_LENGTH", "group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a81aa2a66727d1fe29720067dc4e20879", null ], + [ "CO_EMC_RPDO_TIMEOUT", "group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a4ca48c8d1be6a42ac0c13e551e12b230", null ], + [ "CO_EMC_TIME_DATA_LENGTH", "group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a018485be8125a3515ecd127a08e2e2f1", null ], + [ "CO_EMC_EXTERNAL_ERROR", "group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a1d76eff88ebd6050377c393533aebc8d", null ], + [ "CO_EMC_ADDITIONAL_FUNC", "group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02af5f9446049023ceae559562998172278", null ], + [ "CO_EMC_DEVICE_SPECIFIC", "group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02ad7b895b5e7d0f3fa7ff422157ac36c70", null ], + [ "CO_EMC401_OUT_CUR_HI", "group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02aa432d9c66bb0f6eecc38d720cae6c32e", null ], + [ "CO_EMC401_OUT_SHORTED", "group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a182a0c7afc0cb1c30af42a05430da353", null ], + [ "CO_EMC401_OUT_LOAD_DUMP", "group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a21cd31a1455c9dc379796798f0eecd32", null ], + [ "CO_EMC401_IN_VOLT_HI", "group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02aacdc3517e800b037b46c1b54f454b562", null ], + [ "CO_EMC401_IN_VOLT_LOW", "group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a867eb16fce01ade3c728df7c7527e311", null ], + [ "CO_EMC401_INTERN_VOLT_HI", "group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a9cd0f1a897f40a3a43198ba05de4a11b", null ], + [ "CO_EMC401_INTERN_VOLT_LO", "group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02ac122203ed5c6a71749ace599b13ac594", null ], + [ "CO_EMC401_OUT_VOLT_HIGH", "group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02abf5b6a68120351c2fa52146b45798ed2", null ], + [ "CO_EMC401_OUT_VOLT_LOW", "group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a5f262e622db7482b7230055e5b27c902", null ] + ] ], + [ "CO_EM_errorStatusBits_t", "group__CO__Emergency.html#ga587034df9d350c8e121c253f1d4eeacc", [ + [ "CO_EM_NO_ERROR", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeaccafb7b06b4b1d4fb2f9fa8661fdbaf8b01", null ], + [ "CO_EM_CAN_BUS_WARNING", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca27ebb7f155d4b72618c34dd6aa496aac", null ], + [ "CO_EM_RXMSG_WRONG_LENGTH", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeaccae1e45de61059459a6f1f6e500962f287", null ], + [ "CO_EM_RXMSG_OVERFLOW", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca0b17027ee0097065d92e6c0981e3face", null ], + [ "CO_EM_RPDO_WRONG_LENGTH", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca2a36480d4dd6a24f1f8bb66d79441a8d", null ], + [ "CO_EM_RPDO_OVERFLOW", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca73426de91d49273d023b5084a0cea8e0", null ], + [ "CO_EM_CAN_RX_BUS_PASSIVE", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeaccaab5efa11cefb2cd6125cec3ec1c570e1", null ], + [ "CO_EM_CAN_TX_BUS_PASSIVE", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeaccadb8502da626d80a8c423e94e1c76d0cb", null ], + [ "CO_EM_NMT_WRONG_COMMAND", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeaccac5f82aeeda52c83eee0025c8b387ac5d", null ], + [ "CO_EM_TIME_TIMEOUT", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca5c1a6209ebe6167bbf13f565b6fd994d", null ], + [ "CO_EM_TIME_LENGTH", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca3af179820ed2aa88e2c22b7961de71f8", null ], + [ "CO_EM_0B_unused", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca8bf6fb0db21e29e477b38304279bed5e", null ], + [ "CO_EM_0C_unused", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeaccaa724f4fdeff7043b0d4f454613a96992", null ], + [ "CO_EM_0D_unused", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca933c070fd08c1223462a3a331b016c99", null ], + [ "CO_EM_0E_unused", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca0cabb2e45202f938cfdafe8e7871f4f7", null ], + [ "CO_EM_0F_unused", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeaccac6469cc3e6176136f69e549c4a4f5b71", null ], + [ "CO_EM_10_unused", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca274f46ac0760c4c340f48d1de884f2fe", null ], + [ "CO_EM_11_unused", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca9d8abe2f426ed071febf85a932c1df98", null ], + [ "CO_EM_CAN_TX_BUS_OFF", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeaccae59f8e20795915a0929861809ed42e7c", null ], + [ "CO_EM_CAN_RXB_OVERFLOW", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeaccaf9a86c6c3b87763593dd14be6b0bef29", null ], + [ "CO_EM_CAN_TX_OVERFLOW", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca2dbceee7b6deae231bb40a96f8f748a9", null ], + [ "CO_EM_TPDO_OUTSIDE_WINDOW", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeaccaea141284cd85126a9b3e7b0605a26a94", null ], + [ "CO_EM_16_unused", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca865160ae9fdac8fcba1e5335b31c2f9f", null ], + [ "CO_EM_17_unused", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeaccad5bb584bb3c85ca0ba0313367aa75a9b", null ], + [ "CO_EM_SYNC_TIME_OUT", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeaccafd760392f4d4d6358896486c5b5d7d82", null ], + [ "CO_EM_SYNC_LENGTH", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca09a521bfc9ea08ed340cfa29952a471c", null ], + [ "CO_EM_PDO_WRONG_MAPPING", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca7308b487766b8feca60ef0c1b873f167", null ], + [ "CO_EM_HEARTBEAT_CONSUMER", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca6478d414ea45f6a9129e68a9d57e11b7", null ], + [ "CO_EM_HB_CONSUMER_REMOTE_RESET", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca0b6698662476cc622661fb5a5a75ec31", null ], + [ "CO_EM_1D_unused", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca6d0bf9c926241ec8f67c477928300761", null ], + [ "CO_EM_1E_unused", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeaccab2f3562c4e1f8e25a7837627dc1721db", null ], + [ "CO_EM_1F_unused", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeaccadba4afb9dac78f8eb0c5f494926568b1", null ], + [ "CO_EM_EMERGENCY_BUFFER_FULL", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeaccabd1935c51679f70f509ffd60e28c02b1", null ], + [ "CO_EM_21_unused", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeaccadbce7bd9d5a0ee681104914092b21d8d", null ], + [ "CO_EM_MICROCONTROLLER_RESET", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeaccacb69eecc08e72c56aec215fa55e27e16", null ], + [ "CO_EM_23_unused", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeaccaa0c8857afdd8455b30fd0179e98599fb", null ], + [ "CO_EM_24_unused", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca9cf88b48355b3cc43fe9a8360b8470df", null ], + [ "CO_EM_25_unused", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca0a8abc6fcd7b0d5469b469c2cf370a82", null ], + [ "CO_EM_26_unused", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca0398195eafec5f8d60a76f677ce2a714", null ], + [ "CO_EM_27_unused", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca95ca6848349affc579fff2c2a62e87d7", null ], + [ "CO_EM_WRONG_ERROR_REPORT", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca2d7776243205bc75e6c448e13e697480", null ], + [ "CO_EM_ISR_TIMER_OVERFLOW", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeaccadbf7382f537c9f59f965ce38be464e46", null ], + [ "CO_EM_MEMORY_ALLOCATION_ERROR", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca2575dac21ff9ac9c4c4e5ca63d34fdbc", null ], + [ "CO_EM_GENERIC_ERROR", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca21648a2863590d3cccb469f8ef759267", null ], + [ "CO_EM_GENERIC_SOFTWARE_ERROR", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca6c3e7fff310443f05815ea2b7ac6b289", null ], + [ "CO_EM_INCONSISTENT_OBJECT_DICT", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca2c6a108cabca6f03b1400065f2ad4887", null ], + [ "CO_EM_CALCULATION_OF_PARAMETERS", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca5544a90d3047bc08186ea7412528dc93", null ], + [ "CO_EM_NON_VOLATILE_MEMORY", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeaccac019532cacaa8310f9ca413a2f599af3", null ], + [ "CO_EM_MANUFACTURER_START", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeaccaf850a661aadde65b10b22715cf24942c", null ], + [ "CO_EM_MANUFACTURER_END", "group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca5d3c6fdb77551b3f4aaf993ae1dfb414", null ] + ] ], + [ "CO_EM_init", "group__CO__Emergency.html#ga5b80f59df00b71dca7a5c18c139aa71e", null ], + [ "CO_EM_initCallbackPre", "group__CO__Emergency.html#ga94efd78032de3667e2a89780b08aabed", null ], + [ "CO_EM_initCallbackRx", "group__CO__Emergency.html#ga583245c954327c3cf7f9fdb97854e76b", null ], + [ "CO_EM_process", "group__CO__Emergency.html#ga93ae7be6ef966192f5761ce343345d3b", null ], + [ "CO_error", "group__CO__Emergency.html#ga9221f9f631ead4b6f66cfcff8614ba46", null ], + [ "CO_isError", "group__CO__Emergency.html#ga8e9bae71814a3e7bbd8d59d721174c2b", null ], + [ "CO_getErrorRegister", "group__CO__Emergency.html#gaf0c47186d9e51fb91d48385a9f6bad6b", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__GFC.html b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__GFC.html new file mode 100755 index 0000000..edcf60f --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__GFC.html @@ -0,0 +1,299 @@ + + + + + + + +CANopenNode: GFC + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Files | +Data Structures | +Functions
+
+
GFC
CANopen_304
+
+
+ + + + + + + + +

+Files

file  CO_GFC.c
 CANopen Global fail-safe command protocol.
 
file  CO_GFC.h
 CANopen Global fail-safe command protocol.
 
+ + + + +

+Data Structures

struct  CO_GFC_t
 GFC object. More...
 
+ + + + + + + + + + +

+Functions

CO_ReturnError_t CO_GFC_init (CO_GFC_t *GFC, uint8_t *valid, CO_CANmodule_t *GFC_CANdevRx, uint16_t GFC_rxIdx, uint16_t CANidRxGFC, CO_CANmodule_t *GFC_CANdevTx, uint16_t GFC_txIdx, uint16_t CANidTxGFC)
 Initialize GFC object. More...
 
void CO_GFC_initCallbackEnterSafeState (CO_GFC_t *GFC, void *object, void(*pFunctSignalSafe)(void *object))
 Initialize GFC callback function. More...
 
CO_ReturnError_t CO_GFCsend (CO_GFC_t *GFC)
 Send GFC message. More...
 
+

Detailed Description

+

Global fail-safe command protocol.

+

Very simple consumer/producer protocol. A net can have multiple GFC producer and multiple GFC consumer. On a safety-relevant the producer can send a GFC message (ID 0, DLC 0). The consumer can use this message to start the transition to a safe state. The GFC is optional for the security protocol and is not monitored (timed).

+

Function Documentation

+ +

◆ CO_GFC_init()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
CO_ReturnError_t CO_GFC_init (CO_GFC_t * GFC,
uint8_t * valid,
CO_CANmodule_t * GFC_CANdevRx,
uint16_t GFC_rxIdx,
uint16_t CANidRxGFC,
CO_CANmodule_t * GFC_CANdevTx,
uint16_t GFC_txIdx,
uint16_t CANidTxGFC 
)
+
+ +

Initialize GFC object.

+

Function must be called in the communication reset section.

+
Parameters
+ + + + + + + + + +
GFCThis object will be initialized.
validpointer to the valid flag in OD (0x1300)
GFC_CANdevRxCAN device used for SRDO reception.
GFC_rxIdxIndex of receive buffer in the above CAN device.
CANidRxGFCGFC CAN ID for reception
GFC_CANdevTxAN device used for SRDO transmission.
GFC_txIdxIndex of transmit buffer in the above CAN device.
CANidTxGFCGFC CAN ID for transmission
+
+
+
Returns
CO_ReturnError_t: CO_ERROR_NO or CO_ERROR_ILLEGAL_ARGUMENT.
+ +
+
+ +

◆ CO_GFC_initCallbackEnterSafeState()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + +
void CO_GFC_initCallbackEnterSafeState (CO_GFC_t * GFC,
void * object,
void(*)(void *object) pFunctSignalSafe 
)
+
+ +

Initialize GFC callback function.

+

Function initializes optional callback function, that is called when GFC is received. Callback is called from receive function (interrupt).

+
Parameters
+ + + + +
GFCThis object.
objectPointer to object, which will be passed to pFunctSignalSafe(). Can be NULL
pFunctSignalSafePointer to the callback function. Not called if NULL.
+
+
+ +
+
+ +

◆ CO_GFCsend()

+ +
+
+ + + + + + + + +
CO_ReturnError_t CO_GFCsend (CO_GFC_t * GFC)
+
+ +

Send GFC message.

+

It should be called by application, for example after a safety-relevant change.

+
Parameters
+ + +
GFCGFC object.
+
+
+
Returns
Same as CO_CANsend().
+ +
+
+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__GFC.js b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__GFC.js new file mode 100755 index 0000000..4f1db73 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__GFC.js @@ -0,0 +1,15 @@ +var group__CO__GFC = +[ + [ "CO_GFC.c", "CO__GFC_8c.html", null ], + [ "CO_GFC.h", "CO__GFC_8h.html", null ], + [ "CO_GFC_t", "structCO__GFC__t.html", [ + [ "valid", "structCO__GFC__t.html#a775fa3a4f1afda4a4be200f56d6e2b54", null ], + [ "CANdevTx", "structCO__GFC__t.html#a202258a9732622f41d338c22a991f1a5", null ], + [ "CANtxBuff", "structCO__GFC__t.html#acdfbeaf134252ffcb5dd5bd3dcf3c784", null ], + [ "pFunctSignalSafe", "structCO__GFC__t.html#af7ffb43e3b2a682941404bc5c23512e5", null ], + [ "functSignalObjectSafe", "structCO__GFC__t.html#a890395b6bf1b77055c52ad150356ea33", null ] + ] ], + [ "CO_GFC_init", "group__CO__GFC.html#ga23d83d03ef1b9ad5ffe68103a627026c", null ], + [ "CO_GFC_initCallbackEnterSafeState", "group__CO__GFC.html#gaa7cf845381bf150a5816bc068ab9218f", null ], + [ "CO_GFCsend", "group__CO__GFC.html#ga64a30ac6c275d166a2f4117050b12c8c", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__HBconsumer.html b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__HBconsumer.html new file mode 100755 index 0000000..442d55c --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__HBconsumer.html @@ -0,0 +1,776 @@ + + + + + + + +CANopenNode: Heartbeat consumer + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Files | +Data Structures | +Enumerations | +Functions
+
+
Heartbeat consumer
CANopen_301
+
+
+ + + + + +

+Files

file  CO_HBconsumer.h
 CANopen Heartbeat consumer protocol.
 
+ + + + + + + +

+Data Structures

struct  CO_HBconsNode_t
 One monitored node inside CO_HBconsumer_t. More...
 
struct  CO_HBconsumer_t
 Heartbeat consumer object. More...
 
+ + + + +

+Enumerations

enum  CO_HBconsumer_state_t { CO_HBconsumer_UNCONFIGURED = 0x00U, +CO_HBconsumer_UNKNOWN = 0x01U, +CO_HBconsumer_ACTIVE = 0x02U, +CO_HBconsumer_TIMEOUT = 0x03U + }
 Heartbeat state of a node. More...
 
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

+Functions

CO_ReturnError_t CO_HBconsumer_init (CO_HBconsumer_t *HBcons, CO_EM_t *em, CO_SDO_t *SDO, const uint32_t HBconsTime[], CO_HBconsNode_t monitoredNodes[], uint8_t numberOfMonitoredNodes, CO_CANmodule_t *CANdevRx, uint16_t CANdevRxIdxStart)
 Initialize Heartbeat consumer object. More...
 
CO_ReturnError_t CO_HBconsumer_initEntry (CO_HBconsumer_t *HBcons, uint8_t idx, uint8_t nodeId, uint16_t consumerTime_ms)
 Initialize one Heartbeat consumer entry. More...
 
void CO_HBconsumer_initCallbackPre (CO_HBconsumer_t *HBcons, void *object, void(*pFunctSignal)(void *object))
 Initialize Heartbeat consumer callback function. More...
 
void CO_HBconsumer_initCallbackNmtChanged (CO_HBconsumer_t *HBcons, uint8_t idx, void *object, void(*pFunctSignal)(uint8_t nodeId, uint8_t idx, CO_NMT_internalState_t state, void *object))
 Initialize Heartbeat consumer NMT changed callback function. More...
 
void CO_HBconsumer_initCallbackHeartbeatStarted (CO_HBconsumer_t *HBcons, uint8_t idx, void *object, void(*pFunctSignal)(uint8_t nodeId, uint8_t idx, void *object))
 Initialize Heartbeat consumer started callback function. More...
 
void CO_HBconsumer_initCallbackTimeout (CO_HBconsumer_t *HBcons, uint8_t idx, void *object, void(*pFunctSignal)(uint8_t nodeId, uint8_t idx, void *object))
 Initialize Heartbeat consumer timeout callback function. More...
 
void CO_HBconsumer_initCallbackRemoteReset (CO_HBconsumer_t *HBcons, uint8_t idx, void *object, void(*pFunctSignal)(uint8_t nodeId, uint8_t idx, void *object))
 Initialize Heartbeat consumer remote reset detected callback function. More...
 
void CO_HBconsumer_process (CO_HBconsumer_t *HBcons, bool_t NMTisPreOrOperational, uint32_t timeDifference_us, uint32_t *timerNext_us)
 Process Heartbeat consumer object. More...
 
int8_t CO_HBconsumer_getIdxByNodeId (CO_HBconsumer_t *HBcons, uint8_t nodeId)
 Get the heartbeat producer object index by node ID. More...
 
CO_HBconsumer_state_t CO_HBconsumer_getState (CO_HBconsumer_t *HBcons, uint8_t idx)
 Get the current state of a heartbeat producer by the index in OD 0x1016. More...
 
int8_t CO_HBconsumer_getNmtState (CO_HBconsumer_t *HBcons, uint8_t idx, CO_NMT_internalState_t *nmtState)
 Get the current NMT state of a heartbeat producer by the index in OD 0x1016. More...
 
+

Detailed Description

+

CANopen Heartbeat consumer protocol.

+

Heartbeat consumer monitors Heartbeat messages from remote nodes. If any monitored node don't send his Heartbeat in specified time, Heartbeat consumer sends emergency message. If all monitored nodes are operational, then variable allMonitoredOperational inside CO_HBconsumer_t is set to true. Monitoring starts after the reception of the first HeartBeat (not bootup).

+

Heartbeat set up is done by writing to the OD registers 0x1016 or by using the function CO_HBconsumer_initEntry()

+
See also
NMT and Heartbeat
+

Enumeration Type Documentation

+ +

◆ CO_HBconsumer_state_t

+ +
+
+ + + + +
enum CO_HBconsumer_state_t
+
+ +

Heartbeat state of a node.

+ + + + + +
Enumerator
CO_HBconsumer_UNCONFIGURED 

Consumer entry inactive.

+
CO_HBconsumer_UNKNOWN 

Consumer enabled, but no heartbeat received yet.

+
CO_HBconsumer_ACTIVE 

Heartbeat received within set time.

+
CO_HBconsumer_TIMEOUT 

No heatbeat received for set time.

+
+ +
+
+

Function Documentation

+ +

◆ CO_HBconsumer_init()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
CO_ReturnError_t CO_HBconsumer_init (CO_HBconsumer_t * HBcons,
CO_EM_t * em,
CO_SDO_t * SDO,
const uint32_t HBconsTime[],
CO_HBconsNode_t monitoredNodes[],
uint8_t numberOfMonitoredNodes,
CO_CANmodule_t * CANdevRx,
uint16_t CANdevRxIdxStart 
)
+
+ +

Initialize Heartbeat consumer object.

+

Function must be called in the communication reset section.

+
Parameters
+ + + + + + + + + +
HBconsThis object will be initialized.
emEmergency object.
SDOSDO server object.
HBconsTimePointer to Consumer Heartbeat Time array from Object Dictionary (index 0x1016). Size of array is equal to numberOfMonitoredNodes.
monitoredNodesPointer to the externaly defined array of the same size as numberOfMonitoredNodes.
numberOfMonitoredNodesTotal size of the above arrays.
CANdevRxCAN device for Heartbeat reception.
CANdevRxIdxStartStarting index of receive buffer in the above CAN device. Number of used indexes is equal to numberOfMonitoredNodes.
+
+
+
Returns
CO_ReturnError_t CO_ERROR_NO or CO_ERROR_ILLEGAL_ARGUMENT.
+ +
+
+ +

◆ CO_HBconsumer_initEntry()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
CO_ReturnError_t CO_HBconsumer_initEntry (CO_HBconsumer_t * HBcons,
uint8_t idx,
uint8_t nodeId,
uint16_t consumerTime_ms 
)
+
+ +

Initialize one Heartbeat consumer entry.

+

Calling this function has the same affect as writing to the corresponding entries in the Object Dictionary (index 0x1016)

Remarks
The values in the Object Dictionary must be set manually by the calling function so that heartbeat consumer behaviour matches the OD value.
+
Parameters
+ + + + + +
HBconsThis object.
idxindex of the node in HBcons object
nodeIdsee OD 0x1016 description
consumerTime_msin milliseconds. see OD 0x1016 description
+
+
+
Returns
+ +
+
+ +

◆ CO_HBconsumer_initCallbackPre()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + +
void CO_HBconsumer_initCallbackPre (CO_HBconsumer_t * HBcons,
void * object,
void(*)(void *object) pFunctSignal 
)
+
+ +

Initialize Heartbeat consumer callback function.

+

Function initializes optional callback function, which should immediately start processing of CO_HBconsumer_process() function. Callback is called after HBconsumer message is received from the CAN bus.

+
Parameters
+ + + + +
HBconsThis object.
objectPointer to object, which will be passed to pFunctSignal(). Can be NULL
pFunctSignalPointer to the callback function. Not called if NULL.
+
+
+ +
+
+ +

◆ CO_HBconsumer_initCallbackNmtChanged()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
void CO_HBconsumer_initCallbackNmtChanged (CO_HBconsumer_t * HBcons,
uint8_t idx,
void * object,
void(*)(uint8_t nodeId, uint8_t idx, CO_NMT_internalState_t state, void *object) pFunctSignal 
)
+
+ +

Initialize Heartbeat consumer NMT changed callback function.

+

Function initializes optional callback function, which is called when NMT state from the remote node changes.

+
Parameters
+ + + + + +
HBconsThis object.
idxindex of the node in HBcons object (only when CO_CONFIG_HB_CONS_CALLBACK_MULTI is enabled)
objectPointer to object, which will be passed to pFunctSignal(). Can be NULL.
pFunctSignalPointer to the callback function. Not called if NULL.
+
+
+ +
+
+ +

◆ CO_HBconsumer_initCallbackHeartbeatStarted()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
void CO_HBconsumer_initCallbackHeartbeatStarted (CO_HBconsumer_t * HBcons,
uint8_t idx,
void * object,
void(*)(uint8_t nodeId, uint8_t idx, void *object) pFunctSignal 
)
+
+ +

Initialize Heartbeat consumer started callback function.

+

Function initializes optional callback function, which is called for the first received heartbeat after activating hb consumer or timeout. Function may wake up external task, which handles this event.

+
Parameters
+ + + + + +
HBconsThis object.
idxindex of the node in HBcons object
objectPointer to object, which will be passed to pFunctSignal(). Can be NULL
pFunctSignalPointer to the callback function. Not called if NULL.
+
+
+ +
+
+ +

◆ CO_HBconsumer_initCallbackTimeout()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
void CO_HBconsumer_initCallbackTimeout (CO_HBconsumer_t * HBcons,
uint8_t idx,
void * object,
void(*)(uint8_t nodeId, uint8_t idx, void *object) pFunctSignal 
)
+
+ +

Initialize Heartbeat consumer timeout callback function.

+

Function initializes optional callback function, which is called when the node state changes from active to timeout. Function may wake up external task, which handles this event.

+
Parameters
+ + + + + +
HBconsThis object.
idxindex of the node in HBcons object
objectPointer to object, which will be passed to pFunctSignal(). Can be NULL
pFunctSignalPointer to the callback function. Not called if NULL.
+
+
+ +
+
+ +

◆ CO_HBconsumer_initCallbackRemoteReset()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
void CO_HBconsumer_initCallbackRemoteReset (CO_HBconsumer_t * HBcons,
uint8_t idx,
void * object,
void(*)(uint8_t nodeId, uint8_t idx, void *object) pFunctSignal 
)
+
+ +

Initialize Heartbeat consumer remote reset detected callback function.

+

Function initializes optional callback function, which is called when a bootup message is received from the remote node. Function may wake up external task, which handles this event.

+
Parameters
+ + + + + +
HBconsThis object.
idxindex of the node in HBcons object
objectPointer to object, which will be passed to pFunctSignal(). Can be NULL
pFunctSignalPointer to the callback function. Not called if NULL.
+
+
+ +
+
+ +

◆ CO_HBconsumer_process()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
void CO_HBconsumer_process (CO_HBconsumer_t * HBcons,
bool_t NMTisPreOrOperational,
uint32_t timeDifference_us,
uint32_t * timerNext_us 
)
+
+ +

Process Heartbeat consumer object.

+

Function must be called cyclically.

+
Parameters
+ + + + + +
HBconsThis object.
NMTisPreOrOperationalTrue if this node is NMT_PRE_OPERATIONAL or NMT_OPERATIONAL.
timeDifference_usTime difference from previous function call in [microseconds].
[out]timerNext_usinfo to OS - see CO_process().
+
+
+ +
+
+ +

◆ CO_HBconsumer_getIdxByNodeId()

+ +
+
+ + + + + + + + + + + + + + + + + + +
int8_t CO_HBconsumer_getIdxByNodeId (CO_HBconsumer_t * HBcons,
uint8_t nodeId 
)
+
+ +

Get the heartbeat producer object index by node ID.

+
Parameters
+ + + +
HBconsThis object.
nodeIdproducer node ID
+
+
+
Returns
index. -1 if not found
+ +
+
+ +

◆ CO_HBconsumer_getState()

+ +
+
+ + + + + + + + + + + + + + + + + + +
CO_HBconsumer_state_t CO_HBconsumer_getState (CO_HBconsumer_t * HBcons,
uint8_t idx 
)
+
+ +

Get the current state of a heartbeat producer by the index in OD 0x1016.

+
Parameters
+ + + +
HBconsThis object.
idxobject sub index
+
+
+
Returns
CO_HBconsumer_state_t
+ +
+
+ +

◆ CO_HBconsumer_getNmtState()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + +
int8_t CO_HBconsumer_getNmtState (CO_HBconsumer_t * HBcons,
uint8_t idx,
CO_NMT_internalState_t * nmtState 
)
+
+ +

Get the current NMT state of a heartbeat producer by the index in OD 0x1016.

+

NMT state is only available when heartbeat is enabled for this index!

+
Parameters
+ + + + +
HBconsThis object.
idxobject sub index
[out]nmtStateof this index
+
+
+
Return values
+ + + +
0NMT state has been received and is valid
-1not valid
+
+
+ +
+
+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__HBconsumer.js b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__HBconsumer.js new file mode 100755 index 0000000..98b206f --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__HBconsumer.js @@ -0,0 +1,53 @@ +var group__CO__HBconsumer = +[ + [ "CO_HBconsumer.h", "CO__HBconsumer_8h.html", null ], + [ "CO_HBconsNode_t", "structCO__HBconsNode__t.html", [ + [ "nodeId", "structCO__HBconsNode__t.html#a180aca37057c670be35bbdd89f72b812", null ], + [ "NMTstate", "structCO__HBconsNode__t.html#aca87186237691cc315da47d5bcc8ad31", null ], + [ "HBstate", "structCO__HBconsNode__t.html#a6d16bde174d37094149343fcc7025e3c", null ], + [ "timeoutTimer", "structCO__HBconsNode__t.html#ac4ffced8a11aac2b7383244f306c2081", null ], + [ "time_us", "structCO__HBconsNode__t.html#a489df7aff00a25d8f2a63fe968050b08", null ], + [ "CANrxNew", "structCO__HBconsNode__t.html#a4050f7d0406d85db643410cbca65fd14", null ], + [ "pFunctSignalPre", "structCO__HBconsNode__t.html#a61c753c38666dda8a4d4d870fc593ae5", null ], + [ "functSignalObjectPre", "structCO__HBconsNode__t.html#afd6a4fcc8bc4cc9c647e9ef3d91b2431", null ], + [ "NMTstatePrev", "structCO__HBconsNode__t.html#a7fd2636d9f46b7ff47676f716f6f00a4", null ], + [ "pFunctSignalNmtChanged", "structCO__HBconsNode__t.html#a3f4ec7dfe9e47a3e7a9bdad382fd7f56", null ], + [ "pFunctSignalObjectNmtChanged", "structCO__HBconsNode__t.html#a2a1c5abd88c2ecd451e2a1ca65d3dc93", null ], + [ "pFunctSignalHbStarted", "structCO__HBconsNode__t.html#ac44d2a232ca2b352fd717ee8e6f28e90", null ], + [ "functSignalObjectHbStarted", "structCO__HBconsNode__t.html#a50ed8caa11fce685dfbba13d11ada0ef", null ], + [ "pFunctSignalTimeout", "structCO__HBconsNode__t.html#ab6bbeee344ebd1d657bbabc02f51e597", null ], + [ "functSignalObjectTimeout", "structCO__HBconsNode__t.html#aaf6cc300976931c02e3d46ec2b75cc2e", null ], + [ "pFunctSignalRemoteReset", "structCO__HBconsNode__t.html#a7f7ccf80c31d4c764db24b70f9111e7b", null ], + [ "functSignalObjectRemoteReset", "structCO__HBconsNode__t.html#a94b3c3b2b5b24f5ea3ecc1bbc14b79dd", null ] + ] ], + [ "CO_HBconsumer_t", "structCO__HBconsumer__t.html", [ + [ "em", "structCO__HBconsumer__t.html#aae5e363ccc6a6fd3b17a35e0430add2a", null ], + [ "HBconsTime", "structCO__HBconsumer__t.html#a1cd314f387357f2ce13d4093f477fff5", null ], + [ "monitoredNodes", "structCO__HBconsumer__t.html#a737b37c544a28eff8de0b03b51cbeec8", null ], + [ "numberOfMonitoredNodes", "structCO__HBconsumer__t.html#a5b944043074d42017be3b76320030542", null ], + [ "allMonitoredActive", "structCO__HBconsumer__t.html#aaff60bb59e36a3b0ddd11b45268eaf33", null ], + [ "allMonitoredOperational", "structCO__HBconsumer__t.html#a9407103796db857229ec5b266c580b37", null ], + [ "NMTisPreOrOperationalPrev", "structCO__HBconsumer__t.html#a2fe3d81e2124918d0d5947e6891a060e", null ], + [ "CANdevRx", "structCO__HBconsumer__t.html#af5d8828478e2b51fe47a63f24dd896b1", null ], + [ "CANdevRxIdxStart", "structCO__HBconsumer__t.html#a00d176c84d169115399c276031b71722", null ], + [ "pFunctSignalNmtChanged", "structCO__HBconsumer__t.html#a58314f7d35efc2dbeae14ba4be76dec1", null ], + [ "pFunctSignalObjectNmtChanged", "structCO__HBconsumer__t.html#ad583c93f4e59f98669cd18f263aee45a", null ] + ] ], + [ "CO_HBconsumer_state_t", "group__CO__HBconsumer.html#ga7658e41b7c045b7b612e4ef8a2b663f3", [ + [ "CO_HBconsumer_UNCONFIGURED", "group__CO__HBconsumer.html#gga7658e41b7c045b7b612e4ef8a2b663f3a4c481c1ba58fb71b9870e8b355351211", null ], + [ "CO_HBconsumer_UNKNOWN", "group__CO__HBconsumer.html#gga7658e41b7c045b7b612e4ef8a2b663f3af36bc5f46fd11044dc2d1d995ad8f28b", null ], + [ "CO_HBconsumer_ACTIVE", "group__CO__HBconsumer.html#gga7658e41b7c045b7b612e4ef8a2b663f3ad80c78b38e6d28927bf3d1b1464b36e9", null ], + [ "CO_HBconsumer_TIMEOUT", "group__CO__HBconsumer.html#gga7658e41b7c045b7b612e4ef8a2b663f3a8a7ac49e5c809994ee65f365a7a75f22", null ] + ] ], + [ "CO_HBconsumer_init", "group__CO__HBconsumer.html#gacc31c4848a14c9c367505b20e4a6a496", null ], + [ "CO_HBconsumer_initEntry", "group__CO__HBconsumer.html#gaf4bfa2bbd2b7d70f25b6bf173932170a", null ], + [ "CO_HBconsumer_initCallbackPre", "group__CO__HBconsumer.html#ga2faa596dcebbec8f486788a791d638be", null ], + [ "CO_HBconsumer_initCallbackNmtChanged", "group__CO__HBconsumer.html#gabab4b2dd74f6e341fe8b683f7a6d56f3", null ], + [ "CO_HBconsumer_initCallbackHeartbeatStarted", "group__CO__HBconsumer.html#ga6c9bd0df815428719b9f9429ed4476a9", null ], + [ "CO_HBconsumer_initCallbackTimeout", "group__CO__HBconsumer.html#gaef359610a0cdd1331da266be9c55c2d2", null ], + [ "CO_HBconsumer_initCallbackRemoteReset", "group__CO__HBconsumer.html#ga8758a7bd92aa458b90d5da9221cc694f", null ], + [ "CO_HBconsumer_process", "group__CO__HBconsumer.html#ga29e01b5fe6392ce688e8ac57d966258f", null ], + [ "CO_HBconsumer_getIdxByNodeId", "group__CO__HBconsumer.html#ga041b92d6feb1774cb7eb87fba842fdf2", null ], + [ "CO_HBconsumer_getState", "group__CO__HBconsumer.html#ga7c5d4eccbcb0f1f8965a336fde04e765", null ], + [ "CO_HBconsumer_getNmtState", "group__CO__HBconsumer.html#ga1731e3860fce5ca5d341d9b7fc32d8d6", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__LEDs.html b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__LEDs.html new file mode 100755 index 0000000..60f3019 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__LEDs.html @@ -0,0 +1,354 @@ + + + + + + + +CANopenNode: LED indicators + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Files | +Data Structures | +Macros | +Enumerations | +Functions
+
+
LED indicators
CANopen_303
+
+
+ + + + + +

+Files

file  CO_LEDs.h
 CANopen Indicator specification (CiA 303-3 v1.4.0)
 
+ + + + +

+Data Structures

struct  CO_LEDs_t
 LEDs object, initialized by CO_LEDs_init() More...
 
+ + + + + + + +

+Macros

+#define CO_LED_RED(LEDs, BITFIELD)   (((LEDs)->LEDred & BITFIELD) ? 1 : 0)
 Get on/off state for green led for specified bitfield.
 
+#define CO_LED_GREEN(LEDs, BITFIELD)   (((LEDs)->LEDgreen & BITFIELD) ? 1 : 0)
 Get on/off state for green led for specified bitfield.
 
+ + + + +

+Enumerations

enum  CO_LED_BITFIELD_t {
+  CO_LED_flicker = 0x01, +CO_LED_blink = 0x02, +CO_LED_flash_1 = 0x04, +CO_LED_flash_2 = 0x08, +
+  CO_LED_flash_3 = 0x10, +CO_LED_flash_4 = 0x20, +CO_LED_CANopen = 0x80 +
+ }
 Bitfield for combining with red or green led. More...
 
+ + + + + + + +

+Functions

CO_ReturnError_t CO_LEDs_init (CO_LEDs_t *LEDs)
 Initialize LEDs object. More...
 
void CO_LEDs_process (CO_LEDs_t *LEDs, uint32_t timeDifference_us, CO_NMT_internalState_t NMTstate, bool_t LSSconfig, bool_t ErrCANbusOff, bool_t ErrCANbusWarn, bool_t ErrRpdo, bool_t ErrSync, bool_t ErrHbCons, bool_t ErrOther, bool_t firmwareDownload, uint32_t *timerNext_us)
 Process indicator states. More...
 
+

Detailed Description

+

CIA 303-3 standard specifies indicator LED diodes, which reflects state of the CANopen device. Green and red leds or bi-color led can be used.

+

CANopen green led - run led:

    +
  • flickering: LSS configuration state is active
    • +
  • blinking: device is in NMT pre-operational state
    • +
  • single flash: device is in NMT stopped state
    • +
  • triple flash: a software download is running in the device
    • +
  • on: device is in NMT operational state
    • +
+

CANopen red led - error led:

    +
  • off: no error
    • +
  • flickering: LSS node id is not configured, CANopen is not initialized
    • +
  • blinking: invalid configuration, general error
    • +
  • single flash: CAN warning limit reached
    • +
  • double flash: heartbeat consumer - error in remote monitored node
    • +
  • triple flash: sync message reception timeout
    • +
  • quadruple flash: PDO has not been received before the event timer elapsed
    • +
  • on: CAN bus off
    • +
+

To apply on/off state to led diode, use CO_LED_RED and CO_LED_GREEN macros. For CANopen leds use CO_LED_BITFIELD_t CO_LED_CANopen. Other bitfields are available for implementing custom leds.

+

Enumeration Type Documentation

+ +

◆ CO_LED_BITFIELD_t

+ +
+
+ + + + +
enum CO_LED_BITFIELD_t
+
+ +

Bitfield for combining with red or green led.

+ + + + + + + + +
Enumerator
CO_LED_flicker 

LED flickering 10Hz.

+
CO_LED_blink 

LED blinking 2,5Hz.

+
CO_LED_flash_1 

LED single flash.

+
CO_LED_flash_2 

LED double flash.

+
CO_LED_flash_3 

LED triple flash.

+
CO_LED_flash_4 

LED quadruple flash.

+
CO_LED_CANopen 

LED CANopen according to CiA 303-3.

+
+ +
+
+

Function Documentation

+ +

◆ CO_LEDs_init()

+ +
+
+ + + + + + + + +
CO_ReturnError_t CO_LEDs_init (CO_LEDs_t * LEDs)
+
+ +

Initialize LEDs object.

+

Function must be called in the communication reset section.

+
Parameters
+ + +
LEDsThis object will be initialized.
+
+
+
Returns
CO_ReturnError_t CO_ERROR_NO or CO_ERROR_ILLEGAL_ARGUMENT.
+ +
+
+ +

◆ CO_LEDs_process()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
void CO_LEDs_process (CO_LEDs_t * LEDs,
uint32_t timeDifference_us,
CO_NMT_internalState_t NMTstate,
bool_t LSSconfig,
bool_t ErrCANbusOff,
bool_t ErrCANbusWarn,
bool_t ErrRpdo,
bool_t ErrSync,
bool_t ErrHbCons,
bool_t ErrOther,
bool_t firmwareDownload,
uint32_t * timerNext_us 
)
+
+ +

Process indicator states.

+

Function must be called cyclically.

+
Parameters
+ + + + + + + + + + + + + +
LEDsThis object.
timeDifference_usTime difference from previous function call in [microseconds].
NMTstateNMT operating state.
LSSconfigNode is in LSS configuration state indication.
ErrCANbusOffCAN bus off indication (highest priority).
ErrCANbusWarnCAN error warning limit reached indication.
ErrRpdoRPDO event timer timeout indication.
ErrSyncSync receive timeout indication.
ErrHbConsHeartbeat consumer error (remote node) indication.
ErrOtherOther error indication (lowest priority).
firmwareDownloadFirmware download is in progress indication.
[out]timerNext_usinfo to OS - see CO_process().
+
+
+ +
+
+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__LEDs.js b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__LEDs.js new file mode 100755 index 0000000..df4b5e7 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__LEDs.js @@ -0,0 +1,27 @@ +var group__CO__LEDs = +[ + [ "CO_LEDs.h", "CO__LEDs_8h.html", null ], + [ "CO_LEDs_t", "structCO__LEDs__t.html", [ + [ "LEDtmr50ms", "structCO__LEDs__t.html#ae27f4bbc313e08e25b9b1e1d515ea9e4", null ], + [ "LEDtmr200ms", "structCO__LEDs__t.html#a2690fe669ce01322e14de1b9f559a147", null ], + [ "LEDtmrflash_1", "structCO__LEDs__t.html#a2431d9736416e1b5b6c57f61f029dc04", null ], + [ "LEDtmrflash_2", "structCO__LEDs__t.html#ae6aa2a97a9496abc67774d94caa0ea1a", null ], + [ "LEDtmrflash_3", "structCO__LEDs__t.html#ad3af9ed6c2c43330cc3eadb3b76de2c4", null ], + [ "LEDtmrflash_4", "structCO__LEDs__t.html#ae33491a6eed2761893fea47dd65c5a78", null ], + [ "LEDred", "structCO__LEDs__t.html#ad8624f0baa6186e523e072e6101c7a84", null ], + [ "LEDgreen", "structCO__LEDs__t.html#ac2b4eb053725681f1935c8a6e4184f85", null ] + ] ], + [ "CO_LED_RED", "group__CO__LEDs.html#ga38a415372f20b9444f254a205aa511e8", null ], + [ "CO_LED_GREEN", "group__CO__LEDs.html#ga3e01e6ec590d6d5c7b887b48557498f4", null ], + [ "CO_LED_BITFIELD_t", "group__CO__LEDs.html#ga366de3822a3da8478e97b248bed641fb", [ + [ "CO_LED_flicker", "group__CO__LEDs.html#gga366de3822a3da8478e97b248bed641fba9838518b974c263a401a089901cdcf54", null ], + [ "CO_LED_blink", "group__CO__LEDs.html#gga366de3822a3da8478e97b248bed641fbaa24d4647d37adc17e1d3c242a42f6b68", null ], + [ "CO_LED_flash_1", "group__CO__LEDs.html#gga366de3822a3da8478e97b248bed641fba88e905d94927b3c626b50a48537c7b73", null ], + [ "CO_LED_flash_2", "group__CO__LEDs.html#gga366de3822a3da8478e97b248bed641fbae751e14d72d829b2b7f9a9c1e98e0612", null ], + [ "CO_LED_flash_3", "group__CO__LEDs.html#gga366de3822a3da8478e97b248bed641fba250a08a71c4bbca9761f0dfa54d37938", null ], + [ "CO_LED_flash_4", "group__CO__LEDs.html#gga366de3822a3da8478e97b248bed641fbadd3ec0da4d999e5ffc107d7891c26667", null ], + [ "CO_LED_CANopen", "group__CO__LEDs.html#gga366de3822a3da8478e97b248bed641fbaea338dad48dda75ef1ebac0948093148", null ] + ] ], + [ "CO_LEDs_init", "group__CO__LEDs.html#gadafdaf5de4c227a3df17cbcf1d81be69", null ], + [ "CO_LEDs_process", "group__CO__LEDs.html#gac008ef501f913c9df1ee79c4b071ad80", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__LSS.html b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__LSS.html new file mode 100755 index 0000000..25e54c5 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__LSS.html @@ -0,0 +1,505 @@ + + + + + + + +CANopenNode: LSS + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Files | +Data Structures | +Macros | +Enumerations | +Variables
+
+
LSS
CANopen_305
+
+
+ + + + + + + + + + + +

+Files

file  CO_LSS.h
 CANopen Layer Setting Services protocol (common).
 
file  CO_LSSmaster.h
 CANopen Layer Setting Service - master protocol.
 
file  CO_LSSslave.h
 CANopen Layer Setting Service - slave protocol.
 
+ + + + +

+Data Structures

union  CO_LSS_address_t
 The LSS address is a 128 bit number, uniquely identifying each node. More...
 
+ + + + + + + + + + + + + +

+Macros

+#define CO_LSS_BIT_TIMING_VALID(index)   (index != 5 && (index >= CO_LSS_BIT_TIMING_1000 && index <= CO_LSS_BIT_TIMING_AUTO))
 Macro to check if index contains valid bit timing.
 
+#define CO_LSS_NODE_ID_ASSIGNMENT   0xFFU
 Invalid node ID triggers node ID assignment.
 
+#define CO_LSS_NODE_ID_VALID(nid)   ((nid >= 1 && nid <= 0x7F) || nid == CO_LSS_NODE_ID_ASSIGNMENT)
 Macro to check if node id is valid.
 
+#define CO_LSS_ADDRESS_EQUAL(a1, a2)
 Macro to check if two LSS addresses are equal.
 
+ + + + + + + + + + + + + + + + + + + + + + + + + +

+Enumerations

enum  CO_LSS_cs_t {
+  CO_LSS_SWITCH_STATE_GLOBAL = 0x04U, +CO_LSS_SWITCH_STATE_SEL_VENDOR = 0x40U, +CO_LSS_SWITCH_STATE_SEL_PRODUCT = 0x41U, +CO_LSS_SWITCH_STATE_SEL_REV = 0x42U, +
+  CO_LSS_SWITCH_STATE_SEL_SERIAL = 0x43U, +CO_LSS_SWITCH_STATE_SEL = 0x44U, +CO_LSS_CFG_NODE_ID = 0x11U, +CO_LSS_CFG_BIT_TIMING = 0x13U, +
+  CO_LSS_CFG_ACTIVATE_BIT_TIMING = 0x15U, +CO_LSS_CFG_STORE = 0x17U, +CO_LSS_IDENT_SLAVE = 0x4FU, +CO_LSS_IDENT_FASTSCAN = 0x51U, +
+  CO_LSS_INQUIRE_VENDOR = 0x5AU, +CO_LSS_INQUIRE_PRODUCT = 0x5BU, +CO_LSS_INQUIRE_REV = 0x5CU, +CO_LSS_INQUIRE_SERIAL = 0x5DU, +
+  CO_LSS_INQUIRE_NODE_ID = 0x5EU +
+ }
 LSS protocol command specifiers. More...
 
enum  CO_LSS_cfgNodeId_t { CO_LSS_CFG_NODE_ID_OK = 0x00U, +CO_LSS_CFG_NODE_ID_OUT_OF_RANGE = 0x01U, +CO_LSS_CFG_NODE_ID_MANUFACTURER = 0xFFU + }
 Error codes for Configure node ID protocol. More...
 
enum  CO_LSS_cfgBitTiming_t { CO_LSS_CFG_BIT_TIMING_OK = 0x00U, +CO_LSS_CFG_BIT_TIMING_OUT_OF_RANGE = 0x01U, +CO_LSS_CFG_BIT_TIMING_MANUFACTURER = 0xFFU + }
 Error codes for Configure bit timing parameters protocol. More...
 
enum  CO_LSS_cfgStore_t { CO_LSS_CFG_STORE_OK = 0x00U, +CO_LSS_CFG_STORE_NOT_SUPPORTED = 0x01U, +CO_LSS_CFG_STORE_FAILED = 0x02U, +CO_LSS_CFG_STORE_MANUFACTURER = 0xFFU + }
 Error codes for Store configuration protocol. More...
 
enum  CO_LSS_fastscan_bitcheck { CO_LSS_FASTSCAN_BIT0 = 0x00U, +CO_LSS_FASTSCAN_BIT31 = 0x1FU, +CO_LSS_FASTSCAN_CONFIRM = 0x80U + }
 Fastscan BitCheck. More...
 
enum  CO_LSS_fastscan_lss_sub_next { CO_LSS_FASTSCAN_VENDOR_ID = 0, +CO_LSS_FASTSCAN_PRODUCT = 1, +CO_LSS_FASTSCAN_REV = 2, +CO_LSS_FASTSCAN_SERIAL = 3 + }
 Fastscan LSSsub, LSSnext. More...
 
enum  CO_LSS_state_t { CO_LSS_STATE_WAITING = 0, +CO_LSS_STATE_CONFIGURATION = 1 + }
 LSS finite state automaton. More...
 
enum  CO_LSS_bitTimingTable_t {
+  CO_LSS_BIT_TIMING_1000 = 0, +CO_LSS_BIT_TIMING_800 = 1, +CO_LSS_BIT_TIMING_500 = 2, +CO_LSS_BIT_TIMING_250 = 3, +
+  CO_LSS_BIT_TIMING_125 = 4, +CO_LSS_BIT_TIMING_50 = 6, +CO_LSS_BIT_TIMING_20 = 7, +CO_LSS_BIT_TIMING_10 = 8, +
+  CO_LSS_BIT_TIMING_AUTO = 9 +
+ }
 Definition of table_index for /CiA301/ bit timing table. More...
 
+ + + + +

+Variables

+static const uint16_t CO_LSS_bitTimingTableLookup []
 Lookup table for conversion between bit timing table and numerical bit rate.
 
+

Detailed Description

+

CANopen Layer Setting Services protocol (common).

+

LSS protocol is according to CiA DSP 305 V3.0.0.

+

LSS services and protocols are used to inquire or to change the settings of three parameters of the physical layer, data link layer, and application layer on a CANopen device with LSS slave capability by a CANopen device with LSS master capability via the CAN network.

+

The following parameters may be inquired or changed:

    +
  • Node-ID of the CANopen device
    • +
  • Bit timing parameters of the physical layer (bit rate)
    • +
  • LSS address compliant to the identity object (1018h)
    • +
+

The connection is established in one of two ways:

    +
  • addressing a node by it's 128 bit LSS address. This requires that the master already knows the node's LSS address.
    • +
  • scanning the network for unknown nodes (Fastscan). Using this method, unknown devices can be found and configured one by one.
    • +
+

Be aware that changing the bit rate is a critical step for the network. A failure will render the network unusable!

+

Using this implementation, only master or slave can be included in one node at a time.

+

For CAN identifiers see CO_Default_CAN_ID_t

+

Enumeration Type Documentation

+ +

◆ CO_LSS_cs_t

+ +
+
+ + + + +
enum CO_LSS_cs_t
+
+ +

LSS protocol command specifiers.

+

The LSS protocols are executed between the LSS master device and the LSS slave device(s) to implement the LSS services. Some LSS protocols require a sequence of CAN messages.

+

As identifying method only "LSS fastscan" is supported.

+ + + + + + + + + + + + + + + + + + +
Enumerator
CO_LSS_SWITCH_STATE_GLOBAL 

Switch state global protocol.

+
CO_LSS_SWITCH_STATE_SEL_VENDOR 

Switch state selective protocol - Vendor ID.

+
CO_LSS_SWITCH_STATE_SEL_PRODUCT 

Switch state selective protocol - Product code.

+
CO_LSS_SWITCH_STATE_SEL_REV 

Switch state selective protocol - Revision number.

+
CO_LSS_SWITCH_STATE_SEL_SERIAL 

Switch state selective protocol - Serial number.

+
CO_LSS_SWITCH_STATE_SEL 

Switch state selective protocol - Slave response.

+
CO_LSS_CFG_NODE_ID 

Configure node ID protocol.

+
CO_LSS_CFG_BIT_TIMING 

Configure bit timing parameter protocol.

+
CO_LSS_CFG_ACTIVATE_BIT_TIMING 

Activate bit timing parameter protocol.

+
CO_LSS_CFG_STORE 

Store configuration protocol.

+
CO_LSS_IDENT_SLAVE 

LSS Fastscan response.

+
CO_LSS_IDENT_FASTSCAN 

LSS Fastscan protocol.

+
CO_LSS_INQUIRE_VENDOR 

Inquire identity vendor-ID protocol.

+
CO_LSS_INQUIRE_PRODUCT 

Inquire identity product-code protocol.

+
CO_LSS_INQUIRE_REV 

Inquire identity revision-number protocol.

+
CO_LSS_INQUIRE_SERIAL 

Inquire identity serial-number protocol.

+
CO_LSS_INQUIRE_NODE_ID 

Inquire node-ID protocol.

+
+ +
+
+ +

◆ CO_LSS_cfgNodeId_t

+ +
+
+ + + + +
enum CO_LSS_cfgNodeId_t
+
+ +

Error codes for Configure node ID protocol.

+ + + + +
Enumerator
CO_LSS_CFG_NODE_ID_OK 

Protocol successfully completed.

+
CO_LSS_CFG_NODE_ID_OUT_OF_RANGE 

NID out of range.

+
CO_LSS_CFG_NODE_ID_MANUFACTURER 

Manufacturer specific error.

+

No further support

+
+ +
+
+ +

◆ CO_LSS_cfgBitTiming_t

+ +
+
+ + + + +
enum CO_LSS_cfgBitTiming_t
+
+ +

Error codes for Configure bit timing parameters protocol.

+ + + + +
Enumerator
CO_LSS_CFG_BIT_TIMING_OK 

Protocol successfully completed.

+
CO_LSS_CFG_BIT_TIMING_OUT_OF_RANGE 

Bit timing / Bit rate not supported.

+
CO_LSS_CFG_BIT_TIMING_MANUFACTURER 

Manufacturer specific error.

+

No further support

+
+ +
+
+ +

◆ CO_LSS_cfgStore_t

+ +
+
+ + + + +
enum CO_LSS_cfgStore_t
+
+ +

Error codes for Store configuration protocol.

+ + + + + +
Enumerator
CO_LSS_CFG_STORE_OK 

Protocol successfully completed.

+
CO_LSS_CFG_STORE_NOT_SUPPORTED 

Store configuration not supported.

+
CO_LSS_CFG_STORE_FAILED 

Storage media access error.

+
CO_LSS_CFG_STORE_MANUFACTURER 

Manufacturer specific error.

+

No further support

+
+ +
+
+ +

◆ CO_LSS_fastscan_bitcheck

+ +
+
+ + + + +
enum CO_LSS_fastscan_bitcheck
+
+ +

Fastscan BitCheck.

+

BIT0 means all bits are checked for equality by slave.

+ + + + +
Enumerator
CO_LSS_FASTSCAN_BIT0 

Least significant bit of IDnumbners bit area to be checked.

+
CO_LSS_FASTSCAN_BIT31 

dito

+
CO_LSS_FASTSCAN_CONFIRM 

All LSS slaves waiting for scan respond and previous scan is reset.

+
+ +
+
+ +

◆ CO_LSS_fastscan_lss_sub_next

+ +
+
+ + + + +
enum CO_LSS_fastscan_lss_sub_next
+
+ +

Fastscan LSSsub, LSSnext.

+ + + + + +
Enumerator
CO_LSS_FASTSCAN_VENDOR_ID 

Vendor ID.

+
CO_LSS_FASTSCAN_PRODUCT 

Product code.

+
CO_LSS_FASTSCAN_REV 

Revision number.

+
CO_LSS_FASTSCAN_SERIAL 

Serial number.

+
+ +
+
+ +

◆ CO_LSS_state_t

+ +
+
+ + + + +
enum CO_LSS_state_t
+
+ +

LSS finite state automaton.

+

The LSS FSA shall provide the following states:

    +
  • Initial: Pseudo state, indicating the activation of the FSA.
    • +
  • LSS waiting: In this state, the LSS slave device waits for requests.
    • +
  • LSS configuration: In this state variables may be configured in the LSS slave.
    • +
  • Final: Pseudo state, indicating the deactivation of the FSA.
    • +
+ + + +
Enumerator
CO_LSS_STATE_WAITING 

LSS FSA waiting for requests.

+
CO_LSS_STATE_CONFIGURATION 

LSS FSA waiting for configuration.

+
+ +
+
+ +

◆ CO_LSS_bitTimingTable_t

+ +
+
+ + + + +
enum CO_LSS_bitTimingTable_t
+
+ +

Definition of table_index for /CiA301/ bit timing table.

+ + + + + + + + + + +
Enumerator
CO_LSS_BIT_TIMING_1000 

1000kbit/s

+
CO_LSS_BIT_TIMING_800 

800kbit/s

+
CO_LSS_BIT_TIMING_500 

500kbit/s

+
CO_LSS_BIT_TIMING_250 

250kbit/s

+
CO_LSS_BIT_TIMING_125 

125kbit/s

+
CO_LSS_BIT_TIMING_50 

50kbit/s

+
CO_LSS_BIT_TIMING_20 

20kbit/s

+
CO_LSS_BIT_TIMING_10 

10kbit/s

+
CO_LSS_BIT_TIMING_AUTO 

Automatic bit rate detection.

+
+ +
+
+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__LSS.js b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__LSS.js new file mode 100755 index 0000000..90addc4 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__LSS.js @@ -0,0 +1,73 @@ +var group__CO__LSS = +[ + [ "CO_LSS.h", "CO__LSS_8h.html", null ], + [ "CO_LSSmaster.h", "CO__LSSmaster_8h.html", null ], + [ "CO_LSSslave.h", "CO__LSSslave_8h.html", null ], + [ "CO_LSS_address_t", "unionCO__LSS__address__t.html", null ], + [ "CO_LSS_BIT_TIMING_VALID", "group__CO__LSS.html#gaf1a3d7df8dcd93e4a0e0b29aa6b003a1", null ], + [ "CO_LSS_NODE_ID_ASSIGNMENT", "group__CO__LSS.html#ga02771497ab59dd86f2dbe59cd1fb04b1", null ], + [ "CO_LSS_NODE_ID_VALID", "group__CO__LSS.html#ga939b17fdd44126fb758db46b9cadf79c", null ], + [ "CO_LSS_ADDRESS_EQUAL", "group__CO__LSS.html#ga341d156c334f2a3c2523f03eb24f4710", null ], + [ "CO_LSS_cs_t", "group__CO__LSS.html#gacc7cba1fb1f1f595506751d6af385964", [ + [ "CO_LSS_SWITCH_STATE_GLOBAL", "group__CO__LSS.html#ggacc7cba1fb1f1f595506751d6af385964a6c9f8aaef024d11e50c007b881208113", null ], + [ "CO_LSS_SWITCH_STATE_SEL_VENDOR", "group__CO__LSS.html#ggacc7cba1fb1f1f595506751d6af385964a620895a76069780eb5df8188b6c8a2de", null ], + [ "CO_LSS_SWITCH_STATE_SEL_PRODUCT", "group__CO__LSS.html#ggacc7cba1fb1f1f595506751d6af385964a9caf25d2d4a28e279b1bc364d303ee7d", null ], + [ "CO_LSS_SWITCH_STATE_SEL_REV", "group__CO__LSS.html#ggacc7cba1fb1f1f595506751d6af385964ae02d526a2c1170babeccffd00d477db5", null ], + [ "CO_LSS_SWITCH_STATE_SEL_SERIAL", "group__CO__LSS.html#ggacc7cba1fb1f1f595506751d6af385964add363bec1d5ff239c847425f8b94718d", null ], + [ "CO_LSS_SWITCH_STATE_SEL", "group__CO__LSS.html#ggacc7cba1fb1f1f595506751d6af385964a97495fe55645d498550e0c05417e2c22", null ], + [ "CO_LSS_CFG_NODE_ID", "group__CO__LSS.html#ggacc7cba1fb1f1f595506751d6af385964a777432f5b616a250a9db8bf7328b0a59", null ], + [ "CO_LSS_CFG_BIT_TIMING", "group__CO__LSS.html#ggacc7cba1fb1f1f595506751d6af385964afd4b839204b66547e5ebb0e4ff9c4481", null ], + [ "CO_LSS_CFG_ACTIVATE_BIT_TIMING", "group__CO__LSS.html#ggacc7cba1fb1f1f595506751d6af385964a1046863405c6de85d0d86088d9c034cc", null ], + [ "CO_LSS_CFG_STORE", "group__CO__LSS.html#ggacc7cba1fb1f1f595506751d6af385964ab0a136e255e8c2c32984881487b414d9", null ], + [ "CO_LSS_IDENT_SLAVE", "group__CO__LSS.html#ggacc7cba1fb1f1f595506751d6af385964ad0ddd8e3d472f4d85de8613e7f35902a", null ], + [ "CO_LSS_IDENT_FASTSCAN", "group__CO__LSS.html#ggacc7cba1fb1f1f595506751d6af385964a9e4bd7f4a726aee66157ac9aac446ddc", null ], + [ "CO_LSS_INQUIRE_VENDOR", "group__CO__LSS.html#ggacc7cba1fb1f1f595506751d6af385964ae28351cd1b60bbdd045a9f79cb506023", null ], + [ "CO_LSS_INQUIRE_PRODUCT", "group__CO__LSS.html#ggacc7cba1fb1f1f595506751d6af385964a5fba5f8bd0f6a91b45fda117556b994c", null ], + [ "CO_LSS_INQUIRE_REV", "group__CO__LSS.html#ggacc7cba1fb1f1f595506751d6af385964a6f412845cd8cd4ab62a54a988ccc384c", null ], + [ "CO_LSS_INQUIRE_SERIAL", "group__CO__LSS.html#ggacc7cba1fb1f1f595506751d6af385964a55f0698aa15abf17c41cd344df055184", null ], + [ "CO_LSS_INQUIRE_NODE_ID", "group__CO__LSS.html#ggacc7cba1fb1f1f595506751d6af385964af5d369fc4d3d860dc43de041b9fd59f5", null ] + ] ], + [ "CO_LSS_cfgNodeId_t", "group__CO__LSS.html#gaf8a13f567f8f405e4aae68268ba5d0a5", [ + [ "CO_LSS_CFG_NODE_ID_OK", "group__CO__LSS.html#ggaf8a13f567f8f405e4aae68268ba5d0a5abe91f1d0e99fa890fe69a3e60aab6c2b", null ], + [ "CO_LSS_CFG_NODE_ID_OUT_OF_RANGE", "group__CO__LSS.html#ggaf8a13f567f8f405e4aae68268ba5d0a5a79c95a7e63e6ff09fcbe5494ef59eed5", null ], + [ "CO_LSS_CFG_NODE_ID_MANUFACTURER", "group__CO__LSS.html#ggaf8a13f567f8f405e4aae68268ba5d0a5aa23e0ca77dfb47ff4a1d48ddfaebc98e", null ] + ] ], + [ "CO_LSS_cfgBitTiming_t", "group__CO__LSS.html#ga28b8651550d1719c38cd307f4ef0a8ac", [ + [ "CO_LSS_CFG_BIT_TIMING_OK", "group__CO__LSS.html#gga28b8651550d1719c38cd307f4ef0a8aca028ded96599022c4416e3d8c0798456a", null ], + [ "CO_LSS_CFG_BIT_TIMING_OUT_OF_RANGE", "group__CO__LSS.html#gga28b8651550d1719c38cd307f4ef0a8acac089dd862b289dfc3f6bae0f30409625", null ], + [ "CO_LSS_CFG_BIT_TIMING_MANUFACTURER", "group__CO__LSS.html#gga28b8651550d1719c38cd307f4ef0a8aca3f5cbbebba617a9c12a7ed919a541255", null ] + ] ], + [ "CO_LSS_cfgStore_t", "group__CO__LSS.html#ga1e4e8c43143125ebe8912de81464bd9f", [ + [ "CO_LSS_CFG_STORE_OK", "group__CO__LSS.html#gga1e4e8c43143125ebe8912de81464bd9fa0f0407dee97a1e5e5d26cc4c717103cc", null ], + [ "CO_LSS_CFG_STORE_NOT_SUPPORTED", "group__CO__LSS.html#gga1e4e8c43143125ebe8912de81464bd9faf78b03384da05bedcc45016d10dc0c3b", null ], + [ "CO_LSS_CFG_STORE_FAILED", "group__CO__LSS.html#gga1e4e8c43143125ebe8912de81464bd9fad7f12cd5d1125e97d7b9bacac4b80d69", null ], + [ "CO_LSS_CFG_STORE_MANUFACTURER", "group__CO__LSS.html#gga1e4e8c43143125ebe8912de81464bd9fa7ece37a5aabe812068efe6a2780f31cc", null ] + ] ], + [ "CO_LSS_fastscan_bitcheck", "group__CO__LSS.html#ga65751e78ae5f2674cc7205e13967f7c0", [ + [ "CO_LSS_FASTSCAN_BIT0", "group__CO__LSS.html#gga65751e78ae5f2674cc7205e13967f7c0a2db8b358b4954e5989a347e1e308eb20", null ], + [ "CO_LSS_FASTSCAN_BIT31", "group__CO__LSS.html#gga65751e78ae5f2674cc7205e13967f7c0a48f5e26f67114198d945f37f8f713979", null ], + [ "CO_LSS_FASTSCAN_CONFIRM", "group__CO__LSS.html#gga65751e78ae5f2674cc7205e13967f7c0a72f67194903c03b688373ef859b66a0f", null ] + ] ], + [ "CO_LSS_fastscan_lss_sub_next", "group__CO__LSS.html#ga1ce707d287b285e7d148f37f93e0f02a", [ + [ "CO_LSS_FASTSCAN_VENDOR_ID", "group__CO__LSS.html#gga1ce707d287b285e7d148f37f93e0f02aa4eb5786c488953cdb2d5ffbb25c15298", null ], + [ "CO_LSS_FASTSCAN_PRODUCT", "group__CO__LSS.html#gga1ce707d287b285e7d148f37f93e0f02aa6514ca82752d5496904388a0589da209", null ], + [ "CO_LSS_FASTSCAN_REV", "group__CO__LSS.html#gga1ce707d287b285e7d148f37f93e0f02aa0e153eebb470156f5d0a27caac2bc71f", null ], + [ "CO_LSS_FASTSCAN_SERIAL", "group__CO__LSS.html#gga1ce707d287b285e7d148f37f93e0f02aabe98c25d444fa5971bc81f775cd6bb35", null ] + ] ], + [ "CO_LSS_state_t", "group__CO__LSS.html#gaaa9a270e40ea09850e1661e5aeb080ad", [ + [ "CO_LSS_STATE_WAITING", "group__CO__LSS.html#ggaaa9a270e40ea09850e1661e5aeb080adac688ea4e90b7dfac437a44536f2af8db", null ], + [ "CO_LSS_STATE_CONFIGURATION", "group__CO__LSS.html#ggaaa9a270e40ea09850e1661e5aeb080ada69cc3fe20e50dcd6f7bad8c0b887ff89", null ] + ] ], + [ "CO_LSS_bitTimingTable_t", "group__CO__LSS.html#gacb4c13e75306153eafd535e55ba0ca2c", [ + [ "CO_LSS_BIT_TIMING_1000", "group__CO__LSS.html#ggacb4c13e75306153eafd535e55ba0ca2ca4cc0aa1f074ddaa5db4fdc0ed437a0b7", null ], + [ "CO_LSS_BIT_TIMING_800", "group__CO__LSS.html#ggacb4c13e75306153eafd535e55ba0ca2ca51413e9c20e1eb60f5cc160a4d30cffa", null ], + [ "CO_LSS_BIT_TIMING_500", "group__CO__LSS.html#ggacb4c13e75306153eafd535e55ba0ca2caaa4d4f1a766516c6d97a81483001e8ef", null ], + [ "CO_LSS_BIT_TIMING_250", "group__CO__LSS.html#ggacb4c13e75306153eafd535e55ba0ca2ca3fadf15163c4f45300045e3645bba3ea", null ], + [ "CO_LSS_BIT_TIMING_125", "group__CO__LSS.html#ggacb4c13e75306153eafd535e55ba0ca2ca1d93a0699e4269d809404c40565133cc", null ], + [ "CO_LSS_BIT_TIMING_50", "group__CO__LSS.html#ggacb4c13e75306153eafd535e55ba0ca2caf0e90aeacc6461bd2ace73be51fd5383", null ], + [ "CO_LSS_BIT_TIMING_20", "group__CO__LSS.html#ggacb4c13e75306153eafd535e55ba0ca2ca08fb8c70b5d185521959415ef731de82", null ], + [ "CO_LSS_BIT_TIMING_10", "group__CO__LSS.html#ggacb4c13e75306153eafd535e55ba0ca2ca7a3c4e05623e7f75fe3b622a9b0185c8", null ], + [ "CO_LSS_BIT_TIMING_AUTO", "group__CO__LSS.html#ggacb4c13e75306153eafd535e55ba0ca2ca12ca7fd4a3604cb8281fa7d58d1137e2", null ] + ] ], + [ "CO_LSS_bitTimingTableLookup", "group__CO__LSS.html#ga61bf679482f7d425ceb521ea80a0cc18", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__LSSmaster.html b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__LSSmaster.html new file mode 100755 index 0000000..ccdfd05 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__LSSmaster.html @@ -0,0 +1,921 @@ + + + + + + + +CANopenNode: LSS Master + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Data Structures | +Macros | +Enumerations | +Functions
+
+
LSS Master
CANopen_305
+
+
+ + + + + + + + +

+Data Structures

struct  CO_LSSmaster_t
 LSS master object. More...
 
struct  CO_LSSmaster_fastscan_t
 Parameters for LSS fastscan CO_LSSmaster_IdentifyFastscan. More...
 
+ + + + +

+Macros

#define CO_LSSmaster_DEFAULT_TIMEOUT   1000U /* ms */
 Default timeout for LSS slave in ms. More...
 
+ + + + + + + +

+Enumerations

enum  CO_LSSmaster_return_t {
+  CO_LSSmaster_SCAN_FINISHED = 2, +CO_LSSmaster_WAIT_SLAVE = 1, +CO_LSSmaster_OK = 0, +CO_LSSmaster_TIMEOUT = -1, +
+  CO_LSSmaster_ILLEGAL_ARGUMENT = -2, +CO_LSSmaster_INVALID_STATE = -3, +CO_LSSmaster_SCAN_NOACK = -4, +CO_LSSmaster_SCAN_FAILED = -5, +
+  CO_LSSmaster_OK_ILLEGAL_ARGUMENT = -101, +CO_LSSmaster_OK_MANUFACTURER = -102 +
+ }
 Return values of LSS master functions. More...
 
enum  CO_LSSmaster_scantype_t { CO_LSSmaster_FS_SCAN = 0, +CO_LSSmaster_FS_SKIP = 1, +CO_LSSmaster_FS_MATCH = 2 + }
 Scan type for CO_LSSmaster_fastscan_t scan. More...
 
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

+Functions

CO_ReturnError_t CO_LSSmaster_init (CO_LSSmaster_t *LSSmaster, uint16_t timeout_ms, CO_CANmodule_t *CANdevRx, uint16_t CANdevRxIdx, uint32_t CANidLssSlave, CO_CANmodule_t *CANdevTx, uint16_t CANdevTxIdx, uint32_t CANidLssMaster)
 Initialize LSS object. More...
 
void CO_LSSmaster_changeTimeout (CO_LSSmaster_t *LSSmaster, uint16_t timeout_ms)
 Change LSS master timeout. More...
 
void CO_LSSmaster_initCallbackPre (CO_LSSmaster_t *LSSmaster, void *object, void(*pFunctSignal)(void *object))
 Initialize LSSmasterRx callback function. More...
 
CO_LSSmaster_return_t CO_LSSmaster_switchStateSelect (CO_LSSmaster_t *LSSmaster, uint32_t timeDifference_us, CO_LSS_address_t *lssAddress)
 Request LSS switch state select. More...
 
CO_LSSmaster_return_t CO_LSSmaster_switchStateDeselect (CO_LSSmaster_t *LSSmaster)
 Request LSS switch state deselect. More...
 
CO_LSSmaster_return_t CO_LSSmaster_configureBitTiming (CO_LSSmaster_t *LSSmaster, uint32_t timeDifference_us, uint16_t bit)
 Request LSS configure Bit Timing. More...
 
CO_LSSmaster_return_t CO_LSSmaster_configureNodeId (CO_LSSmaster_t *LSSmaster, uint32_t timeDifference_us, uint8_t nodeId)
 Request LSS configure node ID. More...
 
CO_LSSmaster_return_t CO_LSSmaster_configureStore (CO_LSSmaster_t *LSSmaster, uint32_t timeDifference_us)
 Request LSS store configuration. More...
 
CO_LSSmaster_return_t CO_LSSmaster_ActivateBit (CO_LSSmaster_t *LSSmaster, uint16_t switchDelay_ms)
 Request LSS activate bit timing. More...
 
CO_LSSmaster_return_t CO_LSSmaster_InquireLssAddress (CO_LSSmaster_t *LSSmaster, uint32_t timeDifference_us, CO_LSS_address_t *lssAddress)
 Request LSS inquire LSS address. More...
 
CO_LSSmaster_return_t CO_LSSmaster_Inquire (CO_LSSmaster_t *LSSmaster, uint32_t timeDifference_us, CO_LSS_cs_t lssInquireCs, uint32_t *value)
 Request LSS inquire node ID or part of LSS address. More...
 
CO_LSSmaster_return_t CO_LSSmaster_IdentifyFastscan (CO_LSSmaster_t *LSSmaster, uint32_t timeDifference_us, CO_LSSmaster_fastscan_t *fastscan)
 Select a node by LSS identify fastscan. More...
 
+

Detailed Description

+

CANopen Layer Setting Service - master protocol.

+

The client/master can use the following services

    +
  • node selection via LSS address
    • +
  • node selection via LSS fastscan
    • +
  • Inquire LSS address of currently selected node
    • +
  • Inquire node ID
    • +
  • Configure bit timing
    • +
  • Configure node ID
    • +
  • Activate bit timing parameters
    • +
  • Store configuration
    • +
+

The LSS master is initalized during the CANopenNode initialization process. Except for enabling the LSS master in the configurator, no further run-time configuration is needed for basic operation. The LSS master does basic checking of commands and command sequence.

+

Usage

+

Usage of the CANopen LSS master is demonstrated in CANopenSocket application, see CO_LSS_master.c / CO_LSS_master.h files.

+

It essentially is always as following:

    +
  • select node(s)
    • +
  • call master command(s)
    • +
  • evaluate return value
    • +
  • deselect nodes
    • +
+

All commands need to be run cyclically, e.g. like this

interval = 0;
+
do {
+
ret = CO_LSSmaster_InquireNodeId(LSSmaster, interval, &outval);
+
+
interval = 1; ms
+
sleep(interval);
+
} while (ret == CO_LSSmaster_WAIT_SLAVE);
+

A more advanced implementation can make use of the callback function to shorten waiting times.

+

Macro Definition Documentation

+ +

◆ CO_LSSmaster_DEFAULT_TIMEOUT

+ +
+
+ + + + +
#define CO_LSSmaster_DEFAULT_TIMEOUT   1000U /* ms */
+
+ +

Default timeout for LSS slave in ms.

+

This is the same as for SDO. For more info about LSS timeout see CO_LSSmaster_changeTimeout()

+ +
+
+

Enumeration Type Documentation

+ +

◆ CO_LSSmaster_return_t

+ +
+
+ + + + +
enum CO_LSSmaster_return_t
+
+ +

Return values of LSS master functions.

+ + + + + + + + + + + +
Enumerator
CO_LSSmaster_SCAN_FINISHED 

Scanning finished successful.

+
CO_LSSmaster_WAIT_SLAVE 

No response arrived from slave yet.

+
CO_LSSmaster_OK 

Success, end of communication.

+
CO_LSSmaster_TIMEOUT 

No reply received.

+
CO_LSSmaster_ILLEGAL_ARGUMENT 

Invalid argument.

+
CO_LSSmaster_INVALID_STATE 

State machine not ready or already processing a request.

+
CO_LSSmaster_SCAN_NOACK 

No node found that matches scan request.

+
CO_LSSmaster_SCAN_FAILED 

An error occurred while scanning.

+

Try again

+
CO_LSSmaster_OK_ILLEGAL_ARGUMENT 

LSS success, node rejected argument because of non-supported value.

+
CO_LSSmaster_OK_MANUFACTURER 

LSS success, node rejected argument with manufacturer error code.

+
+ +
+
+ +

◆ CO_LSSmaster_scantype_t

+ +
+
+ + + + +
enum CO_LSSmaster_scantype_t
+
+ +

Scan type for CO_LSSmaster_fastscan_t scan.

+ + + + +
Enumerator
CO_LSSmaster_FS_SCAN 

Do full 32 bit scan.

+
CO_LSSmaster_FS_SKIP 

Skip this value.

+
CO_LSSmaster_FS_MATCH 

Full 32 bit value is given as argument, just verify.

+
+ +
+
+

Function Documentation

+ +

◆ CO_LSSmaster_init()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
CO_ReturnError_t CO_LSSmaster_init (CO_LSSmaster_t * LSSmaster,
uint16_t timeout_ms,
CO_CANmodule_t * CANdevRx,
uint16_t CANdevRxIdx,
uint32_t CANidLssSlave,
CO_CANmodule_t * CANdevTx,
uint16_t CANdevTxIdx,
uint32_t CANidLssMaster 
)
+
+ +

Initialize LSS object.

+

Function must be called in the communication reset section.

+
Parameters
+ + + + + + + + + +
LSSmasterThis object will be initialized.
timeout_msslave response timeout in ms, for more detail see CO_LSSmaster_changeTimeout()
CANdevRxCAN device for LSS master reception.
CANdevRxIdxIndex of receive buffer in the above CAN device.
CANidLssSlaveCOB ID for reception.
CANdevTxCAN device for LSS master transmission.
CANdevTxIdxIndex of transmit buffer in the above CAN device.
CANidLssMasterCOB ID for transmission.
+
+
+
Returns
CO_ReturnError_t: CO_ERROR_NO or CO_ERROR_ILLEGAL_ARGUMENT.
+ +
+
+ +

◆ CO_LSSmaster_changeTimeout()

+ +
+
+ + + + + + + + + + + + + + + + + + +
void CO_LSSmaster_changeTimeout (CO_LSSmaster_t * LSSmaster,
uint16_t timeout_ms 
)
+
+ +

Change LSS master timeout.

+

On LSS, a "negative ack" is signaled by the slave not answering. Because of that, a low timeout value can significantly increase protocol speed in some cases (e.g. fastscan). However, as soon as there is activity on the bus, LSS messages can be delayed because of their low CAN network priority (see CO_Default_CAN_ID_t).

+
Remarks
Be aware that a "late response" will seriously mess up LSS, so this value must be selected "as high as necessary and as low as possible". CiA does neither specify nor recommend a value.
+
+This timeout is per-transfer. If a command internally needs multiple transfers to complete, this timeout is applied on each transfer.
+
Parameters
+ + + +
LSSmasterThis object.
timeout_mstimeout value in ms
+
+
+ +
+
+ +

◆ CO_LSSmaster_initCallbackPre()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + +
void CO_LSSmaster_initCallbackPre (CO_LSSmaster_t * LSSmaster,
void * object,
void(*)(void *object) pFunctSignal 
)
+
+ +

Initialize LSSmasterRx callback function.

+

Function initializes optional callback function, which should immediately start further LSS processing. Callback is called after LSS message is received from the CAN bus. It should signal the RTOS to resume corresponding task.

+
Parameters
+ + + + +
LSSmasterThis object.
objectPointer to object, which will be passed to pFunctSignal(). Can be NULL
pFunctSignalPointer to the callback function. Not called if NULL.
+
+
+ +
+
+ +

◆ CO_LSSmaster_switchStateSelect()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + +
CO_LSSmaster_return_t CO_LSSmaster_switchStateSelect (CO_LSSmaster_t * LSSmaster,
uint32_t timeDifference_us,
CO_LSS_address_t * lssAddress 
)
+
+ +

Request LSS switch state select.

+

This function can select one specific or all nodes.

+

Function must be called cyclically until it returns != CO_LSSmaster_WAIT_SLAVE Function is non-blocking.

+
Remarks
Only one selection can be active at any time.
+
Parameters
+ + + + +
LSSmasterThis object.
timeDifference_usTime difference from previous function call in [microseconds]. Zero when request is started.
lssAddressLSS target address. If NULL, all nodes are selected
+
+
+
Returns
CO_LSSmaster_ILLEGAL_ARGUMENT, CO_LSSmaster_INVALID_STATE, CO_LSSmaster_WAIT_SLAVE, CO_LSSmaster_OK, CO_LSSmaster_TIMEOUT
+ +
+
+ +

◆ CO_LSSmaster_switchStateDeselect()

+ +
+
+ + + + + + + + +
CO_LSSmaster_return_t CO_LSSmaster_switchStateDeselect (CO_LSSmaster_t * LSSmaster)
+
+ +

Request LSS switch state deselect.

+

This function deselects all nodes, so it doesn't matter if a specific node is selected.

+

This function also resets the LSS master state machine to a clean state

+
Parameters
+ + +
LSSmasterThis object.
+
+
+
Returns
CO_LSSmaster_ILLEGAL_ARGUMENT, CO_LSSmaster_INVALID_STATE, CO_LSSmaster_OK
+ +
+
+ +

◆ CO_LSSmaster_configureBitTiming()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + +
CO_LSSmaster_return_t CO_LSSmaster_configureBitTiming (CO_LSSmaster_t * LSSmaster,
uint32_t timeDifference_us,
uint16_t bit 
)
+
+ +

Request LSS configure Bit Timing.

+

The new bit rate is set as new pending value.

+

This function needs one specific node to be selected.

+

Function must be called cyclically until it returns != CO_LSSmaster_WAIT_SLAVE. Function is non-blocking.

+
Parameters
+ + + + +
LSSmasterThis object.
timeDifference_usTime difference from previous function call in [microseconds]. Zero when request is started.
bitnew bit rate
+
+
+
Returns
CO_LSSmaster_ILLEGAL_ARGUMENT, CO_LSSmaster_INVALID_STATE, CO_LSSmaster_WAIT_SLAVE, CO_LSSmaster_OK, CO_LSSmaster_TIMEOUT, CO_LSSmaster_OK_MANUFACTURER, CO_LSSmaster_OK_ILLEGAL_ARGUMENT
+ +
+
+ +

◆ CO_LSSmaster_configureNodeId()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + +
CO_LSSmaster_return_t CO_LSSmaster_configureNodeId (CO_LSSmaster_t * LSSmaster,
uint32_t timeDifference_us,
uint8_t nodeId 
)
+
+ +

Request LSS configure node ID.

+

The new node id is set as new pending node ID.

+

This function needs one specific node to be selected.

+

Function must be called cyclically until it returns != CO_LSSmaster_WAIT_SLAVE. Function is non-blocking.

+
Parameters
+ + + + +
LSSmasterThis object.
timeDifference_usTime difference from previous function call in [microseconds]. Zero when request is started.
nodeIdnew node ID. Special value CO_LSS_NODE_ID_ASSIGNMENT can be used to invalidate node ID.
+
+
+
Returns
CO_LSSmaster_ILLEGAL_ARGUMENT, CO_LSSmaster_INVALID_STATE, CO_LSSmaster_WAIT_SLAVE, CO_LSSmaster_OK, CO_LSSmaster_TIMEOUT, CO_LSSmaster_OK_MANUFACTURER, CO_LSSmaster_OK_ILLEGAL_ARGUMENT
+ +
+
+ +

◆ CO_LSSmaster_configureStore()

+ +
+
+ + + + + + + + + + + + + + + + + + +
CO_LSSmaster_return_t CO_LSSmaster_configureStore (CO_LSSmaster_t * LSSmaster,
uint32_t timeDifference_us 
)
+
+ +

Request LSS store configuration.

+

The current "pending" values for bit rate and node ID in LSS slave are stored as "permanent" values.

+

This function needs one specific node to be selected.

+

Function must be called cyclically until it returns != CO_LSSmaster_WAIT_SLAVE. Function is non-blocking.

+
Parameters
+ + + +
LSSmasterThis object.
timeDifference_usTime difference from previous function call in [microseconds]. Zero when request is started.
+
+
+
Returns
CO_LSSmaster_ILLEGAL_ARGUMENT, CO_LSSmaster_INVALID_STATE, CO_LSSmaster_WAIT_SLAVE, CO_LSSmaster_OK, CO_LSSmaster_TIMEOUT, CO_LSSmaster_OK_MANUFACTURER, CO_LSSmaster_OK_ILLEGAL_ARGUMENT
+ +
+
+ +

◆ CO_LSSmaster_ActivateBit()

+ +
+
+ + + + + + + + + + + + + + + + + + +
CO_LSSmaster_return_t CO_LSSmaster_ActivateBit (CO_LSSmaster_t * LSSmaster,
uint16_t switchDelay_ms 
)
+
+ +

Request LSS activate bit timing.

+

The current "pending" bit rate in LSS slave is applied.

+

Be aware that changing the bit rate is a critical step for the network. A failure will render the network unusable! Therefore, this function only should be called if the following conditions are met:

    +
  • all nodes support changing bit timing
    • +
  • new bit timing is successfully set as "pending" in all nodes
    • +
  • all nodes have to activate the new bit timing roughly at the same time. Therefore this function needs all nodes to be selected.
    • +
+
Parameters
+ + + +
LSSmasterThis object.
switchDelay_msdelay that is applied by the slave once before and once after switching in ms.
+
+
+
Returns
CO_LSSmaster_ILLEGAL_ARGUMENT, CO_LSSmaster_INVALID_STATE, CO_LSSmaster_OK
+ +
+
+ +

◆ CO_LSSmaster_InquireLssAddress()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + +
CO_LSSmaster_return_t CO_LSSmaster_InquireLssAddress (CO_LSSmaster_t * LSSmaster,
uint32_t timeDifference_us,
CO_LSS_address_t * lssAddress 
)
+
+ +

Request LSS inquire LSS address.

+

The LSS address value is read from the node. This is useful when the node was selected by fastscan.

+

This function needs one specific node to be selected.

+

Function must be called cyclically until it returns != CO_LSSmaster_WAIT_SLAVE. Function is non-blocking.

+
Parameters
+ + + + +
LSSmasterThis object.
timeDifference_usTime difference from previous function call in [microseconds]. Zero when request is started.
[out]lssAddressread result when function returns successfully
+
+
+
Returns
CO_LSSmaster_ILLEGAL_ARGUMENT, CO_LSSmaster_INVALID_STATE, CO_LSSmaster_WAIT_SLAVE, CO_LSSmaster_OK, CO_LSSmaster_TIMEOUT
+ +
+
+ +

◆ CO_LSSmaster_Inquire()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
CO_LSSmaster_return_t CO_LSSmaster_Inquire (CO_LSSmaster_t * LSSmaster,
uint32_t timeDifference_us,
CO_LSS_cs_t lssInquireCs,
uint32_t * value 
)
+
+ +

Request LSS inquire node ID or part of LSS address.

+

The node-ID, identity vendor-ID, product-code, revision-number or serial-number value is read from the node.

+

This function needs one specific node to be selected.

+

Function must be called cyclically until it returns != CO_LSSmaster_WAIT_SLAVE. Function is non-blocking.

+
Parameters
+ + + + + +
LSSmasterThis object.
timeDifference_usTime difference from previous function call in [microseconds]. Zero when request is started.
lssInquireCsOne of CO_LSS_INQUIRE_xx commands from CO_LSS_cs_t.
[out]valueread result when function returns successfully
+
+
+
Returns
CO_LSSmaster_ILLEGAL_ARGUMENT, CO_LSSmaster_INVALID_STATE, CO_LSSmaster_WAIT_SLAVE, CO_LSSmaster_OK, CO_LSSmaster_TIMEOUT
+ +
+
+ +

◆ CO_LSSmaster_IdentifyFastscan()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + +
CO_LSSmaster_return_t CO_LSSmaster_IdentifyFastscan (CO_LSSmaster_t * LSSmaster,
uint32_t timeDifference_us,
CO_LSSmaster_fastscan_t * fastscan 
)
+
+ +

Select a node by LSS identify fastscan.

+

This initiates searching for a unconfigured node by the means of LSS fastscan mechanism. When this function is finished

    +
  • a (more or less) arbitrary node is selected and ready for node ID assingment
    • +
  • no node is selected because the given criteria do not match a node
    • +
  • no node is selected because all nodes are already configured
    • +
+

There are multiple ways to scan for a node. Depending on those, the scan will take different amounts of time:

    +
  • full scan
    • +
  • partial scan
    • +
  • verification
    • +
+

Most of the time, those are used in combination. Consider the following example:

    +
  • Vendor ID and product code are known
    • +
  • Software version doesn't matter
    • +
  • Serial number is unknown
    • +
+

In this case, the fastscan structure should be set up as following:

CO_LSSmaster_fastscan_t fastscan;
+
fastscan.scan[CO_LSS_FASTSCAN_VENDOR_ID] = CO_LSSmaster_FS_MATCH;
+
fastscan.match.vendorID = YOUR_VENDOR_ID;
+
fastscan.scan[CO_LSS_FASTSCAN_PRODUCT] = CO_LSSmaster_FS_MATCH;
+
fastscan.match.productCode = YOUR_PRODUCT_CODE;
+
fastscan.scan[CO_LSS_FASTSCAN_REV] = CO_LSSmaster_FS_SKIP;
+
fastscan.scan[CO_LSS_FASTSCAN_SERIAL] = CO_LSSmaster_FS_SCAN;
+

This example will take 2 scan cyles for verifying vendor ID and product code and 33 scan cycles to find the serial number.

+

For scanning, the following limitations apply:

    +
  • No more than two values can be skipped
    • +
  • Vendor ID cannot be skipped
    • +
+
Remarks
When doing partial scans, it is in the responsibility of the user that the LSS address is unique.
+

This function needs that no node is selected when starting the scan process.

+

Function must be called cyclically until it returns != CO_LSSmaster_WAIT_SLAVE. Function is non-blocking.

+
Parameters
+ + + + +
LSSmasterThis object.
timeDifference_usTime difference from previous function call in [microseconds]. Zero when request is started.
fastscanstruct according to CO_LSSmaster_fastscan_t.
+
+
+
Returns
CO_LSSmaster_ILLEGAL_ARGUMENT, CO_LSSmaster_INVALID_STATE, CO_LSSmaster_WAIT_SLAVE, CO_LSSmaster_SCAN_FINISHED, CO_LSSmaster_SCAN_NOACK, CO_LSSmaster_SCAN_FAILED
+ +
+
+
+
+
CO_LSSmaster_fastscan_t::scan
CO_LSSmaster_scantype_t scan[4]
Scan type for each part of the LSS address.
Definition: CO_LSSmaster.h:408
+
CO_LSSmaster_WAIT_SLAVE
@ CO_LSSmaster_WAIT_SLAVE
No response arrived from slave yet.
Definition: CO_LSSmaster.h:93
+
CO_LSSmaster_fastscan_t
Parameters for LSS fastscan CO_LSSmaster_IdentifyFastscan.
Definition: CO_LSSmaster.h:407
+
CO_LSS_FASTSCAN_SERIAL
@ CO_LSS_FASTSCAN_SERIAL
Serial number.
Definition: CO_LSS.h:153
+
CO_LSSmaster_FS_SKIP
@ CO_LSSmaster_FS_SKIP
Skip this value.
Definition: CO_LSSmaster.h:400
+
CO_LSS_FASTSCAN_REV
@ CO_LSS_FASTSCAN_REV
Revision number.
Definition: CO_LSS.h:152
+
CO_LSSmaster_FS_MATCH
@ CO_LSSmaster_FS_MATCH
Full 32 bit value is given as argument, just verify.
Definition: CO_LSSmaster.h:401
+
CO_LSSmaster_FS_SCAN
@ CO_LSSmaster_FS_SCAN
Do full 32 bit scan.
Definition: CO_LSSmaster.h:399
+
CO_LSS_FASTSCAN_PRODUCT
@ CO_LSS_FASTSCAN_PRODUCT
Product code.
Definition: CO_LSS.h:151
+
CO_LSSmaster_fastscan_t::match
CO_LSS_address_t match
Value to match in case of CO_LSSmaster_FS_MATCH.
Definition: CO_LSSmaster.h:409
+
CO_LSS_FASTSCAN_VENDOR_ID
@ CO_LSS_FASTSCAN_VENDOR_ID
Vendor ID.
Definition: CO_LSS.h:150
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__LSSmaster.js b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__LSSmaster.js new file mode 100755 index 0000000..63182d4 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__LSSmaster.js @@ -0,0 +1,54 @@ +var group__CO__LSSmaster = +[ + [ "CO_LSSmaster_t", "structCO__LSSmaster__t.html", [ + [ "timeout_us", "structCO__LSSmaster__t.html#aeac43e30ee9018bb34876a4c2f1a10de", null ], + [ "state", "structCO__LSSmaster__t.html#a1fb90a878809a6690dd4493444795e30", null ], + [ "command", "structCO__LSSmaster__t.html#a745a126919856c94e7f93186830e6940", null ], + [ "timeoutTimer", "structCO__LSSmaster__t.html#aa706f4a19295e26a862127917ebc9eca", null ], + [ "fsState", "structCO__LSSmaster__t.html#ad7ee57af199cfd615f6caa07358f0ce7", null ], + [ "fsLssSub", "structCO__LSSmaster__t.html#a9962ae690930bf58a447a47bf708c5f2", null ], + [ "fsBitChecked", "structCO__LSSmaster__t.html#ab71abc74d5d92f149ca991c9bb47ed99", null ], + [ "fsIdNumber", "structCO__LSSmaster__t.html#aca7e7be8c297c612b2200b866ed7b248", null ], + [ "CANrxNew", "structCO__LSSmaster__t.html#ad5fd09faed937c053f1ded1df54f40cd", null ], + [ "CANrxData", "structCO__LSSmaster__t.html#a65cc40d755d5b2eaaeb6f1b29332dca8", null ], + [ "pFunctSignal", "structCO__LSSmaster__t.html#a9b8376cf89bba5a3650f82da61ffee5b", null ], + [ "functSignalObject", "structCO__LSSmaster__t.html#aafce10b9e126c5c157cb137e8b65b27c", null ], + [ "CANdevTx", "structCO__LSSmaster__t.html#a480123707829720cc82b08b65923abcb", null ], + [ "TXbuff", "structCO__LSSmaster__t.html#ad915ab10431aea283df0d8e1299876e2", null ] + ] ], + [ "CO_LSSmaster_fastscan_t", "structCO__LSSmaster__fastscan__t.html", [ + [ "scan", "structCO__LSSmaster__fastscan__t.html#a42853c0091c96d7fc7e763c2be3b6e8a", null ], + [ "match", "structCO__LSSmaster__fastscan__t.html#a69540f77885162e936803a9526d3c342", null ], + [ "found", "structCO__LSSmaster__fastscan__t.html#a9c34a389339a46da81f2234443f20fb9", null ] + ] ], + [ "CO_LSSmaster_DEFAULT_TIMEOUT", "group__CO__LSSmaster.html#ga76adbab914fff5b18982dad449e0a00a", null ], + [ "CO_LSSmaster_return_t", "group__CO__LSSmaster.html#gae848ff3ff649c8a23b96053efaea4985", [ + [ "CO_LSSmaster_SCAN_FINISHED", "group__CO__LSSmaster.html#ggae848ff3ff649c8a23b96053efaea4985a0f527d1ce01820fa184ccae2510505c7", null ], + [ "CO_LSSmaster_WAIT_SLAVE", "group__CO__LSSmaster.html#ggae848ff3ff649c8a23b96053efaea4985a20fbe514e36bd92534141bb75e68eb34", null ], + [ "CO_LSSmaster_OK", "group__CO__LSSmaster.html#ggae848ff3ff649c8a23b96053efaea4985a1be85610f29d8e6cecebac9db2da3099", null ], + [ "CO_LSSmaster_TIMEOUT", "group__CO__LSSmaster.html#ggae848ff3ff649c8a23b96053efaea4985a3a61073ef2c8ef7c5be946618b95d42d", null ], + [ "CO_LSSmaster_ILLEGAL_ARGUMENT", "group__CO__LSSmaster.html#ggae848ff3ff649c8a23b96053efaea4985a9bd04e1c9923416d1d1ecb1ded6bc7b9", null ], + [ "CO_LSSmaster_INVALID_STATE", "group__CO__LSSmaster.html#ggae848ff3ff649c8a23b96053efaea4985a1e99145e94c3a6fb8cc7c48150ed5b60", null ], + [ "CO_LSSmaster_SCAN_NOACK", "group__CO__LSSmaster.html#ggae848ff3ff649c8a23b96053efaea4985ae9eb103f25dbe694143a64e7bb2c29d9", null ], + [ "CO_LSSmaster_SCAN_FAILED", "group__CO__LSSmaster.html#ggae848ff3ff649c8a23b96053efaea4985af84e0db6bd9aeebffc4266618145a8ea", null ], + [ "CO_LSSmaster_OK_ILLEGAL_ARGUMENT", "group__CO__LSSmaster.html#ggae848ff3ff649c8a23b96053efaea4985ab6985b2cbcb24653ec1f2ae46d3c09cd", null ], + [ "CO_LSSmaster_OK_MANUFACTURER", "group__CO__LSSmaster.html#ggae848ff3ff649c8a23b96053efaea4985a51083a5cf04e03a2f623eddb1d7324c7", null ] + ] ], + [ "CO_LSSmaster_scantype_t", "group__CO__LSSmaster.html#ga6e3f0d07f0712c371fb81cbf4a3dbcb1", [ + [ "CO_LSSmaster_FS_SCAN", "group__CO__LSSmaster.html#gga6e3f0d07f0712c371fb81cbf4a3dbcb1a5666c53e2fc02e214294bd3210146c90", null ], + [ "CO_LSSmaster_FS_SKIP", "group__CO__LSSmaster.html#gga6e3f0d07f0712c371fb81cbf4a3dbcb1ae0e6e92e760129fc173a0a4f19a0cf07", null ], + [ "CO_LSSmaster_FS_MATCH", "group__CO__LSSmaster.html#gga6e3f0d07f0712c371fb81cbf4a3dbcb1ad5a936fd00345aea75e8917f99df4ab3", null ] + ] ], + [ "CO_LSSmaster_init", "group__CO__LSSmaster.html#ga0675297a7e7e1f472ad2e88d6b7408e7", null ], + [ "CO_LSSmaster_changeTimeout", "group__CO__LSSmaster.html#gae22758aff11b796cfaed979c5f2808c0", null ], + [ "CO_LSSmaster_initCallbackPre", "group__CO__LSSmaster.html#gabfeb7e75d88b76bb00c1740381c7b53f", null ], + [ "CO_LSSmaster_switchStateSelect", "group__CO__LSSmaster.html#ga41b4288c03af394261541b9a8395e8f3", null ], + [ "CO_LSSmaster_switchStateDeselect", "group__CO__LSSmaster.html#gac0e13ec42e1fd85da5ddef6f654ef1a4", null ], + [ "CO_LSSmaster_configureBitTiming", "group__CO__LSSmaster.html#ga71a5d90e569ee7e88763a541c286e240", null ], + [ "CO_LSSmaster_configureNodeId", "group__CO__LSSmaster.html#ga2cdba08d9a564c4a61ebbcd0d10342fd", null ], + [ "CO_LSSmaster_configureStore", "group__CO__LSSmaster.html#gacea091d379a5338f13963eb745b25b16", null ], + [ "CO_LSSmaster_ActivateBit", "group__CO__LSSmaster.html#gaa016c0f3dc4dd021801b6139765657ab", null ], + [ "CO_LSSmaster_InquireLssAddress", "group__CO__LSSmaster.html#ga1b0a5c9e27e046736c6ec55a0256ed77", null ], + [ "CO_LSSmaster_Inquire", "group__CO__LSSmaster.html#ga22414a7184ca0c9d371dd67e9990d820", null ], + [ "CO_LSSmaster_IdentifyFastscan", "group__CO__LSSmaster.html#gad01ce178ea43b1843f541d4dd488f90e", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__LSSslave.html b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__LSSslave.html new file mode 100755 index 0000000..fd1aa1d --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__LSSslave.html @@ -0,0 +1,506 @@ + + + + + + + +CANopenNode: LSS Slave + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Data Structures | +Functions
+
+
LSS Slave
CANopen_305
+
+
+ + + + + +

+Data Structures

struct  CO_LSSslave_t
 LSS slave object. More...
 
+ + + + + + + + + + + + + + + + + + + + + + +

+Functions

CO_ReturnError_t CO_LSSslave_init (CO_LSSslave_t *LSSslave, CO_LSS_address_t *lssAddress, uint16_t *pendingBitRate, uint8_t *pendingNodeID, CO_CANmodule_t *CANdevRx, uint16_t CANdevRxIdx, uint32_t CANidLssMaster, CO_CANmodule_t *CANdevTx, uint16_t CANdevTxIdx, uint32_t CANidLssSlave)
 Initialize LSS object. More...
 
bool_t CO_LSSslave_process (CO_LSSslave_t *LSSslave)
 Process LSS communication. More...
 
static CO_LSS_state_t CO_LSSslave_getState (CO_LSSslave_t *LSSslave)
 Get current LSS state. More...
 
void CO_LSSslave_initCallbackPre (CO_LSSslave_t *LSSslave, void *object, void(*pFunctSignalPre)(void *object))
 Initialize LSSslaveRx callback function. More...
 
void CO_LSSslave_initCheckBitRateCallback (CO_LSSslave_t *LSSslave, void *object, bool_t(*pFunctLSScheckBitRate)(void *object, uint16_t bitRate))
 Initialize verify bit rate callback. More...
 
void CO_LSSslave_initActivateBitRateCallback (CO_LSSslave_t *LSSslave, void *object, void(*pFunctLSSactivateBitRate)(void *object, uint16_t delay))
 Initialize activate bit rate callback. More...
 
void CO_LSSslave_initCfgStoreCallback (CO_LSSslave_t *LSSslave, void *object, bool_t(*pFunctLSScfgStore)(void *object, uint8_t id, uint16_t bitRate))
 Store configuration callback. More...
 
+

Detailed Description

+

CANopen Layer Setting Service - slave protocol.

+

The slave provides the following services

    +
  • node selection via LSS address
    • +
  • node selection via LSS fastscan
    • +
  • Inquire LSS address of currently selected node
    • +
  • Inquire node ID
    • +
  • Configure bit timing
    • +
  • Configure node ID
    • +
  • Activate bit timing parameters
    • +
  • Store configuration (bit rate and node ID)
    • +
+

After CAN module start, the LSS slave and NMT slave are started and then coexist alongside each other. To achieve this behaviour, the CANopen node startup process has to be controlled more detailed. Therefore, CO_LSSinit() must be invoked between CO_CANinit() and CO_CANopenInit() in the communication reset section.

+

Moreover, the LSS slave needs to pause the NMT slave initialization in case no valid node ID is available at start up. In that case CO_CANopenInit() skips initialization of other CANopen modules and CO_process() skips processing of other modules than LSS slave automatically.

+

Variables for CAN-bitrate and CANopen node-id must be initialized by application from non-volatile memory or dip switches. Pointers to them are passed to CO_LSSinit() function. Those variables represents pending values. If node-id is valid in the moment it enters CO_LSSinit(), it also becomes active node-id and the stack initialises normally. Otherwise, node-id must be configured by lss and after successful configuration stack passes reset communication autonomously.

+

Device with all threads can be normally initialized and running despite that node-id is not valid. Application must take care, because CANopen is not initialized. In that case CO_CANopenInit() returns error condition CO_ERROR_NODE_ID_UNCONFIGURED_LSS which must be handled properly. Status can also be checked with CO->nodeIdUnconfigured variable.

+

Some callback functions may be initialized by application with CO_LSSslave_initCheckBitRateCallback(), CO_LSSslave_initActivateBitRateCallback() and CO_LSSslave_initCfgStoreCallback().

+

Function Documentation

+ +

◆ CO_LSSslave_init()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
CO_ReturnError_t CO_LSSslave_init (CO_LSSslave_t * LSSslave,
CO_LSS_address_t * lssAddress,
uint16_t * pendingBitRate,
uint8_t * pendingNodeID,
CO_CANmodule_t * CANdevRx,
uint16_t CANdevRxIdx,
uint32_t CANidLssMaster,
CO_CANmodule_t * CANdevTx,
uint16_t CANdevTxIdx,
uint32_t CANidLssSlave 
)
+
+ +

Initialize LSS object.

+

Function must be called in the communication reset section.

+

pendingBitRate and pendingNodeID must be pointers to external variables. Both variables must be initialized on program startup (after CO_NMT_RESET_NODE) from non-volatile memory, dip switches or similar. They must not change during CO_NMT_RESET_COMMUNICATION. Both variables can be changed by CO_LSSslave_process(), depending on commands from the LSS master.

+

If pendingNodeID is valid (1 <= pendingNodeID <= 0x7F), then this becomes valid active nodeId just after exit of this function. In that case all other CANopen objects may be initialized and processed in run time.

+

If pendingNodeID is not valid (pendingNodeID == 0xFF), then only LSS slave is initialized and processed in run time. In that state pendingNodeID can be configured and after successful configuration reset communication with all CANopen object is activated automatically.

+
Remarks
The LSS address needs to be unique on the network. For this, the 128 bit wide identity object (1018h) is used. Therefore, this object has to be fully initialized before passing it to this function (vendorID, product code, revisionNo, serialNo are set to 0 by default). Otherwise, if non-configured devices are present on CANopen network, LSS configuration may behave unpredictable.
+
Parameters
+ + + + + + + + + + + +
LSSslaveThis object will be initialized.
lssAddressLSS address
[in,out]pendingBitRatePending bit rate of the CAN interface
[in,out]pendingNodeIDPending node ID or 0xFF - invalid
CANdevRxCAN device for LSS slave reception.
CANdevRxIdxIndex of receive buffer in the above CAN device.
CANidLssMasterCOB ID for reception.
CANdevTxCAN device for LSS slave transmission.
CANdevTxIdxIndex of transmit buffer in the above CAN device.
CANidLssSlaveCOB ID for transmission.
+
+
+
Returns
CO_ReturnError_t: CO_ERROR_NO or CO_ERROR_ILLEGAL_ARGUMENT.
+ +
+
+ +

◆ CO_LSSslave_process()

+ +
+
+ + + + + + + + +
bool_t CO_LSSslave_process (CO_LSSslave_t * LSSslave)
+
+ +

Process LSS communication.

+

Object is partially pre-processed after LSS message received. Further processing is inside this function.

+

In case that Node-Id is unconfigured, then this function may request CANopen communication reset. This happens, when valid node-id is configured by LSS master.

+
Parameters
+ + +
LSSslaveThis object.
+
+
+
Returns
True, if CO_NMT_RESET_COMMUNICATION is requested
+ +
+
+ +

◆ CO_LSSslave_getState()

+ +
+
+ + + + + +
+ + + + + + + + +
static CO_LSS_state_t CO_LSSslave_getState (CO_LSSslave_t * LSSslave)
+ 		+inlinestatic
+
+ +

Get current LSS state.

+
Parameters
+ + +
LSSslaveThis object.
+
+
+
Returns
CO_LSS_state_t
+ +
+
+ +

◆ CO_LSSslave_initCallbackPre()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + +
void CO_LSSslave_initCallbackPre (CO_LSSslave_t * LSSslave,
void * object,
void(*)(void *object) pFunctSignalPre 
)
+
+ +

Initialize LSSslaveRx callback function.

+

Function initializes optional callback function, which should immediately start further LSS processing. Callback is called after LSS message is received from the CAN bus. It should signal the RTOS to resume corresponding task.

+
Parameters
+ + + + +
LSSslaveThis object.
objectPointer to object, which will be passed to pFunctSignal(). Can be NULL
pFunctSignalPrePointer to the callback function. Not called if NULL.
+
+
+ +
+
+ +

◆ CO_LSSslave_initCheckBitRateCallback()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + +
void CO_LSSslave_initCheckBitRateCallback (CO_LSSslave_t * LSSslave,
void * object,
bool_t(*)(void *object, uint16_t bitRate) pFunctLSScheckBitRate 
)
+
+ +

Initialize verify bit rate callback.

+

Function initializes callback function, which is called when "config bit +timing parameters" is used. The callback function needs to check if the new bit rate is supported by the CANopen device. Callback returns "true" if supported. When no callback is set the LSS slave will no-ack the request, indicating to the master that bit rate change is not supported.

+
Parameters
+ + + + +
LSSslaveThis object.
objectPointer to object, which will be passed to pFunctLSScheckBitRate(). Can be NULL
pFunctLSScheckBitRatePointer to the callback function. Not called if NULL.
+
+
+ +
+
+ +

◆ CO_LSSslave_initActivateBitRateCallback()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + +
void CO_LSSslave_initActivateBitRateCallback (CO_LSSslave_t * LSSslave,
void * object,
void(*)(void *object, uint16_t delay) pFunctLSSactivateBitRate 
)
+
+ +

Initialize activate bit rate callback.

+

Function initializes callback function, which is called when "activate bit +timing parameters" is used. The callback function gives the user an event to allow setting a timer or do calculations based on the exact time the request arrived. According to DSP 305 6.4.4, the delay has to be applied once before and once after switching bit rates. During this time, a device mustn't send any messages.

+
Parameters
+ + + + +
LSSslaveThis object.
objectPointer to object, which will be passed to pFunctLSSactivateBitRate(). Can be NULL
pFunctLSSactivateBitRatePointer to the callback function. Not called if NULL.
+
+
+ +
+
+ +

◆ CO_LSSslave_initCfgStoreCallback()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + +
void CO_LSSslave_initCfgStoreCallback (CO_LSSslave_t * LSSslave,
void * object,
bool_t(*)(void *object, uint8_t id, uint16_t bitRate) pFunctLSScfgStore 
)
+
+ +

Store configuration callback.

+

Function initializes callback function, which is called when "store configuration" is used. The callback function gives the user an event to store the corresponding node id and bit rate to NVM. Those values have to be supplied to the init function as "persistent values" after reset. If callback returns "true", success is send to the LSS master. When no callback is set the LSS slave will no-ack the request, indicating to the master that storing is not supported.

+
Parameters
+ + + + +
LSSslaveThis object.
objectPointer to object, which will be passed to pFunctLSScfgStore(). Can be NULL
pFunctLSScfgStorePointer to the callback function. Not called if NULL.
+
+
+ +
+
+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__LSSslave.js b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__LSSslave.js new file mode 100755 index 0000000..d2dfa81 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__LSSslave.js @@ -0,0 +1,30 @@ +var group__CO__LSSslave = +[ + [ "CO_LSSslave_t", "structCO__LSSslave__t.html", [ + [ "lssAddress", "structCO__LSSslave__t.html#ae18023e6feb33ea4e9ddf26796a99fe0", null ], + [ "lssState", "structCO__LSSslave__t.html#a514565a5cda1630165c3640ef223aa28", null ], + [ "lssSelect", "structCO__LSSslave__t.html#a38d6e43b3a1c3b0ce20bdb9108f8ab91", null ], + [ "lssFastscan", "structCO__LSSslave__t.html#a433215fe4d5f9764427a5e0dc57d60a5", null ], + [ "fastscanPos", "structCO__LSSslave__t.html#af163113cf94a4a2c74521ffe194522b2", null ], + [ "pendingBitRate", "structCO__LSSslave__t.html#a90b0fabc76e788404da520a7a5e90e48", null ], + [ "pendingNodeID", "structCO__LSSslave__t.html#a9a632a4655fb5adfd91bd3e26ed43f0b", null ], + [ "activeNodeID", "structCO__LSSslave__t.html#a91bb370cba5215ddaf52c0883a9bdca2", null ], + [ "sendResponse", "structCO__LSSslave__t.html#af9fb29812cd548f34223cc6d16de8121", null ], + [ "service", "structCO__LSSslave__t.html#acdffe4100d9031c885ba5b5b995e471c", null ], + [ "CANdata", "structCO__LSSslave__t.html#a80021b6e36da1b495453d360a7001287", null ], + [ "pFunctSignalPre", "structCO__LSSslave__t.html#a9a6db85a8e5a14652e3b1fb7026790a1", null ], + [ "functSignalObjectPre", "structCO__LSSslave__t.html#a346a28ca3adef3050f6cc29ec182059d", null ], + [ "pFunctLSScheckBitRate", "structCO__LSSslave__t.html#a1482f4cef4b3ac4afbdf569306e32aab", null ], + [ "pFunctLSSactivateBitRate", "structCO__LSSslave__t.html#a8c2bfab9e6225cd65b48149333d9e1dd", null ], + [ "pFunctLSScfgStore", "structCO__LSSslave__t.html#a53b9620f867ad5f534c99955a2479f16", null ], + [ "CANdevTx", "structCO__LSSslave__t.html#a6ce85337359f0e4cd948c438f2960015", null ], + [ "TXbuff", "structCO__LSSslave__t.html#a12b8505b3f2bcf2085b38bbdcda6736f", null ] + ] ], + [ "CO_LSSslave_init", "group__CO__LSSslave.html#gaaba1fafcd0024609f8a72be4810baf66", null ], + [ "CO_LSSslave_process", "group__CO__LSSslave.html#gae19d7ad84333f1a3f40ecbdbf639e017", null ], + [ "CO_LSSslave_getState", "group__CO__LSSslave.html#ga2692ff0d6837db494c029a3bef735ee7", null ], + [ "CO_LSSslave_initCallbackPre", "group__CO__LSSslave.html#ga64c23178117a707046eb15ebc6506429", null ], + [ "CO_LSSslave_initCheckBitRateCallback", "group__CO__LSSslave.html#ga665f147d6fae6db71173c4a8d602495c", null ], + [ "CO_LSSslave_initActivateBitRateCallback", "group__CO__LSSslave.html#ga0253fffcb36ab6b850563328784a8a5a", null ], + [ "CO_LSSslave_initCfgStoreCallback", "group__CO__LSSslave.html#gadc6187357904293da0a35317f15d0666", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__NMT__Heartbeat.html b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__NMT__Heartbeat.html new file mode 100755 index 0000000..1b30c51 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__NMT__Heartbeat.html @@ -0,0 +1,730 @@ + + + + + + + +CANopenNode: NMT and Heartbeat + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Files | +Data Structures | +Enumerations | +Functions
+
+
NMT and Heartbeat
CANopen_301
+
+
+ + + + + +

+Files

file  CO_NMT_Heartbeat.h
 CANopen Network management and Heartbeat producer protocol.
 
+ + + + +

+Data Structures

struct  CO_NMT_t
 NMT consumer and Heartbeat producer object. More...
 
+ + + + + + + + + + + + + +

+Enumerations

enum  CO_NMT_internalState_t {
+  CO_NMT_UNKNOWN = -1, +CO_NMT_INITIALIZING = 0, +CO_NMT_PRE_OPERATIONAL = 127, +CO_NMT_OPERATIONAL = 5, +
+  CO_NMT_STOPPED = 4 +
+ }
 Internal network state of the CANopen node. More...
 
enum  CO_NMT_command_t {
+  CO_NMT_ENTER_OPERATIONAL = 1, +CO_NMT_ENTER_STOPPED = 2, +CO_NMT_ENTER_PRE_OPERATIONAL = 128, +CO_NMT_RESET_NODE = 129, +
+  CO_NMT_RESET_COMMUNICATION = 130 +
+ }
 Commands from NMT master. More...
 
enum  CO_NMT_reset_cmd_t { CO_RESET_NOT = 0, +CO_RESET_COMM = 1, +CO_RESET_APP = 2, +CO_RESET_QUIT = 3 + }
 Return code from CO_NMT_process() that tells application code what to reset. More...
 
enum  CO_NMT_control_t {
+  CO_NMT_ERR_REG_MASK = 0x00FFU, +CO_NMT_STARTUP_TO_OPERATIONAL = 0x0100U, +CO_NMT_ERR_ON_BUSOFF_HB = 0x1000U, +CO_NMT_ERR_ON_ERR_REG = 0x2000U, +
+  CO_NMT_ERR_TO_STOPPED = 0x4000U, +CO_NMT_ERR_FREE_TO_OPERATIONAL = 0x8000U +
+ }
 NMT control bitfield for NMT internal state. More...
 
+ + + + + + + + + + + + + + + + + + + + + + +

+Functions

CO_ReturnError_t CO_NMT_init (CO_NMT_t *NMT, const OD_entry_t *OD_1017_ProducerHbTime, CO_EM_t *em, uint8_t nodeId, CO_NMT_control_t NMTcontrol, uint16_t firstHBTime_ms, CO_CANmodule_t *NMT_CANdevRx, uint16_t NMT_rxIdx, uint16_t CANidRxNMT, CO_CANmodule_t *NMT_CANdevTx, uint16_t NMT_txIdx, uint16_t CANidTxNMT, CO_CANmodule_t *HB_CANdevTx, uint16_t HB_txIdx, uint16_t CANidTxHB)
 Initialize NMT and Heartbeat producer object. More...
 
void CO_NMT_initCallbackPre (CO_NMT_t *NMT, void *object, void(*pFunctSignal)(void *object))
 Initialize NMT callback function after message preprocessed. More...
 
void CO_NMT_initCallbackChanged (CO_NMT_t *NMT, void(*pFunctNMT)(CO_NMT_internalState_t state))
 Initialize NMT callback function. More...
 
CO_NMT_reset_cmd_t CO_NMT_process (CO_NMT_t *NMT, CO_NMT_internalState_t *NMTstate, uint32_t timeDifference_us, uint32_t *timerNext_us)
 Process received NMT and produce Heartbeat messages. More...
 
static CO_NMT_internalState_t CO_NMT_getInternalState (CO_NMT_t *NMT)
 Query current NMT state. More...
 
static void CO_NMT_setInternalState (CO_NMT_t *NMT, CO_NMT_internalState_t state)
 Set internal NMT state. More...
 
CO_ReturnError_t CO_NMT_sendCommand (CO_NMT_t *NMT, CO_NMT_command_t command, uint8_t nodeID)
 Send NMT master command. More...
 
+

Detailed Description

+

CANopen Network management and Heartbeat producer protocol.

+

CANopen device can be in one of the CO_NMT_internalState_t

    +
  • Initializing. It is active before CANopen is initialized.
    • +
  • Pre-operational. All CANopen objects are active, except PDOs.
    • +
  • Operational. Process data objects (PDOs) are active too.
    • +
  • Stopped. Only Heartbeat producer and NMT consumer are active.
    • +
+

NMT master can change the internal state of the devices by sending CO_NMT_command_t.

+

NMT message contents:

+ + + + + + + +
Byte Description
0 CO_NMT_command_t
1 Node ID. If zero, command addresses all nodes.
+

Heartbeat message contents:

+ + + + + +
Byte Description
0 CO_NMT_internalState_t
+

See CO_Default_CAN_ID_t for CAN identifiers.

+

Enumeration Type Documentation

+ +

◆ CO_NMT_internalState_t

+ +
+
+ + + + +
enum CO_NMT_internalState_t
+
+ +

Internal network state of the CANopen node.

+ + + + + + +
Enumerator
CO_NMT_UNKNOWN 

-1, Device state is unknown (for heartbeat consumer)

+
CO_NMT_INITIALIZING 

0, Device is initializing

+
CO_NMT_PRE_OPERATIONAL 

127, Device is in pre-operational state

+
CO_NMT_OPERATIONAL 

5, Device is in operational state

+
CO_NMT_STOPPED 

4, Device is stopped

+
+ +
+
+ +

◆ CO_NMT_command_t

+ +
+
+ + + + +
enum CO_NMT_command_t
+
+ +

Commands from NMT master.

+ + + + + + +
Enumerator
CO_NMT_ENTER_OPERATIONAL 

1, Start device

+
CO_NMT_ENTER_STOPPED 

2, Stop device

+
CO_NMT_ENTER_PRE_OPERATIONAL 

128, Put device into pre-operational

+
CO_NMT_RESET_NODE 

129, Reset device

+
CO_NMT_RESET_COMMUNICATION 

130, Reset CANopen communication on device

+
+ +
+
+ +

◆ CO_NMT_reset_cmd_t

+ +
+
+ + + + +
enum CO_NMT_reset_cmd_t
+
+ +

Return code from CO_NMT_process() that tells application code what to reset.

+ + + + + +
Enumerator
CO_RESET_NOT 

0, Normal return, no action

+
CO_RESET_COMM 

1, Application must provide communication reset.

+
CO_RESET_APP 

2, Application must provide complete device reset

+
CO_RESET_QUIT 

3, Application must quit, no reset of microcontroller (command is not requested by the stack.)

+
+ +
+
+ +

◆ CO_NMT_control_t

+ +
+
+ + + + +
enum CO_NMT_control_t
+
+ +

NMT control bitfield for NMT internal state.

+

Variable of this type is passed to CO_NMT_init() function. It controls behavior of the CO_NMT_internalState_t of the device according to CANopen error register.

+

Internal NMT state is controlled also with external NMT command, CO_NMT_setInternalState() or CO_NMT_sendCommand() functions.

+ + + + + + + +
Enumerator
CO_NMT_ERR_REG_MASK 

First 8 bits can be used to specify bitmask for the CO_errorRegister_t, to get relevant bits for the calculation.

+
CO_NMT_STARTUP_TO_OPERATIONAL 

If bit is set, then device enters NMT operational state after the initialization phase, otherwise it enters NMT pre-operational state.

+
CO_NMT_ERR_ON_BUSOFF_HB 

If bit is set and device is operational, it enters NMT pre-operational or stopped state, if CAN bus is off or heartbeat consumer timeout is detected.

+
CO_NMT_ERR_ON_ERR_REG 

If bit is set and device is operational, it enters NMT pre-operational or stopped state, if masked CANopen error register is different than zero.

+
CO_NMT_ERR_TO_STOPPED 

If bit is set and CO_NMT_ERR_ON_xx condition is met, then device will enter NMT stopped state, otherwise it will enter NMT pre-op state.

+
CO_NMT_ERR_FREE_TO_OPERATIONAL 

If bit is set and device is pre-operational, it enters NMT operational state automatically, if conditions from CO_NMT_ERR_ON_xx are all false.

+
+ +
+
+

Function Documentation

+ +

◆ CO_NMT_init()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
CO_ReturnError_t CO_NMT_init (CO_NMT_t * NMT,
const OD_entry_t * OD_1017_ProducerHbTime,
CO_EM_t * em,
uint8_t nodeId,
CO_NMT_control_t NMTcontrol,
uint16_t firstHBTime_ms,
CO_CANmodule_t * NMT_CANdevRx,
uint16_t NMT_rxIdx,
uint16_t CANidRxNMT,
CO_CANmodule_t * NMT_CANdevTx,
uint16_t NMT_txIdx,
uint16_t CANidTxNMT,
CO_CANmodule_t * HB_CANdevTx,
uint16_t HB_txIdx,
uint16_t CANidTxHB 
)
+
+ +

Initialize NMT and Heartbeat producer object.

+

Function must be called in the communication reset section.

+
Parameters
+ + + + + + + + + + + + + + + + +
NMTThis object will be initialized.
OD_1017_ProducerHbTimeOD entry for 0x1017 -"Producer heartbeat time", entry is required, IO extension is optional for runtime configuration.
emEmergency object.
nodeIdCANopen Node ID of this device.
NMTcontrolControl variable for calculation of NMT internal state, based on error register, startup and runtime behavior.
firstHBTime_msTime between bootup and first heartbeat message in milliseconds. If firstHBTime_ms is greater than "Producer Heartbeat time" (OD object 0x1017), latter is used instead. Entry is required, IO extension is optional.
NMT_CANdevRxCAN device for NMT reception.
NMT_rxIdxIndex of receive buffer in above CAN device.
CANidRxNMTCAN identifier for NMT receive message.
NMT_CANdevTxCAN device for NMT master transmission.
NMT_txIdxIndex of transmit buffer in above CAN device.
CANidTxNMTCAN identifier for NMT transmit message.
HB_CANdevTxCAN device for HB transmission.
HB_txIdxIndex of transmit buffer in the above CAN device.
CANidTxHBCAN identifier for HB message.
+
+
+
Returns
CO_ReturnError_t CO_ERROR_NO on success.
+ +
+
+ +

◆ CO_NMT_initCallbackPre()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + +
void CO_NMT_initCallbackPre (CO_NMT_t * NMT,
void * object,
void(*)(void *object) pFunctSignal 
)
+
+ +

Initialize NMT callback function after message preprocessed.

+

Function initializes optional callback function, which should immediately start processing of CO_NMT_process() function. Callback is called after NMT message is received from the CAN bus.

+
Parameters
+ + + + +
NMTThis object.
objectPointer to object, which will be passed to pFunctSignal(). Can be NULL
pFunctSignalPointer to the callback function. Not called if NULL.
+
+
+ +
+
+ +

◆ CO_NMT_initCallbackChanged()

+ +
+
+ + + + + + + + + + + + + + + + + + +
void CO_NMT_initCallbackChanged (CO_NMT_t * NMT,
void(*)(CO_NMT_internalState_t state) pFunctNMT 
)
+
+ +

Initialize NMT callback function.

+

Function initializes optional callback function, which is called after NMT State change has occurred. Function may wake up external task which handles NMT events. The first call is made immediately to give the consumer the current NMT state.

+
Parameters
+ + + +
NMTThis object.
pFunctNMTPointer to the callback function. Not called if NULL.
+
+
+ +
+
+ +

◆ CO_NMT_process()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
CO_NMT_reset_cmd_t CO_NMT_process (CO_NMT_t * NMT,
CO_NMT_internalState_t * NMTstate,
uint32_t timeDifference_us,
uint32_t * timerNext_us 
)
+
+ +

Process received NMT and produce Heartbeat messages.

+

Function must be called cyclically.

+
Parameters
+ + + + + +
NMTThis object.
[out]NMTstateIf not NULL, CANopen NMT internal state is returned.
timeDifference_usTime difference from previous function call in microseconds.
[out]timerNext_usinfo to OS - see CO_process().
+
+
+
Returns
CO_NMT_reset_cmd_t
+ +
+
+ +

◆ CO_NMT_getInternalState()

+ +
+
+ + + + + +
+ + + + + + + + +
static CO_NMT_internalState_t CO_NMT_getInternalState (CO_NMT_t * NMT)
+ 		+inlinestatic
+
+ +

Query current NMT state.

+
Parameters
+ + +
NMTThis object.
+
+
+
Returns
CO_NMT_internalState_t
+ +
+
+ +

◆ CO_NMT_setInternalState()

+ +
+
+ + + + + +
+ + + + + + + + + + + + + + + + + + +
static void CO_NMT_setInternalState (CO_NMT_t * NMT,
CO_NMT_internalState_t state 
)
+ 		+inlinestatic
+
+ +

Set internal NMT state.

+

Functions sets state directly, without any checking. CO_NMT_process() may also switch between states automatically, see CO_NMT_control_t.

+
Parameters
+ + + +
NMTThis object.
stateNew state.
+
+
+ +
+
+ +

◆ CO_NMT_sendCommand()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + +
CO_ReturnError_t CO_NMT_sendCommand (CO_NMT_t * NMT,
CO_NMT_command_t command,
uint8_t nodeID 
)
+
+ +

Send NMT master command.

+

This functionality may only be used from NMT master, as specified by standard CiA302-2. Standard provides one exception, where application from slave node may send NMT master command: "If CANopen object 0x1F80 has value of 0x2, then NMT slave shall execute the NMT service start remote node (CO_NMT_ENTER_OPERATIONAL) with nodeID set to 0."

+
Parameters
+ + + + +
NMTThis object.
commandNMT command from CO_NMT_command_t.
nodeIDNode ID of the remote node. 0 for all nodes including self.
+
+
+
Returns
CO_ERROR_NO on success or CO_ReturnError_t from CO_CANsend().
+ +
+
+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__NMT__Heartbeat.js b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__NMT__Heartbeat.js new file mode 100755 index 0000000..7b2cc53 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__NMT__Heartbeat.js @@ -0,0 +1,56 @@ +var group__CO__NMT__Heartbeat = +[ + [ "CO_NMT_Heartbeat.h", "CO__NMT__Heartbeat_8h.html", null ], + [ "CO_NMT_t", "structCO__NMT__t.html", [ + [ "operatingState", "structCO__NMT__t.html#abebf973c819c48fa36c48ba2eb84818a", null ], + [ "operatingStatePrev", "structCO__NMT__t.html#a544df8108d0853dd6d2938fa253ef018", null ], + [ "internalCommand", "structCO__NMT__t.html#a7a56fcf387bd2922ece9106538d986ef", null ], + [ "nodeId", "structCO__NMT__t.html#a6cf0441aff58a3e208d1ed221a26709c", null ], + [ "NMTcontrol", "structCO__NMT__t.html#a0c28cae6c3c7319c8fdfe694a3b4d9a7", null ], + [ "HBproducerTime_us", "structCO__NMT__t.html#a983a4ba9a004d216e32414c0a38736c4", null ], + [ "HBproducerTimer", "structCO__NMT__t.html#a0babc82bd6dfde07511b87167941b4f0", null ], + [ "em", "structCO__NMT__t.html#a60848ed23fb775dc4412be0e7ff9bc4b", null ], + [ "NMT_CANdevTx", "structCO__NMT__t.html#aaa1a7f9278595d82eaddbee8ac434ca9", null ], + [ "NMT_TXbuff", "structCO__NMT__t.html#a6c9ca6315daa74257699ff657165d409", null ], + [ "HB_CANdevTx", "structCO__NMT__t.html#aedcf17c643f41370a978794301f00169", null ], + [ "HB_TXbuff", "structCO__NMT__t.html#aa9fb1bb36758a1ff27b44c20fd6a0192", null ], + [ "pFunctSignalPre", "structCO__NMT__t.html#a31b37e4e64737bb3ef4331d607adbed6", null ], + [ "functSignalObjectPre", "structCO__NMT__t.html#a9ae2c962bc4aaa477555920dc0323302", null ], + [ "pFunctNMT", "structCO__NMT__t.html#a64bc9d74c871bf560d8b5db85e4d051e", null ] + ] ], + [ "CO_NMT_internalState_t", "group__CO__NMT__Heartbeat.html#ga1e8c2a6c0fd4a33183503d25a7c6d744", [ + [ "CO_NMT_UNKNOWN", "group__CO__NMT__Heartbeat.html#gga1e8c2a6c0fd4a33183503d25a7c6d744a52d111d4262b514df4433ae07969cc3c", null ], + [ "CO_NMT_INITIALIZING", "group__CO__NMT__Heartbeat.html#gga1e8c2a6c0fd4a33183503d25a7c6d744a672c48c0e9bd5cb070bd86e1dcdc085c", null ], + [ "CO_NMT_PRE_OPERATIONAL", "group__CO__NMT__Heartbeat.html#gga1e8c2a6c0fd4a33183503d25a7c6d744ae41d4ebcb7e26e165c8685b5809871a9", null ], + [ "CO_NMT_OPERATIONAL", "group__CO__NMT__Heartbeat.html#gga1e8c2a6c0fd4a33183503d25a7c6d744a3f9d07a10f2781201b85e9de9ba1c9a5", null ], + [ "CO_NMT_STOPPED", "group__CO__NMT__Heartbeat.html#gga1e8c2a6c0fd4a33183503d25a7c6d744aba34c0b4b2c6a0c11b01236cbe949ed5", null ] + ] ], + [ "CO_NMT_command_t", "group__CO__NMT__Heartbeat.html#gac396242e2e12ef6b0b22ff48636bc4eb", [ + [ "CO_NMT_ENTER_OPERATIONAL", "group__CO__NMT__Heartbeat.html#ggac396242e2e12ef6b0b22ff48636bc4eba6c7cdae79e5939cf321cdf82aa6b4146", null ], + [ "CO_NMT_ENTER_STOPPED", "group__CO__NMT__Heartbeat.html#ggac396242e2e12ef6b0b22ff48636bc4eba8a2911a2a576a1bd83b7f4a247cd77c9", null ], + [ "CO_NMT_ENTER_PRE_OPERATIONAL", "group__CO__NMT__Heartbeat.html#ggac396242e2e12ef6b0b22ff48636bc4eba575ed4f3386e8a63498921e659aa7ba4", null ], + [ "CO_NMT_RESET_NODE", "group__CO__NMT__Heartbeat.html#ggac396242e2e12ef6b0b22ff48636bc4ebaa8558d18a80148a598e6295ef41cdf97", null ], + [ "CO_NMT_RESET_COMMUNICATION", "group__CO__NMT__Heartbeat.html#ggac396242e2e12ef6b0b22ff48636bc4ebabc6aef9d84c816eecaea38f9fe2cbd9c", null ] + ] ], + [ "CO_NMT_reset_cmd_t", "group__CO__NMT__Heartbeat.html#gaf42f056a571b8e17a2d74428d1a49674", [ + [ "CO_RESET_NOT", "group__CO__NMT__Heartbeat.html#ggaf42f056a571b8e17a2d74428d1a49674a1e75deb437b2e06b2c1f353bf76d8807", null ], + [ "CO_RESET_COMM", "group__CO__NMT__Heartbeat.html#ggaf42f056a571b8e17a2d74428d1a49674a2a48dc2be7c9ffa7874eb08a1ba39f6f", null ], + [ "CO_RESET_APP", "group__CO__NMT__Heartbeat.html#ggaf42f056a571b8e17a2d74428d1a49674a2ab187bf1daccd4bc31685c08ca93bf1", null ], + [ "CO_RESET_QUIT", "group__CO__NMT__Heartbeat.html#ggaf42f056a571b8e17a2d74428d1a49674ae2c7805e42ce9b7c893018922b24a102", null ] + ] ], + [ "CO_NMT_control_t", "group__CO__NMT__Heartbeat.html#gaf92cf5943801e5dda84654345cc3d67f", [ + [ "CO_NMT_ERR_REG_MASK", "group__CO__NMT__Heartbeat.html#ggaf92cf5943801e5dda84654345cc3d67faaa5ab06e2047e51ac7d2767e8a821d80", null ], + [ "CO_NMT_STARTUP_TO_OPERATIONAL", "group__CO__NMT__Heartbeat.html#ggaf92cf5943801e5dda84654345cc3d67faf4da9fa5d805e0f535abf95a175312e8", null ], + [ "CO_NMT_ERR_ON_BUSOFF_HB", "group__CO__NMT__Heartbeat.html#ggaf92cf5943801e5dda84654345cc3d67faf151918ecf07fc43ba30489d31ca275a", null ], + [ "CO_NMT_ERR_ON_ERR_REG", "group__CO__NMT__Heartbeat.html#ggaf92cf5943801e5dda84654345cc3d67faab1cd82001a087114a5361910e206c74", null ], + [ "CO_NMT_ERR_TO_STOPPED", "group__CO__NMT__Heartbeat.html#ggaf92cf5943801e5dda84654345cc3d67fa9498a5af4e0557a2c0284c04b26b3eb5", null ], + [ "CO_NMT_ERR_FREE_TO_OPERATIONAL", "group__CO__NMT__Heartbeat.html#ggaf92cf5943801e5dda84654345cc3d67fa6e79d137c725417786035982cd0e2687", null ] + ] ], + [ "CO_NMT_init", "group__CO__NMT__Heartbeat.html#ga53c92521fff0fab405f8a372702c7259", null ], + [ "CO_NMT_initCallbackPre", "group__CO__NMT__Heartbeat.html#ga58b6123938e950b5e8c38ec0b9caeec4", null ], + [ "CO_NMT_initCallbackChanged", "group__CO__NMT__Heartbeat.html#ga87d153e39b32586b67ee6dd47dfce787", null ], + [ "CO_NMT_process", "group__CO__NMT__Heartbeat.html#ga724ff5b1d9cfee955914f365514deda0", null ], + [ "CO_NMT_getInternalState", "group__CO__NMT__Heartbeat.html#ga2e3f0d579e321d73b53bf469d49b0ce3", null ], + [ "CO_NMT_setInternalState", "group__CO__NMT__Heartbeat.html#ga91b7041eda0487bbc0588d3200e0e868", null ], + [ "CO_NMT_sendCommand", "group__CO__NMT__Heartbeat.html#ga9d557708be1bfb9b169718abea90b663", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__ODdefinition.html b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__ODdefinition.html new file mode 100755 index 0000000..1325f39 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__ODdefinition.html @@ -0,0 +1,192 @@ + + + + + + + +CANopenNode: OD definition objects + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Data Structures | +Enumerations
+
+
OD definition objects
CANopen_301 » OD interface
+
+
+ +

Types and functions used only for definition of Object Dictionary. +More...

+ + + + + + + + + + + + + + + + + +

+Data Structures

struct  OD_obj_var_t
 Object for single OD variable, used for "VAR" type OD objects. More...
 
struct  OD_obj_array_t
 Object for OD array of variables, used for "ARRAY" type OD objects. More...
 
struct  OD_obj_record_t
 Object for OD sub-elements, used in "RECORD" type OD objects. More...
 
struct  OD_extensionIO_t
 Object pointed by OD_obj_extended_t contains application specified parameters for extended OD object. More...
 
struct  OD_obj_extended_t
 Object for extended type of OD variable, configurable by OD_extensionIO_init() function. More...
 
+ + + + +

+Enumerations

enum  OD_objectTypes_t {
+  ODT_VAR = 0x01, +ODT_ARR = 0x02, +ODT_REC = 0x03, +ODT_EVAR = 0x11, +
+  ODT_EARR = 0x12, +ODT_EREC = 0x13, +ODT_TYPE_MASK = 0x0F, +ODT_EXTENSION_MASK = 0x10 +
+ }
 Types for OD object. More...
 
+

Detailed Description

+

Types and functions used only for definition of Object Dictionary.

+

Enumeration Type Documentation

+ +

◆ OD_objectTypes_t

+ +
+
+ + + + +
enum OD_objectTypes_t
+
+ +

Types for OD object.

+ + + + + + + + + +
Enumerator
ODT_VAR 

This type corresponds to CANopen Object Dictionary object with object code equal to VAR.

+

OD object is type of OD_obj_var_t and represents single variable of any type (any length), located on sub-index 0. Other sub-indexes are not used.

+
ODT_ARR 

This type corresponds to CANopen Object Dictionary object with object code equal to ARRAY.

+

OD object is type of OD_obj_array_t and represents array of variables with the same type, located on sub-indexes above 0. Sub-index 0 is of type uint8_t and usually represents length of the array.

+
ODT_REC 

This type corresponds to CANopen Object Dictionary object with object code equal to RECORD.

+

This type of OD object represents structure of the variables. Each variable from the structure can have own type and own attribute. OD object is an array of elements of type OD_obj_var_t. Variable at sub-index 0 is of type uint8_t and usually represents number of sub-elements in the structure.

+
ODT_EVAR 

Same as ODT_VAR, but extended with OD_obj_extended_t type.

+

It includes additional pointer to IO extension and PDO flags

+
ODT_EARR 

Same as ODT_ARR, but extended with OD_obj_extended_t type.

+
ODT_EREC 

Same as ODT_REC, but extended with OD_obj_extended_t type.

+
ODT_TYPE_MASK 

Mask for basic type.

+
ODT_EXTENSION_MASK 

Mask for extension.

+
+ +
+
+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__ODdefinition.js b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__ODdefinition.js new file mode 100755 index 0000000..9f5cf25 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__ODdefinition.js @@ -0,0 +1,42 @@ +var group__CO__ODdefinition = +[ + [ "OD_obj_var_t", "structOD__obj__var__t.html", [ + [ "data", "structOD__obj__var__t.html#a7c15865c69e0dc0a09f6d21bd890d062", null ], + [ "attribute", "structOD__obj__var__t.html#a4662bd6ca12b3ec147f9ffeafb64fe77", null ], + [ "dataLength", "structOD__obj__var__t.html#a385af11ed619b78de9b2f1ae6528a870", null ] + ] ], + [ "OD_obj_array_t", "structOD__obj__array__t.html", [ + [ "data0", "structOD__obj__array__t.html#a9a4ca22f014061ca9d926ab43036bc1f", null ], + [ "data", "structOD__obj__array__t.html#a009bcf700d8e27c93e886ad7ff7fb2eb", null ], + [ "attribute0", "structOD__obj__array__t.html#a1cb4802d94112e5bd2f1b0db5e3e5d99", null ], + [ "attribute", "structOD__obj__array__t.html#a6af20a410bcd0c8c9f619c4a564b962a", null ], + [ "dataElementLength", "structOD__obj__array__t.html#a985fb68eba74f9e8fb76a4c5d85e96e9", null ], + [ "dataElementSizeof", "structOD__obj__array__t.html#ad310fa351ebb2a44f66451dd12675bf9", null ] + ] ], + [ "OD_obj_record_t", "structOD__obj__record__t.html", [ + [ "data", "structOD__obj__record__t.html#a490d4818eaa6ca94d2c6fc598647dbd1", null ], + [ "subIndex", "structOD__obj__record__t.html#a7cfa0012ae3aec7bbfc98e5e8301782c", null ], + [ "attribute", "structOD__obj__record__t.html#a42290a19541170f8d108acf029fec171", null ], + [ "dataLength", "structOD__obj__record__t.html#a1302f14c20e976ac884196d4c3e201c1", null ] + ] ], + [ "OD_extensionIO_t", "structOD__extensionIO__t.html", [ + [ "object", "structOD__extensionIO__t.html#a929dace9c5bf5f1e3090f3fbff40f9b8", null ], + [ "read", "structOD__extensionIO__t.html#af4c210e173adb94e297ad26eacb2b678", null ], + [ "write", "structOD__extensionIO__t.html#a06573b7740c3c991352734bba25f0fd4", null ] + ] ], + [ "OD_obj_extended_t", "structOD__obj__extended__t.html", [ + [ "extIO", "structOD__obj__extended__t.html#a6ccf59c770c3887233ab7b850e77e375", null ], + [ "flagsPDO", "structOD__obj__extended__t.html#a80e8186d50cb6ca8ebf447d43815e566", null ], + [ "odObjectOriginal", "structOD__obj__extended__t.html#abbc62d96e2ecafc99bb1eaf1210f816a", null ] + ] ], + [ "OD_objectTypes_t", "group__CO__ODdefinition.html#gaae426e9d66ec1bacfef2d93f096d7805", [ + [ "ODT_VAR", "group__CO__ODdefinition.html#ggaae426e9d66ec1bacfef2d93f096d7805a829f1df882410efc0ea0e05b3435c820", null ], + [ "ODT_ARR", "group__CO__ODdefinition.html#ggaae426e9d66ec1bacfef2d93f096d7805a1ad5763beafe79c42ca223297c832ff4", null ], + [ "ODT_REC", "group__CO__ODdefinition.html#ggaae426e9d66ec1bacfef2d93f096d7805a9376cface357f03bec8a651a307f33b9", null ], + [ "ODT_EVAR", "group__CO__ODdefinition.html#ggaae426e9d66ec1bacfef2d93f096d7805adb58a7faa735918d39b8bbcd3a6ad594", null ], + [ "ODT_EARR", "group__CO__ODdefinition.html#ggaae426e9d66ec1bacfef2d93f096d7805a1ae954b4709b24d93bdcac69957c8e40", null ], + [ "ODT_EREC", "group__CO__ODdefinition.html#ggaae426e9d66ec1bacfef2d93f096d7805ae176b06a942e47815d2e4c51a8f9b7f8", null ], + [ "ODT_TYPE_MASK", "group__CO__ODdefinition.html#ggaae426e9d66ec1bacfef2d93f096d7805ac9e24bde0d37c35c3065dbaa541e1acb", null ], + [ "ODT_EXTENSION_MASK", "group__CO__ODdefinition.html#ggaae426e9d66ec1bacfef2d93f096d7805a3dd8bc41ec11c475d487b877fdd0a879", null ] + ] ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__ODgetSetters.html b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__ODgetSetters.html new file mode 100755 index 0000000..dc6c768 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__ODgetSetters.html @@ -0,0 +1,452 @@ + + + + + + + +CANopenNode: Getters and setters + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Functions
+
+
Getters and setters
CANopen_301 » OD interface
+
+
+ +

Getter and setter helper functions for accessing different types of Object Dictionary variables. +More...

+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

+Functions

ODR_t OD_get_i8 (const OD_entry_t *entry, uint8_t subIndex, int8_t *val, bool_t odOrig)
 Get int8_t variable from Object Dictionary. More...
 
+ODR_t OD_get_i16 (const OD_entry_t *entry, uint8_t subIndex, int16_t *val, bool_t odOrig)
 Get int16_t variable from Object Dictionary, see OD_get_i8.
 
+ODR_t OD_get_i32 (const OD_entry_t *entry, uint8_t subIndex, int32_t *val, bool_t odOrig)
 Get int32_t variable from Object Dictionary, see OD_get_i8.
 
+ODR_t OD_get_i64 (const OD_entry_t *entry, uint8_t subIndex, int64_t *val, bool_t odOrig)
 Get int64_t variable from Object Dictionary, see OD_get_i8.
 
+ODR_t OD_get_u8 (const OD_entry_t *entry, uint8_t subIndex, uint8_t *val, bool_t odOrig)
 Get uint8_t variable from Object Dictionary, see OD_get_i8.
 
+ODR_t OD_get_u16 (const OD_entry_t *entry, uint8_t subIndex, uint16_t *val, bool_t odOrig)
 Get uint16_t variable from Object Dictionary, see OD_get_i8.
 
+ODR_t OD_get_u32 (const OD_entry_t *entry, uint8_t subIndex, uint32_t *val, bool_t odOrig)
 Get uint32_t variable from Object Dictionary, see OD_get_i8.
 
+ODR_t OD_get_u64 (const OD_entry_t *entry, uint8_t subIndex, uint64_t *val, bool_t odOrig)
 Get uint64_t variable from Object Dictionary, see OD_get_i8.
 
+ODR_t OD_get_r32 (const OD_entry_t *entry, uint8_t subIndex, float32_t *val, bool_t odOrig)
 Get float32_t variable from Object Dictionary, see OD_get_i8.
 
+ODR_t OD_get_r64 (const OD_entry_t *entry, uint8_t subIndex, float64_t *val, bool_t odOrig)
 Get float64_t variable from Object Dictionary, see OD_get_i8.
 
ODR_t OD_set_i8 (const OD_entry_t *entry, uint8_t subIndex, int8_t val, bool_t odOrig)
 Set int8_t variable in Object Dictionary. More...
 
+ODR_t OD_set_i16 (const OD_entry_t *entry, uint8_t subIndex, int16_t val, bool_t odOrig)
 Set int16_t variable in Object Dictionary, see OD_set_i8.
 
+ODR_t OD_set_i32 (const OD_entry_t *entry, uint8_t subIndex, int32_t val, bool_t odOrig)
 Set int16_t variable in Object Dictionary, see OD_set_i8.
 
+ODR_t OD_set_i64 (const OD_entry_t *entry, uint8_t subIndex, int64_t val, bool_t odOrig)
 Set int16_t variable in Object Dictionary, see OD_set_i8.
 
+ODR_t OD_set_u8 (const OD_entry_t *entry, uint8_t subIndex, uint8_t val, bool_t odOrig)
 Set uint8_t variable in Object Dictionary, see OD_set_i8.
 
+ODR_t OD_set_u16 (const OD_entry_t *entry, uint8_t subIndex, uint16_t val, bool_t odOrig)
 Set uint16_t variable in Object Dictionary, see OD_set_i8.
 
+ODR_t OD_set_u32 (const OD_entry_t *entry, uint8_t subIndex, uint32_t val, bool_t odOrig)
 Set uint32_t variable in Object Dictionary, see OD_set_i8.
 
+ODR_t OD_set_u64 (const OD_entry_t *entry, uint8_t subIndex, uint64_t val, bool_t odOrig)
 Set uint64_t variable in Object Dictionary, see OD_set_i8.
 
+ODR_t OD_set_r32 (const OD_entry_t *entry, uint8_t subIndex, float32_t val, bool_t odOrig)
 Set float32_t variable in Object Dictionary, see OD_set_i8.
 
+ODR_t OD_set_r64 (const OD_entry_t *entry, uint8_t subIndex, float64_t val, bool_t odOrig)
 Set float64_t variable in Object Dictionary, see OD_set_i8.
 
ODR_t OD_getPtr_i8 (const OD_entry_t *entry, uint8_t subIndex, int8_t **val)
 Get pointer to int8_t variable from Object Dictionary. More...
 
+ODR_t OD_getPtr_i16 (const OD_entry_t *entry, uint8_t subIndex, int16_t **val)
 Get pointer to int16_t variable from OD, see OD_getPtr_i8.
 
+ODR_t OD_getPtr_i32 (const OD_entry_t *entry, uint8_t subIndex, int32_t **val)
 Get pointer to int32_t variable from OD, see OD_getPtr_i8.
 
+ODR_t OD_getPtr_i64 (const OD_entry_t *entry, uint8_t subIndex, int64_t **val)
 Get pointer to int64_t variable from OD, see OD_getPtr_i8.
 
+ODR_t OD_getPtr_u8 (const OD_entry_t *entry, uint8_t subIndex, uint8_t **val)
 Get pointer to uint8_t variable from OD, see OD_getPtr_i8.
 
+ODR_t OD_getPtr_u16 (const OD_entry_t *entry, uint8_t subIndex, uint16_t **val)
 Get pointer to uint16_t variable from OD, see OD_getPtr_i8.
 
+ODR_t OD_getPtr_u32 (const OD_entry_t *entry, uint8_t subIndex, uint32_t **val)
 Get pointer to uint32_t variable from OD, see OD_getPtr_i8.
 
+ODR_t OD_getPtr_u64 (const OD_entry_t *entry, uint8_t subIndex, uint64_t **val)
 Get pointer to uint64_t variable from OD, see OD_getPtr_i8.
 
+ODR_t OD_getPtr_r32 (const OD_entry_t *entry, uint8_t subIndex, float32_t **val)
 Get pointer to float32_t variable from OD, see OD_getPtr_i8.
 
+ODR_t OD_getPtr_r64 (const OD_entry_t *entry, uint8_t subIndex, float64_t **val)
 Get pointer to float64_t variable from OD, see OD_getPtr_i8.
 
ODR_t OD_getPtr_vs (const OD_entry_t *entry, uint8_t subIndex, char **val, OD_size_t *dataLength)
 Get pointer to "visible string" variable from Object Dictionary. More...
 
+ODR_t OD_getPtr_os (const OD_entry_t *entry, uint8_t subIndex, uint8_t **val, OD_size_t *dataLength)
 Get pointer to "octet string" variable from OD, see OD_getPtr_vs.
 
+ODR_t OD_getPtr_us (const OD_entry_t *entry, uint8_t subIndex, uint16_t **val, OD_size_t *dataLength)
 Get pointer to "unicode string" variable from OD, see OD_getPtr_vs.
 
+

Detailed Description

+

Getter and setter helper functions for accessing different types of Object Dictionary variables.

+

Function Documentation

+ +

◆ OD_get_i8()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
ODR_t OD_get_i8 (const OD_entry_t * entry,
uint8_t subIndex,
int8_t * val,
bool_t odOrig 
)
+
+ +

Get int8_t variable from Object Dictionary.

+
Parameters
+ + + + + +
entryOD entry returned by OD_find().
subIndexSub-index of the variable from the OD object.
[out]valValue will be written here.
odOrigIf true, then potential IO extension on entry will be ignored and data in the original OD location will be returned.
+
+
+
Returns
Value from ODR_t, "ODR_OK" in case of success. Error, if variable does not exist in object dictionary or it does not have the correct length or other reason.
+ +
+
+ +

◆ OD_set_i8()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
ODR_t OD_set_i8 (const OD_entry_t * entry,
uint8_t subIndex,
int8_t val,
bool_t odOrig 
)
+
+ +

Set int8_t variable in Object Dictionary.

+
Parameters
+ + + + + +
entryOD entry returned by OD_find().
subIndexSub-index of the variable from the OD object.
valValue to write.
odOrigIf true, then potential IO extension on entry will be ignored and data in the original OD location will be written.
+
+
+
Returns
Value from ODR_t, "ODR_OK" in case of success. Error, if variable does not exist in object dictionary or it does not have the correct length or other reason.
+ +
+
+ +

◆ OD_getPtr_i8()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + +
ODR_t OD_getPtr_i8 (const OD_entry_t * entry,
uint8_t subIndex,
int8_t ** val 
)
+
+ +

Get pointer to int8_t variable from Object Dictionary.

+

Function always returns "dataObjectOriginal" pointer, which points to data in the original OD location. Take care, if IO extension is enabled on OD entry.

+
Parameters
+ + + + +
entryOD entry returned by OD_find().
subIndexSub-index of the variable from the OD object.
[out]valPointer to variable will be written here.
+
+
+
Returns
Value from ODR_t, "ODR_OK" in case of success. Error, if variable does not exist in object dictionary or it does not have the correct length or other reason.
+ +
+
+ +

◆ OD_getPtr_vs()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
ODR_t OD_getPtr_vs (const OD_entry_t * entry,
uint8_t subIndex,
char ** val,
OD_size_t * dataLength 
)
+
+ +

Get pointer to "visible string" variable from Object Dictionary.

+

Function always returns "dataObjectOriginal" pointer, which points to data in the original OD location. Take care, if IO extension is enabled on OD entry.

+
Parameters
+ + + + + +
entryOD entry returned by OD_find().
subIndexSub-index of the variable from the OD object.
[out]valPointer to variable will be written here.
[out]dataLengthTotal variable length will be written here, may be NULL.
+
+
+
Returns
Value from ODR_t, "ODR_OK" in case of success. Error, if variable does not exist in object dictionary or it has zero length or other reason.
+ +
+
+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__ODgetSetters.js b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__ODgetSetters.js new file mode 100755 index 0000000..7f265da --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__ODgetSetters.js @@ -0,0 +1,36 @@ +var group__CO__ODgetSetters = +[ + [ "OD_get_i8", "group__CO__ODgetSetters.html#gac174f05be716b7d169e0d7b7393e512c", null ], + [ "OD_get_i16", "group__CO__ODgetSetters.html#ga4bcc220e0cc4f8c6bfaf4d5cf31da448", null ], + [ "OD_get_i32", "group__CO__ODgetSetters.html#ga042737cecbf248d2cc6c874022d06e22", null ], + [ "OD_get_i64", "group__CO__ODgetSetters.html#gabfb67bc3602d9b5d901ac13e5271ccd0", null ], + [ "OD_get_u8", "group__CO__ODgetSetters.html#gad8fd318804b9f1ded265bfd07c6cdcf2", null ], + [ "OD_get_u16", "group__CO__ODgetSetters.html#gaa9cba6642facf33cdbe2e0155a90d571", null ], + [ "OD_get_u32", "group__CO__ODgetSetters.html#ga10e1975b6177b92e8da4b2b6a19533be", null ], + [ "OD_get_u64", "group__CO__ODgetSetters.html#ga2ef84e594a6733f7efeaeff55bfc9c9a", null ], + [ "OD_get_r32", "group__CO__ODgetSetters.html#gabbba108fa7a92cb48d31833bb804baa6", null ], + [ "OD_get_r64", "group__CO__ODgetSetters.html#gadf78189c35343bcd2ca1737491b55684", null ], + [ "OD_set_i8", "group__CO__ODgetSetters.html#ga46df632d54b48714cf50e4a3a92b4e98", null ], + [ "OD_set_i16", "group__CO__ODgetSetters.html#gaf207e188609f742be3c12f248f45138c", null ], + [ "OD_set_i32", "group__CO__ODgetSetters.html#ga101037295ea6af8488b499097f0d1ef1", null ], + [ "OD_set_i64", "group__CO__ODgetSetters.html#ga5c19e8e101d06a0203d13f8d3e997d16", null ], + [ "OD_set_u8", "group__CO__ODgetSetters.html#ga3f56347af1f0f9b8bf46463777df87d0", null ], + [ "OD_set_u16", "group__CO__ODgetSetters.html#gac63795511b7decfbbc46ddb7c2b59cfd", null ], + [ "OD_set_u32", "group__CO__ODgetSetters.html#ga5e5e943b89a2385f41a075131f47b5d5", null ], + [ "OD_set_u64", "group__CO__ODgetSetters.html#ga85570b44cc9d0af5b1084f42aeaf5e9f", null ], + [ "OD_set_r32", "group__CO__ODgetSetters.html#ga032219231c5e488969d02787265ad7ab", null ], + [ "OD_set_r64", "group__CO__ODgetSetters.html#ga75c5dd2daa6f0352bf079d18c9e90708", null ], + [ "OD_getPtr_i8", "group__CO__ODgetSetters.html#gaa4e6671855056e5ae13d198acdfab664", null ], + [ "OD_getPtr_i16", "group__CO__ODgetSetters.html#gaddbad04c274a68f3fabfbbdd13e83cfc", null ], + [ "OD_getPtr_i32", "group__CO__ODgetSetters.html#ga085db308dd11ea7047c815d214f12a32", null ], + [ "OD_getPtr_i64", "group__CO__ODgetSetters.html#gae7e11e1bdf04006dcc6a9f91287c459b", null ], + [ "OD_getPtr_u8", "group__CO__ODgetSetters.html#gaaa9f06494001462d32a74d11438d3157", null ], + [ "OD_getPtr_u16", "group__CO__ODgetSetters.html#gad1e334a640dc30553ece27e32e270bbb", null ], + [ "OD_getPtr_u32", "group__CO__ODgetSetters.html#ga92f57ed14ed69ac00c78d48f1e479bb0", null ], + [ "OD_getPtr_u64", "group__CO__ODgetSetters.html#gad192b1efde6a5bd6fa62fc5a4c484d84", null ], + [ "OD_getPtr_r32", "group__CO__ODgetSetters.html#gaf4e3f15fc0fbe2b24e88aeb53c9daa61", null ], + [ "OD_getPtr_r64", "group__CO__ODgetSetters.html#ga87a34e3e04dd2faa851d2d9c0a24e945", null ], + [ "OD_getPtr_vs", "group__CO__ODgetSetters.html#gaad4d755515e8dccf3616f14fe3914bde", null ], + [ "OD_getPtr_os", "group__CO__ODgetSetters.html#ga06361976dce86f180de76e1fced9a212", null ], + [ "OD_getPtr_us", "group__CO__ODgetSetters.html#gaf3efb5f4a50ff7ab17b37bf9c0fcf030", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__ODinterface.html b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__ODinterface.html new file mode 100755 index 0000000..f4d4e84 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__ODinterface.html @@ -0,0 +1,898 @@ + + + + + + + +CANopenNode: OD interface + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Modules | +Files | +Data Structures | +Macros | +Enumerations | +Functions
+
+
OD interface
CANopen_301
+
+
+ + + + + + + + +

+Modules

 Getters and setters
 Getter and setter helper functions for accessing different types of Object Dictionary variables.
 
 OD definition objects
 Types and functions used only for definition of Object Dictionary.
 
+ + + + +

+Files

file  CO_ODinterface.h
 CANopen Object Dictionary interface.
 
+ + + + + + + + + + + + + + + + +

+Data Structures

struct  OD_subEntry_t
 Structure describing properties of a variable, located in specific index and sub-index inside the Object Dictionary. More...
 
struct  OD_stream_t
 IO stream structure, used for read/write access to OD variable, part of OD_IO_t. More...
 
struct  OD_IO_t
 Structure for input / output on the OD variable. More...
 
struct  OD_entry_t
 Object Dictionary entry for one OD object. More...
 
struct  OD_t
 Object Dictionary. More...
 
+ + + + + + + + + + +

+Macros

+#define OD_size_t   uint32_t
 Variable of type OD_size_t contains data length in bytes of OD variable.
 
+#define OD_flagsPDO_t   uint32_t
 Type of flagsPDO variable from OD_subEntry_t.
 
+#define OD_attr_t   uint8_t
 Size of Object Dictionary attribute.
 
+ + + + + + + + + + +

+Enumerations

enum  OD_ObjDicId_30x_t {
+  OD_H1000_DEV_TYPE = 0x1000U, +OD_H1001_ERR_REG = 0x1001U, +OD_H1002_MANUF_STATUS_REG = 0x1002U, +OD_H1003_PREDEF_ERR_FIELD = 0x1003U, +
+  OD_H1004_RSV = 0x1004U, +OD_H1005_COBID_SYNC = 0x1005U, +OD_H1006_COMM_CYCL_PERIOD = 0x1006U, +OD_H1007_SYNC_WINDOW_LEN = 0x1007U, +
+  OD_H1008_MANUF_DEV_NAME = 0x1008U, +OD_H1009_MANUF_HW_VERSION = 0x1009U, +OD_H100A_MANUF_SW_VERSION = 0x100AU, +OD_H100B_RSV = 0x100BU, +
+  OD_H100C_GUARD_TIME = 0x100CU, +OD_H100D_LIFETIME_FACTOR = 0x100DU, +OD_H100E_RSV = 0x100EU, +OD_H100F_RSV = 0x100FU, +
+  OD_H1010_STORE_PARAM_FUNC = 0x1010U, +OD_H1011_REST_PARAM_FUNC = 0x1011U, +OD_H1012_COBID_TIME = 0x1012U, +OD_H1013_HIGH_RES_TIMESTAMP = 0x1013U, +
+  OD_H1014_COBID_EMERGENCY = 0x1014U, +OD_H1015_INHIBIT_TIME_EMCY = 0x1015U, +OD_H1016_CONSUMER_HB_TIME = 0x1016U, +OD_H1017_PRODUCER_HB_TIME = 0x1017U, +
+  OD_H1018_IDENTITY_OBJECT = 0x1018U, +OD_H1019_SYNC_CNT_OVERFLOW = 0x1019U, +OD_H1020_VERIFY_CONFIG = 0x1020U, +OD_H1021_STORE_EDS = 0x1021U, +
+  OD_H1022_STORE_FORMAT = 0x1022U, +OD_H1023_OS_CMD = 0x1023U, +OD_H1024_OS_CMD_MODE = 0x1024U, +OD_H1025_OS_DBG_INTERFACE = 0x1025U, +
+  OD_H1026_OS_PROMPT = 0x1026U, +OD_H1027_MODULE_LIST = 0x1027U, +OD_H1028_EMCY_CONSUMER = 0x1028U, +OD_H1029_ERR_BEHAVIOR = 0x1029U, +
+  OD_H1200_SDO_SERVER_1_PARAM = 0x1200U, +OD_H1280_SDO_CLIENT_1_PARAM = 0x1280U, +OD_H1300_GFC_PARAM = 0x1300U, +OD_H1301_SRDO_1_PARAM = 0x1301U, +
+  OD_H1381_SRDO_1_MAPPING = 0x1381U, +OD_H13FE_SRDO_VALID = 0x13FEU, +OD_H13FF_SRDO_CHECKSUM = 0x13FFU, +OD_H1400_RXPDO_1_PARAM = 0x1400U, +
+  OD_H1600_RXPDO_1_MAPPING = 0x1600U, +OD_H1800_TXPDO_1_PARAM = 0x1800U, +OD_H1A00_TXPDO_1_MAPPING = 0x1A00U +
+ }
 Common DS301 object dictionary entries. More...
 
enum  OD_attributes_t {
+  ODA_SDO_R = 0x01, +ODA_SDO_W = 0x02, +ODA_SDO_RW = 0x03, +ODA_TPDO = 0x04, +
+  ODA_RPDO = 0x08, +ODA_TRPDO = 0x0C, +ODA_TSRDO = 0x10, +ODA_RSRDO = 0x20, +
+  ODA_TRSRDO = 0x30, +ODA_MB = 0x40, +ODA_STR = 0x80 +
+ }
 Attributes (bit masks) for OD sub-object. More...
 
enum  ODR_t {
+  ODR_PARTIAL = -1, +ODR_OK = 0, +ODR_OUT_OF_MEM = 1, +ODR_UNSUPP_ACCESS = 2, +
+  ODR_WRITEONLY = 3, +ODR_READONLY = 4, +ODR_IDX_NOT_EXIST = 5, +ODR_NO_MAP = 6, +
+  ODR_MAP_LEN = 7, +ODR_PAR_INCOMPAT = 8, +ODR_DEV_INCOMPAT = 9, +ODR_HW = 10, +
+  ODR_TYPE_MISMATCH = 11, +ODR_DATA_LONG = 12, +ODR_DATA_SHORT = 13, +ODR_SUB_NOT_EXIST = 14, +
+  ODR_INVALID_VALUE = 15, +ODR_VALUE_HIGH = 16, +ODR_VALUE_LOW = 17, +ODR_MAX_LESS_MIN = 18, +
+  ODR_NO_RESOURCE = 19, +ODR_GENERAL = 20, +ODR_DATA_TRANSF = 21, +ODR_DATA_LOC_CTRL = 22, +
+  ODR_DATA_DEV_STATE = 23, +ODR_OD_MISSING = 23, +ODR_NO_DATA = 25, +ODR_COUNT = 26 +
+ }
 Return codes from OD access functions. More...
 
+ + + + + + + + + + + + + + + + + + + + + + + + + +

+Functions

OD_size_t OD_readOriginal (OD_stream_t *stream, uint8_t subIndex, void *buf, OD_size_t count, ODR_t *returnCode)
 Read value from original OD location. More...
 
OD_size_t OD_writeOriginal (OD_stream_t *stream, uint8_t subIndex, const void *buf, OD_size_t count, ODR_t *returnCode)
 Write value to original OD location. More...
 
const OD_entry_t * OD_find (const OD_t *od, uint16_t index)
 Find OD entry in Object Dictionary. More...
 
ODR_t OD_getSub (const OD_entry_t *entry, uint8_t subIndex, OD_subEntry_t *subEntry, OD_IO_t *io, bool_t odOrig)
 Find sub-object with specified sub-index on OD entry returned by OD_find. More...
 
static uint16_t OD_getIndex (const OD_entry_t *entry)
 Return index from OD entry. More...
 
static void OD_rwRestart (OD_stream_t *stream)
 Restart read or write operation on OD variable. More...
 
uint32_t OD_getSDOabCode (ODR_t returnCode)
 Get SDO abort code from returnCode. More...
 
ODR_t OD_extensionIO_init (const OD_entry_t *entry, void *object, OD_size_t(*read)(OD_stream_t *stream, uint8_t subIndex, void *buf, OD_size_t count, ODR_t *returnCode), OD_size_t(*write)(OD_stream_t *stream, uint8_t subIndex, const void *buf, OD_size_t count, ODR_t *returnCode))
 Initialise extended OD object with own read/write functions. More...
 
+

Detailed Description

+

See Object Dictionary

+

Enumeration Type Documentation

+ +

◆ OD_ObjDicId_30x_t

+ +
+
+ + + + +
enum OD_ObjDicId_30x_t
+
+ +

Common DS301 object dictionary entries.

+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Enumerator
OD_H1000_DEV_TYPE 

Device type.

+
OD_H1001_ERR_REG 

Error register.

+
OD_H1002_MANUF_STATUS_REG 

Manufacturer status register.

+
OD_H1003_PREDEF_ERR_FIELD 

Predefined error field.

+
OD_H1004_RSV 

Reserved.

+
OD_H1005_COBID_SYNC 

Sync message cob-id.

+
OD_H1006_COMM_CYCL_PERIOD 

Communication cycle period.

+
OD_H1007_SYNC_WINDOW_LEN 

Sync windows length.

+
OD_H1008_MANUF_DEV_NAME 

Manufacturer device name.

+
OD_H1009_MANUF_HW_VERSION 

Manufacturer hardware version.

+
OD_H100A_MANUF_SW_VERSION 

Manufacturer software version.

+
OD_H100B_RSV 

Reserved.

+
OD_H100C_GUARD_TIME 

Guard time.

+
OD_H100D_LIFETIME_FACTOR 

Life time factor.

+
OD_H100E_RSV 

Reserved.

+
OD_H100F_RSV 

Reserved.

+
OD_H1010_STORE_PARAM_FUNC 

Store params in persistent mem.

+
OD_H1011_REST_PARAM_FUNC 

Restore default parameters.

+
OD_H1012_COBID_TIME 

Timestamp message cob-id.

+
OD_H1013_HIGH_RES_TIMESTAMP 

High resolution timestamp.

+
OD_H1014_COBID_EMERGENCY 

Emergency message cob-id.

+
OD_H1015_INHIBIT_TIME_EMCY 

Inhibit time emergency message.

+
OD_H1016_CONSUMER_HB_TIME 

Consumer heartbeat time.

+
OD_H1017_PRODUCER_HB_TIME 

Producer heartbeat time.

+
OD_H1018_IDENTITY_OBJECT 

Identity object.

+
OD_H1019_SYNC_CNT_OVERFLOW 

Sync counter overflow value.

+
OD_H1020_VERIFY_CONFIG 

Verify configuration.

+
OD_H1021_STORE_EDS 

Store EDS.

+
OD_H1022_STORE_FORMAT 

Store format.

+
OD_H1023_OS_CMD 

OS command.

+
OD_H1024_OS_CMD_MODE 

OS command mode.

+
OD_H1025_OS_DBG_INTERFACE 

OS debug interface.

+
OD_H1026_OS_PROMPT 

OS prompt.

+
OD_H1027_MODULE_LIST 

Module list.

+
OD_H1028_EMCY_CONSUMER 

Emergency consumer object.

+
OD_H1029_ERR_BEHAVIOR 

Error behaviour.

+
OD_H1200_SDO_SERVER_1_PARAM 

SDO server parameter.

+
OD_H1280_SDO_CLIENT_1_PARAM 

SDO client parameter.

+
OD_H1300_GFC_PARAM 

Global fail-safe command param.

+
OD_H1301_SRDO_1_PARAM 

SRDO communication parameter.

+
OD_H1381_SRDO_1_MAPPING 

SRDO mapping parameter.

+
OD_H13FE_SRDO_VALID 

SRDO Configuration valid.

+
OD_H13FF_SRDO_CHECKSUM 

SRDO configuration checksum.

+
OD_H1400_RXPDO_1_PARAM 

RXPDO communication parameter.

+
OD_H1600_RXPDO_1_MAPPING 

RXPDO mapping parameters.

+
OD_H1800_TXPDO_1_PARAM 

TXPDO communication parameter.

+
OD_H1A00_TXPDO_1_MAPPING 

TXPDO mapping parameters.

+
+ +
+
+ +

◆ OD_attributes_t

+ +
+
+ + + + +
enum OD_attributes_t
+
+ +

Attributes (bit masks) for OD sub-object.

+ + + + + + + + + + + + +
Enumerator
ODA_SDO_R 

SDO server may read from the variable.

+
ODA_SDO_W 

SDO server may write to the variable.

+
ODA_SDO_RW 

SDO server may read from or write to the variable.

+
ODA_TPDO 

Variable is mappable into TPDO (can be read)

+
ODA_RPDO 

Variable is mappable into RPDO (can be written)

+
ODA_TRPDO 

Variable is mappable into TPDO or RPDO.

+
ODA_TSRDO 

Variable is mappable into transmitting SRDO.

+
ODA_RSRDO 

Variable is mappable into receiving SRDO.

+
ODA_TRSRDO 

Variable is mappable into tx or rx SRDO.

+
ODA_MB 

Variable is multi-byte ((u)int16_t to (u)int64_t)

+
ODA_STR 

Shorter value, than specified variable size, may be written to the variable.

+

SDO write will fill remaining memory with zeroes. Attribute is used for VISIBLE_STRING and UNICODE_STRING.

+
+ +
+
+ +

◆ ODR_t

+ +
+
+ + + + +
enum ODR_t
+
+ +

Return codes from OD access functions.

+

OD_getSDOabCode() can be used to retrieve corresponding SDO abort code.

+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Enumerator
ODR_PARTIAL 

Read/write is only partial, make more calls.

+
ODR_OK 

SDO abort 0x00000000 - Read/write successfully finished.

+
ODR_OUT_OF_MEM 

SDO abort 0x05040005 - Out of memory.

+
ODR_UNSUPP_ACCESS 

SDO abort 0x06010000 - Unsupported access to an object.

+
ODR_WRITEONLY 

SDO abort 0x06010001 - Attempt to read a write only object.

+
ODR_READONLY 

SDO abort 0x06010002 - Attempt to write a read only object.

+
ODR_IDX_NOT_EXIST 

SDO abort 0x06020000 - Object does not exist in the object dict.

+
ODR_NO_MAP 

SDO abort 0x06040041 - Object cannot be mapped to the PDO.

+
ODR_MAP_LEN 

SDO abort 0x06040042 - PDO length exceeded.

+
ODR_PAR_INCOMPAT 

SDO abort 0x06040043 - General parameter incompatibility reasons.

+
ODR_DEV_INCOMPAT 

SDO abort 0x06040047 - General internal incompatibility in device.

+
ODR_HW 

SDO abort 0x06060000 - Access failed due to hardware error.

+
ODR_TYPE_MISMATCH 

SDO abort 0x06070010 - Data type does not match.

+
ODR_DATA_LONG 

SDO abort 0x06070012 - Data type does not match, length too high.

+
ODR_DATA_SHORT 

SDO abort 0x06070013 - Data type does not match, length too short.

+
ODR_SUB_NOT_EXIST 

SDO abort 0x06090011 - Sub index does not exist.

+
ODR_INVALID_VALUE 

SDO abort 0x06090030 - Invalid value for parameter (download only)

+
ODR_VALUE_HIGH 

SDO abort 0x06090031 - Value range of parameter written too high.

+
ODR_VALUE_LOW 

SDO abort 0x06090032 - Value range of parameter written too low.

+
ODR_MAX_LESS_MIN 

SDO abort 0x06090036 - Maximum value is less than minimum value.

+
ODR_NO_RESOURCE 

SDO abort 0x060A0023 - Resource not available: SDO connection.

+
ODR_GENERAL 

SDO abort 0x08000000 - General error.

+
ODR_DATA_TRANSF 

SDO abort 0x08000020 - Data cannot be transferred or stored to app.

+
ODR_DATA_LOC_CTRL 

SDO abort 0x08000021 - Data can't be transferred (local control)

+
ODR_DATA_DEV_STATE 

SDO abort 0x08000022 - Data can't be transf.

+

(present device state)

+
ODR_OD_MISSING 

SDO abort 0x08000023 - Object dictionary not present.

+
ODR_NO_DATA 

SDO abort 0x08000024 - No data available.

+
ODR_COUNT 

Last element, number of responses.

+
+ +
+
+

Function Documentation

+ +

◆ OD_readOriginal()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
OD_size_t OD_readOriginal (OD_stream_t * stream,
uint8_t subIndex,
void * buf,
OD_size_t count,
ODR_t * returnCode 
)
+
+ +

Read value from original OD location.

+

This function can be used inside read / write functions, specified by OD_extensionIO_init(). It reads data directly from memory location specified by Object dictionary. If no IO extension is used on OD entry, then io->read returned by OD_getSub() equals to this function. See also OD_IO_t.

+ +
+
+ +

◆ OD_writeOriginal()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
OD_size_t OD_writeOriginal (OD_stream_t * stream,
uint8_t subIndex,
const void * buf,
OD_size_t count,
ODR_t * returnCode 
)
+
+ +

Write value to original OD location.

+

This function can be used inside read / write functions, specified by OD_extensionIO_init(). It writes data directly to memory location specified by Object dictionary. If no IO extension is used on OD entry, then io->write returned by OD_getSub() equals to this function. See also OD_IO_t.

+ +
+
+ +

◆ OD_find()

+ +
+
+ + + + + + + + + + + + + + + + + + +
const OD_entry_t* OD_find (const OD_t * od,
uint16_t index 
)
+
+ +

Find OD entry in Object Dictionary.

+
Parameters
+ + + +
odObject Dictionary
indexCANopen Object Dictionary index of object in Object Dictionary
+
+
+
Returns
Pointer to OD entry or NULL if not found
+ +
+
+ +

◆ OD_getSub()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
ODR_t OD_getSub (const OD_entry_t * entry,
uint8_t subIndex,
OD_subEntry_t * subEntry,
OD_IO_t * io,
bool_t odOrig 
)
+
+ +

Find sub-object with specified sub-index on OD entry returned by OD_find.

+

Function populates subEntry and io structures with sub-object data.

+
Parameters
+ + + + + + +
entryOD entry returned by OD_find().
subIndexSub-index of the variable from the OD object.
[out]subEntryStructure will be populated on success, may be NULL.
[out]ioStructure will be populated on success.
odOrigIf true, then potential IO extension on entry will be ignored and access to data entry in the original OD location will be returned
+
+
+
Returns
Value from ODR_t, "ODR_OK" in case of success.
+ +
+
+ +

◆ OD_getIndex()

+ +
+
+ + + + + +
+ + + + + + + + +
static uint16_t OD_getIndex (const OD_entry_t * entry)
+ 		+inlinestatic
+
+ +

Return index from OD entry.

+
Parameters
+ + +
entryOD entry returned by OD_find().
+
+
+
Returns
OD index
+ +
+
+ +

◆ OD_rwRestart()

+ +
+
+ + + + + +
+ + + + + + + + +
static void OD_rwRestart (OD_stream_t * stream)
+ 		+inlinestatic
+
+ +

Restart read or write operation on OD variable.

+

It is not necessary to call this function, if stream was initialised by OD_getSub(). It is also not necessary to call this function, if prevous read or write was successfully finished.

+
Parameters
+ + +
streamObject Dictionary stream object.
+
+
+ +
+
+ +

◆ OD_getSDOabCode()

+ +
+
+ + + + + + + + +
uint32_t OD_getSDOabCode (ODR_t returnCode)
+
+ +

Get SDO abort code from returnCode.

+
Parameters
+ + +
returnCodeReturned from some OD access functions
+
+
+
Returns
Corresponding CO_SDO_abortCode_t
+ +
+
+ +

◆ OD_extensionIO_init()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
ODR_t OD_extensionIO_init (const OD_entry_t * entry,
void * object,
OD_size_t(*)(OD_stream_t *stream, uint8_t subIndex, void *buf, OD_size_t count, ODR_t *returnCode) read,
OD_size_t(*)(OD_stream_t *stream, uint8_t subIndex, const void *buf, OD_size_t count, ODR_t *returnCode) write 
)
+
+ +

Initialise extended OD object with own read/write functions.

+

This function works on OD object, which has IO extension enabled. It gives application very powerful tool: definition of own IO access on own OD object. Structure and attributes are the same as defined in original OD object, but data are read directly from (or written directly to) application specified object via custom function calls.

+

If this function is not called yet, then normal access ("odOrig" argument is false) to OD entry is disabled.

+
Warning
Object dictionary storage works only directly on OD variables. It does not access read function specified here. So, if extended OD objects needs to be preserved, then OD_writeOriginal can be used inside custom write function.
+
+Read and write functions may be called from different threads, so critical sections in custom functions must be protected with CO_LOCK_OD() and CO_UNLOCK_OD().
+
Parameters
+ + + + + +
entryOD entry returned by OD_find().
objectObject, which will be passed to read or write function.
readRead function pointer. If NULL, then read will be disabled. OD_readOriginal can be used here to keep original read function. For function description see OD_IO_t.
writeWrite function pointer. If NULL, then write will be disabled. OD_writeOriginal can be used here to keep original write function. For function description see OD_IO_t.
+
+
+
Returns
"ODR_OK" on success, "ODR_IDX_NOT_EXIST" if OD object doesn't exist, "ODR_PAR_INCOMPAT" if OD object is not extended.
+ +
+
+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__ODinterface.js b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__ODinterface.js new file mode 100755 index 0000000..ad7d624 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__ODinterface.js @@ -0,0 +1,137 @@ +var group__CO__ODinterface = +[ + [ "Getters and setters", "group__CO__ODgetSetters.html", "group__CO__ODgetSetters" ], + [ "OD definition objects", "group__CO__ODdefinition.html", "group__CO__ODdefinition" ], + [ "CO_ODinterface.h", "CO__ODinterface_8h.html", null ], + [ "OD_subEntry_t", "structOD__subEntry__t.html", [ + [ "index", "structOD__subEntry__t.html#a4e10db7bdf91d721ecc7d97f4dda67ff", null ], + [ "subIndex", "structOD__subEntry__t.html#a2c39cbf86fc3b384b017ed6261d379bd", null ], + [ "subEntriesCount", "structOD__subEntry__t.html#a49750c447be0b3773e8aa1814d5fa2de", null ], + [ "attribute", "structOD__subEntry__t.html#ae7d83df4e106219f32cb28d7c510b9d2", null ], + [ "flagsPDO", "structOD__subEntry__t.html#ad6a4eb8da2f84f7b84164eac01115dd1", null ] + ] ], + [ "OD_stream_t", "structOD__stream__t.html", [ + [ "dataObjectOriginal", "structOD__stream__t.html#aad2551a7bf0da6396e6b909adf487b01", null ], + [ "object", "structOD__stream__t.html#af3c8ded429eefa8646317207c0b3ff97", null ], + [ "dataLength", "structOD__stream__t.html#a60c4499678a5db84a7f7285b934ce75a", null ], + [ "dataOffset", "structOD__stream__t.html#a97799b896ee689504771a9274575bcdc", null ] + ] ], + [ "OD_IO_t", "structOD__IO__t.html", [ + [ "stream", "structOD__IO__t.html#a7f31cae6c859c2a2eaf2610b65fd6626", null ], + [ "read", "structOD__IO__t.html#ab0ed3186d15d20f80f877a5f087fdebc", null ], + [ "write", "structOD__IO__t.html#aa296d8e76d99af5c395971602a453b78", null ] + ] ], + [ "OD_entry_t", "structOD__entry__t.html", [ + [ "index", "structOD__entry__t.html#ac27d9d9ac18705e84d64f5226a6e352c", null ], + [ "subEntriesCount", "structOD__entry__t.html#a2f8698a482c2994602a24d39d924acc6", null ], + [ "odObjectType", "structOD__entry__t.html#a86a662107a24527872d92cdb13cad29b", null ], + [ "odObject", "structOD__entry__t.html#a8667e5896b5b001270103e59c92bb181", null ] + ] ], + [ "OD_t", "structOD__t.html", [ + [ "size", "structOD__t.html#ab52b946b5f0127fe618c6f5bbe65a698", null ], + [ "list", "structOD__t.html#a935673d88e82c7a72be76766467d74b4", null ] + ] ], + [ "OD_size_t", "group__CO__ODinterface.html#gaef984c993ddbf6a0500391e97f05d08e", null ], + [ "OD_flagsPDO_t", "group__CO__ODinterface.html#ga69f6e1121545e5669098f49e95ce4e47", null ], + [ "OD_attr_t", "group__CO__ODinterface.html#ga8d459f95307815637e41edc4df71a725", null ], + [ "OD_ObjDicId_30x_t", "group__CO__ODinterface.html#gade8960f241ee3b728eac09288a694886", [ + [ "OD_H1000_DEV_TYPE", "group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886af1e65ef6eb730b9302540e0ba44852b1", null ], + [ "OD_H1001_ERR_REG", "group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886af4eb4e0204ae9696f935af5d4fdcff7e", null ], + [ "OD_H1002_MANUF_STATUS_REG", "group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a2cca52f61d70db5ca2aaa168b32f3aaf", null ], + [ "OD_H1003_PREDEF_ERR_FIELD", "group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886adfc213af5af80cf037231621132013fb", null ], + [ "OD_H1004_RSV", "group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886ad7d53fa95504566811bdf0683f645ccd", null ], + [ "OD_H1005_COBID_SYNC", "group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886adcbc9ec0c547b00db2b0403708becb97", null ], + [ "OD_H1006_COMM_CYCL_PERIOD", "group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a463dab19c27811dd6de51fcc082b565b", null ], + [ "OD_H1007_SYNC_WINDOW_LEN", "group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a41f7f0f96dbea4fb0d6bd2bbbd2d59dc", null ], + [ "OD_H1008_MANUF_DEV_NAME", "group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886aa282ef78e8a64ff527c79218d23168f0", null ], + [ "OD_H1009_MANUF_HW_VERSION", "group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886ad91534dd5cc5f382287b9a392c744948", null ], + [ "OD_H100A_MANUF_SW_VERSION", "group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a8bd48d9bb18d1c291249050818c82a57", null ], + [ "OD_H100B_RSV", "group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a561260a506655cf0b7df9d684a08b5be", null ], + [ "OD_H100C_GUARD_TIME", "group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886adf00d5b448274a91940cac15b8e22fc5", null ], + [ "OD_H100D_LIFETIME_FACTOR", "group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a4bfbb36a82606125d52fbe4daff6b5fb", null ], + [ "OD_H100E_RSV", "group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a4d831b2a36d679d31982e35ca38f8f6e", null ], + [ "OD_H100F_RSV", "group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a7d6a3f1ca8f72bf808ee5fe341f2acca", null ], + [ "OD_H1010_STORE_PARAM_FUNC", "group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a0f8c3db5a62d5e4df59d83253b69b0f2", null ], + [ "OD_H1011_REST_PARAM_FUNC", "group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886adad8c0ea18f674f3eb61b43e8259395c", null ], + [ "OD_H1012_COBID_TIME", "group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a5781f519d9ec08fd4389c4761754a4e6", null ], + [ "OD_H1013_HIGH_RES_TIMESTAMP", "group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a7aa32b2b6df7c4d4354599ef2fd2ca29", null ], + [ "OD_H1014_COBID_EMERGENCY", "group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a5cae036f3dd0bc1861dcea7c9a83c6d5", null ], + [ "OD_H1015_INHIBIT_TIME_EMCY", "group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886adbcd0d36ab781fc60b05a94288b8f019", null ], + [ "OD_H1016_CONSUMER_HB_TIME", "group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a996952cb963ce6e2783a6fa915d85612", null ], + [ "OD_H1017_PRODUCER_HB_TIME", "group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886ab2f428825c1127b286f5b8ace5e881b2", null ], + [ "OD_H1018_IDENTITY_OBJECT", "group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886aabb7a688852e453c5535f663be6298d2", null ], + [ "OD_H1019_SYNC_CNT_OVERFLOW", "group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886aa966d2e020222331b18c5b08261acbf0", null ], + [ "OD_H1020_VERIFY_CONFIG", "group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a8c740bbdd0cb98200d402ec6272d7e8b", null ], + [ "OD_H1021_STORE_EDS", "group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a259737f7e9ef239d85cb9e7bdeda550b", null ], + [ "OD_H1022_STORE_FORMAT", "group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a4061d54d1c1583fd178566a3915bcefe", null ], + [ "OD_H1023_OS_CMD", "group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a12ba9d8cdfc20b9ff66167a8d1e5b21c", null ], + [ "OD_H1024_OS_CMD_MODE", "group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886ad7e64256615fcda5b531063eeaa346de", null ], + [ "OD_H1025_OS_DBG_INTERFACE", "group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886aaf890d86468408b0dbe8353a3b270156", null ], + [ "OD_H1026_OS_PROMPT", "group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a6e3b80d148d22f129ed388fad9aaf398", null ], + [ "OD_H1027_MODULE_LIST", "group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a668cdf3b0102b753858b9bfeb7efdc1c", null ], + [ "OD_H1028_EMCY_CONSUMER", "group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a6f093f4fdeaac7b723305fd8d2ce40c1", null ], + [ "OD_H1029_ERR_BEHAVIOR", "group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a67d50722cc629ee8c2a90a123ee41fa3", null ], + [ "OD_H1200_SDO_SERVER_1_PARAM", "group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a7e351a37cb53541b510e97711e167450", null ], + [ "OD_H1280_SDO_CLIENT_1_PARAM", "group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a3e0e0989da485b0511d7e7cadcdb6dbb", null ], + [ "OD_H1300_GFC_PARAM", "group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886afa584e69f73b7a80cd708010fc0ae64f", null ], + [ "OD_H1301_SRDO_1_PARAM", "group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886afbb7d48d240c7584d461712685e62945", null ], + [ "OD_H1381_SRDO_1_MAPPING", "group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a8effa6834289b482e5ac3319ccf2c17b", null ], + [ "OD_H13FE_SRDO_VALID", "group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886aa66d7204dd265d19276113ef7177ce68", null ], + [ "OD_H13FF_SRDO_CHECKSUM", "group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a92c6bed78bc6bbbd182c16fe8890dcbf", null ], + [ "OD_H1400_RXPDO_1_PARAM", "group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886ae64bf5b7166b6adf46b8e965d43150a0", null ], + [ "OD_H1600_RXPDO_1_MAPPING", "group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a7ed53d283e4719920b233b9094b18f9c", null ], + [ "OD_H1800_TXPDO_1_PARAM", "group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886aac35e48e4b717eef309ebb57876d30f1", null ], + [ "OD_H1A00_TXPDO_1_MAPPING", "group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a2a2f1c4cc58d29ccf43e105afd57bc14", null ] + ] ], + [ "OD_attributes_t", "group__CO__ODinterface.html#ga47b0d204aaf1ea64b4f826aaf8f5c151", [ + [ "ODA_SDO_R", "group__CO__ODinterface.html#gga47b0d204aaf1ea64b4f826aaf8f5c151ada3eb985961ffdd9cf655d1a7a7d9485", null ], + [ "ODA_SDO_W", "group__CO__ODinterface.html#gga47b0d204aaf1ea64b4f826aaf8f5c151a9245e7a557f32ab863aef41412df9eb5", null ], + [ "ODA_SDO_RW", "group__CO__ODinterface.html#gga47b0d204aaf1ea64b4f826aaf8f5c151a2c60ba85cbe4f25d5511ffab3dcd7486", null ], + [ "ODA_TPDO", "group__CO__ODinterface.html#gga47b0d204aaf1ea64b4f826aaf8f5c151ad106aacb6b181ab7dac0f6dbc8c50321", null ], + [ "ODA_RPDO", "group__CO__ODinterface.html#gga47b0d204aaf1ea64b4f826aaf8f5c151ae4930aa0efbc2249563613b5107bb107", null ], + [ "ODA_TRPDO", "group__CO__ODinterface.html#gga47b0d204aaf1ea64b4f826aaf8f5c151af750832681daa8d5e44ed8908c4ec552", null ], + [ "ODA_TSRDO", "group__CO__ODinterface.html#gga47b0d204aaf1ea64b4f826aaf8f5c151a3a1b8c2ed54565e89d6e6d3a043bdcfe", null ], + [ "ODA_RSRDO", "group__CO__ODinterface.html#gga47b0d204aaf1ea64b4f826aaf8f5c151a6c6261d5bea91588b2851f5c11faae02", null ], + [ "ODA_TRSRDO", "group__CO__ODinterface.html#gga47b0d204aaf1ea64b4f826aaf8f5c151af887f38d83f35c28d478f1a4b08d1be9", null ], + [ "ODA_MB", "group__CO__ODinterface.html#gga47b0d204aaf1ea64b4f826aaf8f5c151ae02b665e7e8d8bd84f341c9ad040d367", null ], + [ "ODA_STR", "group__CO__ODinterface.html#gga47b0d204aaf1ea64b4f826aaf8f5c151a9527d9c21ba4159d653771e1dedad81d", null ] + ] ], + [ "ODR_t", "group__CO__ODinterface.html#ga0e9afd8ad27de0920d1fe0738834869c", [ + [ "ODR_PARTIAL", "group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869caf7595749473065bfc81cfa6709370fee", null ], + [ "ODR_OK", "group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869ca15f7f20e27f1c5f174bdeecfeef45cc2", null ], + [ "ODR_OUT_OF_MEM", "group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869ca118a63a81ef2fd802c925bf4c79975fa", null ], + [ "ODR_UNSUPP_ACCESS", "group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869ca17d694adc9471112cbb2740f7f45a2d0", null ], + [ "ODR_WRITEONLY", "group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869ca44ab94bfc7547122b96498c781291df6", null ], + [ "ODR_READONLY", "group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869caa119384effe499c9bffb874219c6433a", null ], + [ "ODR_IDX_NOT_EXIST", "group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869ca64ea80d1baebf136382d53d5580fbc85", null ], + [ "ODR_NO_MAP", "group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869ca853242f74446c58773099bdef9835a94", null ], + [ "ODR_MAP_LEN", "group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869cad6c92203fa86ee8ff7d7271bf81e7d9e", null ], + [ "ODR_PAR_INCOMPAT", "group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869ca36edb0ad1c8c5c0d804bb88274bfe165", null ], + [ "ODR_DEV_INCOMPAT", "group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869ca3d259ef030c2a10afecb253b532a0323", null ], + [ "ODR_HW", "group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869caeaac26c680e626185429468dda9c2433", null ], + [ "ODR_TYPE_MISMATCH", "group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869cad197c1462f472a21be2e3ed5c5880aa4", null ], + [ "ODR_DATA_LONG", "group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869ca3f93f54b7f130bb7b266b2d36c24caec", null ], + [ "ODR_DATA_SHORT", "group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869cae0b103160b23ff40047fcf85225121d2", null ], + [ "ODR_SUB_NOT_EXIST", "group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869ca4f76fa87ea446616ff2f6195e7bec67c", null ], + [ "ODR_INVALID_VALUE", "group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869ca0646f592124a9b4d2835c9c0296c6a0c", null ], + [ "ODR_VALUE_HIGH", "group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869ca95291309267a732c380e13ad6c17a986", null ], + [ "ODR_VALUE_LOW", "group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869ca4db02b8575a8a10786959a5472f1c0f4", null ], + [ "ODR_MAX_LESS_MIN", "group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869cac9c8eda20bfc7dfffe17f170c377d646", null ], + [ "ODR_NO_RESOURCE", "group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869ca15fe8d8b791b90373e59bc5bc5d3f8c8", null ], + [ "ODR_GENERAL", "group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869ca968d2a28cb866cacf7ef8b8cd0b76e2c", null ], + [ "ODR_DATA_TRANSF", "group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869ca341104b9ac43168d0c668586b8f750bb", null ], + [ "ODR_DATA_LOC_CTRL", "group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869ca28b28d8f091eefb7e8fac92bbdae82bb", null ], + [ "ODR_DATA_DEV_STATE", "group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869ca16bfe0c712aaea3841ae2b250331b276", null ], + [ "ODR_OD_MISSING", "group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869caeae1e00129ff22708ffdcb2c8b3f083b", null ], + [ "ODR_NO_DATA", "group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869caff503d4f6cc55429913680d071ca3c4d", null ], + [ "ODR_COUNT", "group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869cac43414be729ea2b701380c4400658c37", null ] + ] ], + [ "OD_readOriginal", "group__CO__ODinterface.html#gadf9ac60f94e1f9fc21b7f10a0d254503", null ], + [ "OD_writeOriginal", "group__CO__ODinterface.html#ga648f0b0bfabde2d377149bf84e937422", null ], + [ "OD_find", "group__CO__ODinterface.html#gaaacaadfc28bfaf485cefc8bff64310f4", null ], + [ "OD_getSub", "group__CO__ODinterface.html#gaf1f736d4b4d6754d971f0c0a63655bcf", null ], + [ "OD_getIndex", "group__CO__ODinterface.html#gac84e7390f50e7e5c5e8ba42714e51aaf", null ], + [ "OD_rwRestart", "group__CO__ODinterface.html#ga3715e0a6b15bdf45659e1e01f9fc4e65", null ], + [ "OD_getSDOabCode", "group__CO__ODinterface.html#ga7c24b06bb9365d41b8e60acb4eaecc6c", null ], + [ "OD_extensionIO_init", "group__CO__ODinterface.html#gac07bbe54fbfecc6bc8da2e10b2c0f7e8", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__PDO.html b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__PDO.html new file mode 100755 index 0000000..a013c5e --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__PDO.html @@ -0,0 +1,626 @@ + + + + + + + +CANopenNode: PDO + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Files | +Data Structures | +Functions
+
+
PDO
CANopen_301
+
+
+ + + + + +

+Files

file  CO_PDO.h
 CANopen Process Data Object protocol.
 
+ + + + + + + + + + + + + + + + + + + +

+Data Structures

struct  CO_RPDOCommPar_t
 RPDO communication parameter. More...
 
struct  CO_RPDOMapPar_t
 RPDO mapping parameter. More...
 
struct  CO_TPDOCommPar_t
 TPDO communication parameter. More...
 
struct  CO_TPDOMapPar_t
 TPDO mapping parameter. More...
 
struct  CO_RPDO_t
 RPDO object. More...
 
struct  CO_TPDO_t
 TPDO object. More...
 
+ + + + + + + + + + + + + + + + + + + + + + +

+Functions

CO_ReturnError_t CO_RPDO_init (CO_RPDO_t *RPDO, CO_EM_t *em, CO_SDO_t *SDO, CO_SYNC_t *SYNC, CO_NMT_internalState_t *operatingState, uint8_t nodeId, uint16_t defaultCOB_ID, uint8_t restrictionFlags, const CO_RPDOCommPar_t *RPDOCommPar, const CO_RPDOMapPar_t *RPDOMapPar, uint16_t idx_RPDOCommPar, uint16_t idx_RPDOMapPar, CO_CANmodule_t *CANdevRx, uint16_t CANdevRxIdx)
 Initialize RPDO object. More...
 
void CO_RPDO_initCallbackPre (CO_RPDO_t *RPDO, void *object, void(*pFunctSignalPre)(void *object))
 Initialize RPDO callback function. More...
 
CO_ReturnError_t CO_TPDO_init (CO_TPDO_t *TPDO, CO_EM_t *em, CO_SDO_t *SDO, CO_SYNC_t *SYNC, CO_NMT_internalState_t *operatingState, uint8_t nodeId, uint16_t defaultCOB_ID, uint8_t restrictionFlags, const CO_TPDOCommPar_t *TPDOCommPar, const CO_TPDOMapPar_t *TPDOMapPar, uint16_t idx_TPDOCommPar, uint16_t idx_TPDOMapPar, CO_CANmodule_t *CANdevTx, uint16_t CANdevTxIdx)
 Initialize TPDO object. More...
 
uint8_t CO_TPDOisCOS (CO_TPDO_t *TPDO)
 Verify Change of State of the PDO. More...
 
CO_ReturnError_t CO_TPDOsend (CO_TPDO_t *TPDO)
 Send TPDO message. More...
 
void CO_RPDO_process (CO_RPDO_t *RPDO, bool_t syncWas)
 Process received PDO messages. More...
 
void CO_TPDO_process (CO_TPDO_t *TPDO, bool_t syncWas, uint32_t timeDifference_us, uint32_t *timerNext_us)
 Process transmitting PDO messages. More...
 
+

Detailed Description

+

CANopen Process Data Object protocol.

+

Process data objects are used for real-time data transfer with no protocol overhead.

+

TPDO with specific identifier is transmitted by one device and recieved by zero or more devices as RPDO. PDO communication parameters(COB-ID, transmission type, etc.) are in Object Dictionary at index 0x1400+ and 0x1800+. PDO mapping parameters (size and contents of the PDO) are in Object Dictionary at index 0x1600+ and 0x1A00+.

+

Features of the PDO as implemented here, in CANopenNode:

    +
  • Dynamic PDO mapping.
    • +
  • Map granularity of one byte.
    • +
  • After RPDO is received from CAN bus, its data are copied to buffer. Function CO_RPDO_process() (called by application) copies data to mapped objects in Object Dictionary. Synchronous RPDOs are processed AFTER reception of the next SYNC message.
    • +
  • Function CO_TPDO_process() (called by application) sends TPDO if necessary. There are possible different transmission types, including automatic detection of Change of State of specific variable.
    • +
+

Function Documentation

+ +

◆ CO_RPDO_init()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
CO_ReturnError_t CO_RPDO_init (CO_RPDO_t * RPDO,
CO_EM_t * em,
CO_SDO_t * SDO,
CO_SYNC_t * SYNC,
CO_NMT_internalState_t * operatingState,
uint8_t nodeId,
uint16_t defaultCOB_ID,
uint8_t restrictionFlags,
const CO_RPDOCommPar_t * RPDOCommPar,
const CO_RPDOMapPar_t * RPDOMapPar,
uint16_t idx_RPDOCommPar,
uint16_t idx_RPDOMapPar,
CO_CANmodule_t * CANdevRx,
uint16_t CANdevRxIdx 
)
+
+ +

Initialize RPDO object.

+

Function must be called in the communication reset section.

+
Parameters
+ + + + + + + + + + + + + + + +
RPDOThis object will be initialized.
emEmergency object.
SDOSDO server object.
SYNCvoid pointer to SYNC object or NULL.
operatingStatePointer to variable indicating CANopen device NMT internal state.
nodeIdCANopen Node ID of this device. If default COB_ID is used, value will be added.
defaultCOB_IDDefault COB ID for this PDO (without NodeId). See CO_Default_CAN_ID_t
restrictionFlagsFlag bits indicates, how PDO communication and mapping parameters are handled:
    +
  • Bit1: If true, communication parameters are writeable only in pre-operational NMT state.
    • +
  • Bit2: If true, mapping parameters are writeable only in pre-operational NMT state.
    • +
  • Bit3: If true, communication parameters are read-only.
    • +
  • Bit4: If true, mapping parameters are read-only.
    • +
+
RPDOCommParPointer to RPDO communication parameter record from Object dictionary (index 0x1400+).
RPDOMapParPointer to RPDO mapping parameter record from Object dictionary (index 0x1600+).
idx_RPDOCommParIndex in Object Dictionary.
idx_RPDOMapParIndex in Object Dictionary.
CANdevRxCAN device for PDO reception.
CANdevRxIdxIndex of receive buffer in the above CAN device.
+
+
+
Returns
CO_ReturnError_t: CO_ERROR_NO or CO_ERROR_ILLEGAL_ARGUMENT.
+ +
+
+ +

◆ CO_RPDO_initCallbackPre()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + +
void CO_RPDO_initCallbackPre (CO_RPDO_t * RPDO,
void * object,
void(*)(void *object) pFunctSignalPre 
)
+
+ +

Initialize RPDO callback function.

+

Function initializes optional callback function, which should immediately start processing of CO_RPDO_process() function. Callback is called after RPDO message is received from the CAN bus.

+
Parameters
+ + + + +
RPDOThis object.
objectPointer to object, which will be passed to pFunctSignalPre(). Can be NULL
pFunctSignalPrePointer to the callback function. Not called if NULL.
+
+
+ +
+
+ +

◆ CO_TPDO_init()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
CO_ReturnError_t CO_TPDO_init (CO_TPDO_t * TPDO,
CO_EM_t * em,
CO_SDO_t * SDO,
CO_SYNC_t * SYNC,
CO_NMT_internalState_t * operatingState,
uint8_t nodeId,
uint16_t defaultCOB_ID,
uint8_t restrictionFlags,
const CO_TPDOCommPar_t * TPDOCommPar,
const CO_TPDOMapPar_t * TPDOMapPar,
uint16_t idx_TPDOCommPar,
uint16_t idx_TPDOMapPar,
CO_CANmodule_t * CANdevTx,
uint16_t CANdevTxIdx 
)
+
+ +

Initialize TPDO object.

+

Function must be called in the communication reset section.

+
Parameters
+ + + + + + + + + + + + + + + +
TPDOThis object will be initialized.
emEmergency object.
SDOSDO object.
SYNCvoid pointer to SYNC object or NULL.
operatingStatePointer to variable indicating CANopen device NMT internal state.
nodeIdCANopen Node ID of this device. If default COB_ID is used, value will be added.
defaultCOB_IDDefault COB ID for this PDO (without NodeId). See CO_Default_CAN_ID_t
restrictionFlagsFlag bits indicates, how PDO communication and mapping parameters are handled:
    +
  • Bit1: If true, communication parameters are writeable only in pre-operational NMT state.
    • +
  • Bit2: If true, mapping parameters are writeable only in pre-operational NMT state.
    • +
  • Bit3: If true, communication parameters are read-only.
    • +
  • Bit4: If true, mapping parameters are read-only.
    • +
+
TPDOCommParPointer to TPDO communication parameter record from Object dictionary (index 0x1400+).
TPDOMapParPointer to TPDO mapping parameter record from Object dictionary (index 0x1600+).
idx_TPDOCommParIndex in Object Dictionary.
idx_TPDOMapParIndex in Object Dictionary.
CANdevTxCAN device used for PDO transmission.
CANdevTxIdxIndex of transmit buffer in the above CAN device.
+
+
+
Returns
CO_ReturnError_t: CO_ERROR_NO or CO_ERROR_ILLEGAL_ARGUMENT.
+ +
+
+ +

◆ CO_TPDOisCOS()

+ +
+
+ + + + + + + + +
uint8_t CO_TPDOisCOS (CO_TPDO_t * TPDO)
+
+ +

Verify Change of State of the PDO.

+

Function verifies if variable mapped to TPDO has changed its value. Verified are only variables, which has set attribute CO_ODA_TPDO_DETECT_COS in #CO_SDO_OD_attributes_t.

+

Function may be called by application just before CO_TPDO_process() function, for example: TPDOx->sendRequest = CO_TPDOisCOS(TPDOx); CO_TPDO_process(TPDOx, ....

+
Parameters
+ + +
TPDOTPDO object.
+
+
+
Returns
True if COS was detected.
+ +
+
+ +

◆ CO_TPDOsend()

+ +
+
+ + + + + + + + +
CO_ReturnError_t CO_TPDOsend (CO_TPDO_t * TPDO)
+
+ +

Send TPDO message.

+

Function prepares TPDO data from Object Dictionary variables. It should not be called by application, it is called from CO_TPDO_process().

+
Parameters
+ + +
TPDOTPDO object.
+
+
+
Returns
Same as CO_CANsend().
+ +
+
+ +

◆ CO_RPDO_process()

+ +
+
+ + + + + + + + + + + + + + + + + + +
void CO_RPDO_process (CO_RPDO_t * RPDO,
bool_t syncWas 
)
+
+ +

Process received PDO messages.

+

Function must be called cyclically in any NMT state. It copies data from RPDO to Object Dictionary variables if: new PDO receives and PDO is valid and NMT operating state is operational. It does not verify transmission type.

+
Parameters
+ + + +
RPDOThis object.
syncWasTrue, if CANopen SYNC message was just received or transmitted.
+
+
+ +
+
+ +

◆ CO_TPDO_process()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
void CO_TPDO_process (CO_TPDO_t * TPDO,
bool_t syncWas,
uint32_t timeDifference_us,
uint32_t * timerNext_us 
)
+
+ +

Process transmitting PDO messages.

+

Function must be called cyclically in any NMT state. It prepares and sends TPDO if necessary. If Change of State needs to be detected, function CO_TPDOisCOS() must be called before.

+
Parameters
+ + + + + +
TPDOThis object.
syncWasTrue, if CANopen SYNC message was just received or transmitted.
timeDifference_usTime difference from previous function call in [microseconds].
[out]timerNext_usinfo to OS - see CO_process_SYNC_PDO().
+
+
+ +
+
+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__PDO.js b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__PDO.js new file mode 100755 index 0000000..3f1cace --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__PDO.js @@ -0,0 +1,90 @@ +var group__CO__PDO = +[ + [ "CO_PDO.h", "CO__PDO_8h.html", null ], + [ "CO_RPDOCommPar_t", "structCO__RPDOCommPar__t.html", [ + [ "maxSubIndex", "structCO__RPDOCommPar__t.html#ac49d1bc96c31ec32015628128a6e60a7", null ], + [ "COB_IDUsedByRPDO", "structCO__RPDOCommPar__t.html#a798b8c59ea8f8b627c307f1246b6ee78", null ], + [ "transmissionType", "structCO__RPDOCommPar__t.html#a09eb4787337cf8579c0ff1d4ae968aba", null ] + ] ], + [ "CO_RPDOMapPar_t", "structCO__RPDOMapPar__t.html", [ + [ "numberOfMappedObjects", "structCO__RPDOMapPar__t.html#a479613deae0d06607897093d617edb1d", null ], + [ "mappedObject1", "structCO__RPDOMapPar__t.html#a0a161a9792479c07aaf12447fbac2499", null ], + [ "mappedObject2", "structCO__RPDOMapPar__t.html#a63f9b87f27d30e6f4faf65a56b458ffb", null ], + [ "mappedObject3", "structCO__RPDOMapPar__t.html#a3ac0b7cd18683c45d3dcd0e18b13810b", null ], + [ "mappedObject4", "structCO__RPDOMapPar__t.html#a0a3ef7ae329b91ec73d05301e870ad75", null ], + [ "mappedObject5", "structCO__RPDOMapPar__t.html#af532c96971699321f04aad14dbee52a4", null ], + [ "mappedObject6", "structCO__RPDOMapPar__t.html#a1082b9de3f132363f78c19004be017fb", null ], + [ "mappedObject7", "structCO__RPDOMapPar__t.html#ad66f07a873d2ceb0b10bdad242925984", null ], + [ "mappedObject8", "structCO__RPDOMapPar__t.html#a2f1467c9ba8a91e4f2742c08ed8551f6", null ] + ] ], + [ "CO_TPDOCommPar_t", "structCO__TPDOCommPar__t.html", [ + [ "maxSubIndex", "structCO__TPDOCommPar__t.html#a74a8681177fabbb55ebf5be843f12fc5", null ], + [ "COB_IDUsedByTPDO", "structCO__TPDOCommPar__t.html#a65ee2e80b1078e84479ab749012f94cc", null ], + [ "transmissionType", "structCO__TPDOCommPar__t.html#a328398227ff1f167649d54453e44df97", null ], + [ "inhibitTime", "structCO__TPDOCommPar__t.html#ad53403c65582d166898546e329ea9587", null ], + [ "compatibilityEntry", "structCO__TPDOCommPar__t.html#acba30527976f508b43b3348846f2e657", null ], + [ "eventTimer", "structCO__TPDOCommPar__t.html#ab01f44570dca08c910c17b14fc664414", null ], + [ "SYNCStartValue", "structCO__TPDOCommPar__t.html#a597bd93f097550c3d869307e733cd198", null ] + ] ], + [ "CO_TPDOMapPar_t", "structCO__TPDOMapPar__t.html", [ + [ "numberOfMappedObjects", "structCO__TPDOMapPar__t.html#a10718cf84dc6a975509d327efb8403de", null ], + [ "mappedObject1", "structCO__TPDOMapPar__t.html#a6869485bc0705188069c37d6b6ee1694", null ], + [ "mappedObject2", "structCO__TPDOMapPar__t.html#a496d610062c09a1667e7541659339eea", null ], + [ "mappedObject3", "structCO__TPDOMapPar__t.html#a8e79bb51d865cae1bab20621db898ddb", null ], + [ "mappedObject4", "structCO__TPDOMapPar__t.html#aa8a046a95b3ac151f5d8d8cb4415851e", null ], + [ "mappedObject5", "structCO__TPDOMapPar__t.html#a0fbdd6c39635c8288771867bdc061d6f", null ], + [ "mappedObject6", "structCO__TPDOMapPar__t.html#ad8f8f0e0629b1c0467599ea8b138e3eb", null ], + [ "mappedObject7", "structCO__TPDOMapPar__t.html#a1fb19d7423d2fd74bc8575aff1657552", null ], + [ "mappedObject8", "structCO__TPDOMapPar__t.html#aa482b092dd475bac21193c671dabdb49", null ] + ] ], + [ "CO_RPDO_t", "structCO__RPDO__t.html", [ + [ "em", "structCO__RPDO__t.html#a5261e898fc67ecba19f0fc146e4a13ef", null ], + [ "SDO", "structCO__RPDO__t.html#abcf8134da148073ec8e3f1dd6f0c8da1", null ], + [ "RPDOCommPar", "structCO__RPDO__t.html#a1aaaf9abb01030dbd732985d07ed8c33", null ], + [ "RPDOMapPar", "structCO__RPDO__t.html#a69ad9068dfcee1d12697430856e62d10", null ], + [ "operatingState", "structCO__RPDO__t.html#a85583dccb8f2d0515288888e56065e70", null ], + [ "nodeId", "structCO__RPDO__t.html#a15e1425101d92521ad219695036b1cd2", null ], + [ "defaultCOB_ID", "structCO__RPDO__t.html#a9e327dba172ebbd3112097e5085eea2f", null ], + [ "restrictionFlags", "structCO__RPDO__t.html#a1759ebaef816a352d37e717c9360458a", null ], + [ "valid", "structCO__RPDO__t.html#a1d9a4be6ad3245309ffe6e3ad5637942", null ], + [ "dataLength", "structCO__RPDO__t.html#af742fd80b982822c3e06770ab0877cc3", null ], + [ "mapPointer", "structCO__RPDO__t.html#a998b83bc1cbf11aa4e9170ce03c9d203", null ], + [ "SYNC", "structCO__RPDO__t.html#a2545c12748b54a1ef3e5660c82e1adf8", null ], + [ "synchronous", "structCO__RPDO__t.html#a08c58c120349e150e4188666a5113916", null ], + [ "CANrxNew", "structCO__RPDO__t.html#a4ce980b6cc3a0e497a0138c9c4a69d41", null ], + [ "CANrxData", "structCO__RPDO__t.html#a514e6d57efc477bdb49cea75e6495a95", null ], + [ "pFunctSignalPre", "structCO__RPDO__t.html#a3c0573cc4e76a601bbbd710f19b9615f", null ], + [ "functSignalObjectPre", "structCO__RPDO__t.html#aff7dd123460cc50708f28d40d851f8dd", null ], + [ "CANdevRx", "structCO__RPDO__t.html#a3f153d5326f07ec7c8f3570287547454", null ], + [ "CANdevRxIdx", "structCO__RPDO__t.html#a000fe56fcaf727c59b7dc049ec7fc4b1", null ] + ] ], + [ "CO_TPDO_t", "structCO__TPDO__t.html", [ + [ "em", "structCO__TPDO__t.html#ac7ce3386549e212300bb85bfc5e88f2e", null ], + [ "SDO", "structCO__TPDO__t.html#a52c1e56f282549b0949721049bcc7e73", null ], + [ "TPDOCommPar", "structCO__TPDO__t.html#ab9bd2bf1a76f1f0e253dd4c4a941ba67", null ], + [ "TPDOMapPar", "structCO__TPDO__t.html#a6f6202c2b866f552c512a3513c27be8b", null ], + [ "operatingState", "structCO__TPDO__t.html#a7ba73f70490869a38ff561aa6c32489f", null ], + [ "nodeId", "structCO__TPDO__t.html#a4ef8ced15f6fffb56a0ec4aeb48d4551", null ], + [ "defaultCOB_ID", "structCO__TPDO__t.html#a95e95dc4668b41de0b8eb8504e83c944", null ], + [ "restrictionFlags", "structCO__TPDO__t.html#aa0d1d4b71933c7bac438d97ca280fe56", null ], + [ "valid", "structCO__TPDO__t.html#a201c8a0726347a747f6b315915c797fb", null ], + [ "dataLength", "structCO__TPDO__t.html#a223e60deb77d4ef8a78da37e4d9cdf85", null ], + [ "sendRequest", "structCO__TPDO__t.html#afc375e06e5931cb8bae7886f2f5f0f7a", null ], + [ "mapPointer", "structCO__TPDO__t.html#acb6fa2c4037b1e41afdf6195cd6e93ec", null ], + [ "inhibitTimer", "structCO__TPDO__t.html#a9277687ef658353801435638c8aa2bee", null ], + [ "eventTimer", "structCO__TPDO__t.html#ae71e875f41d8f14f02e757dbc3b2d0c5", null ], + [ "sendIfCOSFlags", "structCO__TPDO__t.html#a0f5dbb51df08261c466b079c08119a04", null ], + [ "syncCounter", "structCO__TPDO__t.html#a5bf0fa473c5eb92e3b56fc1f17222976", null ], + [ "SYNC", "structCO__TPDO__t.html#ae6c014bdb855d57d4c53bc702734b51f", null ], + [ "CANdevTx", "structCO__TPDO__t.html#a42d9798fca6a122bd06391b7d23ec254", null ], + [ "CANtxBuff", "structCO__TPDO__t.html#a2a2a10d57e1eeab4e0717caa302a6605", null ], + [ "CANdevTxIdx", "structCO__TPDO__t.html#ab401d61f30b73d530df3b8d42015407d", null ] + ] ], + [ "CO_RPDO_init", "group__CO__PDO.html#ga92c484ada2ad240c1b8c891c88d56901", null ], + [ "CO_RPDO_initCallbackPre", "group__CO__PDO.html#ga34532746ccf88ccfa835716e89369478", null ], + [ "CO_TPDO_init", "group__CO__PDO.html#ga8fb100744dc91f84b236c55ee37200a1", null ], + [ "CO_TPDOisCOS", "group__CO__PDO.html#gafec3eb12b93146a3706cbf03d3770a8d", null ], + [ "CO_TPDOsend", "group__CO__PDO.html#ga9b2c8692f74f6a6a389ef88bf9c682a5", null ], + [ "CO_RPDO_process", "group__CO__PDO.html#gad77bfd4c7f64e75e7ddee5c926477e66", null ], + [ "CO_TPDO_process", "group__CO__PDO.html#ga0bb0d1b09d37ca19e01d47d8d0004f6b", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__SDOclient.html b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__SDOclient.html new file mode 100755 index 0000000..17307f4 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__SDOclient.html @@ -0,0 +1,786 @@ + + + + + + + +CANopenNode: SDO client + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Files | +Data Structures | +Functions
+
+
SDO client
CANopen_301
+
+
+ + + + + +

+Files

file  CO_SDOclient.h
 CANopen Service Data Object - client protocol.
 
+ + + + +

+Data Structures

struct  CO_SDOclient_t
 SDO client object. More...
 
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

+Functions

CO_ReturnError_t CO_SDOclient_init (CO_SDOclient_t *SDO_C, const OD_t *OD, const OD_entry_t *OD_1280_SDOcliPar, uint8_t nodeId, CO_CANmodule_t *CANdevRx, uint16_t CANdevRxIdx, CO_CANmodule_t *CANdevTx, uint16_t CANdevTxIdx)
 Initialize SDO client object. More...
 
void CO_SDOclient_initCallbackPre (CO_SDOclient_t *SDOclient, void *object, void(*pFunctSignal)(void *object))
 Initialize SDOclient callback function. More...
 
CO_SDO_return_t CO_SDOclient_setup (CO_SDOclient_t *SDO_C, uint32_t COB_IDClientToServer, uint32_t COB_IDServerToClient, uint8_t nodeIDOfTheSDOServer)
 Setup SDO client object. More...
 
CO_SDO_return_t CO_SDOclientDownloadInitiate (CO_SDOclient_t *SDO_C, uint16_t index, uint8_t subIndex, size_t sizeIndicated, uint16_t SDOtimeoutTime_ms, bool_t blockEnable)
 Initiate SDO download communication. More...
 
void CO_SDOclientDownloadInitiateSize (CO_SDOclient_t *SDO_C, size_t sizeIndicated)
 Initiate SDO download communication - update size. More...
 
size_t CO_SDOclientDownloadBufWrite (CO_SDOclient_t *SDO_C, const char *buf, size_t count)
 Write data into SDO client buffer. More...
 
CO_SDO_return_t CO_SDOclientDownload (CO_SDOclient_t *SDO_C, uint32_t timeDifference_us, bool_t abort, bool_t bufferPartial, CO_SDO_abortCode_t *SDOabortCode, size_t *sizeTransferred, uint32_t *timerNext_us)
 Process SDO download communication. More...
 
CO_SDO_return_t CO_SDOclientUploadInitiate (CO_SDOclient_t *SDO_C, uint16_t index, uint8_t subIndex, uint16_t SDOtimeoutTime_ms, bool_t blockEnable)
 Initiate SDO upload communication. More...
 
CO_SDO_return_t CO_SDOclientUpload (CO_SDOclient_t *SDO_C, uint32_t timeDifference_us, bool_t abort, CO_SDO_abortCode_t *SDOabortCode, size_t *sizeIndicated, size_t *sizeTransferred, uint32_t *timerNext_us)
 Process SDO upload communication. More...
 
size_t CO_SDOclientUploadBufRead (CO_SDOclient_t *SDO_C, char *buf, size_t count)
 Read data from SDO client buffer. More...
 
void CO_SDOclientClose (CO_SDOclient_t *SDO_C)
 Close SDO communication temporary. More...
 
+

Detailed Description

+

CANopen Service Data Object - client protocol (master functionality).

+
See also
SDO server
+

Function Documentation

+ +

◆ CO_SDOclient_init()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
CO_ReturnError_t CO_SDOclient_init (CO_SDOclient_t * SDO_C,
const OD_t * OD,
const OD_entry_t * OD_1280_SDOcliPar,
uint8_t nodeId,
CO_CANmodule_t * CANdevRx,
uint16_t CANdevRxIdx,
CO_CANmodule_t * CANdevTx,
uint16_t CANdevTxIdx 
)
+
+ +

Initialize SDO client object.

+

Function must be called in the communication reset section.

+
Parameters
+ + + + + + + + + +
SDO_CThis object will be initialized.
ODObject Dictionary. It is used in case, if client is accessing object dictionary from its own device. If NULL, it will be ignored.
OD_1280_SDOcliParOD entry for SDO client parameter (0x1280+). It may have IO extension enabled to allow dynamic configuration (see also CO_CONFIG_FLAG_OD_DYNAMIC). Entry is required.
nodeIdCANopen Node ID of this device. It is used in case, if client is accessing object dictionary from its own device. If 0, it will be ignored.
CANdevRxCAN device for SDO client reception.
CANdevRxIdxIndex of receive buffer in the above CAN device.
CANdevTxCAN device for SDO client transmission.
CANdevTxIdxIndex of transmit buffer in the above CAN device.
+
+
+
Returns
CO_ReturnError_t: CO_ERROR_NO or CO_ERROR_ILLEGAL_ARGUMENT.
+ +
+
+ +

◆ CO_SDOclient_initCallbackPre()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + +
void CO_SDOclient_initCallbackPre (CO_SDOclient_t * SDOclient,
void * object,
void(*)(void *object) pFunctSignal 
)
+
+ +

Initialize SDOclient callback function.

+

Function initializes optional callback function, which should immediately start processing of CO_SDOclientDownload() or CO_SDOclientUpload() function. Callback is called after SDOclient message is received from the CAN bus or when new call without delay is necessary (exchange data with own SDO server or SDO block transfer is in progress).

+
Parameters
+ + + + +
SDOclientThis object.
objectPointer to object, which will be passed to pFunctSignal(). Can be NULL.
pFunctSignalPointer to the callback function. Not called if NULL.
+
+
+ +
+
+ +

◆ CO_SDOclient_setup()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
CO_SDO_return_t CO_SDOclient_setup (CO_SDOclient_t * SDO_C,
uint32_t COB_IDClientToServer,
uint32_t COB_IDServerToClient,
uint8_t nodeIDOfTheSDOServer 
)
+
+ +

Setup SDO client object.

+

Function is called in from CO_SDOclient_init() and each time when "SDO client parameter" is written. Application can call this function before new SDO communication. If parameters to this function are the same as before, then CAN is not reconfigured.

+
Parameters
+ + + + + +
SDO_CThis object.
COB_IDClientToServerSee CO_SDOclient_t.
COB_IDServerToClientSee CO_SDOclient_t.
nodeIDOfTheSDOServerNode-ID of the SDO server. If it is the same as node-ID of this node, then data will be exchanged with this node (without CAN communication).
+
+
+
Returns
CO_SDO_return_t, CO_SDO_RT_ok_communicationEnd or CO_SDO_RT_wrongArguments
+ +
+
+ +

◆ CO_SDOclientDownloadInitiate()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
CO_SDO_return_t CO_SDOclientDownloadInitiate (CO_SDOclient_t * SDO_C,
uint16_t index,
uint8_t subIndex,
size_t sizeIndicated,
uint16_t SDOtimeoutTime_ms,
bool_t blockEnable 
)
+
+ +

Initiate SDO download communication.

+

Function initiates SDO download communication with server specified in CO_SDOclient_init() function. Data will be written to remote node. Function is non-blocking.

+
Parameters
+ + + + + + + +
SDO_CThis object.
indexIndex of object in object dictionary in remote node.
subIndexSubindex of object in object dictionary in remote node.
sizeIndicatedOptionally indicate size of data to be downloaded. Actual data are written with one or multiple CO_SDOclientDownloadBufWrite() calls.
    +
  • If sizeIndicated is different than 0, then total number of data written by CO_SDOclientDownloadBufWrite() will be compared against sizeIndicated. Also sizeIndicated info will be passed to the server, which will compare actual data size downloaded. In case of mismatch, SDO abort message will be generated.
    • +
  • If sizeIndicated is 0, then actual data size will not be verified.
    • +
+
SDOtimeoutTime_msTimeout time for SDO communication in milliseconds.
blockEnableTry to initiate block transfer.
+
+
+
Returns
CO_SDO_return_t
+ +
+
+ +

◆ CO_SDOclientDownloadInitiateSize()

+ +
+
+ + + + + + + + + + + + + + + + + + +
void CO_SDOclientDownloadInitiateSize (CO_SDOclient_t * SDO_C,
size_t sizeIndicated 
)
+
+ +

Initiate SDO download communication - update size.

+

This is optional function, which updates sizeIndicated, if it was not known in the CO_SDOclientDownloadInitiate() function call. This function can be used after CO_SDOclientDownloadBufWrite(), but must be used before CO_SDOclientDownload().

+
Parameters
+ + + +
SDO_CThis object.
sizeIndicatedSame as in CO_SDOclientDownloadInitiate().
+
+
+ +
+
+ +

◆ CO_SDOclientDownloadBufWrite()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + +
size_t CO_SDOclientDownloadBufWrite (CO_SDOclient_t * SDO_C,
const char * buf,
size_t count 
)
+
+ +

Write data into SDO client buffer.

+

This function copies data from buf into internal SDO client fifo buffer. Function returns number of bytes successfully copied. If there is not enough space in destination, not all bytes will be copied. Additional data can be copied in next cycles. If there is enough space in destination and sizeIndicated is different than zero, then all data must be written at once.

+

This function is basically a wrapper for CO_fifo_write() function. As alternative, other functions from CO_fifo can be used directly, for example CO_fifo_cpyTok2U8() or similar.

+
Parameters
+ + + + +
SDO_CThis object.
bufBuffer which will be copied
countNumber of bytes in buf
+
+
+
Returns
number of bytes actually written.
+ +
+
+ +

◆ CO_SDOclientDownload()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
CO_SDO_return_t CO_SDOclientDownload (CO_SDOclient_t * SDO_C,
uint32_t timeDifference_us,
bool_t abort,
bool_t bufferPartial,
CO_SDO_abortCode_t * SDOabortCode,
size_t * sizeTransferred,
uint32_t * timerNext_us 
)
+
+ +

Process SDO download communication.

+

Function must be called cyclically until it returns <=0. It Proceeds SDO download communication initiated with CO_SDOclientDownloadInitiate(). Function is non-blocking.

+

If function returns CO_SDO_RT_blockDownldInProgress and OS has buffer for CAN tx messages, then this function may be called multiple times within own loop. This can speed-up SDO block transfer.

+
Parameters
+ + + + + + + + +
SDO_CThis object.
timeDifference_usTime difference from previous function call in [microseconds].
abortIf true, SDO client will send abort message from SDOabortCode and transmission will be aborted.
bufferPartialTrue indicates, not all data were copied to internal buffer yet. Buffer will be refilled later with CO_SDOclientDownloadBufWrite.
[out]SDOabortCodeIn case of error in communication, SDO abort code contains reason of error. Ignored if NULL.
[out]sizeTransferredActual size of data transferred. Ignored if NULL
[out]timerNext_usinfo to OS - see CO_process(). Ignored if NULL.
+
+
+
Returns
CO_SDO_return_t. If less than 0, then error occurred, SDOabortCode contains reason and state becomes idle. If 0, communication ends successfully and state becomes idle. If greater than 0, then communication is in progress.
+ +
+
+ +

◆ CO_SDOclientUploadInitiate()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
CO_SDO_return_t CO_SDOclientUploadInitiate (CO_SDOclient_t * SDO_C,
uint16_t index,
uint8_t subIndex,
uint16_t SDOtimeoutTime_ms,
bool_t blockEnable 
)
+
+ +

Initiate SDO upload communication.

+

Function initiates SDO upload communication with server specified in CO_SDOclient_init() function. Data will be read from remote node. Function is non-blocking.

+
Parameters
+ + + + + + +
SDO_CThis object.
indexIndex of object in object dictionary in remote node.
subIndexSubindex of object in object dictionary in remote node.
SDOtimeoutTime_msTimeout time for SDO communication in milliseconds.
blockEnableTry to initiate block transfer.
+
+
+
Returns
CO_SDO_return_t
+ +
+
+ +

◆ CO_SDOclientUpload()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
CO_SDO_return_t CO_SDOclientUpload (CO_SDOclient_t * SDO_C,
uint32_t timeDifference_us,
bool_t abort,
CO_SDO_abortCode_t * SDOabortCode,
size_t * sizeIndicated,
size_t * sizeTransferred,
uint32_t * timerNext_us 
)
+
+ +

Process SDO upload communication.

+

Function must be called cyclically until it returns <=0. It Proceeds SDO upload communication initiated with CO_SDOclientUploadInitiate(). Function is non-blocking.

+

If this function returns CO_SDO_RT_uploadDataBufferFull, then data must be read from fifo buffer to make it empty. This function can then be called once again immediately to speed-up block transfer. Note also, that remaining data must be read after function returns CO_SDO_RT_ok_communicationEnd. Data must not be read, if function returns CO_SDO_RT_blockUploadInProgress.

+
Parameters
+ + + + + + + + +
SDO_CThis object.
timeDifference_usTime difference from previous function call in [microseconds].
abortIf true, SDO client will send abort message from SDOabortCode and reception will be aborted.
[out]SDOabortCodeIn case of error in communication, SDO abort code contains reason of error. Ignored if NULL.
[out]sizeIndicatedIf larger than 0, then SDO server has indicated size of data transfer. Ignored if NULL.
[out]sizeTransferredActual size of data transferred. Ignored if NULL
[out]timerNext_usinfo to OS - see CO_process(). Ignored if NULL.
+
+
+
Returns
CO_SDO_return_t. If less than 0, then error occurred, SDOabortCode contains reason and state becomes idle. If 0, communication ends successfully and state becomes idle. If greater than 0, then communication is in progress.
+ +
+
+ +

◆ CO_SDOclientUploadBufRead()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + +
size_t CO_SDOclientUploadBufRead (CO_SDOclient_t * SDO_C,
char * buf,
size_t count 
)
+
+ +

Read data from SDO client buffer.

+

This function copies data from internal fifo buffer of SDO client into buf. Function returns number of bytes successfully copied. It can be called in multiple cycles, if data length is large.

+

This function is basically a wrapper for CO_fifo_read() function. As alternative, other functions from CO_fifo can be used directly, for example CO_fifo_readU82a() or similar.

+
Warning
This function (or similar) must NOT be called when CO_SDOclientUpload() returns CO_SDO_RT_blockUploadInProgress!
+
Parameters
+ + + + +
SDO_CThis object.
bufBuffer into which data will be copied
countCopy up to count bytes into buffer
+
+
+
Returns
number of bytes actually read.
+ +
+
+ +

◆ CO_SDOclientClose()

+ +
+
+ + + + + + + + +
void CO_SDOclientClose (CO_SDOclient_t * SDO_C)
+
+ +

Close SDO communication temporary.

+

Function must be called after finish of each SDO client communication cycle. It disables reception of SDO client CAN messages. It is necessary, because CO_SDOclient_receive function may otherwise write into undefined SDO buffer.

+
Parameters
+ + +
SDO_CThis object.
+
+
+ +
+
+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__SDOclient.js b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__SDOclient.js new file mode 100755 index 0000000..a3c98fb --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__SDOclient.js @@ -0,0 +1,51 @@ +var group__CO__SDOclient = +[ + [ "CO_SDOclient.h", "CO__SDOclient_8h.html", null ], + [ "CO_SDOclient_t", "structCO__SDOclient__t.html", [ + [ "OD", "structCO__SDOclient__t.html#a89334ad9cc4ac6751f4a8225f1645923", null ], + [ "nodeId", "structCO__SDOclient__t.html#a13de9457791eecf17051e405665bfa4a", null ], + [ "OD_IO", "structCO__SDOclient__t.html#a16fa659bc3098a3fbd60d0f6efde937a", null ], + [ "attribute", "structCO__SDOclient__t.html#a609088a2005febd6cd1561288cf1b7d1", null ], + [ "CANdevRx", "structCO__SDOclient__t.html#a3283cda81a28d3b193a6a6bef29fadab", null ], + [ "CANdevRxIdx", "structCO__SDOclient__t.html#afb8613dbcacfcefb970fabca4106eaeb", null ], + [ "CANdevTx", "structCO__SDOclient__t.html#ab4f18ed8c085ea333ca165e486f4ead3", null ], + [ "CANdevTxIdx", "structCO__SDOclient__t.html#a2a44e72381604a972f0e289495a41c37", null ], + [ "CANtxBuff", "structCO__SDOclient__t.html#a58efad796664487290b52b79cbdb3ae0", null ], + [ "COB_IDClientToServer", "structCO__SDOclient__t.html#a8752f1ad790cc61af17ff8b93b80c7ef", null ], + [ "COB_IDServerToClient", "structCO__SDOclient__t.html#a7b29a785240ca994c7dafd07d5a396e4", null ], + [ "nodeIDOfTheSDOServer", "structCO__SDOclient__t.html#a9e3c564cd4d027c5bd93bd25293ebacc", null ], + [ "index", "structCO__SDOclient__t.html#ad4fc4deee415a621f3558266ba447455", null ], + [ "subIndex", "structCO__SDOclient__t.html#a7f36981af65dc318dd69213c408b1b62", null ], + [ "sizeInd", "structCO__SDOclient__t.html#a1b9dc211b9b90dac825097757f985583", null ], + [ "sizeTran", "structCO__SDOclient__t.html#aa7a9ddc5dbf035644cf59857dcfa83db", null ], + [ "state", "structCO__SDOclient__t.html#a511a3981c4664be192b19714b95995c0", null ], + [ "SDOtimeoutTime_us", "structCO__SDOclient__t.html#ab2f8be2e90734e78f4e78bdabbb13488", null ], + [ "timeoutTimer", "structCO__SDOclient__t.html#a4e2ff087f13ff5a4754b9186c1a2929e", null ], + [ "bufFifo", "structCO__SDOclient__t.html#a634a9311de3569cf38841d2faaac20b3", null ], + [ "buf", "structCO__SDOclient__t.html#aa56b1f115aee473f5c264142053ed0ae", null ], + [ "CANrxNew", "structCO__SDOclient__t.html#a8020d62a62634a531a2efa43ef4534f5", null ], + [ "CANrxData", "structCO__SDOclient__t.html#a0bc8e66b7818bca04a4b49ae8210b387", null ], + [ "pFunctSignal", "structCO__SDOclient__t.html#ae7bbefdb0854addfa2ae45dcfc39d738", null ], + [ "functSignalObject", "structCO__SDOclient__t.html#a582292a9d5db2b3bf09bb089c788ecd0", null ], + [ "toggle", "structCO__SDOclient__t.html#a5962727f5c1830337146c7b2b389b391", null ], + [ "block_SDOtimeoutTime_us", "structCO__SDOclient__t.html#ae86079157706e7db12d9d4817172ba10", null ], + [ "block_timeoutTimer", "structCO__SDOclient__t.html#a8a667736bf5d22e7bb76f8b25d8b0268", null ], + [ "block_seqno", "structCO__SDOclient__t.html#a628780da4dccceab6ce79ad880989b26", null ], + [ "block_blksize", "structCO__SDOclient__t.html#a36b10791595b638309a01418d13a745f", null ], + [ "block_noData", "structCO__SDOclient__t.html#a26f9fcf95f47a4f7eeaefdf684e317a1", null ], + [ "block_crcEnabled", "structCO__SDOclient__t.html#a8690a5e7ee83fb7e0fa3a76cdec83f3a", null ], + [ "block_dataUploadLast", "structCO__SDOclient__t.html#ae9e678cb0e461851298658c7eee01334", null ], + [ "block_crc", "structCO__SDOclient__t.html#a51322a623ff85d36a8be60c0fe11430e", null ] + ] ], + [ "CO_SDOclient_init", "group__CO__SDOclient.html#ga754160e34089aea70f84d22b06eaff9e", null ], + [ "CO_SDOclient_initCallbackPre", "group__CO__SDOclient.html#ga4377eaecc3bd0a8320a2bbe1ef0ef776", null ], + [ "CO_SDOclient_setup", "group__CO__SDOclient.html#gade3bf4e3249aa4c611570ec43563a08d", null ], + [ "CO_SDOclientDownloadInitiate", "group__CO__SDOclient.html#ga40f6e79592e1d587d02bbb4eaefa9489", null ], + [ "CO_SDOclientDownloadInitiateSize", "group__CO__SDOclient.html#gaf58b7731b4285538c26a0c7c49ab24b6", null ], + [ "CO_SDOclientDownloadBufWrite", "group__CO__SDOclient.html#ga958d0568bd47d9a3152f9ea8d104b5f5", null ], + [ "CO_SDOclientDownload", "group__CO__SDOclient.html#gaab262f0a8d08ba023639a2c197d0943a", null ], + [ "CO_SDOclientUploadInitiate", "group__CO__SDOclient.html#ga3180f82563b3ed768fe7d3bd34fe1886", null ], + [ "CO_SDOclientUpload", "group__CO__SDOclient.html#gabd3a3be7e3d1649adfdd253c979ec21f", null ], + [ "CO_SDOclientUploadBufRead", "group__CO__SDOclient.html#gaf5cd4e009476b15a2cd995a9841fb175", null ], + [ "CO_SDOclientClose", "group__CO__SDOclient.html#ga9b98ea2c36f864f1a589c842528b12ab", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__SDOserver.html b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__SDOserver.html new file mode 100755 index 0000000..333ebe3 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__SDOserver.html @@ -0,0 +1,845 @@ + + + + + + + +CANopenNode: SDO server + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Files | +Data Structures | +Macros | +Enumerations | +Functions
+
+
SDO server
CANopen_301
+
+
+ + + + + +

+Files

file  CO_SDOserver.h
 CANopen Service Data Object - server protocol.
 
+ + + + +

+Data Structures

struct  CO_SDOserver_t
 SDO server object. More...
 
+ + + + +

+Macros

#define CO_SDO_ST_FLAG_DOWNLOAD   0x10U
 Internal state flags indicate type of transfer. More...
 
+ + + + + + + + + + +

+Enumerations

enum  CO_SDO_state_t {
+  CO_SDO_ST_IDLE = 0x00U, +CO_SDO_ST_ABORT = 0x01U, +CO_SDO_ST_DOWNLOAD_LOCAL_TRANSFER = 0x10U, +CO_SDO_ST_DOWNLOAD_INITIATE_REQ = 0x11U, +
+  CO_SDO_ST_DOWNLOAD_INITIATE_RSP = 0x12U, +CO_SDO_ST_DOWNLOAD_SEGMENT_REQ = 0x13U, +CO_SDO_ST_DOWNLOAD_SEGMENT_RSP = 0x14U, +CO_SDO_ST_UPLOAD_LOCAL_TRANSFER = 0x20U, +
+  CO_SDO_ST_UPLOAD_INITIATE_REQ = 0x21U, +CO_SDO_ST_UPLOAD_INITIATE_RSP = 0x22U, +CO_SDO_ST_UPLOAD_SEGMENT_REQ = 0x23U, +CO_SDO_ST_UPLOAD_SEGMENT_RSP = 0x24U, +
+  CO_SDO_ST_DOWNLOAD_BLK_INITIATE_REQ = 0x51U, +CO_SDO_ST_DOWNLOAD_BLK_INITIATE_RSP = 0x52U, +CO_SDO_ST_DOWNLOAD_BLK_SUBBLOCK_REQ = 0x53U, +CO_SDO_ST_DOWNLOAD_BLK_SUBBLOCK_RSP = 0x54U, +
+  CO_SDO_ST_DOWNLOAD_BLK_END_REQ = 0x55U, +CO_SDO_ST_DOWNLOAD_BLK_END_RSP = 0x56U, +CO_SDO_ST_UPLOAD_BLK_INITIATE_REQ = 0x61U, +CO_SDO_ST_UPLOAD_BLK_INITIATE_RSP = 0x62U, +
+  CO_SDO_ST_UPLOAD_BLK_INITIATE_REQ2 = 0x63U, +CO_SDO_ST_UPLOAD_BLK_SUBBLOCK_SREQ = 0x64U, +CO_SDO_ST_UPLOAD_BLK_SUBBLOCK_CRSP = 0x65U, +CO_SDO_ST_UPLOAD_BLK_END_SREQ = 0x66U, +
+  CO_SDO_ST_UPLOAD_BLK_END_CRSP = 0x67U +
+ }
 Internal states of the SDO state machine. More...
 
enum  CO_SDO_abortCode_t {
+  CO_SDO_AB_NONE = 0x00000000UL, +CO_SDO_AB_TOGGLE_BIT = 0x05030000UL, +CO_SDO_AB_TIMEOUT = 0x05040000UL, +CO_SDO_AB_CMD = 0x05040001UL, +
+  CO_SDO_AB_BLOCK_SIZE = 0x05040002UL, +CO_SDO_AB_SEQ_NUM = 0x05040003UL, +CO_SDO_AB_CRC = 0x05040004UL, +CO_SDO_AB_OUT_OF_MEM = 0x05040005UL, +
+  CO_SDO_AB_UNSUPPORTED_ACCESS = 0x06010000UL, +CO_SDO_AB_WRITEONLY = 0x06010001UL, +CO_SDO_AB_READONLY = 0x06010002UL, +CO_SDO_AB_NOT_EXIST = 0x06020000UL, +
+  CO_SDO_AB_NO_MAP = 0x06040041UL, +CO_SDO_AB_MAP_LEN = 0x06040042UL, +CO_SDO_AB_PRAM_INCOMPAT = 0x06040043UL, +CO_SDO_AB_DEVICE_INCOMPAT = 0x06040047UL, +
+  CO_SDO_AB_HW = 0x06060000UL, +CO_SDO_AB_TYPE_MISMATCH = 0x06070010UL, +CO_SDO_AB_DATA_LONG = 0x06070012UL, +CO_SDO_AB_DATA_SHORT = 0x06070013UL, +
+  CO_SDO_AB_SUB_UNKNOWN = 0x06090011UL, +CO_SDO_AB_INVALID_VALUE = 0x06090030UL, +CO_SDO_AB_VALUE_HIGH = 0x06090031UL, +CO_SDO_AB_VALUE_LOW = 0x06090032UL, +
+  CO_SDO_AB_MAX_LESS_MIN = 0x06090036UL, +CO_SDO_AB_NO_RESOURCE = 0x060A0023UL, +CO_SDO_AB_GENERAL = 0x08000000UL, +CO_SDO_AB_DATA_TRANSF = 0x08000020UL, +
+  CO_SDO_AB_DATA_LOC_CTRL = 0x08000021UL, +CO_SDO_AB_DATA_DEV_STATE = 0x08000022UL, +CO_SDO_AB_DATA_OD = 0x08000023UL, +CO_SDO_AB_NO_DATA = 0x08000024UL +
+ }
 SDO abort codes. More...
 
enum  CO_SDO_return_t {
+  CO_SDO_RT_waitingLocalTransfer = 6, +CO_SDO_RT_uploadDataBufferFull = 5, +CO_SDO_RT_transmittBufferFull = 4, +CO_SDO_RT_blockDownldInProgress = 3, +
+  CO_SDO_RT_blockUploadInProgress = 2, +CO_SDO_RT_waitingResponse = 1, +CO_SDO_RT_ok_communicationEnd = 0, +CO_SDO_RT_wrongArguments = -2, +
+  CO_SDO_RT_endedWithClientAbort = -9, +CO_SDO_RT_endedWithServerAbort = -10 +
+ }
 Return values from SDO server or client functions. More...
 
+ + + + + + + + + + +

+Functions

CO_ReturnError_t CO_SDOserver_init (CO_SDOserver_t *SDO, const OD_t *OD, const OD_entry_t *OD_1200_SDOsrvPar, uint8_t nodeId, uint16_t SDOtimeoutTime_ms, CO_CANmodule_t *CANdevRx, uint16_t CANdevRxIdx, CO_CANmodule_t *CANdevTx, uint16_t CANdevTxIdx)
 Initialize SDO object. More...
 
void CO_SDOserver_initCallbackPre (CO_SDOserver_t *SDO, void *object, void(*pFunctSignalPre)(void *object))
 Initialize SDOrx callback function. More...
 
CO_SDO_return_t CO_SDOserver_process (CO_SDOserver_t *SDO, bool_t NMTisPreOrOperational, uint32_t timeDifference_us, uint32_t *timerNext_us)
 Process SDO communication. More...
 
+

Detailed Description

+

CANopen Service Data Object - server protocol.

+

Service data objects (SDOs) allow the access to any entry of the CANopen Object dictionary. By SDO a peer-to-peer communication channel between two CANopen devices is established. In addition, the SDO protocol enables to transfer any amount of data in a segmented way. Therefore the SDO protocol is mainly used in order to communicate configuration data.

+

All CANopen devices must have implemented SDO server and first SDO server channel. Servers serves data from Object dictionary. Object dictionary is a collection of variables, arrays or records (structures), which can be used by the stack or by the application. This file (CO_SDOserver.h) implements SDO server.

+

SDO client can be (optionally) implemented on one (or multiple, if multiple SDO channels are used) device in CANopen network. Usually this is master device and provides also some kind of user interface, so configuration of the network is possible. Code for the SDO client is in file CO_SDOclient.h.

+

SDO communication cycle is initiated by the client. Client can upload (read) data from device or can download (write) data to device. If data size is less or equal to 4 bytes, communication is finished by one server response (expedited transfer). If data size is longer, data are split into multiple segments of request/response pairs (normal or segmented transfer). For longer data there is also a block transfer protocol, which transfers larger block of data in secure way with little protocol overhead. If error occurs during SDO transfer CO_SDO_abortCode_t is send by client or server and transfer is terminated. For more details see CO_SDO_state_t.

+

Access to Object dictionary is specified in OD interface.

+

Macro Definition Documentation

+ +

◆ CO_SDO_ST_FLAG_DOWNLOAD

+ +
+
+ + + + +
#define CO_SDO_ST_FLAG_DOWNLOAD   0x10U
+
+ +

Internal state flags indicate type of transfer.

+

These flags correspond to the upper nibble of the SDO state machine states and can be used to determine the type of state an SDO object is in.

+ +
+
+

Enumeration Type Documentation

+ +

◆ CO_SDO_state_t

+ +
+
+ + + + +
enum CO_SDO_state_t
+
+ +

Internal states of the SDO state machine.

+

Upper nibble of byte indicates type of state: 0x10: Download 0x20: Upload 0x40: Block Mode

+

Note: CANopen has little endian byte order.

+ + + + + + + + + + + + + + + + + + + + + + + + + + +
Enumerator
CO_SDO_ST_IDLE 
    +
  • SDO client may start new download to or upload from specified node, specified index and specified subindex. It can start normal or block communication.
    • +
  • SDO server is waiting for client request.
    • +
+
CO_SDO_ST_ABORT 
    +
  • SDO client or server may send SDO abort message in case of error:
      +
    • byte 0: 10000000 binary.
      • +
    • byte 1..3: Object index and subIndex.
      • +
    • byte 4..7: CO_SDO_abortCode_t.
      • +
    +
    • +
+
CO_SDO_ST_DOWNLOAD_LOCAL_TRANSFER 
    +
  • SDO client: Node-ID of the SDO server is the same as node-ID of this node, SDO client is the same device as SDO server. Transfer data directly without communication on CAN.
    • +
  • SDO server does not use this state.
    • +
+
CO_SDO_ST_DOWNLOAD_INITIATE_REQ 
    +
  • SDO client initiates SDO download:
      +
    • byte 0: 0010nnes binary: (nn: if e=s=1, number of data bytes, that do not contain data; e=1 for expedited transfer; s=1 if data size is indicated.)
      • +
    • byte 1..3: Object index and subIndex.
      • +
    • byte 4..7: If e=1, expedited data are here. If e=0 s=1, size of data for segmented transfer is indicated here.
      • +
    +
    • +
  • SDO server is in CO_SDO_ST_IDLE state and waits for client request.
    • +
+
CO_SDO_ST_DOWNLOAD_INITIATE_RSP 
    +
  • SDO client waits for response.
    • +
  • SDO server responses:
      +
    • byte 0: 01100000 binary.
      • +
    • byte 1..3: Object index and subIndex.
      • +
    • byte 4..7: Reserved.
      • +
    +
    • +
  • In case of expedited transfer communication ends here.
    • +
+
CO_SDO_ST_DOWNLOAD_SEGMENT_REQ 
    +
  • SDO client sends SDO segment:
      +
    • byte 0: 000tnnnc binary: (t: toggle bit, set to 0 in first segment; nnn: number of data bytes, that do not contain data; c=1 if this is the last segment).
      • +
    • byte 1..7: Data segment.
      • +
    +
    • +
  • SDO server waits for segment.
    • +
+
CO_SDO_ST_DOWNLOAD_SEGMENT_RSP 
    +
  • SDO client waits for response.
    • +
  • SDO server responses:
      +
    • byte 0: 001t0000 binary: (t: toggle bit, set to 0 in first segment).
      • +
    • byte 1..7: Reserved.
      • +
    +
    • +
  • If c was set to 1, then communication ends here.
    • +
+
CO_SDO_ST_UPLOAD_LOCAL_TRANSFER 
    +
  • SDO client: Node-ID of the SDO server is the same as node-ID of this node, SDO client is the same device as SDO server. Transfer data directly without communication on CAN.
    • +
  • SDO server does not use this state.
    • +
+
CO_SDO_ST_UPLOAD_INITIATE_REQ 
    +
  • SDO client initiates SDO upload:
      +
    • byte 0: 01000000 binary.
      • +
    • byte 1..3: Object index and subIndex.
      • +
    • byte 4..7: Reserved.
      • +
    +
    • +
  • SDO server is in CO_SDO_ST_IDLE state and waits for client request.
    • +
+
CO_SDO_ST_UPLOAD_INITIATE_RSP 
    +
  • SDO client waits for response.
    • +
  • SDO server responses:
      +
    • byte 0: 0100nnes binary: (nn: if e=s=1, number of data bytes, that do not contain data; e=1 for expedited transfer; s=1 if data size is indicated).
      • +
    • byte 1..3: Object index and subIndex.
      • +
    • byte 4..7: If e=1, expedited data are here. If e=0 s=1, size of data for segmented transfer is indicated here.
      • +
    +
    • +
  • In case of expedited transfer communication ends here.
    • +
+
CO_SDO_ST_UPLOAD_SEGMENT_REQ 
    +
  • SDO client requests SDO segment:
      +
    • byte 0: 011t0000 binary: (t: toggle bit, set to 0 in first segment).
      • +
    • byte 1..7: Reserved.
      • +
    +
    • +
  • SDO server waits for segment request.
    • +
+
CO_SDO_ST_UPLOAD_SEGMENT_RSP 
    +
  • SDO client waits for response.
    • +
  • SDO server responses with data:
      +
    • byte 0: 000tnnnc binary: (t: toggle bit, set to 0 in first segment; nnn: number of data bytes, that do not contain data; c=1 if this is the last segment).
      • +
    • byte 1..7: Data segment.
      • +
    +
    • +
  • If c is set to 1, then communication ends here.
    • +
+
CO_SDO_ST_DOWNLOAD_BLK_INITIATE_REQ 
    +
  • SDO client initiates SDO block download:
      +
    • byte 0: 11000rs0 binary: (r=1 if client supports generating CRC on data; s=1 if data size is indicated.)
      • +
    • byte 1..3: Object index and subIndex.
      • +
    • byte 4..7: If s=1, then size of data for block download is indicated here.
      • +
    +
    • +
  • SDO server is in CO_SDO_ST_IDLE state and waits for client request.
    • +
+
CO_SDO_ST_DOWNLOAD_BLK_INITIATE_RSP 
    +
  • SDO client waits for response.
    • +
  • SDO server responses:
      +
    • byte 0: 10100r00 binary: (r=1 if server supports generating CRC on data.)
      • +
    • byte 1..3: Object index and subIndex.
      • +
    • byte 4: blksize: Number of segments per block that shall be used by the client for the following block download with 0 < blksize < 128.
      • +
    • byte 5..7: Reserved.
      • +
    +
    • +
+
CO_SDO_ST_DOWNLOAD_BLK_SUBBLOCK_REQ 
    +
  • SDO client sends 'blksize' segments of data in sequence:
      +
    • byte 0: cnnnnnnn binary: (c=1 if no more segments to be downloaded, enter SDO block download end phase; nnnnnnn is sequence number of segment, 1..127.
      • +
    • byte 1..7: At most 7 bytes of segment data to be downloaded.
      • +
    +
    • +
  • SDO server reads sequence of 'blksize' blocks.
    • +
+
CO_SDO_ST_DOWNLOAD_BLK_SUBBLOCK_RSP 
    +
  • SDO client waits for response.
    • +
  • SDO server responses:
      +
    • byte 0: 10100010 binary.
      • +
    • byte 1: ackseq: sequence number of last segment that was received successfully during the last block download. If ackseq is set to 0 the server indicates the client that the segment with the sequence number 1 was not received correctly and all segments shall be retransmitted by the client.
      • +
    • byte 2: Number of segments per block that shall be used by the client for the following block download with 0 < blksize < 128.
      • +
    • byte 3..7: Reserved.
      • +
    +
    • +
  • If c was set to 1, then communication enters SDO block download end phase.
    • +
+
CO_SDO_ST_DOWNLOAD_BLK_END_REQ 
    +
  • SDO client sends SDO block download end:
      +
    • byte 0: 110nnn01 binary: (nnn: number of data bytes, that do not contain data)
      • +
    • byte 1..2: 16 bit CRC for the data set, if enabled by client and server.
      • +
    • byte 3..7: Reserved.
      • +
    +
    • +
  • SDO server waits for client request.
    • +
+
CO_SDO_ST_DOWNLOAD_BLK_END_RSP 
    +
  • SDO client waits for response.
    • +
  • SDO server responses:
      +
    • byte 0: 10100001 binary.
      • +
    • byte 1..7: Reserved.
      • +
    +
    • +
  • Block download successfully ends here.
    • +
+
CO_SDO_ST_UPLOAD_BLK_INITIATE_REQ 
    +
  • SDO client initiates SDO block upload:
      +
    • byte 0: 10100r00 binary: (r=1 if client supports generating CRC on data.)
      • +
    • byte 1..3: Object index and subIndex.
      • +
    • byte 4: blksize: Number of segments per block with 0 < blksize < 128.
      • +
    • byte 5: pst - protocol switch threshold. If pst > 0 and size of the data in bytes is less or equal pst, then the server may switch to the SDO upload protocol CO_SDO_ST_UPLOAD_INITIATE_RSP.
      • +
    • byte 6..7: Reserved.
      • +
    +
    • +
  • SDO server is in CO_SDO_ST_IDLE state and waits for client request.
    • +
+
CO_SDO_ST_UPLOAD_BLK_INITIATE_RSP 
    +
  • SDO client waits for response.
    • +
  • SDO server responses:
      +
    • byte 0: 11000rs0 binary: (r=1 if server supports generating CRC on data; s=1 if data size is indicated. )
      • +
    • byte 1..3: Object index and subIndex.
      • +
    • byte 4..7: If s=1, then size of data for block upload is indicated here.
      • +
    +
    • +
  • If enabled by pst, then server may alternatively response with CO_SDO_ST_UPLOAD_INITIATE_RSP
    • +
+
CO_SDO_ST_UPLOAD_BLK_INITIATE_REQ2 
    +
  • SDO client sends second initiate for SDO block upload:
      +
    • byte 0: 10100011 binary.
      • +
    • byte 1..7: Reserved.
      • +
    +
    • +
  • SDO server waits for client request.
    • +
+
CO_SDO_ST_UPLOAD_BLK_SUBBLOCK_SREQ 
    +
  • SDO client reads sequence of 'blksize' blocks.
    • +
  • SDO server sends 'blksize' segments of data in sequence:
      +
    • byte 0: cnnnnnnn binary: (c=1 if no more segments to be uploaded, enter SDO block upload end phase; nnnnnnn is sequence number of segment, 1..127.
      • +
    • byte 1..7: At most 7 bytes of segment data to be uploaded.
      • +
    +
    • +
+
CO_SDO_ST_UPLOAD_BLK_SUBBLOCK_CRSP 
    +
  • SDO client responses:
      +
    • byte 0: 10100010 binary.
      • +
    • byte 1: ackseq: sequence number of last segment that was received successfully during the last block upload. If ackseq is set to 0 the client indicates the server that the segment with the sequence number 1 was not received correctly and all segments shall be retransmitted by the server.
      • +
    • byte 2: Number of segments per block that shall be used by the server for the following block upload with 0 < blksize < 128.
      • +
    • byte 3..7: Reserved.
      • +
    +
    • +
  • SDO server waits for response.
    • +
  • If c was set to 1 and all segments were successfull received, then communication enters SDO block upload end phase.
    • +
+
CO_SDO_ST_UPLOAD_BLK_END_SREQ 
    +
  • SDO client waits for server request.
    • +
  • SDO server sends SDO block upload end:
      +
    • byte 0: 110nnn01 binary: (nnn: number of data bytes, that do not contain data)
      • +
    • byte 1..2: 16 bit CRC for the data set, if enabled by client and server.
      • +
    • byte 3..7: Reserved.
      • +
    +
    • +
+
CO_SDO_ST_UPLOAD_BLK_END_CRSP 
    +
  • SDO client responses:
      +
    • byte 0: 10100001 binary.
      • +
    • byte 1..7: Reserved.
      • +
    +
    • +
  • SDO server waits for response.
    • +
  • Block download successfully ends here. Note that this communication ends with client response. Client may then start next SDO communication immediately.
    • +
+
+ +
+
+ +

◆ CO_SDO_abortCode_t

+ +
+
+ + + + +
enum CO_SDO_abortCode_t
+
+ +

SDO abort codes.

+

Send with Abort SDO transfer message.

+

The abort codes not listed here are reserved.

+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Enumerator
CO_SDO_AB_NONE 

0x00000000, No abort

+
CO_SDO_AB_TOGGLE_BIT 

0x05030000, Toggle bit not altered

+
CO_SDO_AB_TIMEOUT 

0x05040000, SDO protocol timed out

+
CO_SDO_AB_CMD 

0x05040001, Command specifier not valid or unknown

+
CO_SDO_AB_BLOCK_SIZE 

0x05040002, Invalid block size in block mode

+
CO_SDO_AB_SEQ_NUM 

0x05040003, Invalid sequence number in block mode

+
CO_SDO_AB_CRC 

0x05040004, CRC error (block mode only)

+
CO_SDO_AB_OUT_OF_MEM 

0x05040005, Out of memory

+
CO_SDO_AB_UNSUPPORTED_ACCESS 

0x06010000, Unsupported access to an object

+
CO_SDO_AB_WRITEONLY 

0x06010001, Attempt to read a write only object

+
CO_SDO_AB_READONLY 

0x06010002, Attempt to write a read only object

+
CO_SDO_AB_NOT_EXIST 

0x06020000, Object does not exist in the object dictionary

+
CO_SDO_AB_NO_MAP 

0x06040041, Object cannot be mapped to the PDO

+
CO_SDO_AB_MAP_LEN 

0x06040042, Number and length of object to be mapped exceeds PDO length

+
CO_SDO_AB_PRAM_INCOMPAT 

0x06040043, General parameter incompatibility reasons

+
CO_SDO_AB_DEVICE_INCOMPAT 

0x06040047, General internal incompatibility in device

+
CO_SDO_AB_HW 

0x06060000, Access failed due to hardware error

+
CO_SDO_AB_TYPE_MISMATCH 

0x06070010, Data type does not match, length of service parameter does not match

+
CO_SDO_AB_DATA_LONG 

0x06070012, Data type does not match, length of service parameter too high

+
CO_SDO_AB_DATA_SHORT 

0x06070013, Data type does not match, length of service parameter too short

+
CO_SDO_AB_SUB_UNKNOWN 

0x06090011, Sub index does not exist

+
CO_SDO_AB_INVALID_VALUE 

0x06090030, Invalid value for parameter (download only).

+
CO_SDO_AB_VALUE_HIGH 

0x06090031, Value range of parameter written too high

+
CO_SDO_AB_VALUE_LOW 

0x06090032, Value range of parameter written too low

+
CO_SDO_AB_MAX_LESS_MIN 

0x06090036, Maximum value is less than minimum value.

+
CO_SDO_AB_NO_RESOURCE 

0x060A0023, Resource not available: SDO connection

+
CO_SDO_AB_GENERAL 

0x08000000, General error

+
CO_SDO_AB_DATA_TRANSF 

0x08000020, Data cannot be transferred or stored to application

+
CO_SDO_AB_DATA_LOC_CTRL 

0x08000021, Data cannot be transferred or stored to application because of local control

+
CO_SDO_AB_DATA_DEV_STATE 

0x08000022, Data cannot be transferred or stored to application because of present device state

+
CO_SDO_AB_DATA_OD 

0x08000023, Object dictionary not present or dynamic generation fails

+
CO_SDO_AB_NO_DATA 

0x08000024, No data available

+
+ +
+
+ +

◆ CO_SDO_return_t

+ +
+
+ + + + +
enum CO_SDO_return_t
+
+ +

Return values from SDO server or client functions.

+ + + + + + + + + + + +
Enumerator
CO_SDO_RT_waitingLocalTransfer 

Waiting in client local transfer.

+
CO_SDO_RT_uploadDataBufferFull 

Data buffer is full.

+

SDO client: data must be read before next upload cycle begins.

+
CO_SDO_RT_transmittBufferFull 

CAN transmit buffer is full.

+

Waiting.

+
CO_SDO_RT_blockDownldInProgress 

Block download is in progress.

+

Sending train of messages.

+
CO_SDO_RT_blockUploadInProgress 

Block upload is in progress.

+

Receiving train of messages. SDO client: Data must not be read in this state.

+
CO_SDO_RT_waitingResponse 

Waiting server or client response.

+
CO_SDO_RT_ok_communicationEnd 

Success, end of communication.

+

SDO client: uploaded data must be read.

+
CO_SDO_RT_wrongArguments 

Error in arguments.

+
CO_SDO_RT_endedWithClientAbort 

Communication ended with client abort.

+
CO_SDO_RT_endedWithServerAbort 

Communication ended with server abort.

+
+ +
+
+

Function Documentation

+ +

◆ CO_SDOserver_init()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
CO_ReturnError_t CO_SDOserver_init (CO_SDOserver_t * SDO,
const OD_t * OD,
const OD_entry_t * OD_1200_SDOsrvPar,
uint8_t nodeId,
uint16_t SDOtimeoutTime_ms,
CO_CANmodule_t * CANdevRx,
uint16_t CANdevRxIdx,
CO_CANmodule_t * CANdevTx,
uint16_t CANdevTxIdx 
)
+
+ +

Initialize SDO object.

+

Function must be called in the communication reset section.

+
Parameters
+ + + + + + + + + + +
SDOThis object will be initialized.
ODObject Dictionary.
OD_1200_SDOsrvParOD entry for SDO server parameter (0x1200+), can be NULL for default single SDO server and must not be NULL for additional SDO servers. With additional SDO servers it may also have IO extension enabled, to allow dynamic configuration (see also CO_CONFIG_FLAG_OD_DYNAMIC).
nodeIdIf this is first SDO channel, then "nodeId" is CANopen Node ID of this device. In all additional channels "nodeId" is ignored.
SDOtimeoutTime_msTimeout time for SDO communication in milliseconds.
CANdevRxCAN device for SDO server reception.
CANdevRxIdxIndex of receive buffer in the above CAN device.
CANdevTxCAN device for SDO server transmission.
CANdevTxIdxIndex of transmit buffer in the above CAN device.
+
+
+
Returns
CO_ReturnError_t CO_ERROR_NO in case of success.
+ +
+
+ +

◆ CO_SDOserver_initCallbackPre()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + +
void CO_SDOserver_initCallbackPre (CO_SDOserver_t * SDO,
void * object,
void(*)(void *object) pFunctSignalPre 
)
+
+ +

Initialize SDOrx callback function.

+

Function initializes optional callback function, which should immediately start processing of CO_SDOserver_process() function. Callback is called after SDOserver message is received from the CAN bus or when new call without delay is necessary (SDO block transfer is in progress).

+
Parameters
+ + + + +
SDOThis object.
objectPointer to object, which will be passed to pFunctSignalPre(). Can be NULL
pFunctSignalPrePointer to the callback function. Not called if NULL.
+
+
+ +
+
+ +

◆ CO_SDOserver_process()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
CO_SDO_return_t CO_SDOserver_process (CO_SDOserver_t * SDO,
bool_t NMTisPreOrOperational,
uint32_t timeDifference_us,
uint32_t * timerNext_us 
)
+
+ +

Process SDO communication.

+

Function must be called cyclically.

+
Parameters
+ + + + + +
SDOThis object.
NMTisPreOrOperationalTrue if CO_NMT_internalState_t is NMT_PRE_OPERATIONAL or NMT_OPERATIONAL.
timeDifference_usTime difference from previous function call in [microseconds].
[out]timerNext_usinfo to OS - see CO_process().
+
+
+
Returns
CO_SDO_return_t
+ +
+
+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__SDOserver.js b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__SDOserver.js new file mode 100755 index 0000000..186d325 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__SDOserver.js @@ -0,0 +1,117 @@ +var group__CO__SDOserver = +[ + [ "CO_SDOserver.h", "CO__SDOserver_8h.html", null ], + [ "CO_SDOserver_t", "structCO__SDOserver__t.html", [ + [ "CANdevTx", "structCO__SDOserver__t.html#add469f75cf702d340d069aadfe8ede14", null ], + [ "CANtxBuff", "structCO__SDOserver__t.html#a36ae3c719d96121b95b77f76d2cce723", null ], + [ "OD", "structCO__SDOserver__t.html#a8c104c076f57ffe039e0e41a1b12377e", null ], + [ "nodeId", "structCO__SDOserver__t.html#a38d0b70cb37d6be927208e3662105c6c", null ], + [ "state", "structCO__SDOserver__t.html#a0b457d35679acb51d11d21cd9f3660fd", null ], + [ "OD_IO", "structCO__SDOserver__t.html#a19e7e8afc09ced5629e3a1e04b83aa9f", null ], + [ "index", "structCO__SDOserver__t.html#ae0b1720a88d948fbf6d8e20b333abb17", null ], + [ "subIndex", "structCO__SDOserver__t.html#a5051d5aeaa97e5d40ccfb39461706f10", null ], + [ "attribute", "structCO__SDOserver__t.html#a3b2febaed4df4921626367a741008400", null ], + [ "CANrxNew", "structCO__SDOserver__t.html#a48e9b1237bc0dd46945762416d09c5bb", null ], + [ "CANrxData", "structCO__SDOserver__t.html#abfbff2e51c54be56f0ba090864c7e2f6", null ], + [ "CANdevRx", "structCO__SDOserver__t.html#a3e6255db1f66d742debd80ff2ce25012", null ], + [ "CANdevRxIdx", "structCO__SDOserver__t.html#a8c5ca24946f34e174fce129a2f5cb38a", null ], + [ "CANdevTxIdx", "structCO__SDOserver__t.html#acbb02ed7ddf534c8f0c41acd25478f47", null ], + [ "COB_IDClientToServer", "structCO__SDOserver__t.html#a7c9113f146613eec4b76888bd8d0f2fd", null ], + [ "COB_IDServerToClient", "structCO__SDOserver__t.html#a47cf7cde974f0ff8fd20f7a5363857cf", null ], + [ "sizeInd", "structCO__SDOserver__t.html#a95be5bcb7257f818a294c9b03b0d1386", null ], + [ "sizeTran", "structCO__SDOserver__t.html#a86eaba6efabaa335b769924edd80f1ae", null ], + [ "toggle", "structCO__SDOserver__t.html#a158797aa411b8d3b5c2079907d04ca0d", null ], + [ "finished", "structCO__SDOserver__t.html#a90997c7a119e97d2cb8b9bbc5fb5145f", null ], + [ "SDOtimeoutTime_us", "structCO__SDOserver__t.html#a09381f3b6885d0a3bc7d1fa96805eb5d", null ], + [ "timeoutTimer", "structCO__SDOserver__t.html#a69dfa2436ba0bb51c527eb03330b48a7", null ], + [ "buf", "structCO__SDOserver__t.html#ab834e69e77b72e1ee1d39d35e21e0df4", null ], + [ "bufOffsetWr", "structCO__SDOserver__t.html#a69f52a30b4e7dc0a6e55c88b3126f814", null ], + [ "bufOffsetRd", "structCO__SDOserver__t.html#a93557d1ee64582bc37c76f4d4680f7a5", null ], + [ "block_SDOtimeoutTime_us", "structCO__SDOserver__t.html#ae1e16955965dced9464abd0a6bf8c2b2", null ], + [ "block_timeoutTimer", "structCO__SDOserver__t.html#ad8748718c76f53347dde5248b1152626", null ], + [ "block_seqno", "structCO__SDOserver__t.html#aeefd77d5200958a30f63f7e1f4f474fa", null ], + [ "block_blksize", "structCO__SDOserver__t.html#a76e4c66e15027e78b8ba67bdd3089cc3", null ], + [ "block_noData", "structCO__SDOserver__t.html#a54bac23ac93450234a858c50f0516d05", null ], + [ "block_crcEnabled", "structCO__SDOserver__t.html#abdf7205835b75f0f6feef3bc89a86c17", null ], + [ "block_crc", "structCO__SDOserver__t.html#a33c2432ccea06da1d5c33c89c14caf63", null ], + [ "pFunctSignalPre", "structCO__SDOserver__t.html#ace57036f1dfd1ffe35f4b13e50fc4a41", null ], + [ "functSignalObjectPre", "structCO__SDOserver__t.html#adac867a846309166bca3f5dd0b550e65", null ] + ] ], + [ "CO_SDO_ST_FLAG_DOWNLOAD", "group__CO__SDOserver.html#ga84d9afbba1769aada5c52c81b7f5c3f4", null ], + [ "CO_SDO_state_t", "group__CO__SDOserver.html#ga0b0e614dadcc1c005185b8bc9a7fec11", [ + [ "CO_SDO_ST_IDLE", "group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11a2eee38ba2a2d52890281ae54b12d50b3", null ], + [ "CO_SDO_ST_ABORT", "group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11ac40cb6c0b2f2eb1877aee3963dc1927d", null ], + [ "CO_SDO_ST_DOWNLOAD_LOCAL_TRANSFER", "group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11a8f685c4d233c35defb423fda8ff5544c", null ], + [ "CO_SDO_ST_DOWNLOAD_INITIATE_REQ", "group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11ac07432ccfaa6be8730cc8c306b3e42bb", null ], + [ "CO_SDO_ST_DOWNLOAD_INITIATE_RSP", "group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11a49b060ebf39c4bfb498b8691c16bb882", null ], + [ "CO_SDO_ST_DOWNLOAD_SEGMENT_REQ", "group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11a6b44777e7e209313612baab5f83745ff", null ], + [ "CO_SDO_ST_DOWNLOAD_SEGMENT_RSP", "group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11ae5b55aec51372cbc2a6e32ce1456c11c", null ], + [ "CO_SDO_ST_UPLOAD_LOCAL_TRANSFER", "group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11aa07fe53d69ec7e0d56db39111867f8ce", null ], + [ "CO_SDO_ST_UPLOAD_INITIATE_REQ", "group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11aa8a8b5050c6528fdaa19bbb429d8e4f4", null ], + [ "CO_SDO_ST_UPLOAD_INITIATE_RSP", "group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11aa096d10c9eb891cfedddc16276f58aaf", null ], + [ "CO_SDO_ST_UPLOAD_SEGMENT_REQ", "group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11ad610c289b85192d70c835b033b49b3fb", null ], + [ "CO_SDO_ST_UPLOAD_SEGMENT_RSP", "group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11a210a3eb6acfdb055bb72a59d8e24a6b6", null ], + [ "CO_SDO_ST_DOWNLOAD_BLK_INITIATE_REQ", "group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11a5d4ead9d3f06962987b6af8c073b6a2e", null ], + [ "CO_SDO_ST_DOWNLOAD_BLK_INITIATE_RSP", "group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11af25ee4e636a98dd72fe4c5bef9bcecf2", null ], + [ "CO_SDO_ST_DOWNLOAD_BLK_SUBBLOCK_REQ", "group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11a320cc9749db35473265b5203c547bbf8", null ], + [ "CO_SDO_ST_DOWNLOAD_BLK_SUBBLOCK_RSP", "group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11a70e97f34a6a98014bef1d2eeb3b5247c", null ], + [ "CO_SDO_ST_DOWNLOAD_BLK_END_REQ", "group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11af955593bb966b324bfda361b0364d15b", null ], + [ "CO_SDO_ST_DOWNLOAD_BLK_END_RSP", "group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11af511c26db1fb7ba18d6054255b560be7", null ], + [ "CO_SDO_ST_UPLOAD_BLK_INITIATE_REQ", "group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11a49b5c39c9e5d025c85eedffa28aa22ed", null ], + [ "CO_SDO_ST_UPLOAD_BLK_INITIATE_RSP", "group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11ae9be0eeb0711890d1b9c5cbfbd204ed8", null ], + [ "CO_SDO_ST_UPLOAD_BLK_INITIATE_REQ2", "group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11acc4e87ad1ad20eddd19a60d9592bbada", null ], + [ "CO_SDO_ST_UPLOAD_BLK_SUBBLOCK_SREQ", "group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11a39f1cb5426ee3c3689ed833cb66e231c", null ], + [ "CO_SDO_ST_UPLOAD_BLK_SUBBLOCK_CRSP", "group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11af762eb5a985cf79a3e7423a39b29b328", null ], + [ "CO_SDO_ST_UPLOAD_BLK_END_SREQ", "group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11a98896138e97542e659051fff33b1a692", null ], + [ "CO_SDO_ST_UPLOAD_BLK_END_CRSP", "group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11ab75a573a45778a0c4bea2c50402be03e", null ] + ] ], + [ "CO_SDO_abortCode_t", "group__CO__SDOserver.html#ga7587ddcf798747fe6d97d03bf1bf2979", [ + [ "CO_SDO_AB_NONE", "group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979a5fc84558a4ca47e067189a14543691b6", null ], + [ "CO_SDO_AB_TOGGLE_BIT", "group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979ad4e9214eab1d034e9c10eb6c7638e592", null ], + [ "CO_SDO_AB_TIMEOUT", "group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979a036d0be874d10f66aa6601d76a9aa2f0", null ], + [ "CO_SDO_AB_CMD", "group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979a26b4e2680c16ce6a09d3e3a8293472ce", null ], + [ "CO_SDO_AB_BLOCK_SIZE", "group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979ac86b70b71d601658c93a1dd270a902b0", null ], + [ "CO_SDO_AB_SEQ_NUM", "group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979a778ef6b5751cb8ba10b67436409c3fd2", null ], + [ "CO_SDO_AB_CRC", "group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979aee7fcab60a6fde6e41d999f5a2b10aa5", null ], + [ "CO_SDO_AB_OUT_OF_MEM", "group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979adc021e79ace03edbd279a3c492853c7f", null ], + [ "CO_SDO_AB_UNSUPPORTED_ACCESS", "group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979a370ff72a5bddee5760ba0930c3b13ba0", null ], + [ "CO_SDO_AB_WRITEONLY", "group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979a457e80af0f952c272fa90ebd45cdb8cd", null ], + [ "CO_SDO_AB_READONLY", "group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979a48c8a5f4939372564a17b31f992b82a4", null ], + [ "CO_SDO_AB_NOT_EXIST", "group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979a6ef5b921ac0f299f34e9860eb82e332e", null ], + [ "CO_SDO_AB_NO_MAP", "group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979a729452df9557e4acbda8691efb4da310", null ], + [ "CO_SDO_AB_MAP_LEN", "group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979a07edee9ce8ec5cd01cfd3cfbff48b96c", null ], + [ "CO_SDO_AB_PRAM_INCOMPAT", "group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979acaedcf71c4638efb40fc6debfa9dba67", null ], + [ "CO_SDO_AB_DEVICE_INCOMPAT", "group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979ad07acc06f76122627412a71f2f2e39fc", null ], + [ "CO_SDO_AB_HW", "group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979a070f096bb09f5a6235643702b5a40759", null ], + [ "CO_SDO_AB_TYPE_MISMATCH", "group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979a838c274eaa14626514da8f7a8ac043c3", null ], + [ "CO_SDO_AB_DATA_LONG", "group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979a50d373f7a7ba976dc2277a2111cf56c3", null ], + [ "CO_SDO_AB_DATA_SHORT", "group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979a481537e4c170066ca31b167fa598bb54", null ], + [ "CO_SDO_AB_SUB_UNKNOWN", "group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979a3e48e535fddeaa78a4059c2f91f9bb8e", null ], + [ "CO_SDO_AB_INVALID_VALUE", "group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979afff1ec491c628031e65672383f3e3c76", null ], + [ "CO_SDO_AB_VALUE_HIGH", "group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979a4983bce8e9503f9e7a720a44528036ad", null ], + [ "CO_SDO_AB_VALUE_LOW", "group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979ab402816165086fbad21a130e9f488d52", null ], + [ "CO_SDO_AB_MAX_LESS_MIN", "group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979a15d49829c0d15f8cb9995f07617d874f", null ], + [ "CO_SDO_AB_NO_RESOURCE", "group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979ab54dd042727804cd8f310a04fd4575f7", null ], + [ "CO_SDO_AB_GENERAL", "group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979a58d6be7d156bbe576b8438a6fd5b446d", null ], + [ "CO_SDO_AB_DATA_TRANSF", "group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979a631a043a79c7eef4ddb2f874365c6660", null ], + [ "CO_SDO_AB_DATA_LOC_CTRL", "group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979ac489bb77a98f65008932861924bc4bbf", null ], + [ "CO_SDO_AB_DATA_DEV_STATE", "group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979ac92ccaa16d833cac6d2f6d8c2836d886", null ], + [ "CO_SDO_AB_DATA_OD", "group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979aec1840b00621e92f27da2d0705ddab63", null ], + [ "CO_SDO_AB_NO_DATA", "group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979a3e007eeec7538b5dbe7e78240632b415", null ] + ] ], + [ "CO_SDO_return_t", "group__CO__SDOserver.html#ga7f729ab203285c7623df493916f22a73", [ + [ "CO_SDO_RT_waitingLocalTransfer", "group__CO__SDOserver.html#gga7f729ab203285c7623df493916f22a73ab9191f8a57b840a81457591f0fbd8a76", null ], + [ "CO_SDO_RT_uploadDataBufferFull", "group__CO__SDOserver.html#gga7f729ab203285c7623df493916f22a73ada069dad6b1e0bec180600b1d34758d2", null ], + [ "CO_SDO_RT_transmittBufferFull", "group__CO__SDOserver.html#gga7f729ab203285c7623df493916f22a73ad62e2421dcee78ba0477fb379a6e7e4e", null ], + [ "CO_SDO_RT_blockDownldInProgress", "group__CO__SDOserver.html#gga7f729ab203285c7623df493916f22a73aa8036db7f41e8958c057da0d4ab24f8f", null ], + [ "CO_SDO_RT_blockUploadInProgress", "group__CO__SDOserver.html#gga7f729ab203285c7623df493916f22a73ad73a50f4a1d7ef69797cbf7c930293f2", null ], + [ "CO_SDO_RT_waitingResponse", "group__CO__SDOserver.html#gga7f729ab203285c7623df493916f22a73a15d85fc411d0c6e69888c2ec9d641eb5", null ], + [ "CO_SDO_RT_ok_communicationEnd", "group__CO__SDOserver.html#gga7f729ab203285c7623df493916f22a73a2d0d1d8d1bc297205b3e87174642199c", null ], + [ "CO_SDO_RT_wrongArguments", "group__CO__SDOserver.html#gga7f729ab203285c7623df493916f22a73af1dc6a56b2b38fb5f4c878661173decc", null ], + [ "CO_SDO_RT_endedWithClientAbort", "group__CO__SDOserver.html#gga7f729ab203285c7623df493916f22a73a9aafefd96d032c1b65cb6c23bc53f0aa", null ], + [ "CO_SDO_RT_endedWithServerAbort", "group__CO__SDOserver.html#gga7f729ab203285c7623df493916f22a73ae2fce3f477766eb188502886705dc177", null ] + ] ], + [ "CO_SDOserver_init", "group__CO__SDOserver.html#gac989ba60f25fd2bc48bca6df0c0c1dde", null ], + [ "CO_SDOserver_initCallbackPre", "group__CO__SDOserver.html#ga3eeea49e2fb36da22dc754c62b03a423", null ], + [ "CO_SDOserver_process", "group__CO__SDOserver.html#ga360bc6aa1659a5572d4d1077d787433a", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__SRDO.html b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__SRDO.html new file mode 100755 index 0000000..ea08936 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__SRDO.html @@ -0,0 +1,575 @@ + + + + + + + +CANopenNode: SRDO + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Files | +Data Structures | +Functions
+
+
SRDO
CANopen_304
+
+
+ + + + + + + + +

+Files

file  CO_SRDO.c
 CANopen Safety Related Data Object protocol.
 
file  CO_SRDO.h
 CANopen Safety Related Data Object protocol.
 
+ + + + + + + + + + +

+Data Structures

struct  CO_SRDOCommPar_t
 SRDO communication parameter. More...
 
struct  CO_SRDOGuard_t
 Gurad Object for SRDO monitors: More...
 
struct  CO_SRDO_t
 SRDO object. More...
 
+ + + + + + + + + + + + + + + + + + + + + + +

+Functions

CO_ReturnError_t CO_SRDOGuard_init (CO_SRDOGuard_t *SRDOGuard, CO_SDO_t *SDO, CO_NMT_internalState_t *operatingState, uint8_t *configurationValid, uint16_t idx_SRDOvalid, uint16_t idx_SRDOcrc)
 Initialize SRDOGuard object. More...
 
uint8_t CO_SRDOGuard_process (CO_SRDOGuard_t *SRDOGuard)
 Process operation and valid state changes. More...
 
CO_ReturnError_t CO_SRDO_init (CO_SRDO_t *SRDO, CO_SRDOGuard_t *SRDOGuard, CO_EM_t *em, CO_SDO_t *SDO, uint8_t nodeId, uint16_t defaultCOB_ID, const CO_SRDOCommPar_t *SRDOCommPar, const CO_SRDOMapPar_t *SRDOMapPar, const uint16_t *checksum, uint16_t idx_SRDOCommPar, uint16_t idx_SRDOMapPar, CO_CANmodule_t *CANdevRx, uint16_t CANdevRxIdxNormal, uint16_t CANdevRxIdxInverted, CO_CANmodule_t *CANdevTx, uint16_t CANdevTxIdxNormal, uint16_t CANdevTxIdxInverted)
 Initialize SRDO object. More...
 
void CO_SRDO_initCallbackPre (CO_SRDO_t *SRDO, void *object, void(*pFunctSignalPre)(void *object))
 Initialize SRDO callback function. More...
 
void CO_SRDO_initCallbackEnterSafeState (CO_SRDO_t *SRDO, void *object, void(*pFunctSignalSafe)(void *object))
 Initialize SRDO callback function. More...
 
CO_ReturnError_t CO_SRDO_requestSend (CO_SRDO_t *SRDO)
 Send SRDO on event. More...
 
void CO_SRDO_process (CO_SRDO_t *SRDO, uint8_t commands, uint32_t timeDifference_us, uint32_t *timerNext_us)
 Process transmitting/receiving SRDO messages. More...
 
+

Detailed Description

+

CANopen Safety Related Data Object protocol.

+

The functionality is very similar to that of the PDOs. The main differences is every message is send and received twice. The second message must be bitwise inverted. The delay between the two messages and between each message pair is monitored. The distinction between sending and receiving SRDO is made at runtime (for PDO it is compile time). If the security protocol is used, at least one SRDO is mandatory.

+

Function Documentation

+ +

◆ CO_SRDOGuard_init()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
CO_ReturnError_t CO_SRDOGuard_init (CO_SRDOGuard_t * SRDOGuard,
CO_SDO_t * SDO,
CO_NMT_internalState_t * operatingState,
uint8_t * configurationValid,
uint16_t idx_SRDOvalid,
uint16_t idx_SRDOcrc 
)
+
+ +

Initialize SRDOGuard object.

+

Function must be called in the communication reset section.

+
Parameters
+ + + + + + + +
SRDOGuardThis object will be initialized.
SDOSDO object.
operatingStatePointer to variable indicating CANopen device NMT internal state.
configurationValidPointer to variable with the SR valid flag
idx_SRDOvalidIndex in Object Dictionary
idx_SRDOcrcIndex in Object Dictionary
+
+
+
Returns
CO_ReturnError_t: CO_ERROR_NO or CO_ERROR_ILLEGAL_ARGUMENT.
+ +
+
+ +

◆ CO_SRDOGuard_process()

+ +
+
+ + + + + + + + +
uint8_t CO_SRDOGuard_process (CO_SRDOGuard_t * SRDOGuard)
+
+ +

Process operation and valid state changes.

+
Parameters
+ + +
SRDOGuardThis object.
+
+
+
Returns
uint8_t command for CO_SRDO_process().
    +
  • bit 0 entered operational
    • +
  • bit 1 validate checksum
    • +
+
+ +
+
+ +

◆ CO_SRDO_init()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
CO_ReturnError_t CO_SRDO_init (CO_SRDO_t * SRDO,
CO_SRDOGuard_t * SRDOGuard,
CO_EM_t * em,
CO_SDO_t * SDO,
uint8_t nodeId,
uint16_t defaultCOB_ID,
const CO_SRDOCommPar_t * SRDOCommPar,
const CO_SRDOMapPar_t * SRDOMapPar,
const uint16_t * checksum,
uint16_t idx_SRDOCommPar,
uint16_t idx_SRDOMapPar,
CO_CANmodule_t * CANdevRx,
uint16_t CANdevRxIdxNormal,
uint16_t CANdevRxIdxInverted,
CO_CANmodule_t * CANdevTx,
uint16_t CANdevTxIdxNormal,
uint16_t CANdevTxIdxInverted 
)
+
+ +

Initialize SRDO object.

+

Function must be called in the communication reset section.

+
Parameters
+ + + + + + + + + + + + + + + + + + +
SRDOThis object will be initialized.
SRDOGuardSRDOGuard object.
emEmergency object.
SDOSDO object.
nodeIdCANopen Node ID of this device. If default COB_ID is used, value will be added.
defaultCOB_IDDefault COB ID for this SRDO (without NodeId).
SRDOCommParPointer to SRDO communication parameter record from Object dictionary (index 0x1301+).
SRDOMapParPointer to SRDO mapping parameter record from Object dictionary (index 0x1381+).
checksum
idx_SRDOCommParIndex in Object Dictionary
idx_SRDOMapParIndex in Object Dictionary
CANdevRxCAN device used for SRDO reception.
CANdevRxIdxNormalIndex of receive buffer in the above CAN device.
CANdevRxIdxInvertedIndex of receive buffer in the above CAN device.
CANdevTxCAN device used for SRDO transmission.
CANdevTxIdxNormalIndex of transmit buffer in the above CAN device.
CANdevTxIdxInvertedIndex of transmit buffer in the above CAN device.
+
+
+
Returns
CO_ReturnError_t: CO_ERROR_NO or CO_ERROR_ILLEGAL_ARGUMENT.
+ +
+
+ +

◆ CO_SRDO_initCallbackPre()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + +
void CO_SRDO_initCallbackPre (CO_SRDO_t * SRDO,
void * object,
void(*)(void *object) pFunctSignalPre 
)
+
+ +

Initialize SRDO callback function.

+

Function initializes optional callback function, which should immediately start processing of CO_SRDO_process() function. Callback is called after SRDO message is received from the CAN bus.

+
Parameters
+ + + + +
SRDOThis object.
objectPointer to object, which will be passed to pFunctSignalPre(). Can be NULL
pFunctSignalPrePointer to the callback function. Not called if NULL.
+
+
+ +
+
+ +

◆ CO_SRDO_initCallbackEnterSafeState()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + +
void CO_SRDO_initCallbackEnterSafeState (CO_SRDO_t * SRDO,
void * object,
void(*)(void *object) pFunctSignalSafe 
)
+
+ +

Initialize SRDO callback function.

+

Function initializes optional callback function, that is called when SRDO enters a safe state. This happens when a timeout is reached or the data is inconsistent. The safe state itself is not further defined. One measure, for example, would be to go back to the pre-operational state Callback is called from CO_SRDO_process().

+
Parameters
+ + + + +
SRDOThis object.
objectPointer to object, which will be passed to pFunctSignalSafe(). Can be NULL
pFunctSignalSafePointer to the callback function. Not called if NULL.
+
+
+ +
+
+ +

◆ CO_SRDO_requestSend()

+ +
+
+ + + + + + + + +
CO_ReturnError_t CO_SRDO_requestSend (CO_SRDO_t * SRDO)
+
+ +

Send SRDO on event.

+

Sends SRDO before the next refresh timer tiggers. The message itself is send in CO_SRDO_process(). After the transmission the timer is reset to the full refresh time.

+
Parameters
+ + +
SRDOThis object.
+
+
+
Returns
CO_ReturnError_t CO_ERROR_NO if request is granted
+ +
+
+ +

◆ CO_SRDO_process()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
void CO_SRDO_process (CO_SRDO_t * SRDO,
uint8_t commands,
uint32_t timeDifference_us,
uint32_t * timerNext_us 
)
+
+ +

Process transmitting/receiving SRDO messages.

+

This function verifies the checksum on demand. This function also configures the SRDO on operation state change to operational

+
Parameters
+ + + + + +
SRDOThis object.
commandsresult from CO_SRDOGuard_process().
timeDifference_usTime difference from previous function call in [microseconds].
[out]timerNext_usinfo to OS.
+
+
+ +
+
+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__SRDO.js b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__SRDO.js new file mode 100755 index 0000000..e6d0a6c --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__SRDO.js @@ -0,0 +1,53 @@ +var group__CO__SRDO = +[ + [ "CO_SRDO.c", "CO__SRDO_8c.html", null ], + [ "CO_SRDO.h", "CO__SRDO_8h.html", null ], + [ "CO_SRDOCommPar_t", "structCO__SRDOCommPar__t.html", [ + [ "maxSubIndex", "structCO__SRDOCommPar__t.html#af156b61e6d278b014466e860f073cf05", null ], + [ "informationDirection", "structCO__SRDOCommPar__t.html#ac8f865699090f666910e66dabf53b339", null ], + [ "safetyCycleTime", "structCO__SRDOCommPar__t.html#a257a5534889060efc7477be7fc0adbad", null ], + [ "safetyRelatedValidationTime", "structCO__SRDOCommPar__t.html#a5c02007d0e4f1b1cfd8a07da93b954cb", null ], + [ "transmissionType", "structCO__SRDOCommPar__t.html#a8716aa43cf70db8af86c4125b14cd538", null ], + [ "COB_ID1_normal", "structCO__SRDOCommPar__t.html#a89d3762612ef971aecaae43ce94141cc", null ], + [ "COB_ID2_inverted", "structCO__SRDOCommPar__t.html#a574ade8ff753ae742f6a3b51b32a11fe", null ] + ] ], + [ "CO_SRDOGuard_t", "structCO__SRDOGuard__t.html", [ + [ "operatingState", "structCO__SRDOGuard__t.html#a5e80afb9ddd0debd94eaafa9c6f4ad36", null ], + [ "operatingStatePrev", "structCO__SRDOGuard__t.html#ac6f6a3b1465360e035656933470d6b4d", null ], + [ "configurationValid", "structCO__SRDOGuard__t.html#aa4c8b5e7d9d8fa54d91ad0797ea9b39b", null ], + [ "checkCRC", "structCO__SRDOGuard__t.html#ad8cd90a85e6e6d551fb04e8fa94feffd", null ] + ] ], + [ "CO_SRDO_t", "structCO__SRDO__t.html", [ + [ "em", "structCO__SRDO__t.html#a1779ab0170d7b604948a9396681212c8", null ], + [ "SDO", "structCO__SRDO__t.html#af8121f4d1f0879eba793afaee5c6c804", null ], + [ "SRDOGuard", "structCO__SRDO__t.html#ab96c524d0b0243527a6b2d3ee89de302", null ], + [ "mapPointer", "structCO__SRDO__t.html#a3e7570a1aeef89502702175eccb93500", null ], + [ "dataLength", "structCO__SRDO__t.html#ae994e87a71e85342f3a5b3c046d8a47f", null ], + [ "nodeId", "structCO__SRDO__t.html#ac4cc841f24894e41a5bbdbd386e62a0e", null ], + [ "defaultCOB_ID", "structCO__SRDO__t.html#a14da12ca5af61cd6cc6e442a0baa5c26", null ], + [ "valid", "structCO__SRDO__t.html#a6eef41749d7862ef2a29108f4f08185a", null ], + [ "SRDOCommPar", "structCO__SRDO__t.html#aba2d09de18da68fb01ab23696de452ac", null ], + [ "SRDOMapPar", "structCO__SRDO__t.html#a876184e5c0b2809fc4b412cc2accb43e", null ], + [ "checksum", "structCO__SRDO__t.html#adffd373a96a9570b0cbd4487c50d5359", null ], + [ "CANdevRx", "structCO__SRDO__t.html#ad5c0cea22d56cef74f42728107748b38", null ], + [ "CANdevTx", "structCO__SRDO__t.html#a2fe71edd01cb39629fe42753e84df1fc", null ], + [ "CANtxBuff", "structCO__SRDO__t.html#a693cc1ce40b882e85359291744d68b99", null ], + [ "CANdevRxIdx", "structCO__SRDO__t.html#aa1d8ca455950557652636bc8021928a4", null ], + [ "CANdevTxIdx", "structCO__SRDO__t.html#a0f56f71798c4781bf436d03a0f20938b", null ], + [ "toogle", "structCO__SRDO__t.html#a79114736807f3aa8ab19e6e88df93050", null ], + [ "timer", "structCO__SRDO__t.html#a1d3553daf7179b3389023ac78287dfbc", null ], + [ "CANrxNew", "structCO__SRDO__t.html#acbf5e7096cf72e911a4391ffb8dc939d", null ], + [ "CANrxData", "structCO__SRDO__t.html#ae6edba4f0446c10b971a90bde01c8a0b", null ], + [ "pFunctSignalSafe", "structCO__SRDO__t.html#ab000fd951d5b72615a3b3086335a6add", null ], + [ "functSignalObjectSafe", "structCO__SRDO__t.html#a037ba4d3ca1e33c6187219ba4766b2a8", null ], + [ "pFunctSignalPre", "structCO__SRDO__t.html#a5c9c5a746066607b6052c66bcf07190e", null ], + [ "functSignalObjectPre", "structCO__SRDO__t.html#a5807d03b19a7e2ae7c4250dd4e61f235", null ] + ] ], + [ "CO_SRDOGuard_init", "group__CO__SRDO.html#gacd578e8a5a4af024f8e6f8aef87cbd14", null ], + [ "CO_SRDOGuard_process", "group__CO__SRDO.html#ga0201fa4da8b37a18f864a9fd7c826a6c", null ], + [ "CO_SRDO_init", "group__CO__SRDO.html#ga608e6d48e97f1b4da316b36f10e389c6", null ], + [ "CO_SRDO_initCallbackPre", "group__CO__SRDO.html#gac066166b35bcb3d0b01adfc0e2eb9bb1", null ], + [ "CO_SRDO_initCallbackEnterSafeState", "group__CO__SRDO.html#ga5303601f6f94c83530b5e165f54b54bb", null ], + [ "CO_SRDO_requestSend", "group__CO__SRDO.html#gac9a21725c4bb0373ea18a414019cf339", null ], + [ "CO_SRDO_process", "group__CO__SRDO.html#gacb94aa4f279a4476c193ee50c408dfbb", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__STACK__CONFIG.html b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__STACK__CONFIG.html new file mode 100755 index 0000000..a1b8a00 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__STACK__CONFIG.html @@ -0,0 +1,156 @@ + + + + + + + +CANopenNode: Stack configuration + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Modules
+
+
Stack configuration
CANopen_301 » Driver
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

+Modules

 Common definitions
 
 NMT master/slave and HB producer/consumer
 
 Emergency producer/consumer
 
 SDO server/client
 
 Time producer/consumer
 
 SYNC and PDO producer/consumer
 
 CANopen LED diodes
 Specified in standard CiA 303-3.
 
 Safety Related Data Objects (SRDO)
 Specified in standard EN 50325-5 (CiA 304)
 
 LSS master/slave
 Specified in standard CiA 305.
 
 CANopen gateway
 Specified in standard CiA 309.
 
 CRC 16 calculation
 Helper object.
 
 FIFO buffer
 Helper object.
 
 Trace recorder
 Non standard object.
 
 Debug messages
 Messages from different parts of the stack.
 
+

Detailed Description

+

Stack configuration macros specify, which parts of the stack will be enabled.

+

Default values for stack configuration macros are set in corresponding header files. The same default values are also provided in this file, but only for documentation generator. Default values can be overridden by CO_driver_target.h file. If specified so, they can further be overridden by CO_driver_custom.h file.

+

Stack configuration macro is specified as bits, where each bit enables/disables some part of the configurable CANopenNode object. Flags are used for enabling or checking specific bit. Multiple flags can be ORed together.

+

Some functionalities of CANopenNode objects, enabled by configuration macros, requires some objects from Object Dictionary to exist. Object Dictionary configuration must match Stack configuration.

+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__STACK__CONFIG.js b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__STACK__CONFIG.js new file mode 100755 index 0000000..8b40152 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__STACK__CONFIG.js @@ -0,0 +1,17 @@ +var group__CO__STACK__CONFIG = +[ + [ "Common definitions", "group__CO__STACK__CONFIG__COMMON.html", "group__CO__STACK__CONFIG__COMMON" ], + [ "NMT master/slave and HB producer/consumer", "group__CO__STACK__CONFIG__NMT__HB.html", "group__CO__STACK__CONFIG__NMT__HB" ], + [ "Emergency producer/consumer", "group__CO__STACK__CONFIG__EMERGENCY.html", "group__CO__STACK__CONFIG__EMERGENCY" ], + [ "SDO server/client", "group__CO__STACK__CONFIG__SDO.html", "group__CO__STACK__CONFIG__SDO" ], + [ "Time producer/consumer", "group__CO__STACK__CONFIG__TIME.html", "group__CO__STACK__CONFIG__TIME" ], + [ "SYNC and PDO producer/consumer", "group__CO__STACK__CONFIG__SYNC__PDO.html", "group__CO__STACK__CONFIG__SYNC__PDO" ], + [ "CANopen LED diodes", "group__CO__STACK__CONFIG__LEDS.html", "group__CO__STACK__CONFIG__LEDS" ], + [ "Safety Related Data Objects (SRDO)", "group__CO__STACK__CONFIG__SRDO.html", "group__CO__STACK__CONFIG__SRDO" ], + [ "LSS master/slave", "group__CO__STACK__CONFIG__LSS.html", "group__CO__STACK__CONFIG__LSS" ], + [ "CANopen gateway", "group__CO__STACK__CONFIG__GATEWAY.html", "group__CO__STACK__CONFIG__GATEWAY" ], + [ "CRC 16 calculation", "group__CO__STACK__CONFIG__CRC16.html", "group__CO__STACK__CONFIG__CRC16" ], + [ "FIFO buffer", "group__CO__STACK__CONFIG__FIFO.html", "group__CO__STACK__CONFIG__FIFO" ], + [ "Trace recorder", "group__CO__STACK__CONFIG__TRACE.html", "group__CO__STACK__CONFIG__TRACE" ], + [ "Debug messages", "group__CO__STACK__CONFIG__DEBUG.html", "group__CO__STACK__CONFIG__DEBUG" ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__STACK__CONFIG__COMMON.html b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__STACK__CONFIG__COMMON.html new file mode 100755 index 0000000..9cbee83 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__STACK__CONFIG__COMMON.html @@ -0,0 +1,181 @@ + + + + + + + +CANopenNode: Common definitions + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Macros
+
+
Common definitions
CANopen_301 » Driver » Stack configuration
+
+
+ + + + + + + + + + + +

+Macros

#define CO_CONFIG_FLAG_CALLBACK_PRE   0x1000
 Enable custom callback after CAN receive. More...
 
#define CO_CONFIG_FLAG_TIMERNEXT   0x2000
 Enable calculation of timerNext_us variable. More...
 
#define CO_CONFIG_FLAG_OD_DYNAMIC   0x4000
 Enable dynamic behaviour of Object Dictionary variables. More...
 
+

Detailed Description

+

Macro Definition Documentation

+ +

◆ CO_CONFIG_FLAG_CALLBACK_PRE

+ +
+
+ + + + +
#define CO_CONFIG_FLAG_CALLBACK_PRE   0x1000
+
+ +

Enable custom callback after CAN receive.

+

Flag enables optional callback functions, which are part of some CANopenNode objects. Callbacks can optionally be registered by application, which configures threads in operating system. Callbacks are called after something has been preprocessed by higher priority thread and must be further processed by lower priority thread. For example when CAN message is received and preprocessed, callback should wake up mainline processing function. See also CO_process() function.

+

If callback functions are used, they must be initialized separately, after the object initialization.

+

This flag is common to multiple configuration macros.

+ +
+
+ +

◆ CO_CONFIG_FLAG_TIMERNEXT

+ +
+
+ + + + +
#define CO_CONFIG_FLAG_TIMERNEXT   0x2000
+
+ +

Enable calculation of timerNext_us variable.

+

Calculation of the timerNext_us variable is useful for smooth operation on operating system. See also CO_process() function.

+

This flag is common to multiple configuration macros.

+ +
+
+ +

◆ CO_CONFIG_FLAG_OD_DYNAMIC

+ +
+
+ + + + +
#define CO_CONFIG_FLAG_OD_DYNAMIC   0x4000
+
+ +

Enable dynamic behaviour of Object Dictionary variables.

+

Some CANopen objects uses Object Dictionary variables as arguments to initialization functions, which are processed in communication reset section. If this flag is set, then writing to OD variable will reconfigure corresponding CANopen object also during CANopen normal operation.

+

This flag is common to multiple configuration macros.

+ +
+
+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__STACK__CONFIG__COMMON.js b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__STACK__CONFIG__COMMON.js new file mode 100755 index 0000000..055ee3b --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__STACK__CONFIG__COMMON.js @@ -0,0 +1,6 @@ +var group__CO__STACK__CONFIG__COMMON = +[ + [ "CO_CONFIG_FLAG_CALLBACK_PRE", "group__CO__STACK__CONFIG__COMMON.html#gab55099df45bed12f182ef7c0c779dc14", null ], + [ "CO_CONFIG_FLAG_TIMERNEXT", "group__CO__STACK__CONFIG__COMMON.html#ga9e84c3a9256f15246be7766a61096c2d", null ], + [ "CO_CONFIG_FLAG_OD_DYNAMIC", "group__CO__STACK__CONFIG__COMMON.html#gaf0f46ccffdd156cc7c2d8774ecb2060d", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__STACK__CONFIG__CRC16.html b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__STACK__CONFIG__CRC16.html new file mode 100755 index 0000000..f37b8e7 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__STACK__CONFIG__CRC16.html @@ -0,0 +1,144 @@ + + + + + + + +CANopenNode: CRC 16 calculation + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Macros
+
+
CRC 16 calculation
CANopen_301 » Driver » Stack configuration
+
+
+ +

Helper object. +More...

+ + + + + +

+Macros

#define CO_CONFIG_CRC16   (0)
 Configuration of CRC 16 CCITT calculation. More...
 
+

Detailed Description

+

Helper object.

+

Macro Definition Documentation

+ +

◆ CO_CONFIG_CRC16

+ +
+
+ + + + +
#define CO_CONFIG_CRC16   (0)
+
+ +

Configuration of CRC 16 CCITT calculation.

+

Possible flags, can be ORed:

    +
  • CO_CONFIG_CRC16_ENABLE - Enable CRC16 calculation
    • +
  • CO_CONFIG_CRC16_EXTERNAL - CRC functions are defined externally
    • +
+ +
+
+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__STACK__CONFIG__CRC16.js b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__STACK__CONFIG__CRC16.js new file mode 100755 index 0000000..77fc209 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__STACK__CONFIG__CRC16.js @@ -0,0 +1,4 @@ +var group__CO__STACK__CONFIG__CRC16 = +[ + [ "CO_CONFIG_CRC16", "group__CO__STACK__CONFIG__CRC16.html#ga15737bc0ede4bcd56968e5f96b2e8c9b", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__STACK__CONFIG__DEBUG.html b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__STACK__CONFIG__DEBUG.html new file mode 100755 index 0000000..7f7ebcb --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__STACK__CONFIG__DEBUG.html @@ -0,0 +1,145 @@ + + + + + + + +CANopenNode: Debug messages + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Macros
+
+
Debug messages
CANopen_301 » Driver » Stack configuration
+
+
+ +

Messages from different parts of the stack. +More...

+ + + + + +

+Macros

#define CO_CONFIG_DEBUG   (0)
 Configuration of debug messages from different parts of the stack, which can be logged according to target specific function. More...
 
+

Detailed Description

+

Messages from different parts of the stack.

+

Macro Definition Documentation

+ +

◆ CO_CONFIG_DEBUG

+ +
+
+ + + + +
#define CO_CONFIG_DEBUG   (0)
+
+ +

Configuration of debug messages from different parts of the stack, which can be logged according to target specific function.

+

Possible flags, can be ORed:

    +
  • CO_CONFIG_DEBUG_COMMON - Define default CO_DEBUG_COMMON(msg) macro. This macro is target specific. This macro is then used as default macro in all other defined CO_DEBUG_XXX(msg) macros.
    • +
  • CO_CONFIG_DEBUG_SDO_CLIENT - Define default CO_DEBUG_SDO_CLIENT(msg) macro.
    • +
  • CO_CONFIG_DEBUG_SDO_SERVER - Define default CO_DEBUG_SDO_SERVER(msg) macro.
    • +
+ +
+
+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__STACK__CONFIG__DEBUG.js b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__STACK__CONFIG__DEBUG.js new file mode 100755 index 0000000..b30ec7d --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__STACK__CONFIG__DEBUG.js @@ -0,0 +1,4 @@ +var group__CO__STACK__CONFIG__DEBUG = +[ + [ "CO_CONFIG_DEBUG", "group__CO__STACK__CONFIG__DEBUG.html#ga1ce7b96c60a5ab9349b66b77f6a6e2a7", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__STACK__CONFIG__EMERGENCY.html b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__STACK__CONFIG__EMERGENCY.html new file mode 100755 index 0000000..b8108f3 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__STACK__CONFIG__EMERGENCY.html @@ -0,0 +1,332 @@ + + + + + + + +CANopenNode: Emergency producer/consumer + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Macros
+
+
Emergency producer/consumer
CANopen_301 » Driver » Stack configuration
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

+Macros

#define CO_CONFIG_EM   (CO_CONFIG_EM_PRODUCER | CO_CONFIG_EM_HISTORY)
 Configuration of Emergency. More...
 
#define CO_CONFIG_EM_ERR_STATUS_BITS_COUNT   (10*8)
 Maximum number of CO_EM_errorStatusBits_t. More...
 
#define CO_CONFIG_EM_BUFFER_SIZE   16
 Size of the internal buffer, where emergencies are stored after error indication with CO_error() function. More...
 
#define CO_CONFIG_ERR_CONDITION_GENERIC   (em->errorStatusBits[5] != 0)
 Condition for calculating CANopen Error register, "generic" error bit. More...
 
#define CO_CONFIG_ERR_CONDITION_CURRENT
 Condition for calculating CANopen Error register, "current" error bit. More...
 
#define CO_CONFIG_ERR_CONDITION_VOLTAGE
 Condition for calculating CANopen Error register, "voltage" error bit. More...
 
#define CO_CONFIG_ERR_CONDITION_TEMPERATURE
 Condition for calculating CANopen Error register, "temperature" error bit. More...
 
#define CO_CONFIG_ERR_CONDITION_COMMUNICATION   (em->errorStatusBits[2] || em->errorStatusBits[3])
 Condition for calculating CANopen Error register, "communication" error bit. More...
 
#define CO_CONFIG_ERR_CONDITION_DEV_PROFILE
 Condition for calculating CANopen Error register, "device profile" error bit. More...
 
#define CO_CONFIG_ERR_CONDITION_MANUFACTURER   (em->errorStatusBits[8] || em->errorStatusBits[9])
 Condition for calculating CANopen Error register, "manufacturer" error bit. More...
 
+

Detailed Description

+

Macro Definition Documentation

+ +

◆ CO_CONFIG_EM

+ +
+
+ + + + +
#define CO_CONFIG_EM   (CO_CONFIG_EM_PRODUCER | CO_CONFIG_EM_HISTORY)
+
+ +

Configuration of Emergency.

+

Possible flags, can be ORed:

    +
  • CO_CONFIG_EM_PRODUCER - Enable emergency producer.
    • +
  • CO_CONFIG_EM_PROD_CONFIGURABLE - Emergency producer COB-ID is configurable, OD object 0x1014. If not configurable, then 0x1014 is read-only, COB_ID is set to CO_CAN_ID_EMERGENCY + nodeId and write is not verified.
    • +
  • CO_CONFIG_EM_PROD_INHIBIT - Enable inhibit timer on emergency producer, OD object 0x1015.
    • +
  • CO_CONFIG_EM_HISTORY - Enable error history, OD object 0x1003, "Pre-defined error field"
    • +
  • CO_CONFIG_EM_CONSUMER - Enable simple emergency consumer with callback.
    • +
  • CO_CONFIG_EM_STATUS_BITS - Access CO_EM_errorStatusBits_t from OD.
    • +
  • CO_CONFIG_FLAG_CALLBACK_PRE - Enable custom callback after preprocessing emergency condition by CO_errorReport() or CO_errorReset() call. Callback is configured by CO_EM_initCallbackPre().
    • +
  • CO_CONFIG_FLAG_TIMERNEXT - Enable calculation of timerNext_us variable inside CO_EM_process().
    • +
+ +
+
+ +

◆ CO_CONFIG_EM_ERR_STATUS_BITS_COUNT

+ +
+
+ + + + +
#define CO_CONFIG_EM_ERR_STATUS_BITS_COUNT   (10*8)
+
+ +

Maximum number of CO_EM_errorStatusBits_t.

+

Stack uses 6*8 = 48 CO_EM_errorStatusBits_t, others are free to use by manufacturer. Allowable value range is from 48 to 256 bits in steps of 8. Default is 80.

+ +
+
+ +

◆ CO_CONFIG_EM_BUFFER_SIZE

+ +
+
+ + + + +
#define CO_CONFIG_EM_BUFFER_SIZE   16
+
+ +

Size of the internal buffer, where emergencies are stored after error indication with CO_error() function.

+

Each emergency has to be post- processed by the CO_EM_process() function. In case of overflow, error is indicated but emergency message is not sent.

+

The same buffer is also used for OD object 0x1003, "Pre-defined error field".

+

Each buffer element consumes 8 bytes. Valid values are 1 to 254.

+ +
+
+ +

◆ CO_CONFIG_ERR_CONDITION_GENERIC

+ +
+
+ + + + +
#define CO_CONFIG_ERR_CONDITION_GENERIC   (em->errorStatusBits[5] != 0)
+
+ +

Condition for calculating CANopen Error register, "generic" error bit.

+

Condition must observe suitable CO_EM_errorStatusBits_t and use corresponding member of errorStatusBits array from CO_EM_t to calculate the condition. See also CO_errorRegister_t.

+
Warning
Size of CO_CONFIG_EM_ERR_STATUS_BITS_COUNT must be large enough. (CO_CONFIG_EM_ERR_STATUS_BITS_COUNT/8) must be larger than index of array member in em->errorStatusBits[index].
+

em->errorStatusBits[5] should be included in the condition, because they are used by the stack.

+ +
+
+ +

◆ CO_CONFIG_ERR_CONDITION_CURRENT

+ +
+
+ + + + +
#define CO_CONFIG_ERR_CONDITION_CURRENT
+
+ +

Condition for calculating CANopen Error register, "current" error bit.

+

See CO_CONFIG_ERR_CONDITION_GENERIC for description. Macro is not defined by default, so no error is verified.

+ +
+
+ +

◆ CO_CONFIG_ERR_CONDITION_VOLTAGE

+ +
+
+ + + + +
#define CO_CONFIG_ERR_CONDITION_VOLTAGE
+
+ +

Condition for calculating CANopen Error register, "voltage" error bit.

+

See CO_CONFIG_ERR_CONDITION_GENERIC for description. Macro is not defined by default, so no error is verified.

+ +
+
+ +

◆ CO_CONFIG_ERR_CONDITION_TEMPERATURE

+ +
+
+ + + + +
#define CO_CONFIG_ERR_CONDITION_TEMPERATURE
+
+ +

Condition for calculating CANopen Error register, "temperature" error bit.

+

See CO_CONFIG_ERR_CONDITION_GENERIC for description. Macro is not defined by default, so no error is verified.

+ +
+
+ +

◆ CO_CONFIG_ERR_CONDITION_COMMUNICATION

+ +
+
+ + + + +
#define CO_CONFIG_ERR_CONDITION_COMMUNICATION   (em->errorStatusBits[2] || em->errorStatusBits[3])
+
+ +

Condition for calculating CANopen Error register, "communication" error bit.

+

See CO_CONFIG_ERR_CONDITION_GENERIC for description.

+

em->errorStatusBits[2] and em->errorStatusBits[3] must be included in the condition, because they are used by the stack.

+ +
+
+ +

◆ CO_CONFIG_ERR_CONDITION_DEV_PROFILE

+ +
+
+ + + + +
#define CO_CONFIG_ERR_CONDITION_DEV_PROFILE
+
+ +

Condition for calculating CANopen Error register, "device profile" error bit.

+

See CO_CONFIG_ERR_CONDITION_GENERIC for description. Macro is not defined by default, so no error is verified.

+ +
+
+ +

◆ CO_CONFIG_ERR_CONDITION_MANUFACTURER

+ +
+
+ + + + +
#define CO_CONFIG_ERR_CONDITION_MANUFACTURER   (em->errorStatusBits[8] || em->errorStatusBits[9])
+
+ +

Condition for calculating CANopen Error register, "manufacturer" error bit.

+

See CO_CONFIG_ERR_CONDITION_GENERIC for description.

+

em->errorStatusBits[8] and em->errorStatusBits[8] are pre-defined, but can be changed.

+ +
+
+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__STACK__CONFIG__EMERGENCY.js b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__STACK__CONFIG__EMERGENCY.js new file mode 100755 index 0000000..50d419c --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__STACK__CONFIG__EMERGENCY.js @@ -0,0 +1,13 @@ +var group__CO__STACK__CONFIG__EMERGENCY = +[ + [ "CO_CONFIG_EM", "group__CO__STACK__CONFIG__EMERGENCY.html#ga16aa1479ffd52a627d1053c20f844b62", null ], + [ "CO_CONFIG_EM_ERR_STATUS_BITS_COUNT", "group__CO__STACK__CONFIG__EMERGENCY.html#gab87776d4802748671b234112263760af", null ], + [ "CO_CONFIG_EM_BUFFER_SIZE", "group__CO__STACK__CONFIG__EMERGENCY.html#ga3c35cf4947c82a0b15afdbfa43a10d67", null ], + [ "CO_CONFIG_ERR_CONDITION_GENERIC", "group__CO__STACK__CONFIG__EMERGENCY.html#gad6270eb7887b22c0365c304d7cf2c633", null ], + [ "CO_CONFIG_ERR_CONDITION_CURRENT", "group__CO__STACK__CONFIG__EMERGENCY.html#ga63af1aaa73297df53b555cb89cd0c07f", null ], + [ "CO_CONFIG_ERR_CONDITION_VOLTAGE", "group__CO__STACK__CONFIG__EMERGENCY.html#ga2b1c3c4f106a8a5d7efda475b469a727", null ], + [ "CO_CONFIG_ERR_CONDITION_TEMPERATURE", "group__CO__STACK__CONFIG__EMERGENCY.html#gaeb96443d9ea2142c346638612fd5c717", null ], + [ "CO_CONFIG_ERR_CONDITION_COMMUNICATION", "group__CO__STACK__CONFIG__EMERGENCY.html#gae47daba892331857e65df82272ed4152", null ], + [ "CO_CONFIG_ERR_CONDITION_DEV_PROFILE", "group__CO__STACK__CONFIG__EMERGENCY.html#gaec2f6161e439dba5376808dcb0cdc36a", null ], + [ "CO_CONFIG_ERR_CONDITION_MANUFACTURER", "group__CO__STACK__CONFIG__EMERGENCY.html#ga3717ce44b5db3189757d874f440adce1", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__STACK__CONFIG__FIFO.html b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__STACK__CONFIG__FIFO.html new file mode 100755 index 0000000..2fdc86d --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__STACK__CONFIG__FIFO.html @@ -0,0 +1,148 @@ + + + + + + + +CANopenNode: FIFO buffer + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Macros
+
+
FIFO buffer
CANopen_301 » Driver » Stack configuration
+
+
+ +

Helper object. +More...

+ + + + + +

+Macros

#define CO_CONFIG_FIFO   (0)
 Configuration of FIFO circular buffer. More...
 
+

Detailed Description

+

Helper object.

+

Macro Definition Documentation

+ +

◆ CO_CONFIG_FIFO

+ +
+
+ + + + +
#define CO_CONFIG_FIFO   (0)
+
+ +

Configuration of FIFO circular buffer.

+

FIFO buffer is basically a simple first-in first-out circular data buffer. It is used by the SDO client and by the CANopen gateway. It has additional advanced functions for data passed to FIFO.

+

Possible flags, can be ORed:

    +
  • CO_CONFIG_FIFO_ENABLE - Enable FIFO buffer
    • +
  • CO_CONFIG_FIFO_ALT_READ - This must be enabled, when SDO client has CO_CONFIG_SDO_CLI_BLOCK enabled. See CO_fifo_altRead().
    • +
  • CO_CONFIG_FIFO_CRC16_CCITT - This must be enabled, when SDO client has CO_CONFIG_SDO_CLI_BLOCK enabled. It enables CRC calculation on data.
    • +
  • CO_CONFIG_FIFO_ASCII_COMMANDS - This must be enabled, when CANopen gateway has CO_CONFIG_GTW_ASCII enabled. It adds command handling functions.
    • +
  • CO_CONFIG_FIFO_ASCII_DATATYPES - This must be enabled, when CANopen gateway has CO_CONFIG_GTW_ASCII and CO_CONFIG_GTW_ASCII_SDO enabled. It adds datatype transform functions between binary and ascii, which are necessary for SDO client.
    • +
+ +
+
+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__STACK__CONFIG__FIFO.js b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__STACK__CONFIG__FIFO.js new file mode 100755 index 0000000..711c47a --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__STACK__CONFIG__FIFO.js @@ -0,0 +1,4 @@ +var group__CO__STACK__CONFIG__FIFO = +[ + [ "CO_CONFIG_FIFO", "group__CO__STACK__CONFIG__FIFO.html#ga055654eb6f93ba05e3534b31626eec3a", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__STACK__CONFIG__GATEWAY.html b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__STACK__CONFIG__GATEWAY.html new file mode 100755 index 0000000..ece64ad --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__STACK__CONFIG__GATEWAY.html @@ -0,0 +1,196 @@ + + + + + + + +CANopenNode: CANopen gateway + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Macros
+
+
CANopen gateway
CANopen_301 » Driver » Stack configuration
+
+
+ +

Specified in standard CiA 309. +More...

+ + + + + + + + + + + + + + +

+Macros

#define CO_CONFIG_GTW   (0)
 Configuration of Gateway ASCII mapping. More...
 
#define CO_CONFIG_GTW_BLOCK_DL_LOOP   1
 Number of loops of CO_SDOclientDownload() in case of block download. More...
 
#define CO_CONFIG_GTWA_COMM_BUF_SIZE   200
 Size of command buffer in ASCII gateway object. More...
 
+#define CO_CONFIG_GTWA_LOG_BUF_SIZE   2000
 Size of message log buffer in ASCII gateway object.
 
+

Detailed Description

+

Specified in standard CiA 309.

+

Macro Definition Documentation

+ +

◆ CO_CONFIG_GTW

+ +
+
+ + + + +
#define CO_CONFIG_GTW   (0)
+
+ +

Configuration of Gateway ASCII mapping.

+

Gateway object is covered by standard CiA 309 - CANopen access from other networks. It enables usage of the NMT master, SDO client and LSS master as a gateway device.

+

Possible flags, can be ORed:

    +
  • CO_CONFIG_GTW_MULTI_NET - Enable multiple network interfaces in gateway device. This functionality is currently not implemented.
    • +
  • CO_CONFIG_GTW_ASCII - Enable gateway device with ASCII mapping (CiA 309-3) If set, then CO_CONFIG_FIFO_ASCII_COMMANDS must also be set.
    • +
  • CO_CONFIG_GTW_ASCII_SDO - Enable SDO client. If set, then CO_CONFIG_FIFO_ASCII_DATATYPES must also be set.
    • +
  • CO_CONFIG_GTW_ASCII_NMT - Enable NMT master
    • +
  • CO_CONFIG_GTW_ASCII_LSS - Enable LSS master
    • +
  • CO_CONFIG_GTW_ASCII_LOG - Enable non-standard message log read
    • +
  • CO_CONFIG_GTW_ASCII_ERROR_DESC - Print error description as additional comments in gateway-ascii device for SDO and gateway errors.
    • +
  • CO_CONFIG_GTW_ASCII_PRINT_HELP - use non-standard command "help" to print help usage.
    • +
  • CO_CONFIG_GTW_ASCII_PRINT_LEDS - Display "red" and "green" CANopen status LED diodes on terminal.
    • +
+ +
+
+ +

◆ CO_CONFIG_GTW_BLOCK_DL_LOOP

+ +
+
+ + + + +
#define CO_CONFIG_GTW_BLOCK_DL_LOOP   1
+
+ +

Number of loops of CO_SDOclientDownload() in case of block download.

+

If SDO clint has block download in progress and OS has buffer for CAN tx messages, then CO_SDOclientDownload() functionion can be called multiple times within own loop (up to 127). This can speed-up SDO block transfer.

+ +
+
+ +

◆ CO_CONFIG_GTWA_COMM_BUF_SIZE

+ +
+
+ + + + +
#define CO_CONFIG_GTWA_COMM_BUF_SIZE   200
+
+ +

Size of command buffer in ASCII gateway object.

+

If large amount of data is transferred (block transfer), then this should be increased to 1000 or more. Buffer may be refilled between the block transfer.

+ +
+
+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__STACK__CONFIG__GATEWAY.js b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__STACK__CONFIG__GATEWAY.js new file mode 100755 index 0000000..dd1030b --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__STACK__CONFIG__GATEWAY.js @@ -0,0 +1,7 @@ +var group__CO__STACK__CONFIG__GATEWAY = +[ + [ "CO_CONFIG_GTW", "group__CO__STACK__CONFIG__GATEWAY.html#ga9af15f76cd14fece499764499c6bc2d3", null ], + [ "CO_CONFIG_GTW_BLOCK_DL_LOOP", "group__CO__STACK__CONFIG__GATEWAY.html#gaa864e7c6e7ebd3fc7ce424dc3c94db9d", null ], + [ "CO_CONFIG_GTWA_COMM_BUF_SIZE", "group__CO__STACK__CONFIG__GATEWAY.html#ga7903ae4ca7939fc32bd747224e868a38", null ], + [ "CO_CONFIG_GTWA_LOG_BUF_SIZE", "group__CO__STACK__CONFIG__GATEWAY.html#ga4f471dca1341879dc56c2e0a2c73cb29", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__STACK__CONFIG__LEDS.html b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__STACK__CONFIG__LEDS.html new file mode 100755 index 0000000..aa356dc --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__STACK__CONFIG__LEDS.html @@ -0,0 +1,144 @@ + + + + + + + +CANopenNode: CANopen LED diodes + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Macros
+
+
CANopen LED diodes
CANopen_301 » Driver » Stack configuration
+
+
+ +

Specified in standard CiA 303-3. +More...

+ + + + + +

+Macros

#define CO_CONFIG_LEDS   (CO_CONFIG_LEDS_ENABLE)
 Configuration of LED indicators. More...
 
+

Detailed Description

+

Specified in standard CiA 303-3.

+

Macro Definition Documentation

+ +

◆ CO_CONFIG_LEDS

+ +
+
+ + + + +
#define CO_CONFIG_LEDS   (CO_CONFIG_LEDS_ENABLE)
+
+ +

Configuration of LED indicators.

+

Possible flags, can be ORed:

+ +
+
+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__STACK__CONFIG__LEDS.js b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__STACK__CONFIG__LEDS.js new file mode 100755 index 0000000..75973db --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__STACK__CONFIG__LEDS.js @@ -0,0 +1,4 @@ +var group__CO__STACK__CONFIG__LEDS = +[ + [ "CO_CONFIG_LEDS", "group__CO__STACK__CONFIG__LEDS.html#ga423160131d618b5d57bc7c016ee369fd", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__STACK__CONFIG__LSS.html b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__STACK__CONFIG__LSS.html new file mode 100755 index 0000000..8a253cb --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__STACK__CONFIG__LSS.html @@ -0,0 +1,146 @@ + + + + + + + +CANopenNode: LSS master/slave + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Macros
+
+
LSS master/slave
CANopen_301 » Driver » Stack configuration
+
+
+ +

Specified in standard CiA 305. +More...

+ + + + + +

+Macros

#define CO_CONFIG_LSS   (CO_CONFIG_LSS_SLAVE)
 Configuration of LSS. More...
 
+

Detailed Description

+

Specified in standard CiA 305.

+

Macro Definition Documentation

+ +

◆ CO_CONFIG_LSS

+ +
+
+ + + + +
#define CO_CONFIG_LSS   (CO_CONFIG_LSS_SLAVE)
+
+ +

Configuration of LSS.

+

Possible flags, can be ORed:

    +
  • CO_CONFIG_LSS_SLAVE - Enable LSS slave
    • +
  • CO_CONFIG_LSS_SLAVE_FASTSCAN_DIRECT_RESPOND - Send LSS fastscan respond directly from CO_LSSslave_receive() function.
    • +
  • CO_CONFIG_LSS_MASTER - Enable LSS master
    • +
  • CO_CONFIG_FLAG_CALLBACK_PRE - Enable custom callback after preprocessing received CAN message. Callback is configured by CO_LSSmaster_initCallbackPre().
    • +
+ +
+
+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__STACK__CONFIG__LSS.js b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__STACK__CONFIG__LSS.js new file mode 100755 index 0000000..2deb5fc --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__STACK__CONFIG__LSS.js @@ -0,0 +1,4 @@ +var group__CO__STACK__CONFIG__LSS = +[ + [ "CO_CONFIG_LSS", "group__CO__STACK__CONFIG__LSS.html#gafeb75d750efb0879fe11a5482b6629f3", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__STACK__CONFIG__NMT__HB.html b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__STACK__CONFIG__NMT__HB.html new file mode 100755 index 0000000..4358c21 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__STACK__CONFIG__NMT__HB.html @@ -0,0 +1,189 @@ + + + + + + + +CANopenNode: NMT master/slave and HB producer/consumer + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Macros
+
+
NMT master/slave and HB producer/consumer
CANopen_301 » Driver » Stack configuration
+
+
+ + + + + + + + + + + +

+Macros

#define CO_CONFIG_NMT   (0)
 Configuration of NMT and Heartbeat. More...
 
#define CO_CONFIG_HB_CONS   (CO_CONFIG_HB_CONS_ENABLE)
 Configuration of Heartbeat consumer. More...
 
#define CO_CONFIG_HB_CONS_SIZE   8
 Number of heartbeat consumer objects, where each object corresponds to one sub-index in OD object 0x1016, "Consumer heartbeat time". More...
 
+

Detailed Description

+

Macro Definition Documentation

+ +

◆ CO_CONFIG_NMT

+ +
+
+ + + + +
#define CO_CONFIG_NMT   (0)
+
+ +

Configuration of NMT and Heartbeat.

+

Possible flags, can be ORed:

+ +
+
+ +

◆ CO_CONFIG_HB_CONS

+ +
+
+ + + + +
#define CO_CONFIG_HB_CONS   (CO_CONFIG_HB_CONS_ENABLE)
+
+ +

Configuration of Heartbeat consumer.

+

Possible flags, can be ORed:

+ +
+
+ +

◆ CO_CONFIG_HB_CONS_SIZE

+ +
+
+ + + + +
#define CO_CONFIG_HB_CONS_SIZE   8
+
+ +

Number of heartbeat consumer objects, where each object corresponds to one sub-index in OD object 0x1016, "Consumer heartbeat time".

+

If heartbeat consumer is enabled, then valid values are 1 to 127.

+ +
+
+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__STACK__CONFIG__NMT__HB.js b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__STACK__CONFIG__NMT__HB.js new file mode 100755 index 0000000..1c6d427 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__STACK__CONFIG__NMT__HB.js @@ -0,0 +1,6 @@ +var group__CO__STACK__CONFIG__NMT__HB = +[ + [ "CO_CONFIG_NMT", "group__CO__STACK__CONFIG__NMT__HB.html#gafa3b1f1b4931175bf9c67a5d45633e76", null ], + [ "CO_CONFIG_HB_CONS", "group__CO__STACK__CONFIG__NMT__HB.html#ga7368d68cb039983bc8cc164410877098", null ], + [ "CO_CONFIG_HB_CONS_SIZE", "group__CO__STACK__CONFIG__NMT__HB.html#ga0cbe9ab929ff9d122ab6727d66fe7752", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__STACK__CONFIG__SDO.html b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__STACK__CONFIG__SDO.html new file mode 100755 index 0000000..926e01a --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__STACK__CONFIG__SDO.html @@ -0,0 +1,211 @@ + + + + + + + +CANopenNode: SDO server/client + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Macros
+
+
SDO server/client
CANopen_301 » Driver » Stack configuration
+
+
+ + + + + + + + + + + + + + +

+Macros

#define CO_CONFIG_SDO_SRV   (CO_CONFIG_SDO_SRV_SEGMENTED)
 Configuration of SDO server. More...
 
#define CO_CONFIG_SDO_SRV_BUFFER_SIZE   32
 Size of the internal data buffer for the SDO server. More...
 
#define CO_CONFIG_SDO_CLI   (0)
 Configuration of SDO client. More...
 
#define CO_CONFIG_SDO_CLI_BUFFER_SIZE   32
 Size of the internal data buffer for the SDO client. More...
 
+

Detailed Description

+

Macro Definition Documentation

+ +

◆ CO_CONFIG_SDO_SRV

+ +
+
+ + + + +
#define CO_CONFIG_SDO_SRV   (CO_CONFIG_SDO_SRV_SEGMENTED)
+
+ +

Configuration of SDO server.

+

Possible flags, can be ORed:

+ +
+
+ +

◆ CO_CONFIG_SDO_SRV_BUFFER_SIZE

+ +
+
+ + + + +
#define CO_CONFIG_SDO_SRV_BUFFER_SIZE   32
+
+ +

Size of the internal data buffer for the SDO server.

+

If size is less than size of some variables in Object Dictionary, then data will be transferred to internal buffer in several segments. Minimum size is 8 or 899 (127*7) for block transfer.

+ +
+
+ +

◆ CO_CONFIG_SDO_CLI

+ +
+
+ + + + +
#define CO_CONFIG_SDO_CLI   (0)
+
+ +

Configuration of SDO client.

+

Possible flags, can be ORed:

+ +
+
+ +

◆ CO_CONFIG_SDO_CLI_BUFFER_SIZE

+ +
+
+ + + + +
#define CO_CONFIG_SDO_CLI_BUFFER_SIZE   32
+
+ +

Size of the internal data buffer for the SDO client.

+

Circular buffer is used for SDO communication. It can be read or written between successive SDO calls. So size of the buffer can be lower than size of the actual size of data transferred. If only segmented transfer is used, then buffer size can be as low as 7 bytes, if data are read/written each cycle. If block transfer is used, buffer size should be set to at least 1000 bytes, so maximum blksize can be used (blksize is calculated from free buffer space). Default value for block transfer is 1000, otherwise 32.

+ +
+
+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__STACK__CONFIG__SDO.js b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__STACK__CONFIG__SDO.js new file mode 100755 index 0000000..e9556dd --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__STACK__CONFIG__SDO.js @@ -0,0 +1,7 @@ +var group__CO__STACK__CONFIG__SDO = +[ + [ "CO_CONFIG_SDO_SRV", "group__CO__STACK__CONFIG__SDO.html#ga2928cc23dd27138821d48c2fb3e24222", null ], + [ "CO_CONFIG_SDO_SRV_BUFFER_SIZE", "group__CO__STACK__CONFIG__SDO.html#gacad3d0d9060469aedcb9e058c1883375", null ], + [ "CO_CONFIG_SDO_CLI", "group__CO__STACK__CONFIG__SDO.html#gac8ee65cd62dbee2982f5304513402a57", null ], + [ "CO_CONFIG_SDO_CLI_BUFFER_SIZE", "group__CO__STACK__CONFIG__SDO.html#ga763b09ab827365e46f10234bd9c0acfd", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__STACK__CONFIG__SRDO.html b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__STACK__CONFIG__SRDO.html new file mode 100755 index 0000000..a191ab8 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__STACK__CONFIG__SRDO.html @@ -0,0 +1,192 @@ + + + + + + + +CANopenNode: Safety Related Data Objects (SRDO) + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Macros
+
+
Safety Related Data Objects (SRDO)
CANopen_301 » Driver » Stack configuration
+
+
+ +

Specified in standard EN 50325-5 (CiA 304) +More...

+ + + + + + + + + + + +

+Macros

#define CO_CONFIG_GFC   (0)
 Configuration of GFC. More...
 
#define CO_CONFIG_SRDO   (0)
 Configuration of SRDO. More...
 
#define CO_CONFIG_SRDO_MINIMUM_DELAY   0
 SRDO Tx time delay. More...
 
+

Detailed Description

+

Specified in standard EN 50325-5 (CiA 304)

+

Macro Definition Documentation

+ +

◆ CO_CONFIG_GFC

+ +
+
+ + + + +
#define CO_CONFIG_GFC   (0)
+
+ +

Configuration of GFC.

+

Possible flags, can be ORed:

    +
  • CO_CONFIG_GFC_ENABLE - Enable the GFC object
    • +
  • CO_CONFIG_GFC_CONSUMER - Enable the GFC consumer
    • +
  • CO_CONFIG_GFC_PRODUCER - Enable the GFC producer
    • +
+ +
+
+ +

◆ CO_CONFIG_SRDO

+ +
+
+ + + + +
#define CO_CONFIG_SRDO   (0)
+
+ +

Configuration of SRDO.

+

Possible flags, can be ORed:

    +
  • CO_CONFIG_SRDO_ENABLE - Enable the SRDO object.
    • +
  • CO_CONFIG_SRDO_CHECK_TX - Enable checking data before sending.
    • +
  • CO_CONFIG_RSRDO_CALLS_EXTENSION - Enable calling configured extension callbacks when received RSRDO CAN message modifies OD entries.
    • +
  • CO_CONFIG_TRSRDO_CALLS_EXTENSION - Enable calling configured extension callbacks before TSRDO CAN message is sent.
    • +
  • CO_CONFIG_FLAG_CALLBACK_PRE - Enable custom callback after preprocessing received RSRDO CAN message. Callback is configured by CO_SRDO_initCallbackPre().
    • +
  • CO_CONFIG_FLAG_TIMERNEXT - Enable calculation of timerNext_us variable inside CO_SRDO_process() (Tx SRDO only).
    • +
+ +
+
+ +

◆ CO_CONFIG_SRDO_MINIMUM_DELAY

+ +
+
+ + + + +
#define CO_CONFIG_SRDO_MINIMUM_DELAY   0
+
+ +

SRDO Tx time delay.

+

minimum time between the first and second SRDO (Tx) message in us

+ +
+
+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__STACK__CONFIG__SRDO.js b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__STACK__CONFIG__SRDO.js new file mode 100755 index 0000000..430b1fe --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__STACK__CONFIG__SRDO.js @@ -0,0 +1,6 @@ +var group__CO__STACK__CONFIG__SRDO = +[ + [ "CO_CONFIG_GFC", "group__CO__STACK__CONFIG__SRDO.html#ga71d11e8460a5410be21863a0f99cbab2", null ], + [ "CO_CONFIG_SRDO", "group__CO__STACK__CONFIG__SRDO.html#ga61645e6ad8a02e356abde012434bedf9", null ], + [ "CO_CONFIG_SRDO_MINIMUM_DELAY", "group__CO__STACK__CONFIG__SRDO.html#gaebb5427a155133b50622e60acdd0e650", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__STACK__CONFIG__SYNC__PDO.html b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__STACK__CONFIG__SYNC__PDO.html new file mode 100755 index 0000000..9a72435 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__STACK__CONFIG__SYNC__PDO.html @@ -0,0 +1,170 @@ + + + + + + + +CANopenNode: SYNC and PDO producer/consumer + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Macros
+
+
SYNC and PDO producer/consumer
CANopen_301 » Driver » Stack configuration
+
+
+ + + + + + + + +

+Macros

#define CO_CONFIG_SYNC   (CO_CONFIG_SYNC_ENABLE | CO_CONFIG_SYNC_PRODUCER)
 Configuration of SYNC. More...
 
#define CO_CONFIG_PDO   (CO_CONFIG_RPDO_ENABLE | CO_CONFIG_TPDO_ENABLE | CO_CONFIG_PDO_SYNC_ENABLE)
 Configuration of PDO. More...
 
+

Detailed Description

+

Macro Definition Documentation

+ +

◆ CO_CONFIG_SYNC

+ +
+
+ + + + +
#define CO_CONFIG_SYNC   (CO_CONFIG_SYNC_ENABLE | CO_CONFIG_SYNC_PRODUCER)
+
+ +

Configuration of SYNC.

+

Possible flags, can be ORed:

+ +
+
+ +

◆ CO_CONFIG_PDO

+ +
+
+ + + + +
#define CO_CONFIG_PDO   (CO_CONFIG_RPDO_ENABLE | CO_CONFIG_TPDO_ENABLE | CO_CONFIG_PDO_SYNC_ENABLE)
+
+ +

Configuration of PDO.

+

Possible flags, can be ORed:

    +
  • CO_CONFIG_RPDO_ENABLE - Enable receive PDO objects.
    • +
  • CO_CONFIG_TPDO_ENABLE - Enable transmit PDO objects.
    • +
  • CO_CONFIG_PDO_SYNC_ENABLE - Enable SYNC in PDO objects.
    • +
  • CO_CONFIG_RPDO_CALLS_EXTENSION - Enable calling configured extension callbacks when received RPDO CAN message modifies OD entries.
    • +
  • CO_CONFIG_TPDO_CALLS_EXTENSION - Enable calling configured extension callbacks before TPDO CAN message is sent.
    • +
  • CO_CONFIG_FLAG_CALLBACK_PRE - Enable custom callback after preprocessing received RPDO CAN message. Callback is configured by CO_RPDO_initCallbackPre().
    • +
  • CO_CONFIG_FLAG_TIMERNEXT - Enable calculation of timerNext_us variable inside CO_TPDO_process().
    • +
+ +
+
+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__STACK__CONFIG__SYNC__PDO.js b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__STACK__CONFIG__SYNC__PDO.js new file mode 100755 index 0000000..947ed69 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__STACK__CONFIG__SYNC__PDO.js @@ -0,0 +1,5 @@ +var group__CO__STACK__CONFIG__SYNC__PDO = +[ + [ "CO_CONFIG_SYNC", "group__CO__STACK__CONFIG__SYNC__PDO.html#ga7d1d2210fdf2b916ca1d82c4933856bc", null ], + [ "CO_CONFIG_PDO", "group__CO__STACK__CONFIG__SYNC__PDO.html#gaa20d1b49249b7f5a15963cc1a4611be9", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__STACK__CONFIG__TIME.html b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__STACK__CONFIG__TIME.html new file mode 100755 index 0000000..c8f1d61 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__STACK__CONFIG__TIME.html @@ -0,0 +1,141 @@ + + + + + + + +CANopenNode: Time producer/consumer + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Macros
+
+
Time producer/consumer
CANopen_301 » Driver » Stack configuration
+
+
+ + + + + +

+Macros

#define CO_CONFIG_TIME   (0)
 Configuration of TIME. More...
 
+

Detailed Description

+

Macro Definition Documentation

+ +

◆ CO_CONFIG_TIME

+ +
+
+ + + + +
#define CO_CONFIG_TIME   (0)
+
+ +

Configuration of TIME.

+

Possible flags, can be ORed:

    +
  • CO_CONFIG_TIME_ENABLE - Enable TIME object and TIME consumer.
    • +
  • CO_CONFIG_TIME_PRODUCER - Enable TIME producer.
    • +
  • CO_CONFIG_FLAG_CALLBACK_PRE - Enable custom callback after preprocessing received TIME CAN message. Callback is configured by CO_TIME_initCallbackPre().
    • +
+ +
+
+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__STACK__CONFIG__TIME.js b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__STACK__CONFIG__TIME.js new file mode 100755 index 0000000..18469a3 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__STACK__CONFIG__TIME.js @@ -0,0 +1,4 @@ +var group__CO__STACK__CONFIG__TIME = +[ + [ "CO_CONFIG_TIME", "group__CO__STACK__CONFIG__TIME.html#gaba4a59929bbd8512ca954ba8fcf1dfe6", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__STACK__CONFIG__TRACE.html b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__STACK__CONFIG__TRACE.html new file mode 100755 index 0000000..9392f36 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__STACK__CONFIG__TRACE.html @@ -0,0 +1,144 @@ + + + + + + + +CANopenNode: Trace recorder + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Macros
+
+
Trace recorder
CANopen_301 » Driver » Stack configuration
+
+
+ +

Non standard object. +More...

+ + + + + +

+Macros

#define CO_CONFIG_TRACE   (0)
 Configuration of Trace for recording variables over time. More...
 
+

Detailed Description

+

Non standard object.

+

Macro Definition Documentation

+ +

◆ CO_CONFIG_TRACE

+ +
+
+ + + + +
#define CO_CONFIG_TRACE   (0)
+
+ +

Configuration of Trace for recording variables over time.

+

Possible flags, can be ORed:

    +
  • CO_CONFIG_TRACE_ENABLE - Enable Trace recorder
    • +
  • CO_CONFIG_TRACE_OWN_INTTYPES - If set, then macros PRIu32("u" or "lu") and PRId32("d" or "ld") must be set. (File inttypes.h can not be included).
    • +
+ +
+
+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__STACK__CONFIG__TRACE.js b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__STACK__CONFIG__TRACE.js new file mode 100755 index 0000000..c0dd586 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__STACK__CONFIG__TRACE.js @@ -0,0 +1,4 @@ +var group__CO__STACK__CONFIG__TRACE = +[ + [ "CO_CONFIG_TRACE", "group__CO__STACK__CONFIG__TRACE.html#ga9d4e333d0b599c2369366defc6ce5e62", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__SYNC.html b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__SYNC.html new file mode 100755 index 0000000..09e4409 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__SYNC.html @@ -0,0 +1,413 @@ + + + + + + + +CANopenNode: SYNC + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Files | +Data Structures | +Enumerations | +Functions
+
+
SYNC
CANopen_301
+
+
+ + + + + +

+Files

file  CO_SYNC.h
 CANopen Synchronisation protocol.
 
+ + + + +

+Data Structures

struct  CO_SYNC_t
 SYNC producer and consumer object. More...
 
+ + + + +

+Enumerations

enum  CO_SYNC_status_t { CO_SYNC_NONE = 0, +CO_SYNC_RECEIVED = 1, +CO_SYNC_OUTSIDE_WINDOW = 2 + }
 Return value for CO_SYNC_process. More...
 
+ + + + + + + + + + + + + +

+Functions

CO_ReturnError_t CO_SYNC_init (CO_SYNC_t *SYNC, CO_EM_t *em, CO_SDO_t *SDO, CO_NMT_internalState_t *operatingState, uint32_t COB_ID_SYNCMessage, uint32_t communicationCyclePeriod, uint8_t synchronousCounterOverflowValue, CO_CANmodule_t *CANdevRx, uint16_t CANdevRxIdx, CO_CANmodule_t *CANdevTx, uint16_t CANdevTxIdx)
 Initialize SYNC object. More...
 
void CO_SYNC_initCallbackPre (CO_SYNC_t *SYNC, void *object, void(*pFunctSignalPre)(void *object))
 Initialize SYNC callback function. More...
 
CO_ReturnError_t CO_SYNCsend (CO_SYNC_t *SYNC)
 Send SYNC message. More...
 
CO_SYNC_status_t CO_SYNC_process (CO_SYNC_t *SYNC, uint32_t timeDifference_us, uint32_t ObjDict_synchronousWindowLength, uint32_t *timerNext_us)
 Process SYNC communication. More...
 
+

Detailed Description

+

CANopen Synchronisation protocol.

+

For CAN identifier see CO_Default_CAN_ID_t

+

SYNC message is used for synchronization of the nodes on network. One node can be SYNC producer, others can be SYNC consumers. Synchronous TPDOs are transmitted after the CANopen SYNC message. Synchronous received PDOs are accepted(copied to OD) immediatelly after the reception of the next SYNC message.

+

Contents of SYNC message

+

By default SYNC message has no data. If Synchronous counter overflow value from Object dictionary (index 0x1019) is different than 0, SYNC message has one data byte: counter incremented by 1 with every SYNC transmission.

+

SYNC in CANopenNode

+

According to CANopen, synchronous RPDOs must be processed after reception of the next sync messsage. For that reason, there is a double receive buffer for each synchronous RPDO. At the moment, when SYNC is received or transmitted, internal variable CANrxToggle toggles. That variable is then used by synchronous RPDO to determine, which of the two buffers is used for RPDO reception and which for RPDO processing.

+

Enumeration Type Documentation

+ +

◆ CO_SYNC_status_t

+ +
+
+ + + + +
enum CO_SYNC_status_t
+
+ +

Return value for CO_SYNC_process.

+ + + + +
Enumerator
CO_SYNC_NONE 

SYNC not received.

+
CO_SYNC_RECEIVED 

SYNC received.

+
CO_SYNC_OUTSIDE_WINDOW 

SYNC received outside SYNC window.

+
+ +
+
+

Function Documentation

+ +

◆ CO_SYNC_init()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
CO_ReturnError_t CO_SYNC_init (CO_SYNC_t * SYNC,
CO_EM_t * em,
CO_SDO_t * SDO,
CO_NMT_internalState_t * operatingState,
uint32_t COB_ID_SYNCMessage,
uint32_t communicationCyclePeriod,
uint8_t synchronousCounterOverflowValue,
CO_CANmodule_t * CANdevRx,
uint16_t CANdevRxIdx,
CO_CANmodule_t * CANdevTx,
uint16_t CANdevTxIdx 
)
+
+ +

Initialize SYNC object.

+

Function must be called in the communication reset section.

+
Parameters
+ + + + + + + + + + + + +
SYNCThis object will be initialized.
emEmergency object.
SDOSDO server object.
operatingStatePointer to variable indicating CANopen device NMT internal state.
COB_ID_SYNCMessageFrom Object dictionary (index 0x1005).
communicationCyclePeriodFrom Object dictionary (index 0x1006).
synchronousCounterOverflowValueFrom Object dictionary (index 0x1019).
CANdevRxCAN device for SYNC reception.
CANdevRxIdxIndex of receive buffer in the above CAN device.
CANdevTxCAN device for SYNC transmission.
CANdevTxIdxIndex of transmit buffer in the above CAN device.
+
+
+
Returns
CO_ReturnError_t: CO_ERROR_NO or CO_ERROR_ILLEGAL_ARGUMENT.
+ +
+
+ +

◆ CO_SYNC_initCallbackPre()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + +
void CO_SYNC_initCallbackPre (CO_SYNC_t * SYNC,
void * object,
void(*)(void *object) pFunctSignalPre 
)
+
+ +

Initialize SYNC callback function.

+

Function initializes optional callback function, which should immediately start processing of CO_SYNC_process() function. Callback is called after SYNC message is received from the CAN bus.

+
Parameters
+ + + + +
SYNCThis object.
objectPointer to object, which will be passed to pFunctSignalPre(). Can be NULL
pFunctSignalPrePointer to the callback function. Not called if NULL.
+
+
+ +
+
+ +

◆ CO_SYNCsend()

+ +
+
+ + + + + + + + +
CO_ReturnError_t CO_SYNCsend (CO_SYNC_t * SYNC)
+
+ +

Send SYNC message.

+

This function prepares and sends a SYNC object. The application should only call this if direct control of SYNC transmission is needed, otherwise use CO_SYNC_process().

+
Parameters
+ + +
SYNCSYNC object.
+
+
+
Returns
Same as CO_CANsend().
+ +
+
+ +

◆ CO_SYNC_process()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
CO_SYNC_status_t CO_SYNC_process (CO_SYNC_t * SYNC,
uint32_t timeDifference_us,
uint32_t ObjDict_synchronousWindowLength,
uint32_t * timerNext_us 
)
+
+ +

Process SYNC communication.

+

Function must be called cyclically.

+
Parameters
+ + + + + +
SYNCThis object.
timeDifference_usTime difference from previous function call in [microseconds].
ObjDict_synchronousWindowLengthSynchronous window length variable from Object dictionary (index 0x1007).
[out]timerNext_usinfo to OS - see CO_process_SYNC_PDO().
+
+
+
Returns
CO_SYNC_status_t: CO_SYNC_NONE, CO_SYNC_RECEIVED or CO_SYNC_OUTSIDE_WINDOW.
+ +
+
+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__SYNC.js b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__SYNC.js new file mode 100755 index 0000000..8bc3f12 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__SYNC.js @@ -0,0 +1,35 @@ +var group__CO__SYNC = +[ + [ "CO_SYNC.h", "CO__SYNC_8h.html", null ], + [ "CO_SYNC_t", "structCO__SYNC__t.html", [ + [ "em", "structCO__SYNC__t.html#acf987cc40eb5f005a92f10210353ea1f", null ], + [ "operatingState", "structCO__SYNC__t.html#a70fea8996ebfbe7d163bae11201dac6e", null ], + [ "isProducer", "structCO__SYNC__t.html#af37a656db91d31a8187e0350f472ea36", null ], + [ "COB_ID", "structCO__SYNC__t.html#af528cdc487bdaee3dfc0a3baa3f7c7cc", null ], + [ "periodTime", "structCO__SYNC__t.html#a15337541e9b3defa2dbae2df3dd01e72", null ], + [ "periodTimeoutTime", "structCO__SYNC__t.html#a9f612a7f9d691edeef11d8df9cf166a7", null ], + [ "counterOverflowValue", "structCO__SYNC__t.html#aee5eb3e245e54e509d2f2cc05cf4a664", null ], + [ "curentSyncTimeIsInsideWindow", "structCO__SYNC__t.html#adbc85ba8f5f4cbca0caa645216204a88", null ], + [ "CANrxNew", "structCO__SYNC__t.html#afa9120621a777413be033005eb43e771", null ], + [ "CANrxToggle", "structCO__SYNC__t.html#a25aeadeddcae8209b6b18034036a0aaf", null ], + [ "counter", "structCO__SYNC__t.html#a0977c2f09f69755e4b53df68bb1fee0b", null ], + [ "timer", "structCO__SYNC__t.html#a491f176a5fb70400647474555628bf6f", null ], + [ "receiveError", "structCO__SYNC__t.html#ae108ac75f1fb7c797393646cc6c494af", null ], + [ "pFunctSignalPre", "structCO__SYNC__t.html#ac56a3411d0cd312e51d905937c628b6f", null ], + [ "functSignalObjectPre", "structCO__SYNC__t.html#a6a219b1fd9d3382131878af1f016b83d", null ], + [ "CANdevRx", "structCO__SYNC__t.html#ae799680e430d4d6b05d7a3da216f06be", null ], + [ "CANdevRxIdx", "structCO__SYNC__t.html#a1a489d5fd447a8b5e16fb82d957d0667", null ], + [ "CANdevTx", "structCO__SYNC__t.html#a3b89fbc55cce155f500bbda463b61d8a", null ], + [ "CANtxBuff", "structCO__SYNC__t.html#a79d41eeaf724741d0ed281ab3b6a95ef", null ], + [ "CANdevTxIdx", "structCO__SYNC__t.html#ad07d96af0baa18907d4865ecf244d420", null ] + ] ], + [ "CO_SYNC_status_t", "group__CO__SYNC.html#ga121ede6e0c90c66076a7ed950db38517", [ + [ "CO_SYNC_NONE", "group__CO__SYNC.html#gga121ede6e0c90c66076a7ed950db38517aeff7846423b9eb92cd2c69df745ea429", null ], + [ "CO_SYNC_RECEIVED", "group__CO__SYNC.html#gga121ede6e0c90c66076a7ed950db38517aa03c21a78b503a4adebb2d9d7aa655bf", null ], + [ "CO_SYNC_OUTSIDE_WINDOW", "group__CO__SYNC.html#gga121ede6e0c90c66076a7ed950db38517a5b112bec6cb10119879703a6313de41e", null ] + ] ], + [ "CO_SYNC_init", "group__CO__SYNC.html#ga3047b679d5e3261eedf1980ea87b5135", null ], + [ "CO_SYNC_initCallbackPre", "group__CO__SYNC.html#gaf1005766c7f1588262b018fe04960777", null ], + [ "CO_SYNCsend", "group__CO__SYNC.html#gabbc8625d068d19d8b632d034f396a1ff", null ], + [ "CO_SYNC_process", "group__CO__SYNC.html#ga66b8f42fd430daa2a57ff15aa49c814c", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__TIME.html b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__TIME.html new file mode 100755 index 0000000..e4d7146 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__TIME.html @@ -0,0 +1,344 @@ + + + + + + + +CANopenNode: TIME + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Files | +Data Structures | +Functions
+
+
TIME
CANopen_301
+
+
+ + + + + +

+Files

file  CO_TIME.h
 CANopen Time-stamp protocol.
 
+ + + + +

+Data Structures

struct  CO_TIME_t
 TIME producer and consumer object. More...
 
+ + + + + + + + + + +

+Functions

CO_ReturnError_t CO_TIME_init (CO_TIME_t *TIME, CO_EM_t *em, CO_SDO_t *SDO, CO_NMT_internalState_t *operatingState, uint32_t COB_ID_TIMEMessage, uint32_t TIMECyclePeriod, CO_CANmodule_t *CANdevRx, uint16_t CANdevRxIdx, CO_CANmodule_t *CANdevTx, uint16_t CANdevTxIdx)
 Initialize TIME object. More...
 
void CO_TIME_initCallbackPre (CO_TIME_t *TIME, void *object, void(*pFunctSignalPre)(void *object))
 Initialize TIME callback function. More...
 
uint8_t CO_TIME_process (CO_TIME_t *TIME, uint32_t timeDifference_us)
 Process TIME communication. More...
 
+

Detailed Description

+

CANopen Time-stamp protocol.

+

For CAN identifier see CO_Default_CAN_ID_t

+

TIME message is used for time synchronization of the nodes on network. One node should be TIME producer, others can be TIME consumers. This is configured with COB_ID_TIME object 0x1012 :

+
    +
  • bit 31 should be set for a consumer
    • +
  • bit 30 should be set for a producer
    • +
+

TIME CONSUMER

+

CO_TIME_init() configuration :

    +
  • COB_ID_TIME : 0x80000100L -> TIME consumer with TIME_COB_ID = 0x100
    • +
  • TIMECyclePeriod :
      +
    • 0 -> no EMCY will be transmitted in case of TIME timeout
      • +
    • X -> an EMCY will be transmitted in case of TIME timeout (X * 1.5) ms
      • +
    +
    • +
+

Latest time value is stored in CO->TIME->Time variable.

+

TIME PRODUCER

+

CO_TIME_init() configuration :

    +
  • COB_ID_TIME : 0x40000100L -> TIME producer with TIME_COB_ID = 0x100
    • +
  • TIMECyclePeriod : Time transmit period in ms
    • +
+

Write time value in CO->TIME->Time variable, this will be sent at TIMECyclePeriod.

+

Function Documentation

+ +

◆ CO_TIME_init()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
CO_ReturnError_t CO_TIME_init (CO_TIME_t * TIME,
CO_EM_t * em,
CO_SDO_t * SDO,
CO_NMT_internalState_t * operatingState,
uint32_t COB_ID_TIMEMessage,
uint32_t TIMECyclePeriod,
CO_CANmodule_t * CANdevRx,
uint16_t CANdevRxIdx,
CO_CANmodule_t * CANdevTx,
uint16_t CANdevTxIdx 
)
+
+ +

Initialize TIME object.

+

Function must be called in the communication reset section.

+
Parameters
+ + + + + + + + + + + +
TIMEThis object will be initialized.
emEmergency object.
SDOSDO server object.
operatingStatePointer to variable indicating CANopen device NMT internal state.
COB_ID_TIMEMessageShould be intialized with CO_CAN_ID_TIME_STAMP
TIMECyclePeriodTIME period in ms (may also be used in consumer mode for timeout detection (1.5x period)).
CANdevRxCAN device for TIME reception.
CANdevRxIdxIndex of receive buffer in the above CAN device.
CANdevTxCAN device for TIME transmission.
CANdevTxIdxIndex of transmit buffer in the above CAN device.
+
+
+
Returns
CO_ReturnError_t: CO_ERROR_NO or CO_ERROR_ILLEGAL_ARGUMENT.
+ +
+
+ +

◆ CO_TIME_initCallbackPre()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + +
void CO_TIME_initCallbackPre (CO_TIME_t * TIME,
void * object,
void(*)(void *object) pFunctSignalPre 
)
+
+ +

Initialize TIME callback function.

+

Function initializes optional callback function, which should immediately start processing of CO_TIME_process() function. Callback is called after TIME message is received from the CAN bus.

+
Parameters
+ + + + +
TIMEThis object.
objectPointer to object, which will be passed to pFunctSignalPre(). Can be NULL
pFunctSignalPrePointer to the callback function. Not called if NULL.
+
+
+ +
+
+ +

◆ CO_TIME_process()

+ +
+
+ + + + + + + + + + + + + + + + + + +
uint8_t CO_TIME_process (CO_TIME_t * TIME,
uint32_t timeDifference_us 
)
+
+ +

Process TIME communication.

+

Function must be called cyclically.

+
Parameters
+ + + +
TIMEThis object.
timeDifference_usTime difference from previous function call in [microseconds].
+
+
+
Returns
0: No special meaning.
+
+1: New TIME message recently received (consumer) / transmited (producer).
+ +
+
+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__TIME.js b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__TIME.js new file mode 100755 index 0000000..f966066 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__TIME.js @@ -0,0 +1,26 @@ +var group__CO__TIME = +[ + [ "CO_TIME.h", "CO__TIME_8h.html", null ], + [ "CO_TIME_t", "structCO__TIME__t.html", [ + [ "em", "structCO__TIME__t.html#ad38fd490eece812c27048a0ffde75c9e", null ], + [ "operatingState", "structCO__TIME__t.html#a2afb9ad74a04332b644669c7fdb187ac", null ], + [ "isConsumer", "structCO__TIME__t.html#aeea1ecdf76e04f37c1d760eee72a1395", null ], + [ "isProducer", "structCO__TIME__t.html#a691d02cb2128a81f4af345165146b761", null ], + [ "COB_ID", "structCO__TIME__t.html#a6bcf872834e3a76869943108fd55ffc3", null ], + [ "periodTime", "structCO__TIME__t.html#a7c0090cb2e1e3af1fab308640c3d25a5", null ], + [ "periodTimeoutTime", "structCO__TIME__t.html#a9f2d24892e5b318f5eea5844f3b29a4a", null ], + [ "CANrxNew", "structCO__TIME__t.html#a893e358443b67c38eee33182c496f2a4", null ], + [ "timer", "structCO__TIME__t.html#a02fa485a1a5064ad6e0fc07ea3d5d092", null ], + [ "receiveError", "structCO__TIME__t.html#abbff3c5ce159992f4b45ec042849cbc0", null ], + [ "pFunctSignalPre", "structCO__TIME__t.html#aed7685145a3b3cfb80e8865aa3e8df6d", null ], + [ "functSignalObjectPre", "structCO__TIME__t.html#a91e41ac57bb634b53b6698b58efc4616", null ], + [ "CANdevRx", "structCO__TIME__t.html#ad21e78dd36756f1868056bf6638f5ccd", null ], + [ "CANdevRxIdx", "structCO__TIME__t.html#a4d9335af44c9b5e1224a6691df2f9594", null ], + [ "CANdevTx", "structCO__TIME__t.html#ac5fb7127ca474b6aa2afa207f01f8cae", null ], + [ "CANdevTxIdx", "structCO__TIME__t.html#a205dfcde7a830d6a15eda16911e328cd", null ], + [ "TXbuff", "structCO__TIME__t.html#aa658a7b219b1dad6b722ec41670da626", null ] + ] ], + [ "CO_TIME_init", "group__CO__TIME.html#ga9e02651c1662d3da13a9a071c73347bb", null ], + [ "CO_TIME_initCallbackPre", "group__CO__TIME.html#gae2f03663f1477cdc551b61cf5689cd6b", null ], + [ "CO_TIME_process", "group__CO__TIME.html#ga39f71192db2b40da6dcf8f4fceac9bb6", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__crc16__ccitt.html b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__crc16__ccitt.html new file mode 100755 index 0000000..b5c004c --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__crc16__ccitt.html @@ -0,0 +1,216 @@ + + + + + + + +CANopenNode: CRC 16 CCITT + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Files | +Functions
+
+
CRC 16 CCITT
CANopen_301
+
+
+ + + + + +

+Files

file  crc16-ccitt.h
 Calculation of CRC 16 CCITT polynomial.
 
+ + + + + + + +

+Functions

void crc16_ccitt_single (uint16_t *crc, const uint8_t chr)
 Update crc16_ccitt variable with one data byte. More...
 
uint16_t crc16_ccitt (const uint8_t block[], size_t blockLength, uint16_t crc)
 Calculate CRC sum on block of data. More...
 
+

Detailed Description

+

Calculation of CRC 16 CCITT polynomial.

+

Equation:

+

x^16 + x^12 + x^5 + 1

+

Function Documentation

+ +

◆ crc16_ccitt_single()

+ +
+
+ + + + + + + + + + + + + + + + + + +
void crc16_ccitt_single (uint16_t * crc,
const uint8_t chr 
)
+
+ +

Update crc16_ccitt variable with one data byte.

+

This function updates crc variable for one data byte using crc16 ccitt algorithm.

+
Parameters
+ + + +
[in,out]crcExternally defined variable for CRC checksum. Before start of new CRC calculation, variable must be initialized (zero for xmodem).
chrOne byte of data
+
+
+ +
+
+ +

◆ crc16_ccitt()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + +
uint16_t crc16_ccitt (const uint8_t block[],
size_t blockLength,
uint16_t crc 
)
+
+ +

Calculate CRC sum on block of data.

+
Parameters
+ + + + +
blockPointer to block of data.
blockLengthLength of data in bytes;
crcInitial value (zero for xmodem). If block is split into multiple segments, previous CRC is used as initial.
+
+
+
Returns
Calculated CRC.
+ +
+
+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__crc16__ccitt.js b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__crc16__ccitt.js new file mode 100755 index 0000000..c347bda --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__crc16__ccitt.js @@ -0,0 +1,6 @@ +var group__CO__crc16__ccitt = +[ + [ "crc16-ccitt.h", "crc16-ccitt_8h.html", null ], + [ "crc16_ccitt_single", "group__CO__crc16__ccitt.html#gad0ef7bb8f47a7eb3ff71d603beba7f92", null ], + [ "crc16_ccitt", "group__CO__crc16__ccitt.html#gab03185ec096eb8792b52d657ed6cbea1", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__critical__sections.html b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__critical__sections.html new file mode 100755 index 0000000..848e6a8 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__critical__sections.html @@ -0,0 +1,163 @@ + + + + + + + +CANopenNode: Critical sections + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Macros
+
+
Critical sections
CANopen_301 » Driver
+
+
+ +

CANopenNode is designed to run in different threads, as described in README.md. Threads are implemented differently in different systems. In microcontrollers threads are interrupts with different priorities, for example. It is necessary to protect sections, where different threads access to the same resource. In simple systems interrupts or scheduler may be temporary disabled between access to the shared resource. Otherwise mutexes or semaphores can be used. +More...

+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

+Macros

+#define CO_LOCK_CAN_SEND()
 Lock critical section in CO_CANsend()
 
+#define CO_UNLOCK_CAN_SEND()
 Unlock critical section in CO_CANsend()
 
+#define CO_LOCK_EMCY()
 Lock critical section in CO_errorReport() or CO_errorReset()
 
+#define CO_UNLOCK_EMCY()
 Unlock critical section in CO_errorReport() or CO_errorReset()
 
+#define CO_LOCK_OD()
 Lock critical section when accessing Object Dictionary.
 
+#define CO_UNLOCK_OD()
 Unock critical section when accessing Object Dictionary.
 
+#define CO_FLAG_READ(rxNew)   ((rxNew) != NULL)
 Check if new message has arrived.
 
+#define CO_FLAG_SET(rxNew)   { __sync_synchronize(); rxNew = (void *)1L; }
 Set new message flag.
 
+#define CO_FLAG_CLEAR(rxNew)   { __sync_synchronize(); rxNew = NULL; }
 Clear new message flag.
 
+

Detailed Description

+

CANopenNode is designed to run in different threads, as described in README.md. Threads are implemented differently in different systems. In microcontrollers threads are interrupts with different priorities, for example. It is necessary to protect sections, where different threads access to the same resource. In simple systems interrupts or scheduler may be temporary disabled between access to the shared resource. Otherwise mutexes or semaphores can be used.

+

Reentrant functions

+

Functions CO_CANsend() from C_driver.h, CO_errorReport() from CO_Emergency.h and CO_errorReset() from CO_Emergency.h may be called from different threads. Critical sections must be protected. Either by disabling scheduler or interrupts or by mutexes or semaphores.

+

Object Dictionary variables

+

In general, there are two threads, which accesses OD variables: mainline and timer. CANopenNode initialization and SDO server runs in mainline. PDOs runs in faster timer thread. Processing of PDOs must not be interrupted by mainline. Mainline thread must protect sections, which accesses the same OD variables as timer thread. This care must also take the application. Note that not all variables are allowed to be mapped to PDOs, so they may not need to be protected. SDO server protects sections with access to OD variables.

+

Synchronization functions for CAN receive

+

After CAN message is received, it is pre-processed in CANrx_callback(), which copies some data into appropriate object and at the end sets new_message flag. This flag is then pooled in another thread, which further processes the message. The problem is, that compiler optimization may shuffle memory operations, so it is necessary to ensure, that new_message flag is surely set at the end. It is necessary to use Memory barrier.

+

If receive function runs inside IRQ, no further synchronization is needed. Otherwise, some kind of synchronization has to be included. The following example uses GCC builtin memory barrier __sync_synchronize(). More information can be found here.

+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__critical__sections.js b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__critical__sections.js new file mode 100755 index 0000000..e702b88 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__critical__sections.js @@ -0,0 +1,12 @@ +var group__CO__critical__sections = +[ + [ "CO_LOCK_CAN_SEND", "group__CO__critical__sections.html#ga7566ee901bbf1a0d76d771d72d2f826f", null ], + [ "CO_UNLOCK_CAN_SEND", "group__CO__critical__sections.html#ga511a5a0bf905c2207d5c9e26d35fe3cc", null ], + [ "CO_LOCK_EMCY", "group__CO__critical__sections.html#ga3052a84235f56d535a14705e0cfda799", null ], + [ "CO_UNLOCK_EMCY", "group__CO__critical__sections.html#ga720a798f2bf7fe20d9c95a212b4df417", null ], + [ "CO_LOCK_OD", "group__CO__critical__sections.html#ga3850830931ced2bd3d7e15821572bbcc", null ], + [ "CO_UNLOCK_OD", "group__CO__critical__sections.html#ga2477f5d24fd31a9f4052cf451b87809f", null ], + [ "CO_FLAG_READ", "group__CO__critical__sections.html#ga577a6ebcf246087f084c75d9ae25eeb7", null ], + [ "CO_FLAG_SET", "group__CO__critical__sections.html#gac54b5e4f680aa8b0177f0df5d5be2e88", null ], + [ "CO_FLAG_CLEAR", "group__CO__critical__sections.html#ga044da4253aeed15c3e0bb7fce13664af", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__dataTypes.html b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__dataTypes.html new file mode 100755 index 0000000..fd3c336 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__dataTypes.html @@ -0,0 +1,211 @@ + + + + + + + +CANopenNode: Basic definitions + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Macros | +Typedefs
+
+
Basic definitions
CANopen_301 » Driver
+
+
+ +

Target specific basic definitions and data types according to Misra C specification. +More...

+ + + + + + + + + + + + + + + + + + + + + + + +

+Macros

+#define CO_LITTLE_ENDIAN
 CO_LITTLE_ENDIAN or CO_BIG_ENDIAN must be defined.
 
+#define CO_SWAP_16(x)   x
 Macro must swap bytes, if CO_BIG_ENDIAN is defined.
 
+#define CO_SWAP_32(x)   x
 Macro must swap bytes, if CO_BIG_ENDIAN is defined.
 
+#define CO_SWAP_64(x)   x
 Macro must swap bytes, if CO_BIG_ENDIAN is defined.
 
+#define NULL   (0)
 NULL, for general usage.
 
+#define true   1
 Logical true, for general use.
 
+#define false   0
 Logical false, for general use.
 
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

+Typedefs

+typedef unsigned char bool_t
 Boolean data type for general use.
 
+typedef signed char int8_t
 INTEGER8 in CANopen (0002h), 8-bit signed integer.
 
+typedef signed int int16_t
 INTEGER16 in CANopen (0003h), 16-bit signed integer.
 
+typedef signed long int int32_t
 INTEGER32 in CANopen (0004h), 32-bit signed integer.
 
+typedef signed long long int int64_t
 INTEGER64 in CANopen (0015h), 64-bit signed integer.
 
+typedef unsigned char uint8_t
 UNSIGNED8 in CANopen (0005h), 8-bit unsigned integer.
 
+typedef unsigned int uint16_t
 UNSIGNED16 in CANopen (0006h), 16-bit unsigned integer.
 
+typedef unsigned long int uint32_t
 UNSIGNED32 in CANopen (0007h), 32-bit unsigned integer.
 
+typedef unsigned long long int uint64_t
 UNSIGNED64 in CANopen (001Bh), 64-bit unsigned integer.
 
+typedef float float32_t
 REAL32 in CANopen (0008h), single precision floating point value, 32-bit.
 
+typedef double float64_t
 REAL64 in CANopen (0011h), double precision floating point value, 64-bit.
 
+typedef char char_t
 VISIBLE_STRING in CANopen (0009h), string of signed 8-bit values.
 
+typedef unsigned char oChar_t
 OCTET_STRING in CANopen (000Ah), string of unsigned 8-bit values.
 
+typedef unsigned char domain_t
 DOMAIN in CANopen (000Fh), used to transfer a large block of data.
 
+

Detailed Description

+

Target specific basic definitions and data types according to Misra C specification.

+

Must be defined in the CO_driver_target.h file.

+

Depending on processor or compiler architecture, one of the two macros must be defined: CO_LITTLE_ENDIAN or CO_BIG_ENDIAN. CANopen itself is little endian.

+

Basic data types may be specified differently on different architectures. Usually true and false are defined in <stdbool.h>, NULL is defined in <stddef.h>, int8_t to uint64_t are defined in <stdint.h>.

+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__dataTypes.js b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__dataTypes.js new file mode 100755 index 0000000..df05ac4 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__dataTypes.js @@ -0,0 +1,24 @@ +var group__CO__dataTypes = +[ + [ "CO_LITTLE_ENDIAN", "group__CO__dataTypes.html#gaed3e1bffaf912485092fc20193705f35", null ], + [ "CO_SWAP_16", "group__CO__dataTypes.html#ga1717fcaabbe2cd6cd9fc0bc0cb917a6c", null ], + [ "CO_SWAP_32", "group__CO__dataTypes.html#gadf87da54942e3a0ff159688c5e0e267b", null ], + [ "CO_SWAP_64", "group__CO__dataTypes.html#gaec0fc209357883f42d66cd2cdaa7236f", null ], + [ "NULL", "group__CO__dataTypes.html#ga070d2ce7b6bb7e5c05602aa8c308d0c4", null ], + [ "true", "group__CO__dataTypes.html#ga41f9c5fb8b08eb5dc3edce4dcb37fee7", null ], + [ "false", "group__CO__dataTypes.html#ga65e9886d74aaee76545e83dd09011727", null ], + [ "bool_t", "group__CO__dataTypes.html#ga449976458a084f880dc8e3d29e7eb6f5", null ], + [ "int8_t", "group__CO__dataTypes.html#gaef44329758059c91c76d334e8fc09700", null ], + [ "int16_t", "group__CO__dataTypes.html#ga932e6ccc3d54c58f761c1aead83bd6d7", null ], + [ "int32_t", "group__CO__dataTypes.html#gadb828ef50c2dbb783109824e94cf6c47", null ], + [ "int64_t", "group__CO__dataTypes.html#ga831d6234342279926bb11bad3a37add9", null ], + [ "uint8_t", "group__CO__dataTypes.html#gaba7bc1797add20fe3efdf37ced1182c5", null ], + [ "uint16_t", "group__CO__dataTypes.html#ga1f1825b69244eb3ad2c7165ddc99c956", null ], + [ "uint32_t", "group__CO__dataTypes.html#ga33594304e786b158f3fb30289278f5af", null ], + [ "uint64_t", "group__CO__dataTypes.html#gad27ed092432b64ff558d2254c278720f", null ], + [ "float32_t", "group__CO__dataTypes.html#ga4611b605e45ab401f02cab15c5e38715", null ], + [ "float64_t", "group__CO__dataTypes.html#gac55f3ae81b5bc9053760baacf57e47f4", null ], + [ "char_t", "group__CO__dataTypes.html#ga40bb5262bf908c328fbcfbe5d29d0201", null ], + [ "oChar_t", "group__CO__dataTypes.html#ga00f664c467579d7b2839d6926b6f33a6", null ], + [ "domain_t", "group__CO__dataTypes.html#gadc433a2a90dacd3b2b3801dd9431c254", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__driver.html b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__driver.html new file mode 100755 index 0000000..218e7fe --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__driver.html @@ -0,0 +1,969 @@ + + + + + + + +CANopenNode: Driver + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Modules | +Files | +Data Structures | +Macros | +Enumerations | +Functions
+
+
Driver
CANopen_301
+
+
+ + + + + + + + + + + + + + + + +

+Modules

 Stack configuration
 
 Basic definitions
 Target specific basic definitions and data types according to Misra C specification.
 
 Reception of CAN messages
 Target specific definitions and description of CAN message reception.
 
 Transmission of CAN messages
 Target specific definitions and description of CAN message transmission.
 
 Critical sections
 CANopenNode is designed to run in different threads, as described in README.md. Threads are implemented differently in different systems. In microcontrollers threads are interrupts with different priorities, for example. It is necessary to protect sections, where different threads access to the same resource. In simple systems interrupts or scheduler may be temporary disabled between access to the shared resource. Otherwise mutexes or semaphores can be used.
 
+ + + + + + + +

+Files

file  CO_config.h
 Configuration macros for CANopenNode.
 
file  CO_driver.h
 Interface between CAN hardware and CANopenNode.
 
+ + + + +

+Data Structures

struct  CO_CANmodule_t
 Complete CAN module object. More...
 
+ + + + + + + + + + +

+Macros

+#define CO_VERSION_MAJOR   4
 Major version number of CANopenNode.
 
+#define CO_VERSION_MINOR   0
 Minor version number of CANopenNode.
 
#define CO_errinfo(CANmodule, err)
 Macro for passing additional information about error. More...
 
+ + + + + + + + + + +

+Enumerations

enum  CO_Default_CAN_ID_t {
+  CO_CAN_ID_NMT_SERVICE = 0x000, +CO_CAN_ID_GFC = 0x001, +CO_CAN_ID_SYNC = 0x080, +CO_CAN_ID_EMERGENCY = 0x080, +
+  CO_CAN_ID_TIME = 0x100, +CO_CAN_ID_SRDO_1 = 0x0FF, +CO_CAN_ID_TPDO_1 = 0x180, +CO_CAN_ID_RPDO_1 = 0x200, +
+  CO_CAN_ID_TPDO_2 = 0x280, +CO_CAN_ID_RPDO_2 = 0x300, +CO_CAN_ID_TPDO_3 = 0x380, +CO_CAN_ID_RPDO_3 = 0x400, +
+  CO_CAN_ID_TPDO_4 = 0x480, +CO_CAN_ID_RPDO_4 = 0x500, +CO_CAN_ID_SDO_SRV = 0x580, +CO_CAN_ID_SDO_CLI = 0x600, +
+  CO_CAN_ID_HEARTBEAT = 0x700, +CO_CAN_ID_LSS_SLV = 0x7E4, +CO_CAN_ID_LSS_MST = 0x7E5 +
+ }
 Default CANopen identifiers. More...
 
enum  CO_CAN_ERR_status_t {
+  CO_CAN_ERRTX_WARNING = 0x0001, +CO_CAN_ERRTX_PASSIVE = 0x0002, +CO_CAN_ERRTX_BUS_OFF = 0x0004, +CO_CAN_ERRTX_OVERFLOW = 0x0008, +
+  CO_CAN_ERRTX_PDO_LATE = 0x0080, +CO_CAN_ERRRX_WARNING = 0x0100, +CO_CAN_ERRRX_PASSIVE = 0x0200, +CO_CAN_ERRRX_OVERFLOW = 0x0800, +
+  CO_CAN_ERR_WARN_PASSIVE = 0x0303 +
+ }
 CAN error status bitmasks. More...
 
enum  CO_ReturnError_t {
+  CO_ERROR_NO = 0, +CO_ERROR_ILLEGAL_ARGUMENT = -1, +CO_ERROR_OUT_OF_MEMORY = -2, +CO_ERROR_TIMEOUT = -3, +
+  CO_ERROR_ILLEGAL_BAUDRATE = -4, +CO_ERROR_RX_OVERFLOW = -5, +CO_ERROR_RX_PDO_OVERFLOW = -6, +CO_ERROR_RX_MSG_LENGTH = -7, +
+  CO_ERROR_RX_PDO_LENGTH = -8, +CO_ERROR_TX_OVERFLOW = -9, +CO_ERROR_TX_PDO_WINDOW = -10, +CO_ERROR_TX_UNCONFIGURED = -11, +
+  CO_ERROR_OD_PARAMETERS = -12, +CO_ERROR_DATA_CORRUPT = -13, +CO_ERROR_CRC = -14, +CO_ERROR_TX_BUSY = -15, +
+  CO_ERROR_WRONG_NMT_STATE = -16, +CO_ERROR_SYSCALL = -17, +CO_ERROR_INVALID_STATE = -18, +CO_ERROR_NODE_ID_UNCONFIGURED_LSS = -19 +
+ }
 Return values of some CANopen functions. More...
 
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

+Functions

void CO_CANsetConfigurationMode (void *CANptr)
 Request CAN configuration (stopped) mode and wait until it is set. More...
 
void CO_CANsetNormalMode (CO_CANmodule_t *CANmodule)
 Request CAN normal (operational) mode and wait until it is set. More...
 
CO_ReturnError_t CO_CANmodule_init (CO_CANmodule_t *CANmodule, void *CANptr, CO_CANrx_t rxArray[], uint16_t rxSize, CO_CANtx_t txArray[], uint16_t txSize, uint16_t CANbitRate)
 Initialize CAN module object. More...
 
void CO_CANmodule_disable (CO_CANmodule_t *CANmodule)
 Switch off CANmodule. More...
 
CO_ReturnError_t CO_CANrxBufferInit (CO_CANmodule_t *CANmodule, uint16_t index, uint16_t ident, uint16_t mask, bool_t rtr, void *object, void(*CANrx_callback)(void *object, void *message))
 Configure CAN message receive buffer. More...
 
CO_CANtx_t * CO_CANtxBufferInit (CO_CANmodule_t *CANmodule, uint16_t index, uint16_t ident, bool_t rtr, uint8_t noOfBytes, bool_t syncFlag)
 Configure CAN message transmit buffer. More...
 
CO_ReturnError_t CO_CANsend (CO_CANmodule_t *CANmodule, CO_CANtx_t *buffer)
 Send CAN message. More...
 
void CO_CANclearPendingSyncPDOs (CO_CANmodule_t *CANmodule)
 Clear all synchronous TPDOs from CAN module transmit buffers. More...
 
void CO_CANmodule_process (CO_CANmodule_t *CANmodule)
 Process can module - verify CAN errors. More...
 
static uint8_t CO_getUint8 (const void *buf)
 Get uint8_t value from memory buffer. More...
 
+static uint16_t CO_getUint16 (const void *buf)
 Get uint16_t value from memory buffer, see CO_getUint8.
 
+static uint32_t CO_getUint32 (const void *buf)
 Get uint32_t value from memory buffer, see CO_getUint8.
 
static uint8_t CO_setUint8 (void *buf, uint8_t value)
 Write uint8_t value into memory buffer. More...
 
+static uint8_t CO_setUint16 (void *buf, uint16_t value)
 Write uint16_t value into memory buffer, see CO_setUint8.
 
+static uint8_t CO_setUint32 (void *buf, uint32_t value)
 Write uint32_t value into memory buffer, see CO_setUint8.
 
+

Detailed Description

+

Interface between CAN hardware and CANopenNode.

+

CANopenNode is designed for speed and portability. It runs efficiently on devices from simple 16-bit microcontrollers to PC computers. It can run in multiple threads. Reception of CAN messages is pre-processed with very fast functions. Time critical objects, such as PDO or SYNC are processed in real-time thread and other objects are processed in normal thread. See Flowchart in README.md for more information.

+

CANopenNode Object

+

CANopenNode is implemented as a collection of different objects, for example SDO, SYNC, Emergency, PDO, NMT, Heartbeat, etc. Code is written in C language and tries to be object oriented. So each CANopenNode Object is implemented in a pair of .h/.c files. It basically contains a structure with all necessary variables and some functions which operates on it. CANopenNode Object is usually connected with one or more CAN receive or transmit Message Objects. (CAN message Object is a CAN message with specific 11-bit CAN identifier (usually one fixed or a range).)

+

Hardware interface of CANopenNode

+

It consists of minimum three files:

    +
  • CO_driver.h file declares common functions. This file is part of the CANopenNode. It is included from each .c file from CANopenNode.
    • +
  • CO_driver_target.h file declares microcontroller specific type declarations and defines some macros, which are necessary for CANopenNode. This file is included from CO_driver.h.
    • +
  • CO_driver.c file defines functions declared in CO_driver.h.
    • +
+

CO_driver_target.h and CO_driver.c files are specific for each different microcontroller and are not part of CANopenNode. There are separate projects for different microcontrollers, which usually include CANopenNode as a git submodule. CANopenNode only includes those two files in the example directory and they are basically empty. It should be possible to compile the CANopenNode/example on any system, however compiled program is not usable. CO_driver.h contains documentation for all necessary macros, types and functions.

+

See CANopenNode/Wiki for a known list of available implementations of CANopenNode on different systems and microcontrollers. Everybody is welcome to extend the list with a link to his own implementation.

+

Implementation of the hardware interface for specific microcontroller is not always an easy task. For reliable and efficient operation it is necessary to know some parts of the target microcontroller in detail (for example threads (or interrupts), CAN module, etc.).

+

Macro Definition Documentation

+ +

◆ CO_errinfo

+ +
+
+ + + + + + + + + + + + + + + + + + +
#define CO_errinfo( CANmodule,
 err 
)
+
+ +

Macro for passing additional information about error.

+

This macro is called from several CANopen init functions, which returns CO_ReturnError_t.

+

CO_driver_target.h may implement this macro. Usually macro only sets CANmodule->errinfo to err. Application may then use CANmodule->errinfo to determine the reason of failure. errinfo must be type of int32_t. By default macro does not record anything.

+

CO_errinfo is called in following CO_ReturnError_t reasons:

    +
  • 'CO_ERROR_OD_PARAMETERS' - Index of erroneous OD parameter.
    • +
+ +
+
+

Enumeration Type Documentation

+ +

◆ CO_Default_CAN_ID_t

+ +
+
+ + + + +
enum CO_Default_CAN_ID_t
+
+ +

Default CANopen identifiers.

+

Default CANopen identifiers for CANopen communication objects. Same as 11-bit addresses of CAN messages. These are default identifiers and can be changed in CANopen. Especially PDO identifiers are configured in PDO linking phase of the CANopen network configuration.

+ + + + + + + + + + + + + + + + + + + + +
Enumerator
CO_CAN_ID_NMT_SERVICE 

0x000, Network management

+
CO_CAN_ID_GFC 

0x001, Global fail-safe command

+
CO_CAN_ID_SYNC 

0x080, Synchronous message

+
CO_CAN_ID_EMERGENCY 

0x080, Emergency messages (+nodeID)

+
CO_CAN_ID_TIME 

0x100, Time message

+
CO_CAN_ID_SRDO_1 

0x0FF, Default SRDO1 (+2*nodeID)

+
CO_CAN_ID_TPDO_1 

0x180, Default TPDO1 (+nodeID)

+
CO_CAN_ID_RPDO_1 

0x200, Default RPDO1 (+nodeID)

+
CO_CAN_ID_TPDO_2 

0x280, Default TPDO2 (+nodeID)

+
CO_CAN_ID_RPDO_2 

0x300, Default RPDO2 (+nodeID)

+
CO_CAN_ID_TPDO_3 

0x380, Default TPDO3 (+nodeID)

+
CO_CAN_ID_RPDO_3 

0x400, Default RPDO3 (+nodeID)

+
CO_CAN_ID_TPDO_4 

0x480, Default TPDO4 (+nodeID)

+
CO_CAN_ID_RPDO_4 

0x500, Default RPDO5 (+nodeID)

+
CO_CAN_ID_SDO_SRV 

0x580, SDO response from server (+nodeID)

+
CO_CAN_ID_SDO_CLI 

0x600, SDO request from client (+nodeID)

+
CO_CAN_ID_HEARTBEAT 

0x700, Heartbeat message

+
CO_CAN_ID_LSS_SLV 

0x7E4, LSS response from slave

+
CO_CAN_ID_LSS_MST 

0x7E5, LSS request from master

+
+ +
+
+ +

◆ CO_CAN_ERR_status_t

+ +
+
+ + + + +
enum CO_CAN_ERR_status_t
+
+ +

CAN error status bitmasks.

+

CAN warning level is reached, if CAN transmit or receive error counter is more or equal to 96. CAN passive level is reached, if counters are more or equal to 128. Transmitter goes in error state 'bus off' if transmit error counter is more or equal to 256.

+ + + + + + + + + + +
Enumerator
CO_CAN_ERRTX_WARNING 

0x0001, CAN transmitter warning

+
CO_CAN_ERRTX_PASSIVE 

0x0002, CAN transmitter passive

+
CO_CAN_ERRTX_BUS_OFF 

0x0004, CAN transmitter bus off

+
CO_CAN_ERRTX_OVERFLOW 

0x0008, CAN transmitter overflow

+
CO_CAN_ERRTX_PDO_LATE 

0x0080, TPDO is outside sync window

+
CO_CAN_ERRRX_WARNING 

0x0100, CAN receiver warning

+
CO_CAN_ERRRX_PASSIVE 

0x0200, CAN receiver passive

+
CO_CAN_ERRRX_OVERFLOW 

0x0800, CAN receiver overflow

+
CO_CAN_ERR_WARN_PASSIVE 

0x0303, combination

+
+ +
+
+ +

◆ CO_ReturnError_t

+ +
+
+ + + + +
enum CO_ReturnError_t
+
+ +

Return values of some CANopen functions.

+

If function was executed successfully it returns 0 otherwise it returns <0.

+ + + + + + + + + + + + + + + + + + + + + +
Enumerator
CO_ERROR_NO 

Operation completed successfully.

+
CO_ERROR_ILLEGAL_ARGUMENT 

Error in function arguments.

+
CO_ERROR_OUT_OF_MEMORY 

Memory allocation failed.

+
CO_ERROR_TIMEOUT 

Function timeout.

+
CO_ERROR_ILLEGAL_BAUDRATE 

Illegal baudrate passed to function CO_CANmodule_init()

+
CO_ERROR_RX_OVERFLOW 

Previous message was not processed yet.

+
CO_ERROR_RX_PDO_OVERFLOW 

previous PDO was not processed yet

+
CO_ERROR_RX_MSG_LENGTH 

Wrong receive message length.

+
CO_ERROR_RX_PDO_LENGTH 

Wrong receive PDO length.

+
CO_ERROR_TX_OVERFLOW 

Previous message is still waiting, buffer full.

+
CO_ERROR_TX_PDO_WINDOW 

Synchronous TPDO is outside window.

+
CO_ERROR_TX_UNCONFIGURED 

Transmit buffer was not configured properly.

+
CO_ERROR_OD_PARAMETERS 

Error in Object Dictionary parameters.

+
CO_ERROR_DATA_CORRUPT 

Stored data are corrupt.

+
CO_ERROR_CRC 

CRC does not match.

+
CO_ERROR_TX_BUSY 

Sending rejected because driver is busy.

+

Try again

+
CO_ERROR_WRONG_NMT_STATE 

Command can't be processed in current state.

+
CO_ERROR_SYSCALL 

Syscall failed.

+
CO_ERROR_INVALID_STATE 

Driver not ready.

+
CO_ERROR_NODE_ID_UNCONFIGURED_LSS 

Node-id is in LSS unconfigured state.

+

If objects are handled properly, this may not be an error.

+
+ +
+
+

Function Documentation

+ +

◆ CO_CANsetConfigurationMode()

+ +
+
+ + + + + + + + +
void CO_CANsetConfigurationMode (void * CANptr)
+
+ +

Request CAN configuration (stopped) mode and wait until it is set.

+
Parameters
+ + +
CANptrPointer to CAN device
+
+
+ +
+
+ +

◆ CO_CANsetNormalMode()

+ +
+
+ + + + + + + + +
void CO_CANsetNormalMode (CO_CANmodule_t * CANmodule)
+
+ +

Request CAN normal (operational) mode and wait until it is set.

+
Parameters
+ + +
CANmoduleCO_CANmodule_t object.
+
+
+ +
+
+ +

◆ CO_CANmodule_init()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
CO_ReturnError_t CO_CANmodule_init (CO_CANmodule_t * CANmodule,
void * CANptr,
CO_CANrx_t rxArray[],
uint16_t rxSize,
CO_CANtx_t txArray[],
uint16_t txSize,
uint16_t CANbitRate 
)
+
+ +

Initialize CAN module object.

+

Function must be called in the communication reset section. CAN module must be in Configuration Mode before.

+
Parameters
+ + + + + + + + +
CANmoduleThis object will be initialized.
CANptrPointer to CAN device.
rxArrayArray for handling received CAN messages
rxSizeSize of the above array. Must be equal to number of receiving CAN objects.
txArrayArray for handling transmitting CAN messages
txSizeSize of the above array. Must be equal to number of transmitting CAN objects.
CANbitRateValid values are (in kbps): 10, 20, 50, 125, 250, 500, 800,
    +
  1. If value is illegal, bitrate defaults to 125.
    2. +
+
+
+
+

Return CO_ReturnError_t: CO_ERROR_NO or CO_ERROR_ILLEGAL_ARGUMENT.

+ +
+
+ +

◆ CO_CANmodule_disable()

+ +
+
+ + + + + + + + +
void CO_CANmodule_disable (CO_CANmodule_t * CANmodule)
+
+ +

Switch off CANmodule.

+

Call at program exit.

+
Parameters
+ + +
CANmoduleCAN module object.
+
+
+ +
+
+ +

◆ CO_CANrxBufferInit()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
CO_ReturnError_t CO_CANrxBufferInit (CO_CANmodule_t * CANmodule,
uint16_t index,
uint16_t ident,
uint16_t mask,
bool_t rtr,
void * object,
void(*)(void *object, void *message) CANrx_callback 
)
+
+ +

Configure CAN message receive buffer.

+

Function configures specific CAN receive buffer. It sets CAN identifier and connects buffer with specific object. Function must be called for each member in rxArray from CO_CANmodule_t.

+
Parameters
+ + + + + + + + +
CANmoduleThis object.
indexIndex of the specific buffer in rxArray.
ident11-bit standard CAN Identifier. If two or more CANrx buffers have the same ident, then buffer with lowest index has precedence and other CANrx buffers will be ignored.
mask11-bit mask for identifier. Most usually set to 0x7FF. Received message (rcvMsg) will be accepted if the following condition is true: (((rcvMsgId ^ ident) & mask) == 0).
rtrIf true, 'Remote Transmit Request' messages will be accepted.
objectCANopen object, to which buffer is connected. It will be used as an argument to CANrx_callback. Its type is (void), CANrx_callback will change its type back to the correct object type.
CANrx_callbackPointer to function, which will be called, if received CAN message matches the identifier. It must be fast function.
+
+
+

Return CO_ReturnError_t: CO_ERROR_NO CO_ERROR_ILLEGAL_ARGUMENT or CO_ERROR_OUT_OF_MEMORY (not enough masks for configuration).

+ +
+
+ +

◆ CO_CANtxBufferInit()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
CO_CANtx_t* CO_CANtxBufferInit (CO_CANmodule_t * CANmodule,
uint16_t index,
uint16_t ident,
bool_t rtr,
uint8_t noOfBytes,
bool_t syncFlag 
)
+
+ +

Configure CAN message transmit buffer.

+

Function configures specific CAN transmit buffer. Function must be called for each member in txArray from CO_CANmodule_t.

+
Parameters
+ + + + + + + +
CANmoduleThis object.
indexIndex of the specific buffer in txArray.
ident11-bit standard CAN Identifier.
rtrIf true, 'Remote Transmit Request' messages will be transmitted.
noOfBytesLength of CAN message in bytes (0 to 8 bytes).
syncFlagThis flag bit is used for synchronous TPDO messages. If it is set, message will not be sent, if current time is outside synchronous window.
+
+
+
Returns
Pointer to CAN transmit message buffer. 8 bytes data array inside buffer should be written, before CO_CANsend() function is called. Zero is returned in case of wrong arguments.
+ +
+
+ +

◆ CO_CANsend()

+ +
+
+ + + + + + + + + + + + + + + + + + +
CO_ReturnError_t CO_CANsend (CO_CANmodule_t * CANmodule,
CO_CANtx_t * buffer 
)
+
+ +

Send CAN message.

+
Parameters
+ + + +
CANmoduleThis object.
bufferPointer to transmit buffer, returned by CO_CANtxBufferInit(). Data bytes must be written in buffer before function call.
+
+
+
Returns
CO_ReturnError_t: CO_ERROR_NO, CO_ERROR_TX_OVERFLOW or CO_ERROR_TX_PDO_WINDOW (Synchronous TPDO is outside window).
+ +
+
+ +

◆ CO_CANclearPendingSyncPDOs()

+ +
+
+ + + + + + + + +
void CO_CANclearPendingSyncPDOs (CO_CANmodule_t * CANmodule)
+
+ +

Clear all synchronous TPDOs from CAN module transmit buffers.

+

CANopen allows synchronous PDO communication only inside time between SYNC message and SYNC Window. If time is outside this window, new synchronous PDOs must not be sent and all pending sync TPDOs, which may be on CAN TX buffers, may optionally be cleared.

+

This function checks (and aborts transmission if necessary) CAN TX buffers when it is called. Function should be called by the stack in the moment, when SYNC time was just passed out of synchronous window.

+
Parameters
+ + +
CANmoduleThis object.
+
+
+ +
+
+ +

◆ CO_CANmodule_process()

+ +
+
+ + + + + + + + +
void CO_CANmodule_process (CO_CANmodule_t * CANmodule)
+
+ +

Process can module - verify CAN errors.

+

Function must be called cyclically. It should calculate CANerrorStatus bitfield for CAN errors defined in CO_CAN_ERR_status_t.

+
Parameters
+ + +
CANmoduleThis object.
+
+
+ +
+
+ +

◆ CO_getUint8()

+ +
+
+ + + + + +
+ + + + + + + + +
static uint8_t CO_getUint8 (const void * buf)
+ 		+inlinestatic
+
+ +

Get uint8_t value from memory buffer.

+
Parameters
+ + +
bufMemory buffer to get value from.
+
+
+
Returns
Value
+ +
+
+ +

◆ CO_setUint8()

+ +
+
+ + + + + +
+ + + + + + + + + + + + + + + + + + +
static uint8_t CO_setUint8 (void * buf,
uint8_t value 
)
+ 		+inlinestatic
+
+ +

Write uint8_t value into memory buffer.

+
Parameters
+ + + +
bufMemory buffer.
valueValue to be written into buf.
+
+
+
Returns
number of bytes written.
+ +
+
+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__driver.js b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__driver.js new file mode 100755 index 0000000..02dc715 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__driver.js @@ -0,0 +1,97 @@ +var group__CO__driver = +[ + [ "Stack configuration", "group__CO__STACK__CONFIG.html", "group__CO__STACK__CONFIG" ], + [ "Basic definitions", "group__CO__dataTypes.html", "group__CO__dataTypes" ], + [ "Reception of CAN messages", "group__CO__CAN__Message__reception.html", "group__CO__CAN__Message__reception" ], + [ "Transmission of CAN messages", "group__CO__CAN__Message__transmission.html", "group__CO__CAN__Message__transmission" ], + [ "Critical sections", "group__CO__critical__sections.html", "group__CO__critical__sections" ], + [ "CO_config.h", "CO__config_8h.html", null ], + [ "CO_driver.h", "CO__driver_8h.html", null ], + [ "CO_CANmodule_t", "structCO__CANmodule__t.html", [ + [ "CANptr", "structCO__CANmodule__t.html#aad1c18f6d47621e5a611333dc57cb349", null ], + [ "rxArray", "structCO__CANmodule__t.html#a747e694e15cca5a5579c2c4397a6da39", null ], + [ "rxSize", "structCO__CANmodule__t.html#a88edfd32c2bff5b9f29a2bccd1ac96f3", null ], + [ "txArray", "structCO__CANmodule__t.html#af623f2a045716af77e906b28deee3d3c", null ], + [ "txSize", "structCO__CANmodule__t.html#a6a53d3d12efbe30d1ee08e821d1b4139", null ], + [ "CANerrorStatus", "structCO__CANmodule__t.html#a5757052c57726cb4fb2ac5cdd8ff744d", null ], + [ "CANnormal", "structCO__CANmodule__t.html#a5555f7d10e09da5815526d7c8b10901d", null ], + [ "useCANrxFilters", "structCO__CANmodule__t.html#a9b28f7a6f02d398b3a0ea6cf70fa64f0", null ], + [ "bufferInhibitFlag", "structCO__CANmodule__t.html#ad7af19de0bf39c8927c926b095cb5292", null ], + [ "firstCANtxMessage", "structCO__CANmodule__t.html#ac12bd37971b89b5796fec0de71be0f07", null ], + [ "CANtxCount", "structCO__CANmodule__t.html#a269294aa6a15ba56f92a460fae0536ac", null ], + [ "errOld", "structCO__CANmodule__t.html#aa0627988ddedc3a4ec3bbfcf3b818d06", null ], + [ "errinfo", "structCO__CANmodule__t.html#aead6585dcb9b8214dd77e8f74ca2aea1", null ] + ] ], + [ "CO_VERSION_MAJOR", "group__CO__driver.html#ga0e351c2972f6d8f2e08fb5ac21a833b8", null ], + [ "CO_VERSION_MINOR", "group__CO__driver.html#gaac3f110c1dd3cfc2b994b5c20d1c6ace", null ], + [ "CO_errinfo", "group__CO__driver.html#gaaa84189910b720ce18c8d83aab405d86", null ], + [ "CO_Default_CAN_ID_t", "group__CO__driver.html#ga01dd35ae53fd2209ceccabdc8bf8dd06", [ + [ "CO_CAN_ID_NMT_SERVICE", "group__CO__driver.html#gga01dd35ae53fd2209ceccabdc8bf8dd06a335d0f6204819d267ba396b715f66ead", null ], + [ "CO_CAN_ID_GFC", "group__CO__driver.html#gga01dd35ae53fd2209ceccabdc8bf8dd06a0ba8a628aa1a873a21820070261c2783", null ], + [ "CO_CAN_ID_SYNC", "group__CO__driver.html#gga01dd35ae53fd2209ceccabdc8bf8dd06a7a8486aaf2f35eb83c6ca690d0cdce06", null ], + [ "CO_CAN_ID_EMERGENCY", "group__CO__driver.html#gga01dd35ae53fd2209ceccabdc8bf8dd06a94ffef8babcef5b807c5f8c865ef7666", null ], + [ "CO_CAN_ID_TIME", "group__CO__driver.html#gga01dd35ae53fd2209ceccabdc8bf8dd06ab2e20e54189f5cb565e80b05eb8c4931", null ], + [ "CO_CAN_ID_SRDO_1", "group__CO__driver.html#gga01dd35ae53fd2209ceccabdc8bf8dd06aefe4dd6630902d36173b81c106a813bc", null ], + [ "CO_CAN_ID_TPDO_1", "group__CO__driver.html#gga01dd35ae53fd2209ceccabdc8bf8dd06a848f2bdb085bc3a342400a6b43c37f82", null ], + [ "CO_CAN_ID_RPDO_1", "group__CO__driver.html#gga01dd35ae53fd2209ceccabdc8bf8dd06a0ab4be02961987ad817a99a4ef379517", null ], + [ "CO_CAN_ID_TPDO_2", "group__CO__driver.html#gga01dd35ae53fd2209ceccabdc8bf8dd06acb90f0dab2e31982df1bebae6dd02e4b", null ], + [ "CO_CAN_ID_RPDO_2", "group__CO__driver.html#gga01dd35ae53fd2209ceccabdc8bf8dd06a60081a7b09921c6bfce3762a3dd4e49f", null ], + [ "CO_CAN_ID_TPDO_3", "group__CO__driver.html#gga01dd35ae53fd2209ceccabdc8bf8dd06a83b73730655607582d1dabc8f78f7ca4", null ], + [ "CO_CAN_ID_RPDO_3", "group__CO__driver.html#gga01dd35ae53fd2209ceccabdc8bf8dd06ad634b89f227db86bc8c633dda327e5fb", null ], + [ "CO_CAN_ID_TPDO_4", "group__CO__driver.html#gga01dd35ae53fd2209ceccabdc8bf8dd06abe92c9f7938ad6566e8aa010ab6f5cae", null ], + [ "CO_CAN_ID_RPDO_4", "group__CO__driver.html#gga01dd35ae53fd2209ceccabdc8bf8dd06afec9dfa33a34beef50c434e5cde68c6b", null ], + [ "CO_CAN_ID_SDO_SRV", "group__CO__driver.html#gga01dd35ae53fd2209ceccabdc8bf8dd06a3c84d92ad004cfc04e398193b742d30c", null ], + [ "CO_CAN_ID_SDO_CLI", "group__CO__driver.html#gga01dd35ae53fd2209ceccabdc8bf8dd06acfe8640033d9668fafc63aa81d68ede5", null ], + [ "CO_CAN_ID_HEARTBEAT", "group__CO__driver.html#gga01dd35ae53fd2209ceccabdc8bf8dd06a0cfd21623475a1a8522b30b8b16d9874", null ], + [ "CO_CAN_ID_LSS_SLV", "group__CO__driver.html#gga01dd35ae53fd2209ceccabdc8bf8dd06a4a62af7fb0b8768e57945a558a0ceee4", null ], + [ "CO_CAN_ID_LSS_MST", "group__CO__driver.html#gga01dd35ae53fd2209ceccabdc8bf8dd06ad7d16ed89e513b035104e4b2634ce287", null ] + ] ], + [ "CO_CAN_ERR_status_t", "group__CO__driver.html#ga6c5539afb3a95af43f5477d904607426", [ + [ "CO_CAN_ERRTX_WARNING", "group__CO__driver.html#gga6c5539afb3a95af43f5477d904607426a725e4fe057c2f9d222850686a76c724d", null ], + [ "CO_CAN_ERRTX_PASSIVE", "group__CO__driver.html#gga6c5539afb3a95af43f5477d904607426a85d05e861dc03e256dccf977ae16ad6e", null ], + [ "CO_CAN_ERRTX_BUS_OFF", "group__CO__driver.html#gga6c5539afb3a95af43f5477d904607426ae58aa7d0eb5d13630f858a3869f0ee7d", null ], + [ "CO_CAN_ERRTX_OVERFLOW", "group__CO__driver.html#gga6c5539afb3a95af43f5477d904607426ad6fba8a27d82774f53812d3a49655d12", null ], + [ "CO_CAN_ERRTX_PDO_LATE", "group__CO__driver.html#gga6c5539afb3a95af43f5477d904607426ac0b993c7786f41a8c73ad0339124881b", null ], + [ "CO_CAN_ERRRX_WARNING", "group__CO__driver.html#gga6c5539afb3a95af43f5477d904607426a6df88a8a296eb4addc12d9136c0566b0", null ], + [ "CO_CAN_ERRRX_PASSIVE", "group__CO__driver.html#gga6c5539afb3a95af43f5477d904607426a51494ad0df1e2de6d9395f1803c4b233", null ], + [ "CO_CAN_ERRRX_OVERFLOW", "group__CO__driver.html#gga6c5539afb3a95af43f5477d904607426a714a0b9c0978ffde5f138a81880a1fdd", null ], + [ "CO_CAN_ERR_WARN_PASSIVE", "group__CO__driver.html#gga6c5539afb3a95af43f5477d904607426a5ad44f86d5691f2bc809f722364516e0", null ] + ] ], + [ "CO_ReturnError_t", "group__CO__driver.html#ga1cb2d3466eb0c6d267f3b5ff1a0d9532", [ + [ "CO_ERROR_NO", "group__CO__driver.html#gga1cb2d3466eb0c6d267f3b5ff1a0d9532aff1c3e9fd4bf65e6757b020f752cdac8", null ], + [ "CO_ERROR_ILLEGAL_ARGUMENT", "group__CO__driver.html#gga1cb2d3466eb0c6d267f3b5ff1a0d9532a3e78d588c0e21630c9cb7b43bfda3dae", null ], + [ "CO_ERROR_OUT_OF_MEMORY", "group__CO__driver.html#gga1cb2d3466eb0c6d267f3b5ff1a0d9532a2f07295c4c6539c2b390db2d7c351267", null ], + [ "CO_ERROR_TIMEOUT", "group__CO__driver.html#gga1cb2d3466eb0c6d267f3b5ff1a0d9532a129c7141c52ae59d37a2fff163fec600", null ], + [ "CO_ERROR_ILLEGAL_BAUDRATE", "group__CO__driver.html#gga1cb2d3466eb0c6d267f3b5ff1a0d9532a0214308865e83b8c21de7317b3070097", null ], + [ "CO_ERROR_RX_OVERFLOW", "group__CO__driver.html#gga1cb2d3466eb0c6d267f3b5ff1a0d9532ab0746d0801a99639077b325594b347b5", null ], + [ "CO_ERROR_RX_PDO_OVERFLOW", "group__CO__driver.html#gga1cb2d3466eb0c6d267f3b5ff1a0d9532a7e4cefeb35775754c87cf6a559f1bbd9", null ], + [ "CO_ERROR_RX_MSG_LENGTH", "group__CO__driver.html#gga1cb2d3466eb0c6d267f3b5ff1a0d9532a3e40e8440a8480c413d3ff724b875de4", null ], + [ "CO_ERROR_RX_PDO_LENGTH", "group__CO__driver.html#gga1cb2d3466eb0c6d267f3b5ff1a0d9532a4829a460cbba557a2ff7f52bd912aa65", null ], + [ "CO_ERROR_TX_OVERFLOW", "group__CO__driver.html#gga1cb2d3466eb0c6d267f3b5ff1a0d9532a757b4d226a3e00a9e2543cf21833d46f", null ], + [ "CO_ERROR_TX_PDO_WINDOW", "group__CO__driver.html#gga1cb2d3466eb0c6d267f3b5ff1a0d9532a48993dc18698738d168071b4f4c3d244", null ], + [ "CO_ERROR_TX_UNCONFIGURED", "group__CO__driver.html#gga1cb2d3466eb0c6d267f3b5ff1a0d9532a6f07e7c21acde035b561edc7f55edb89", null ], + [ "CO_ERROR_OD_PARAMETERS", "group__CO__driver.html#gga1cb2d3466eb0c6d267f3b5ff1a0d9532a737e67c9f8d3ca882a3cba852153c1f3", null ], + [ "CO_ERROR_DATA_CORRUPT", "group__CO__driver.html#gga1cb2d3466eb0c6d267f3b5ff1a0d9532aa053301213d13670e9af7d31abc1ee48", null ], + [ "CO_ERROR_CRC", "group__CO__driver.html#gga1cb2d3466eb0c6d267f3b5ff1a0d9532a25eaff6474c6bc2aae67b1e7c7a35109", null ], + [ "CO_ERROR_TX_BUSY", "group__CO__driver.html#gga1cb2d3466eb0c6d267f3b5ff1a0d9532ab47ecc1b08463eb986fd29c187096343", null ], + [ "CO_ERROR_WRONG_NMT_STATE", "group__CO__driver.html#gga1cb2d3466eb0c6d267f3b5ff1a0d9532a07457c9c6eda89fc0260e6d7e431424c", null ], + [ "CO_ERROR_SYSCALL", "group__CO__driver.html#gga1cb2d3466eb0c6d267f3b5ff1a0d9532a46d81c922ecbaf8f136626725ad8399d", null ], + [ "CO_ERROR_INVALID_STATE", "group__CO__driver.html#gga1cb2d3466eb0c6d267f3b5ff1a0d9532a57f1c4d2d960ab2550669a1c5ebbf4e1", null ], + [ "CO_ERROR_NODE_ID_UNCONFIGURED_LSS", "group__CO__driver.html#gga1cb2d3466eb0c6d267f3b5ff1a0d9532ac7a6f0554ae52997b88a97b46a16e5a3", null ] + ] ], + [ "CO_CANsetConfigurationMode", "group__CO__driver.html#ga88ef52baae169a80e4c2f2cb93b33747", null ], + [ "CO_CANsetNormalMode", "group__CO__driver.html#gad654edfa651bf7b68263913786697200", null ], + [ "CO_CANmodule_init", "group__CO__driver.html#ga3a1131813110529494cee5e27c0bf28d", null ], + [ "CO_CANmodule_disable", "group__CO__driver.html#gac6f60f9da27dda0c9b3950c4e96bd687", null ], + [ "CO_CANrxBufferInit", "group__CO__driver.html#ga25ee22cd2e3a2f3bb49990e8bc3076b0", null ], + [ "CO_CANtxBufferInit", "group__CO__driver.html#ga01e2ee79e7e3a8b321dac831e7e00976", null ], + [ "CO_CANsend", "group__CO__driver.html#ga4664a9f5d547cb0605a9e929fb079f2e", null ], + [ "CO_CANclearPendingSyncPDOs", "group__CO__driver.html#gabbeac85cbf513162b11fc3d0717b0753", null ], + [ "CO_CANmodule_process", "group__CO__driver.html#ga066c6742f75b2daac585735ffd6c9928", null ], + [ "CO_getUint8", "group__CO__driver.html#ga27a4052f87c60cf2df472378689c2ef9", null ], + [ "CO_getUint16", "group__CO__driver.html#ga6590fe7a05ecb4b81ee3d7e233274ea4", null ], + [ "CO_getUint32", "group__CO__driver.html#ga21cd9e2391f276b908bcde5769e967ed", null ], + [ "CO_setUint8", "group__CO__driver.html#ga61de0908305223a0a7949e184cc1d644", null ], + [ "CO_setUint16", "group__CO__driver.html#gaf74a6b7343bc9d6efd5ec5d9d9f965fb", null ], + [ "CO_setUint32", "group__CO__driver.html#ga9dbc7193fbd875503e0b3b34ea22434e", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__epoll__interface.html b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__epoll__interface.html new file mode 100755 index 0000000..ecdd72f --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__epoll__interface.html @@ -0,0 +1,602 @@ + + + + + + + +CANopenNode: Epoll interface + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Files | +Data Structures | +Enumerations | +Functions
+
+
Epoll interface
socketCAN
+
+
+ + + + + +

+Files

file  CO_epoll_interface.h
 Helper functions for Linux epoll interface to CANopenNode.
 
+ + + + + + + +

+Data Structures

struct  CO_epoll_t
 Object for epoll, timer and event API. More...
 
struct  CO_epoll_gtw_t
 Object for gateway. More...
 
+ + + + +

+Enumerations

enum  CO_commandInterface_t
 Command interface type for gateway-ascii.
 
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

+Functions

CO_ReturnError_t CO_epoll_create (CO_epoll_t *ep, uint32_t timerInterval_us)
 Create Linux epoll, timerfd and eventfd. More...
 
void CO_epoll_close (CO_epoll_t *ep)
 Close epoll, timerfd and eventfd. More...
 
void CO_epoll_wait (CO_epoll_t *ep)
 Wait for an epoll event. More...
 
void CO_epoll_processLast (CO_epoll_t *ep)
 Closing function for an epoll event. More...
 
void CO_epoll_initCANopenMain (CO_epoll_t *ep, CO_t *co)
 Initialization of functions in CANopen reset-communication section. More...
 
void CO_epoll_processMain (CO_epoll_t *ep, CO_t *co, bool_t enableGateway, CO_NMT_reset_cmd_t *reset)
 Process CANopen mainline functions. More...
 
void CO_epoll_processRT (CO_epoll_t *ep, CO_t *co, bool_t realtime)
 Process CAN receive and realtime functions. More...
 
CO_ReturnError_t CO_epoll_createGtw (CO_epoll_gtw_t *epGtw, int epoll_fd, int32_t commandInterface, uint32_t socketTimeout_ms, char *localSocketPath)
 Create socket for gateway-ascii command interface and add it to epoll. More...
 
void CO_epoll_closeGtw (CO_epoll_gtw_t *epGtw)
 Close gateway-ascii sockets. More...
 
void CO_epoll_initCANopenGtw (CO_epoll_gtw_t *epGtw, CO_t *co)
 Initialization of gateway functions in CANopen reset-communication section. More...
 
void CO_epoll_processGtw (CO_epoll_gtw_t *epGtw, CO_t *co, CO_epoll_t *ep)
 Process CANopen gateway functions. More...
 
+

Detailed Description

+

Linux epoll interface to CANopenNode.

+

The Linux epoll API performs a monitoring multiple file descriptors to see if I/O is possible on any of them.

+

CANopenNode uses epoll interface to provide an event based mechanism. Epoll waits for multiple different events, such as: interval timer event, notification event, CAN receive event or socket based event for gateway. CANopenNode non-blocking functions are processed after each event.

+

CANopenNode itself offers functionality for calculation of time, when next interval timer event should trigger the processing. It can also trigger notification events in case of multi-thread operation.

+

Function Documentation

+ +

◆ CO_epoll_create()

+ +
+
+ + + + + + + + + + + + + + + + + + +
CO_ReturnError_t CO_epoll_create (CO_epoll_t * ep,
uint32_t timerInterval_us 
)
+
+ +

Create Linux epoll, timerfd and eventfd.

+

Create and configure multiple Linux notification facilities, which trigger execution of the task. Epoll blocks and monitors multiple file descriptors, timerfd triggers in constant timer intervals and eventfd triggers on external signal.

+
Parameters
+ + + +
epThis object
timerInterval_usTimer interval in microseconds
+
+
+
Returns
CO_ReturnError_t CO_ERROR_NO, CO_ERROR_ILLEGAL_ARGUMENT or CO_ERROR_SYSCALL.
+ +
+
+ +

◆ CO_epoll_close()

+ +
+
+ + + + + + + + +
void CO_epoll_close (CO_epoll_t * ep)
+
+ +

Close epoll, timerfd and eventfd.

+
Parameters
+ + +
epThis object
+
+
+ +
+
+ +

◆ CO_epoll_wait()

+ +
+
+ + + + + + + + +
void CO_epoll_wait (CO_epoll_t * ep)
+
+ +

Wait for an epoll event.

+

This function blocks until event registered on epoll: timerfd, eventfd, or application specified event. Function also calculates timeDifference_us since last call and prepares timerNext_us.

+
Parameters
+ + +
epThis object
+
+
+ +
+
+ +

◆ CO_epoll_processLast()

+ +
+
+ + + + + + + + +
void CO_epoll_processLast (CO_epoll_t * ep)
+
+ +

Closing function for an epoll event.

+

This function must be called after CO_epoll_wait(). Between them should be application specified processing functions, which can check for own events and do own processing. Application may also lower timerNext_us variable. If lowered, then interval timer will be reconfigured and CO_epoll_wait() will be triggered earlier.

+
Parameters
+ + +
epThis object
+
+
+ +
+
+ +

◆ CO_epoll_initCANopenMain()

+ +
+
+ + + + + + + + + + + + + + + + + + +
void CO_epoll_initCANopenMain (CO_epoll_t * ep,
CO_t * co 
)
+
+ +

Initialization of functions in CANopen reset-communication section.

+

Configure callbacks for CANopen objects.

+
Parameters
+ + + +
epThis object
coCANopen object
+
+
+ +
+
+ +

◆ CO_epoll_processMain()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
void CO_epoll_processMain (CO_epoll_t * ep,
CO_t * co,
bool_t enableGateway,
CO_NMT_reset_cmd_t * reset 
)
+
+ +

Process CANopen mainline functions.

+

This function calls CO_process(). It is non-blocking and should execute cyclically. It should be between CO_epoll_wait() and CO_epoll_processLast() functions.

+
Parameters
+ + + + + +
epThis object
coCANopen object
enableGatewayIf true, gateway to external world will be enabled.
[out]resetReturn from CO_process().
+
+
+ +
+
+ +

◆ CO_epoll_processRT()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + +
void CO_epoll_processRT (CO_epoll_t * ep,
CO_t * co,
bool_t realtime 
)
+
+ +

Process CAN receive and realtime functions.

+

This function checks epoll for CAN receive event and processes CANopen realtime functions: CO_process_SYNC(), CO_process_RPDO() and CO_process_TPDO(). It is non-blocking and should execute cyclically. It should be between CO_epoll_wait() and CO_epoll_processLast() functions.

+

Function can be used in the mainline thread or in own realtime thread.

+

Processing of CANopen realtime functions is protected with CO_LOCK_OD. Also Node-Id must be configured and CANmodule must be in CANnormal for processing.

+
Parameters
+ + + + +
epPointer to CO_epoll_t object.
coCANopen object
realtimeSet to true, if function is called from the own realtime thread, and is executed at short constant interval.
+
+
+ +
+
+ +

◆ CO_epoll_createGtw()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
CO_ReturnError_t CO_epoll_createGtw (CO_epoll_gtw_t * epGtw,
int epoll_fd,
int32_t commandInterface,
uint32_t socketTimeout_ms,
char * localSocketPath 
)
+
+ +

Create socket for gateway-ascii command interface and add it to epoll.

+

Depending on arguments function configures stdio interface or local socket or IP socket.

+
Parameters
+ + + + + + +
epGtwThis object
epoll_fdAlready configured epoll file descriptor
commandInterfaceCommand interface type from CO_commandInterface_t
socketTimeout_msTimeout for established socket connection in [ms]
localSocketPathFile path, if commandInterface is local socket
+
+
+
Returns
CO_ReturnError_t CO_ERROR_NO, CO_ERROR_ILLEGAL_ARGUMENT or CO_ERROR_SYSCALL.
+ +
+
+ +

◆ CO_epoll_closeGtw()

+ +
+
+ + + + + + + + +
void CO_epoll_closeGtw (CO_epoll_gtw_t * epGtw)
+
+ +

Close gateway-ascii sockets.

+
Parameters
+ + +
epGtwThis object
+
+
+ +
+
+ +

◆ CO_epoll_initCANopenGtw()

+ +
+
+ + + + + + + + + + + + + + + + + + +
void CO_epoll_initCANopenGtw (CO_epoll_gtw_t * epGtw,
CO_t * co 
)
+
+ +

Initialization of gateway functions in CANopen reset-communication section.

+
Parameters
+ + + +
epGtwThis object
coCANopen object
+
+
+ +
+
+ +

◆ CO_epoll_processGtw()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + +
void CO_epoll_processGtw (CO_epoll_gtw_t * epGtw,
CO_t * co,
CO_epoll_t * ep 
)
+
+ +

Process CANopen gateway functions.

+

This function checks for epoll events and verifies socket connection timeout. It is non-blocking and should execute cyclically. It should be between CO_epoll_wait() and CO_epoll_processLast() functions.

+
Parameters
+ + + + +
epGtwThis object
coCANopen object
epPointer to CO_epoll_t object.
+
+
+ +
+
+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__epoll__interface.js b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__epoll__interface.js new file mode 100755 index 0000000..5994c00 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__epoll__interface.js @@ -0,0 +1,39 @@ +var group__CO__epoll__interface = +[ + [ "CO_epoll_interface.h", "CO__epoll__interface_8h.html", null ], + [ "CO_epoll_t", "structCO__epoll__t.html", [ + [ "epoll_fd", "structCO__epoll__t.html#a6e4aeb640b634a49a290aa9b4bc3b517", null ], + [ "event_fd", "structCO__epoll__t.html#af0e85776b360999b0830c943eb4505eb", null ], + [ "timer_fd", "structCO__epoll__t.html#aef151da1fe1a293f34f4f546c24bb0f7", null ], + [ "timerInterval_us", "structCO__epoll__t.html#a0595fb48d72a8592e6649e3de7b3477c", null ], + [ "timeDifference_us", "structCO__epoll__t.html#a46f38181dc8483ce83af566ee3f8aff3", null ], + [ "timerNext_us", "structCO__epoll__t.html#a0cb262435f594f9d7ea42eb976294262", null ], + [ "timerEvent", "structCO__epoll__t.html#a9fa1c50ecb111049eda7e45132d3dfd1", null ], + [ "previousTime_us", "structCO__epoll__t.html#aa047c5d68bb87515a1089cf5c75d847f", null ], + [ "tm", "structCO__epoll__t.html#a759aff5ae2eb019aa2ce8117f9ce9836", null ], + [ "ev", "structCO__epoll__t.html#a5ace69fd90506eae4f3f538fcb160c01", null ], + [ "epoll_new", "structCO__epoll__t.html#ac4113f19873fc3217e1e997a28f5bdd4", null ] + ] ], + [ "CO_epoll_gtw_t", "structCO__epoll__gtw__t.html", [ + [ "epoll_fd", "structCO__epoll__gtw__t.html#a7804e63fe1f7f5f6df68b32d8c25da2d", null ], + [ "commandInterface", "structCO__epoll__gtw__t.html#a96544bbc848d4627f3147496cc40d9f4", null ], + [ "socketTimeout_us", "structCO__epoll__gtw__t.html#a876c2b8ae6e9585155c5cf035c876c91", null ], + [ "socketTimeoutTmr_us", "structCO__epoll__gtw__t.html#a94ff0ff20f321b749202d9f3aa92a66d", null ], + [ "localSocketPath", "structCO__epoll__gtw__t.html#a5f11979c2bc301d6b8cc7ddce8d45688", null ], + [ "gtwa_fdSocket", "structCO__epoll__gtw__t.html#a418eccf136c9f8e081d61f98289e0759", null ], + [ "gtwa_fd", "structCO__epoll__gtw__t.html#a79b9c968b1f44ad5a4c55c36174d6898", null ], + [ "freshCommand", "structCO__epoll__gtw__t.html#ae529dc4279406811f0b97593816e863c", null ] + ] ], + [ "CO_commandInterface_t", "group__CO__epoll__interface.html#gad5a4218d5775fd7cda81a8015e1ee6f0", null ], + [ "CO_epoll_create", "group__CO__epoll__interface.html#ga9bb687bf26f711ce0573581984b79443", null ], + [ "CO_epoll_close", "group__CO__epoll__interface.html#ga72c3ebdede1207e55e0915128f5a2c6a", null ], + [ "CO_epoll_wait", "group__CO__epoll__interface.html#ga5cfd1df62494cf9d9e85dbb9fe981a57", null ], + [ "CO_epoll_processLast", "group__CO__epoll__interface.html#ga2aef637d4f2f818a7d95a7bfac251132", null ], + [ "CO_epoll_initCANopenMain", "group__CO__epoll__interface.html#ga3668c8b600022ffecdd3c2fa643b5e7d", null ], + [ "CO_epoll_processMain", "group__CO__epoll__interface.html#gad79d73fbac0e816f81f75507bd164515", null ], + [ "CO_epoll_processRT", "group__CO__epoll__interface.html#ga6ed67073bc96e575bec6fceb627b1245", null ], + [ "CO_epoll_createGtw", "group__CO__epoll__interface.html#ga7165df8b37ca1ed59476773fa075470c", null ], + [ "CO_epoll_closeGtw", "group__CO__epoll__interface.html#gab2f3f7bb7d885799c462e95a45563b69", null ], + [ "CO_epoll_initCANopenGtw", "group__CO__epoll__interface.html#ga07eaf2c954bb09b6420acee62ff207c3", null ], + [ "CO_epoll_processGtw", "group__CO__epoll__interface.html#ga97aa0bc09684b0ed78b708198e663407", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__socketCAN.html b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__socketCAN.html new file mode 100755 index 0000000..5a2a1bc --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__socketCAN.html @@ -0,0 +1,135 @@ + + + + + + + +CANopenNode: socketCAN + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Modules
+
+
socketCAN
+
+
+ +

Linux specific interface to CANopenNode. +More...

+ + + + + + + + + + +

+Modules

 CO_driver_target.h
 
 Epoll interface
 
 CAN errors & Log
 
 OD storage
 
+

Detailed Description

+

Linux specific interface to CANopenNode.

+

Linux includes CAN interface inside its kernel, so called SocketCAN. It operates as a network device. For more information on Linux SocketCAN see https://www.kernel.org/doc/html/latest/networking/can.html

+

Linux specific files for interfacing with Linux SocketCAN are located inside "CANopenNode/socketCAN" directory.

+

CANopenNode runs as a set of non-blocking functions. It can run in single or multiple threads. Best approach for RT IO device can be with two threads:

+

Main references for Linux functions used here are Linux man pages and the book: The Linux Programming Interface by Michael Kerrisk.

+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__socketCAN.js b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__socketCAN.js new file mode 100755 index 0000000..4d52fe0 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__socketCAN.js @@ -0,0 +1,7 @@ +var group__CO__socketCAN = +[ + [ "CO_driver_target.h", "group__CO__socketCAN__driver__target.html", "group__CO__socketCAN__driver__target" ], + [ "Epoll interface", "group__CO__epoll__interface.html", "group__CO__epoll__interface" ], + [ "CAN errors & Log", "group__CO__socketCAN__ERROR.html", "group__CO__socketCAN__ERROR" ], + [ "OD storage", "group__CO__socketCAN__OD__storage.html", "group__CO__socketCAN__OD__storage" ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__socketCAN__ERROR.html b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__socketCAN__ERROR.html new file mode 100755 index 0000000..9f86af4 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__socketCAN__ERROR.html @@ -0,0 +1,430 @@ + + + + + + + +CANopenNode: CAN errors & Log + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Files | +Data Structures | +Macros | +Enumerations | +Functions
+
+
CAN errors & Log
socketCAN
+
+
+ + + + + +

+Files

file  CO_error.h
 CANopenNode Linux socketCAN Error handling.
 
+ + + + +

+Data Structures

struct  CO_CANinterfaceErrorhandler_t
 socketCAN interface error handling More...
 
+ + + + + + + +

+Macros

+#define CO_CANerror_NOACK_MAX   16
 This is how many NO-ACKs need to be received in a row to assume that no other nodes are connected to a bus and therefore are assuming listen-only.
 
#define CO_CANerror_LISTEN_ONLY   10
 This is how long we are going to block transmission if listen-only mode is active. More...
 
+ + + + +

+Enumerations

enum  CO_CANinterfaceState_t { CO_INTERFACE_ACTIVE, +CO_INTERFACE_LISTEN_ONLY, +CO_INTERFACE_BUS_OFF + }
 driver interface state More...
 
+ + + + + + + + + + + + + + + + + + + +

+Functions

void log_printf (int priority, const char *format,...)
 Message logging function. More...
 
void CO_CANerror_init (CO_CANinterfaceErrorhandler_t *CANerrorhandler, int fd, const char *ifname)
 Initialize CAN error handler. More...
 
void CO_CANerror_disable (CO_CANinterfaceErrorhandler_t *CANerrorhandler)
 Reset CAN error handler. More...
 
void CO_CANerror_rxMsg (CO_CANinterfaceErrorhandler_t *CANerrorhandler)
 Message received event. More...
 
CO_CANinterfaceState_t CO_CANerror_txMsg (CO_CANinterfaceErrorhandler_t *CANerrorhandler)
 Check if interface is ready for message transmission. More...
 
CO_CANinterfaceState_t CO_CANerror_rxMsgError (CO_CANinterfaceErrorhandler_t *CANerrorhandler, const struct can_frame *msg)
 Error message received event. More...
 
+

Detailed Description

+

CANopen Errors and System message log

+

Macro Definition Documentation

+ +

◆ CO_CANerror_LISTEN_ONLY

+ +
+
+ + + + +
#define CO_CANerror_LISTEN_ONLY   10
+
+ +

This is how long we are going to block transmission if listen-only mode is active.

+

Time is in seconds.

+ +
+
+

Enumeration Type Documentation

+ +

◆ CO_CANinterfaceState_t

+ +
+
+ + + + +
enum CO_CANinterfaceState_t
+
+ +

driver interface state

+

CAN hardware can be in the followning states:

    +
  • error active (OK)
    • +
  • error passive (Can't generate error flags)
    • +
  • bus off (no influence on bus)
    • +
+ + + + +
Enumerator
CO_INTERFACE_ACTIVE 

CAN error passive/active.

+
CO_INTERFACE_LISTEN_ONLY 

CAN error passive/active, but currently no other device on bus.

+
CO_INTERFACE_BUS_OFF 

CAN bus off.

+
+ +
+
+

Function Documentation

+ +

◆ log_printf()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + +
void log_printf (int priority,
const char * format,
 ... 
)
+
+ +

Message logging function.

+

Function must be defined by application. It should record log message to some place, for example syslog() call in Linux or logging functionality in CANopen gateway Gateway ASCII mapping.

+

By default system stores messages in /var/log/syslog file. Log can optionally be configured before, for example to filter out less critical errors than LOG_NOTICE, specify program name, print also process PID and print also to standard error, set 'user' type of program, use:

setlogmask (LOG_UPTO (LOG_NOTICE));
+
openlog ("exampleprog", LOG_PID | LOG_PERROR, LOG_USER);
+
Parameters
+ + + +
priorityone of LOG_EMERG, LOG_ALERT, LOG_CRIT, LOG_ERR, LOG_WARNING, LOG_NOTICE, LOG_INFO, LOG_DEBUG
formatformat string as in printf
+
+
+ +
+
+ +

◆ CO_CANerror_init()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + +
void CO_CANerror_init (CO_CANinterfaceErrorhandler_t * CANerrorhandler,
int fd,
const char * ifname 
)
+
+ +

Initialize CAN error handler.

+

We need one error handler per interface

+
Parameters
+ + + + +
CANerrorhandlerThis object will be initialized.
fdinterface file descriptor
ifnameinterface name as string
+
+
+ +
+
+ +

◆ CO_CANerror_disable()

+ +
+
+ + + + + + + + +
void CO_CANerror_disable (CO_CANinterfaceErrorhandler_t * CANerrorhandler)
+
+ +

Reset CAN error handler.

+
Parameters
+ + +
CANerrorhandlerCAN error object.
+
+
+ +
+
+ +

◆ CO_CANerror_rxMsg()

+ +
+
+ + + + + + + + +
void CO_CANerror_rxMsg (CO_CANinterfaceErrorhandler_t * CANerrorhandler)
+
+ +

Message received event.

+

When a message is received at least one other CAN module is connected. Function clears listenOnly and noackCounter error flags.

+
Parameters
+ + +
CANerrorhandlerCAN error object.
+
+
+ +
+
+ +

◆ CO_CANerror_txMsg()

+ +
+
+ + + + + + + + +
CO_CANinterfaceState_t CO_CANerror_txMsg (CO_CANinterfaceErrorhandler_t * CANerrorhandler)
+
+ +

Check if interface is ready for message transmission.

+

Message mustn't be transmitted if not ready.

+
Parameters
+ + +
CANerrorhandlerCAN error object.
+
+
+
Returns
CO_INTERFACE_ACTIVE message transmission ready
+ +
+
+ +

◆ CO_CANerror_rxMsgError()

+ +
+
+ + + + + + + + + + + + + + + + + + +
CO_CANinterfaceState_t CO_CANerror_rxMsgError (CO_CANinterfaceErrorhandler_t * CANerrorhandler,
const struct can_frame * msg 
)
+
+ +

Error message received event.

+

This handles all received error messages.

+
Parameters
+ + + +
CANerrorhandlerCAN error object.
msgreceived error message
+
+
+
Returns
CO_CANinterfaceState_t
+ +
+
+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__socketCAN__ERROR.js b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__socketCAN__ERROR.js new file mode 100755 index 0000000..5196591 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__socketCAN__ERROR.js @@ -0,0 +1,25 @@ +var group__CO__socketCAN__ERROR = +[ + [ "CO_error.h", "CO__error_8h.html", null ], + [ "CO_CANinterfaceErrorhandler_t", "structCO__CANinterfaceErrorhandler__t.html", [ + [ "fd", "structCO__CANinterfaceErrorhandler__t.html#a06e667f1f90ad62495aa2172d97305b8", null ], + [ "ifName", "structCO__CANinterfaceErrorhandler__t.html#a58a5219f8dad7dc1c98db2b463e6e005", null ], + [ "noackCounter", "structCO__CANinterfaceErrorhandler__t.html#a6d17a248ec3ce1f6c53f4315c0cb9282", null ], + [ "listenOnly", "structCO__CANinterfaceErrorhandler__t.html#a96ddaefd75e680898f93d4891f5fc195", null ], + [ "timestamp", "structCO__CANinterfaceErrorhandler__t.html#a615dfcafdb866ae951296333df35d1c7", null ], + [ "CANerrorStatus", "structCO__CANinterfaceErrorhandler__t.html#a3f9369fb469c76c66a5e06eb7b909c54", null ] + ] ], + [ "CO_CANerror_NOACK_MAX", "group__CO__socketCAN__ERROR.html#ga85f1fa06be5e9a8337e1ec86546ea72d", null ], + [ "CO_CANerror_LISTEN_ONLY", "group__CO__socketCAN__ERROR.html#ga02f9c19a953d2bd9005033c35c2cb6de", null ], + [ "CO_CANinterfaceState_t", "group__CO__socketCAN__ERROR.html#ga7bf6a56766c008531d32b4218a5a9061", [ + [ "CO_INTERFACE_ACTIVE", "group__CO__socketCAN__ERROR.html#gga7bf6a56766c008531d32b4218a5a9061ad22fcb069e808548dee28e4aae580c1a", null ], + [ "CO_INTERFACE_LISTEN_ONLY", "group__CO__socketCAN__ERROR.html#gga7bf6a56766c008531d32b4218a5a9061a9b74584a321fe8a89cf74e087b094581", null ], + [ "CO_INTERFACE_BUS_OFF", "group__CO__socketCAN__ERROR.html#gga7bf6a56766c008531d32b4218a5a9061a8e51897ec66a9b37865659bbc348e212", null ] + ] ], + [ "log_printf", "group__CO__socketCAN__ERROR.html#gac9aeec86e89e5525b4e13e3b1e21866d", null ], + [ "CO_CANerror_init", "group__CO__socketCAN__ERROR.html#ga30c3cb98d37aedf45d49643064fee4cd", null ], + [ "CO_CANerror_disable", "group__CO__socketCAN__ERROR.html#ga6a74902a35f8a260cdc3956b83c17c77", null ], + [ "CO_CANerror_rxMsg", "group__CO__socketCAN__ERROR.html#ga5d5ee1aac31cf0334108cd147b9c9038", null ], + [ "CO_CANerror_txMsg", "group__CO__socketCAN__ERROR.html#gac40e1c681a9721c91ed75c49afda913c", null ], + [ "CO_CANerror_rxMsgError", "group__CO__socketCAN__ERROR.html#ga975e329593af25c4467f3ddde5cf5f5c", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__socketCAN__OD__storage.html b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__socketCAN__OD__storage.html new file mode 100755 index 0000000..7c777a9 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__socketCAN__OD__storage.html @@ -0,0 +1,357 @@ + + + + + + + +CANopenNode: OD storage + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Files | +Data Structures | +Functions
+
+
OD storage
socketCAN
+
+
+ + + + + +

+Files

file  CO_OD_storage.h
 CANopen Object Dictionary storage object for Linux SocketCAN.
 
+ + + + +

+Data Structures

struct  CO_OD_storage_t
 Object Dictionary storage object. More...
 
+ + + + + + + + + + + + + + + + + + + + + + +

+Functions

+CO_SDO_abortCode_t CO_ODF_1010 (CO_ODF_arg_t *ODF_arg)
 Callback for use inside CO_OD_configure() function for OD object 1010.
 
+CO_SDO_abortCode_t CO_ODF_1011 (CO_ODF_arg_t *ODF_arg)
 Callback for use inside CO_OD_configure() function for OD object 1011.
 
int CO_OD_storage_saveSecure (uint8_t *odAddress, uint32_t odSize, char *filename)
 Save memory block to a file. More...
 
int CO_OD_storage_restoreSecure (char *filename)
 Remove OD storage file. More...
 
CO_ReturnError_t CO_OD_storage_init (CO_OD_storage_t *odStor, uint8_t *odAddress, uint32_t odSize, char *filename)
 Initialize OD storage object and load data from file. More...
 
CO_ReturnError_t CO_OD_storage_autoSave (CO_OD_storage_t *odStor, uint32_t timer1usDiff, uint32_t delay_us)
 Automatically save memory block if differs from file. More...
 
void CO_OD_storage_autoSaveClose (CO_OD_storage_t *odStor)
 Closes file opened by CO_OD_storage_autoSave. More...
 
+

Detailed Description

+

Object Dictionary storage implementation for CANopenNode on Linux

+

Function Documentation

+ +

◆ CO_OD_storage_saveSecure()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + +
int CO_OD_storage_saveSecure (uint8_t * odAddress,
uint32_t odSize,
char * filename 
)
+
+ +

Save memory block to a file.

+

Function renames current file to filename.old, copies contents from odAddress to filename, adds two bytes of CRC code. It then verifies the written file and in case of errors sets back the old file and returns error.

+

Function is used with CANopen OD object at index 1010.

+
Parameters
+ + + + +
odAddressAddress of the memory block, which will be stored.
odSizeSize of the above memory block.
filenameName of the file, where data will be stored.
+
+
+
Returns
0 on success, -1 on error.
+ +
+
+ +

◆ CO_OD_storage_restoreSecure()

+ +
+
+ + + + + + + + +
int CO_OD_storage_restoreSecure (char * filename)
+
+ +

Remove OD storage file.

+

Function renames current file to filename.old, then creates empty file and writes two bytes "-\n" to it. When program will start next time, default values are used for Object Dictionary. In case of error in renaming to .old it keeps the original file and returns error.

+

Writing data to file is secured with mutex CO_LOCK_OD.

+

Function is used with CANopen OD object at index 1011.

+
Parameters
+ + +
filenameName of the file.
+
+
+
Returns
0 on success, -1 on error.
+ +
+
+ +

◆ CO_OD_storage_init()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
CO_ReturnError_t CO_OD_storage_init (CO_OD_storage_t * odStor,
uint8_t * odAddress,
uint32_t odSize,
char * filename 
)
+
+ +

Initialize OD storage object and load data from file.

+

Called after program startup. Load storage file and copy data to Object Dictionary variables.

+
Parameters
+ + + + + +
odStorThis object will be initialized.
odAddressAddress of the memory block from Object dictionary, where data will be copied.
odSizeSize of the above memory block.
filenameName of the file, where data are stored.
+
+
+
Returns
CO_ReturnError_t: CO_ERROR_NO, CO_ERROR_DATA_CORRUPT (Data in file corrupt), CO_ERROR_CRC (CRC from MBR does not match the CRC of OD_ROM block in file), CO_ERROR_ILLEGAL_ARGUMENT or CO_ERROR_OUT_OF_MEMORY (malloc failed).
+ +
+
+ +

◆ CO_OD_storage_autoSave()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + +
CO_ReturnError_t CO_OD_storage_autoSave (CO_OD_storage_t * odStor,
uint32_t timer1usDiff,
uint32_t delay_us 
)
+
+ +

Automatically save memory block if differs from file.

+

Should be called cyclically by program. It first verifies, if memory block differs from file and if it does, it saves it to file with two additional CRC bytes. File remains opened.

+
Parameters
+ + + + +
odStorOD storage object.
timer1usDiffTime difference in microseconds since last call.
delay_usDelay (inhibit) time between writes to disk in microseconds (60000 for example).
+
+
+
Returns
CO_ReturnError_t: CO_ERROR_NO, CO_ERROR_DATA_CORRUPT (Data in file corrupt), CO_ERROR_ILLEGAL_ARGUMENT or CO_ERROR_OUT_OF_MEMORY (malloc failed).
+ +
+
+ +

◆ CO_OD_storage_autoSaveClose()

+ +
+
+ + + + + + + + +
void CO_OD_storage_autoSaveClose (CO_OD_storage_t * odStor)
+
+ +

Closes file opened by CO_OD_storage_autoSave.

+
Parameters
+ + +
odStorOD storage object.
+
+
+ +
+
+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__socketCAN__OD__storage.js b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__socketCAN__OD__storage.js new file mode 100755 index 0000000..ca04032 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__socketCAN__OD__storage.js @@ -0,0 +1,18 @@ +var group__CO__socketCAN__OD__storage = +[ + [ "CO_OD_storage.h", "CO__OD__storage_8h.html", null ], + [ "CO_OD_storage_t", "structCO__OD__storage__t.html", [ + [ "odAddress", "structCO__OD__storage__t.html#ac434d6480330c026761fe8a82e32839b", null ], + [ "odSize", "structCO__OD__storage__t.html#aefd1cb33fa031c1592b20643bb38bfbb", null ], + [ "filename", "structCO__OD__storage__t.html#afb3dd08b01bf20f251754c32b116d8fe", null ], + [ "fp", "structCO__OD__storage__t.html#a8976cb73fd4a2baadc4689fdb8b876a1", null ], + [ "lastSavedUs", "structCO__OD__storage__t.html#a94d80f0c140485ab426891839b356347", null ] + ] ], + [ "CO_ODF_1010", "group__CO__socketCAN__OD__storage.html#ga4a5e807a83eeab172bb3b0aeb6fa92c2", null ], + [ "CO_ODF_1011", "group__CO__socketCAN__OD__storage.html#ga059fcd46d8b15caf86c57d541a09576a", null ], + [ "CO_OD_storage_saveSecure", "group__CO__socketCAN__OD__storage.html#ga7f6124c9079807bc2f8f3d860571ccec", null ], + [ "CO_OD_storage_restoreSecure", "group__CO__socketCAN__OD__storage.html#ga63b392fa7eb2bdc92ecd2f1ff6f4ced0", null ], + [ "CO_OD_storage_init", "group__CO__socketCAN__OD__storage.html#ga5a26b63e7222b058c6024e54c6e0d5cd", null ], + [ "CO_OD_storage_autoSave", "group__CO__socketCAN__OD__storage.html#ga9381d4f670cef9068efaf7d42097de2f", null ], + [ "CO_OD_storage_autoSaveClose", "group__CO__socketCAN__OD__storage.html#ga6c51a86516ff7e128873f6cf1fdef5eb", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__socketCAN__driver__target.html b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__socketCAN__driver__target.html new file mode 100755 index 0000000..ba82abf --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__socketCAN__driver__target.html @@ -0,0 +1,392 @@ + + + + + + + +CANopenNode: CO_driver_target.h + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Files | +Macros | +Functions
+
+
CO_driver_target.h
socketCAN
+
+
+ + + + + +

+Files

file  CO_driver_target.h
 Linux socketCAN specific definitions for CANopenNode.
 
+ + + + + + + +

+Macros

#define CO_DRIVER_MULTI_INTERFACE   0
 Multi interface support. More...
 
#define CO_DRIVER_ERROR_REPORTING   1
 CAN bus error reporting. More...
 
+ + + + + + + + + + + + + +

+Functions

CO_ReturnError_t CO_CANmodule_addInterface (CO_CANmodule_t *CANmodule, int can_ifindex)
 Add socketCAN interface to can driver. More...
 
bool_t CO_CANrxBuffer_getInterface (CO_CANmodule_t *CANmodule, uint16_t ident, const void **const CANptrRx, struct timespec *timestamp)
 Check on which interface the last message for one message buffer was received. More...
 
CO_ReturnError_t CO_CANtxBuffer_setInterface (CO_CANmodule_t *CANmodule, uint16_t ident, const void *CANptrTx)
 Set which interface should be used for message buffer transmission. More...
 
bool_t CO_CANrxFromEpoll (CO_CANmodule_t *CANmodule, struct epoll_event *ev, CO_CANrxMsg_t *buffer, int32_t *msgIndex)
 Receives CAN messages from matching epoll event. More...
 
+

Detailed Description

+

Linux socketCAN specific Driver definitions for CANopenNode.

+

Macro Definition Documentation

+ +

◆ CO_DRIVER_MULTI_INTERFACE

+ +
+
+ + + + +
#define CO_DRIVER_MULTI_INTERFACE   0
+
+ +

Multi interface support.

+

Enable this to use interface combining at driver level. This adds functions to broadcast/selective transmit messages on the given interfaces as well as combining all received message into one queue.

+

If CO_DRIVER_MULTI_INTERFACE is set to 0, then CO_CANmodule_init() adds single socketCAN interface specified by CANptr argument. In case of failure, CO_CANmodule_init() returns CO_ERROR_SYSCALL.

+

If CO_DRIVER_MULTI_INTERFACE is set to 1, then CO_CANmodule_init() ignores CANptr argument. Interfaces must be added by CO_CANmodule_addInterface() function after CO_CANmodule_init().

+

Macro is set to 0 (disabled) by default. It can be overridden.

+

This is not intended to realize interface redundancy!!!

+ +
+
+ +

◆ CO_DRIVER_ERROR_REPORTING

+ +
+
+ + + + +
#define CO_DRIVER_ERROR_REPORTING   1
+
+ +

CAN bus error reporting.

+

CO_DRIVER_ERROR_REPORTING enabled adds support for socketCAN error detection and handling functions inside the driver. This is needed when you have CANopen with "0" connected nodes as a use case, as this is normally forbidden in CAN.

+

Macro is set to 1 (enabled) by default. It can be overridden.

+

you need to enable error reporting in your kernel driver using:

ip link set canX type can berr-reporting on
+

Of course, the kernel driver for your hardware needs this functionality to be implemented...

+ +
+
+

Function Documentation

+ +

◆ CO_CANmodule_addInterface()

+ +
+
+ + + + + + + + + + + + + + + + + + +
CO_ReturnError_t CO_CANmodule_addInterface (CO_CANmodule_t * CANmodule,
int can_ifindex 
)
+
+ +

Add socketCAN interface to can driver.

+

Function must be called after CO_CANmodule_init.

+
Parameters
+ + + +
CANmoduleThis object will be initialized.
can_ifindexCAN Interface index
+
+
+
Returns
CO_ReturnError_t: CO_ERROR_NO, CO_ERROR_ILLEGAL_ARGUMENT, CO_ERROR_SYSCALL or CO_ERROR_INVALID_STATE.
+ +
+
+ +

◆ CO_CANrxBuffer_getInterface()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
bool_t CO_CANrxBuffer_getInterface (CO_CANmodule_t * CANmodule,
uint16_t ident,
const void **const CANptrRx,
struct timespec * timestamp 
)
+
+ +

Check on which interface the last message for one message buffer was received.

+

It is in the responsibility of the user to check that this information is useful as some messages can be received at any time on any bus.

+
Parameters
+ + + + + +
CANmoduleThis object.
ident11-bit standard CAN Identifier.
[out]CANptrRxmessage was received on this interface
[out]timestampmessage was received at this time (system clock)
+
+
+
Return values
+ + + +
falsemessage has never been received, therefore no base address and timestamp are available
truebase address and timestamp are valid
+
+
+ +
+
+ +

◆ CO_CANtxBuffer_setInterface()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + +
CO_ReturnError_t CO_CANtxBuffer_setInterface (CO_CANmodule_t * CANmodule,
uint16_t ident,
const void * CANptrTx 
)
+
+ +

Set which interface should be used for message buffer transmission.

+

It is in the responsibility of the user to ensure that the correct interface is used. Some messages need to be transmitted on all interfaces.

+

If given interface is unknown or NULL is used, a message is transmitted on all available interfaces.

+
Parameters
+ + + + +
CANmoduleThis object.
ident11-bit standard CAN Identifier.
CANptrTxuse this interface. NULL = not specified
+
+
+
Returns
CO_ReturnError_t: CO_ERROR_NO or CO_ERROR_ILLEGAL_ARGUMENT.
+ +
+
+ +

◆ CO_CANrxFromEpoll()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
bool_t CO_CANrxFromEpoll (CO_CANmodule_t * CANmodule,
struct epoll_event * ev,
CO_CANrxMsg_t * buffer,
int32_t * msgIndex 
)
+
+ +

Receives CAN messages from matching epoll event.

+

This function verifies, if epoll event matches event from any CANinterface. In case of match, message is read from CAN and pre-processed for CANopenNode objects. CAN error frames are also processed.

+

In case of CAN message function searches rxArray from CO_CANmodule_t and if matched it calls the corresponding CANrx_callback, optionally copies received CAN message to buffer and returns index of matched rxArray.

+

This function can be used in two ways, which can be combined:

    +
  • automatic mode: If CANrx_callback is specified for matched rxArray, then calls its callback.
    • +
  • manual mode: evaluate message filters, return received message
    • +
+
Parameters
+ + + + + +
CANmoduleThis object.
evEpoll event, which vill be verified for matches.
[out]bufferStorage for received message or NULL if not used.
[out]msgIndexIndex of received message in array from CO_CANmodule_t rxArray, copy of CAN message is available in buffer.
+
+
+
Returns
True, if epoll event matches any CAN interface.
+ +
+
+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__socketCAN__driver__target.js b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__socketCAN__driver__target.js new file mode 100755 index 0000000..55d9af4 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__socketCAN__driver__target.js @@ -0,0 +1,10 @@ +var group__CO__socketCAN__driver__target = +[ + [ "CO_driver_target.h", "CO__driver__target_8h.html", null ], + [ "CO_DRIVER_MULTI_INTERFACE", "group__CO__socketCAN__driver__target.html#ga63d0056e7f18144c6eee7b66f196377c", null ], + [ "CO_DRIVER_ERROR_REPORTING", "group__CO__socketCAN__driver__target.html#ga88077a1ecc6ae53de0b40ae092d48452", null ], + [ "CO_CANmodule_addInterface", "group__CO__socketCAN__driver__target.html#gaaffa26125993a7a1f6cbfdb468b59333", null ], + [ "CO_CANrxBuffer_getInterface", "group__CO__socketCAN__driver__target.html#gaf266a58e21acf104d9f19cd0da704afe", null ], + [ "CO_CANtxBuffer_setInterface", "group__CO__socketCAN__driver__target.html#ga0d7a8fdf7a2fafd4c6d2f2dd1e1017b0", null ], + [ "CO_CANrxFromEpoll", "group__CO__socketCAN__driver__target.html#ga072c53260a32d7cd30d9ad1b5bb2c359", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__trace.html b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__trace.html new file mode 100755 index 0000000..18eb191 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__trace.html @@ -0,0 +1,328 @@ + + + + + + + +CANopenNode: Trace + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Files | +Data Structures | +Macros | +Functions
+
+
Trace
CANopen_extra
+
+
+ + + + + +

+Files

file  CO_trace.h
 CANopen trace object for recording variables over time.
 
+ + + + + + + +

+Data Structures

struct  CO_trace_dataType_t
 structure for reading variables and printing points for specific data type. More...
 
struct  CO_trace_t
 Trace object. More...
 
+ + + + +

+Macros

+#define OD_INDEX_TRACE_CONFIG   0x2301
 Start index of traceConfig and Trace objects in Object Dictionary.
 
+ + + + + + + +

+Functions

void CO_trace_init (CO_trace_t *trace, CO_SDO_t *SDO, uint8_t enabled, uint32_t *timeBuffer, int32_t *valueBuffer, uint32_t bufferSize, uint32_t *map, uint8_t *format, uint8_t *trigger, int32_t *threshold, int32_t *value, int32_t *minValue, int32_t *maxValue, uint32_t *triggerTime, uint16_t idx_OD_traceConfig, uint16_t idx_OD_trace)
 Initialize trace object. More...
 
void CO_trace_process (CO_trace_t *trace, uint32_t timestamp)
 Process trace object. More...
 
+

Detailed Description

+

CANopen trace object for recording variables over time.

+

In embedded systems there is often a need to monitor some variables over time. Results are then displayed on graph, similar as in oscilloscope.

+

CANopen trace is a configurable object, accessible via CANopen Object Dictionary, which records chosen variable over time. It generates a curve, which can be read via SDO and can then be displayed in a graph.

+

CO_trace_process() runs in 1 ms intervals and monitors one variable. If it changes, it makes a record with timestamp into circular buffer. When trace is accessed by CANopen SDO object, it reads latest points from the the circular buffer, prints a SVG curve into string and sends it as a SDO response. If a SDO request was received from the same device, then no traffic occupies CAN network.

+

Function Documentation

+ +

◆ CO_trace_init()

+ +
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
void CO_trace_init (CO_trace_t * trace,
CO_SDO_t * SDO,
uint8_t enabled,
uint32_t * timeBuffer,
int32_t * valueBuffer,
uint32_t bufferSize,
uint32_t * map,
uint8_t * format,
uint8_t * trigger,
int32_t * threshold,
int32_t * value,
int32_t * minValue,
int32_t * maxValue,
uint32_t * triggerTime,
uint16_t idx_OD_traceConfig,
uint16_t idx_OD_trace 
)
+
+ +

Initialize trace object.

+

Function must be called in the communication reset section.

+
Parameters
+ + + + + + + + + + + + + + + + + +
traceThis object will be initialized.
SDOSDO server object.
enabledIs trace enabled.
timeBufferMemory block for storing time records.
valueBufferMemory block for storing value records.
bufferSizeSize of the above buffers.
mapMap to variable in Object Dictionary, which will be monitored. Same structure as in PDO.
formatFormat of the plot. If first bit is 1, above variable is unsigned. For more info see Object Dictionary.
triggerIf different than zero, trigger time is recorded, when variable goes through threshold.
thresholdUsed with trigger.
valuePointer to variable, which will show last value of the variable.
minValuePointer to variable, which will show minimum value of the variable.
maxValuePointer to variable, which will show maximum value of the variable.
triggerTimePointer to variable, which will show last trigger time of the variable.
idx_OD_traceConfigIndex in Object Dictionary.
idx_OD_traceIndex in Object Dictionary.
+
+
+
Returns
0 on success, -1 on error.
+ +
+
+ +

◆ CO_trace_process()

+ +
+
+ + + + + + + + + + + + + + + + + + +
void CO_trace_process (CO_trace_t * trace,
uint32_t timestamp 
)
+
+ +

Process trace object.

+

Function must be called cyclically in 1ms intervals.

+
Parameters
+ + + +
traceThis object.
timestampTimestamp (usually in millisecond resolution).
+
+
+
Returns
0 on success, -1 on error.
+ +
+
+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__trace.js b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__trace.js new file mode 100755 index 0000000..14f85b7 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/group__CO__trace.js @@ -0,0 +1,34 @@ +var group__CO__trace = +[ + [ "CO_trace.h", "CO__trace_8h.html", null ], + [ "CO_trace_dataType_t", "structCO__trace__dataType__t.html", [ + [ "pGetValue", "structCO__trace__dataType__t.html#a8ac5cd02c39b591354149f8c5d6357f7", null ], + [ "printPointStart", "structCO__trace__dataType__t.html#a751d9b94aba8f663de5b5f7c7f074893", null ], + [ "printPoint", "structCO__trace__dataType__t.html#aef2341fd443f3aa33a11db1b429b7dc1", null ], + [ "printPointEnd", "structCO__trace__dataType__t.html#a48ec81e33885a9114f2ea0be237a0059", null ] + ] ], + [ "CO_trace_t", "structCO__trace__t.html", [ + [ "enabled", "structCO__trace__t.html#aefa5e9934aaac1f00d0a1866100dae50", null ], + [ "SDO", "structCO__trace__t.html#a33616f410feb93ddb6a2ac238027996b", null ], + [ "timeBuffer", "structCO__trace__t.html#a83806299bd57a23f5946f81cb05ab41d", null ], + [ "valueBuffer", "structCO__trace__t.html#abc2f00a5f99453c77dd8515f3526e428", null ], + [ "bufferSize", "structCO__trace__t.html#a3dfe996be2bef106f50de085a7f5ca9f", null ], + [ "writePtr", "structCO__trace__t.html#ae3a556a180e38e7247b39b84de609b5d", null ], + [ "readPtr", "structCO__trace__t.html#af439eee35bf45f6ec14a32270577ff85", null ], + [ "lastTimeStamp", "structCO__trace__t.html#af3ffa370f4b25a3c3fd27f9ce75379c0", null ], + [ "OD_variable", "structCO__trace__t.html#a2d71398a641edb4fe095aabb6e81f834", null ], + [ "dt", "structCO__trace__t.html#a31e42b2511450377294475fcf0189d89", null ], + [ "valuePrev", "structCO__trace__t.html#abe713136228c54327c1540b99a1a2fd1", null ], + [ "map", "structCO__trace__t.html#ad6e329507a29f46b41ea6bef210b6502", null ], + [ "format", "structCO__trace__t.html#a3ea91e63f2c2f6c34b3ee1ae0f84e939", null ], + [ "value", "structCO__trace__t.html#a24fa467aeeb1581c6a3272bd397a2f10", null ], + [ "minValue", "structCO__trace__t.html#a0f1b287dae5b6a30b43f481d2bee41ab", null ], + [ "maxValue", "structCO__trace__t.html#a7b41b99eb65d49fe522c25d08127f81d", null ], + [ "triggerTime", "structCO__trace__t.html#a1b7176f0ad48679449a666c34fe45a3f", null ], + [ "trigger", "structCO__trace__t.html#a879f362c9d4a88de9e48d23c4d0c770c", null ], + [ "threshold", "structCO__trace__t.html#a6400336e1207dcfa5fa8ef2a908fda08", null ] + ] ], + [ "OD_INDEX_TRACE_CONFIG", "group__CO__trace.html#ga4edaf4c29180028f5f128f0aed2a417d", null ], + [ "CO_trace_init", "group__CO__trace.html#ga50621e5b8e28349f7caf260b6cf9962e", null ], + [ "CO_trace_process", "group__CO__trace.html#ga5b8d9460d7a42920325cf66eb8b725ec", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/jquery.js b/Devices/Libraries/Systems/CANopenSocket/docs/jquery.js new file mode 100755 index 0000000..103c32d --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/jquery.js @@ -0,0 +1,35 @@ +/*! jQuery v3.4.1 | (c) JS Foundation and other contributors | jquery.org/license */ +!function(e,t){"use strict";"object"==typeof module&&"object"==typeof module.exports?module.exports=e.document?t(e,!0):function(e){if(!e.document)throw new Error("jQuery requires a window with a document");return t(e)}:t(e)}("undefined"!=typeof window?window:this,function(C,e){"use strict";var t=[],E=C.document,r=Object.getPrototypeOf,s=t.slice,g=t.concat,u=t.push,i=t.indexOf,n={},o=n.toString,v=n.hasOwnProperty,a=v.toString,l=a.call(Object),y={},m=function(e){return"function"==typeof e&&"number"!=typeof e.nodeType},x=function(e){return null!=e&&e===e.window},c={type:!0,src:!0,nonce:!0,noModule:!0};function b(e,t,n){var r,i,o=(n=n||E).createElement("script");if(o.text=e,t)for(r in c)(i=t[r]||t.getAttribute&&t.getAttribute(r))&&o.setAttribute(r,i);n.head.appendChild(o).parentNode.removeChild(o)}function w(e){return null==e?e+"":"object"==typeof e||"function"==typeof e?n[o.call(e)]||"object":typeof e}var f="3.4.1",k=function(e,t){return new k.fn.init(e,t)},p=/^[\s\uFEFF\xA0]+|[\s\uFEFF\xA0]+$/g;function d(e){var t=!!e&&"length"in e&&e.length,n=w(e);return!m(e)&&!x(e)&&("array"===n||0===t||"number"==typeof t&&0+~]|"+M+")"+M+"*"),U=new RegExp(M+"|>"),X=new RegExp($),V=new RegExp("^"+I+"$"),G={ID:new RegExp("^#("+I+")"),CLASS:new RegExp("^\\.("+I+")"),TAG:new RegExp("^("+I+"|[*])"),ATTR:new RegExp("^"+W),PSEUDO:new RegExp("^"+$),CHILD:new RegExp("^:(only|first|last|nth|nth-last)-(child|of-type)(?:\\("+M+"*(even|odd|(([+-]|)(\\d*)n|)"+M+"*(?:([+-]|)"+M+"*(\\d+)|))"+M+"*\\)|)","i"),bool:new RegExp("^(?:"+R+")$","i"),needsContext:new RegExp("^"+M+"*[>+~]|:(even|odd|eq|gt|lt|nth|first|last)(?:\\("+M+"*((?:-\\d)?\\d*)"+M+"*\\)|)(?=[^-]|$)","i")},Y=/HTML$/i,Q=/^(?:input|select|textarea|button)$/i,J=/^h\d$/i,K=/^[^{]+\{\s*\[native \w/,Z=/^(?:#([\w-]+)|(\w+)|\.([\w-]+))$/,ee=/[+~]/,te=new RegExp("\\\\([\\da-f]{1,6}"+M+"?|("+M+")|.)","ig"),ne=function(e,t,n){var r="0x"+t-65536;return r!=r||n?t:r<0?String.fromCharCode(r+65536):String.fromCharCode(r>>10|55296,1023&r|56320)},re=/([\0-\x1f\x7f]|^-?\d)|^-$|[^\0-\x1f\x7f-\uFFFF\w-]/g,ie=function(e,t){return t?"\0"===e?"\ufffd":e.slice(0,-1)+"\\"+e.charCodeAt(e.length-1).toString(16)+" ":"\\"+e},oe=function(){T()},ae=be(function(e){return!0===e.disabled&&"fieldset"===e.nodeName.toLowerCase()},{dir:"parentNode",next:"legend"});try{H.apply(t=O.call(m.childNodes),m.childNodes),t[m.childNodes.length].nodeType}catch(e){H={apply:t.length?function(e,t){L.apply(e,O.call(t))}:function(e,t){var n=e.length,r=0;while(e[n++]=t[r++]);e.length=n-1}}}function se(t,e,n,r){var i,o,a,s,u,l,c,f=e&&e.ownerDocument,p=e?e.nodeType:9;if(n=n||[],"string"!=typeof t||!t||1!==p&&9!==p&&11!==p)return n;if(!r&&((e?e.ownerDocument||e:m)!==C&&T(e),e=e||C,E)){if(11!==p&&(u=Z.exec(t)))if(i=u[1]){if(9===p){if(!(a=e.getElementById(i)))return n;if(a.id===i)return n.push(a),n}else if(f&&(a=f.getElementById(i))&&y(e,a)&&a.id===i)return n.push(a),n}else{if(u[2])return H.apply(n,e.getElementsByTagName(t)),n;if((i=u[3])&&d.getElementsByClassName&&e.getElementsByClassName)return H.apply(n,e.getElementsByClassName(i)),n}if(d.qsa&&!A[t+" "]&&(!v||!v.test(t))&&(1!==p||"object"!==e.nodeName.toLowerCase())){if(c=t,f=e,1===p&&U.test(t)){(s=e.getAttribute("id"))?s=s.replace(re,ie):e.setAttribute("id",s=k),o=(l=h(t)).length;while(o--)l[o]="#"+s+" "+xe(l[o]);c=l.join(","),f=ee.test(t)&&ye(e.parentNode)||e}try{return H.apply(n,f.querySelectorAll(c)),n}catch(e){A(t,!0)}finally{s===k&&e.removeAttribute("id")}}}return g(t.replace(B,"$1"),e,n,r)}function ue(){var r=[];return function e(t,n){return r.push(t+" ")>b.cacheLength&&delete e[r.shift()],e[t+" "]=n}}function le(e){return e[k]=!0,e}function ce(e){var t=C.createElement("fieldset");try{return!!e(t)}catch(e){return!1}finally{t.parentNode&&t.parentNode.removeChild(t),t=null}}function fe(e,t){var n=e.split("|"),r=n.length;while(r--)b.attrHandle[n[r]]=t}function pe(e,t){var n=t&&e,r=n&&1===e.nodeType&&1===t.nodeType&&e.sourceIndex-t.sourceIndex;if(r)return r;if(n)while(n=n.nextSibling)if(n===t)return-1;return e?1:-1}function de(t){return function(e){return"input"===e.nodeName.toLowerCase()&&e.type===t}}function he(n){return function(e){var t=e.nodeName.toLowerCase();return("input"===t||"button"===t)&&e.type===n}}function ge(t){return function(e){return"form"in e?e.parentNode&&!1===e.disabled?"label"in e?"label"in e.parentNode?e.parentNode.disabled===t:e.disabled===t:e.isDisabled===t||e.isDisabled!==!t&&ae(e)===t:e.disabled===t:"label"in e&&e.disabled===t}}function ve(a){return le(function(o){return o=+o,le(function(e,t){var n,r=a([],e.length,o),i=r.length;while(i--)e[n=r[i]]&&(e[n]=!(t[n]=e[n]))})})}function ye(e){return e&&"undefined"!=typeof e.getElementsByTagName&&e}for(e in d=se.support={},i=se.isXML=function(e){var t=e.namespaceURI,n=(e.ownerDocument||e).documentElement;return!Y.test(t||n&&n.nodeName||"HTML")},T=se.setDocument=function(e){var t,n,r=e?e.ownerDocument||e:m;return r!==C&&9===r.nodeType&&r.documentElement&&(a=(C=r).documentElement,E=!i(C),m!==C&&(n=C.defaultView)&&n.top!==n&&(n.addEventListener?n.addEventListener("unload",oe,!1):n.attachEvent&&n.attachEvent("onunload",oe)),d.attributes=ce(function(e){return e.className="i",!e.getAttribute("className")}),d.getElementsByTagName=ce(function(e){return e.appendChild(C.createComment("")),!e.getElementsByTagName("*").length}),d.getElementsByClassName=K.test(C.getElementsByClassName),d.getById=ce(function(e){return a.appendChild(e).id=k,!C.getElementsByName||!C.getElementsByName(k).length}),d.getById?(b.filter.ID=function(e){var t=e.replace(te,ne);return function(e){return e.getAttribute("id")===t}},b.find.ID=function(e,t){if("undefined"!=typeof t.getElementById&&E){var n=t.getElementById(e);return n?[n]:[]}}):(b.filter.ID=function(e){var n=e.replace(te,ne);return function(e){var t="undefined"!=typeof e.getAttributeNode&&e.getAttributeNode("id");return t&&t.value===n}},b.find.ID=function(e,t){if("undefined"!=typeof t.getElementById&&E){var n,r,i,o=t.getElementById(e);if(o){if((n=o.getAttributeNode("id"))&&n.value===e)return[o];i=t.getElementsByName(e),r=0;while(o=i[r++])if((n=o.getAttributeNode("id"))&&n.value===e)return[o]}return[]}}),b.find.TAG=d.getElementsByTagName?function(e,t){return"undefined"!=typeof t.getElementsByTagName?t.getElementsByTagName(e):d.qsa?t.querySelectorAll(e):void 0}:function(e,t){var n,r=[],i=0,o=t.getElementsByTagName(e);if("*"===e){while(n=o[i++])1===n.nodeType&&r.push(n);return r}return o},b.find.CLASS=d.getElementsByClassName&&function(e,t){if("undefined"!=typeof t.getElementsByClassName&&E)return t.getElementsByClassName(e)},s=[],v=[],(d.qsa=K.test(C.querySelectorAll))&&(ce(function(e){a.appendChild(e).innerHTML="",e.querySelectorAll("[msallowcapture^='']").length&&v.push("[*^$]="+M+"*(?:''|\"\")"),e.querySelectorAll("[selected]").length||v.push("\\["+M+"*(?:value|"+R+")"),e.querySelectorAll("[id~="+k+"-]").length||v.push("~="),e.querySelectorAll(":checked").length||v.push(":checked"),e.querySelectorAll("a#"+k+"+*").length||v.push(".#.+[+~]")}),ce(function(e){e.innerHTML="";var t=C.createElement("input");t.setAttribute("type","hidden"),e.appendChild(t).setAttribute("name","D"),e.querySelectorAll("[name=d]").length&&v.push("name"+M+"*[*^$|!~]?="),2!==e.querySelectorAll(":enabled").length&&v.push(":enabled",":disabled"),a.appendChild(e).disabled=!0,2!==e.querySelectorAll(":disabled").length&&v.push(":enabled",":disabled"),e.querySelectorAll("*,:x"),v.push(",.*:")})),(d.matchesSelector=K.test(c=a.matches||a.webkitMatchesSelector||a.mozMatchesSelector||a.oMatchesSelector||a.msMatchesSelector))&&ce(function(e){d.disconnectedMatch=c.call(e,"*"),c.call(e,"[s!='']:x"),s.push("!=",$)}),v=v.length&&new RegExp(v.join("|")),s=s.length&&new RegExp(s.join("|")),t=K.test(a.compareDocumentPosition),y=t||K.test(a.contains)?function(e,t){var n=9===e.nodeType?e.documentElement:e,r=t&&t.parentNode;return e===r||!(!r||1!==r.nodeType||!(n.contains?n.contains(r):e.compareDocumentPosition&&16&e.compareDocumentPosition(r)))}:function(e,t){if(t)while(t=t.parentNode)if(t===e)return!0;return!1},D=t?function(e,t){if(e===t)return l=!0,0;var n=!e.compareDocumentPosition-!t.compareDocumentPosition;return n||(1&(n=(e.ownerDocument||e)===(t.ownerDocument||t)?e.compareDocumentPosition(t):1)||!d.sortDetached&&t.compareDocumentPosition(e)===n?e===C||e.ownerDocument===m&&y(m,e)?-1:t===C||t.ownerDocument===m&&y(m,t)?1:u?P(u,e)-P(u,t):0:4&n?-1:1)}:function(e,t){if(e===t)return l=!0,0;var n,r=0,i=e.parentNode,o=t.parentNode,a=[e],s=[t];if(!i||!o)return e===C?-1:t===C?1:i?-1:o?1:u?P(u,e)-P(u,t):0;if(i===o)return pe(e,t);n=e;while(n=n.parentNode)a.unshift(n);n=t;while(n=n.parentNode)s.unshift(n);while(a[r]===s[r])r++;return r?pe(a[r],s[r]):a[r]===m?-1:s[r]===m?1:0}),C},se.matches=function(e,t){return se(e,null,null,t)},se.matchesSelector=function(e,t){if((e.ownerDocument||e)!==C&&T(e),d.matchesSelector&&E&&!A[t+" "]&&(!s||!s.test(t))&&(!v||!v.test(t)))try{var n=c.call(e,t);if(n||d.disconnectedMatch||e.document&&11!==e.document.nodeType)return n}catch(e){A(t,!0)}return 0":{dir:"parentNode",first:!0}," ":{dir:"parentNode"},"+":{dir:"previousSibling",first:!0},"~":{dir:"previousSibling"}},preFilter:{ATTR:function(e){return e[1]=e[1].replace(te,ne),e[3]=(e[3]||e[4]||e[5]||"").replace(te,ne),"~="===e[2]&&(e[3]=" "+e[3]+" "),e.slice(0,4)},CHILD:function(e){return e[1]=e[1].toLowerCase(),"nth"===e[1].slice(0,3)?(e[3]||se.error(e[0]),e[4]=+(e[4]?e[5]+(e[6]||1):2*("even"===e[3]||"odd"===e[3])),e[5]=+(e[7]+e[8]||"odd"===e[3])):e[3]&&se.error(e[0]),e},PSEUDO:function(e){var t,n=!e[6]&&e[2];return G.CHILD.test(e[0])?null:(e[3]?e[2]=e[4]||e[5]||"":n&&X.test(n)&&(t=h(n,!0))&&(t=n.indexOf(")",n.length-t)-n.length)&&(e[0]=e[0].slice(0,t),e[2]=n.slice(0,t)),e.slice(0,3))}},filter:{TAG:function(e){var t=e.replace(te,ne).toLowerCase();return"*"===e?function(){return!0}:function(e){return e.nodeName&&e.nodeName.toLowerCase()===t}},CLASS:function(e){var t=p[e+" "];return t||(t=new RegExp("(^|"+M+")"+e+"("+M+"|$)"))&&p(e,function(e){return t.test("string"==typeof e.className&&e.className||"undefined"!=typeof e.getAttribute&&e.getAttribute("class")||"")})},ATTR:function(n,r,i){return function(e){var t=se.attr(e,n);return null==t?"!="===r:!r||(t+="","="===r?t===i:"!="===r?t!==i:"^="===r?i&&0===t.indexOf(i):"*="===r?i&&-1:\x20\t\r\n\f]*)[\x20\t\r\n\f]*\/?>(?:<\/\1>|)$/i;function j(e,n,r){return m(n)?k.grep(e,function(e,t){return!!n.call(e,t,e)!==r}):n.nodeType?k.grep(e,function(e){return e===n!==r}):"string"!=typeof n?k.grep(e,function(e){return-1)[^>]*|#([\w-]+))$/;(k.fn.init=function(e,t,n){var r,i;if(!e)return this;if(n=n||q,"string"==typeof e){if(!(r="<"===e[0]&&">"===e[e.length-1]&&3<=e.length?[null,e,null]:L.exec(e))||!r[1]&&t)return!t||t.jquery?(t||n).find(e):this.constructor(t).find(e);if(r[1]){if(t=t instanceof k?t[0]:t,k.merge(this,k.parseHTML(r[1],t&&t.nodeType?t.ownerDocument||t:E,!0)),D.test(r[1])&&k.isPlainObject(t))for(r in t)m(this[r])?this[r](t[r]):this.attr(r,t[r]);return this}return(i=E.getElementById(r[2]))&&(this[0]=i,this.length=1),this}return e.nodeType?(this[0]=e,this.length=1,this):m(e)?void 0!==n.ready?n.ready(e):e(k):k.makeArray(e,this)}).prototype=k.fn,q=k(E);var H=/^(?:parents|prev(?:Until|All))/,O={children:!0,contents:!0,next:!0,prev:!0};function P(e,t){while((e=e[t])&&1!==e.nodeType);return e}k.fn.extend({has:function(e){var t=k(e,this),n=t.length;return this.filter(function(){for(var e=0;e\x20\t\r\n\f]*)/i,he=/^$|^module$|\/(?:java|ecma)script/i,ge={option:[1,""],thead:[1,"","
"],col:[2,"","
"],tr:[2,"","
"],td:[3,"","
"],_default:[0,"",""]};function ve(e,t){var n;return n="undefined"!=typeof e.getElementsByTagName?e.getElementsByTagName(t||"*"):"undefined"!=typeof e.querySelectorAll?e.querySelectorAll(t||"*"):[],void 0===t||t&&A(e,t)?k.merge([e],n):n}function ye(e,t){for(var n=0,r=e.length;nx",y.noCloneChecked=!!me.cloneNode(!0).lastChild.defaultValue;var Te=/^key/,Ce=/^(?:mouse|pointer|contextmenu|drag|drop)|click/,Ee=/^([^.]*)(?:\.(.+)|)/;function ke(){return!0}function Se(){return!1}function Ne(e,t){return e===function(){try{return E.activeElement}catch(e){}}()==("focus"===t)}function Ae(e,t,n,r,i,o){var a,s;if("object"==typeof t){for(s in"string"!=typeof n&&(r=r||n,n=void 0),t)Ae(e,s,n,r,t[s],o);return e}if(null==r&&null==i?(i=n,r=n=void 0):null==i&&("string"==typeof n?(i=r,r=void 0):(i=r,r=n,n=void 0)),!1===i)i=Se;else if(!i)return e;return 1===o&&(a=i,(i=function(e){return k().off(e),a.apply(this,arguments)}).guid=a.guid||(a.guid=k.guid++)),e.each(function(){k.event.add(this,t,i,r,n)})}function De(e,i,o){o?(Q.set(e,i,!1),k.event.add(e,i,{namespace:!1,handler:function(e){var t,n,r=Q.get(this,i);if(1&e.isTrigger&&this[i]){if(r.length)(k.event.special[i]||{}).delegateType&&e.stopPropagation();else if(r=s.call(arguments),Q.set(this,i,r),t=o(this,i),this[i](),r!==(n=Q.get(this,i))||t?Q.set(this,i,!1):n={},r!==n)return e.stopImmediatePropagation(),e.preventDefault(),n.value}else r.length&&(Q.set(this,i,{value:k.event.trigger(k.extend(r[0],k.Event.prototype),r.slice(1),this)}),e.stopImmediatePropagation())}})):void 0===Q.get(e,i)&&k.event.add(e,i,ke)}k.event={global:{},add:function(t,e,n,r,i){var o,a,s,u,l,c,f,p,d,h,g,v=Q.get(t);if(v){n.handler&&(n=(o=n).handler,i=o.selector),i&&k.find.matchesSelector(ie,i),n.guid||(n.guid=k.guid++),(u=v.events)||(u=v.events={}),(a=v.handle)||(a=v.handle=function(e){return"undefined"!=typeof k&&k.event.triggered!==e.type?k.event.dispatch.apply(t,arguments):void 0}),l=(e=(e||"").match(R)||[""]).length;while(l--)d=g=(s=Ee.exec(e[l])||[])[1],h=(s[2]||"").split(".").sort(),d&&(f=k.event.special[d]||{},d=(i?f.delegateType:f.bindType)||d,f=k.event.special[d]||{},c=k.extend({type:d,origType:g,data:r,handler:n,guid:n.guid,selector:i,needsContext:i&&k.expr.match.needsContext.test(i),namespace:h.join(".")},o),(p=u[d])||((p=u[d]=[]).delegateCount=0,f.setup&&!1!==f.setup.call(t,r,h,a)||t.addEventListener&&t.addEventListener(d,a)),f.add&&(f.add.call(t,c),c.handler.guid||(c.handler.guid=n.guid)),i?p.splice(p.delegateCount++,0,c):p.push(c),k.event.global[d]=!0)}},remove:function(e,t,n,r,i){var o,a,s,u,l,c,f,p,d,h,g,v=Q.hasData(e)&&Q.get(e);if(v&&(u=v.events)){l=(t=(t||"").match(R)||[""]).length;while(l--)if(d=g=(s=Ee.exec(t[l])||[])[1],h=(s[2]||"").split(".").sort(),d){f=k.event.special[d]||{},p=u[d=(r?f.delegateType:f.bindType)||d]||[],s=s[2]&&new RegExp("(^|\\.)"+h.join("\\.(?:.*\\.|)")+"(\\.|$)"),a=o=p.length;while(o--)c=p[o],!i&&g!==c.origType||n&&n.guid!==c.guid||s&&!s.test(c.namespace)||r&&r!==c.selector&&("**"!==r||!c.selector)||(p.splice(o,1),c.selector&&p.delegateCount--,f.remove&&f.remove.call(e,c));a&&!p.length&&(f.teardown&&!1!==f.teardown.call(e,h,v.handle)||k.removeEvent(e,d,v.handle),delete u[d])}else for(d in u)k.event.remove(e,d+t[l],n,r,!0);k.isEmptyObject(u)&&Q.remove(e,"handle events")}},dispatch:function(e){var t,n,r,i,o,a,s=k.event.fix(e),u=new Array(arguments.length),l=(Q.get(this,"events")||{})[s.type]||[],c=k.event.special[s.type]||{};for(u[0]=s,t=1;t\x20\t\r\n\f]*)[^>]*)\/>/gi,qe=/\s*$/g;function Oe(e,t){return A(e,"table")&&A(11!==t.nodeType?t:t.firstChild,"tr")&&k(e).children("tbody")[0]||e}function Pe(e){return e.type=(null!==e.getAttribute("type"))+"/"+e.type,e}function Re(e){return"true/"===(e.type||"").slice(0,5)?e.type=e.type.slice(5):e.removeAttribute("type"),e}function Me(e,t){var n,r,i,o,a,s,u,l;if(1===t.nodeType){if(Q.hasData(e)&&(o=Q.access(e),a=Q.set(t,o),l=o.events))for(i in delete a.handle,a.events={},l)for(n=0,r=l[i].length;n")},clone:function(e,t,n){var r,i,o,a,s,u,l,c=e.cloneNode(!0),f=oe(e);if(!(y.noCloneChecked||1!==e.nodeType&&11!==e.nodeType||k.isXMLDoc(e)))for(a=ve(c),r=0,i=(o=ve(e)).length;r").attr(n.scriptAttrs||{}).prop({charset:n.scriptCharset,src:n.url}).on("load error",i=function(e){r.remove(),i=null,e&&t("error"===e.type?404:200,e.type)}),E.head.appendChild(r[0])},abort:function(){i&&i()}}});var Vt,Gt=[],Yt=/(=)\?(?=&|$)|\?\?/;k.ajaxSetup({jsonp:"callback",jsonpCallback:function(){var e=Gt.pop()||k.expando+"_"+kt++;return this[e]=!0,e}}),k.ajaxPrefilter("json jsonp",function(e,t,n){var r,i,o,a=!1!==e.jsonp&&(Yt.test(e.url)?"url":"string"==typeof e.data&&0===(e.contentType||"").indexOf("application/x-www-form-urlencoded")&&Yt.test(e.data)&&"data");if(a||"jsonp"===e.dataTypes[0])return r=e.jsonpCallback=m(e.jsonpCallback)?e.jsonpCallback():e.jsonpCallback,a?e[a]=e[a].replace(Yt,"$1"+r):!1!==e.jsonp&&(e.url+=(St.test(e.url)?"&":"?")+e.jsonp+"="+r),e.converters["script json"]=function(){return o||k.error(r+" was not called"),o[0]},e.dataTypes[0]="json",i=C[r],C[r]=function(){o=arguments},n.always(function(){void 0===i?k(C).removeProp(r):C[r]=i,e[r]&&(e.jsonpCallback=t.jsonpCallback,Gt.push(r)),o&&m(i)&&i(o[0]),o=i=void 0}),"script"}),y.createHTMLDocument=((Vt=E.implementation.createHTMLDocument("").body).innerHTML="
",2===Vt.childNodes.length),k.parseHTML=function(e,t,n){return"string"!=typeof e?[]:("boolean"==typeof t&&(n=t,t=!1),t||(y.createHTMLDocument?((r=(t=E.implementation.createHTMLDocument("")).createElement("base")).href=E.location.href,t.head.appendChild(r)):t=E),o=!n&&[],(i=D.exec(e))?[t.createElement(i[1])]:(i=we([e],t,o),o&&o.length&&k(o).remove(),k.merge([],i.childNodes)));var r,i,o},k.fn.load=function(e,t,n){var r,i,o,a=this,s=e.indexOf(" ");return-1").append(k.parseHTML(e)).find(r):e)}).always(n&&function(e,t){a.each(function(){n.apply(this,o||[e.responseText,t,e])})}),this},k.each(["ajaxStart","ajaxStop","ajaxComplete","ajaxError","ajaxSuccess","ajaxSend"],function(e,t){k.fn[t]=function(e){return this.on(t,e)}}),k.expr.pseudos.animated=function(t){return k.grep(k.timers,function(e){return t===e.elem}).length},k.offset={setOffset:function(e,t,n){var r,i,o,a,s,u,l=k.css(e,"position"),c=k(e),f={};"static"===l&&(e.style.position="relative"),s=c.offset(),o=k.css(e,"top"),u=k.css(e,"left"),("absolute"===l||"fixed"===l)&&-1<(o+u).indexOf("auto")?(a=(r=c.position()).top,i=r.left):(a=parseFloat(o)||0,i=parseFloat(u)||0),m(t)&&(t=t.call(e,n,k.extend({},s))),null!=t.top&&(f.top=t.top-s.top+a),null!=t.left&&(f.left=t.left-s.left+i),"using"in t?t.using.call(e,f):c.css(f)}},k.fn.extend({offset:function(t){if(arguments.length)return void 0===t?this:this.each(function(e){k.offset.setOffset(this,t,e)});var e,n,r=this[0];return r?r.getClientRects().length?(e=r.getBoundingClientRect(),n=r.ownerDocument.defaultView,{top:e.top+n.pageYOffset,left:e.left+n.pageXOffset}):{top:0,left:0}:void 0},position:function(){if(this[0]){var e,t,n,r=this[0],i={top:0,left:0};if("fixed"===k.css(r,"position"))t=r.getBoundingClientRect();else{t=this.offset(),n=r.ownerDocument,e=r.offsetParent||n.documentElement;while(e&&(e===n.body||e===n.documentElement)&&"static"===k.css(e,"position"))e=e.parentNode;e&&e!==r&&1===e.nodeType&&((i=k(e).offset()).top+=k.css(e,"borderTopWidth",!0),i.left+=k.css(e,"borderLeftWidth",!0))}return{top:t.top-i.top-k.css(r,"marginTop",!0),left:t.left-i.left-k.css(r,"marginLeft",!0)}}},offsetParent:function(){return this.map(function(){var e=this.offsetParent;while(e&&"static"===k.css(e,"position"))e=e.offsetParent;return e||ie})}}),k.each({scrollLeft:"pageXOffset",scrollTop:"pageYOffset"},function(t,i){var o="pageYOffset"===i;k.fn[t]=function(e){return _(this,function(e,t,n){var r;if(x(e)?r=e:9===e.nodeType&&(r=e.defaultView),void 0===n)return r?r[i]:e[t];r?r.scrollTo(o?r.pageXOffset:n,o?n:r.pageYOffset):e[t]=n},t,e,arguments.length)}}),k.each(["top","left"],function(e,n){k.cssHooks[n]=ze(y.pixelPosition,function(e,t){if(t)return t=_e(e,n),$e.test(t)?k(e).position()[n]+"px":t})}),k.each({Height:"height",Width:"width"},function(a,s){k.each({padding:"inner"+a,content:s,"":"outer"+a},function(r,o){k.fn[o]=function(e,t){var n=arguments.length&&(r||"boolean"!=typeof e),i=r||(!0===e||!0===t?"margin":"border");return _(this,function(e,t,n){var r;return x(e)?0===o.indexOf("outer")?e["inner"+a]:e.document.documentElement["client"+a]:9===e.nodeType?(r=e.documentElement,Math.max(e.body["scroll"+a],r["scroll"+a],e.body["offset"+a],r["offset"+a],r["client"+a])):void 0===n?k.css(e,t,i):k.style(e,t,n,i)},s,n?e:void 0,n)}})}),k.each("blur focus focusin focusout resize scroll click dblclick mousedown mouseup mousemove mouseover mouseout mouseenter mouseleave change select submit keydown keypress keyup contextmenu".split(" "),function(e,n){k.fn[n]=function(e,t){return 0a;a++)for(i in o[a])n=o[a][i],o[a].hasOwnProperty(i)&&void 0!==n&&(e[i]=t.isPlainObject(n)?t.isPlainObject(e[i])?t.widget.extend({},e[i],n):t.widget.extend({},n):n);return e},t.widget.bridge=function(e,i){var n=i.prototype.widgetFullName||e;t.fn[e]=function(o){var a="string"==typeof o,r=s.call(arguments,1),h=this;return a?this.length||"instance"!==o?this.each(function(){var i,s=t.data(this,n);return"instance"===o?(h=s,!1):s?t.isFunction(s[o])&&"_"!==o.charAt(0)?(i=s[o].apply(s,r),i!==s&&void 0!==i?(h=i&&i.jquery?h.pushStack(i.get()):i,!1):void 0):t.error("no such method '"+o+"' for "+e+" widget instance"):t.error("cannot call methods on "+e+" prior to initialization; "+"attempted to call method '"+o+"'")}):h=void 0:(r.length&&(o=t.widget.extend.apply(null,[o].concat(r))),this.each(function(){var e=t.data(this,n);e?(e.option(o||{}),e._init&&e._init()):t.data(this,n,new i(o,this))})),h}},t.Widget=function(){},t.Widget._childConstructors=[],t.Widget.prototype={widgetName:"widget",widgetEventPrefix:"",defaultElement:"
",options:{classes:{},disabled:!1,create:null},_createWidget:function(e,s){s=t(s||this.defaultElement||this)[0],this.element=t(s),this.uuid=i++,this.eventNamespace="."+this.widgetName+this.uuid,this.bindings=t(),this.hoverable=t(),this.focusable=t(),this.classesElementLookup={},s!==this&&(t.data(s,this.widgetFullName,this),this._on(!0,this.element,{remove:function(t){t.target===s&&this.destroy()}}),this.document=t(s.style?s.ownerDocument:s.document||s),this.window=t(this.document[0].defaultView||this.document[0].parentWindow)),this.options=t.widget.extend({},this.options,this._getCreateOptions(),e),this._create(),this.options.disabled&&this._setOptionDisabled(this.options.disabled),this._trigger("create",null,this._getCreateEventData()),this._init()},_getCreateOptions:function(){return{}},_getCreateEventData:t.noop,_create:t.noop,_init:t.noop,destroy:function(){var e=this;this._destroy(),t.each(this.classesElementLookup,function(t,i){e._removeClass(i,t)}),this.element.off(this.eventNamespace).removeData(this.widgetFullName),this.widget().off(this.eventNamespace).removeAttr("aria-disabled"),this.bindings.off(this.eventNamespace)},_destroy:t.noop,widget:function(){return this.element},option:function(e,i){var s,n,o,a=e;if(0===arguments.length)return t.widget.extend({},this.options);if("string"==typeof e)if(a={},s=e.split("."),e=s.shift(),s.length){for(n=a[e]=t.widget.extend({},this.options[e]),o=0;s.length-1>o;o++)n[s[o]]=n[s[o]]||{},n=n[s[o]];if(e=s.pop(),1===arguments.length)return void 0===n[e]?null:n[e];n[e]=i}else{if(1===arguments.length)return void 0===this.options[e]?null:this.options[e];a[e]=i}return this._setOptions(a),this},_setOptions:function(t){var e;for(e in t)this._setOption(e,t[e]);return this},_setOption:function(t,e){return"classes"===t&&this._setOptionClasses(e),this.options[t]=e,"disabled"===t&&this._setOptionDisabled(e),this},_setOptionClasses:function(e){var i,s,n;for(i in e)n=this.classesElementLookup[i],e[i]!==this.options.classes[i]&&n&&n.length&&(s=t(n.get()),this._removeClass(n,i),s.addClass(this._classes({element:s,keys:i,classes:e,add:!0})))},_setOptionDisabled:function(t){this._toggleClass(this.widget(),this.widgetFullName+"-disabled",null,!!t),t&&(this._removeClass(this.hoverable,null,"ui-state-hover"),this._removeClass(this.focusable,null,"ui-state-focus"))},enable:function(){return this._setOptions({disabled:!1})},disable:function(){return this._setOptions({disabled:!0})},_classes:function(e){function i(i,o){var a,r;for(r=0;i.length>r;r++)a=n.classesElementLookup[i[r]]||t(),a=e.add?t(t.unique(a.get().concat(e.element.get()))):t(a.not(e.element).get()),n.classesElementLookup[i[r]]=a,s.push(i[r]),o&&e.classes[i[r]]&&s.push(e.classes[i[r]])}var s=[],n=this;return e=t.extend({element:this.element,classes:this.options.classes||{}},e),this._on(e.element,{remove:"_untrackClassesElement"}),e.keys&&i(e.keys.match(/\S+/g)||[],!0),e.extra&&i(e.extra.match(/\S+/g)||[]),s.join(" ")},_untrackClassesElement:function(e){var i=this;t.each(i.classesElementLookup,function(s,n){-1!==t.inArray(e.target,n)&&(i.classesElementLookup[s]=t(n.not(e.target).get()))})},_removeClass:function(t,e,i){return this._toggleClass(t,e,i,!1)},_addClass:function(t,e,i){return this._toggleClass(t,e,i,!0)},_toggleClass:function(t,e,i,s){s="boolean"==typeof s?s:i;var n="string"==typeof t||null===t,o={extra:n?e:i,keys:n?t:e,element:n?this.element:t,add:s};return o.element.toggleClass(this._classes(o),s),this},_on:function(e,i,s){var n,o=this;"boolean"!=typeof e&&(s=i,i=e,e=!1),s?(i=n=t(i),this.bindings=this.bindings.add(i)):(s=i,i=this.element,n=this.widget()),t.each(s,function(s,a){function r(){return e||o.options.disabled!==!0&&!t(this).hasClass("ui-state-disabled")?("string"==typeof a?o[a]:a).apply(o,arguments):void 0}"string"!=typeof a&&(r.guid=a.guid=a.guid||r.guid||t.guid++);var h=s.match(/^([\w:-]*)\s*(.*)$/),l=h[1]+o.eventNamespace,c=h[2];c?n.on(l,c,r):i.on(l,r)})},_off:function(e,i){i=(i||"").split(" ").join(this.eventNamespace+" ")+this.eventNamespace,e.off(i).off(i),this.bindings=t(this.bindings.not(e).get()),this.focusable=t(this.focusable.not(e).get()),this.hoverable=t(this.hoverable.not(e).get())},_delay:function(t,e){function i(){return("string"==typeof t?s[t]:t).apply(s,arguments)}var s=this;return setTimeout(i,e||0)},_hoverable:function(e){this.hoverable=this.hoverable.add(e),this._on(e,{mouseenter:function(e){this._addClass(t(e.currentTarget),null,"ui-state-hover")},mouseleave:function(e){this._removeClass(t(e.currentTarget),null,"ui-state-hover")}})},_focusable:function(e){this.focusable=this.focusable.add(e),this._on(e,{focusin:function(e){this._addClass(t(e.currentTarget),null,"ui-state-focus")},focusout:function(e){this._removeClass(t(e.currentTarget),null,"ui-state-focus")}})},_trigger:function(e,i,s){var n,o,a=this.options[e];if(s=s||{},i=t.Event(i),i.type=(e===this.widgetEventPrefix?e:this.widgetEventPrefix+e).toLowerCase(),i.target=this.element[0],o=i.originalEvent)for(n in o)n in i||(i[n]=o[n]);return this.element.trigger(i,s),!(t.isFunction(a)&&a.apply(this.element[0],[i].concat(s))===!1||i.isDefaultPrevented())}},t.each({show:"fadeIn",hide:"fadeOut"},function(e,i){t.Widget.prototype["_"+e]=function(s,n,o){"string"==typeof n&&(n={effect:n});var a,r=n?n===!0||"number"==typeof n?i:n.effect||i:e;n=n||{},"number"==typeof n&&(n={duration:n}),a=!t.isEmptyObject(n),n.complete=o,n.delay&&s.delay(n.delay),a&&t.effects&&t.effects.effect[r]?s[e](n):r!==e&&s[r]?s[r](n.duration,n.easing,o):s.queue(function(i){t(this)[e](),o&&o.call(s[0]),i()})}}),t.widget,function(){function e(t,e,i){return[parseFloat(t[0])*(u.test(t[0])?e/100:1),parseFloat(t[1])*(u.test(t[1])?i/100:1)]}function i(e,i){return parseInt(t.css(e,i),10)||0}function s(e){var i=e[0];return 9===i.nodeType?{width:e.width(),height:e.height(),offset:{top:0,left:0}}:t.isWindow(i)?{width:e.width(),height:e.height(),offset:{top:e.scrollTop(),left:e.scrollLeft()}}:i.preventDefault?{width:0,height:0,offset:{top:i.pageY,left:i.pageX}}:{width:e.outerWidth(),height:e.outerHeight(),offset:e.offset()}}var n,o=Math.max,a=Math.abs,r=/left|center|right/,h=/top|center|bottom/,l=/[\+\-]\d+(\.[\d]+)?%?/,c=/^\w+/,u=/%$/,d=t.fn.position;t.position={scrollbarWidth:function(){if(void 0!==n)return n;var e,i,s=t("
"),o=s.children()[0];return t("body").append(s),e=o.offsetWidth,s.css("overflow","scroll"),i=o.offsetWidth,e===i&&(i=s[0].clientWidth),s.remove(),n=e-i},getScrollInfo:function(e){var i=e.isWindow||e.isDocument?"":e.element.css("overflow-x"),s=e.isWindow||e.isDocument?"":e.element.css("overflow-y"),n="scroll"===i||"auto"===i&&e.widthi?"left":e>0?"right":"center",vertical:0>r?"top":s>0?"bottom":"middle"};l>p&&p>a(e+i)&&(u.horizontal="center"),c>f&&f>a(s+r)&&(u.vertical="middle"),u.important=o(a(e),a(i))>o(a(s),a(r))?"horizontal":"vertical",n.using.call(this,t,u)}),h.offset(t.extend(D,{using:r}))})},t.ui.position={fit:{left:function(t,e){var i,s=e.within,n=s.isWindow?s.scrollLeft:s.offset.left,a=s.width,r=t.left-e.collisionPosition.marginLeft,h=n-r,l=r+e.collisionWidth-a-n;e.collisionWidth>a?h>0&&0>=l?(i=t.left+h+e.collisionWidth-a-n,t.left+=h-i):t.left=l>0&&0>=h?n:h>l?n+a-e.collisionWidth:n:h>0?t.left+=h:l>0?t.left-=l:t.left=o(t.left-r,t.left)},top:function(t,e){var i,s=e.within,n=s.isWindow?s.scrollTop:s.offset.top,a=e.within.height,r=t.top-e.collisionPosition.marginTop,h=n-r,l=r+e.collisionHeight-a-n;e.collisionHeight>a?h>0&&0>=l?(i=t.top+h+e.collisionHeight-a-n,t.top+=h-i):t.top=l>0&&0>=h?n:h>l?n+a-e.collisionHeight:n:h>0?t.top+=h:l>0?t.top-=l:t.top=o(t.top-r,t.top)}},flip:{left:function(t,e){var i,s,n=e.within,o=n.offset.left+n.scrollLeft,r=n.width,h=n.isWindow?n.scrollLeft:n.offset.left,l=t.left-e.collisionPosition.marginLeft,c=l-h,u=l+e.collisionWidth-r-h,d="left"===e.my[0]?-e.elemWidth:"right"===e.my[0]?e.elemWidth:0,p="left"===e.at[0]?e.targetWidth:"right"===e.at[0]?-e.targetWidth:0,f=-2*e.offset[0];0>c?(i=t.left+d+p+f+e.collisionWidth-r-o,(0>i||a(c)>i)&&(t.left+=d+p+f)):u>0&&(s=t.left-e.collisionPosition.marginLeft+d+p+f-h,(s>0||u>a(s))&&(t.left+=d+p+f))},top:function(t,e){var i,s,n=e.within,o=n.offset.top+n.scrollTop,r=n.height,h=n.isWindow?n.scrollTop:n.offset.top,l=t.top-e.collisionPosition.marginTop,c=l-h,u=l+e.collisionHeight-r-h,d="top"===e.my[1],p=d?-e.elemHeight:"bottom"===e.my[1]?e.elemHeight:0,f="top"===e.at[1]?e.targetHeight:"bottom"===e.at[1]?-e.targetHeight:0,m=-2*e.offset[1];0>c?(s=t.top+p+f+m+e.collisionHeight-r-o,(0>s||a(c)>s)&&(t.top+=p+f+m)):u>0&&(i=t.top-e.collisionPosition.marginTop+p+f+m-h,(i>0||u>a(i))&&(t.top+=p+f+m))}},flipfit:{left:function(){t.ui.position.flip.left.apply(this,arguments),t.ui.position.fit.left.apply(this,arguments)},top:function(){t.ui.position.flip.top.apply(this,arguments),t.ui.position.fit.top.apply(this,arguments)}}}}(),t.ui.position,t.extend(t.expr[":"],{data:t.expr.createPseudo?t.expr.createPseudo(function(e){return function(i){return!!t.data(i,e)}}):function(e,i,s){return!!t.data(e,s[3])}}),t.fn.extend({disableSelection:function(){var t="onselectstart"in document.createElement("div")?"selectstart":"mousedown";return function(){return this.on(t+".ui-disableSelection",function(t){t.preventDefault()})}}(),enableSelection:function(){return this.off(".ui-disableSelection")}}),t.ui.focusable=function(i,s){var n,o,a,r,h,l=i.nodeName.toLowerCase();return"area"===l?(n=i.parentNode,o=n.name,i.href&&o&&"map"===n.nodeName.toLowerCase()?(a=t("img[usemap='#"+o+"']"),a.length>0&&a.is(":visible")):!1):(/^(input|select|textarea|button|object)$/.test(l)?(r=!i.disabled,r&&(h=t(i).closest("fieldset")[0],h&&(r=!h.disabled))):r="a"===l?i.href||s:s,r&&t(i).is(":visible")&&e(t(i)))},t.extend(t.expr[":"],{focusable:function(e){return t.ui.focusable(e,null!=t.attr(e,"tabindex"))}}),t.ui.focusable,t.fn.form=function(){return"string"==typeof this[0].form?this.closest("form"):t(this[0].form)},t.ui.formResetMixin={_formResetHandler:function(){var e=t(this);setTimeout(function(){var i=e.data("ui-form-reset-instances");t.each(i,function(){this.refresh()})})},_bindFormResetHandler:function(){if(this.form=this.element.form(),this.form.length){var t=this.form.data("ui-form-reset-instances")||[];t.length||this.form.on("reset.ui-form-reset",this._formResetHandler),t.push(this),this.form.data("ui-form-reset-instances",t)}},_unbindFormResetHandler:function(){if(this.form.length){var e=this.form.data("ui-form-reset-instances");e.splice(t.inArray(this,e),1),e.length?this.form.data("ui-form-reset-instances",e):this.form.removeData("ui-form-reset-instances").off("reset.ui-form-reset")}}},"1.7"===t.fn.jquery.substring(0,3)&&(t.each(["Width","Height"],function(e,i){function s(e,i,s,o){return t.each(n,function(){i-=parseFloat(t.css(e,"padding"+this))||0,s&&(i-=parseFloat(t.css(e,"border"+this+"Width"))||0),o&&(i-=parseFloat(t.css(e,"margin"+this))||0)}),i}var n="Width"===i?["Left","Right"]:["Top","Bottom"],o=i.toLowerCase(),a={innerWidth:t.fn.innerWidth,innerHeight:t.fn.innerHeight,outerWidth:t.fn.outerWidth,outerHeight:t.fn.outerHeight};t.fn["inner"+i]=function(e){return void 0===e?a["inner"+i].call(this):this.each(function(){t(this).css(o,s(this,e)+"px")})},t.fn["outer"+i]=function(e,n){return"number"!=typeof e?a["outer"+i].call(this,e):this.each(function(){t(this).css(o,s(this,e,!0,n)+"px")})}}),t.fn.addBack=function(t){return this.add(null==t?this.prevObject:this.prevObject.filter(t))}),t.ui.keyCode={BACKSPACE:8,COMMA:188,DELETE:46,DOWN:40,END:35,ENTER:13,ESCAPE:27,HOME:36,LEFT:37,PAGE_DOWN:34,PAGE_UP:33,PERIOD:190,RIGHT:39,SPACE:32,TAB:9,UP:38},t.ui.escapeSelector=function(){var t=/([!"#$%&'()*+,./:;<=>?@[\]^`{|}~])/g;return function(e){return e.replace(t,"\\$1")}}(),t.fn.labels=function(){var e,i,s,n,o;return this[0].labels&&this[0].labels.length?this.pushStack(this[0].labels):(n=this.eq(0).parents("label"),s=this.attr("id"),s&&(e=this.eq(0).parents().last(),o=e.add(e.length?e.siblings():this.siblings()),i="label[for='"+t.ui.escapeSelector(s)+"']",n=n.add(o.find(i).addBack(i))),this.pushStack(n))},t.fn.scrollParent=function(e){var i=this.css("position"),s="absolute"===i,n=e?/(auto|scroll|hidden)/:/(auto|scroll)/,o=this.parents().filter(function(){var e=t(this);return s&&"static"===e.css("position")?!1:n.test(e.css("overflow")+e.css("overflow-y")+e.css("overflow-x"))}).eq(0);return"fixed"!==i&&o.length?o:t(this[0].ownerDocument||document)},t.extend(t.expr[":"],{tabbable:function(e){var i=t.attr(e,"tabindex"),s=null!=i;return(!s||i>=0)&&t.ui.focusable(e,s)}}),t.fn.extend({uniqueId:function(){var t=0;return function(){return this.each(function(){this.id||(this.id="ui-id-"+ ++t)})}}(),removeUniqueId:function(){return this.each(function(){/^ui-id-\d+$/.test(this.id)&&t(this).removeAttr("id")})}}),t.ui.ie=!!/msie [\w.]+/.exec(navigator.userAgent.toLowerCase());var n=!1;t(document).on("mouseup",function(){n=!1}),t.widget("ui.mouse",{version:"1.12.1",options:{cancel:"input, textarea, button, select, option",distance:1,delay:0},_mouseInit:function(){var e=this;this.element.on("mousedown."+this.widgetName,function(t){return e._mouseDown(t)}).on("click."+this.widgetName,function(i){return!0===t.data(i.target,e.widgetName+".preventClickEvent")?(t.removeData(i.target,e.widgetName+".preventClickEvent"),i.stopImmediatePropagation(),!1):void 0}),this.started=!1},_mouseDestroy:function(){this.element.off("."+this.widgetName),this._mouseMoveDelegate&&this.document.off("mousemove."+this.widgetName,this._mouseMoveDelegate).off("mouseup."+this.widgetName,this._mouseUpDelegate)},_mouseDown:function(e){if(!n){this._mouseMoved=!1,this._mouseStarted&&this._mouseUp(e),this._mouseDownEvent=e;var i=this,s=1===e.which,o="string"==typeof this.options.cancel&&e.target.nodeName?t(e.target).closest(this.options.cancel).length:!1;return s&&!o&&this._mouseCapture(e)?(this.mouseDelayMet=!this.options.delay,this.mouseDelayMet||(this._mouseDelayTimer=setTimeout(function(){i.mouseDelayMet=!0},this.options.delay)),this._mouseDistanceMet(e)&&this._mouseDelayMet(e)&&(this._mouseStarted=this._mouseStart(e)!==!1,!this._mouseStarted)?(e.preventDefault(),!0):(!0===t.data(e.target,this.widgetName+".preventClickEvent")&&t.removeData(e.target,this.widgetName+".preventClickEvent"),this._mouseMoveDelegate=function(t){return i._mouseMove(t)},this._mouseUpDelegate=function(t){return i._mouseUp(t)},this.document.on("mousemove."+this.widgetName,this._mouseMoveDelegate).on("mouseup."+this.widgetName,this._mouseUpDelegate),e.preventDefault(),n=!0,!0)):!0}},_mouseMove:function(e){if(this._mouseMoved){if(t.ui.ie&&(!document.documentMode||9>document.documentMode)&&!e.button)return this._mouseUp(e);if(!e.which)if(e.originalEvent.altKey||e.originalEvent.ctrlKey||e.originalEvent.metaKey||e.originalEvent.shiftKey)this.ignoreMissingWhich=!0;else if(!this.ignoreMissingWhich)return this._mouseUp(e)}return(e.which||e.button)&&(this._mouseMoved=!0),this._mouseStarted?(this._mouseDrag(e),e.preventDefault()):(this._mouseDistanceMet(e)&&this._mouseDelayMet(e)&&(this._mouseStarted=this._mouseStart(this._mouseDownEvent,e)!==!1,this._mouseStarted?this._mouseDrag(e):this._mouseUp(e)),!this._mouseStarted)},_mouseUp:function(e){this.document.off("mousemove."+this.widgetName,this._mouseMoveDelegate).off("mouseup."+this.widgetName,this._mouseUpDelegate),this._mouseStarted&&(this._mouseStarted=!1,e.target===this._mouseDownEvent.target&&t.data(e.target,this.widgetName+".preventClickEvent",!0),this._mouseStop(e)),this._mouseDelayTimer&&(clearTimeout(this._mouseDelayTimer),delete this._mouseDelayTimer),this.ignoreMissingWhich=!1,n=!1,e.preventDefault()},_mouseDistanceMet:function(t){return Math.max(Math.abs(this._mouseDownEvent.pageX-t.pageX),Math.abs(this._mouseDownEvent.pageY-t.pageY))>=this.options.distance},_mouseDelayMet:function(){return this.mouseDelayMet},_mouseStart:function(){},_mouseDrag:function(){},_mouseStop:function(){},_mouseCapture:function(){return!0}}),t.ui.plugin={add:function(e,i,s){var n,o=t.ui[e].prototype;for(n in s)o.plugins[n]=o.plugins[n]||[],o.plugins[n].push([i,s[n]])},call:function(t,e,i,s){var n,o=t.plugins[e];if(o&&(s||t.element[0].parentNode&&11!==t.element[0].parentNode.nodeType))for(n=0;o.length>n;n++)t.options[o[n][0]]&&o[n][1].apply(t.element,i)}},t.widget("ui.resizable",t.ui.mouse,{version:"1.12.1",widgetEventPrefix:"resize",options:{alsoResize:!1,animate:!1,animateDuration:"slow",animateEasing:"swing",aspectRatio:!1,autoHide:!1,classes:{"ui-resizable-se":"ui-icon ui-icon-gripsmall-diagonal-se"},containment:!1,ghost:!1,grid:!1,handles:"e,s,se",helper:!1,maxHeight:null,maxWidth:null,minHeight:10,minWidth:10,zIndex:90,resize:null,start:null,stop:null},_num:function(t){return parseFloat(t)||0},_isNumber:function(t){return!isNaN(parseFloat(t))},_hasScroll:function(e,i){if("hidden"===t(e).css("overflow"))return!1;var s=i&&"left"===i?"scrollLeft":"scrollTop",n=!1;return e[s]>0?!0:(e[s]=1,n=e[s]>0,e[s]=0,n)},_create:function(){var e,i=this.options,s=this;this._addClass("ui-resizable"),t.extend(this,{_aspectRatio:!!i.aspectRatio,aspectRatio:i.aspectRatio,originalElement:this.element,_proportionallyResizeElements:[],_helper:i.helper||i.ghost||i.animate?i.helper||"ui-resizable-helper":null}),this.element[0].nodeName.match(/^(canvas|textarea|input|select|button|img)$/i)&&(this.element.wrap(t("
").css({position:this.element.css("position"),width:this.element.outerWidth(),height:this.element.outerHeight(),top:this.element.css("top"),left:this.element.css("left")})),this.element=this.element.parent().data("ui-resizable",this.element.resizable("instance")),this.elementIsWrapper=!0,e={marginTop:this.originalElement.css("marginTop"),marginRight:this.originalElement.css("marginRight"),marginBottom:this.originalElement.css("marginBottom"),marginLeft:this.originalElement.css("marginLeft")},this.element.css(e),this.originalElement.css("margin",0),this.originalResizeStyle=this.originalElement.css("resize"),this.originalElement.css("resize","none"),this._proportionallyResizeElements.push(this.originalElement.css({position:"static",zoom:1,display:"block"})),this.originalElement.css(e),this._proportionallyResize()),this._setupHandles(),i.autoHide&&t(this.element).on("mouseenter",function(){i.disabled||(s._removeClass("ui-resizable-autohide"),s._handles.show())}).on("mouseleave",function(){i.disabled||s.resizing||(s._addClass("ui-resizable-autohide"),s._handles.hide())}),this._mouseInit()},_destroy:function(){this._mouseDestroy();var e,i=function(e){t(e).removeData("resizable").removeData("ui-resizable").off(".resizable").find(".ui-resizable-handle").remove()};return this.elementIsWrapper&&(i(this.element),e=this.element,this.originalElement.css({position:e.css("position"),width:e.outerWidth(),height:e.outerHeight(),top:e.css("top"),left:e.css("left")}).insertAfter(e),e.remove()),this.originalElement.css("resize",this.originalResizeStyle),i(this.originalElement),this},_setOption:function(t,e){switch(this._super(t,e),t){case"handles":this._removeHandles(),this._setupHandles();break;default:}},_setupHandles:function(){var e,i,s,n,o,a=this.options,r=this;if(this.handles=a.handles||(t(".ui-resizable-handle",this.element).length?{n:".ui-resizable-n",e:".ui-resizable-e",s:".ui-resizable-s",w:".ui-resizable-w",se:".ui-resizable-se",sw:".ui-resizable-sw",ne:".ui-resizable-ne",nw:".ui-resizable-nw"}:"e,s,se"),this._handles=t(),this.handles.constructor===String)for("all"===this.handles&&(this.handles="n,e,s,w,se,sw,ne,nw"),s=this.handles.split(","),this.handles={},i=0;s.length>i;i++)e=t.trim(s[i]),n="ui-resizable-"+e,o=t("
"),this._addClass(o,"ui-resizable-handle "+n),o.css({zIndex:a.zIndex}),this.handles[e]=".ui-resizable-"+e,this.element.append(o);this._renderAxis=function(e){var i,s,n,o;e=e||this.element;for(i in this.handles)this.handles[i].constructor===String?this.handles[i]=this.element.children(this.handles[i]).first().show():(this.handles[i].jquery||this.handles[i].nodeType)&&(this.handles[i]=t(this.handles[i]),this._on(this.handles[i],{mousedown:r._mouseDown})),this.elementIsWrapper&&this.originalElement[0].nodeName.match(/^(textarea|input|select|button)$/i)&&(s=t(this.handles[i],this.element),o=/sw|ne|nw|se|n|s/.test(i)?s.outerHeight():s.outerWidth(),n=["padding",/ne|nw|n/.test(i)?"Top":/se|sw|s/.test(i)?"Bottom":/^e$/.test(i)?"Right":"Left"].join(""),e.css(n,o),this._proportionallyResize()),this._handles=this._handles.add(this.handles[i])},this._renderAxis(this.element),this._handles=this._handles.add(this.element.find(".ui-resizable-handle")),this._handles.disableSelection(),this._handles.on("mouseover",function(){r.resizing||(this.className&&(o=this.className.match(/ui-resizable-(se|sw|ne|nw|n|e|s|w)/i)),r.axis=o&&o[1]?o[1]:"se")}),a.autoHide&&(this._handles.hide(),this._addClass("ui-resizable-autohide"))},_removeHandles:function(){this._handles.remove()},_mouseCapture:function(e){var i,s,n=!1;for(i in this.handles)s=t(this.handles[i])[0],(s===e.target||t.contains(s,e.target))&&(n=!0);return!this.options.disabled&&n},_mouseStart:function(e){var i,s,n,o=this.options,a=this.element;return this.resizing=!0,this._renderProxy(),i=this._num(this.helper.css("left")),s=this._num(this.helper.css("top")),o.containment&&(i+=t(o.containment).scrollLeft()||0,s+=t(o.containment).scrollTop()||0),this.offset=this.helper.offset(),this.position={left:i,top:s},this.size=this._helper?{width:this.helper.width(),height:this.helper.height()}:{width:a.width(),height:a.height()},this.originalSize=this._helper?{width:a.outerWidth(),height:a.outerHeight()}:{width:a.width(),height:a.height()},this.sizeDiff={width:a.outerWidth()-a.width(),height:a.outerHeight()-a.height()},this.originalPosition={left:i,top:s},this.originalMousePosition={left:e.pageX,top:e.pageY},this.aspectRatio="number"==typeof o.aspectRatio?o.aspectRatio:this.originalSize.width/this.originalSize.height||1,n=t(".ui-resizable-"+this.axis).css("cursor"),t("body").css("cursor","auto"===n?this.axis+"-resize":n),this._addClass("ui-resizable-resizing"),this._propagate("start",e),!0},_mouseDrag:function(e){var i,s,n=this.originalMousePosition,o=this.axis,a=e.pageX-n.left||0,r=e.pageY-n.top||0,h=this._change[o];return this._updatePrevProperties(),h?(i=h.apply(this,[e,a,r]),this._updateVirtualBoundaries(e.shiftKey),(this._aspectRatio||e.shiftKey)&&(i=this._updateRatio(i,e)),i=this._respectSize(i,e),this._updateCache(i),this._propagate("resize",e),s=this._applyChanges(),!this._helper&&this._proportionallyResizeElements.length&&this._proportionallyResize(),t.isEmptyObject(s)||(this._updatePrevProperties(),this._trigger("resize",e,this.ui()),this._applyChanges()),!1):!1},_mouseStop:function(e){this.resizing=!1;var i,s,n,o,a,r,h,l=this.options,c=this;return this._helper&&(i=this._proportionallyResizeElements,s=i.length&&/textarea/i.test(i[0].nodeName),n=s&&this._hasScroll(i[0],"left")?0:c.sizeDiff.height,o=s?0:c.sizeDiff.width,a={width:c.helper.width()-o,height:c.helper.height()-n},r=parseFloat(c.element.css("left"))+(c.position.left-c.originalPosition.left)||null,h=parseFloat(c.element.css("top"))+(c.position.top-c.originalPosition.top)||null,l.animate||this.element.css(t.extend(a,{top:h,left:r})),c.helper.height(c.size.height),c.helper.width(c.size.width),this._helper&&!l.animate&&this._proportionallyResize()),t("body").css("cursor","auto"),this._removeClass("ui-resizable-resizing"),this._propagate("stop",e),this._helper&&this.helper.remove(),!1},_updatePrevProperties:function(){this.prevPosition={top:this.position.top,left:this.position.left},this.prevSize={width:this.size.width,height:this.size.height}},_applyChanges:function(){var t={};return this.position.top!==this.prevPosition.top&&(t.top=this.position.top+"px"),this.position.left!==this.prevPosition.left&&(t.left=this.position.left+"px"),this.size.width!==this.prevSize.width&&(t.width=this.size.width+"px"),this.size.height!==this.prevSize.height&&(t.height=this.size.height+"px"),this.helper.css(t),t},_updateVirtualBoundaries:function(t){var e,i,s,n,o,a=this.options;o={minWidth:this._isNumber(a.minWidth)?a.minWidth:0,maxWidth:this._isNumber(a.maxWidth)?a.maxWidth:1/0,minHeight:this._isNumber(a.minHeight)?a.minHeight:0,maxHeight:this._isNumber(a.maxHeight)?a.maxHeight:1/0},(this._aspectRatio||t)&&(e=o.minHeight*this.aspectRatio,s=o.minWidth/this.aspectRatio,i=o.maxHeight*this.aspectRatio,n=o.maxWidth/this.aspectRatio,e>o.minWidth&&(o.minWidth=e),s>o.minHeight&&(o.minHeight=s),o.maxWidth>i&&(o.maxWidth=i),o.maxHeight>n&&(o.maxHeight=n)),this._vBoundaries=o},_updateCache:function(t){this.offset=this.helper.offset(),this._isNumber(t.left)&&(this.position.left=t.left),this._isNumber(t.top)&&(this.position.top=t.top),this._isNumber(t.height)&&(this.size.height=t.height),this._isNumber(t.width)&&(this.size.width=t.width)},_updateRatio:function(t){var e=this.position,i=this.size,s=this.axis;return this._isNumber(t.height)?t.width=t.height*this.aspectRatio:this._isNumber(t.width)&&(t.height=t.width/this.aspectRatio),"sw"===s&&(t.left=e.left+(i.width-t.width),t.top=null),"nw"===s&&(t.top=e.top+(i.height-t.height),t.left=e.left+(i.width-t.width)),t},_respectSize:function(t){var e=this._vBoundaries,i=this.axis,s=this._isNumber(t.width)&&e.maxWidth&&e.maxWidtht.width,a=this._isNumber(t.height)&&e.minHeight&&e.minHeight>t.height,r=this.originalPosition.left+this.originalSize.width,h=this.originalPosition.top+this.originalSize.height,l=/sw|nw|w/.test(i),c=/nw|ne|n/.test(i);return o&&(t.width=e.minWidth),a&&(t.height=e.minHeight),s&&(t.width=e.maxWidth),n&&(t.height=e.maxHeight),o&&l&&(t.left=r-e.minWidth),s&&l&&(t.left=r-e.maxWidth),a&&c&&(t.top=h-e.minHeight),n&&c&&(t.top=h-e.maxHeight),t.width||t.height||t.left||!t.top?t.width||t.height||t.top||!t.left||(t.left=null):t.top=null,t},_getPaddingPlusBorderDimensions:function(t){for(var e=0,i=[],s=[t.css("borderTopWidth"),t.css("borderRightWidth"),t.css("borderBottomWidth"),t.css("borderLeftWidth")],n=[t.css("paddingTop"),t.css("paddingRight"),t.css("paddingBottom"),t.css("paddingLeft")];4>e;e++)i[e]=parseFloat(s[e])||0,i[e]+=parseFloat(n[e])||0;return{height:i[0]+i[2],width:i[1]+i[3]}},_proportionallyResize:function(){if(this._proportionallyResizeElements.length)for(var t,e=0,i=this.helper||this.element;this._proportionallyResizeElements.length>e;e++)t=this._proportionallyResizeElements[e],this.outerDimensions||(this.outerDimensions=this._getPaddingPlusBorderDimensions(t)),t.css({height:i.height()-this.outerDimensions.height||0,width:i.width()-this.outerDimensions.width||0})},_renderProxy:function(){var e=this.element,i=this.options;this.elementOffset=e.offset(),this._helper?(this.helper=this.helper||t("
"),this._addClass(this.helper,this._helper),this.helper.css({width:this.element.outerWidth(),height:this.element.outerHeight(),position:"absolute",left:this.elementOffset.left+"px",top:this.elementOffset.top+"px",zIndex:++i.zIndex}),this.helper.appendTo("body").disableSelection()):this.helper=this.element +},_change:{e:function(t,e){return{width:this.originalSize.width+e}},w:function(t,e){var i=this.originalSize,s=this.originalPosition;return{left:s.left+e,width:i.width-e}},n:function(t,e,i){var s=this.originalSize,n=this.originalPosition;return{top:n.top+i,height:s.height-i}},s:function(t,e,i){return{height:this.originalSize.height+i}},se:function(e,i,s){return t.extend(this._change.s.apply(this,arguments),this._change.e.apply(this,[e,i,s]))},sw:function(e,i,s){return t.extend(this._change.s.apply(this,arguments),this._change.w.apply(this,[e,i,s]))},ne:function(e,i,s){return t.extend(this._change.n.apply(this,arguments),this._change.e.apply(this,[e,i,s]))},nw:function(e,i,s){return t.extend(this._change.n.apply(this,arguments),this._change.w.apply(this,[e,i,s]))}},_propagate:function(e,i){t.ui.plugin.call(this,e,[i,this.ui()]),"resize"!==e&&this._trigger(e,i,this.ui())},plugins:{},ui:function(){return{originalElement:this.originalElement,element:this.element,helper:this.helper,position:this.position,size:this.size,originalSize:this.originalSize,originalPosition:this.originalPosition}}}),t.ui.plugin.add("resizable","animate",{stop:function(e){var i=t(this).resizable("instance"),s=i.options,n=i._proportionallyResizeElements,o=n.length&&/textarea/i.test(n[0].nodeName),a=o&&i._hasScroll(n[0],"left")?0:i.sizeDiff.height,r=o?0:i.sizeDiff.width,h={width:i.size.width-r,height:i.size.height-a},l=parseFloat(i.element.css("left"))+(i.position.left-i.originalPosition.left)||null,c=parseFloat(i.element.css("top"))+(i.position.top-i.originalPosition.top)||null;i.element.animate(t.extend(h,c&&l?{top:c,left:l}:{}),{duration:s.animateDuration,easing:s.animateEasing,step:function(){var s={width:parseFloat(i.element.css("width")),height:parseFloat(i.element.css("height")),top:parseFloat(i.element.css("top")),left:parseFloat(i.element.css("left"))};n&&n.length&&t(n[0]).css({width:s.width,height:s.height}),i._updateCache(s),i._propagate("resize",e)}})}}),t.ui.plugin.add("resizable","containment",{start:function(){var e,i,s,n,o,a,r,h=t(this).resizable("instance"),l=h.options,c=h.element,u=l.containment,d=u instanceof t?u.get(0):/parent/.test(u)?c.parent().get(0):u;d&&(h.containerElement=t(d),/document/.test(u)||u===document?(h.containerOffset={left:0,top:0},h.containerPosition={left:0,top:0},h.parentData={element:t(document),left:0,top:0,width:t(document).width(),height:t(document).height()||document.body.parentNode.scrollHeight}):(e=t(d),i=[],t(["Top","Right","Left","Bottom"]).each(function(t,s){i[t]=h._num(e.css("padding"+s))}),h.containerOffset=e.offset(),h.containerPosition=e.position(),h.containerSize={height:e.innerHeight()-i[3],width:e.innerWidth()-i[1]},s=h.containerOffset,n=h.containerSize.height,o=h.containerSize.width,a=h._hasScroll(d,"left")?d.scrollWidth:o,r=h._hasScroll(d)?d.scrollHeight:n,h.parentData={element:d,left:s.left,top:s.top,width:a,height:r}))},resize:function(e){var i,s,n,o,a=t(this).resizable("instance"),r=a.options,h=a.containerOffset,l=a.position,c=a._aspectRatio||e.shiftKey,u={top:0,left:0},d=a.containerElement,p=!0;d[0]!==document&&/static/.test(d.css("position"))&&(u=h),l.left<(a._helper?h.left:0)&&(a.size.width=a.size.width+(a._helper?a.position.left-h.left:a.position.left-u.left),c&&(a.size.height=a.size.width/a.aspectRatio,p=!1),a.position.left=r.helper?h.left:0),l.top<(a._helper?h.top:0)&&(a.size.height=a.size.height+(a._helper?a.position.top-h.top:a.position.top),c&&(a.size.width=a.size.height*a.aspectRatio,p=!1),a.position.top=a._helper?h.top:0),n=a.containerElement.get(0)===a.element.parent().get(0),o=/relative|absolute/.test(a.containerElement.css("position")),n&&o?(a.offset.left=a.parentData.left+a.position.left,a.offset.top=a.parentData.top+a.position.top):(a.offset.left=a.element.offset().left,a.offset.top=a.element.offset().top),i=Math.abs(a.sizeDiff.width+(a._helper?a.offset.left-u.left:a.offset.left-h.left)),s=Math.abs(a.sizeDiff.height+(a._helper?a.offset.top-u.top:a.offset.top-h.top)),i+a.size.width>=a.parentData.width&&(a.size.width=a.parentData.width-i,c&&(a.size.height=a.size.width/a.aspectRatio,p=!1)),s+a.size.height>=a.parentData.height&&(a.size.height=a.parentData.height-s,c&&(a.size.width=a.size.height*a.aspectRatio,p=!1)),p||(a.position.left=a.prevPosition.left,a.position.top=a.prevPosition.top,a.size.width=a.prevSize.width,a.size.height=a.prevSize.height)},stop:function(){var e=t(this).resizable("instance"),i=e.options,s=e.containerOffset,n=e.containerPosition,o=e.containerElement,a=t(e.helper),r=a.offset(),h=a.outerWidth()-e.sizeDiff.width,l=a.outerHeight()-e.sizeDiff.height;e._helper&&!i.animate&&/relative/.test(o.css("position"))&&t(this).css({left:r.left-n.left-s.left,width:h,height:l}),e._helper&&!i.animate&&/static/.test(o.css("position"))&&t(this).css({left:r.left-n.left-s.left,width:h,height:l})}}),t.ui.plugin.add("resizable","alsoResize",{start:function(){var e=t(this).resizable("instance"),i=e.options;t(i.alsoResize).each(function(){var e=t(this);e.data("ui-resizable-alsoresize",{width:parseFloat(e.width()),height:parseFloat(e.height()),left:parseFloat(e.css("left")),top:parseFloat(e.css("top"))})})},resize:function(e,i){var s=t(this).resizable("instance"),n=s.options,o=s.originalSize,a=s.originalPosition,r={height:s.size.height-o.height||0,width:s.size.width-o.width||0,top:s.position.top-a.top||0,left:s.position.left-a.left||0};t(n.alsoResize).each(function(){var e=t(this),s=t(this).data("ui-resizable-alsoresize"),n={},o=e.parents(i.originalElement[0]).length?["width","height"]:["width","height","top","left"];t.each(o,function(t,e){var i=(s[e]||0)+(r[e]||0);i&&i>=0&&(n[e]=i||null)}),e.css(n)})},stop:function(){t(this).removeData("ui-resizable-alsoresize")}}),t.ui.plugin.add("resizable","ghost",{start:function(){var e=t(this).resizable("instance"),i=e.size;e.ghost=e.originalElement.clone(),e.ghost.css({opacity:.25,display:"block",position:"relative",height:i.height,width:i.width,margin:0,left:0,top:0}),e._addClass(e.ghost,"ui-resizable-ghost"),t.uiBackCompat!==!1&&"string"==typeof e.options.ghost&&e.ghost.addClass(this.options.ghost),e.ghost.appendTo(e.helper)},resize:function(){var e=t(this).resizable("instance");e.ghost&&e.ghost.css({position:"relative",height:e.size.height,width:e.size.width})},stop:function(){var e=t(this).resizable("instance");e.ghost&&e.helper&&e.helper.get(0).removeChild(e.ghost.get(0))}}),t.ui.plugin.add("resizable","grid",{resize:function(){var e,i=t(this).resizable("instance"),s=i.options,n=i.size,o=i.originalSize,a=i.originalPosition,r=i.axis,h="number"==typeof s.grid?[s.grid,s.grid]:s.grid,l=h[0]||1,c=h[1]||1,u=Math.round((n.width-o.width)/l)*l,d=Math.round((n.height-o.height)/c)*c,p=o.width+u,f=o.height+d,m=s.maxWidth&&p>s.maxWidth,g=s.maxHeight&&f>s.maxHeight,_=s.minWidth&&s.minWidth>p,v=s.minHeight&&s.minHeight>f;s.grid=h,_&&(p+=l),v&&(f+=c),m&&(p-=l),g&&(f-=c),/^(se|s|e)$/.test(r)?(i.size.width=p,i.size.height=f):/^(ne)$/.test(r)?(i.size.width=p,i.size.height=f,i.position.top=a.top-d):/^(sw)$/.test(r)?(i.size.width=p,i.size.height=f,i.position.left=a.left-u):((0>=f-c||0>=p-l)&&(e=i._getPaddingPlusBorderDimensions(this)),f-c>0?(i.size.height=f,i.position.top=a.top-d):(f=c-e.height,i.size.height=f,i.position.top=a.top+o.height-f),p-l>0?(i.size.width=p,i.position.left=a.left-u):(p=l-e.width,i.size.width=p,i.position.left=a.left+o.width-p))}}),t.ui.resizable});/** + * Copyright (c) 2007 Ariel Flesler - aflesler ○ gmail • com | https://github.com/flesler + * Licensed under MIT + * @author Ariel Flesler + * @version 2.1.2 + */ +;(function(f){"use strict";"function"===typeof define&&define.amd?define(["jquery"],f):"undefined"!==typeof module&&module.exports?module.exports=f(require("jquery")):f(jQuery)})(function($){"use strict";function n(a){return!a.nodeName||-1!==$.inArray(a.nodeName.toLowerCase(),["iframe","#document","html","body"])}function h(a){return $.isFunction(a)||$.isPlainObject(a)?a:{top:a,left:a}}var p=$.scrollTo=function(a,d,b){return $(window).scrollTo(a,d,b)};p.defaults={axis:"xy",duration:0,limit:!0};$.fn.scrollTo=function(a,d,b){"object"=== typeof d&&(b=d,d=0);"function"===typeof b&&(b={onAfter:b});"max"===a&&(a=9E9);b=$.extend({},p.defaults,b);d=d||b.duration;var u=b.queue&&1=f[g]?0:Math.min(f[g],n));!a&&1-1){targetElements.on(evt+EVENT_NAMESPACE,function elementToggle(event){$.powerTip.toggle(this,event)})}else{targetElements.on(evt+EVENT_NAMESPACE,function elementOpen(event){$.powerTip.show(this,event)})}});$.each(options.closeEvents,function(idx,evt){if($.inArray(evt,options.openEvents)<0){targetElements.on(evt+EVENT_NAMESPACE,function elementClose(event){$.powerTip.hide(this,!isMouseEvent(event))})}});targetElements.on("keydown"+EVENT_NAMESPACE,function elementKeyDown(event){if(event.keyCode===27){$.powerTip.hide(this,true)}})}return targetElements};$.fn.powerTip.defaults={fadeInTime:200,fadeOutTime:100,followMouse:false,popupId:"powerTip",popupClass:null,intentSensitivity:7,intentPollInterval:100,closeDelay:100,placement:"n",smartPlacement:false,offset:10,mouseOnToPopup:false,manual:false,openEvents:["mouseenter","focus"],closeEvents:["mouseleave","blur"]};$.fn.powerTip.smartPlacementLists={n:["n","ne","nw","s"],e:["e","ne","se","w","nw","sw","n","s","e"],s:["s","se","sw","n"],w:["w","nw","sw","e","ne","se","n","s","w"],nw:["nw","w","sw","n","s","se","nw"],ne:["ne","e","se","n","s","sw","ne"],sw:["sw","w","nw","s","n","ne","sw"],se:["se","e","ne","s","n","nw","se"],"nw-alt":["nw-alt","n","ne-alt","sw-alt","s","se-alt","w","e"],"ne-alt":["ne-alt","n","nw-alt","se-alt","s","sw-alt","e","w"],"sw-alt":["sw-alt","s","se-alt","nw-alt","n","ne-alt","w","e"],"se-alt":["se-alt","s","sw-alt","ne-alt","n","nw-alt","e","w"]};$.powerTip={show:function apiShowTip(element,event){if(isMouseEvent(event)){trackMouse(event);session.previousX=event.pageX;session.previousY=event.pageY;$(element).data(DATA_DISPLAYCONTROLLER).show()}else{$(element).first().data(DATA_DISPLAYCONTROLLER).show(true,true)}return element},reposition:function apiResetPosition(element){$(element).first().data(DATA_DISPLAYCONTROLLER).resetPosition();return element},hide:function apiCloseTip(element,immediate){var displayController;immediate=element?immediate:true;if(element){displayController=$(element).first().data(DATA_DISPLAYCONTROLLER)}else if(session.activeHover){displayController=session.activeHover.data(DATA_DISPLAYCONTROLLER)}if(displayController){displayController.hide(immediate)}return element},toggle:function apiToggle(element,event){if(session.activeHover&&session.activeHover.is(element)){$.powerTip.hide(element,!isMouseEvent(event))}else{$.powerTip.show(element,event)}return element}};$.powerTip.showTip=$.powerTip.show;$.powerTip.closeTip=$.powerTip.hide;function CSSCoordinates(){var me=this;me.top="auto";me.left="auto";me.right="auto";me.bottom="auto";me.set=function(property,value){if($.isNumeric(value)){me[property]=Math.round(value)}}}function DisplayController(element,options,tipController){var hoverTimer=null,myCloseDelay=null;function openTooltip(immediate,forceOpen){cancelTimer();if(!element.data(DATA_HASACTIVEHOVER)){if(!immediate){session.tipOpenImminent=true;hoverTimer=setTimeout(function intentDelay(){hoverTimer=null;checkForIntent()},options.intentPollInterval)}else{if(forceOpen){element.data(DATA_FORCEDOPEN,true)}closeAnyDelayed();tipController.showTip(element)}}else{cancelClose()}}function closeTooltip(disableDelay){if(myCloseDelay){myCloseDelay=session.closeDelayTimeout=clearTimeout(myCloseDelay);session.delayInProgress=false}cancelTimer();session.tipOpenImminent=false;if(element.data(DATA_HASACTIVEHOVER)){element.data(DATA_FORCEDOPEN,false);if(!disableDelay){session.delayInProgress=true;session.closeDelayTimeout=setTimeout(function closeDelay(){session.closeDelayTimeout=null;tipController.hideTip(element);session.delayInProgress=false;myCloseDelay=null},options.closeDelay);myCloseDelay=session.closeDelayTimeout}else{tipController.hideTip(element)}}}function checkForIntent(){var xDifference=Math.abs(session.previousX-session.currentX),yDifference=Math.abs(session.previousY-session.currentY),totalDifference=xDifference+yDifference;if(totalDifference",{id:options.popupId});if($body.length===0){$body=$("body")}$body.append(tipElement);session.tooltips=session.tooltips?session.tooltips.add(tipElement):tipElement}if(options.followMouse){if(!tipElement.data(DATA_HASMOUSEMOVE)){$document.on("mousemove"+EVENT_NAMESPACE,positionTipOnCursor);$window.on("scroll"+EVENT_NAMESPACE,positionTipOnCursor);tipElement.data(DATA_HASMOUSEMOVE,true)}}function beginShowTip(element){element.data(DATA_HASACTIVEHOVER,true);tipElement.queue(function queueTipInit(next){showTip(element);next()})}function showTip(element){var tipContent;if(!element.data(DATA_HASACTIVEHOVER)){return}if(session.isTipOpen){if(!session.isClosing){hideTip(session.activeHover)}tipElement.delay(100).queue(function queueTipAgain(next){showTip(element);next()});return}element.trigger("powerTipPreRender");tipContent=getTooltipContent(element);if(tipContent){tipElement.empty().append(tipContent)}else{return}element.trigger("powerTipRender");session.activeHover=element;session.isTipOpen=true;tipElement.data(DATA_MOUSEONTOTIP,options.mouseOnToPopup);tipElement.addClass(options.popupClass);if(!options.followMouse||element.data(DATA_FORCEDOPEN)){positionTipOnElement(element);session.isFixedTipOpen=true}else{positionTipOnCursor()}if(!element.data(DATA_FORCEDOPEN)&&!options.followMouse){$document.on("click"+EVENT_NAMESPACE,function documentClick(event){var target=event.target;if(target!==element[0]){if(options.mouseOnToPopup){if(target!==tipElement[0]&&!$.contains(tipElement[0],target)){$.powerTip.hide()}}else{$.powerTip.hide()}}})}if(options.mouseOnToPopup&&!options.manual){tipElement.on("mouseenter"+EVENT_NAMESPACE,function tipMouseEnter(){if(session.activeHover){session.activeHover.data(DATA_DISPLAYCONTROLLER).cancel()}});tipElement.on("mouseleave"+EVENT_NAMESPACE,function tipMouseLeave(){if(session.activeHover){session.activeHover.data(DATA_DISPLAYCONTROLLER).hide()}})}tipElement.fadeIn(options.fadeInTime,function fadeInCallback(){if(!session.desyncTimeout){session.desyncTimeout=setInterval(closeDesyncedTip,500)}element.trigger("powerTipOpen")})}function hideTip(element){session.isClosing=true;session.isTipOpen=false;session.desyncTimeout=clearInterval(session.desyncTimeout);element.data(DATA_HASACTIVEHOVER,false);element.data(DATA_FORCEDOPEN,false);$document.off("click"+EVENT_NAMESPACE);tipElement.off(EVENT_NAMESPACE);tipElement.fadeOut(options.fadeOutTime,function fadeOutCallback(){var coords=new CSSCoordinates;session.activeHover=null;session.isClosing=false;session.isFixedTipOpen=false;tipElement.removeClass();coords.set("top",session.currentY+options.offset);coords.set("left",session.currentX+options.offset);tipElement.css(coords);element.trigger("powerTipClose")})}function positionTipOnCursor(){var tipWidth,tipHeight,coords,collisions,collisionCount;if(!session.isFixedTipOpen&&(session.isTipOpen||session.tipOpenImminent&&tipElement.data(DATA_HASMOUSEMOVE))){tipWidth=tipElement.outerWidth();tipHeight=tipElement.outerHeight();coords=new CSSCoordinates;coords.set("top",session.currentY+options.offset);coords.set("left",session.currentX+options.offset);collisions=getViewportCollisions(coords,tipWidth,tipHeight);if(collisions!==Collision.none){collisionCount=countFlags(collisions);if(collisionCount===1){if(collisions===Collision.right){coords.set("left",session.scrollLeft+session.windowWidth-tipWidth)}else if(collisions===Collision.bottom){coords.set("top",session.scrollTop+session.windowHeight-tipHeight)}}else{coords.set("left",session.currentX-tipWidth-options.offset);coords.set("top",session.currentY-tipHeight-options.offset)}}tipElement.css(coords)}}function positionTipOnElement(element){var priorityList,finalPlacement;if(options.smartPlacement||options.followMouse&&element.data(DATA_FORCEDOPEN)){priorityList=$.fn.powerTip.smartPlacementLists[options.placement];$.each(priorityList,function(idx,pos){var collisions=getViewportCollisions(placeTooltip(element,pos),tipElement.outerWidth(),tipElement.outerHeight());finalPlacement=pos;return collisions!==Collision.none})}else{placeTooltip(element,options.placement);finalPlacement=options.placement}tipElement.removeClass("w nw sw e ne se n s w se-alt sw-alt ne-alt nw-alt");tipElement.addClass(finalPlacement)}function placeTooltip(element,placement){var iterationCount=0,tipWidth,tipHeight,coords=new CSSCoordinates;coords.set("top",0);coords.set("left",0);tipElement.css(coords);do{tipWidth=tipElement.outerWidth();tipHeight=tipElement.outerHeight();coords=placementCalculator.compute(element,placement,tipWidth,tipHeight,options.offset);tipElement.css(coords)}while(++iterationCount<=5&&(tipWidth!==tipElement.outerWidth()||tipHeight!==tipElement.outerHeight()));return coords}function closeDesyncedTip(){var isDesynced=false,hasDesyncableCloseEvent=$.grep(["mouseleave","mouseout","blur","focusout"],function(eventType){return $.inArray(eventType,options.closeEvents)!==-1}).length>0;if(session.isTipOpen&&!session.isClosing&&!session.delayInProgress&&hasDesyncableCloseEvent){if(session.activeHover.data(DATA_HASACTIVEHOVER)===false||session.activeHover.is(":disabled")){isDesynced=true}else if(!isMouseOver(session.activeHover)&&!session.activeHover.is(":focus")&&!session.activeHover.data(DATA_FORCEDOPEN)){if(tipElement.data(DATA_MOUSEONTOTIP)){if(!isMouseOver(tipElement)){isDesynced=true}}else{isDesynced=true}}if(isDesynced){hideTip(session.activeHover)}}}this.showTip=beginShowTip;this.hideTip=hideTip;this.resetPosition=positionTipOnElement}function isSvgElement(element){return Boolean(window.SVGElement&&element[0]instanceof SVGElement)}function isMouseEvent(event){return Boolean(event&&$.inArray(event.type,MOUSE_EVENTS)>-1&&typeof event.pageX==="number")}function initTracking(){if(!session.mouseTrackingActive){session.mouseTrackingActive=true;getViewportDimensions();$(getViewportDimensions);$document.on("mousemove"+EVENT_NAMESPACE,trackMouse);$window.on("resize"+EVENT_NAMESPACE,trackResize);$window.on("scroll"+EVENT_NAMESPACE,trackScroll)}}function getViewportDimensions(){session.scrollLeft=$window.scrollLeft();session.scrollTop=$window.scrollTop();session.windowWidth=$window.width();session.windowHeight=$window.height()}function trackResize(){session.windowWidth=$window.width();session.windowHeight=$window.height()}function trackScroll(){var x=$window.scrollLeft(),y=$window.scrollTop();if(x!==session.scrollLeft){session.currentX+=x-session.scrollLeft;session.scrollLeft=x}if(y!==session.scrollTop){session.currentY+=y-session.scrollTop;session.scrollTop=y}}function trackMouse(event){session.currentX=event.pageX;session.currentY=event.pageY}function isMouseOver(element){var elementPosition=element.offset(),elementBox=element[0].getBoundingClientRect(),elementWidth=elementBox.right-elementBox.left,elementHeight=elementBox.bottom-elementBox.top;return session.currentX>=elementPosition.left&&session.currentX<=elementPosition.left+elementWidth&&session.currentY>=elementPosition.top&&session.currentY<=elementPosition.top+elementHeight}function getTooltipContent(element){var tipText=element.data(DATA_POWERTIP),tipObject=element.data(DATA_POWERTIPJQ),tipTarget=element.data(DATA_POWERTIPTARGET),targetElement,content;if(tipText){if($.isFunction(tipText)){tipText=tipText.call(element[0])}content=tipText}else if(tipObject){if($.isFunction(tipObject)){tipObject=tipObject.call(element[0])}if(tipObject.length>0){content=tipObject.clone(true,true)}}else if(tipTarget){targetElement=$("#"+tipTarget);if(targetElement.length>0){content=targetElement.html()}}return content}function getViewportCollisions(coords,elementWidth,elementHeight){var viewportTop=session.scrollTop,viewportLeft=session.scrollLeft,viewportBottom=viewportTop+session.windowHeight,viewportRight=viewportLeft+session.windowWidth,collisions=Collision.none;if(coords.topviewportBottom||Math.abs(coords.bottom-session.windowHeight)>viewportBottom){collisions|=Collision.bottom}if(coords.leftviewportRight){collisions|=Collision.left}if(coords.left+elementWidth>viewportRight||coords.right1)){a.preventDefault();var c=a.originalEvent.changedTouches[0],d=document.createEvent("MouseEvents");d.initMouseEvent(b,!0,!0,window,1,c.screenX,c.screenY,c.clientX,c.clientY,!1,!1,!1,!1,0,null),a.target.dispatchEvent(d)}}if(a.support.touch="ontouchend"in document,a.support.touch){var e,b=a.ui.mouse.prototype,c=b._mouseInit,d=b._mouseDestroy;b._touchStart=function(a){var b=this;!e&&b._mouseCapture(a.originalEvent.changedTouches[0])&&(e=!0,b._touchMoved=!1,f(a,"mouseover"),f(a,"mousemove"),f(a,"mousedown"))},b._touchMove=function(a){e&&(this._touchMoved=!0,f(a,"mousemove"))},b._touchEnd=function(a){e&&(f(a,"mouseup"),f(a,"mouseout"),this._touchMoved||f(a,"click"),e=!1)},b._mouseInit=function(){var b=this;b.element.bind({touchstart:a.proxy(b,"_touchStart"),touchmove:a.proxy(b,"_touchMove"),touchend:a.proxy(b,"_touchEnd")}),c.call(b)},b._mouseDestroy=function(){var b=this;b.element.unbind({touchstart:a.proxy(b,"_touchStart"),touchmove:a.proxy(b,"_touchMove"),touchend:a.proxy(b,"_touchEnd")}),d.call(b)}}}(jQuery);/*! SmartMenus jQuery Plugin - v1.1.0 - September 17, 2017 + * http://www.smartmenus.org/ + * Copyright Vasil Dinkov, Vadikom Web Ltd. http://vadikom.com; Licensed MIT */(function(t){"function"==typeof define&&define.amd?define(["jquery"],t):"object"==typeof module&&"object"==typeof module.exports?module.exports=t(require("jquery")):t(jQuery)})(function($){function initMouseDetection(t){var e=".smartmenus_mouse";if(mouseDetectionEnabled||t)mouseDetectionEnabled&&t&&($(document).off(e),mouseDetectionEnabled=!1);else{var i=!0,s=null,o={mousemove:function(t){var e={x:t.pageX,y:t.pageY,timeStamp:(new Date).getTime()};if(s){var o=Math.abs(s.x-e.x),a=Math.abs(s.y-e.y);if((o>0||a>0)&&2>=o&&2>=a&&300>=e.timeStamp-s.timeStamp&&(mouse=!0,i)){var n=$(t.target).closest("a");n.is("a")&&$.each(menuTrees,function(){return $.contains(this.$root[0],n[0])?(this.itemEnter({currentTarget:n[0]}),!1):void 0}),i=!1}}s=e}};o[touchEvents?"touchstart":"pointerover pointermove pointerout MSPointerOver MSPointerMove MSPointerOut"]=function(t){isTouchEvent(t.originalEvent)&&(mouse=!1)},$(document).on(getEventsNS(o,e)),mouseDetectionEnabled=!0}}function isTouchEvent(t){return!/^(4|mouse)$/.test(t.pointerType)}function getEventsNS(t,e){e||(e="");var i={};for(var s in t)i[s.split(" ").join(e+" ")+e]=t[s];return i}var menuTrees=[],mouse=!1,touchEvents="ontouchstart"in window,mouseDetectionEnabled=!1,requestAnimationFrame=window.requestAnimationFrame||function(t){return setTimeout(t,1e3/60)},cancelAnimationFrame=window.cancelAnimationFrame||function(t){clearTimeout(t)},canAnimate=!!$.fn.animate;return $.SmartMenus=function(t,e){this.$root=$(t),this.opts=e,this.rootId="",this.accessIdPrefix="",this.$subArrow=null,this.activatedItems=[],this.visibleSubMenus=[],this.showTimeout=0,this.hideTimeout=0,this.scrollTimeout=0,this.clickActivated=!1,this.focusActivated=!1,this.zIndexInc=0,this.idInc=0,this.$firstLink=null,this.$firstSub=null,this.disabled=!1,this.$disableOverlay=null,this.$touchScrollingSub=null,this.cssTransforms3d="perspective"in t.style||"webkitPerspective"in t.style,this.wasCollapsible=!1,this.init()},$.extend($.SmartMenus,{hideAll:function(){$.each(menuTrees,function(){this.menuHideAll()})},destroy:function(){for(;menuTrees.length;)menuTrees[0].destroy();initMouseDetection(!0)},prototype:{init:function(t){var e=this;if(!t){menuTrees.push(this),this.rootId=((new Date).getTime()+Math.random()+"").replace(/\D/g,""),this.accessIdPrefix="sm-"+this.rootId+"-",this.$root.hasClass("sm-rtl")&&(this.opts.rightToLeftSubMenus=!0);var i=".smartmenus";this.$root.data("smartmenus",this).attr("data-smartmenus-id",this.rootId).dataSM("level",1).on(getEventsNS({"mouseover focusin":$.proxy(this.rootOver,this),"mouseout focusout":$.proxy(this.rootOut,this),keydown:$.proxy(this.rootKeyDown,this)},i)).on(getEventsNS({mouseenter:$.proxy(this.itemEnter,this),mouseleave:$.proxy(this.itemLeave,this),mousedown:$.proxy(this.itemDown,this),focus:$.proxy(this.itemFocus,this),blur:$.proxy(this.itemBlur,this),click:$.proxy(this.itemClick,this)},i),"a"),i+=this.rootId,this.opts.hideOnClick&&$(document).on(getEventsNS({touchstart:$.proxy(this.docTouchStart,this),touchmove:$.proxy(this.docTouchMove,this),touchend:$.proxy(this.docTouchEnd,this),click:$.proxy(this.docClick,this)},i)),$(window).on(getEventsNS({"resize orientationchange":$.proxy(this.winResize,this)},i)),this.opts.subIndicators&&(this.$subArrow=$("").addClass("sub-arrow"),this.opts.subIndicatorsText&&this.$subArrow.html(this.opts.subIndicatorsText)),initMouseDetection()}if(this.$firstSub=this.$root.find("ul").each(function(){e.menuInit($(this))}).eq(0),this.$firstLink=this.$root.find("a").eq(0),this.opts.markCurrentItem){var s=/(index|default)\.[^#\?\/]*/i,o=/#.*/,a=window.location.href.replace(s,""),n=a.replace(o,"");this.$root.find("a").each(function(){var t=this.href.replace(s,""),i=$(this);(t==a||t==n)&&(i.addClass("current"),e.opts.markCurrentTree&&i.parentsUntil("[data-smartmenus-id]","ul").each(function(){$(this).dataSM("parent-a").addClass("current")}))})}this.wasCollapsible=this.isCollapsible()},destroy:function(t){if(!t){var e=".smartmenus";this.$root.removeData("smartmenus").removeAttr("data-smartmenus-id").removeDataSM("level").off(e),e+=this.rootId,$(document).off(e),$(window).off(e),this.opts.subIndicators&&(this.$subArrow=null)}this.menuHideAll();var i=this;this.$root.find("ul").each(function(){var t=$(this);t.dataSM("scroll-arrows")&&t.dataSM("scroll-arrows").remove(),t.dataSM("shown-before")&&((i.opts.subMenusMinWidth||i.opts.subMenusMaxWidth)&&t.css({width:"",minWidth:"",maxWidth:""}).removeClass("sm-nowrap"),t.dataSM("scroll-arrows")&&t.dataSM("scroll-arrows").remove(),t.css({zIndex:"",top:"",left:"",marginLeft:"",marginTop:"",display:""})),0==(t.attr("id")||"").indexOf(i.accessIdPrefix)&&t.removeAttr("id")}).removeDataSM("in-mega").removeDataSM("shown-before").removeDataSM("scroll-arrows").removeDataSM("parent-a").removeDataSM("level").removeDataSM("beforefirstshowfired").removeAttr("role").removeAttr("aria-hidden").removeAttr("aria-labelledby").removeAttr("aria-expanded"),this.$root.find("a.has-submenu").each(function(){var t=$(this);0==t.attr("id").indexOf(i.accessIdPrefix)&&t.removeAttr("id")}).removeClass("has-submenu").removeDataSM("sub").removeAttr("aria-haspopup").removeAttr("aria-controls").removeAttr("aria-expanded").closest("li").removeDataSM("sub"),this.opts.subIndicators&&this.$root.find("span.sub-arrow").remove(),this.opts.markCurrentItem&&this.$root.find("a.current").removeClass("current"),t||(this.$root=null,this.$firstLink=null,this.$firstSub=null,this.$disableOverlay&&(this.$disableOverlay.remove(),this.$disableOverlay=null),menuTrees.splice($.inArray(this,menuTrees),1))},disable:function(t){if(!this.disabled){if(this.menuHideAll(),!t&&!this.opts.isPopup&&this.$root.is(":visible")){var e=this.$root.offset();this.$disableOverlay=$('
').css({position:"absolute",top:e.top,left:e.left,width:this.$root.outerWidth(),height:this.$root.outerHeight(),zIndex:this.getStartZIndex(!0),opacity:0}).appendTo(document.body)}this.disabled=!0}},docClick:function(t){return this.$touchScrollingSub?(this.$touchScrollingSub=null,void 0):((this.visibleSubMenus.length&&!$.contains(this.$root[0],t.target)||$(t.target).closest("a").length)&&this.menuHideAll(),void 0)},docTouchEnd:function(){if(this.lastTouch){if(!(!this.visibleSubMenus.length||void 0!==this.lastTouch.x2&&this.lastTouch.x1!=this.lastTouch.x2||void 0!==this.lastTouch.y2&&this.lastTouch.y1!=this.lastTouch.y2||this.lastTouch.target&&$.contains(this.$root[0],this.lastTouch.target))){this.hideTimeout&&(clearTimeout(this.hideTimeout),this.hideTimeout=0);var t=this;this.hideTimeout=setTimeout(function(){t.menuHideAll()},350)}this.lastTouch=null}},docTouchMove:function(t){if(this.lastTouch){var e=t.originalEvent.touches[0];this.lastTouch.x2=e.pageX,this.lastTouch.y2=e.pageY}},docTouchStart:function(t){var e=t.originalEvent.touches[0];this.lastTouch={x1:e.pageX,y1:e.pageY,target:e.target}},enable:function(){this.disabled&&(this.$disableOverlay&&(this.$disableOverlay.remove(),this.$disableOverlay=null),this.disabled=!1)},getClosestMenu:function(t){for(var e=$(t).closest("ul");e.dataSM("in-mega");)e=e.parent().closest("ul");return e[0]||null},getHeight:function(t){return this.getOffset(t,!0)},getOffset:function(t,e){var i;"none"==t.css("display")&&(i={position:t[0].style.position,visibility:t[0].style.visibility},t.css({position:"absolute",visibility:"hidden"}).show());var s=t[0].getBoundingClientRect&&t[0].getBoundingClientRect(),o=s&&(e?s.height||s.bottom-s.top:s.width||s.right-s.left);return o||0===o||(o=e?t[0].offsetHeight:t[0].offsetWidth),i&&t.hide().css(i),o},getStartZIndex:function(t){var e=parseInt(this[t?"$root":"$firstSub"].css("z-index"));return!t&&isNaN(e)&&(e=parseInt(this.$root.css("z-index"))),isNaN(e)?1:e},getTouchPoint:function(t){return t.touches&&t.touches[0]||t.changedTouches&&t.changedTouches[0]||t},getViewport:function(t){var e=t?"Height":"Width",i=document.documentElement["client"+e],s=window["inner"+e];return s&&(i=Math.min(i,s)),i},getViewportHeight:function(){return this.getViewport(!0)},getViewportWidth:function(){return this.getViewport()},getWidth:function(t){return this.getOffset(t)},handleEvents:function(){return!this.disabled&&this.isCSSOn()},handleItemEvents:function(t){return this.handleEvents()&&!this.isLinkInMegaMenu(t)},isCollapsible:function(){return"static"==this.$firstSub.css("position")},isCSSOn:function(){return"inline"!=this.$firstLink.css("display")},isFixed:function(){var t="fixed"==this.$root.css("position");return t||this.$root.parentsUntil("body").each(function(){return"fixed"==$(this).css("position")?(t=!0,!1):void 0}),t},isLinkInMegaMenu:function(t){return $(this.getClosestMenu(t[0])).hasClass("mega-menu")},isTouchMode:function(){return!mouse||this.opts.noMouseOver||this.isCollapsible()},itemActivate:function(t,e){var i=t.closest("ul"),s=i.dataSM("level");if(s>1&&(!this.activatedItems[s-2]||this.activatedItems[s-2][0]!=i.dataSM("parent-a")[0])){var o=this;$(i.parentsUntil("[data-smartmenus-id]","ul").get().reverse()).add(i).each(function(){o.itemActivate($(this).dataSM("parent-a"))})}if((!this.isCollapsible()||e)&&this.menuHideSubMenus(this.activatedItems[s-1]&&this.activatedItems[s-1][0]==t[0]?s:s-1),this.activatedItems[s-1]=t,this.$root.triggerHandler("activate.smapi",t[0])!==!1){var a=t.dataSM("sub");a&&(this.isTouchMode()||!this.opts.showOnClick||this.clickActivated)&&this.menuShow(a)}},itemBlur:function(t){var e=$(t.currentTarget);this.handleItemEvents(e)&&this.$root.triggerHandler("blur.smapi",e[0])},itemClick:function(t){var e=$(t.currentTarget);if(this.handleItemEvents(e)){if(this.$touchScrollingSub&&this.$touchScrollingSub[0]==e.closest("ul")[0])return this.$touchScrollingSub=null,t.stopPropagation(),!1;if(this.$root.triggerHandler("click.smapi",e[0])===!1)return!1;var i=$(t.target).is(".sub-arrow"),s=e.dataSM("sub"),o=s?2==s.dataSM("level"):!1,a=this.isCollapsible(),n=/toggle$/.test(this.opts.collapsibleBehavior),r=/link$/.test(this.opts.collapsibleBehavior),h=/^accordion/.test(this.opts.collapsibleBehavior);if(s&&!s.is(":visible")){if((!r||!a||i)&&(this.opts.showOnClick&&o&&(this.clickActivated=!0),this.itemActivate(e,h),s.is(":visible")))return this.focusActivated=!0,!1}else if(a&&(n||i))return this.itemActivate(e,h),this.menuHide(s),n&&(this.focusActivated=!1),!1;return this.opts.showOnClick&&o||e.hasClass("disabled")||this.$root.triggerHandler("select.smapi",e[0])===!1?!1:void 0}},itemDown:function(t){var e=$(t.currentTarget);this.handleItemEvents(e)&&e.dataSM("mousedown",!0)},itemEnter:function(t){var e=$(t.currentTarget);if(this.handleItemEvents(e)){if(!this.isTouchMode()){this.showTimeout&&(clearTimeout(this.showTimeout),this.showTimeout=0);var i=this;this.showTimeout=setTimeout(function(){i.itemActivate(e)},this.opts.showOnClick&&1==e.closest("ul").dataSM("level")?1:this.opts.showTimeout)}this.$root.triggerHandler("mouseenter.smapi",e[0])}},itemFocus:function(t){var e=$(t.currentTarget);this.handleItemEvents(e)&&(!this.focusActivated||this.isTouchMode()&&e.dataSM("mousedown")||this.activatedItems.length&&this.activatedItems[this.activatedItems.length-1][0]==e[0]||this.itemActivate(e,!0),this.$root.triggerHandler("focus.smapi",e[0]))},itemLeave:function(t){var e=$(t.currentTarget);this.handleItemEvents(e)&&(this.isTouchMode()||(e[0].blur(),this.showTimeout&&(clearTimeout(this.showTimeout),this.showTimeout=0)),e.removeDataSM("mousedown"),this.$root.triggerHandler("mouseleave.smapi",e[0]))},menuHide:function(t){if(this.$root.triggerHandler("beforehide.smapi",t[0])!==!1&&(canAnimate&&t.stop(!0,!0),"none"!=t.css("display"))){var e=function(){t.css("z-index","")};this.isCollapsible()?canAnimate&&this.opts.collapsibleHideFunction?this.opts.collapsibleHideFunction.call(this,t,e):t.hide(this.opts.collapsibleHideDuration,e):canAnimate&&this.opts.hideFunction?this.opts.hideFunction.call(this,t,e):t.hide(this.opts.hideDuration,e),t.dataSM("scroll")&&(this.menuScrollStop(t),t.css({"touch-action":"","-ms-touch-action":"","-webkit-transform":"",transform:""}).off(".smartmenus_scroll").removeDataSM("scroll").dataSM("scroll-arrows").hide()),t.dataSM("parent-a").removeClass("highlighted").attr("aria-expanded","false"),t.attr({"aria-expanded":"false","aria-hidden":"true"});var i=t.dataSM("level");this.activatedItems.splice(i-1,1),this.visibleSubMenus.splice($.inArray(t,this.visibleSubMenus),1),this.$root.triggerHandler("hide.smapi",t[0])}},menuHideAll:function(){this.showTimeout&&(clearTimeout(this.showTimeout),this.showTimeout=0);for(var t=this.opts.isPopup?1:0,e=this.visibleSubMenus.length-1;e>=t;e--)this.menuHide(this.visibleSubMenus[e]);this.opts.isPopup&&(canAnimate&&this.$root.stop(!0,!0),this.$root.is(":visible")&&(canAnimate&&this.opts.hideFunction?this.opts.hideFunction.call(this,this.$root):this.$root.hide(this.opts.hideDuration))),this.activatedItems=[],this.visibleSubMenus=[],this.clickActivated=!1,this.focusActivated=!1,this.zIndexInc=0,this.$root.triggerHandler("hideAll.smapi")},menuHideSubMenus:function(t){for(var e=this.activatedItems.length-1;e>=t;e--){var i=this.activatedItems[e].dataSM("sub");i&&this.menuHide(i)}},menuInit:function(t){if(!t.dataSM("in-mega")){t.hasClass("mega-menu")&&t.find("ul").dataSM("in-mega",!0);for(var e=2,i=t[0];(i=i.parentNode.parentNode)!=this.$root[0];)e++;var s=t.prevAll("a").eq(-1);s.length||(s=t.prevAll().find("a").eq(-1)),s.addClass("has-submenu").dataSM("sub",t),t.dataSM("parent-a",s).dataSM("level",e).parent().dataSM("sub",t);var o=s.attr("id")||this.accessIdPrefix+ ++this.idInc,a=t.attr("id")||this.accessIdPrefix+ ++this.idInc;s.attr({id:o,"aria-haspopup":"true","aria-controls":a,"aria-expanded":"false"}),t.attr({id:a,role:"group","aria-hidden":"true","aria-labelledby":o,"aria-expanded":"false"}),this.opts.subIndicators&&s[this.opts.subIndicatorsPos](this.$subArrow.clone())}},menuPosition:function(t){var e,i,s=t.dataSM("parent-a"),o=s.closest("li"),a=o.parent(),n=t.dataSM("level"),r=this.getWidth(t),h=this.getHeight(t),u=s.offset(),l=u.left,c=u.top,d=this.getWidth(s),m=this.getHeight(s),p=$(window),f=p.scrollLeft(),v=p.scrollTop(),b=this.getViewportWidth(),S=this.getViewportHeight(),g=a.parent().is("[data-sm-horizontal-sub]")||2==n&&!a.hasClass("sm-vertical"),M=this.opts.rightToLeftSubMenus&&!o.is("[data-sm-reverse]")||!this.opts.rightToLeftSubMenus&&o.is("[data-sm-reverse]"),w=2==n?this.opts.mainMenuSubOffsetX:this.opts.subMenusSubOffsetX,T=2==n?this.opts.mainMenuSubOffsetY:this.opts.subMenusSubOffsetY;if(g?(e=M?d-r-w:w,i=this.opts.bottomToTopSubMenus?-h-T:m+T):(e=M?w-r:d-w,i=this.opts.bottomToTopSubMenus?m-T-h:T),this.opts.keepInViewport){var y=l+e,I=c+i;if(M&&f>y?e=g?f-y+e:d-w:!M&&y+r>f+b&&(e=g?f+b-r-y+e:w-r),g||(S>h&&I+h>v+S?i+=v+S-h-I:(h>=S||v>I)&&(i+=v-I)),g&&(I+h>v+S+.49||v>I)||!g&&h>S+.49){var x=this;t.dataSM("scroll-arrows")||t.dataSM("scroll-arrows",$([$('')[0],$('')[0]]).on({mouseenter:function(){t.dataSM("scroll").up=$(this).hasClass("scroll-up"),x.menuScroll(t)},mouseleave:function(e){x.menuScrollStop(t),x.menuScrollOut(t,e)},"mousewheel DOMMouseScroll":function(t){t.preventDefault()}}).insertAfter(t));var A=".smartmenus_scroll";if(t.dataSM("scroll",{y:this.cssTransforms3d?0:i-m,step:1,itemH:m,subH:h,arrowDownH:this.getHeight(t.dataSM("scroll-arrows").eq(1))}).on(getEventsNS({mouseover:function(e){x.menuScrollOver(t,e)},mouseout:function(e){x.menuScrollOut(t,e)},"mousewheel DOMMouseScroll":function(e){x.menuScrollMousewheel(t,e)}},A)).dataSM("scroll-arrows").css({top:"auto",left:"0",marginLeft:e+(parseInt(t.css("border-left-width"))||0),width:r-(parseInt(t.css("border-left-width"))||0)-(parseInt(t.css("border-right-width"))||0),zIndex:t.css("z-index")}).eq(g&&this.opts.bottomToTopSubMenus?0:1).show(),this.isFixed()){var C={};C[touchEvents?"touchstart touchmove touchend":"pointerdown pointermove pointerup MSPointerDown MSPointerMove MSPointerUp"]=function(e){x.menuScrollTouch(t,e)},t.css({"touch-action":"none","-ms-touch-action":"none"}).on(getEventsNS(C,A))}}}t.css({top:"auto",left:"0",marginLeft:e,marginTop:i-m})},menuScroll:function(t,e,i){var s,o=t.dataSM("scroll"),a=t.dataSM("scroll-arrows"),n=o.up?o.upEnd:o.downEnd;if(!e&&o.momentum){if(o.momentum*=.92,s=o.momentum,.5>s)return this.menuScrollStop(t),void 0}else s=i||(e||!this.opts.scrollAccelerate?this.opts.scrollStep:Math.floor(o.step));var r=t.dataSM("level");if(this.activatedItems[r-1]&&this.activatedItems[r-1].dataSM("sub")&&this.activatedItems[r-1].dataSM("sub").is(":visible")&&this.menuHideSubMenus(r-1),o.y=o.up&&o.y>=n||!o.up&&n>=o.y?o.y:Math.abs(n-o.y)>s?o.y+(o.up?s:-s):n,t.css(this.cssTransforms3d?{"-webkit-transform":"translate3d(0, "+o.y+"px, 0)",transform:"translate3d(0, "+o.y+"px, 0)"}:{marginTop:o.y}),mouse&&(o.up&&o.y>o.downEnd||!o.up&&o.y0;t.dataSM("scroll-arrows").eq(i?0:1).is(":visible")&&(t.dataSM("scroll").up=i,this.menuScroll(t,!0))}e.preventDefault()},menuScrollOut:function(t,e){mouse&&(/^scroll-(up|down)/.test((e.relatedTarget||"").className)||(t[0]==e.relatedTarget||$.contains(t[0],e.relatedTarget))&&this.getClosestMenu(e.relatedTarget)==t[0]||t.dataSM("scroll-arrows").css("visibility","hidden"))},menuScrollOver:function(t,e){if(mouse&&!/^scroll-(up|down)/.test(e.target.className)&&this.getClosestMenu(e.target)==t[0]){this.menuScrollRefreshData(t);var i=t.dataSM("scroll"),s=$(window).scrollTop()-t.dataSM("parent-a").offset().top-i.itemH;t.dataSM("scroll-arrows").eq(0).css("margin-top",s).end().eq(1).css("margin-top",s+this.getViewportHeight()-i.arrowDownH).end().css("visibility","visible")}},menuScrollRefreshData:function(t){var e=t.dataSM("scroll"),i=$(window).scrollTop()-t.dataSM("parent-a").offset().top-e.itemH;this.cssTransforms3d&&(i=-(parseFloat(t.css("margin-top"))-i)),$.extend(e,{upEnd:i,downEnd:i+this.getViewportHeight()-e.subH})},menuScrollStop:function(t){return this.scrollTimeout?(cancelAnimationFrame(this.scrollTimeout),this.scrollTimeout=0,t.dataSM("scroll").step=1,!0):void 0},menuScrollTouch:function(t,e){if(e=e.originalEvent,isTouchEvent(e)){var i=this.getTouchPoint(e);if(this.getClosestMenu(i.target)==t[0]){var s=t.dataSM("scroll");if(/(start|down)$/i.test(e.type))this.menuScrollStop(t)?(e.preventDefault(),this.$touchScrollingSub=t):this.$touchScrollingSub=null,this.menuScrollRefreshData(t),$.extend(s,{touchStartY:i.pageY,touchStartTime:e.timeStamp});else if(/move$/i.test(e.type)){var o=void 0!==s.touchY?s.touchY:s.touchStartY;if(void 0!==o&&o!=i.pageY){this.$touchScrollingSub=t;var a=i.pageY>o;void 0!==s.up&&s.up!=a&&$.extend(s,{touchStartY:i.pageY,touchStartTime:e.timeStamp}),$.extend(s,{up:a,touchY:i.pageY}),this.menuScroll(t,!0,Math.abs(i.pageY-o))}e.preventDefault()}else void 0!==s.touchY&&((s.momentum=15*Math.pow(Math.abs(i.pageY-s.touchStartY)/(e.timeStamp-s.touchStartTime),2))&&(this.menuScrollStop(t),this.menuScroll(t),e.preventDefault()),delete s.touchY)}}},menuShow:function(t){if((t.dataSM("beforefirstshowfired")||(t.dataSM("beforefirstshowfired",!0),this.$root.triggerHandler("beforefirstshow.smapi",t[0])!==!1))&&this.$root.triggerHandler("beforeshow.smapi",t[0])!==!1&&(t.dataSM("shown-before",!0),canAnimate&&t.stop(!0,!0),!t.is(":visible"))){var e=t.dataSM("parent-a"),i=this.isCollapsible();if((this.opts.keepHighlighted||i)&&e.addClass("highlighted"),i)t.removeClass("sm-nowrap").css({zIndex:"",width:"auto",minWidth:"",maxWidth:"",top:"",left:"",marginLeft:"",marginTop:""});else{if(t.css("z-index",this.zIndexInc=(this.zIndexInc||this.getStartZIndex())+1),(this.opts.subMenusMinWidth||this.opts.subMenusMaxWidth)&&(t.css({width:"auto",minWidth:"",maxWidth:""}).addClass("sm-nowrap"),this.opts.subMenusMinWidth&&t.css("min-width",this.opts.subMenusMinWidth),this.opts.subMenusMaxWidth)){var s=this.getWidth(t);t.css("max-width",this.opts.subMenusMaxWidth),s>this.getWidth(t)&&t.removeClass("sm-nowrap").css("width",this.opts.subMenusMaxWidth)}this.menuPosition(t)}var o=function(){t.css("overflow","")};i?canAnimate&&this.opts.collapsibleShowFunction?this.opts.collapsibleShowFunction.call(this,t,o):t.show(this.opts.collapsibleShowDuration,o):canAnimate&&this.opts.showFunction?this.opts.showFunction.call(this,t,o):t.show(this.opts.showDuration,o),e.attr("aria-expanded","true"),t.attr({"aria-expanded":"true","aria-hidden":"false"}),this.visibleSubMenus.push(t),this.$root.triggerHandler("show.smapi",t[0])}},popupHide:function(t){this.hideTimeout&&(clearTimeout(this.hideTimeout),this.hideTimeout=0);var e=this;this.hideTimeout=setTimeout(function(){e.menuHideAll()},t?1:this.opts.hideTimeout)},popupShow:function(t,e){if(!this.opts.isPopup)return alert('SmartMenus jQuery Error:\n\nIf you want to show this menu via the "popupShow" method, set the isPopup:true option.'),void 0;if(this.hideTimeout&&(clearTimeout(this.hideTimeout),this.hideTimeout=0),this.$root.dataSM("shown-before",!0),canAnimate&&this.$root.stop(!0,!0),!this.$root.is(":visible")){this.$root.css({left:t,top:e});var i=this,s=function(){i.$root.css("overflow","")};canAnimate&&this.opts.showFunction?this.opts.showFunction.call(this,this.$root,s):this.$root.show(this.opts.showDuration,s),this.visibleSubMenus[0]=this.$root}},refresh:function(){this.destroy(!0),this.init(!0)},rootKeyDown:function(t){if(this.handleEvents())switch(t.keyCode){case 27:var e=this.activatedItems[0];if(e){this.menuHideAll(),e[0].focus();var i=e.dataSM("sub");i&&this.menuHide(i)}break;case 32:var s=$(t.target);if(s.is("a")&&this.handleItemEvents(s)){var i=s.dataSM("sub");i&&!i.is(":visible")&&(this.itemClick({currentTarget:t.target}),t.preventDefault())}}},rootOut:function(t){if(this.handleEvents()&&!this.isTouchMode()&&t.target!=this.$root[0]&&(this.hideTimeout&&(clearTimeout(this.hideTimeout),this.hideTimeout=0),!this.opts.showOnClick||!this.opts.hideOnClick)){var e=this;this.hideTimeout=setTimeout(function(){e.menuHideAll()},this.opts.hideTimeout)}},rootOver:function(t){this.handleEvents()&&!this.isTouchMode()&&t.target!=this.$root[0]&&this.hideTimeout&&(clearTimeout(this.hideTimeout),this.hideTimeout=0)},winResize:function(t){if(this.handleEvents()){if(!("onorientationchange"in window)||"orientationchange"==t.type){var e=this.isCollapsible();this.wasCollapsible&&e||(this.activatedItems.length&&this.activatedItems[this.activatedItems.length-1][0].blur(),this.menuHideAll()),this.wasCollapsible=e}}else if(this.$disableOverlay){var i=this.$root.offset();this.$disableOverlay.css({top:i.top,left:i.left,width:this.$root.outerWidth(),height:this.$root.outerHeight()})}}}}),$.fn.dataSM=function(t,e){return e?this.data(t+"_smartmenus",e):this.data(t+"_smartmenus")},$.fn.removeDataSM=function(t){return this.removeData(t+"_smartmenus")},$.fn.smartmenus=function(options){if("string"==typeof options){var args=arguments,method=options;return Array.prototype.shift.call(args),this.each(function(){var t=$(this).data("smartmenus");t&&t[method]&&t[method].apply(t,args)})}return this.each(function(){var dataOpts=$(this).data("sm-options")||null;if(dataOpts)try{dataOpts=eval("("+dataOpts+")")}catch(e){dataOpts=null,alert('ERROR\n\nSmartMenus jQuery init:\nInvalid "data-sm-options" attribute value syntax.')}new $.SmartMenus(this,$.extend({},$.fn.smartmenus.defaults,options,dataOpts))})},$.fn.smartmenus.defaults={isPopup:!1,mainMenuSubOffsetX:0,mainMenuSubOffsetY:0,subMenusSubOffsetX:0,subMenusSubOffsetY:0,subMenusMinWidth:"10em",subMenusMaxWidth:"20em",subIndicators:!0,subIndicatorsPos:"append",subIndicatorsText:"",scrollStep:30,scrollAccelerate:!0,showTimeout:250,hideTimeout:500,showDuration:0,showFunction:null,hideDuration:0,hideFunction:function(t,e){t.fadeOut(200,e)},collapsibleShowDuration:0,collapsibleShowFunction:function(t,e){t.slideDown(200,e)},collapsibleHideDuration:0,collapsibleHideFunction:function(t,e){t.slideUp(200,e)},showOnClick:!1,hideOnClick:!0,noMouseOver:!1,keepInViewport:!0,keepHighlighted:!0,markCurrentItem:!1,markCurrentTree:!0,rightToLeftSubMenus:!1,bottomToTopSubMenus:!1,collapsibleBehavior:"default"},$}); \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/md_doc_CHANGELOG.html b/Devices/Libraries/Systems/CANopenSocket/docs/md_doc_CHANGELOG.html new file mode 100755 index 0000000..070b32f --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/md_doc_CHANGELOG.html @@ -0,0 +1,231 @@ + + + + + + + +CANopenNode: Change Log + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+
Change Log
+
+
+

newOD

+ +
    +
  • New Object dictionary interface. It has very similar principles as before. All parts of CANopenNode objects, which works with OD entries, are rewritten.
    • +
  • New OD.h and OD.c files, replaces CO_OD files.
    • +
  • CANopen.c and CANopen.h files redesigned. Integration of "OD.h" is optional. Configuration of multiple object dictionaries is possible with one CANopen device. Interface is the same, with some changes to function arguments.
    • +
  • Rewritten SDO server. Object dictionary part is removed.
    • +
  • CO_Emergency is mostly rewritten. Now is much easier customization.
    • +
  • CO_NMT_Heartbeat is redesigned.
    • +
+

Unreleased master

+ +
    +
  • All drivers removed from this project, except Neuberger-socketCAN for Linux.

    Changed

    +
    • +
+
    +
  • Directory structure rearranged. Before was all CANopen object files in stack directory, now they are in separate directories according to standard (301, 305, extra, socketCAN for Linux driver). Include directives for that files now contain directory path. CO_SDO renamed to CO_SDOserver and CO_SDOmaster renamed to CO_SDOclient. Change of the project files will be necessary.
    • +
  • Driver interface clarified. Before was pair of CO_driver.h/.c files for each microcontroller, now there is common CO_driver.h file. Drivers for other microcontrollers will be separate projects. Each driver must have own CO_driver_target.h file and function definitions from C_driver.h file. See documentation in CO_driver.h, example/CO_driver_target.h and example/CO_driver.c. There was no other major changes in driver interface.
    • +
  • Time base is now microsecond in all functions.
    • +
  • CANopen.h/.c files simplified and changed. CO_USE_GLOBALS and CO_init() removed. Interface to those functions changed.
    • +
  • CO_NMT_sendCommand() master function renamed and moved from CANopen.c into CO_NMT_Heartbeat.c.
    • +
  • Heartbeat consumer CO_HBconsumer_process() optimized.
    • +
  • Rename in CO_driver_target.h: IS_CANrxNew -> CO_FLAG_READ, SET_CANrxNew -> CO_FLAG_SET, CLEAR_CANrxNew -> CO_FLAG_CLEAR
    • +
  • CO_driver.h file, function CO_CANrxBufferInit(), last argument (callback) changed from (*pFunct)(void *object, const CO_CANrxMsg_t *message) to void (*CANrx_callback)(void *object, void *message). New functions are defined in CO_driver_target.h file: CO_CANrxMsg_readIdent(), CO_CANrxMsg_readDLC() and CO_CANrxMsg_readData().
    • +
  • It is necessary to manually update CO_OD.c file - it must include: 301/CO_driver.h, CO_OD.h and 301/CO_SDOserver.h.
    • +
  • Added void *object argument to CO_*_initCallback() functions. API clarified.
    • +
  • Add emergency receive callback also for own emergency messages.
    • +
  • Heartbeat is send immediately after NMT state changes.
    • +
  • SDO client is rewritten. Now includes read/write fifo interface to transfer data.
    • +
  • LED indicator indication (CiA303-3) moved from NMT into own files. Now fully comply to standard.
    • +
  • LSS slave is integrated into CANopenNode more directly.
    • +
  • CO_driver interface: remove Emergency object dependency for reporting CAN errors, use CANerrorStatus own variable instead. Emergency object updated.

    Changed SocketCAN

    +
    • +
+
    +
  • ./stack/socketCAN removed from the project, ./stack/Neuberger-socketCAN moved to ./socketCAN
    • +
  • driver API updated
    • +
  • CO_Linux_threads.h, function void CANrx_threadTmr_init(uint16_t interval_in_milliseconds (changed to) uint32_t interval_in_microseconds)
    • +
  • CO_CANrxBufferInit(): remove check COB ID already used.
    • +
  • change macros CO_DRIVER_MULTI_INTERFACE and CO_DRIVER_ERROR_REPORTING. To enable(disable), set to 1(0).
    • +
  • Rename CO_Linux_threads.h/.c to CO_epoll_interface.h/.c and reorganize them. Move epoll, timerfd and eventfd system calls from CO_driver.c to here.
    • +
  • Can run in single thread, including gateway.

    Fixed

    +
    • +
+
    +
  • Bugfix in CO_HBconsumer_process(): argument timeDifference_us was set to 0 inside for loop, fixed now.
    • +
  • BUG in CO_HBconsumer.c #168

    Added

    +
    • +
+
    +
  • Documentation added to doc directory: CHANGELOG.md, deviceSupport.md, gettingStarted.md, LSSusage.md and traceUsage.md.
    • +
  • All CANopen objects calculates next timer info for OS. Useful for energy saving.
    • +
  • Added file CO_config.h for stack configuration. Can be overridden by target specific or by custom definitions. It enables/disables whole CanOpenNode objects or parts of them. It also specifies some constants.
    • +
  • CO_fifo.h/c for fifo data buffer, used with rewritten SDO client, etc.
    • +
  • CANopen gateway-ascii command interface according to CiA309-3 as a microcontroller independent module. It includes NMT master, LSS master and SDO client interface. Interface is non-blocking, it is added to mainline. Example for Linux stdio and socket is included.
    • +
+

v1.3 - 2020-04-27

+ +
    +
  • License changed to Apache 2.0.
    • +
  • NMT self start functionality (OD object 1F80) implemented to strictly follow standard. Default value for object 1F80 have to be updated in OD editor. See README.md.

    Fixed

    +
    • +
+
    +
  • Various fixes.
    • +
  • neuberger-socketCAN fixed.

    Added

    +
    • +
+
    +
  • CANopen TIME protocol added.
    • +
+

v1.2 - 2019-10-08

+ +
    +
  • Memory barrier implemented for setting/clearing flags for CAN received message.
    • +
  • CO_Emergency and CO_HBconsumer files revised.

    Added

    +
    • +
+
    +
  • CANopen LSS master/slave protocol added for configuration of bitrate and node ID.
    • +
  • Neuberger-socketCAN driver added.
    • +
  • Emergency consumer added.
    • +
  • Callbacks added to Emergency and Heartbeat consumer.
    • +
+

v1.1 - 2019-10-08

+
    +
  • Full ChangeLog
    • +
  • Bugfixes. Some non-critical warnings in stack, some formatting warnings in tracing stuff.
    • +
+

v1.0 - 2017-08-01

+ +

v0.5 - 2015-10-20

+
    +
  • Git repository started on GitHub.
    • +
+

v0.4 - 2012-02-26

+
    +
  • Git repository started on Sourceforge git.
    • +
+

v0.1 - 2004-06-29

+
    +
  • First edition of CANopenNode on SourceForge, files section. (V0.80 on SourceForge).
    • +
+
+

Changelog written according to recommendations from https://keepachangelog.com/

+
+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/md_doc_LSSusage.html b/Devices/Libraries/Systems/CANopenSocket/docs/md_doc_LSSusage.html new file mode 100755 index 0000000..5bdb494 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/md_doc_LSSusage.html @@ -0,0 +1,206 @@ + + + + + + + +CANopenNode: LSS usage + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+
LSS usage
+
+
+

LSS (Layer settings service) is an extension to CANopen described in CiA DSP 305. The interface is described in CiA DS 309 3.0.0 (ASCII mapping). LSS allows the user to change node ID and bitrate, as well as setting the node ID on an unconfigured node.

+

LSS uses the the OD Identity register (0x1018) as an unique value to select a node. Therefore the LSS address always consists of four 32 bit values. This also means that LSS relies on this register to actually be unique. (vendorID, productCode, revisionNumber and serialNumber must be configured and unique on each device.)

+

Preparation for testing on Linux virtual CAN

+

LSS can be tested on Linux virtual CAN, similar as in gettingStarted.md.

+
    +
  1. Open terminal, setup vcan0 and cd to CANopenNode directory.
    2. +
  2. Make some unique CANopen devices:
      +
    • Edit CO_OD.c, change initialization for identity, for example change line to /*1018*/ {0x4, 0x1L, 0x2L, 0x3L, 0x4L},
      • +
    • make
      • +
    • mv canopend canopend4
      • +
    • Edit CO_OD.c, for example: /*1018*/ {0x4, 0x1L, 0x2L, 0x3L, 0x5L},
      • +
    • make
      • +
    • mv canopend canopend5
      • +
    • Repeat this step and create three further "unique" CANopen devices.
      • +
    +
    3. +
  3. Clear default OD storage file. We will use default (empty) storage for all instances:
      +
    • echo "-" > od_storage
      • +
    +
    4. +
  4. Run "master" with command interface and node-id = 1. Note that this device has enabled both: LSS master and LSS slave. But LSS master does not 'see' own LSS slave.
      +
    • make
      • +
    • ./canopend vcan0 -i1 -c "stdio"
      • +
    +
    5. +
  5. Run one CANopen device with node-id=22 in own terminal:
      +
    • ./canopend4 vcan0 -i22
      • +
    +
    6. +
  6. Run other unique CANopen devices with unconfigured node-id, each in own terminal:
      +
    • ./canopend5 vcan0 -i0xFF
      • +
    • ./canopend6 vcan0 -i0xFF
      • +
    • ./canopend7 vcan0 -i0xFF
      • +
    • ./canopend8 vcan0 -i0xFF
      • +
    +
    7. +
  7. Note that lss_store does not work in this example. For it to work, OD storage must be used properly.
    8. +
+

Typical usage of LSS

+
    +

  • Changing the node ID for a known slave, store the new node ID to eeprom, apply new node ID. The node currently has the node ID 22. 

     help lss
+
+ lss_switch_sel 0x00000001 0x00000002 0x00000003 0x00000004
+ lss_set_node 10
+ lss_store
+ lss_switch_glob 0
+ 22 reset communication
+

    Note that the node ID change is not done until reset communication/node.

    +
    • +

  • Changing the node ID for a known slave, store the new node ID to eeprom, apply new node ID. The node currently has an invalid node ID. 

     lss_switch_sel 0x00000001 0x00000002 0x00000003 0x00000005
+ lss_set_node 11
+ lss_store
+ lss_switch_glob 0
+

    Note that the node ID is automatically applied. This can be seen on candump.

    +
    • +
+

LSS fastscan

+
    +

  • Search for a node via LSS fastscan, store the new node ID to eeprom, apply new node ID 

     _lss_fastscan
+
+ [0] 0x00000001 0x00000002 0x00000003 0x00000006
+
+ lss_set_node 12
+ lss_store
+ lss_switch_glob 0
+

    To increase scanning speed, you can use 

    _lss_fastscan 25
+

    where 25 is the scan step delay in ms. Be aware that the scan will become unreliable when the delay is set to low.

    +

    We won't configure this node now, reset LSS. Now we have 1+3 nodes operational in our example. 

    lss_switch_glob 0
+
    • +
+

Auto enumerate all nodes

+
    +
  • Auto enumerate all nodes via LSS fastscan. Enumeration automatically begins at node ID 2 and node ID is automatically stored to eeprom. Like with _lss_fastscan, an optional parameter can be used to change default delay time. 
     lss_allnodes
+
+ # Node-ID 2 assigned to: 0x00000001 0x00000002 0x00000003 0x00000007
+ # Node-ID 3 assigned to: 0x00000001 0x00000002 0x00000003 0x00000008
+ # Found 2 nodes, search finished.
+ [0] OK
+
    • +

  • To get further control over the fastscan process, the lss_allnodes command supports an extended parameter set. Auto enumerate all nodes via LSS fastscan. Set delay time to 25ms, set enumeration start to node ID 7, do not store LSS address in eeprom, enumerate only devices with vendor ID "0x428", ignore product code and software revision, scan for serial number 

     lss_allnodes 25 7 0 2 0x428 1 0 1 0 0 0
+

    The parameters are as following:

      +
    • 25 scan step delay time in ms
      • +
    • 7 enumeration start
      • +
    • 0 store node IDs to eeprom; 0 = no, 1 = yes
      • +
    • 2 vendor ID scan selector; 0 = fastscan, 2 = match value in next parameter
      • +
    • 0x428 vendor ID to match
      • +
    • 1 product code scan selector; 0 = fastscan, 1 = ignore, 2 = match value in next parameter
      • +
    • 0 product code to match (ignored in this example)
      • +
    • 1 software version scan selector; 0 = fastscan, 1 = ignore, 2 = match value in next parameter
      • +
    • 0 software version to match (ignored in this example)
      • +
    • 0 serial number scan selector; 0 = fastscan, 1 = ignore, 2 = match value in next parameter
      • +
    • 0 serial number to match (not used in this example)
      • +
    +
    • +
+

Note that only unconfigured nodes (those without a valid node ID) will take part in fastscan!

+
+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/md_doc_deviceSupport.html b/Devices/Libraries/Systems/CANopenSocket/docs/md_doc_deviceSupport.html new file mode 100755 index 0000000..20216cf --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/md_doc_deviceSupport.html @@ -0,0 +1,210 @@ + + + + + + + +CANopenNode: Device Support + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+
Device Support
+
+
+

CANopenNode can run on many different devices. There are possible many different implementations on many different hardware, with many different development tools, by many different developers. It is not possible for single project maintainer to keep all the hardware interfaces updated. For that reason all hardware specific files are not part of the CANopenNode project.

+

It is necessary to implement interface to specific hardware. Interface to Linux socketCAN is part of this projects. Interfaces to other controllers are separate projects. There are interfaces to: Zephyr RTOS, PIC, Mbed-os RTOS + STM32, NXP, etc.

+

Note for device driver contributors

+

Most up-to-date implementations of CANopenNode are: socketCAN for Linux, which is part of CANopenNode and CANopenPIC for PIC32 microcontroller (bare-metal). Those can be used for reference. There is also an example directory, which doesn't include specific device interface. It should compile on any system and can be used as a template. Device interface is documented in common CO_driver.h file.

+

There are many advantages of sharing the base code such as this. For the driver developers, who wish to share and cooperate, I recommend the following approach:

    +
  1. Make own git repo for the Device specific demo project on the Github or somewhere.
    2. +
  2. Add https://github.com/CANopenNode/CANopenNode into your project (or at side of your project). For example, include it in your project as a git submodule: git submodule add https://github.com/CANopenNode/CANopenNode
    3. +
  3. Add specific driver and other files.
    4. +
  4. Add description of new device into this file (deviceSupport.md) and make a pull request to CANopenNode. Alternatively create an issue for new device on https://github.com/CANopenNode/CANopenNode/issues.
    5. +
  5. Make a demo folder, which contains project files, etc., necessary to run the demo.
    6. +
  6. Write a good README.md file, where you describe your project, specify demo board, tools used, etc.
    7. +
+

Linux

+
    +
  • CANopenNode integration with Linux socketCAN with master command interface. SocketCAN is part of the Linux kernel.
    • +
  • https://github.com/CANopenNode/CANopenNode (this project).
    • +
  • CANopenNode version: (will be v2.0)
    • +
  • Status: stable
    • +
  • Features: OD storage, error counters, master (SDO client, LSS master, NMT master)
    • +
  • Systems: Linux PC, Raspberry PI, etc.
    • +
  • Development tools: Linux
    • +
  • Information updated 2020-02-14
    • +
+

PIC32, dsPIC30, dsPIC33

+
    +
  • CANopenNode integration with 16 and 32 bit PIC microcontrollers from Microchip.
    • +
  • https://github.com/CANopenNode/CANopenPIC
    • +
  • CANopenNode version: (near v2.0)
    • +
  • Status: stable
    • +
  • Features: OD storage, SDO client demo for PIC32, error counters
    • +
  • Development tools: MPLAB X
    • +
  • Demo hardware: Explorer 16 from Microchip
    • +
  • Information updated 2020-02-14
    • +
+

Zephyr RTOS

+ +

Mbed-os RTOS + STM32 (F091RC, L496ZG)

+ +

Kinetis K20 (NXP Arm)

+ +

S32DS (NXP S32 Design studio for Arm or Powerpc)

+ +

Other

+ +

Other old versions

+ +
+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/md_doc_gettingStarted.html b/Devices/Libraries/Systems/CANopenSocket/docs/md_doc_gettingStarted.html new file mode 100755 index 0000000..ffc2aa8 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/md_doc_gettingStarted.html @@ -0,0 +1,210 @@ + + + + + + + +CANopenNode: Getting Started + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+
Getting Started
+
+
+

CANopen

+

Before getting started with CANopenNode you should be familiar with the CANopen. CANopen is the internationally standardized CAN-based higher-layer protocol for embedded control system. It is specified by CiA301 (or by EN 50325-4) standard. It can be freely downloaded from https://can-cia.org/groups/specifications/. Some information about CAN and CANopen can be found on https://can-cia.org/can-knowledge/ website. Very efficient way to get familiar with CANopen is by reading a book, for example Embedded Networking with CAN and CANopen.

+

CANopen itself is not a typical master/slave protocol. It is more like producer/consumer protocol. It is also possible to operate CANopen network without a master. For example, pre-configured process data objects (PDO) are transmitted from producers. Each PDO may be consumed by multiple nodes. Other useful CANopen functionalities of each CANopen device are also: Heartbeat producer and consumer, Emergency producer, Sync producer or consumer, Time producer or consumer, SDO server (Service data objects - serve variables from Object dictionary), NMT slave (network management - start or stop parts of communication), LSS slave (configuration of Node-Id and Bitrate).

+

CANopen network usually has one device with master functionalities for network configuration. It may have additional CANopen functionalities, such as: NMT master, LSS master, SDO client, Emergency consumer. Master functionalities in CANopenNode are implemented with Ascii command line interface according to standard CiA309-3.

+

CANopenNode on Linux

+

CANopenNode should run on any Linux machine. Examples below was tested on Debian based machines, including Ubuntu and Raspberry PI. It is possible to run tests described below without real CAN interface, because Linux kernel already contains virtual CAN interface. All necessary Linux specific files are included in socketCAN directory of CANopenNode and Makefile is included in base directory.

+

Windows or Mac users, who don't have Linux installed, can use VirtualBox and install Ubuntu or similar.

+

Preparation

+

We will use Linux command line interface (Terminal) for all examples below. Open the terminal and cd to your working directory. First install supporting packages: can-utils, which is very useful tool for working with CAN interface and git, which is recommended for working with repositories. Then clone CANopenNode from Github and build the executable program. 

sudo apt-get install git
+sudo apt-get install can-utils
+git clone https://github.com/CANopenNode/CANopenNode.git
+cd CANopenNode
+# For update just use 'git pull' here
+make
+

Now prepare CAN virtual device and run candump, which will show all CAN traffic. Use a second terminal: 

sudo modprobe vcan
+sudo ip link add dev vcan0 type vcan
+sudo ip link set up vcan0
+candump vcan0
+

First CANopen device

+

Go to the first terminal, where we have recently build executable, named canopend. First print help, then run the program with some options. 

./canopend --help
+./canopend vcan0 -i 4 -s od4_storage -a od4_storage_auto
+

You are now running a fully functional CANopen device on virtual CAN network. It is running in background until you terminate the process (with CTRL+C for example) or it receives a reset message from CAN network. By default process also shows some info messages on terminal, for example changes of NMT state or emergency messages, own and remote.

+

On the second terminal you can see some CAN traffic. After canopend startup, first messages are: 

vcan0  704   [1]  00                        # Boot-up message.
+vcan0  084   [8]  00 50 01 2F F3 FF FF FF   # Emergency message.
+

Boot-up message of node 4 have CAN-ID equal to 0x704. CAN-ID is 11-bit standard CAN identifier.

+

Also, both, first and second terminal shows, that there is an Emergency message after the boot-up. Also Heartbeat messages shows NMT pre-operational state.

+

The easiest way to find the reason of the emergency message is to check the byte 4 (errorBit). It has value of 0x2F. Go to CANopenNode source code and open the file "301/CO_Emergency.h", section "Error status bits". 0x2F means "CO_EM_NON_VOLATILE_MEMORY", which is generic, critical error with access to non volatile device memory.

+

This byte is CANopenNode specific. You can observe also first two bytes, which shows standard error code (0x5000 - Device Hardware) or third byte, which shows error register. If error register is different than zero, then node may be prohibited to enter operational and PDOs can not be exchanged with it.

+

You can follow the reason of the problem inside the source code. However, there are missing non-default storage files. Go to the first terminal, terminate the application with CTRL+C, add files and run canopend again. 

echo "-" > od4_storage
+echo "-" > od4_storage_auto
+./canopend vcan0 -i 4 -s od4_storage -a od4_storage_auto
+

Second terminal now shows new boot-up message without emergency. 

vcan0  704   [1]  00
+

Second CANopen device

+

Open the third terminal and cd to the same directory as is in the first terminal. First generate default storage files. Then start second instance of canopend with NodeID = 1. Use default od_storage files and enable command interface on standard IO (terminal). 

echo "-" > od_storage
+echo "-" > od_storage_auto
+./canopend vcan0 -i1 -c "stdio"
+

Now you should see in second terminal (candump) boot-up message of new CANopen device.

+

CANopen command interface

+

Second instance of canopend was started with command interface enabled. This is CANopen gateway interface with ascii mapping, as specified in standard CiA309-3. This enables usage of CANopen master functionalities via basic terminal. Go to third terminal, type "help" and press enter to see its functionalities. 

help
+help datatype
+

SDO client

+

For example read Heartbeat producer parameter on CANopen device with ID=4. Parameter is located at index 0x1017, subindex 0, it is 16-bit unsigned integer. 

[1] 4 read 0x1017 0 u16
+

You should see the response, which says that Heartbeats are disabled: 

[1] 0
+

In CAN dump you can see some SDO communication. SDO communication can be quite complicated, if observing on candump, especially if larger data is split between multiple segments. However, there is no need to know the details, everything should work correctly in the background. Details about SDO communication can be found in CiA301 standard and partly also in 301/CO_SDOserver.h file, description of CO_SDO_state_t enumerator. 

[2] 4 write 0x1017 0 u16 5000
+[2] OK      #response
+

In candump you will notice, that heartbeats from node 4 are coming in 5 second interval now. Heartbeat message is similar to boot-up message. 0x7F in heartbeat message means, that node is in NMT pre-operational state. 0x05 means operational and 0x04 means stopped.

+

You can do the same also for node 1, but you won't see SDO messages on candump, because data are written into itself directly. In "stdio" you can omit sequence number, to make typing easier. 

1 w 0x1017 0 u16 2500
+[0] OK
+

Now store Object dictionary on node-ID 4, so it will preserve variables on next start of the program. 

4 w 0x1010 1 u32 0x65766173
+[0] OK
+

More details about Object dictionary variables can be found in CiA301 standard or in example/DS301_profile.md file.

+

NMT master

+

If node is operational (started), it can exchange all objects, including PDO, SDO, etc. In NMT pre-operational, PDOs are disabled, SDOs works. In stopped only NMT messages are accepted. Try following commands and observe candump. 

set node 4
+[0] OK
+
+start
+[0] OK
+
+preop
+[0] OK
+
+stop
+[0] OK
+
+r 0x1017 0 i16
+[0] ERROR: 0x05040000 #SDO protocol timed out.
+
+reset communication
+[0] OK
+
+0 start
+[0] OK
+
+reset node
+[0] OK
+
+1 reset communication
+[0] OK
+
+1 reset node
+[0] OK
+

Other communication channels

+

We used simple stdio for command interface. In Linux also sockets can be used, either local or tcp. See ./canopend --help for options. Simple Linux tool for establishing socket connection is netcat or nc. For example nc -U /tmp/CO_command_socket for local socket or nc <host> 60000 for tcp socket. There are also some tools from CANopenSocket project.

+

Please be careful when exposing your CANopen network to the outside world, it is your responsibility, if something dangerous happen.

+

Next steps

+

Assigning Node-ID or CAN bitrate, which support LSS configuration, is described in LSSusage.md.

+

Further CANopenNode related tools and examples are available in CANopenSocket. Especially interesting is basicDevice

+

Custom CANopen device can be created based on own Object Dictionary, see README.md. There are also many very useful and high quality specifications for different device profiles, some of them are public and free to download, for example CiA401.

+

For own CANopen device with own microcontroller, see deviceSupport.md. There is a bare-metal demo for PIC microcontrollers, most complete example is for PIC32.

+

Another interesting tool is CANopen for Python.

+

Examples here worked in virtual CAN interface, for simplicity. Virtual CAN runs inside Linux kernel only, it does not have much practical usability. If one has real CAN network configuration, then above examples are suitable also for this network, if Linux machine is connected to it and CAN interface is properly configured. When connecting your devices to real CAN network, make sure, you have at least two devices communicating, connected with ground and pair of wires, terminated with two 120ohm resistors, correct baudrate, etc.

+

Accessing real CANopen devices is the same as described above for virtual CAN interface. Some tested USB to CAN interfaces, which are native in Linux kernel are:

    +
  • Simple serial USBtin - Start with: sudo slcand -f -o -c -s8 /dev/ttyACM0 can0; sudo ip link set up can0
    • +
  • EMS CPC-USB or PCAN-USB FD - Start with: sudo ip link set up can0 type can bitrate 250000
    • +
  • You can get the idea of other supported CAN interfaces in Linux kernel source (Kconfig files).
    • +
  • Raspberry PI or similar has CAN capes available.
    • +
+

Examples here run in Linux, for simplicity. However, real usability of CANopen network is, when simple, microcontroller based devices are connected together with or without more advanced commander device. CANopenNode is basically written for simple microcontrollers and also has more advanced commander features, like above used CANopen gateway with ascii command interface.

+

Now you can enter the big world of CANopen devices.

+

Here we played with virtual CAN interface and result shown as pixels on screen. If you connect a real CAN interface to your computer, things may become dangerous. Keep control and safety on your machines!

+
+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/md_doc_objectDictionary.html b/Devices/Libraries/Systems/CANopenSocket/docs/md_doc_objectDictionary.html new file mode 100755 index 0000000..645cd7f --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/md_doc_objectDictionary.html @@ -0,0 +1,691 @@ + + + + + + + +CANopenNode: Object Dictionary + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+
Object Dictionary
+
+
+

+Definitions from CiA 301

+

The Object Dictionary is a collection of all the data items which have an influence on the behavior of the application objects, the communication objects and the state machine used on this device. It serves as an interface between the communication and the application. The object dictionary is essentially a grouping of objects accessible via the network in an ordered pre-defined fashion. Each object within the object dictionary is addressed using a 16-bit index and a 8-bit sub-index.

+

A SDO (Service Data Object) is providing direct access to object entries of a CANopen device's object dictionary. As these object entries may contain data of arbitrary size and data type. SDOs may be used to transfer multiple data sets (each containing an arbitrary large block of data) from a client to a server and vice versa. The client shall control via a multiplexer (index and sub-index of the object dictionary) which data set shall be transferred. The content of the data set is defined within the object dictionary.

+

A PDO (Process Data Object) is providing real-time data transfer of object entries of a CANopen device's object dictionary. The transfer of PDO is performed with no protocol overhead. The PDO correspond to objects in the object dictionary and provide the interface to the application objects. Data type and mapping of application objects into a PDO is determined by a corresponding PDO mapping structure within the object dictionary.

+

+Operation

+

Terms

+

The term OD object means object from object dictionary located at specific 16-bit index. There are different types of OD objects in CANopen: variables, arrays and records (structures). Each OD object contains pointer to actual data, data length(s) and attribute(s). See OD_objectTypes_t.

+

The term OD variable is basic variable of specified type. For example: int8_t, uint32_t, float64_t, ... or just sequence of binary data with known or unknown data length. Each OD variable resides in Object dictionary at specified 16-bit index and 8-bit sub-index.

+

The term OD entry means structure element, which contains some basic properties of the OD object, indication of type of OD object and pointer to all necessary data for the OD object. An array of OD entries together with information about total number of OD entries represents object dictionary as defined inside CANopenNode. See OD_entry_t and OD_t.

+

Access

+

Application and the stack have access to OD objects via universal OD_t object and OD_find() function. No direct access to custom structures, which define object dictionary, is required. Properties for specific OD variable is fetched with OD_getSub() function. Access to actual variable is via read and write functions. Pointer to those two functions is fetched by OD_getSub(). See OD_stream_t and OD_subEntry_t. See also shortcuts: Getters and setters, for access to data of different type.

+

Optional extensions

+

There are some optional extensions to the object dictionary:

    +
  • PDO flags informs application, if specific OD variable was received or sent by PDO. And also gives the application ability to request a TPDO, to which variable is possibly mapped.
    • +
  • IO extension gives the application ability to take full control over the OD object. Application can specify own read and write functions and own object, on which they operate.
    • +
+

Example usage

+
extern const OD_t ODxyz;
+
+
void myFunc(const OD_t *od) {
+
ODR_t ret;
+
const OD_entry_t *entry;
+
OD_subEntry_t subEntry;
+
OD_IO_t io1008;
+
char buf[50];
+
OD_size_t bytesRd;
+
int error = 0;
+
+
/* Init IO for "Manufacturer device name" at index 0x1008, sub-index 0x00 */
+
entry = OD_find(od, 0x1008);
+
ret = OD_getSub(entry, 0x00, &subEntry, &io1008, false);
+
/* Read with io1008, subindex = 0x00 */
+
if (ret == ODR_OK)
+
bytesRd = io1008.read(&io1008.stream, 0x00, &buf[0], sizeof(buf), &ret);
+
if (ret != ODR_OK) error++;
+
+
/* Use helper and set "Producer heartbeat time" at index 0x1017, sub 0x00 */
+
ret = OD_set_u16(OD_find(od, 0x1017), 0x00, 500, false);
+
if (ret != ODR_OK) error++;
+
}
+

There is no need to include ODxyt.h file, it is only necessary to know, we have ODxyz defined somewhere.

+

Second example is simpler and use helper function to access OD variable. However it is not very efficient, because it goes through all search procedures.

+

If access to the same variable is very frequent, it is better to use first example. After initialization, application has to remember only "io1008" object. Frequent reading of the variable is then very efficient.

+

Simple access to OD via globals

+

Some simple user applications can also access some OD variables directly via globals.

+
Warning
If OD object has IO extension enabled, then direct access to its OD variables must not be used. Only valid access is via read or write or helper functions.
+
#include ODxyz.h
+
+
void myFuncGlob(void) {
+
//Direct address instead of OD_find()
+
const OD_entry_t *entry_errReg = ODxyz_1001_errorRegister;
+
+
//Direct access to OD variable
+
uint32_t devType = ODxyz_0.x1000_deviceType;
+
ODxyz_0.x1018_identity.serialNumber = 0x12345678;
+
}
+

+Object Dictionary Example

+

Actual Object dictionary for one CANopen device is defined by pair of OD_xyz.h and ODxyz.h files.

+

Suffix "xyz" is unique name of the object dictionary. If single default object dictionary is used, suffix is omitted. Such way configuration with multiple object dictionaries is possible.

+

Data for OD definition are arranged inside multiple structures. Structures are different for different configuration of OD. Data objects, created with those structures, are constant or are variable.

+

Actual OD variables are arranged inside multiple structures, so called storage groups. Selected groups can optionally be stored to non-volatile memory.

+
Warning
Manual editing of ODxyz.h/.c files is very error-prone.
+

Pair of ODxyz.h/.c files can be generated by OD editor tool. The tool can edit standard CANopen device description file in xml format. Xml file may include also some non-standard elements, specific to CANopenNode. Xml file is then used for automatic generation of ODxyz.h/.c files.

+

Example ODxyz.h file

+
/* OD data declaration of all groups ******************************************/
+
typedef struct {
+
uint32_t x1000_deviceType;
+
struct {
+
uint8_t maxSubIndex;
+
uint32_t vendorID;
+
uint32_t productCode;
+
uint32_t revisionNumber;
+
uint32_t serialNumber;
+
} x1018_identity;
+
} ODxyz_PERSIST_COMM_t;
+
+
typedef struct {
+
uint8_t x1001_errorRegister;
+
uint8_t x1003_preDefinedErrorField_sub0;
+
uint32_t x1003_preDefinedErrorField[8];
+
} ODxyz_RAM_t;
+
+
extern ODxyz_PERSIST_COMM_t ODxyz_PERSIST_COMM;
+
extern ODxyz_RAM_t ODxyz_RAM;
+
extern const OD_t ODxyz;
+
+
/* Object dictionary entries - shortcuts **************************************/
+
#define ODxyz_ENTRY_H1000 &ODxyz.list[0]
+
#define ODxyz_ENTRY_H1001 &ODxyz.list[1]
+
#define ODxyz_ENTRY_H1003 &ODxyz.list[2]
+
#define ODxyz_ENTRY_H1018 &ODxyz.list[3]
+
+
#define ODxyz_ENTRY_H1000_deviceType &ODxyz.list[0]
+
#define ODxyz_ENTRY_H1001_errorRegister &ODxyz.list[1]
+
#define ODxyz_ENTRY_H1003_preDefinedErrorField &ODxyz.list[2]
+
#define ODxyz_ENTRY_H1018_identity &ODxyz.list[3]
+

Example ODxyz.c file

+
#define OD_DEFINITION
+
#include "301/CO_ODinterface.h"
+
#include "ODxyz.h"
+
+
/* OD data initialization of all groups ***************************************/
+
ODxyz_PERSIST_COMM_t ODxyz_PERSIST_COMM = {
+
.x1000_deviceType = 0L,
+
.x1018_identity = {
+
.maxSubIndex = 4,
+
.vendorID = 0L,
+
.productCode = 0L,
+
.revisionNumber = 0L,
+
.serialNumber = 0L
+
}
+
};
+
+
ODxyz_RAM_t ODxyz_RAM = {
+
.x1001_errorRegister = 0,
+
.x1003_preDefinedErrorField_sub0 = 0,
+
.x1003_preDefinedErrorField = {0, 0, 0, 0, 0, 0, 0, 0}
+
};
+
+
/* IO extensions and flagsPDO (configurable by application) *******************/
+
typedef struct {
+
OD_extensionIO_t xio_1003_preDefinedErrorField;
+
} ODxyzExts_t;
+
+
static ODxyzExts_t ODxyzExts = {0};
+
+
/* All OD objects (constant) **************************************************/
+
typedef struct {
+
OD_obj_var_t o_1000_deviceType;
+
OD_obj_var_t o_1001_errorRegister;
+
OD_obj_array_t o_1003_preDefinedErrorField;
+
OD_obj_extended_t oE_1003_preDefinedErrorField;
+
OD_obj_record_t o_1018_identity[5];
+
} ODxyzObjs_t;
+
+
static const ODxyzObjs_t ODxyzObjs = {
+
.o_1000_deviceType = {
+
.data = &ODxyz_PERSIST_COMM.x1000_deviceType,
+
.attribute = ODA_SDO_R | ODA_MB,
+
.dataLength = 4
+
},
+
.o_1001_errorRegister = {
+
.data = &ODxyz_RAM.x1001_errorRegister,
+
.attribute = ODA_SDO_R,
+
.dataLength = 1
+
},
+
.o_1003_preDefinedErrorField = {
+
.data0 = &ODxyz_RAM.x1003_preDefinedErrorField_sub0,
+
.data = &ODxyz_RAM.x1003_preDefinedErrorField[0],
+
.attribute0 = ODA_SDO_RW,
+
.attribute = ODA_SDO_R | ODA_MB,
+
.dataElementLength = 4,
+
.dataElementSizeof = sizeof(uint32_t)
+
},
+
.oE_1003_preDefinedErrorField = {
+
.extIO = &ODxyzExts.xio_1003_preDefinedErrorField,
+
.flagsPDO = NULL,
+
.odObjectOriginal = &ODxyzObjs.o_1003_preDefinedErrorField
+
},
+
.o_1018_identity = {
+
{
+
.data = &ODxyz_PERSIST_COMM.x1018_identity.maxSubIndex,
+
.subIndex = 0,
+
.attribute = ODA_SDO_R,
+
.dataLength = 1
+
},
+
{
+
.data = &ODxyz_PERSIST_COMM.x1018_identity.vendorID,
+
.subIndex = 1,
+
.attribute = ODA_SDO_R | ODA_MB,
+
.dataLength = 4
+
},
+
{
+
.data = &ODxyz_PERSIST_COMM.x1018_identity.productCode,
+
.subIndex = 2,
+
.attribute = ODA_SDO_R | ODA_MB,
+
.dataLength = 4
+
},
+
{
+
.data = &ODxyz_PERSIST_COMM.x1018_identity.revisionNumber,
+
.subIndex = 3,
+
.attribute = ODA_SDO_R | ODA_MB,
+
.dataLength = 4
+
},
+
{
+
.data = &ODxyz_PERSIST_COMM.x1018_identity.serialNumber,
+
.subIndex = 4,
+
.attribute = ODA_SDO_R | ODA_MB,
+
.dataLength = 4
+
}
+
}
+
};
+
+
/* Object dictionary **********************************************************/
+
static const OD_entry_t ODxyzList[] = {
+
{0x1000, 1, ODT_VAR, &ODxyzObjs.o_1000_deviceType},
+
{0x1001, 1, ODT_VAR, &ODxyzObjs.o_1001_errorRegister},
+
{0x1003, 9, ODT_EVAR, &ODxyzObjs.oE_1003_preDefinedErrorField},
+
{0x1018, 5, ODT_REC, &ODxyzObjs.o_1018_identity},
+
{0x0000, 0, 0, NULL}
+
};
+
+
const OD_t ODxyz = {
+
(sizeof(ODxyzList) / sizeof(ODxyzList[0])) - 1,
+
&ODxyzList[0]
+
};
+

+XML Device Description

+

CANopen device description - XML schema definition - is specified by CiA 311 standard.

+

CiA 311 complies with standard ISO 15745-1:2005/Amd1 (Industrial automation systems and integration - Open systems application integration framework).

+

CANopen device description is basically a XML file with all the information about CANopen device. The larges part of the file is a list of all object dictionary variables with all necessary properties and documentation. This file can be edited with OD editor application and can be used as data source, from which Object dictionary for CANopenNode is generated. Furthermore, this file can be used with CANopen configuration tool, which interacts with CANopen devices on running CANopen network.

+

XML schema definitions are available at: http://www.canopen.org/xml/1.1 One of the tools for viewing XML schemas and validating XDD project files is "xsddiagram" (https://github.com/dgis/xsddiagram). Command line alternative to XDD file validation against schema is: xmlstarlet val --err --xsd 311/CANopen.xsd project_file.xdd

+

CANopen specifies also another type of files for CANopen device description. These are EDS files, which are in INI format. It is possible to convert between those two formats. But CANopenNode uses XML format.

+

The device description file has "XDD" file extension. The name of this file shall contain the vendor-ID of the CANopen device in the form of 8 hexadecimal digits in any position of the name and separated with underscores. For example "name1_12345678_name2.XDD".

+

CANopenNode includes multiple profile definition files, one for each CANopen object. Those files have "XPD" extension. They are in the same XML format as XDD files. The XML editor tool can use XPD files to insert prepared data into device description file (XDD), which is being edited.

+

There are also device configuration files with "XDC" extension. They are describing a configured CANopen device and include additional elements, such as default value, denominator and device commissioning elements. Similar as "dcf" files in INI format.

+

XDD, XPD file example

+
<?xml version="1.0" encoding="utf-8"?>
+
<ISO15745ProfileContainer xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xmlns="http://www.canopen.org/xml/1.1">
+
<ISO15745Profile>
+
<ProfileHeader>...</ProfileHeader>
+
<ProfileBody xsi:type="ProfileBody_Device_CANopen" fileName="..." fileCreator="..." fileCreationDate="..." fileVersion="...">
+
<DeviceIdentity>...</DeviceIdentity>
+
<DeviceFunction>...</DeviceFunction>
+
<ApplicationProcess>
+
<dataTypeList>...</dataTypeList>
+
<parameterList>
+
<parameter uniqueID="UID_PARAM_1000" access="read">
+
<label lang="en">Device type</label>
+
<description lang="en">...</description>
+
<denotation>...</denotation>
+
<UINT/>
+
<defaultValue value="0x00000000" />
+
</parameter>
+
<parameter uniqueID="UID_PARAM_1001" access="read">
+
<label lang="en">Error register</label>
+
<description lang="en">...</description>
+
<denotation>...</denotation>
+
<BYTE/>
+
<defaultValue value="0" />
+
<property name="CO_storageGroup" value="RAM">
+
<property name="CO_extensionIO" value="true">
+
</parameter>
+
<parameter uniqueID="UID_PARAM_1018">
+
<label lang="en">Identity</label>
+
<description lang="en">...</description>
+
<denotation>...</denotation>
+
<dataTypeIDRef uniqueIDRef="..." />
+
</parameter>
+
<parameter uniqueID="UID_PARAM_101800" access="read">
+
<label lang="en">max sub-index</label>
+
<description lang="en">...</description>
+
<denotation>...</denotation>
+
<USINT/>
+
<defaultValue value="4" />
+
</parameter>
+
<parameter uniqueID="UID_PARAM_101801" access="read">
+
<label lang="en">Vendor-ID</label>
+
<description lang="en">...</description>
+
<denotation>...</denotation>
+
<UINT/>
+
<defaultValue value="0x00000000" />
+
</parameter>
+
<parameter uniqueID="UID_PARAM_101802" access="read">
+
<label lang="en">Product code</label>
+
<description lang="en">...</description>
+
<denotation>...</denotation>
+
<UINT/>
+
<defaultValue value="0x00000000" />
+
</parameter>
+
<parameter uniqueID="UID_PARAM_101803" access="read">
+
<label lang="en">Revision number</label>
+
<description lang="en">...</description>
+
<denotation>...</denotation>
+
<UINT/>
+
<defaultValue value="0x00000000" />
+
</parameter>
+
<parameter uniqueID="UID_PARAM_101804" access="read">
+
<label lang="en">Serial number</label>
+
<description lang="en">...</description>
+
<denotation>...</denotation>
+
<UINT/>
+
<defaultValue value="0x00000000" />
+
</parameter>
+
</parameterList>
+
</ApplicationProcess>
+
</ProfileBody>
+
</ISO15745Profile>
+
<ISO15745Profile>
+
<ProfileHeader>...</ProfileHeader>
+
<ProfileBody xsi:type="ProfileBody_CommunicationNetwork_CANopen" fileName="..." fileCreator="..." fileCreationDate="..." fileVersion="...">
+
<ApplicationLayers>
+
<CANopenObjectList>
+
<CANopenObject index="1000" name="Device type" objectType="7" PDOmapping="no" uniqueIDRef="UID_PARAM_1000" />
+
<CANopenObject index="1001" name="Error register" objectType="7" PDOmapping="TPDO" uniqueIDRef="UID_PARAM_1001" />
+
<CANopenObject index="1018" name="Identity" objectType="9" subNumber="5" uniqueIDRef="UID_PARAM_1018">
+
<CANopenSubObject subIndex="00" name="max sub-index" objectType="7" PDOmapping="no" uniqueIDRef="UID_PARAM_101800" />
+
<CANopenSubObject subIndex="01" name="Vendor-ID" objectType="7" PDOmapping="no" uniqueIDRef="UID_PARAM_101801" />
+
<CANopenSubObject subIndex="02" name="Product code" objectType="7" PDOmapping="no" uniqueIDRef="UID_PARAM_101802" />
+
<CANopenSubObject subIndex="03" name="Revision number" objectType="7" PDOmapping="no" uniqueIDRef="UID_PARAM_101803" />
+
<CANopenSubObject subIndex="04" name="Serial number" objectType="7" PDOmapping="no" uniqueIDRef="UID_PARAM_101804" />
+
</CANopenObject>
+
</CANopenObjectList>
+
</ApplicationLayers>
+
<TransportLayers>...</TransportLayers>
+
</ProfileBody>
+
</ISO15745Profile>
+
</ISO15745ProfileContainer>
+

Parameter description

+

Above XML file example shows necessary data for OD interface used by CANopenNode and other parameters required by the standard. Standard specifies many other parameters, which are not used by CANopenNode for simplicity.

+

XML file is divided into two parts:

    +
  1. "ProfileBody_Device_CANopen" - more standardized information
    2. +
  2. "ProfileBody_CommunicationNetwork_CANopen" - CANopen specific information
    3. +
+

Largest part of the XML file is definition of each OD object. All OD objects are listed in "CANopenObjectList", which resides in the second part of the XML file. Each "CANopenObject" and "CANopenSubObject" of the list contains a link to parameter ("uniqueIDRef"), which resides in the first part of the XML file. So data for each OD object is split between two parts of the XML file.

+

<CANopenObject>

+
    +
  • "index" (required) - Object dictionary index
    • +
  • "name" (required) - Name of the parameter
    • +
  • "objectType" (required) - "7"=VAR, "8"=ARRAY, "9"=RECORD
    • +
  • "subNumber" (required if "objectType" is "8" or "9")
    • +
  • "PDOmapping" (optional if "objectType" is "7"):
      +
    • "no" - mapping not allowed
      • +
    • "default" - not used, same as "optional"
      • +
    • "optional" - mapping allowed to TPDO or RPDO
      • +
    • "TPDO" - mapping allowed to TPDO
      • +
    • "RPDO" mapping allowed to RPDO
      • +
    +
    • +
  • "uniqueIDRef" (required or see below) - Reference to <parameter>
    • +
+

<CANopenSubObject>

+
    +
  • "subIndex" (required) - Object dictionary sub-index
    • +
  • "name" (required) - Name of the parameter
    • +
  • "objectType" (required, always "7")
    • +
  • "PDOmapping" (optional, same as above)
    • +
  • "uniqueIDRef" (required or see below) - Reference to <parameter>
    • +
+

uniqueIDRef

+

This is required attribute from "CANopenObject" and "CANopenSubObject". It contains reference to <parameter> in "ProfileBody_Device_CANopen" section of the XML file.

+

If "uniqueIDRef" attribute is not specified and "objectType" is 7(VAR), then "CANopenObject" or "CANopenSubObject" must contain additional attributes:

    +
  • "dataType" (required for VAR) - CANopen basic data type, see below
    • +
  • "accessType" (required for VAR) - "ro", "wo", "rw" or "const"
    • +
  • "defaultValue" (optional) - Default value for the variable.
    • +
+

<parameter>

+
    +
  • "uniqueID" (required)
    • +
  • "access" (required for VAR) - can be one of:
      +
    • "const" - same as "read"
      • +
    • "read" - only read access with SDO or PDO
      • +
    • "write" - only write access with SDO or PDO
      • +
    • "readWrite" - read or write access with SDO or PDO
      • +
    • "readWriteInput" - same as "readWrite"
      • +
    • "readWriteOutput" - same as "readWrite"
      • +
    • "noAccess" - object will be in object dictionary, but no access.
      • +
    +
    • +
  • <label lang="en">, <description lang="en"> (required) - several elements in multiple languages possible
    • +
  • <INT and similar/> (required) - Basic or complex data type. Basic data type (for VAR) is specified in IEC 61131-3 (see below). If data type is complex (ARRAY or RECORD), then <dataTypeIDRef> must be specified and entry must be added in the <dataTypeList>. Such definition of complex data types is required by the standard, but it is not required by CANopenNode.
    • +
  • <defaultValue> (optional for VAR) - Default value for the variable. If it is empty string, then data is not stored inside object dictionary. Application should provide own data via IO extension.
    • +
  • <property name="..." value="..."> (optional) - Multiple elements may contain additional custom properties. CANopenNode uses custom properties with following names:
      +
    • "CO_disabled" (used for base OD entry) - Valid value is "false" (default) or "true". if OD entry is disabled, then it will not be present in generated Object Dictionary .h and .c files.
      • +
    • "CO_countLabel" (used for base OD entry) - Valid value is any string without spaces. OD exporter will generate a macro for each different string. For example, if four OD objects have "CO_countLabel" set to "TPDO", then macro "#define ODxyz_CNT_TPDO 4" will be generated by OD exporter. "CO_countLabel" is not required for configuration with multiple object dictionaries (CO_MULTIPLE_OD), although may be useful. Default is "".
      • +
    • "CO_storageGroup" (used for base OD entry) - group name (string) into which the C variable will belong. Variables from specific storage group may then be stored into non-volatile memory, automatically or by SDO command. Default is "RAM". Please note: if <defaultValue> is empty string at specific subindex, then no storage group will be used for that variable.
      • +
    • "CO_extensionIO" (used for base OD entry) - Valid value is "false" (default) or "true", if IO extension is enabled.
      • +
    • "CO_flagsPDO" (used for base OD entry) - Valid value is "false" (default) or "true", if PDO flags are enabled.
      • +
    • "CO_accessSRDO" (used for each "VAR") - Valid values are: "tx", "rx", "trx", "no"(default).
      • +
    • "CO_stringLengthMin" (used for each "VAR") - Minimum length of the string. Used with "VISIBLE_STRING" and "UNICODE_STRING". If CO_stringLengthMin is smaller than length of the string in <defaultValue>, then it is ignored. Byte length of unicode string is 2 * CO_stringLengthMin. If <defaultValue> is empty and CO_stringLengthMin is 0, then data is not stored inside object dictionary. Default is 0.
      • +
    +
    • +
+

Other elements

+

Other elements listed in the above XML example are required by the standard. There are also many other elements, not listed above. All those does not influence the CANopenNode object dictionary exporter.

+

CANopen basic data types

+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
CANopenNode IEC 61131-3 CANopen dataType
bool_t BOOL BOOLEAN 0x01
int8_t SINT, (CHAR) INTEGER8 0x02
int16_t INT INTEGER16 0x03
int32_t DINT INTEGER32 0x04
int64_t LINT INTEGER64 0x15
uint8_t USINT, (BYTE) UNSIGNED8 0x05
uint16_t UINT, (WORD) UNSIGNED16 0x06
uint32_t UDINT, (DWORD) UNSIGNED32 0x07
uint64_t ULINT, (LWORD) UNSIGNED64 0x1B
float32_t REAL REAL32 0x08
float64_t LREAL REAL64 0x11
- (1) INTEGER24 0x10
- (1) INTEGER40 0x12
- (1) INTEGER48 0x13
- (1) INTEGER56 0x14
- (1) UNSIGNED24 0x16
- (1) UNSIGNED40 0x18
- (1) UNSIGNED48 0x19
- (1) UNSIGNED56 0x1A
char [] STRING (2) VISIBLE_STRING 0x09
uint8_t [] BITSTRING (3) OCTET_STRING 0x0A
uint16_t [] WSTRING (2) UNICODE_STRING 0x0B
- (1) TIME_OF_DAY 0x0C
- (1) TIME_DIFFERENCE 0x0D
app. specific BITSTRING (4) DOMAIN 0x0F
+

(1) Data is translated into OCTET_STRING.

+

(2) Additional property "CO_stringLengthMin" indicates the minimum length of the string in CANopenNode. Property is ignored, if strlen(defaultValue) is larger. Otherwise remaining data locations are filled with zeroes. Strings always have additional element on the end of the array with value=0, which is never written (string is always null terminated). All strings have additional attribute "ODA_STR" in object dictionary. Attribute enables SDO to transfer data of length, which corresponds to actual string length. (Actual size of data read or written by SDO may be smaller than data size of the OD variable.) VISIBLE_STRING may contain control characters and UTF-8 characters, which should work on most target systems (like printf("%s", ...)). So VISIBLE_STRING may be more usable for printing complete set of unicode characters than UNICODE_STRING.

+

(3) Default value for BITSTRING (OCTET_STRING) is written as space separated, two hex digit bytes in little-endian format.

+

(4) Default value for DOMAIN is stored as empty string.

+

+Object Dictionary Requirements By CANopenNode

+
    +
  • Used by column indicates CANopenNode object or its part, which uses the OD object. It also indicates, if OD object is required or optional for actual configuration. For the configuration of the CANopenNode objects see Stack configuration. If CANopenNode object or its part is disabled in stack configuration, then OD object is not used. Note that OD objects: 1000, 1001 and 1017 and 1018 are mandatory for CANopen.
    • +
  • CO_extensionIO column indicates, if OD object must have property "CO_extensionIO" set to true:
      +
    • "no" - no IO extension used
      • +
    • "optional" - If IO extension is enabled, then writing to the OD parameter will reflect in CANopen run time, otherwise reset communication is required for changes to take effect.
      • +
    • "required" - IO extension is required on OD object and init function will return error if not enabled.
      • +
    • "req if DYNAMIC" - IO extension is required on OD object if CO_CONFIG_FLAG_OD_DYNAMIC is set.
      • +
    • "yes, own data" - OD object don't need own data and IO extension is necessary for OD object to work. Init function will not return error, if OD object does not exist or doesn't have IO extension enabled.
      • +
    +
    • +
  • CO_countLabel column indicates, which value must have property "CO_countLabel" inside OD object.
    • +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
index Description Used by CO_extensionIO CO_countLabel
1000 Device type CANopen, req no NMT
1001 Error register CANopen, EM, req no EM
1002 Manufacturer status register
1003 Pre-defined error field EM_HISTORY, opt yes, own data
1005 COB-ID SYNC message SYNC, req req if DYNAMIC SYNC
1006 Communication cycle period SYNC_PRODUCER, req opt SYNC_PROD
1007 Synchronous window length SYNC, opt opt
1008 Manufacturer device name
1009 Manufacturer hardware version
100A Manufacturer software version
100C Guard time
100D Life time factor
1010 Store parameters
1011 Restore default parameters
1012 COB-ID time stamp object TIME, req required TIME
1013 High resolution time stamp
1014 COB-ID EMCY EM_PRODUCER, req required EM_PROD
1015 Inhibit time EMCY EM_PROD_INHIBIT, opt optional
1016 Consumer heartbeat time HB_CONS, req optional HB_CONS
1017 Producer heartbeat time CANopen, NMT, req required HB_PROD
1018 Identity object CANopen, LSS_SL, req no
1019 Synch. counter overflow value SYNC, opt no
1020 Verify configuration
1021 Store EDS
1022 Store format
1023 OS command
1024 OS command mode
1025 OS debugger interface
1026 OS prompt
1027 Module list
1028 Emergency consumer object
1029 Error behavior object
1200 SDO server parameter (first) SDO optional required SDO_SRV
1201+ SDO server parameter SDO+, req req if DYNAMIC SDO_SRV
1280+ SDO client parameter SDO_CLI, req req if DYNAMIC SDO_CLI
1300 Global fail-safe command par GFC, req GFC
1301+ SRDO communication parameter SRDO, req SRDO
1381+ SRDO mapping parameter SRDO, req
13FE Configuration valid SRDO, req
13FF Safety configuration checksum SRDO, req
1400+ RPDO communication parameter RPDO, req req if DYNAMIC RPDO
1600+ RPDO mapping parameter RPDO, req req if DYNAMIC
1800+ TPDO communication parameter TPDO, req req if DYNAMIC TPDO
1A00+ TPDO mapping parameter TPDO, req req if DYNAMIC
1FA0+ Object scanner list
1FD0+ Object dispatching list
Custom objects
any Error status bits EM_STATUS_BITS, opt yes, own data
any+ Trace TRACE, req yes, own data TRACE
+
+
+
+
uint32_t
unsigned long int uint32_t
UNSIGNED32 in CANopen (0007h), 32-bit unsigned integer.
Definition: CO_driver.h:155
+
OD_IO_t::stream
OD_stream_t stream
Object Dictionary stream object, passed to read or write.
Definition: CO_ODinterface.h:265
+
OD_extensionIO_t
Object pointed by OD_obj_extended_t contains application specified parameters for extended OD object.
Definition: CO_ODinterface.h:734
+
ODT_REC
@ ODT_REC
This type corresponds to CANopen Object Dictionary object with object code equal to RECORD.
Definition: CO_ODinterface.h:682
+
ODT_VAR
@ ODT_VAR
This type corresponds to CANopen Object Dictionary object with object code equal to VAR.
Definition: CO_ODinterface.h:669
+
OD_obj_record_t::data
void * data
Pointer to data.
Definition: CO_ODinterface.h:724
+
OD_IO_t::read
OD_size_t(* read)(OD_stream_t *stream, uint8_t subIndex, void *buf, OD_size_t count, ODR_t *returnCode)
Function pointer for reading value from specified variable from Object Dictionary.
Definition: CO_ODinterface.h:296
+
ODT_EVAR
@ ODT_EVAR
Same as ODT_VAR, but extended with OD_obj_extended_t type.
Definition: CO_ODinterface.h:686
+
ODA_MB
@ ODA_MB
Variable is multi-byte ((u)int16_t to (u)int64_t)
Definition: CO_ODinterface.h:120
+
OD_find
const OD_entry_t * OD_find(const OD_t *od, uint16_t index)
Find OD entry in Object Dictionary.
Definition: CO_ODinterface.c:160
+
OD_obj_var_t
Object for single OD variable, used for "VAR" type OD objects.
Definition: CO_ODinterface.h:701
+
OD_getSub
ODR_t OD_getSub(const OD_entry_t *entry, uint8_t subIndex, OD_subEntry_t *subEntry, OD_IO_t *io, bool_t odOrig)
Find sub-object with specified sub-index on OD entry returned by OD_find.
Definition: CO_ODinterface.c:200
+
ODA_SDO_R
@ ODA_SDO_R
SDO server may read from the variable.
Definition: CO_ODinterface.h:111
+
OD_subEntry_t
Structure describing properties of a variable, located in specific index and sub-index inside the Obj...
Definition: CO_ODinterface.h:201
+
OD_obj_array_t
Object for OD array of variables, used for "ARRAY" type OD objects.
Definition: CO_ODinterface.h:710
+
OD_obj_extended_t
Object for extended type of OD variable, configurable by OD_extensionIO_init() function.
Definition: CO_ODinterface.h:749
+
CO_ODinterface.h
CANopen Object Dictionary interface.
+
ODR_OK
@ ODR_OK
SDO abort 0x00000000 - Read/write successfully finished.
Definition: CO_ODinterface.h:139
+
ODR_t
ODR_t
Return codes from OD access functions.
Definition: CO_ODinterface.h:132
+
OD_t
Object Dictionary.
Definition: CO_ODinterface.h:352
+
OD_set_u16
ODR_t OD_set_u16(const OD_entry_t *entry, uint8_t subIndex, uint16_t val, bool_t odOrig)
Set uint16_t variable in Object Dictionary, see OD_set_i8.
Definition: CO_ODinterface.c:545
+
NULL
#define NULL
NULL, for general usage.
Definition: CO_driver.h:135
+
ODA_SDO_RW
@ ODA_SDO_RW
SDO server may read from or write to the variable.
Definition: CO_ODinterface.h:113
+
OD_IO_t
Structure for input / output on the OD variable.
Definition: CO_ODinterface.h:263
+
OD_obj_record_t
Object for OD sub-elements, used in "RECORD" type OD objects.
Definition: CO_ODinterface.h:723
+
OD_entry_t
Object Dictionary entry for one OD object.
Definition: CO_ODinterface.h:336
+
uint8_t
unsigned char uint8_t
UNSIGNED8 in CANopen (0005h), 8-bit unsigned integer.
Definition: CO_driver.h:151
+
OD_size_t
#define OD_size_t
Variable of type OD_size_t contains data length in bytes of OD variable.
Definition: CO_ODinterface.h:44
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/md_doc_traceUsage.html b/Devices/Libraries/Systems/CANopenSocket/docs/md_doc_traceUsage.html new file mode 100755 index 0000000..dc04d9c --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/md_doc_traceUsage.html @@ -0,0 +1,128 @@ + + + + + + + +CANopenNode: Trace usage + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+
Trace usage
+
+
+

CANopenNode includes optional trace functionality (non-standard). It monitors choosen variables from Object Dictionary. On change of state of variable it makes a record with timestamp into circular buffer. String with points can later be read via SDO.

+

Trace is disabled by default. It can be enabled using Object Dictionary editor. Include also CO_trace.h/.c into project, compile and run.

+

Here is en example of monitoring variable, connected with buttons (OD_readInput8Bit, index 0x6000, subindex 0x01). It was tested on PIC32:

+
# Enable trace first:
+
./canopencomm 0x30 w 0x2400 0 u8 1
+
+
# Press and hold the button on Explorer16 and execute SDO read command:
+
./canopencomm 0x30 r 0x6000 1 u8
+
[1] 0x08
+
# It displays same value, as was transmitted via PDO and visible on candump.
+
+
# Now get the complete history for that buttons with timestamp for each change
+
# and store it as a text to the file:
+
./canopencomm 0x30 r 0x2401 5 vs > plot1.csv
+
cat plot1.csv
+

If large data blocks are transmitted via CAN bus, then more efficient SDO block transfer can be enabled with command ./canopencomm set sdo_block 1

+

For more info on using trace functionality see CANopenNode/example/IO.html file. There is also a description of all Object Dictionary variables.

+

Trace functionality can also be configured on CANopenSocket directly. In that case CANopenSocket must first receive PDO data from remote node(s) and store it to the local Object Dictionary variable. CANopenSocket's trace then monitors that variable. Text buffer is then read with the similar command as above. But local SDO data access from CANopenSocket itself doesn't occupy CAN bus, so large data is transfered realy fast. Besides that, Linux machine has much more RAM to store the monitored data. Except timestamp is less accurate.

+
+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/md_example_DS301_profile.html b/Devices/Libraries/Systems/CANopenSocket/docs/md_example_DS301_profile.html new file mode 100755 index 0000000..6688048 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/md_example_DS301_profile.html @@ -0,0 +1,1227 @@ + + + + + + + +CANopenNode: CANopen documentation + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+
CANopen documentation
+
+
+

New Product

+ + + + + + + + + + + + + + + +
Project File DS301_profile.xpd
File Version 1
Created 23. 11. 2020 12:00:00
Created By
Modified 24. 11. 2020 13:31:53
Modified By
+

This file was automatically generated with libedssharp Object Dictionary Editor v0.8-99-g0425f94

+ +

+Device Information

+ + + + + + + + + + + + + + + + + + + + + +
Vendor Name
Vendor ID
Product Name New Product
Product ID
Granularity 8
RPDO count 4
TPDO count 4
LSS Slave True
LSS Master False
+

Supported Baud rates

+
    +
  • [x] 10 kBit/s
    • +
  • [x] 20 kBit/s
    • +
  • [x] 50 kBit/s
    • +
  • [x] 125 kBit/s
    • +
  • [x] 250 kBit/s
    • +
  • [x] 500 kBit/s
    • +
  • [x] 800 kBit/s
    • +
  • [x] 1000 kBit/s
    • +
  • [ ] auto
    • +
+

+PDO Mapping

+

+Communication Specific Parameters

+

0x1000 - Device type

+ + + + + +
Object Type Count Label Storage Group IO extension PDO flags
VAR NMT PERSIST_COMM False False
+ + + + + +
Data Type SDO PDO SRDO Default Value
UNSIGNED32 ro no no 0x00000000
+
    +
  • bit 16-31: Additional information
    • +
  • bit 0-15: Device profile number
    • +
+

0x1001 - Error register

+ + + + + +
Object Type Count Label Storage Group IO extension PDO flags
VAR EM RAM False False
+ + + + + +
Data Type SDO PDO SRDO Default Value
UNSIGNED8 ro tr no 0x00
+
    +
  • bit 7: manufacturer specific
    • +
  • bit 6: Reserved (always 0)
    • +
  • bit 5: device profile specific
    • +
  • bit 4: communication error (overrun, error state)
    • +
  • bit 3: temperature
    • +
  • bit 2: voltage
    • +
  • bit 1: current
    • +
  • bit 0: generic error
    • +
+

0x1003 - Pre-defined error field

+ + + + + +
Object Type Count Label Storage Group IO extension PDO flags
ARRAY RAM True False
+ + + + + + + + + + + + + + + + + + + + + +
Sub Name Data Type SDO PDO SRDO Default Value
0x00 Number of errors UNSIGNED8 rw no no 0
0x01 Standard error field UNSIGNED32 ro no no 0x00000000
0x02 Standard error field UNSIGNED32 ro no no 0x00000000
0x03 Standard error field UNSIGNED32 ro no no 0x00000000
0x04 Standard error field UNSIGNED32 ro no no 0x00000000
0x05 Standard error field UNSIGNED32 ro no no 0x00000000
0x06 Standard error field UNSIGNED32 ro no no 0x00000000
0x07 Standard error field UNSIGNED32 ro no no 0x00000000
0x08 Standard error field UNSIGNED32 ro no no 0x00000000
+
    +
  • Sub Index 0: Contains number of actual errors. 0 can be written to clear error history.
    • +
  • sub-index 1 and above:
      +
    • bit 16-31: Manufacturer specific additional information
      • +
    • bit 0-15: Error code as transmited in the Emergency object
      • +
    +
    • +
+

0x1005 - COB-ID SYNC message

+ + + + + +
Object Type Count Label Storage Group IO extension PDO flags
VAR SYNC PERSIST_COMM True False
+ + + + + +
Data Type SDO PDO SRDO Default Value
UNSIGNED32 rw no no 0x00000080
+
    +
  • bit 31: set to 0
    • +
  • bit 30: If set, CANopen device generates SYNC object
    • +
  • bit 11-29: set to 0
    • +
  • bit 0-10: 11-bit CAN-ID
    • +
+

0x1006 - Communication cycle period

+ + + + + +
Object Type Count Label Storage Group IO extension PDO flags
VAR SYNC_PROD PERSIST_COMM True False
+ + + + + +
Data Type SDO PDO SRDO Default Value
UNSIGNED32 rw no no 0
+

Period of SYNC transmission in µs (0 = transmission disabled).

+

0x1007 - Synchronous window length

+ + + + + +
Object Type Count Label Storage Group IO extension PDO flags
VAR PERSIST_COMM True False
+ + + + + +
Data Type SDO PDO SRDO Default Value
UNSIGNED32 rw no no 0
+

Synchronous window leghth in µs (0 = not used). All synchronous PDOs must be transmitted within this time window.

+

0x1010 - Store parameters

+ + + + + +
Object Type Count Label Storage Group IO extension PDO flags
ARRAY RAM True False
+ + + + + + + + + + + + + +
Sub Name Data Type SDO PDO SRDO Default Value
0x00 Highest sub-index supported UNSIGNED8 ro no no 0x04
0x01 Save all parameters UNSIGNED32 rw no no 0x00000001
0x02 Save communication parameters UNSIGNED32 rw no no 0x00000001
0x03 Save application parameters UNSIGNED32 rw no no 0x00000001
0x04 Save manufacturer defined parameters UNSIGNED32 rw no no 0x00000001
+

Sub-indexes 1 and above:

    +
  • Reading provides information about its storage functionality:
      +
    • bit 1: If set, CANopen device saves parameters autonomously
      • +
    • bit 0: If set, CANopen device saves parameters on command
      • +
    +
    • +
  • Writing value 0x65766173 ('s','a','v','e' from LSB to MSB) stores corresponding data.
    • +
+

0x1011 - Restore default parameters

+ + + + + +
Object Type Count Label Storage Group IO extension PDO flags
ARRAY RAM True False
+ + + + + + + + + + + + + +
Sub Name Data Type SDO PDO SRDO Default Value
0x00 Highest sub-index supported UNSIGNED8 ro no no 0x04
0x01 Restore all default parameters UNSIGNED32 rw no no 0x00000001
0x02 Restore communication default parameters UNSIGNED32 rw no no 0x00000001
0x03 Restore application default parameters UNSIGNED32 rw no no 0x00000001
0x04 Restore manufacturer defined default parameters UNSIGNED32 rw no no 0x00000001
+

Sub-indexes 1 and above:

    +
  • Reading provides information about its restoring capability:
      +
    • bit 0: If set, CANopen device restores parameters
      • +
    +
    • +
  • Writing value 0x64616F6C ('l','o','a','d' from LSB to MSB) restores corresponding data.
    • +
+

0x1012 - COB-ID time stamp object

+ + + + + +
Object Type Count Label Storage Group IO extension PDO flags
VAR TIME PERSIST_COMM True False
+ + + + + +
Data Type SDO PDO SRDO Default Value
UNSIGNED32 rw no no 0x00000100
+
    +
  • bit 31: If set, CANopen device consumes TIME message
    • +
  • bit 30: If set, CANopen device produces TIME message
    • +
  • bit 11-29: set to 0
    • +
  • bit 0-10: 11-bit CAN-ID
    • +
+

0x1014 - COB-ID EMCY

+ + + + + +
Object Type Count Label Storage Group IO extension PDO flags
VAR EM_PROD PERSIST_COMM True False
+ + + + + +
Data Type SDO PDO SRDO Default Value
UNSIGNED32 rw no no 0x80+$NODEID
+
    +
  • bit 31: If set, EMCY does NOT exist / is NOT valid
    • +
  • bit 11-30: set to 0
    • +
  • bit 0-10: 11-bit CAN-ID
    • +
+

0x1015 - Inhibit time EMCY

+ + + + + +
Object Type Count Label Storage Group IO extension PDO flags
VAR PERSIST_COMM True False
+ + + + + +
Data Type SDO PDO SRDO Default Value
UNSIGNED16 rw no no 0
+

Inhibit time of emergency message in multiples of 100µs. The value 0 disables the inhibit time.

+

0x1016 - Consumer heartbeat time

+ + + + + +
Object Type Count Label Storage Group IO extension PDO flags
ARRAY HB_CONS PERSIST_COMM True False
+ + + + + + + + + + + + + + + + + + + + + +
Sub Name Data Type SDO PDO SRDO Default Value
0x00 Highest sub-index supported UNSIGNED8 ro no no 0x08
0x01 Consumer heartbeat time UNSIGNED32 rw no no 0x00000000
0x02 Consumer heartbeat time UNSIGNED32 rw no no 0x00000000
0x03 Consumer heartbeat time UNSIGNED32 rw no no 0x00000000
0x04 Consumer heartbeat time UNSIGNED32 rw no no 0x00000000
0x05 Consumer heartbeat time UNSIGNED32 rw no no 0x00000000
0x06 Consumer heartbeat time UNSIGNED32 rw no no 0x00000000
0x07 Consumer heartbeat time UNSIGNED32 rw no no 0x00000000
0x08 Consumer heartbeat time UNSIGNED32 rw no no 0x00000000
+

Consumer Heartbeat Time:

    +
  • bit 24-31: set to 0
    • +
  • bit 16-23: Node ID of the monitored node. If 0 or greater than 127, sub-entry is not used.
    • +
  • bit 0-15: Heartbeat time in ms (if 0, sub-intry is not used). Value should be higher than the corresponding producer heartbeat time.
    • +
+

0x1017 - Producer heartbeat time

+ + + + + +
Object Type Count Label Storage Group IO extension PDO flags
VAR HB_PROD PERSIST_COMM True False
+ + + + + +
Data Type SDO PDO SRDO Default Value
UNSIGNED16 rw no no 0
+

Heartbeat producer time in ms (0 = disable transmission).

+

0x1018 - Identity

+ + + + + +
Object Type Count Label Storage Group IO extension PDO flags
RECORD PERSIST_COMM False False
+ + + + + + + + + + + + + +
Sub Name Data Type SDO PDO SRDO Default Value
0x00 Highest sub-index supported UNSIGNED8 ro no no 0x04
0x01 Vendor-ID UNSIGNED32 ro no no 0x00000000
0x02 Product code UNSIGNED32 ro no no 0x00000000
0x03 Revision number UNSIGNED32 ro no no 0x00000000
0x04 Serial number UNSIGNED32 ro no no 0x00000000
+
    +
  • Vendor-ID, assigned by CiA
    • +
  • Product code, manufacturer specific
    • +
  • Revision number:
      +
    • bit 16-31: Major revision number (CANopen behavior has changed)
      • +
    • bit 0-15: Minor revision num. (CANopen behavior has not changed)
      • +
    +
    • +
  • Serial number, manufacturer specific
    • +
+

0x1019 - Synchronous counter overflow value

+ + + + + +
Object Type Count Label Storage Group IO extension PDO flags
VAR PERSIST_COMM False False
+ + + + + +
Data Type SDO PDO SRDO Default Value
UNSIGNED8 rw no no 0
+
    +
  • Value 0: SYNC message is transmitted with data length 0.
    • +
  • Value 1: reserved.
    • +
  • Value 2-240: SYNC message has one data byte, which contains the counter.
    • +
  • Value 241-255: reserved.
    • +
+

0x1200 - SDO server parameter

+ + + + + +
Object Type Count Label Storage Group IO extension PDO flags
RECORD SDO_SRV RAM True False
+ + + + + + + + + +
Sub Name Data Type SDO PDO SRDO Default Value
0x00 Highest sub-index supported UNSIGNED8 ro no no 2
0x01 COB-ID client to server (rx) UNSIGNED32 ro t no 0x600+$NODEID
0x02 COB-ID server to client (tx) UNSIGNED32 ro t no 0x580+$NODEID
+

Sub-indexes 1 and 2:

    +
  • bit 11-31: set to 0
    • +
  • bit 0-10: 11-bit CAN-ID
    • +
+

0x1280 - SDO client parameter

+ + + + + +
Object Type Count Label Storage Group IO extension PDO flags
RECORD SDO_CLI PERSIST_COMM True False
+ + + + + + + + + + + +
Sub Name Data Type SDO PDO SRDO Default Value
0x00 Highest sub-index supported UNSIGNED8 ro no no 0x03
0x01 COB-ID client to server (tx) UNSIGNED32 rw tr no 0x80000000
0x02 COB-ID server to client (rx) UNSIGNED32 rw tr no 0x80000000
0x03 Node-ID of the SDO server UNSIGNED8 rw no no 0x01
+
    +
  • Sub-indexes 1 and 2:
      +
    • bit 31: If set, SDO does NOT exist / is NOT valid
      • +
    • bit 30: If set, value is assigned dynamically
      • +
    • bit 11-29: set to 0
      • +
    • bit 0-10: 11-bit CAN-ID
      • +
    +
    • +
  • Node-ID of the SDO server, 0x01 to 0x7F
    • +
+

0x1400 - RPDO communication parameter

+ + + + + +
Object Type Count Label Storage Group IO extension PDO flags
RECORD RPDO PERSIST_COMM True False
+ + + + + + + + + + + +
Sub Name Data Type SDO PDO SRDO Default Value
0x00 Highest sub-index supported UNSIGNED8 ro no no 0x05
0x01 COB-ID used by RPDO UNSIGNED32 rw no no 0x80000200+$NODEID
0x02 Transmission type UNSIGNED8 rw no no 254
0x05 Event timer UNSIGNED16 rw no no 0
+
    +
  • COB-ID used by RPDO:
      +
    • bit 31: If set, PDO does not exist / is not valid
      • +
    • bit 11-30: set to 0
      • +
    • bit 0-10: 11-bit CAN-ID
      • +
    +
    • +
  • Transmission type:
      +
    • Value 0-240: synchronous, processed after next reception of SYNC object
      • +
    • Value 241-253: not used
      • +
    • Value 254: event-driven (manufacturer-specific)
      • +
    • Value 255: event-driven (device profile and application profile specific)
      • +
    +
    • +
  • Event timer in ms (0 = disabled) for deadline monitoring.
    • +
+

0x1401 - RPDO communication parameter

+ + + + + +
Object Type Count Label Storage Group IO extension PDO flags
RECORD RPDO PERSIST_COMM True False
+ + + + + + + + + + + +
Sub Name Data Type SDO PDO SRDO Default Value
0x00 Highest sub-index supported UNSIGNED8 ro no no 0x05
0x01 COB-ID used by RPDO UNSIGNED32 rw no no 0x80000300+$NODEID
0x02 Transmission type UNSIGNED8 rw no no 254
0x05 Event timer UNSIGNED16 rw no no 0
+
    +
  • COB-ID used by RPDO:
      +
    • bit 31: If set, PDO does not exist / is not valid
      • +
    • bit 11-30: set to 0
      • +
    • bit 0-10: 11-bit CAN-ID
      • +
    +
    • +
  • Transmission type:
      +
    • Value 0-240: synchronous, processed after next reception of SYNC object
      • +
    • Value 241-253: not used
      • +
    • Value 254: event-driven (manufacturer-specific)
      • +
    • Value 255: event-driven (device profile and application profile specific)
      • +
    +
    • +
  • Event timer in ms (0 = disabled) for deadline monitoring.
    • +
+

0x1402 - RPDO communication parameter

+ + + + + +
Object Type Count Label Storage Group IO extension PDO flags
RECORD RPDO PERSIST_COMM True False
+ + + + + + + + + + + +
Sub Name Data Type SDO PDO SRDO Default Value
0x00 Highest sub-index supported UNSIGNED8 ro no no 0x05
0x01 COB-ID used by RPDO UNSIGNED32 rw no no 0x80000400+$NODEID
0x02 Transmission type UNSIGNED8 rw no no 254
0x05 Event timer UNSIGNED16 rw no no 0
+
    +
  • COB-ID used by RPDO:
      +
    • bit 31: If set, PDO does not exist / is not valid
      • +
    • bit 11-30: set to 0
      • +
    • bit 0-10: 11-bit CAN-ID
      • +
    +
    • +
  • Transmission type:
      +
    • Value 0-240: synchronous, processed after next reception of SYNC object
      • +
    • Value 241-253: not used
      • +
    • Value 254: event-driven (manufacturer-specific)
      • +
    • Value 255: event-driven (device profile and application profile specific)
      • +
    +
    • +
  • Event timer in ms (0 = disabled) for deadline monitoring.
    • +
+

0x1403 - RPDO communication parameter

+ + + + + +
Object Type Count Label Storage Group IO extension PDO flags
RECORD RPDO PERSIST_COMM True False
+ + + + + + + + + + + +
Sub Name Data Type SDO PDO SRDO Default Value
0x00 Highest sub-index supported UNSIGNED8 ro no no 0x05
0x01 COB-ID used by RPDO UNSIGNED32 rw no no 0x80000500+$NODEID
0x02 Transmission type UNSIGNED8 rw no no 254
0x05 Event timer UNSIGNED16 rw no no 0
+
    +
  • COB-ID used by RPDO:
      +
    • bit 31: If set, PDO does not exist / is not valid
      • +
    • bit 11-30: set to 0
      • +
    • bit 0-10: 11-bit CAN-ID
      • +
    +
    • +
  • Transmission type:
      +
    • Value 0-240: synchronous, processed after next reception of SYNC object
      • +
    • Value 241-253: not used
      • +
    • Value 254: event-driven (manufacturer-specific)
      • +
    • Value 255: event-driven (device profile and application profile specific)
      • +
    +
    • +
  • Event timer in ms (0 = disabled) for deadline monitoring.
    • +
+

0x1600 - RPDO mapping parameter

+ + + + + +
Object Type Count Label Storage Group IO extension PDO flags
RECORD PERSIST_COMM True False
+ + + + + + + + + + + + + + + + + + + + + +
Sub Name Data Type SDO PDO SRDO Default Value
0x00 Number of mapped application objects in PDO UNSIGNED8 rw no no 0
0x01 Application object 1 UNSIGNED32 rw no no 0x00000000
0x02 Application object 2 UNSIGNED32 rw no no 0x00000000
0x03 Application object 3 UNSIGNED32 rw no no 0x00000000
0x04 Application object 4 UNSIGNED32 rw no no 0x00000000
0x05 Application object 5 UNSIGNED32 rw no no 0x00000000
0x06 Application object 6 UNSIGNED32 rw no no 0x00000000
0x07 Application object 7 UNSIGNED32 rw no no 0x00000000
0x08 Application object 8 UNSIGNED32 rw no no 0x00000000
+
    +
  • Number of mapped application objects in PDO:
      +
    • Value 0: mapping is disabled.
      • +
    • Value 1: sub-index 0x01 is valid.
      • +
    • Value 2-8: sub-indexes 0x01 to (0x02 to 0x08) are valid.
      • +
    +
    • +
  • Application object 1-8:
      +
    • bit 16-31: index
      • +
    • bit 8-15: sub-index
      • +
    • bit 0-7: data length in bits
      • +
    +
    • +
+

0x1601 - RPDO mapping parameter

+ + + + + +
Object Type Count Label Storage Group IO extension PDO flags
RECORD PERSIST_COMM True False
+ + + + + + + + + + + + + + + + + + + + + +
Sub Name Data Type SDO PDO SRDO Default Value
0x00 Number of mapped application objects in PDO UNSIGNED8 rw no no 0
0x01 Application object 1 UNSIGNED32 rw no no 0x00000000
0x02 Application object 2 UNSIGNED32 rw no no 0x00000000
0x03 Application object 3 UNSIGNED32 rw no no 0x00000000
0x04 Application object 4 UNSIGNED32 rw no no 0x00000000
0x05 Application object 5 UNSIGNED32 rw no no 0x00000000
0x06 Application object 6 UNSIGNED32 rw no no 0x00000000
0x07 Application object 7 UNSIGNED32 rw no no 0x00000000
0x08 Application object 8 UNSIGNED32 rw no no 0x00000000
+
    +
  • Number of mapped application objects in PDO:
      +
    • Value 0: mapping is disabled.
      • +
    • Value 1: sub-index 0x01 is valid.
      • +
    • Value 2-8: sub-indexes 0x01 to (0x02 to 0x08) are valid.
      • +
    +
    • +
  • Application object 1-8:
      +
    • bit 16-31: index
      • +
    • bit 8-15: sub-index
      • +
    • bit 0-7: data length in bits
      • +
    +
    • +
+

0x1602 - RPDO mapping parameter

+ + + + + +
Object Type Count Label Storage Group IO extension PDO flags
RECORD PERSIST_COMM True False
+ + + + + + + + + + + + + + + + + + + + + +
Sub Name Data Type SDO PDO SRDO Default Value
0x00 Number of mapped application objects in PDO UNSIGNED8 rw no no 0
0x01 Application object 1 UNSIGNED32 rw no no 0x00000000
0x02 Application object 2 UNSIGNED32 rw no no 0x00000000
0x03 Application object 3 UNSIGNED32 rw no no 0x00000000
0x04 Application object 4 UNSIGNED32 rw no no 0x00000000
0x05 Application object 5 UNSIGNED32 rw no no 0x00000000
0x06 Application object 6 UNSIGNED32 rw no no 0x00000000
0x07 Application object 7 UNSIGNED32 rw no no 0x00000000
0x08 Application object 8 UNSIGNED32 rw no no 0x00000000
+
    +
  • Number of mapped application objects in PDO:
      +
    • Value 0: mapping is disabled.
      • +
    • Value 1: sub-index 0x01 is valid.
      • +
    • Value 2-8: sub-indexes 0x01 to (0x02 to 0x08) are valid.
      • +
    +
    • +
  • Application object 1-8:
      +
    • bit 16-31: index
      • +
    • bit 8-15: sub-index
      • +
    • bit 0-7: data length in bits
      • +
    +
    • +
+

0x1603 - RPDO mapping parameter

+ + + + + +
Object Type Count Label Storage Group IO extension PDO flags
RECORD PERSIST_COMM True False
+ + + + + + + + + + + + + + + + + + + + + +
Sub Name Data Type SDO PDO SRDO Default Value
0x00 Number of mapped application objects in PDO UNSIGNED8 rw no no 0
0x01 Application object 1 UNSIGNED32 rw no no 0x00000000
0x02 Application object 2 UNSIGNED32 rw no no 0x00000000
0x03 Application object 3 UNSIGNED32 rw no no 0x00000000
0x04 Application object 4 UNSIGNED32 rw no no 0x00000000
0x05 Application object 5 UNSIGNED32 rw no no 0x00000000
0x06 Application object 6 UNSIGNED32 rw no no 0x00000000
0x07 Application object 7 UNSIGNED32 rw no no 0x00000000
0x08 Application object 8 UNSIGNED32 rw no no 0x00000000
+
    +
  • Number of mapped application objects in PDO:
      +
    • Value 0: mapping is disabled.
      • +
    • Value 1: sub-index 0x01 is valid.
      • +
    • Value 2-8: sub-indexes 0x01 to (0x02 to 0x08) are valid.
      • +
    +
    • +
  • Application object 1-8:
      +
    • bit 16-31: index
      • +
    • bit 8-15: sub-index
      • +
    • bit 0-7: data length in bits
      • +
    +
    • +
+

0x1800 - TPDO communication parameter

+ + + + + +
Object Type Count Label Storage Group IO extension PDO flags
RECORD TPDO PERSIST_COMM True False
+ + + + + + + + + + + + + + + +
Sub Name Data Type SDO PDO SRDO Default Value
0x00 Highest sub-index supported UNSIGNED8 ro no no 0x06
0x01 COB-ID used by TPDO UNSIGNED32 rw no no 0xC0000180+$NODEID
0x02 Transmission type UNSIGNED8 rw no no 254
0x03 Inhibit time UNSIGNED16 rw no no 0
0x05 Event timer UNSIGNED16 rw no no 0
0x06 SYNC start value UNSIGNED8 rw no no 0
+
    +
  • COB-ID used by RPDO:
      +
    • bit 31: If set, PDO does not exist / is not valid
      • +
    • bit 30: If set, NO RTR is allowed on this PDO
      • +
    • bit 11-29: set to 0
      • +
    • bit 0-10: 11-bit CAN-ID
      • +
    +
    • +
  • Transmission type:
      +
    • Value 0: synchronous (acyclic)
      • +
    • Value 1-240: synchronous (cyclic every (1-240)-th sync)
      • +
    • Value 241-253: not used
      • +
    • Value 254: event-driven (manufacturer-specific)
      • +
    • Value 255: event-driven (device profile and application profile specific)
      • +
    +
    • +
  • Inhibit time in multiple of 100µs, if the transmission type is set to 254 or 255 (0 = disabled).
    • +
  • Event timer interval in ms, if the transmission type is set to 254 or 255 (0 = disabled).
    • +
  • SYNC start value
      +
    • Value 0: Counter of the SYNC message shall not be processed.
      • +
    • Value 1-240: The SYNC message with the counter value equal to this value shall be regarded as the first received SYNC message.
      • +
    +
    • +
+

0x1801 - TPDO communication parameter

+ + + + + +
Object Type Count Label Storage Group IO extension PDO flags
RECORD TPDO PERSIST_COMM True False
+ + + + + + + + + + + + + + + +
Sub Name Data Type SDO PDO SRDO Default Value
0x00 Highest sub-index supported UNSIGNED8 ro no no 0x06
0x01 COB-ID used by TPDO UNSIGNED32 rw no no 0xC0000280+$NODEID
0x02 Transmission type UNSIGNED8 rw no no 254
0x03 Inhibit time UNSIGNED16 rw no no 0
0x05 Event timer UNSIGNED16 rw no no 0
0x06 SYNC start value UNSIGNED8 rw no no 0
+
    +
  • COB-ID used by RPDO:
      +
    • bit 31: If set, PDO does not exist / is not valid
      • +
    • bit 30: If set, NO RTR is allowed on this PDO
      • +
    • bit 11-29: set to 0
      • +
    • bit 0-10: 11-bit CAN-ID
      • +
    +
    • +
  • Transmission type:
      +
    • Value 0: synchronous (acyclic)
      • +
    • Value 1-240: synchronous (cyclic every (1-240)-th sync)
      • +
    • Value 241-253: not used
      • +
    • Value 254: event-driven (manufacturer-specific)
      • +
    • Value 255: event-driven (device profile and application profile specific)
      • +
    +
    • +
  • Inhibit time in multiple of 100µs, if the transmission type is set to 254 or 255 (0 = disabled).
    • +
  • Event timer interval in ms, if the transmission type is set to 254 or 255 (0 = disabled).
    • +
  • SYNC start value
      +
    • Value 0: Counter of the SYNC message shall not be processed.
      • +
    • Value 1-240: The SYNC message with the counter value equal to this value shall be regarded as the first received SYNC message.
      • +
    +
    • +
+

0x1802 - TPDO communication parameter

+ + + + + +
Object Type Count Label Storage Group IO extension PDO flags
RECORD TPDO PERSIST_COMM True False
+ + + + + + + + + + + + + + + +
Sub Name Data Type SDO PDO SRDO Default Value
0x00 Highest sub-index supported UNSIGNED8 ro no no 0x06
0x01 COB-ID used by TPDO UNSIGNED32 rw no no 0xC0000380+$NODEID
0x02 Transmission type UNSIGNED8 rw no no 254
0x03 Inhibit time UNSIGNED16 rw no no 0
0x05 Event timer UNSIGNED16 rw no no 0
0x06 SYNC start value UNSIGNED8 rw no no 0
+
    +
  • COB-ID used by RPDO:
      +
    • bit 31: If set, PDO does not exist / is not valid
      • +
    • bit 30: If set, NO RTR is allowed on this PDO
      • +
    • bit 11-29: set to 0
      • +
    • bit 0-10: 11-bit CAN-ID
      • +
    +
    • +
  • Transmission type:
      +
    • Value 0: synchronous (acyclic)
      • +
    • Value 1-240: synchronous (cyclic every (1-240)-th sync)
      • +
    • Value 241-253: not used
      • +
    • Value 254: event-driven (manufacturer-specific)
      • +
    • Value 255: event-driven (device profile and application profile specific)
      • +
    +
    • +
  • Inhibit time in multiple of 100µs, if the transmission type is set to 254 or 255 (0 = disabled).
    • +
  • Event timer interval in ms, if the transmission type is set to 254 or 255 (0 = disabled).
    • +
  • SYNC start value
      +
    • Value 0: Counter of the SYNC message shall not be processed.
      • +
    • Value 1-240: The SYNC message with the counter value equal to this value shall be regarded as the first received SYNC message.
      • +
    +
    • +
+

0x1803 - TPDO communication parameter

+ + + + + +
Object Type Count Label Storage Group IO extension PDO flags
RECORD TPDO PERSIST_COMM True False
+ + + + + + + + + + + + + + + +
Sub Name Data Type SDO PDO SRDO Default Value
0x00 Highest sub-index supported UNSIGNED8 ro no no 0x06
0x01 COB-ID used by TPDO UNSIGNED32 rw no no 0xC0000480+$NODEID
0x02 Transmission type UNSIGNED8 rw no no 254
0x03 Inhibit time UNSIGNED16 rw no no 0
0x05 Event timer UNSIGNED16 rw no no 0
0x06 SYNC start value UNSIGNED8 rw no no 0
+
    +
  • COB-ID used by RPDO:
      +
    • bit 31: If set, PDO does not exist / is not valid
      • +
    • bit 30: If set, NO RTR is allowed on this PDO
      • +
    • bit 11-29: set to 0
      • +
    • bit 0-10: 11-bit CAN-ID
      • +
    +
    • +
  • Transmission type:
      +
    • Value 0: synchronous (acyclic)
      • +
    • Value 1-240: synchronous (cyclic every (1-240)-th sync)
      • +
    • Value 241-253: not used
      • +
    • Value 254: event-driven (manufacturer-specific)
      • +
    • Value 255: event-driven (device profile and application profile specific)
      • +
    +
    • +
  • Inhibit time in multiple of 100µs, if the transmission type is set to 254 or 255 (0 = disabled).
    • +
  • Event timer interval in ms, if the transmission type is set to 254 or 255 (0 = disabled).
    • +
  • SYNC start value
      +
    • Value 0: Counter of the SYNC message shall not be processed.
      • +
    • Value 1-240: The SYNC message with the counter value equal to this value shall be regarded as the first received SYNC message.
      • +
    +
    • +
+

0x1A00 - TPDO mapping parameter

+ + + + + +
Object Type Count Label Storage Group IO extension PDO flags
RECORD PERSIST_COMM True False
+ + + + + + + + + + + + + + + + + + + + + +
Sub Name Data Type SDO PDO SRDO Default Value
0x00 Number of mapped application objects in PDO UNSIGNED8 rw no no 0
0x01 Application object 1 UNSIGNED32 rw no no 0x00000000
0x02 Application object 2 UNSIGNED32 rw no no 0x00000000
0x03 Application object 3 UNSIGNED32 rw no no 0x00000000
0x04 Application object 4 UNSIGNED32 rw no no 0x00000000
0x05 Application object 5 UNSIGNED32 rw no no 0x00000000
0x06 Application object 6 UNSIGNED32 rw no no 0x00000000
0x07 Application object 7 UNSIGNED32 rw no no 0x00000000
0x08 Application object 8 UNSIGNED32 rw no no 0x00000000
+
    +
  • Number of mapped application objects in PDO:
      +
    • Value 0: mapping is disabled.
      • +
    • Value 1: sub-index 0x01 is valid.
      • +
    • Value 2-8: sub-indexes 0x01 to (0x02 to 0x08) are valid.
      • +
    +
    • +
  • Application object 1-8:
      +
    • bit 16-31: index
      • +
    • bit 8-15: sub-index
      • +
    • bit 0-7: data length in bits
      • +
    +
    • +
+

0x1A01 - TPDO mapping parameter

+ + + + + +
Object Type Count Label Storage Group IO extension PDO flags
RECORD PERSIST_COMM True False
+ + + + + + + + + + + + + + + + + + + + + +
Sub Name Data Type SDO PDO SRDO Default Value
0x00 Number of mapped application objects in PDO UNSIGNED8 rw no no 0
0x01 Application object 1 UNSIGNED32 rw no no 0x00000000
0x02 Application object 2 UNSIGNED32 rw no no 0x00000000
0x03 Application object 3 UNSIGNED32 rw no no 0x00000000
0x04 Application object 4 UNSIGNED32 rw no no 0x00000000
0x05 Application object 5 UNSIGNED32 rw no no 0x00000000
0x06 Application object 6 UNSIGNED32 rw no no 0x00000000
0x07 Application object 7 UNSIGNED32 rw no no 0x00000000
0x08 Application object 8 UNSIGNED32 rw no no 0x00000000
+
    +
  • Number of mapped application objects in PDO:
      +
    • Value 0: mapping is disabled.
      • +
    • Value 1: sub-index 0x01 is valid.
      • +
    • Value 2-8: sub-indexes 0x01 to (0x02 to 0x08) are valid.
      • +
    +
    • +
  • Application object 1-8:
      +
    • bit 16-31: index
      • +
    • bit 8-15: sub-index
      • +
    • bit 0-7: data length in bits
      • +
    +
    • +
+

0x1A02 - TPDO mapping parameter

+ + + + + +
Object Type Count Label Storage Group IO extension PDO flags
RECORD PERSIST_COMM True False
+ + + + + + + + + + + + + + + + + + + + + +
Sub Name Data Type SDO PDO SRDO Default Value
0x00 Number of mapped application objects in PDO UNSIGNED8 rw no no 0
0x01 Application object 1 UNSIGNED32 rw no no 0x00000000
0x02 Application object 2 UNSIGNED32 rw no no 0x00000000
0x03 Application object 3 UNSIGNED32 rw no no 0x00000000
0x04 Application object 4 UNSIGNED32 rw no no 0x00000000
0x05 Application object 5 UNSIGNED32 rw no no 0x00000000
0x06 Application object 6 UNSIGNED32 rw no no 0x00000000
0x07 Application object 7 UNSIGNED32 rw no no 0x00000000
0x08 Application object 8 UNSIGNED32 rw no no 0x00000000
+
    +
  • Number of mapped application objects in PDO:
      +
    • Value 0: mapping is disabled.
      • +
    • Value 1: sub-index 0x01 is valid.
      • +
    • Value 2-8: sub-indexes 0x01 to (0x02 to 0x08) are valid.
      • +
    +
    • +
  • Application object 1-8:
      +
    • bit 16-31: index
      • +
    • bit 8-15: sub-index
      • +
    • bit 0-7: data length in bits
      • +
    +
    • +
+

0x1A03 - TPDO mapping parameter

+ + + + + +
Object Type Count Label Storage Group IO extension PDO flags
RECORD PERSIST_COMM True False
+ + + + + + + + + + + + + + + + + + + + + +
Sub Name Data Type SDO PDO SRDO Default Value
0x00 Number of mapped application objects in PDO UNSIGNED8 rw no no 0
0x01 Application object 1 UNSIGNED32 rw no no 0x00000000
0x02 Application object 2 UNSIGNED32 rw no no 0x00000000
0x03 Application object 3 UNSIGNED32 rw no no 0x00000000
0x04 Application object 4 UNSIGNED32 rw no no 0x00000000
0x05 Application object 5 UNSIGNED32 rw no no 0x00000000
0x06 Application object 6 UNSIGNED32 rw no no 0x00000000
0x07 Application object 7 UNSIGNED32 rw no no 0x00000000
0x08 Application object 8 UNSIGNED32 rw no no 0x00000000
+
    +
  • Number of mapped application objects in PDO:
      +
    • Value 0: mapping is disabled.
      • +
    • Value 1: sub-index 0x01 is valid.
      • +
    • Value 2-8: sub-indexes 0x01 to (0x02 to 0x08) are valid.
      • +
    +
    • +
  • Application object 1-8:
      +
    • bit 16-31: index
      • +
    • bit 8-15: sub-index
      • +
    • bit 0-7: data length in bits
      • +
    +
    • +
+
+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/modules.html b/Devices/Libraries/Systems/CANopenSocket/docs/modules.html new file mode 100755 index 0000000..4618d06 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/modules.html @@ -0,0 +1,167 @@ + + + + + + + +CANopenNode: Modules + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+
Modules
+
+
+
Here is a list of all modules:
+
[detail level 1234]
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
 CANopenCANopenNode is free and open source implementation of CANopen communication protocol
 ▼CANopen_301CANopen application layer and communication profile (CiA 301 v4.2.0)
 ▼Driver
 ▼Stack configuration
 Common definitions
 NMT master/slave and HB producer/consumer
 Emergency producer/consumer
 SDO server/client
 Time producer/consumer
 SYNC and PDO producer/consumer
 CANopen LED diodesSpecified in standard CiA 303-3
 Safety Related Data Objects (SRDO)Specified in standard EN 50325-5 (CiA 304)
 LSS master/slaveSpecified in standard CiA 305
 CANopen gatewaySpecified in standard CiA 309
 CRC 16 calculationHelper object
 FIFO bufferHelper object
 Trace recorderNon standard object
 Debug messagesMessages from different parts of the stack
 Basic definitionsTarget specific basic definitions and data types according to Misra C specification
 Reception of CAN messagesTarget specific definitions and description of CAN message reception
 Transmission of CAN messagesTarget specific definitions and description of CAN message transmission
 Critical sectionsCANopenNode is designed to run in different threads, as described in README.md. Threads are implemented differently in different systems. In microcontrollers threads are interrupts with different priorities, for example. It is necessary to protect sections, where different threads access to the same resource. In simple systems interrupts or scheduler may be temporary disabled between access to the shared resource. Otherwise mutexes or semaphores can be used
 Emergency
 FIFO circular buffer
 Heartbeat consumer
 NMT and Heartbeat
 ▼OD interface
 Getters and settersGetter and setter helper functions for accessing different types of Object Dictionary variables
 OD definition objectsTypes and functions used only for definition of Object Dictionary
 PDO
 SDO client
 SDO server
 SYNC
 TIME
 CRC 16 CCITT
 ▼CANopen_303CANopen recommendation for indicator specification (CiA 303-3 v1.4.0)
 LED indicators
 ▼CANopen_304CANopen Safety (EN 50325­-5:2010 (formerly CiA 304))
 GFC
 SRDO
 ▼CANopen_305CANopen layer setting services (LSS) and protocols (CiA 305 DSP v3.0.0)
 LSS
 LSS Master
 LSS Slave
 ▼CANopen_309CANopen access from other networks (CiA 309)
 ▼Gateway ASCII mapping
 Command syntax
 ▼CANopen_extraAdditional non-standard objects related to CANopenNode
 Trace
 ▼socketCANLinux specific interface to CANopenNode
 CO_driver_target.h
 Epoll interface
 CAN errors & Log
 OD storage
+
+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/modules.js b/Devices/Libraries/Systems/CANopenSocket/docs/modules.js new file mode 100755 index 0000000..44291b6 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/modules.js @@ -0,0 +1,11 @@ +var modules = +[ + [ "CANopen", "group__CO__CANopen.html", "group__CO__CANopen" ], + [ "CANopen_301", "group__CO__CANopen__301.html", "group__CO__CANopen__301" ], + [ "CANopen_303", "group__CO__CANopen__303.html", "group__CO__CANopen__303" ], + [ "CANopen_304", "group__CO__CANopen__304.html", "group__CO__CANopen__304" ], + [ "CANopen_305", "group__CO__CANopen__305.html", "group__CO__CANopen__305" ], + [ "CANopen_309", "group__CO__CANopen__309.html", "group__CO__CANopen__309" ], + [ "CANopen_extra", "group__CO__CANopen__extra.html", "group__CO__CANopen__extra" ], + [ "socketCAN", "group__CO__socketCAN.html", "group__CO__socketCAN" ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/nav_f.png b/Devices/Libraries/Systems/CANopenSocket/docs/nav_f.png new file mode 100755 index 0000000..72a58a5 Binary files /dev/null and b/Devices/Libraries/Systems/CANopenSocket/docs/nav_f.png differ diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/nav_g.png b/Devices/Libraries/Systems/CANopenSocket/docs/nav_g.png new file mode 100755 index 0000000..2093a23 Binary files /dev/null and b/Devices/Libraries/Systems/CANopenSocket/docs/nav_g.png differ diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/nav_h.png b/Devices/Libraries/Systems/CANopenSocket/docs/nav_h.png new file mode 100755 index 0000000..33389b1 Binary files /dev/null and b/Devices/Libraries/Systems/CANopenSocket/docs/nav_h.png differ diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/navtree.css b/Devices/Libraries/Systems/CANopenSocket/docs/navtree.css new file mode 100755 index 0000000..33341a6 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/navtree.css @@ -0,0 +1,146 @@ +#nav-tree .children_ul { + margin:0; + padding:4px; +} + +#nav-tree ul { + list-style:none outside none; + margin:0px; + padding:0px; +} + +#nav-tree li { + white-space:nowrap; + margin:0px; + padding:0px; +} + +#nav-tree .plus { + margin:0px; +} + +#nav-tree .selected { + background-image: url('tab_a.png'); + background-repeat:repeat-x; + color: #fff; + text-shadow: 0px 1px 1px rgba(0, 0, 0, 1.0); +} + +#nav-tree img { + margin:0px; + padding:0px; + border:0px; + vertical-align: middle; +} + +#nav-tree a { + text-decoration:none; + padding:0px; + margin:0px; + outline:none; +} + +#nav-tree .label { + margin:0px; + padding:0px; + font: 12px 'Lucida Grande',Geneva,Helvetica,Arial,sans-serif; +} + +#nav-tree .label a { + padding:2px; +} + +#nav-tree .selected a { + text-decoration:none; + color:#fff; +} + +#nav-tree .children_ul { + margin:0px; + padding:0px; +} + +#nav-tree .item { + margin:0px; + padding:0px; +} + +#nav-tree { + padding: 0px 0px; + background-color: #FAFAFF; + font-size:14px; + overflow:auto; +} + +#doc-content { + overflow:auto; + display:block; + padding:0px; + margin:0px; + -webkit-overflow-scrolling : touch; /* iOS 5+ */ +} + +#side-nav { + padding:0 6px 0 0; + margin: 0px; + display:block; + position: absolute; + left: 0px; + width: 250px; +} + +.ui-resizable .ui-resizable-handle { + display:block; +} + +.ui-resizable-e { + background-image:url("splitbar.png"); + background-size:100%; + background-repeat:repeat-y; + background-attachment: scroll; + cursor:ew-resize; + height:100%; + right:0; + top:0; + width:6px; +} + +.ui-resizable-handle { + display:none; + font-size:0.1px; + position:absolute; + z-index:1; +} + +#nav-tree-contents { + margin: 6px 0px 0px 0px; +} + +#nav-tree { + background-image:url('nav_h.png'); + background-repeat:repeat-x; + background-color: #F9FAFC; + -webkit-overflow-scrolling : touch; /* iOS 5+ */ +} + +#nav-sync { + position:absolute; + top:5px; + right:24px; + z-index:0; +} + +#nav-sync img { + opacity:0.3; +} + +#nav-sync img:hover { + opacity:0.9; +} + +@media print +{ + #nav-tree { display: none; } + div.ui-resizable-handle { display: none; position: relative; } +} + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/navtree.js b/Devices/Libraries/Systems/CANopenSocket/docs/navtree.js new file mode 100755 index 0000000..edc31ef --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/navtree.js @@ -0,0 +1,544 @@ +/* + @licstart The following is the entire license notice for the + JavaScript code in this file. + + Copyright (C) 1997-2019 by Dimitri van Heesch + + This program is free software; you can redistribute it and/or modify + it under the terms of version 2 of the GNU General Public License as + published by the Free Software Foundation. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License along + with this program; if not, write to the Free Software Foundation, Inc., + 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. + + @licend The above is the entire license notice + for the JavaScript code in this file + */ +var navTreeSubIndices = new Array(); +var arrowDown = '▼'; +var arrowRight = '►'; + +function getData(varName) +{ + var i = varName.lastIndexOf('/'); + var n = i>=0 ? varName.substring(i+1) : varName; + return eval(n.replace(/\-/g,'_')); +} + +function stripPath(uri) +{ + return uri.substring(uri.lastIndexOf('/')+1); +} + +function stripPath2(uri) +{ + var i = uri.lastIndexOf('/'); + var s = uri.substring(i+1); + var m = uri.substring(0,i+1).match(/\/d\w\/d\w\w\/$/); + return m ? uri.substring(i-6) : s; +} + +function hashValue() +{ + return $(location).attr('hash').substring(1).replace(/[^\w\-]/g,''); +} + +function hashUrl() +{ + return '#'+hashValue(); +} + +function pathName() +{ + return $(location).attr('pathname').replace(/[^-A-Za-z0-9+&@#/%?=~_|!:,.;\(\)]/g, ''); +} + +function localStorageSupported() +{ + try { + return 'localStorage' in window && window['localStorage'] !== null && window.localStorage.getItem; + } + catch(e) { + return false; + } +} + +function storeLink(link) +{ + if (!$("#nav-sync").hasClass('sync') && localStorageSupported()) { + window.localStorage.setItem('navpath',link); + } +} + +function deleteLink() +{ + if (localStorageSupported()) { + window.localStorage.setItem('navpath',''); + } +} + +function cachedLink() +{ + if (localStorageSupported()) { + return window.localStorage.getItem('navpath'); + } else { + return ''; + } +} + +function getScript(scriptName,func,show) +{ + var head = document.getElementsByTagName("head")[0]; + var script = document.createElement('script'); + script.id = scriptName; + script.type = 'text/javascript'; + script.onload = func; + script.src = scriptName+'.js'; + head.appendChild(script); +} + +function createIndent(o,domNode,node,level) +{ + var level=-1; + var n = node; + while (n.parentNode) { level++; n=n.parentNode; } + if (node.childrenData) { + var imgNode = document.createElement("span"); + imgNode.className = 'arrow'; + imgNode.style.paddingLeft=(16*level).toString()+'px'; + imgNode.innerHTML=arrowRight; + node.plus_img = imgNode; + node.expandToggle = document.createElement("a"); + node.expandToggle.href = "javascript:void(0)"; + node.expandToggle.onclick = function() { + if (node.expanded) { + $(node.getChildrenUL()).slideUp("fast"); + node.plus_img.innerHTML=arrowRight; + node.expanded = false; + } else { + expandNode(o, node, false, false); + } + } + node.expandToggle.appendChild(imgNode); + domNode.appendChild(node.expandToggle); + } else { + var span = document.createElement("span"); + span.className = 'arrow'; + span.style.width = 16*(level+1)+'px'; + span.innerHTML = ' '; + domNode.appendChild(span); + } +} + +var animationInProgress = false; + +function gotoAnchor(anchor,aname,updateLocation) +{ + var pos, docContent = $('#doc-content'); + var ancParent = $(anchor.parent()); + if (ancParent.hasClass('memItemLeft') || + ancParent.hasClass('memtitle') || + ancParent.hasClass('fieldname') || + ancParent.hasClass('fieldtype') || + ancParent.is(':header')) + { + pos = ancParent.position().top; + } else if (anchor.position()) { + pos = anchor.position().top; + } + if (pos) { + var dist = Math.abs(Math.min( + pos-docContent.offset().top, + docContent[0].scrollHeight- + docContent.height()-docContent.scrollTop())); + animationInProgress=true; + docContent.animate({ + scrollTop: pos + docContent.scrollTop() - docContent.offset().top + },Math.max(50,Math.min(500,dist)),function(){ + if (updateLocation) window.location.href=aname; + animationInProgress=false; + }); + } +} + +function newNode(o, po, text, link, childrenData, lastNode) +{ + var node = new Object(); + node.children = Array(); + node.childrenData = childrenData; + node.depth = po.depth + 1; + node.relpath = po.relpath; + node.isLast = lastNode; + + node.li = document.createElement("li"); + po.getChildrenUL().appendChild(node.li); + node.parentNode = po; + + node.itemDiv = document.createElement("div"); + node.itemDiv.className = "item"; + + node.labelSpan = document.createElement("span"); + node.labelSpan.className = "label"; + + createIndent(o,node.itemDiv,node,0); + node.itemDiv.appendChild(node.labelSpan); + node.li.appendChild(node.itemDiv); + + var a = document.createElement("a"); + node.labelSpan.appendChild(a); + node.label = document.createTextNode(text); + node.expanded = false; + a.appendChild(node.label); + if (link) { + var url; + if (link.substring(0,1)=='^') { + url = link.substring(1); + link = url; + } else { + url = node.relpath+link; + } + a.className = stripPath(link.replace('#',':')); + if (link.indexOf('#')!=-1) { + var aname = '#'+link.split('#')[1]; + var srcPage = stripPath(pathName()); + var targetPage = stripPath(link.split('#')[0]); + a.href = srcPage!=targetPage ? url : "javascript:void(0)"; + a.onclick = function(){ + storeLink(link); + if (!$(a).parent().parent().hasClass('selected')) + { + $('.item').removeClass('selected'); + $('.item').removeAttr('id'); + $(a).parent().parent().addClass('selected'); + $(a).parent().parent().attr('id','selected'); + } + var anchor = $(aname); + gotoAnchor(anchor,aname,true); + }; + } else { + a.href = url; + a.onclick = function() { storeLink(link); } + } + } else { + if (childrenData != null) + { + a.className = "nolink"; + a.href = "javascript:void(0)"; + a.onclick = node.expandToggle.onclick; + } + } + + node.childrenUL = null; + node.getChildrenUL = function() { + if (!node.childrenUL) { + node.childrenUL = document.createElement("ul"); + node.childrenUL.className = "children_ul"; + node.childrenUL.style.display = "none"; + node.li.appendChild(node.childrenUL); + } + return node.childrenUL; + }; + + return node; +} + +function showRoot() +{ + var headerHeight = $("#top").height(); + var footerHeight = $("#nav-path").height(); + var windowHeight = $(window).height() - headerHeight - footerHeight; + (function (){ // retry until we can scroll to the selected item + try { + var navtree=$('#nav-tree'); + navtree.scrollTo('#selected',100,{offset:-windowHeight/2}); + } catch (err) { + setTimeout(arguments.callee, 0); + } + })(); +} + +function expandNode(o, node, imm, showRoot) +{ + if (node.childrenData && !node.expanded) { + if (typeof(node.childrenData)==='string') { + var varName = node.childrenData; + getScript(node.relpath+varName,function(){ + node.childrenData = getData(varName); + expandNode(o, node, imm, showRoot); + }, showRoot); + } else { + if (!node.childrenVisited) { + getNode(o, node); + } + $(node.getChildrenUL()).slideDown("fast"); + node.plus_img.innerHTML = arrowDown; + node.expanded = true; + } + } +} + +function glowEffect(n,duration) +{ + n.addClass('glow').delay(duration).queue(function(next){ + $(this).removeClass('glow');next(); + }); +} + +function highlightAnchor() +{ + var aname = hashUrl(); + var anchor = $(aname); + if (anchor.parent().attr('class')=='memItemLeft'){ + var rows = $('.memberdecls tr[class$="'+hashValue()+'"]'); + glowEffect(rows.children(),300); // member without details + } else if (anchor.parent().attr('class')=='fieldname'){ + glowEffect(anchor.parent().parent(),1000); // enum value + } else if (anchor.parent().attr('class')=='fieldtype'){ + glowEffect(anchor.parent().parent(),1000); // struct field + } else if (anchor.parent().is(":header")) { + glowEffect(anchor.parent(),1000); // section header + } else { + glowEffect(anchor.next(),1000); // normal member + } +} + +function selectAndHighlight(hash,n) +{ + var a; + if (hash) { + var link=stripPath(pathName())+':'+hash.substring(1); + a=$('.item a[class$="'+link+'"]'); + } + if (a && a.length) { + a.parent().parent().addClass('selected'); + a.parent().parent().attr('id','selected'); + highlightAnchor(); + } else if (n) { + $(n.itemDiv).addClass('selected'); + $(n.itemDiv).attr('id','selected'); + } + if ($('#nav-tree-contents .item:first').hasClass('selected')) { + $('#nav-sync').css('top','30px'); + } else { + $('#nav-sync').css('top','5px'); + } + showRoot(); +} + +function showNode(o, node, index, hash) +{ + if (node && node.childrenData) { + if (typeof(node.childrenData)==='string') { + var varName = node.childrenData; + getScript(node.relpath+varName,function(){ + node.childrenData = getData(varName); + showNode(o,node,index,hash); + },true); + } else { + if (!node.childrenVisited) { + getNode(o, node); + } + $(node.getChildrenUL()).css({'display':'block'}); + node.plus_img.innerHTML = arrowDown; + node.expanded = true; + var n = node.children[o.breadcrumbs[index]]; + if (index+11) hash = '#'+parts[1].replace(/[^\w\-]/g,''); + else hash=''; + } + if (hash.match(/^#l\d+$/)) { + var anchor=$('a[name='+hash.substring(1)+']'); + glowEffect(anchor.parent(),1000); // line number + hash=''; // strip line number anchors + } + var url=root+hash; + var i=-1; + while (NAVTREEINDEX[i+1]<=url) i++; + if (i==-1) { i=0; root=NAVTREE[0][1]; } // fallback: show index + if (navTreeSubIndices[i]) { + gotoNode(o,i,root,hash,relpath) + } else { + getScript(relpath+'navtreeindex'+i,function(){ + navTreeSubIndices[i] = eval('NAVTREEINDEX'+i); + if (navTreeSubIndices[i]) { + gotoNode(o,i,root,hash,relpath); + } + },true); + } +} + +function showSyncOff(n,relpath) +{ + n.html(''); +} + +function showSyncOn(n,relpath) +{ + n.html(''); +} + +function toggleSyncButton(relpath) +{ + var navSync = $('#nav-sync'); + if (navSync.hasClass('sync')) { + navSync.removeClass('sync'); + showSyncOff(navSync,relpath); + storeLink(stripPath2(pathName())+hashUrl()); + } else { + navSync.addClass('sync'); + showSyncOn(navSync,relpath); + deleteLink(); + } +} + +var loadTriggered = false; +var readyTriggered = false; +var loadObject,loadToRoot,loadUrl,loadRelPath; + +$(window).on('load',function(){ + if (readyTriggered) { // ready first + navTo(loadObject,loadToRoot,loadUrl,loadRelPath); + showRoot(); + } + loadTriggered=true; +}); + +function initNavTree(toroot,relpath) +{ + var o = new Object(); + o.toroot = toroot; + o.node = new Object(); + o.node.li = document.getElementById("nav-tree-contents"); + o.node.childrenData = NAVTREE; + o.node.children = new Array(); + o.node.childrenUL = document.createElement("ul"); + o.node.getChildrenUL = function() { return o.node.childrenUL; }; + o.node.li.appendChild(o.node.childrenUL); + o.node.depth = 0; + o.node.relpath = relpath; + o.node.expanded = false; + o.node.isLast = true; + o.node.plus_img = document.createElement("span"); + o.node.plus_img.className = 'arrow'; + o.node.plus_img.innerHTML = arrowRight; + + if (localStorageSupported()) { + var navSync = $('#nav-sync'); + if (cachedLink()) { + showSyncOff(navSync,relpath); + navSync.removeClass('sync'); + } else { + showSyncOn(navSync,relpath); + } + navSync.click(function(){ toggleSyncButton(relpath); }); + } + + if (loadTriggered) { // load before ready + navTo(o,toroot,hashUrl(),relpath); + showRoot(); + } else { // ready before load + loadObject = o; + loadToRoot = toroot; + loadUrl = hashUrl(); + loadRelPath = relpath; + readyTriggered=true; + } + + $(window).bind('hashchange', function(){ + if (window.location.hash && window.location.hash.length>1){ + var a; + if ($(location).attr('hash')){ + var clslink=stripPath(pathName())+':'+hashValue(); + a=$('.item a[class$="'+clslink.replace(/ + + + + + + +CANopenNode: Related Pages + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+
Related Pages
+
+
+
Here is a list of all related documentation pages:
+ + + + + + + + +
 Change Log
 Device Support
 Getting Started
 LSS usage
 Object Dictionary
 Trace usage
 CANopen documentation
+
+
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/resize.js b/Devices/Libraries/Systems/CANopenSocket/docs/resize.js new file mode 100755 index 0000000..a0bb5f4 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/resize.js @@ -0,0 +1,137 @@ +/* + @licstart The following is the entire license notice for the + JavaScript code in this file. + + Copyright (C) 1997-2017 by Dimitri van Heesch + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 2 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License along + with this program; if not, write to the Free Software Foundation, Inc., + 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. + + @licend The above is the entire license notice + for the JavaScript code in this file + */ +function initResizable() +{ + var cookie_namespace = 'doxygen'; + var sidenav,navtree,content,header,collapsed,collapsedWidth=0,barWidth=6,desktop_vp=768,titleHeight; + + function readCookie(cookie) + { + var myCookie = cookie_namespace+"_"+cookie+"="; + if (document.cookie) { + var index = document.cookie.indexOf(myCookie); + if (index != -1) { + var valStart = index + myCookie.length; + var valEnd = document.cookie.indexOf(";", valStart); + if (valEnd == -1) { + valEnd = document.cookie.length; + } + var val = document.cookie.substring(valStart, valEnd); + return val; + } + } + return 0; + } + + function writeCookie(cookie, val, expiration) + { + if (val==undefined) return; + if (expiration == null) { + var date = new Date(); + date.setTime(date.getTime()+(10*365*24*60*60*1000)); // default expiration is one week + expiration = date.toGMTString(); + } + document.cookie = cookie_namespace + "_" + cookie + "=" + val + "; expires=" + expiration+"; path=/"; + } + + function resizeWidth() + { + var windowWidth = $(window).width() + "px"; + var sidenavWidth = $(sidenav).outerWidth(); + content.css({marginLeft:parseInt(sidenavWidth)+"px"}); + writeCookie('width',sidenavWidth-barWidth, null); + } + + function restoreWidth(navWidth) + { + var windowWidth = $(window).width() + "px"; + content.css({marginLeft:parseInt(navWidth)+barWidth+"px"}); + sidenav.css({width:navWidth + "px"}); + } + + function resizeHeight() + { + var headerHeight = header.outerHeight(); + var footerHeight = footer.outerHeight(); + var windowHeight = $(window).height() - headerHeight - footerHeight; + content.css({height:windowHeight + "px"}); + navtree.css({height:windowHeight + "px"}); + sidenav.css({height:windowHeight + "px"}); + var width=$(window).width(); + if (width!=collapsedWidth) { + if (width=desktop_vp) { + if (!collapsed) { + collapseExpand(); + } + } else if (width>desktop_vp && collapsedWidth0) { + restoreWidth(0); + collapsed=true; + } + else { + var width = readCookie('width'); + if (width>200 && width<$(window).width()) { restoreWidth(width); } else { restoreWidth(200); } + collapsed=false; + } + } + + header = $("#top"); + sidenav = $("#side-nav"); + content = $("#doc-content"); + navtree = $("#nav-tree"); + footer = $("#nav-path"); + $(".side-nav-resizable").resizable({resize: function(e, ui) { resizeWidth(); } }); + $(sidenav).resizable({ minWidth: 0 }); + $(window).resize(function() { resizeHeight(); }); + var device = navigator.userAgent.toLowerCase(); + var touch_device = device.match(/(iphone|ipod|ipad|android)/); + if (touch_device) { /* wider split bar for touch only devices */ + $(sidenav).css({ paddingRight:'20px' }); + $('.ui-resizable-e').css({ width:'20px' }); + $('#nav-sync').css({ right:'34px' }); + barWidth=20; + } + var width = readCookie('width'); + if (width) { restoreWidth(width); } else { resizeWidth(); } + resizeHeight(); + var url = location.href; + var i=url.indexOf("#"); + if (i>=0) window.location.hash=url.substr(i); + var _preventDefault = function(evt) { evt.preventDefault(); }; + $("#splitbar").bind("dragstart", _preventDefault).bind("selectstart", _preventDefault); + $(".ui-resizable-handle").dblclick(collapseExpand); + $(window).on('load',resizeHeight); +} +/* @license-end */ diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/all_0.html b/Devices/Libraries/Systems/CANopenSocket/docs/search/all_0.html new file mode 100755 index 0000000..26dd244 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/all_0.html @@ -0,0 +1,30 @@ + + + + + + + + + +
+
Loading...
+
+ +
Searching...
+
No Matches
+ +
+ + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/all_0.js b/Devices/Libraries/Systems/CANopenSocket/docs/search/all_0.js new file mode 100755 index 0000000..d9e964b --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/all_0.js @@ -0,0 +1,10 @@ +var searchData= +[ + ['activenodeid_0',['activeNodeID',['../structCO__LSSslave__t.html#a91bb370cba5215ddaf52c0883a9bdca2',1,'CO_LSSslave_t']]], + ['allmonitoredactive_1',['allMonitoredActive',['../structCO__HBconsumer__t.html#aaff60bb59e36a3b0ddd11b45268eaf33',1,'CO_HBconsumer_t']]], + ['allmonitoredoperational_2',['allMonitoredOperational',['../structCO__HBconsumer__t.html#a9407103796db857229ec5b266c580b37',1,'CO_HBconsumer_t']]], + ['altreadptr_3',['altReadPtr',['../structCO__fifo__t.html#a4f8eadd2e9b966ce21274cbbceb3adbe',1,'CO_fifo_t']]], + ['attribute_4',['attribute',['../structOD__subEntry__t.html#ae7d83df4e106219f32cb28d7c510b9d2',1,'OD_subEntry_t::attribute()'],['../structOD__obj__var__t.html#a4662bd6ca12b3ec147f9ffeafb64fe77',1,'OD_obj_var_t::attribute()'],['../structOD__obj__array__t.html#a6af20a410bcd0c8c9f619c4a564b962a',1,'OD_obj_array_t::attribute()'],['../structOD__obj__record__t.html#a42290a19541170f8d108acf029fec171',1,'OD_obj_record_t::attribute()'],['../structCO__SDOclient__t.html#a609088a2005febd6cd1561288cf1b7d1',1,'CO_SDOclient_t::attribute()'],['../structCO__SDOserver__t.html#a3b2febaed4df4921626367a741008400',1,'CO_SDOserver_t::attribute()']]], + ['attribute0_5',['attribute0',['../structOD__obj__array__t.html#a1cb4802d94112e5bd2f1b0db5e3e5d99',1,'OD_obj_array_t']]], + ['aux_6',['aux',['../structCO__fifo__t.html#aa255bcb00601a8f4225c97ad6cd854a7',1,'CO_fifo_t']]] +]; diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/all_1.html b/Devices/Libraries/Systems/CANopenSocket/docs/search/all_1.html new file mode 100755 index 0000000..8eb215b --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/all_1.html @@ -0,0 +1,30 @@ + + + + + + + + + +
+
Loading...
+
+ +
Searching...
+
No Matches
+ +
+ + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/all_1.js b/Devices/Libraries/Systems/CANopenSocket/docs/search/all_1.js new file mode 100755 index 0000000..ecee1a5 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/all_1.js @@ -0,0 +1,21 @@ +var searchData= +[ + ['block_5fblksize_7',['block_blksize',['../structCO__SDOclient__t.html#a36b10791595b638309a01418d13a745f',1,'CO_SDOclient_t::block_blksize()'],['../structCO__SDOserver__t.html#a76e4c66e15027e78b8ba67bdd3089cc3',1,'CO_SDOserver_t::block_blksize()']]], + ['block_5fcrc_8',['block_crc',['../structCO__SDOclient__t.html#a51322a623ff85d36a8be60c0fe11430e',1,'CO_SDOclient_t::block_crc()'],['../structCO__SDOserver__t.html#a33c2432ccea06da1d5c33c89c14caf63',1,'CO_SDOserver_t::block_crc()']]], + ['block_5fcrcenabled_9',['block_crcEnabled',['../structCO__SDOclient__t.html#a8690a5e7ee83fb7e0fa3a76cdec83f3a',1,'CO_SDOclient_t::block_crcEnabled()'],['../structCO__SDOserver__t.html#abdf7205835b75f0f6feef3bc89a86c17',1,'CO_SDOserver_t::block_crcEnabled()']]], + ['block_5fdatauploadlast_10',['block_dataUploadLast',['../structCO__SDOclient__t.html#ae9e678cb0e461851298658c7eee01334',1,'CO_SDOclient_t']]], + ['block_5fnodata_11',['block_noData',['../structCO__SDOclient__t.html#a26f9fcf95f47a4f7eeaefdf684e317a1',1,'CO_SDOclient_t::block_noData()'],['../structCO__SDOserver__t.html#a54bac23ac93450234a858c50f0516d05',1,'CO_SDOserver_t::block_noData()']]], + ['block_5fsdotimeouttime_5fus_12',['block_SDOtimeoutTime_us',['../structCO__SDOclient__t.html#ae86079157706e7db12d9d4817172ba10',1,'CO_SDOclient_t::block_SDOtimeoutTime_us()'],['../structCO__SDOserver__t.html#ae1e16955965dced9464abd0a6bf8c2b2',1,'CO_SDOserver_t::block_SDOtimeoutTime_us()']]], + ['block_5fseqno_13',['block_seqno',['../structCO__SDOclient__t.html#a628780da4dccceab6ce79ad880989b26',1,'CO_SDOclient_t::block_seqno()'],['../structCO__SDOserver__t.html#aeefd77d5200958a30f63f7e1f4f474fa',1,'CO_SDOserver_t::block_seqno()']]], + ['block_5ftimeouttimer_14',['block_timeoutTimer',['../structCO__SDOclient__t.html#a8a667736bf5d22e7bb76f8b25d8b0268',1,'CO_SDOclient_t::block_timeoutTimer()'],['../structCO__SDOserver__t.html#ad8748718c76f53347dde5248b1152626',1,'CO_SDOserver_t::block_timeoutTimer()']]], + ['bool_5ft_15',['bool_t',['../group__CO__dataTypes.html#ga449976458a084f880dc8e3d29e7eb6f5',1,'CO_driver.h']]], + ['buf_16',['buf',['../structCO__fifo__t.html#aa4a8bae66b107809c099cd1d2de5c966',1,'CO_fifo_t::buf()'],['../structCO__SDOclient__t.html#aa56b1f115aee473f5c264142053ed0ae',1,'CO_SDOclient_t::buf()'],['../structCO__SDOserver__t.html#ab834e69e77b72e1ee1d39d35e21e0df4',1,'CO_SDOserver_t::buf()']]], + ['bufferfull_17',['bufferFull',['../structCO__CANtx__t.html#a305f0687a4ed7cd533e7937d6ff7d31b',1,'CO_CANtx_t']]], + ['bufferinhibitflag_18',['bufferInhibitFlag',['../structCO__CANmodule__t.html#ad7af19de0bf39c8927c926b095cb5292',1,'CO_CANmodule_t']]], + ['buffersize_19',['bufferSize',['../structCO__trace__t.html#a3dfe996be2bef106f50de085a7f5ca9f',1,'CO_trace_t']]], + ['buffifo_20',['bufFifo',['../structCO__SDOclient__t.html#a634a9311de3569cf38841d2faaac20b3',1,'CO_SDOclient_t']]], + ['bufoffsetrd_21',['bufOffsetRd',['../structCO__SDOserver__t.html#a93557d1ee64582bc37c76f4d4680f7a5',1,'CO_SDOserver_t']]], + ['bufoffsetwr_22',['bufOffsetWr',['../structCO__SDOserver__t.html#a69f52a30b4e7dc0a6e55c88b3126f814',1,'CO_SDOserver_t']]], + ['bufsize_23',['bufSize',['../structCO__fifo__t.html#a35d9f2fcb8d2ef00794cb305d8344e61',1,'CO_fifo_t']]], + ['basic_20definitions_24',['Basic definitions',['../group__CO__dataTypes.html',1,'']]] +]; diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/all_10.html b/Devices/Libraries/Systems/CANopenSocket/docs/search/all_10.html new file mode 100755 index 0000000..6fd3a4a --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/all_10.html @@ -0,0 +1,30 @@ + + + + + + + + + +
+
Loading...
+
+ +
Searching...
+
No Matches
+ +
+ + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/all_10.js b/Devices/Libraries/Systems/CANopenSocket/docs/search/all_10.js new file mode 100755 index 0000000..a46bc53 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/all_10.js @@ -0,0 +1,49 @@ +var searchData= +[ + ['transmission_20of_20can_20messages_1281',['Transmission of CAN messages',['../group__CO__CAN__Message__transmission.html',1,'']]], + ['time_20producer_2fconsumer_1282',['Time producer/consumer',['../group__CO__STACK__CONFIG__TIME.html',1,'']]], + ['trace_20recorder_1283',['Trace recorder',['../group__CO__STACK__CONFIG__TRACE.html',1,'']]], + ['time_1284',['TIME',['../group__CO__TIME.html',1,'']]], + ['trace_1285',['Trace',['../group__CO__trace.html',1,'']]], + ['trace_20usage_1286',['Trace usage',['../md_doc_traceUsage.html',1,'']]], + ['threshold_1287',['threshold',['../structCO__trace__t.html#a6400336e1207dcfa5fa8ef2a908fda08',1,'CO_trace_t']]], + ['time_1288',['TIME',['../structCO__t.html#a80052c25c47be2460e02cf88ff91bbb7',1,'CO_t']]], + ['time_5fus_1289',['time_us',['../structCO__HBconsNode__t.html#a489df7aff00a25d8f2a63fe968050b08',1,'CO_HBconsNode_t']]], + ['timebuffer_1290',['timeBuffer',['../structCO__trace__t.html#a83806299bd57a23f5946f81cb05ab41d',1,'CO_trace_t']]], + ['timedifference_5fus_1291',['timeDifference_us',['../structCO__epoll__t.html#a46f38181dc8483ce83af566ee3f8aff3',1,'CO_epoll_t']]], + ['timedifference_5fus_5fcumulative_1292',['timeDifference_us_cumulative',['../structCO__GTWA__t.html#a8ba7809acba0f2de26eda4a890e68160',1,'CO_GTWA_t']]], + ['timeout_5fus_1293',['timeout_us',['../structCO__LSSmaster__t.html#aeac43e30ee9018bb34876a4c2f1a10de',1,'CO_LSSmaster_t']]], + ['timeouttimer_1294',['timeoutTimer',['../structCO__HBconsNode__t.html#ac4ffced8a11aac2b7383244f306c2081',1,'CO_HBconsNode_t::timeoutTimer()'],['../structCO__SDOclient__t.html#a4e2ff087f13ff5a4754b9186c1a2929e',1,'CO_SDOclient_t::timeoutTimer()'],['../structCO__SDOserver__t.html#a69dfa2436ba0bb51c527eb03330b48a7',1,'CO_SDOserver_t::timeoutTimer()'],['../structCO__LSSmaster__t.html#aa706f4a19295e26a862127917ebc9eca',1,'CO_LSSmaster_t::timeoutTimer()']]], + ['timer_1295',['timer',['../structCO__SYNC__t.html#a491f176a5fb70400647474555628bf6f',1,'CO_SYNC_t::timer()'],['../structCO__TIME__t.html#a02fa485a1a5064ad6e0fc07ea3d5d092',1,'CO_TIME_t::timer()'],['../structCO__SRDO__t.html#a1d3553daf7179b3389023ac78287dfbc',1,'CO_SRDO_t::timer()']]], + ['timer_5ffd_1296',['timer_fd',['../structCO__epoll__t.html#aef151da1fe1a293f34f4f546c24bb0f7',1,'CO_epoll_t']]], + ['timerevent_1297',['timerEvent',['../structCO__epoll__t.html#a9fa1c50ecb111049eda7e45132d3dfd1',1,'CO_epoll_t']]], + ['timerinterval_5fus_1298',['timerInterval_us',['../structCO__epoll__t.html#a0595fb48d72a8592e6649e3de7b3477c',1,'CO_epoll_t']]], + ['timernext_5fus_1299',['timerNext_us',['../structCO__epoll__t.html#a0cb262435f594f9d7ea42eb976294262',1,'CO_epoll_t']]], + ['timestamp_1300',['timestamp',['../structCO__CANinterfaceErrorhandler__t.html#a615dfcafdb866ae951296333df35d1c7',1,'CO_CANinterfaceErrorhandler_t']]], + ['tm_1301',['tm',['../structCO__epoll__t.html#a759aff5ae2eb019aa2ce8117f9ce9836',1,'CO_epoll_t']]], + ['toggle_1302',['toggle',['../structCO__SDOclient__t.html#a5962727f5c1830337146c7b2b389b391',1,'CO_SDOclient_t::toggle()'],['../structCO__SDOserver__t.html#a158797aa411b8d3b5c2079907d04ca0d',1,'CO_SDOserver_t::toggle()']]], + ['toogle_1303',['toogle',['../structCO__SRDO__t.html#a79114736807f3aa8ab19e6e88df93050',1,'CO_SRDO_t']]], + ['tpdo_1304',['TPDO',['../structCO__t.html#ab5027ca1447bf64e5e93935c1e5466c2',1,'CO_t']]], + ['tpdocommpar_1305',['TPDOCommPar',['../structCO__TPDO__t.html#ab9bd2bf1a76f1f0e253dd4c4a941ba67',1,'CO_TPDO_t']]], + ['tpdomappar_1306',['TPDOMapPar',['../structCO__TPDO__t.html#a6f6202c2b866f552c512a3513c27be8b',1,'CO_TPDO_t']]], + ['trace_1307',['trace',['../structCO__t.html#a4f7a05e49ea89178cb61e14c4c4575f1',1,'CO_t']]], + ['transmissiontype_1308',['transmissionType',['../structCO__RPDOCommPar__t.html#a09eb4787337cf8579c0ff1d4ae968aba',1,'CO_RPDOCommPar_t::transmissionType()'],['../structCO__TPDOCommPar__t.html#a328398227ff1f167649d54453e44df97',1,'CO_TPDOCommPar_t::transmissionType()'],['../structCO__SRDOCommPar__t.html#a8716aa43cf70db8af86c4125b14cd538',1,'CO_SRDOCommPar_t::transmissionType()']]], + ['trigger_1309',['trigger',['../structCO__trace__t.html#a879f362c9d4a88de9e48d23c4d0c770c',1,'CO_trace_t']]], + ['triggertime_1310',['triggerTime',['../structCO__trace__t.html#a1b7176f0ad48679449a666c34fe45a3f',1,'CO_trace_t']]], + ['true_1311',['true',['../group__CO__dataTypes.html#ga41f9c5fb8b08eb5dc3edce4dcb37fee7',1,'CO_driver.h']]], + ['tx_5fidx_5fem_5fprod_1312',['TX_IDX_EM_PROD',['../structCO__t.html#a3d2f250c5c3bc972ce45418d22e79caa',1,'CO_t']]], + ['tx_5fidx_5fgfc_1313',['TX_IDX_GFC',['../structCO__t.html#a709e86d5401484bb7d0c5f17e824636d',1,'CO_t']]], + ['tx_5fidx_5fhb_5fprod_1314',['TX_IDX_HB_PROD',['../structCO__t.html#ad2718c137526676467853885d286fc74',1,'CO_t']]], + ['tx_5fidx_5flss_5fmst_1315',['TX_IDX_LSS_MST',['../structCO__t.html#a38e102b879d1dd04f69bacac56266fdf',1,'CO_t']]], + ['tx_5fidx_5flss_5fslv_1316',['TX_IDX_LSS_SLV',['../structCO__t.html#a7d1159efd4a31d0701c497d25343073d',1,'CO_t']]], + ['tx_5fidx_5fnmt_5fmst_1317',['TX_IDX_NMT_MST',['../structCO__t.html#a77bf70da8db7ecfe78f3720bfca12101',1,'CO_t']]], + ['tx_5fidx_5fsdo_5fcli_1318',['TX_IDX_SDO_CLI',['../structCO__t.html#adc86b044f08602394cbb4346b41a9a45',1,'CO_t']]], + ['tx_5fidx_5fsdo_5fsrv_1319',['TX_IDX_SDO_SRV',['../structCO__t.html#a9287e4d62f23f4222fc90c4a6ded2ef0',1,'CO_t']]], + ['tx_5fidx_5fsrdo_1320',['TX_IDX_SRDO',['../structCO__t.html#a3dbc89e76d1627f2b360ddaefd7a7b51',1,'CO_t']]], + ['tx_5fidx_5fsync_1321',['TX_IDX_SYNC',['../structCO__t.html#a2eb3962f37ce7a4b51d8779f96db58bc',1,'CO_t']]], + ['tx_5fidx_5ftime_1322',['TX_IDX_TIME',['../structCO__t.html#ad5cbb43e75d99c5479efb3eddd36ec3d',1,'CO_t']]], + ['tx_5fidx_5ftpdo_1323',['TX_IDX_TPDO',['../structCO__t.html#a9451781c713680d8bab86cda2a1ca570',1,'CO_t']]], + ['txarray_1324',['txArray',['../structCO__CANmodule__t.html#af623f2a045716af77e906b28deee3d3c',1,'CO_CANmodule_t']]], + ['txbuff_1325',['TXbuff',['../structCO__TIME__t.html#aa658a7b219b1dad6b722ec41670da626',1,'CO_TIME_t::TXbuff()'],['../structCO__LSSmaster__t.html#ad915ab10431aea283df0d8e1299876e2',1,'CO_LSSmaster_t::TXbuff()'],['../structCO__LSSslave__t.html#a12b8505b3f2bcf2085b38bbdcda6736f',1,'CO_LSSslave_t::TXbuff()']]], + ['txsize_1326',['txSize',['../structCO__CANmodule__t.html#a6a53d3d12efbe30d1ee08e821d1b4139',1,'CO_CANmodule_t']]] +]; diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/all_11.html b/Devices/Libraries/Systems/CANopenSocket/docs/search/all_11.html new file mode 100755 index 0000000..f78343b --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/all_11.html @@ -0,0 +1,30 @@ + + + + + + + + + +
+
Loading...
+
+ +
Searching...
+
No Matches
+ +
+ + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/all_11.js b/Devices/Libraries/Systems/CANopenSocket/docs/search/all_11.js new file mode 100755 index 0000000..b5fb7e4 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/all_11.js @@ -0,0 +1,8 @@ +var searchData= +[ + ['uint16_5ft_1327',['uint16_t',['../group__CO__dataTypes.html#ga1f1825b69244eb3ad2c7165ddc99c956',1,'CO_driver.h']]], + ['uint32_5ft_1328',['uint32_t',['../group__CO__dataTypes.html#ga33594304e786b158f3fb30289278f5af',1,'CO_driver.h']]], + ['uint64_5ft_1329',['uint64_t',['../group__CO__dataTypes.html#gad27ed092432b64ff558d2254c278720f',1,'CO_driver.h']]], + ['uint8_5ft_1330',['uint8_t',['../group__CO__dataTypes.html#gaba7bc1797add20fe3efdf37ced1182c5',1,'CO_driver.h']]], + ['usecanrxfilters_1331',['useCANrxFilters',['../structCO__CANmodule__t.html#a9b28f7a6f02d398b3a0ea6cf70fa64f0',1,'CO_CANmodule_t']]] +]; diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/all_12.html b/Devices/Libraries/Systems/CANopenSocket/docs/search/all_12.html new file mode 100755 index 0000000..dd9ff1d --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/all_12.html @@ -0,0 +1,30 @@ + + + + + + + + + +
+
Loading...
+
+ +
Searching...
+
No Matches
+ +
+ + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/all_12.js b/Devices/Libraries/Systems/CANopenSocket/docs/search/all_12.js new file mode 100755 index 0000000..0fd25f5 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/all_12.js @@ -0,0 +1,7 @@ +var searchData= +[ + ['valid_1332',['valid',['../structCO__RPDO__t.html#a1d9a4be6ad3245309ffe6e3ad5637942',1,'CO_RPDO_t::valid()'],['../structCO__TPDO__t.html#a201c8a0726347a747f6b315915c797fb',1,'CO_TPDO_t::valid()'],['../structCO__GFC__t.html#a775fa3a4f1afda4a4be200f56d6e2b54',1,'CO_GFC_t::valid()'],['../structCO__SRDO__t.html#a6eef41749d7862ef2a29108f4f08185a',1,'CO_SRDO_t::valid()']]], + ['value_1333',['value',['../structCO__trace__t.html#a24fa467aeeb1581c6a3272bd397a2f10',1,'CO_trace_t']]], + ['valuebuffer_1334',['valueBuffer',['../structCO__trace__t.html#abc2f00a5f99453c77dd8515f3526e428',1,'CO_trace_t']]], + ['valueprev_1335',['valuePrev',['../structCO__trace__t.html#abe713136228c54327c1540b99a1a2fd1',1,'CO_trace_t']]] +]; diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/all_13.html b/Devices/Libraries/Systems/CANopenSocket/docs/search/all_13.html new file mode 100755 index 0000000..2611a10 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/all_13.html @@ -0,0 +1,30 @@ + + + + + + + + + +
+
Loading...
+
+ +
Searching...
+
No Matches
+ +
+ + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/all_13.js b/Devices/Libraries/Systems/CANopenSocket/docs/search/all_13.js new file mode 100755 index 0000000..42cc52f --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/all_13.js @@ -0,0 +1,5 @@ +var searchData= +[ + ['write_1336',['write',['../structOD__IO__t.html#aa296d8e76d99af5c395971602a453b78',1,'OD_IO_t::write()'],['../structOD__extensionIO__t.html#a06573b7740c3c991352734bba25f0fd4',1,'OD_extensionIO_t::write()']]], + ['writeptr_1337',['writePtr',['../structCO__fifo__t.html#a540fbc52344d1205de11625cd81f351d',1,'CO_fifo_t::writePtr()'],['../structCO__trace__t.html#ae3a556a180e38e7247b39b84de609b5d',1,'CO_trace_t::writePtr()']]] +]; diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/all_2.html b/Devices/Libraries/Systems/CANopenSocket/docs/search/all_2.html new file mode 100755 index 0000000..b26d916 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/all_2.html @@ -0,0 +1,30 @@ + + + + + + + + + +
+
Loading...
+
+ +
Searching...
+
No Matches
+ +
+ + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/all_2.js b/Devices/Libraries/Systems/CANopenSocket/docs/search/all_2.js new file mode 100755 index 0000000..0d8caad --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/all_2.js @@ -0,0 +1,806 @@ +var searchData= +[ + ['candata_25',['CANdata',['../structCO__LSSslave__t.html#a80021b6e36da1b495453d360a7001287',1,'CO_LSSslave_t']]], + ['candevrx_26',['CANdevRx',['../structCO__HBconsumer__t.html#af5d8828478e2b51fe47a63f24dd896b1',1,'CO_HBconsumer_t::CANdevRx()'],['../structCO__RPDO__t.html#a3f153d5326f07ec7c8f3570287547454',1,'CO_RPDO_t::CANdevRx()'],['../structCO__SDOclient__t.html#a3283cda81a28d3b193a6a6bef29fadab',1,'CO_SDOclient_t::CANdevRx()'],['../structCO__SDOserver__t.html#a3e6255db1f66d742debd80ff2ce25012',1,'CO_SDOserver_t::CANdevRx()'],['../structCO__SYNC__t.html#ae799680e430d4d6b05d7a3da216f06be',1,'CO_SYNC_t::CANdevRx()'],['../structCO__TIME__t.html#ad21e78dd36756f1868056bf6638f5ccd',1,'CO_TIME_t::CANdevRx()'],['../structCO__SRDO__t.html#ad5c0cea22d56cef74f42728107748b38',1,'CO_SRDO_t::CANdevRx()']]], + ['candevrxidx_27',['CANdevRxIdx',['../structCO__RPDO__t.html#a000fe56fcaf727c59b7dc049ec7fc4b1',1,'CO_RPDO_t::CANdevRxIdx()'],['../structCO__SDOclient__t.html#afb8613dbcacfcefb970fabca4106eaeb',1,'CO_SDOclient_t::CANdevRxIdx()'],['../structCO__SDOserver__t.html#a8c5ca24946f34e174fce129a2f5cb38a',1,'CO_SDOserver_t::CANdevRxIdx()'],['../structCO__SYNC__t.html#a1a489d5fd447a8b5e16fb82d957d0667',1,'CO_SYNC_t::CANdevRxIdx()'],['../structCO__TIME__t.html#a4d9335af44c9b5e1224a6691df2f9594',1,'CO_TIME_t::CANdevRxIdx()'],['../structCO__SRDO__t.html#aa1d8ca455950557652636bc8021928a4',1,'CO_SRDO_t::CANdevRxIdx()']]], + ['candevrxidxstart_28',['CANdevRxIdxStart',['../structCO__HBconsumer__t.html#a00d176c84d169115399c276031b71722',1,'CO_HBconsumer_t']]], + ['candevtx_29',['CANdevTx',['../structCO__EM__t.html#a5d24b22a05c354937894109a30b1f641',1,'CO_EM_t::CANdevTx()'],['../structCO__TPDO__t.html#a42d9798fca6a122bd06391b7d23ec254',1,'CO_TPDO_t::CANdevTx()'],['../structCO__SDOclient__t.html#ab4f18ed8c085ea333ca165e486f4ead3',1,'CO_SDOclient_t::CANdevTx()'],['../structCO__SDOserver__t.html#add469f75cf702d340d069aadfe8ede14',1,'CO_SDOserver_t::CANdevTx()'],['../structCO__SYNC__t.html#a3b89fbc55cce155f500bbda463b61d8a',1,'CO_SYNC_t::CANdevTx()'],['../structCO__TIME__t.html#ac5fb7127ca474b6aa2afa207f01f8cae',1,'CO_TIME_t::CANdevTx()'],['../structCO__GFC__t.html#a202258a9732622f41d338c22a991f1a5',1,'CO_GFC_t::CANdevTx()'],['../structCO__SRDO__t.html#a2fe71edd01cb39629fe42753e84df1fc',1,'CO_SRDO_t::CANdevTx()'],['../structCO__LSSmaster__t.html#a480123707829720cc82b08b65923abcb',1,'CO_LSSmaster_t::CANdevTx()'],['../structCO__LSSslave__t.html#a6ce85337359f0e4cd948c438f2960015',1,'CO_LSSslave_t::CANdevTx()']]], + ['candevtxidx_30',['CANdevTxIdx',['../structCO__EM__t.html#a9a56cba0d9fada8b489884ec766aab04',1,'CO_EM_t::CANdevTxIdx()'],['../structCO__TPDO__t.html#ab401d61f30b73d530df3b8d42015407d',1,'CO_TPDO_t::CANdevTxIdx()'],['../structCO__SDOclient__t.html#a2a44e72381604a972f0e289495a41c37',1,'CO_SDOclient_t::CANdevTxIdx()'],['../structCO__SDOserver__t.html#acbb02ed7ddf534c8f0c41acd25478f47',1,'CO_SDOserver_t::CANdevTxIdx()'],['../structCO__SYNC__t.html#ad07d96af0baa18907d4865ecf244d420',1,'CO_SYNC_t::CANdevTxIdx()'],['../structCO__TIME__t.html#a205dfcde7a830d6a15eda16911e328cd',1,'CO_TIME_t::CANdevTxIdx()'],['../structCO__SRDO__t.html#a0f56f71798c4781bf436d03a0f20938b',1,'CO_SRDO_t::CANdevTxIdx()']]], + ['canerrorstatus_31',['CANerrorStatus',['../structCO__CANmodule__t.html#a5757052c57726cb4fb2ac5cdd8ff744d',1,'CO_CANmodule_t::CANerrorStatus()'],['../structCO__CANinterfaceErrorhandler__t.html#a3f9369fb469c76c66a5e06eb7b909c54',1,'CO_CANinterfaceErrorhandler_t::CANerrorStatus()']]], + ['canerrorstatusold_32',['CANerrorStatusOld',['../structCO__EM__t.html#a2bbed8454995910f4ac54035a0129b1b',1,'CO_EM_t']]], + ['canmodule_33',['CANmodule',['../structCO__t.html#a65bea9a6028917db3b0b3a95a60aea18',1,'CO_t']]], + ['cannormal_34',['CANnormal',['../structCO__CANmodule__t.html#a5555f7d10e09da5815526d7c8b10901d',1,'CO_CANmodule_t']]], + ['canopen_2eh_35',['CANopen.h',['../CANopen_8h.html',1,'']]], + ['canptr_36',['CANptr',['../structCO__CANmodule__t.html#aad1c18f6d47621e5a611333dc57cb349',1,'CO_CANmodule_t']]], + ['canrx_37',['CANrx',['../structCO__t.html#a9665dd795f4c5d26910eb1c5b06503e5',1,'CO_t']]], + ['canrx_5fcallback_38',['CANrx_callback',['../group__CO__CAN__Message__reception.html#ga23168979123a5ca8a87d49307eb2990e',1,'CO_driver.h']]], + ['canrxdata_39',['CANrxData',['../structCO__RPDO__t.html#a514e6d57efc477bdb49cea75e6495a95',1,'CO_RPDO_t::CANrxData()'],['../structCO__SDOclient__t.html#a0bc8e66b7818bca04a4b49ae8210b387',1,'CO_SDOclient_t::CANrxData()'],['../structCO__SDOserver__t.html#abfbff2e51c54be56f0ba090864c7e2f6',1,'CO_SDOserver_t::CANrxData()'],['../structCO__SRDO__t.html#ae6edba4f0446c10b971a90bde01c8a0b',1,'CO_SRDO_t::CANrxData()'],['../structCO__LSSmaster__t.html#a65cc40d755d5b2eaaeb6f1b29332dca8',1,'CO_LSSmaster_t::CANrxData()']]], + ['canrxnew_40',['CANrxNew',['../structCO__HBconsNode__t.html#a4050f7d0406d85db643410cbca65fd14',1,'CO_HBconsNode_t::CANrxNew()'],['../structCO__RPDO__t.html#a4ce980b6cc3a0e497a0138c9c4a69d41',1,'CO_RPDO_t::CANrxNew()'],['../structCO__SDOclient__t.html#a8020d62a62634a531a2efa43ef4534f5',1,'CO_SDOclient_t::CANrxNew()'],['../structCO__SDOserver__t.html#a48e9b1237bc0dd46945762416d09c5bb',1,'CO_SDOserver_t::CANrxNew()'],['../structCO__SYNC__t.html#afa9120621a777413be033005eb43e771',1,'CO_SYNC_t::CANrxNew()'],['../structCO__TIME__t.html#a893e358443b67c38eee33182c496f2a4',1,'CO_TIME_t::CANrxNew()'],['../structCO__SRDO__t.html#acbf5e7096cf72e911a4391ffb8dc939d',1,'CO_SRDO_t::CANrxNew()'],['../structCO__LSSmaster__t.html#ad5fd09faed937c053f1ded1df54f40cd',1,'CO_LSSmaster_t::CANrxNew()']]], + ['canrxtoggle_41',['CANrxToggle',['../structCO__SYNC__t.html#a25aeadeddcae8209b6b18034036a0aaf',1,'CO_SYNC_t']]], + ['cantx_42',['CANtx',['../structCO__t.html#a990b6bd828bc23b455887b5a72f79dca',1,'CO_t']]], + ['cantxbuff_43',['CANtxBuff',['../structCO__EM__t.html#a81e9d98c9384b573a8adefaacec3fdec',1,'CO_EM_t::CANtxBuff()'],['../structCO__TPDO__t.html#a2a2a10d57e1eeab4e0717caa302a6605',1,'CO_TPDO_t::CANtxBuff()'],['../structCO__SDOclient__t.html#a58efad796664487290b52b79cbdb3ae0',1,'CO_SDOclient_t::CANtxBuff()'],['../structCO__SDOserver__t.html#a36ae3c719d96121b95b77f76d2cce723',1,'CO_SDOserver_t::CANtxBuff()'],['../structCO__SYNC__t.html#a79d41eeaf724741d0ed281ab3b6a95ef',1,'CO_SYNC_t::CANtxBuff()'],['../structCO__GFC__t.html#acdfbeaf134252ffcb5dd5bd3dcf3c784',1,'CO_GFC_t::CANtxBuff()'],['../structCO__SRDO__t.html#a693cc1ce40b882e85359291744d68b99',1,'CO_SRDO_t::CANtxBuff()']]], + ['cantxcount_44',['CANtxCount',['../structCO__CANmodule__t.html#a269294aa6a15ba56f92a460fae0536ac',1,'CO_CANmodule_t']]], + ['char_5ft_45',['char_t',['../group__CO__dataTypes.html#ga40bb5262bf908c328fbcfbe5d29d0201',1,'CO_driver.h']]], + ['checkcrc_46',['checkCRC',['../structCO__SRDOGuard__t.html#ad8cd90a85e6e6d551fb04e8fa94feffd',1,'CO_SRDOGuard_t']]], + ['checksum_47',['checksum',['../structCO__SRDO__t.html#adffd373a96a9570b0cbd4487c50d5359',1,'CO_SRDO_t']]], + ['cnt_5fall_5frx_5fmsgs_48',['CNT_ALL_RX_MSGS',['../structCO__t.html#a5b9bea68bd5c63af52819c3c4cdb7592',1,'CO_t']]], + ['cnt_5fall_5ftx_5fmsgs_49',['CNT_ALL_TX_MSGS',['../structCO__t.html#a9e9e7548a182ed9d4aad12e6a41be495',1,'CO_t']]], + ['cnt_5fem_50',['CNT_EM',['../structCO__config__t.html#a515e08f68835f71a6f145be8f27b510a',1,'CO_config_t']]], + ['cnt_5fgfc_51',['CNT_GFC',['../structCO__config__t.html#ae282bab830810b61c0b0c3223654d674',1,'CO_config_t']]], + ['cnt_5fgtwa_52',['CNT_GTWA',['../structCO__config__t.html#a64725014ecce342843f14ffc4b57e2a2',1,'CO_config_t']]], + ['cnt_5fhb_5fcons_53',['CNT_HB_CONS',['../structCO__config__t.html#a0031fc8f80e95f8480c918dbf8289671',1,'CO_config_t']]], + ['cnt_5fleds_54',['CNT_LEDS',['../structCO__config__t.html#a642809cc681792bca855906241d891cc',1,'CO_config_t']]], + ['cnt_5flss_5fmst_55',['CNT_LSS_MST',['../structCO__config__t.html#ac253cae7039090a6c04bc1e385f3ec21',1,'CO_config_t']]], + ['cnt_5flss_5fslv_56',['CNT_LSS_SLV',['../structCO__config__t.html#a00a7a598b946ed13e3af7696e9f92dcc',1,'CO_config_t']]], + ['cnt_5fnmt_57',['CNT_NMT',['../structCO__config__t.html#aeef814580eb5ece5156e63bfc1b490c9',1,'CO_config_t']]], + ['cnt_5frpdo_58',['CNT_RPDO',['../structCO__config__t.html#a7a75302ac077462b67d767b0a11c9f56',1,'CO_config_t']]], + ['cnt_5fsdo_5fcli_59',['CNT_SDO_CLI',['../structCO__config__t.html#a2fc9606643a7fb4d4237f01812d3a6d2',1,'CO_config_t']]], + ['cnt_5fsdo_5fsrv_60',['CNT_SDO_SRV',['../structCO__config__t.html#aac83faf556924515cc2aa8003753ab58',1,'CO_config_t']]], + ['cnt_5fsrdo_61',['CNT_SRDO',['../structCO__config__t.html#ae58a44be57069709af3f6acbd10953e1',1,'CO_config_t']]], + ['cnt_5fsync_62',['CNT_SYNC',['../structCO__config__t.html#af6dbc7d9f31b4cb050e23af8cff3df33',1,'CO_config_t']]], + ['cnt_5ftime_63',['CNT_TIME',['../structCO__config__t.html#ada2a43384a544fa2f235de24a874b1e6',1,'CO_config_t']]], + ['cnt_5ftpdo_64',['CNT_TPDO',['../structCO__config__t.html#a1d830617f50e3235de35a403a1513693',1,'CO_config_t']]], + ['cnt_5ftrace_65',['CNT_TRACE',['../structCO__config__t.html#aaafb8ffff236b51cd6d4ab16426d460f',1,'CO_config_t']]], + ['co_5fcan_5ferr_5fstatus_5ft_66',['CO_CAN_ERR_status_t',['../group__CO__driver.html#ga6c5539afb3a95af43f5477d904607426',1,'CO_driver.h']]], + ['co_5fcan_5ferr_5fwarn_5fpassive_67',['CO_CAN_ERR_WARN_PASSIVE',['../group__CO__driver.html#gga6c5539afb3a95af43f5477d904607426a5ad44f86d5691f2bc809f722364516e0',1,'CO_driver.h']]], + ['co_5fcan_5ferrrx_5foverflow_68',['CO_CAN_ERRRX_OVERFLOW',['../group__CO__driver.html#gga6c5539afb3a95af43f5477d904607426a714a0b9c0978ffde5f138a81880a1fdd',1,'CO_driver.h']]], + ['co_5fcan_5ferrrx_5fpassive_69',['CO_CAN_ERRRX_PASSIVE',['../group__CO__driver.html#gga6c5539afb3a95af43f5477d904607426a51494ad0df1e2de6d9395f1803c4b233',1,'CO_driver.h']]], + ['co_5fcan_5ferrrx_5fwarning_70',['CO_CAN_ERRRX_WARNING',['../group__CO__driver.html#gga6c5539afb3a95af43f5477d904607426a6df88a8a296eb4addc12d9136c0566b0',1,'CO_driver.h']]], + ['co_5fcan_5ferrtx_5fbus_5foff_71',['CO_CAN_ERRTX_BUS_OFF',['../group__CO__driver.html#gga6c5539afb3a95af43f5477d904607426ae58aa7d0eb5d13630f858a3869f0ee7d',1,'CO_driver.h']]], + ['co_5fcan_5ferrtx_5foverflow_72',['CO_CAN_ERRTX_OVERFLOW',['../group__CO__driver.html#gga6c5539afb3a95af43f5477d904607426ad6fba8a27d82774f53812d3a49655d12',1,'CO_driver.h']]], + ['co_5fcan_5ferrtx_5fpassive_73',['CO_CAN_ERRTX_PASSIVE',['../group__CO__driver.html#gga6c5539afb3a95af43f5477d904607426a85d05e861dc03e256dccf977ae16ad6e',1,'CO_driver.h']]], + ['co_5fcan_5ferrtx_5fpdo_5flate_74',['CO_CAN_ERRTX_PDO_LATE',['../group__CO__driver.html#gga6c5539afb3a95af43f5477d904607426ac0b993c7786f41a8c73ad0339124881b',1,'CO_driver.h']]], + ['co_5fcan_5ferrtx_5fwarning_75',['CO_CAN_ERRTX_WARNING',['../group__CO__driver.html#gga6c5539afb3a95af43f5477d904607426a725e4fe057c2f9d222850686a76c724d',1,'CO_driver.h']]], + ['co_5fcan_5fid_5femergency_76',['CO_CAN_ID_EMERGENCY',['../group__CO__driver.html#gga01dd35ae53fd2209ceccabdc8bf8dd06a94ffef8babcef5b807c5f8c865ef7666',1,'CO_driver.h']]], + ['co_5fcan_5fid_5fgfc_77',['CO_CAN_ID_GFC',['../group__CO__driver.html#gga01dd35ae53fd2209ceccabdc8bf8dd06a0ba8a628aa1a873a21820070261c2783',1,'CO_driver.h']]], + ['co_5fcan_5fid_5fheartbeat_78',['CO_CAN_ID_HEARTBEAT',['../group__CO__driver.html#gga01dd35ae53fd2209ceccabdc8bf8dd06a0cfd21623475a1a8522b30b8b16d9874',1,'CO_driver.h']]], + ['co_5fcan_5fid_5flss_5fmst_79',['CO_CAN_ID_LSS_MST',['../group__CO__driver.html#gga01dd35ae53fd2209ceccabdc8bf8dd06ad7d16ed89e513b035104e4b2634ce287',1,'CO_driver.h']]], + ['co_5fcan_5fid_5flss_5fslv_80',['CO_CAN_ID_LSS_SLV',['../group__CO__driver.html#gga01dd35ae53fd2209ceccabdc8bf8dd06a4a62af7fb0b8768e57945a558a0ceee4',1,'CO_driver.h']]], + ['co_5fcan_5fid_5fnmt_5fservice_81',['CO_CAN_ID_NMT_SERVICE',['../group__CO__driver.html#gga01dd35ae53fd2209ceccabdc8bf8dd06a335d0f6204819d267ba396b715f66ead',1,'CO_driver.h']]], + ['co_5fcan_5fid_5frpdo_5f1_82',['CO_CAN_ID_RPDO_1',['../group__CO__driver.html#gga01dd35ae53fd2209ceccabdc8bf8dd06a0ab4be02961987ad817a99a4ef379517',1,'CO_driver.h']]], + ['co_5fcan_5fid_5frpdo_5f2_83',['CO_CAN_ID_RPDO_2',['../group__CO__driver.html#gga01dd35ae53fd2209ceccabdc8bf8dd06a60081a7b09921c6bfce3762a3dd4e49f',1,'CO_driver.h']]], + ['co_5fcan_5fid_5frpdo_5f3_84',['CO_CAN_ID_RPDO_3',['../group__CO__driver.html#gga01dd35ae53fd2209ceccabdc8bf8dd06ad634b89f227db86bc8c633dda327e5fb',1,'CO_driver.h']]], + ['co_5fcan_5fid_5frpdo_5f4_85',['CO_CAN_ID_RPDO_4',['../group__CO__driver.html#gga01dd35ae53fd2209ceccabdc8bf8dd06afec9dfa33a34beef50c434e5cde68c6b',1,'CO_driver.h']]], + ['co_5fcan_5fid_5fsdo_5fcli_86',['CO_CAN_ID_SDO_CLI',['../group__CO__driver.html#gga01dd35ae53fd2209ceccabdc8bf8dd06acfe8640033d9668fafc63aa81d68ede5',1,'CO_driver.h']]], + ['co_5fcan_5fid_5fsdo_5fsrv_87',['CO_CAN_ID_SDO_SRV',['../group__CO__driver.html#gga01dd35ae53fd2209ceccabdc8bf8dd06a3c84d92ad004cfc04e398193b742d30c',1,'CO_driver.h']]], + ['co_5fcan_5fid_5fsrdo_5f1_88',['CO_CAN_ID_SRDO_1',['../group__CO__driver.html#gga01dd35ae53fd2209ceccabdc8bf8dd06aefe4dd6630902d36173b81c106a813bc',1,'CO_driver.h']]], + ['co_5fcan_5fid_5fsync_89',['CO_CAN_ID_SYNC',['../group__CO__driver.html#gga01dd35ae53fd2209ceccabdc8bf8dd06a7a8486aaf2f35eb83c6ca690d0cdce06',1,'CO_driver.h']]], + ['co_5fcan_5fid_5ftime_90',['CO_CAN_ID_TIME',['../group__CO__driver.html#gga01dd35ae53fd2209ceccabdc8bf8dd06ab2e20e54189f5cb565e80b05eb8c4931',1,'CO_driver.h']]], + ['co_5fcan_5fid_5ftpdo_5f1_91',['CO_CAN_ID_TPDO_1',['../group__CO__driver.html#gga01dd35ae53fd2209ceccabdc8bf8dd06a848f2bdb085bc3a342400a6b43c37f82',1,'CO_driver.h']]], + ['co_5fcan_5fid_5ftpdo_5f2_92',['CO_CAN_ID_TPDO_2',['../group__CO__driver.html#gga01dd35ae53fd2209ceccabdc8bf8dd06acb90f0dab2e31982df1bebae6dd02e4b',1,'CO_driver.h']]], + ['co_5fcan_5fid_5ftpdo_5f3_93',['CO_CAN_ID_TPDO_3',['../group__CO__driver.html#gga01dd35ae53fd2209ceccabdc8bf8dd06a83b73730655607582d1dabc8f78f7ca4',1,'CO_driver.h']]], + ['co_5fcan_5fid_5ftpdo_5f4_94',['CO_CAN_ID_TPDO_4',['../group__CO__driver.html#gga01dd35ae53fd2209ceccabdc8bf8dd06abe92c9f7938ad6566e8aa010ab6f5cae',1,'CO_driver.h']]], + ['co_5fcanclearpendingsyncpdos_95',['CO_CANclearPendingSyncPDOs',['../group__CO__driver.html#gabbeac85cbf513162b11fc3d0717b0753',1,'CO_CANclearPendingSyncPDOs(CO_CANmodule_t *CANmodule): CO_driver.c'],['../group__CO__driver.html#gabbeac85cbf513162b11fc3d0717b0753',1,'CO_CANclearPendingSyncPDOs(CO_CANmodule_t *CANmodule): CO_driver.c']]], + ['co_5fcanerror_5fdisable_96',['CO_CANerror_disable',['../group__CO__socketCAN__ERROR.html#ga6a74902a35f8a260cdc3956b83c17c77',1,'CO_CANerror_disable(CO_CANinterfaceErrorhandler_t *CANerrorhandler): CO_error.c'],['../group__CO__socketCAN__ERROR.html#ga6a74902a35f8a260cdc3956b83c17c77',1,'CO_CANerror_disable(CO_CANinterfaceErrorhandler_t *CANerrorhandler): CO_error.c']]], + ['co_5fcanerror_5finit_97',['CO_CANerror_init',['../group__CO__socketCAN__ERROR.html#ga30c3cb98d37aedf45d49643064fee4cd',1,'CO_CANerror_init(CO_CANinterfaceErrorhandler_t *CANerrorhandler, int fd, const char *ifName): CO_error.c'],['../group__CO__socketCAN__ERROR.html#ga30c3cb98d37aedf45d49643064fee4cd',1,'CO_CANerror_init(CO_CANinterfaceErrorhandler_t *CANerrorhandler, int fd, const char *ifname): CO_error.c']]], + ['co_5fcanerror_5flisten_5fonly_98',['CO_CANerror_LISTEN_ONLY',['../group__CO__socketCAN__ERROR.html#ga02f9c19a953d2bd9005033c35c2cb6de',1,'CO_error.h']]], + ['co_5fcanerror_5fnoack_5fmax_99',['CO_CANerror_NOACK_MAX',['../group__CO__socketCAN__ERROR.html#ga85f1fa06be5e9a8337e1ec86546ea72d',1,'CO_error.h']]], + ['co_5fcanerror_5frxmsg_100',['CO_CANerror_rxMsg',['../group__CO__socketCAN__ERROR.html#ga5d5ee1aac31cf0334108cd147b9c9038',1,'CO_CANerror_rxMsg(CO_CANinterfaceErrorhandler_t *CANerrorhandler): CO_error.c'],['../group__CO__socketCAN__ERROR.html#ga5d5ee1aac31cf0334108cd147b9c9038',1,'CO_CANerror_rxMsg(CO_CANinterfaceErrorhandler_t *CANerrorhandler): CO_error.c']]], + ['co_5fcanerror_5frxmsgerror_101',['CO_CANerror_rxMsgError',['../group__CO__socketCAN__ERROR.html#ga975e329593af25c4467f3ddde5cf5f5c',1,'CO_CANerror_rxMsgError(CO_CANinterfaceErrorhandler_t *CANerrorhandler, const struct can_frame *msg): CO_error.c'],['../group__CO__socketCAN__ERROR.html#ga975e329593af25c4467f3ddde5cf5f5c',1,'CO_CANerror_rxMsgError(CO_CANinterfaceErrorhandler_t *CANerrorhandler, const struct can_frame *msg): CO_error.c']]], + ['co_5fcanerror_5ftxmsg_102',['CO_CANerror_txMsg',['../group__CO__socketCAN__ERROR.html#gac40e1c681a9721c91ed75c49afda913c',1,'CO_CANerror_txMsg(CO_CANinterfaceErrorhandler_t *CANerrorhandler): CO_error.c'],['../group__CO__socketCAN__ERROR.html#gac40e1c681a9721c91ed75c49afda913c',1,'CO_CANerror_txMsg(CO_CANinterfaceErrorhandler_t *CANerrorhandler): CO_error.c']]], + ['co_5fcaninit_103',['CO_CANinit',['../group__CO__CANopen.html#ga619d9ee70c17464bb819b48b5eddb074',1,'CANopen.h']]], + ['co_5fcaninterfaceerrorhandler_5ft_104',['CO_CANinterfaceErrorhandler_t',['../structCO__CANinterfaceErrorhandler__t.html',1,'']]], + ['co_5fcaninterfacestate_5ft_105',['CO_CANinterfaceState_t',['../group__CO__socketCAN__ERROR.html#ga7bf6a56766c008531d32b4218a5a9061',1,'CO_error.h']]], + ['co_5fcanmodule_5faddinterface_106',['CO_CANmodule_addInterface',['../group__CO__socketCAN__driver__target.html#gaaffa26125993a7a1f6cbfdb468b59333',1,'CO_driver_target.h']]], + ['co_5fcanmodule_5fdisable_107',['CO_CANmodule_disable',['../group__CO__driver.html#gac6f60f9da27dda0c9b3950c4e96bd687',1,'CO_CANmodule_disable(CO_CANmodule_t *CANmodule): CO_driver.c'],['../group__CO__driver.html#gac6f60f9da27dda0c9b3950c4e96bd687',1,'CO_CANmodule_disable(CO_CANmodule_t *CANmodule): CO_driver.c']]], + ['co_5fcanmodule_5finit_108',['CO_CANmodule_init',['../group__CO__driver.html#ga3a1131813110529494cee5e27c0bf28d',1,'CO_CANmodule_init(CO_CANmodule_t *CANmodule, void *CANptr, CO_CANrx_t rxArray[], uint16_t rxSize, CO_CANtx_t txArray[], uint16_t txSize, uint16_t CANbitRate): CO_driver.c'],['../group__CO__driver.html#ga3a1131813110529494cee5e27c0bf28d',1,'CO_CANmodule_init(CO_CANmodule_t *CANmodule, void *CANptr, CO_CANrx_t rxArray[], uint16_t rxSize, CO_CANtx_t txArray[], uint16_t txSize, uint16_t CANbitRate): CO_driver.c']]], + ['co_5fcanmodule_5fprocess_109',['CO_CANmodule_process',['../group__CO__driver.html#ga066c6742f75b2daac585735ffd6c9928',1,'CO_CANmodule_process(CO_CANmodule_t *CANmodule): CO_driver.c'],['../group__CO__driver.html#ga066c6742f75b2daac585735ffd6c9928',1,'CO_CANmodule_process(CO_CANmodule_t *CANmodule): CO_driver.c']]], + ['co_5fcanmodule_5ft_110',['CO_CANmodule_t',['../structCO__CANmodule__t.html',1,'']]], + ['canopen_111',['CANopen',['../group__CO__CANopen.html',1,'']]], + ['canopen_5f301_112',['CANopen_301',['../group__CO__CANopen__301.html',1,'']]], + ['canopen_5f303_113',['CANopen_303',['../group__CO__CANopen__303.html',1,'']]], + ['canopen_5f304_114',['CANopen_304',['../group__CO__CANopen__304.html',1,'']]], + ['canopen_5f305_115',['CANopen_305',['../group__CO__CANopen__305.html',1,'']]], + ['canopen_5f309_116',['CANopen_309',['../group__CO__CANopen__309.html',1,'']]], + ['command_20syntax_117',['Command syntax',['../group__CO__CANopen__309__3__Syntax.html',1,'']]], + ['canopen_5fextra_118',['CANopen_extra',['../group__CO__CANopen__extra.html',1,'']]], + ['co_5fcanopeninit_119',['CO_CANopenInit',['../group__CO__CANopen.html#ga0b64a860299af6e96f5663419aa6d446',1,'CANopen.h']]], + ['co_5fcanrx_5ft_120',['CO_CANrx_t',['../structCO__CANrx__t.html',1,'']]], + ['co_5fcanrxbuffer_5fgetinterface_121',['CO_CANrxBuffer_getInterface',['../group__CO__socketCAN__driver__target.html#gaf266a58e21acf104d9f19cd0da704afe',1,'CO_driver_target.h']]], + ['co_5fcanrxbufferinit_122',['CO_CANrxBufferInit',['../group__CO__driver.html#ga25ee22cd2e3a2f3bb49990e8bc3076b0',1,'CO_CANrxBufferInit(CO_CANmodule_t *CANmodule, uint16_t index, uint16_t ident, uint16_t mask, bool_t rtr, void *object, void(*CANrx_callback)(void *object, void *message)): CO_driver.c'],['../group__CO__driver.html#ga25ee22cd2e3a2f3bb49990e8bc3076b0',1,'CO_CANrxBufferInit(CO_CANmodule_t *CANmodule, uint16_t index, uint16_t ident, uint16_t mask, bool_t rtr, void *object, void(*CANrx_callback)(void *object, void *message)): CO_driver.c']]], + ['co_5fcanrxfromepoll_123',['CO_CANrxFromEpoll',['../group__CO__socketCAN__driver__target.html#ga072c53260a32d7cd30d9ad1b5bb2c359',1,'CO_CANrxFromEpoll(CO_CANmodule_t *CANmodule, struct epoll_event *ev, CO_CANrxMsg_t *buffer, int32_t *msgIndex): CO_driver.c'],['../group__CO__socketCAN__driver__target.html#ga072c53260a32d7cd30d9ad1b5bb2c359',1,'CO_CANrxFromEpoll(CO_CANmodule_t *CANmodule, struct epoll_event *ev, CO_CANrxMsg_t *buffer, int32_t *msgIndex): CO_driver.c']]], + ['co_5fcanrxmsg_5freaddata_124',['CO_CANrxMsg_readData',['../group__CO__CAN__Message__reception.html#gab02a5fe910acf9aa5021f97e523697f9',1,'CO_driver.h']]], + ['co_5fcanrxmsg_5freaddlc_125',['CO_CANrxMsg_readDLC',['../group__CO__CAN__Message__reception.html#gacec1dcf8b7e66ea2e65905f91321b299',1,'CO_driver.h']]], + ['co_5fcanrxmsg_5freadident_126',['CO_CANrxMsg_readIdent',['../group__CO__CAN__Message__reception.html#ga018e9159b92165b2506a6673113cdc0e',1,'CO_driver.h']]], + ['co_5fcansend_127',['CO_CANsend',['../group__CO__driver.html#ga4664a9f5d547cb0605a9e929fb079f2e',1,'CO_CANsend(CO_CANmodule_t *CANmodule, CO_CANtx_t *buffer): CO_driver.c'],['../group__CO__driver.html#ga4664a9f5d547cb0605a9e929fb079f2e',1,'CO_CANsend(CO_CANmodule_t *CANmodule, CO_CANtx_t *buffer): CO_driver.c']]], + ['co_5fcansetconfigurationmode_128',['CO_CANsetConfigurationMode',['../group__CO__driver.html#ga88ef52baae169a80e4c2f2cb93b33747',1,'CO_CANsetConfigurationMode(void *CANptr): CO_driver.c'],['../group__CO__driver.html#ga88ef52baae169a80e4c2f2cb93b33747',1,'CO_CANsetConfigurationMode(void *CANptr): CO_driver.c']]], + ['co_5fcansetnormalmode_129',['CO_CANsetNormalMode',['../group__CO__driver.html#gad654edfa651bf7b68263913786697200',1,'CO_CANsetNormalMode(CO_CANmodule_t *CANmodule): CO_driver.c'],['../group__CO__driver.html#gad654edfa651bf7b68263913786697200',1,'CO_CANsetNormalMode(CO_CANmodule_t *CANmodule): CO_driver.c']]], + ['co_5fcantx_5ft_130',['CO_CANtx_t',['../structCO__CANtx__t.html',1,'']]], + ['co_5fcantxbuffer_5fsetinterface_131',['CO_CANtxBuffer_setInterface',['../group__CO__socketCAN__driver__target.html#ga0d7a8fdf7a2fafd4c6d2f2dd1e1017b0',1,'CO_driver_target.h']]], + ['co_5fcantxbufferinit_132',['CO_CANtxBufferInit',['../group__CO__driver.html#ga01e2ee79e7e3a8b321dac831e7e00976',1,'CO_CANtxBufferInit(CO_CANmodule_t *CANmodule, uint16_t index, uint16_t ident, bool_t rtr, uint8_t noOfBytes, bool_t syncFlag): CO_driver.c'],['../group__CO__driver.html#ga01e2ee79e7e3a8b321dac831e7e00976',1,'CO_CANtxBufferInit(CO_CANmodule_t *CANmodule, uint16_t index, uint16_t ident, bool_t rtr, uint8_t noOfBytes, bool_t syncFlag): CO_driver.c']]], + ['co_5fcommandinterface_5ft_133',['CO_commandInterface_t',['../group__CO__epoll__interface.html#gad5a4218d5775fd7cda81a8015e1ee6f0',1,'CO_epoll_interface.h']]], + ['co_5fconfig_2eh_134',['CO_config.h',['../CO__config_8h.html',1,'']]], + ['co_5fconfig_5fcrc16_135',['CO_CONFIG_CRC16',['../group__CO__STACK__CONFIG__CRC16.html#ga15737bc0ede4bcd56968e5f96b2e8c9b',1,'CO_config.h']]], + ['co_5fconfig_5fdebug_136',['CO_CONFIG_DEBUG',['../group__CO__STACK__CONFIG__DEBUG.html#ga1ce7b96c60a5ab9349b66b77f6a6e2a7',1,'CO_config.h']]], + ['co_5fconfig_5fem_137',['CO_CONFIG_EM',['../group__CO__STACK__CONFIG__EMERGENCY.html#ga16aa1479ffd52a627d1053c20f844b62',1,'CO_config.h']]], + ['co_5fconfig_5fem_5fbuffer_5fsize_138',['CO_CONFIG_EM_BUFFER_SIZE',['../group__CO__STACK__CONFIG__EMERGENCY.html#ga3c35cf4947c82a0b15afdbfa43a10d67',1,'CO_config.h']]], + ['co_5fconfig_5fem_5ferr_5fstatus_5fbits_5fcount_139',['CO_CONFIG_EM_ERR_STATUS_BITS_COUNT',['../group__CO__STACK__CONFIG__EMERGENCY.html#gab87776d4802748671b234112263760af',1,'CO_config.h']]], + ['co_5fconfig_5ferr_5fcondition_5fcommunication_140',['CO_CONFIG_ERR_CONDITION_COMMUNICATION',['../group__CO__STACK__CONFIG__EMERGENCY.html#gae47daba892331857e65df82272ed4152',1,'CO_config.h']]], + ['co_5fconfig_5ferr_5fcondition_5fcurrent_141',['CO_CONFIG_ERR_CONDITION_CURRENT',['../group__CO__STACK__CONFIG__EMERGENCY.html#ga63af1aaa73297df53b555cb89cd0c07f',1,'CO_config.h']]], + ['co_5fconfig_5ferr_5fcondition_5fdev_5fprofile_142',['CO_CONFIG_ERR_CONDITION_DEV_PROFILE',['../group__CO__STACK__CONFIG__EMERGENCY.html#gaec2f6161e439dba5376808dcb0cdc36a',1,'CO_config.h']]], + ['co_5fconfig_5ferr_5fcondition_5fgeneric_143',['CO_CONFIG_ERR_CONDITION_GENERIC',['../group__CO__STACK__CONFIG__EMERGENCY.html#gad6270eb7887b22c0365c304d7cf2c633',1,'CO_config.h']]], + ['co_5fconfig_5ferr_5fcondition_5fmanufacturer_144',['CO_CONFIG_ERR_CONDITION_MANUFACTURER',['../group__CO__STACK__CONFIG__EMERGENCY.html#ga3717ce44b5db3189757d874f440adce1',1,'CO_config.h']]], + ['co_5fconfig_5ferr_5fcondition_5ftemperature_145',['CO_CONFIG_ERR_CONDITION_TEMPERATURE',['../group__CO__STACK__CONFIG__EMERGENCY.html#gaeb96443d9ea2142c346638612fd5c717',1,'CO_config.h']]], + ['co_5fconfig_5ferr_5fcondition_5fvoltage_146',['CO_CONFIG_ERR_CONDITION_VOLTAGE',['../group__CO__STACK__CONFIG__EMERGENCY.html#ga2b1c3c4f106a8a5d7efda475b469a727',1,'CO_config.h']]], + ['co_5fconfig_5ffifo_147',['CO_CONFIG_FIFO',['../group__CO__STACK__CONFIG__FIFO.html#ga055654eb6f93ba05e3534b31626eec3a',1,'CO_config.h']]], + ['co_5fconfig_5fflag_5fcallback_5fpre_148',['CO_CONFIG_FLAG_CALLBACK_PRE',['../group__CO__STACK__CONFIG__COMMON.html#gab55099df45bed12f182ef7c0c779dc14',1,'CO_config.h']]], + ['co_5fconfig_5fflag_5fod_5fdynamic_149',['CO_CONFIG_FLAG_OD_DYNAMIC',['../group__CO__STACK__CONFIG__COMMON.html#gaf0f46ccffdd156cc7c2d8774ecb2060d',1,'CO_config.h']]], + ['co_5fconfig_5fflag_5ftimernext_150',['CO_CONFIG_FLAG_TIMERNEXT',['../group__CO__STACK__CONFIG__COMMON.html#ga9e84c3a9256f15246be7766a61096c2d',1,'CO_config.h']]], + ['co_5fconfig_5fgfc_151',['CO_CONFIG_GFC',['../group__CO__STACK__CONFIG__SRDO.html#ga71d11e8460a5410be21863a0f99cbab2',1,'CO_config.h']]], + ['co_5fconfig_5fgtw_152',['CO_CONFIG_GTW',['../group__CO__STACK__CONFIG__GATEWAY.html#ga9af15f76cd14fece499764499c6bc2d3',1,'CO_config.h']]], + ['co_5fconfig_5fgtw_5fblock_5fdl_5floop_153',['CO_CONFIG_GTW_BLOCK_DL_LOOP',['../group__CO__STACK__CONFIG__GATEWAY.html#gaa864e7c6e7ebd3fc7ce424dc3c94db9d',1,'CO_config.h']]], + ['co_5fconfig_5fgtwa_5fcomm_5fbuf_5fsize_154',['CO_CONFIG_GTWA_COMM_BUF_SIZE',['../group__CO__STACK__CONFIG__GATEWAY.html#ga7903ae4ca7939fc32bd747224e868a38',1,'CO_config.h']]], + ['co_5fconfig_5fgtwa_5flog_5fbuf_5fsize_155',['CO_CONFIG_GTWA_LOG_BUF_SIZE',['../group__CO__STACK__CONFIG__GATEWAY.html#ga4f471dca1341879dc56c2e0a2c73cb29',1,'CO_config.h']]], + ['co_5fconfig_5fhb_5fcons_156',['CO_CONFIG_HB_CONS',['../group__CO__STACK__CONFIG__NMT__HB.html#ga7368d68cb039983bc8cc164410877098',1,'CO_config.h']]], + ['co_5fconfig_5fhb_5fcons_5fsize_157',['CO_CONFIG_HB_CONS_SIZE',['../group__CO__STACK__CONFIG__NMT__HB.html#ga0cbe9ab929ff9d122ab6727d66fe7752',1,'CO_config.h']]], + ['co_5fconfig_5fleds_158',['CO_CONFIG_LEDS',['../group__CO__STACK__CONFIG__LEDS.html#ga423160131d618b5d57bc7c016ee369fd',1,'CO_config.h']]], + ['co_5fconfig_5flss_159',['CO_CONFIG_LSS',['../group__CO__STACK__CONFIG__LSS.html#gafeb75d750efb0879fe11a5482b6629f3',1,'CO_config.h']]], + ['co_5fconfig_5fnmt_160',['CO_CONFIG_NMT',['../group__CO__STACK__CONFIG__NMT__HB.html#gafa3b1f1b4931175bf9c67a5d45633e76',1,'CO_config.h']]], + ['co_5fconfig_5fpdo_161',['CO_CONFIG_PDO',['../group__CO__STACK__CONFIG__SYNC__PDO.html#gaa20d1b49249b7f5a15963cc1a4611be9',1,'CO_config.h']]], + ['co_5fconfig_5fsdo_5fcli_162',['CO_CONFIG_SDO_CLI',['../group__CO__STACK__CONFIG__SDO.html#gac8ee65cd62dbee2982f5304513402a57',1,'CO_config.h']]], + ['co_5fconfig_5fsdo_5fcli_5fbuffer_5fsize_163',['CO_CONFIG_SDO_CLI_BUFFER_SIZE',['../group__CO__STACK__CONFIG__SDO.html#ga763b09ab827365e46f10234bd9c0acfd',1,'CO_config.h']]], + ['co_5fconfig_5fsdo_5fsrv_164',['CO_CONFIG_SDO_SRV',['../group__CO__STACK__CONFIG__SDO.html#ga2928cc23dd27138821d48c2fb3e24222',1,'CO_config.h']]], + ['co_5fconfig_5fsdo_5fsrv_5fbuffer_5fsize_165',['CO_CONFIG_SDO_SRV_BUFFER_SIZE',['../group__CO__STACK__CONFIG__SDO.html#gacad3d0d9060469aedcb9e058c1883375',1,'CO_config.h']]], + ['co_5fconfig_5fsrdo_166',['CO_CONFIG_SRDO',['../group__CO__STACK__CONFIG__SRDO.html#ga61645e6ad8a02e356abde012434bedf9',1,'CO_config.h']]], + ['co_5fconfig_5fsrdo_5fminimum_5fdelay_167',['CO_CONFIG_SRDO_MINIMUM_DELAY',['../group__CO__STACK__CONFIG__SRDO.html#gaebb5427a155133b50622e60acdd0e650',1,'CO_config.h']]], + ['co_5fconfig_5fsync_168',['CO_CONFIG_SYNC',['../group__CO__STACK__CONFIG__SYNC__PDO.html#ga7d1d2210fdf2b916ca1d82c4933856bc',1,'CO_config.h']]], + ['co_5fconfig_5ft_169',['CO_config_t',['../structCO__config__t.html',1,'']]], + ['co_5fconfig_5ftime_170',['CO_CONFIG_TIME',['../group__CO__STACK__CONFIG__TIME.html#gaba4a59929bbd8512ca954ba8fcf1dfe6',1,'CO_config.h']]], + ['co_5fconfig_5ftrace_171',['CO_CONFIG_TRACE',['../group__CO__STACK__CONFIG__TRACE.html#ga9d4e333d0b599c2369366defc6ce5e62',1,'CO_config.h']]], + ['crc_2016_20ccitt_172',['CRC 16 CCITT',['../group__CO__crc16__ccitt.html',1,'']]], + ['critical_20sections_173',['Critical sections',['../group__CO__critical__sections.html',1,'']]], + ['co_5fdefault_5fcan_5fid_5ft_174',['CO_Default_CAN_ID_t',['../group__CO__driver.html#ga01dd35ae53fd2209ceccabdc8bf8dd06',1,'CO_driver.h']]], + ['co_5fdelete_175',['CO_delete',['../group__CO__CANopen.html#ga7023592c26cf6649aa67bc2c04dfd95d',1,'CANopen.h']]], + ['co_5fdriver_2eh_176',['CO_driver.h',['../CO__driver_8h.html',1,'']]], + ['co_5fdriver_5ferror_5freporting_177',['CO_DRIVER_ERROR_REPORTING',['../group__CO__socketCAN__driver__target.html#ga88077a1ecc6ae53de0b40ae092d48452',1,'CO_driver_target.h']]], + ['co_5fdriver_5fmulti_5finterface_178',['CO_DRIVER_MULTI_INTERFACE',['../group__CO__socketCAN__driver__target.html#ga63d0056e7f18144c6eee7b66f196377c',1,'CO_driver_target.h']]], + ['co_5fdriver_5ftarget_2eh_179',['CO_driver_target.h',['../CO__driver__target_8h.html',1,'']]], + ['co_5fem_5f0b_5funused_180',['CO_EM_0B_unused',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca8bf6fb0db21e29e477b38304279bed5e',1,'CO_Emergency.h']]], + ['co_5fem_5f0c_5funused_181',['CO_EM_0C_unused',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeaccaa724f4fdeff7043b0d4f454613a96992',1,'CO_Emergency.h']]], + ['co_5fem_5f0d_5funused_182',['CO_EM_0D_unused',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca933c070fd08c1223462a3a331b016c99',1,'CO_Emergency.h']]], + ['co_5fem_5f0e_5funused_183',['CO_EM_0E_unused',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca0cabb2e45202f938cfdafe8e7871f4f7',1,'CO_Emergency.h']]], + ['co_5fem_5f0f_5funused_184',['CO_EM_0F_unused',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeaccac6469cc3e6176136f69e549c4a4f5b71',1,'CO_Emergency.h']]], + ['co_5fem_5f10_5funused_185',['CO_EM_10_unused',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca274f46ac0760c4c340f48d1de884f2fe',1,'CO_Emergency.h']]], + ['co_5fem_5f11_5funused_186',['CO_EM_11_unused',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca9d8abe2f426ed071febf85a932c1df98',1,'CO_Emergency.h']]], + ['co_5fem_5f16_5funused_187',['CO_EM_16_unused',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca865160ae9fdac8fcba1e5335b31c2f9f',1,'CO_Emergency.h']]], + ['co_5fem_5f17_5funused_188',['CO_EM_17_unused',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeaccad5bb584bb3c85ca0ba0313367aa75a9b',1,'CO_Emergency.h']]], + ['co_5fem_5f1d_5funused_189',['CO_EM_1D_unused',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca6d0bf9c926241ec8f67c477928300761',1,'CO_Emergency.h']]], + ['co_5fem_5f1e_5funused_190',['CO_EM_1E_unused',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeaccab2f3562c4e1f8e25a7837627dc1721db',1,'CO_Emergency.h']]], + ['co_5fem_5f1f_5funused_191',['CO_EM_1F_unused',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeaccadba4afb9dac78f8eb0c5f494926568b1',1,'CO_Emergency.h']]], + ['co_5fem_5f21_5funused_192',['CO_EM_21_unused',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeaccadbce7bd9d5a0ee681104914092b21d8d',1,'CO_Emergency.h']]], + ['co_5fem_5f23_5funused_193',['CO_EM_23_unused',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeaccaa0c8857afdd8455b30fd0179e98599fb',1,'CO_Emergency.h']]], + ['co_5fem_5f24_5funused_194',['CO_EM_24_unused',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca9cf88b48355b3cc43fe9a8360b8470df',1,'CO_Emergency.h']]], + ['co_5fem_5f25_5funused_195',['CO_EM_25_unused',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca0a8abc6fcd7b0d5469b469c2cf370a82',1,'CO_Emergency.h']]], + ['co_5fem_5f26_5funused_196',['CO_EM_26_unused',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca0398195eafec5f8d60a76f677ce2a714',1,'CO_Emergency.h']]], + ['co_5fem_5f27_5funused_197',['CO_EM_27_unused',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca95ca6848349affc579fff2c2a62e87d7',1,'CO_Emergency.h']]], + ['co_5fem_5fcalculation_5fof_5fparameters_198',['CO_EM_CALCULATION_OF_PARAMETERS',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca5544a90d3047bc08186ea7412528dc93',1,'CO_Emergency.h']]], + ['co_5fem_5fcan_5fbus_5fwarning_199',['CO_EM_CAN_BUS_WARNING',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca27ebb7f155d4b72618c34dd6aa496aac',1,'CO_Emergency.h']]], + ['co_5fem_5fcan_5frx_5fbus_5fpassive_200',['CO_EM_CAN_RX_BUS_PASSIVE',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeaccaab5efa11cefb2cd6125cec3ec1c570e1',1,'CO_Emergency.h']]], + ['co_5fem_5fcan_5frxb_5foverflow_201',['CO_EM_CAN_RXB_OVERFLOW',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeaccaf9a86c6c3b87763593dd14be6b0bef29',1,'CO_Emergency.h']]], + ['co_5fem_5fcan_5ftx_5fbus_5foff_202',['CO_EM_CAN_TX_BUS_OFF',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeaccae59f8e20795915a0929861809ed42e7c',1,'CO_Emergency.h']]], + ['co_5fem_5fcan_5ftx_5fbus_5fpassive_203',['CO_EM_CAN_TX_BUS_PASSIVE',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeaccadb8502da626d80a8c423e94e1c76d0cb',1,'CO_Emergency.h']]], + ['co_5fem_5fcan_5ftx_5foverflow_204',['CO_EM_CAN_TX_OVERFLOW',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca2dbceee7b6deae231bb40a96f8f748a9',1,'CO_Emergency.h']]], + ['co_5fem_5femergency_5fbuffer_5ffull_205',['CO_EM_EMERGENCY_BUFFER_FULL',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeaccabd1935c51679f70f509ffd60e28c02b1',1,'CO_Emergency.h']]], + ['co_5fem_5ferrorcode_5ft_206',['CO_EM_errorCode_t',['../group__CO__Emergency.html#ga0653c307fd6bc5238babf48e01c9fa02',1,'CO_Emergency.h']]], + ['co_5fem_5ferrorstatusbits_5ft_207',['CO_EM_errorStatusBits_t',['../group__CO__Emergency.html#ga587034df9d350c8e121c253f1d4eeacc',1,'CO_Emergency.h']]], + ['co_5fem_5fgeneric_5ferror_208',['CO_EM_GENERIC_ERROR',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca21648a2863590d3cccb469f8ef759267',1,'CO_Emergency.h']]], + ['co_5fem_5fgeneric_5fsoftware_5ferror_209',['CO_EM_GENERIC_SOFTWARE_ERROR',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca6c3e7fff310443f05815ea2b7ac6b289',1,'CO_Emergency.h']]], + ['co_5fem_5fhb_5fconsumer_5fremote_5freset_210',['CO_EM_HB_CONSUMER_REMOTE_RESET',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca0b6698662476cc622661fb5a5a75ec31',1,'CO_Emergency.h']]], + ['co_5fem_5fheartbeat_5fconsumer_211',['CO_EM_HEARTBEAT_CONSUMER',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca6478d414ea45f6a9129e68a9d57e11b7',1,'CO_Emergency.h']]], + ['co_5fem_5finconsistent_5fobject_5fdict_212',['CO_EM_INCONSISTENT_OBJECT_DICT',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca2c6a108cabca6f03b1400065f2ad4887',1,'CO_Emergency.h']]], + ['co_5fem_5finit_213',['CO_EM_init',['../group__CO__Emergency.html#ga5b80f59df00b71dca7a5c18c139aa71e',1,'CO_Emergency.h']]], + ['co_5fem_5finitcallbackpre_214',['CO_EM_initCallbackPre',['../group__CO__Emergency.html#ga94efd78032de3667e2a89780b08aabed',1,'CO_Emergency.h']]], + ['co_5fem_5finitcallbackrx_215',['CO_EM_initCallbackRx',['../group__CO__Emergency.html#ga583245c954327c3cf7f9fdb97854e76b',1,'CO_Emergency.h']]], + ['co_5fem_5fisr_5ftimer_5foverflow_216',['CO_EM_ISR_TIMER_OVERFLOW',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeaccadbf7382f537c9f59f965ce38be464e46',1,'CO_Emergency.h']]], + ['co_5fem_5fmanufacturer_5fend_217',['CO_EM_MANUFACTURER_END',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca5d3c6fdb77551b3f4aaf993ae1dfb414',1,'CO_Emergency.h']]], + ['co_5fem_5fmanufacturer_5fstart_218',['CO_EM_MANUFACTURER_START',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeaccaf850a661aadde65b10b22715cf24942c',1,'CO_Emergency.h']]], + ['co_5fem_5fmemory_5fallocation_5ferror_219',['CO_EM_MEMORY_ALLOCATION_ERROR',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca2575dac21ff9ac9c4c4e5ca63d34fdbc',1,'CO_Emergency.h']]], + ['co_5fem_5fmicrocontroller_5freset_220',['CO_EM_MICROCONTROLLER_RESET',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeaccacb69eecc08e72c56aec215fa55e27e16',1,'CO_Emergency.h']]], + ['co_5fem_5fnmt_5fwrong_5fcommand_221',['CO_EM_NMT_WRONG_COMMAND',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeaccac5f82aeeda52c83eee0025c8b387ac5d',1,'CO_Emergency.h']]], + ['co_5fem_5fno_5ferror_222',['CO_EM_NO_ERROR',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeaccafb7b06b4b1d4fb2f9fa8661fdbaf8b01',1,'CO_Emergency.h']]], + ['co_5fem_5fnon_5fvolatile_5fmemory_223',['CO_EM_NON_VOLATILE_MEMORY',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeaccac019532cacaa8310f9ca413a2f599af3',1,'CO_Emergency.h']]], + ['co_5fem_5fpdo_5fwrong_5fmapping_224',['CO_EM_PDO_WRONG_MAPPING',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca7308b487766b8feca60ef0c1b873f167',1,'CO_Emergency.h']]], + ['co_5fem_5fprocess_225',['CO_EM_process',['../group__CO__Emergency.html#ga93ae7be6ef966192f5761ce343345d3b',1,'CO_EM_process(CO_EM_t *em, bool_t NMTisPreOrOperational, uint32_t timeDifference_us, uint32_t *timerNext_us): CO_Emergency.c'],['../group__CO__Emergency.html#ga93ae7be6ef966192f5761ce343345d3b',1,'CO_EM_process(CO_EM_t *em, bool_t NMTisPreOrOperational, uint32_t timeDifference_us, uint32_t *timerNext_us): CO_Emergency.c']]], + ['co_5fem_5frpdo_5foverflow_226',['CO_EM_RPDO_OVERFLOW',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca73426de91d49273d023b5084a0cea8e0',1,'CO_Emergency.h']]], + ['co_5fem_5frpdo_5fwrong_5flength_227',['CO_EM_RPDO_WRONG_LENGTH',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca2a36480d4dd6a24f1f8bb66d79441a8d',1,'CO_Emergency.h']]], + ['co_5fem_5frxmsg_5foverflow_228',['CO_EM_RXMSG_OVERFLOW',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca0b17027ee0097065d92e6c0981e3face',1,'CO_Emergency.h']]], + ['co_5fem_5frxmsg_5fwrong_5flength_229',['CO_EM_RXMSG_WRONG_LENGTH',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeaccae1e45de61059459a6f1f6e500962f287',1,'CO_Emergency.h']]], + ['co_5fem_5fsync_5flength_230',['CO_EM_SYNC_LENGTH',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca09a521bfc9ea08ed340cfa29952a471c',1,'CO_Emergency.h']]], + ['co_5fem_5fsync_5ftime_5fout_231',['CO_EM_SYNC_TIME_OUT',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeaccafd760392f4d4d6358896486c5b5d7d82',1,'CO_Emergency.h']]], + ['co_5fem_5ft_232',['CO_EM_t',['../structCO__EM__t.html',1,'']]], + ['co_5fem_5ftime_5flength_233',['CO_EM_TIME_LENGTH',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca3af179820ed2aa88e2c22b7961de71f8',1,'CO_Emergency.h']]], + ['co_5fem_5ftime_5ftimeout_234',['CO_EM_TIME_TIMEOUT',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca5c1a6209ebe6167bbf13f565b6fd994d',1,'CO_Emergency.h']]], + ['co_5fem_5ftpdo_5foutside_5fwindow_235',['CO_EM_TPDO_OUTSIDE_WINDOW',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeaccaea141284cd85126a9b3e7b0605a26a94',1,'CO_Emergency.h']]], + ['co_5fem_5fwrong_5ferror_5freport_236',['CO_EM_WRONG_ERROR_REPORT',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca2d7776243205bc75e6c448e13e697480',1,'CO_Emergency.h']]], + ['co_5femc401_5fin_5fvolt_5fhi_237',['CO_EMC401_IN_VOLT_HI',['../group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02aacdc3517e800b037b46c1b54f454b562',1,'CO_Emergency.h']]], + ['co_5femc401_5fin_5fvolt_5flow_238',['CO_EMC401_IN_VOLT_LOW',['../group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a867eb16fce01ade3c728df7c7527e311',1,'CO_Emergency.h']]], + ['co_5femc401_5fintern_5fvolt_5fhi_239',['CO_EMC401_INTERN_VOLT_HI',['../group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a9cd0f1a897f40a3a43198ba05de4a11b',1,'CO_Emergency.h']]], + ['co_5femc401_5fintern_5fvolt_5flo_240',['CO_EMC401_INTERN_VOLT_LO',['../group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02ac122203ed5c6a71749ace599b13ac594',1,'CO_Emergency.h']]], + ['co_5femc401_5fout_5fcur_5fhi_241',['CO_EMC401_OUT_CUR_HI',['../group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02aa432d9c66bb0f6eecc38d720cae6c32e',1,'CO_Emergency.h']]], + ['co_5femc401_5fout_5fload_5fdump_242',['CO_EMC401_OUT_LOAD_DUMP',['../group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a21cd31a1455c9dc379796798f0eecd32',1,'CO_Emergency.h']]], + ['co_5femc401_5fout_5fshorted_243',['CO_EMC401_OUT_SHORTED',['../group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a182a0c7afc0cb1c30af42a05430da353',1,'CO_Emergency.h']]], + ['co_5femc401_5fout_5fvolt_5fhigh_244',['CO_EMC401_OUT_VOLT_HIGH',['../group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02abf5b6a68120351c2fa52146b45798ed2',1,'CO_Emergency.h']]], + ['co_5femc401_5fout_5fvolt_5flow_245',['CO_EMC401_OUT_VOLT_LOW',['../group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a5f262e622db7482b7230055e5b27c902',1,'CO_Emergency.h']]], + ['co_5femc_5fadditional_5ffunc_246',['CO_EMC_ADDITIONAL_FUNC',['../group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02af5f9446049023ceae559562998172278',1,'CO_Emergency.h']]], + ['co_5femc_5fadditional_5fmodul_247',['CO_EMC_ADDITIONAL_MODUL',['../group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02ae210dc1069c7b046527f7d7903ef82cb',1,'CO_Emergency.h']]], + ['co_5femc_5fbus_5foff_5frecovered_248',['CO_EMC_BUS_OFF_RECOVERED',['../group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a2fd717ed311007b4dd6fe92443f134b0',1,'CO_Emergency.h']]], + ['co_5femc_5fcan_5fid_5fcollision_249',['CO_EMC_CAN_ID_COLLISION',['../group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a683bff5350b0cbab24aef2fc8eac363a',1,'CO_Emergency.h']]], + ['co_5femc_5fcan_5foverrun_250',['CO_EMC_CAN_OVERRUN',['../group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a1f04b4ffe9cc1d8f2b294261909dec4e',1,'CO_Emergency.h']]], + ['co_5femc_5fcan_5fpassive_251',['CO_EMC_CAN_PASSIVE',['../group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02aa024c00c21f705474355b9ca7d7ce948',1,'CO_Emergency.h']]], + ['co_5femc_5fcommunication_252',['CO_EMC_COMMUNICATION',['../group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02aab2946bf800f551bcae55dd299ff315b',1,'CO_Emergency.h']]], + ['co_5femc_5fcurrent_253',['CO_EMC_CURRENT',['../group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02abad9ca04a37cc43cacabfef9483699cf',1,'CO_Emergency.h']]], + ['co_5femc_5fcurrent_5finput_254',['CO_EMC_CURRENT_INPUT',['../group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02ab792c971a569d1175666b3fff9ffbe70',1,'CO_Emergency.h']]], + ['co_5femc_5fcurrent_5finside_255',['CO_EMC_CURRENT_INSIDE',['../group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a3ded1c05cbf37d2d7d286af97e833e65',1,'CO_Emergency.h']]], + ['co_5femc_5fcurrent_5foutput_256',['CO_EMC_CURRENT_OUTPUT',['../group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02ad42e8ab666fd3da75d1fa3a7b8708efc',1,'CO_Emergency.h']]], + ['co_5femc_5fdam_5fmpdo_257',['CO_EMC_DAM_MPDO',['../group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02ab58df03302ab06710f7455d37039dea3',1,'CO_Emergency.h']]], + ['co_5femc_5fdata_5fset_258',['CO_EMC_DATA_SET',['../group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02ad22329fc3e44867a365401458e691ddc',1,'CO_Emergency.h']]], + ['co_5femc_5fdevice_5fspecific_259',['CO_EMC_DEVICE_SPECIFIC',['../group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02ad7b895b5e7d0f3fa7ff422157ac36c70',1,'CO_Emergency.h']]], + ['co_5femc_5fexternal_5ferror_260',['CO_EMC_EXTERNAL_ERROR',['../group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a1d76eff88ebd6050377c393533aebc8d',1,'CO_Emergency.h']]], + ['co_5femc_5fgeneric_261',['CO_EMC_GENERIC',['../group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a2eaf45ca12b32b7bcc58df91becda767',1,'CO_Emergency.h']]], + ['co_5femc_5fhardware_262',['CO_EMC_HARDWARE',['../group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a33344d49b9667151d86aef28a73e6f66',1,'CO_Emergency.h']]], + ['co_5femc_5fheartbeat_263',['CO_EMC_HEARTBEAT',['../group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02aff47b500e2e760355ca653b247e4b93f',1,'CO_Emergency.h']]], + ['co_5femc_5fmonitoring_264',['CO_EMC_MONITORING',['../group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a8ecd7e45af52d83d986e3de8e957a986',1,'CO_Emergency.h']]], + ['co_5femc_5fno_5ferror_265',['CO_EMC_NO_ERROR',['../group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02aa60e1333102cbe544eccbaad8e77f6f7',1,'CO_Emergency.h']]], + ['co_5femc_5fpdo_5flength_266',['CO_EMC_PDO_LENGTH',['../group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a80fedd7bbb98ddf1ec26d4b31ed6d749',1,'CO_Emergency.h']]], + ['co_5femc_5fpdo_5flength_5fexc_267',['CO_EMC_PDO_LENGTH_EXC',['../group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a88bac871b7539a579fba73825a2e240a',1,'CO_Emergency.h']]], + ['co_5femc_5fprotocol_5ferror_268',['CO_EMC_PROTOCOL_ERROR',['../group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02ab884b23b23af9671d99cca5865549e5a',1,'CO_Emergency.h']]], + ['co_5femc_5frpdo_5ftimeout_269',['CO_EMC_RPDO_TIMEOUT',['../group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a4ca48c8d1be6a42ac0c13e551e12b230',1,'CO_Emergency.h']]], + ['co_5femc_5fsoftware_5fdevice_270',['CO_EMC_SOFTWARE_DEVICE',['../group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a6d0b3c0c31228e0bc57fc080c754fefa',1,'CO_Emergency.h']]], + ['co_5femc_5fsoftware_5finternal_271',['CO_EMC_SOFTWARE_INTERNAL',['../group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a62e0949639733e85c2b6d4c8b099d467',1,'CO_Emergency.h']]], + ['co_5femc_5fsoftware_5fuser_272',['CO_EMC_SOFTWARE_USER',['../group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a7b6ae38c015688128890bfe42b0271e5',1,'CO_Emergency.h']]], + ['co_5femc_5fsync_5fdata_5flength_273',['CO_EMC_SYNC_DATA_LENGTH',['../group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a81aa2a66727d1fe29720067dc4e20879',1,'CO_Emergency.h']]], + ['co_5femc_5ftemp_5fambient_274',['CO_EMC_TEMP_AMBIENT',['../group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02ae5256d8178374a48750537c3d04c0a30',1,'CO_Emergency.h']]], + ['co_5femc_5ftemp_5fdevice_275',['CO_EMC_TEMP_DEVICE',['../group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a6c29a4b49fae39b45c5c0e553ef6668f',1,'CO_Emergency.h']]], + ['co_5femc_5ftemperature_276',['CO_EMC_TEMPERATURE',['../group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02aa24dfa4c6948187f62d3e8182285d4a3',1,'CO_Emergency.h']]], + ['co_5femc_5ftime_5fdata_5flength_277',['CO_EMC_TIME_DATA_LENGTH',['../group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a018485be8125a3515ecd127a08e2e2f1',1,'CO_Emergency.h']]], + ['co_5femc_5fvoltage_278',['CO_EMC_VOLTAGE',['../group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a84a8f0dfb97e0ec13be9a4cdb0d71233',1,'CO_Emergency.h']]], + ['co_5femc_5fvoltage_5finside_279',['CO_EMC_VOLTAGE_INSIDE',['../group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a78dfa1d496a010ae7ae2e8b6edc1362a',1,'CO_Emergency.h']]], + ['co_5femc_5fvoltage_5fmains_280',['CO_EMC_VOLTAGE_MAINS',['../group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02ab4b095d1d9e7e7f5150bc2ecd83bc140',1,'CO_Emergency.h']]], + ['co_5femc_5fvoltage_5foutput_281',['CO_EMC_VOLTAGE_OUTPUT',['../group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a9c5becd591c91bb3e255badf0a308c2d',1,'CO_Emergency.h']]], + ['co_5femergency_2eh_282',['CO_Emergency.h',['../CO__Emergency_8h.html',1,'']]], + ['co_5fepoll_5fclose_283',['CO_epoll_close',['../group__CO__epoll__interface.html#ga72c3ebdede1207e55e0915128f5a2c6a',1,'CO_epoll_close(CO_epoll_t *ep): CO_epoll_interface.c'],['../group__CO__epoll__interface.html#ga72c3ebdede1207e55e0915128f5a2c6a',1,'CO_epoll_close(CO_epoll_t *ep): CO_epoll_interface.c']]], + ['co_5fepoll_5fclosegtw_284',['CO_epoll_closeGtw',['../group__CO__epoll__interface.html#gab2f3f7bb7d885799c462e95a45563b69',1,'CO_epoll_interface.h']]], + ['co_5fepoll_5fcreate_285',['CO_epoll_create',['../group__CO__epoll__interface.html#ga9bb687bf26f711ce0573581984b79443',1,'CO_epoll_create(CO_epoll_t *ep, uint32_t timerInterval_us): CO_epoll_interface.c'],['../group__CO__epoll__interface.html#ga9bb687bf26f711ce0573581984b79443',1,'CO_epoll_create(CO_epoll_t *ep, uint32_t timerInterval_us): CO_epoll_interface.c']]], + ['co_5fepoll_5fcreategtw_286',['CO_epoll_createGtw',['../group__CO__epoll__interface.html#ga7165df8b37ca1ed59476773fa075470c',1,'CO_epoll_interface.h']]], + ['co_5fepoll_5fgtw_5ft_287',['CO_epoll_gtw_t',['../structCO__epoll__gtw__t.html',1,'']]], + ['co_5fepoll_5finitcanopengtw_288',['CO_epoll_initCANopenGtw',['../group__CO__epoll__interface.html#ga07eaf2c954bb09b6420acee62ff207c3',1,'CO_epoll_interface.h']]], + ['co_5fepoll_5finitcanopenmain_289',['CO_epoll_initCANopenMain',['../group__CO__epoll__interface.html#ga3668c8b600022ffecdd3c2fa643b5e7d',1,'CO_epoll_initCANopenMain(CO_epoll_t *ep, CO_t *co): CO_epoll_interface.c'],['../group__CO__epoll__interface.html#ga3668c8b600022ffecdd3c2fa643b5e7d',1,'CO_epoll_initCANopenMain(CO_epoll_t *ep, CO_t *co): CO_epoll_interface.c']]], + ['co_5fepoll_5finterface_2eh_290',['CO_epoll_interface.h',['../CO__epoll__interface_8h.html',1,'']]], + ['co_5fepoll_5fprocessgtw_291',['CO_epoll_processGtw',['../group__CO__epoll__interface.html#ga97aa0bc09684b0ed78b708198e663407',1,'CO_epoll_interface.h']]], + ['co_5fepoll_5fprocesslast_292',['CO_epoll_processLast',['../group__CO__epoll__interface.html#ga2aef637d4f2f818a7d95a7bfac251132',1,'CO_epoll_processLast(CO_epoll_t *ep): CO_epoll_interface.c'],['../group__CO__epoll__interface.html#ga2aef637d4f2f818a7d95a7bfac251132',1,'CO_epoll_processLast(CO_epoll_t *ep): CO_epoll_interface.c']]], + ['co_5fepoll_5fprocessmain_293',['CO_epoll_processMain',['../group__CO__epoll__interface.html#gad79d73fbac0e816f81f75507bd164515',1,'CO_epoll_processMain(CO_epoll_t *ep, CO_t *co, bool_t enableGateway, CO_NMT_reset_cmd_t *reset): CO_epoll_interface.c'],['../group__CO__epoll__interface.html#gad79d73fbac0e816f81f75507bd164515',1,'CO_epoll_processMain(CO_epoll_t *ep, CO_t *co, bool_t enableGateway, CO_NMT_reset_cmd_t *reset): CO_epoll_interface.c']]], + ['co_5fepoll_5fprocessrt_294',['CO_epoll_processRT',['../group__CO__epoll__interface.html#ga6ed67073bc96e575bec6fceb627b1245',1,'CO_epoll_processRT(CO_epoll_t *ep, CO_t *co, bool_t realtime): CO_epoll_interface.c'],['../group__CO__epoll__interface.html#ga6ed67073bc96e575bec6fceb627b1245',1,'CO_epoll_processRT(CO_epoll_t *ep, CO_t *co, bool_t realtime): CO_epoll_interface.c']]], + ['co_5fepoll_5ft_295',['CO_epoll_t',['../structCO__epoll__t.html',1,'']]], + ['co_5fepoll_5fwait_296',['CO_epoll_wait',['../group__CO__epoll__interface.html#ga5cfd1df62494cf9d9e85dbb9fe981a57',1,'CO_epoll_wait(CO_epoll_t *ep): CO_epoll_interface.c'],['../group__CO__epoll__interface.html#ga5cfd1df62494cf9d9e85dbb9fe981a57',1,'CO_epoll_wait(CO_epoll_t *ep): CO_epoll_interface.c']]], + ['co_5ferr_5freg_5fcommunication_297',['CO_ERR_REG_COMMUNICATION',['../group__CO__Emergency.html#gga2cfc261cce03577083ee3f1a31d5e03ca2f3b7aeac7282281c1d17895406c006a',1,'CO_Emergency.h']]], + ['co_5ferr_5freg_5fcurrent_298',['CO_ERR_REG_CURRENT',['../group__CO__Emergency.html#gga2cfc261cce03577083ee3f1a31d5e03ca61eded29fb0fcd95b2f66c2682de0f2b',1,'CO_Emergency.h']]], + ['co_5ferr_5freg_5fdev_5fprofile_299',['CO_ERR_REG_DEV_PROFILE',['../group__CO__Emergency.html#gga2cfc261cce03577083ee3f1a31d5e03cab137d1705e9ab20e2caeb22f57dd4716',1,'CO_Emergency.h']]], + ['co_5ferr_5freg_5fgeneric_5ferr_300',['CO_ERR_REG_GENERIC_ERR',['../group__CO__Emergency.html#gga2cfc261cce03577083ee3f1a31d5e03ca92a7e121ae04a022fc2fe604eb1c148e',1,'CO_Emergency.h']]], + ['co_5ferr_5freg_5fmanufacturer_301',['CO_ERR_REG_MANUFACTURER',['../group__CO__Emergency.html#gga2cfc261cce03577083ee3f1a31d5e03ca207eee1d9638f61166bc395ee71b84a3',1,'CO_Emergency.h']]], + ['co_5ferr_5freg_5freserved_302',['CO_ERR_REG_RESERVED',['../group__CO__Emergency.html#gga2cfc261cce03577083ee3f1a31d5e03caffdf7f5d9f9ae52fa1bf97a3fb3d848b',1,'CO_Emergency.h']]], + ['co_5ferr_5freg_5ftemperature_303',['CO_ERR_REG_TEMPERATURE',['../group__CO__Emergency.html#gga2cfc261cce03577083ee3f1a31d5e03cab12f2b419af0aeb8aae83a13d5c8b7bf',1,'CO_Emergency.h']]], + ['co_5ferr_5freg_5fvoltage_304',['CO_ERR_REG_VOLTAGE',['../group__CO__Emergency.html#gga2cfc261cce03577083ee3f1a31d5e03ca360c75e04303d1c55e2bc8528407cb87',1,'CO_Emergency.h']]], + ['co_5ferrinfo_305',['CO_errinfo',['../group__CO__driver.html#gaaa84189910b720ce18c8d83aab405d86',1,'CO_driver.h']]], + ['co_5ferror_306',['CO_error',['../group__CO__Emergency.html#ga9221f9f631ead4b6f66cfcff8614ba46',1,'CO_error(CO_EM_t *em, bool_t setError, const uint8_t errorBit, uint16_t errorCode, uint32_t infoCode): CO_Emergency.c'],['../group__CO__Emergency.html#ga9221f9f631ead4b6f66cfcff8614ba46',1,'CO_error(CO_EM_t *em, bool_t setError, const uint8_t errorBit, uint16_t errorCode, uint32_t infoCode): CO_Emergency.c']]], + ['co_5ferror_2eh_307',['CO_error.h',['../CO__error_8h.html',1,'']]], + ['co_5ferror_5fcrc_308',['CO_ERROR_CRC',['../group__CO__driver.html#gga1cb2d3466eb0c6d267f3b5ff1a0d9532a25eaff6474c6bc2aae67b1e7c7a35109',1,'CO_driver.h']]], + ['co_5ferror_5fdata_5fcorrupt_309',['CO_ERROR_DATA_CORRUPT',['../group__CO__driver.html#gga1cb2d3466eb0c6d267f3b5ff1a0d9532aa053301213d13670e9af7d31abc1ee48',1,'CO_driver.h']]], + ['co_5ferror_5fillegal_5fargument_310',['CO_ERROR_ILLEGAL_ARGUMENT',['../group__CO__driver.html#gga1cb2d3466eb0c6d267f3b5ff1a0d9532a3e78d588c0e21630c9cb7b43bfda3dae',1,'CO_driver.h']]], + ['co_5ferror_5fillegal_5fbaudrate_311',['CO_ERROR_ILLEGAL_BAUDRATE',['../group__CO__driver.html#gga1cb2d3466eb0c6d267f3b5ff1a0d9532a0214308865e83b8c21de7317b3070097',1,'CO_driver.h']]], + ['co_5ferror_5finvalid_5fstate_312',['CO_ERROR_INVALID_STATE',['../group__CO__driver.html#gga1cb2d3466eb0c6d267f3b5ff1a0d9532a57f1c4d2d960ab2550669a1c5ebbf4e1',1,'CO_driver.h']]], + ['co_5ferror_5fno_313',['CO_ERROR_NO',['../group__CO__driver.html#gga1cb2d3466eb0c6d267f3b5ff1a0d9532aff1c3e9fd4bf65e6757b020f752cdac8',1,'CO_driver.h']]], + ['co_5ferror_5fnode_5fid_5funconfigured_5flss_314',['CO_ERROR_NODE_ID_UNCONFIGURED_LSS',['../group__CO__driver.html#gga1cb2d3466eb0c6d267f3b5ff1a0d9532ac7a6f0554ae52997b88a97b46a16e5a3',1,'CO_driver.h']]], + ['co_5ferror_5fod_5fparameters_315',['CO_ERROR_OD_PARAMETERS',['../group__CO__driver.html#gga1cb2d3466eb0c6d267f3b5ff1a0d9532a737e67c9f8d3ca882a3cba852153c1f3',1,'CO_driver.h']]], + ['co_5ferror_5fout_5fof_5fmemory_316',['CO_ERROR_OUT_OF_MEMORY',['../group__CO__driver.html#gga1cb2d3466eb0c6d267f3b5ff1a0d9532a2f07295c4c6539c2b390db2d7c351267',1,'CO_driver.h']]], + ['co_5ferror_5frx_5fmsg_5flength_317',['CO_ERROR_RX_MSG_LENGTH',['../group__CO__driver.html#gga1cb2d3466eb0c6d267f3b5ff1a0d9532a3e40e8440a8480c413d3ff724b875de4',1,'CO_driver.h']]], + ['co_5ferror_5frx_5foverflow_318',['CO_ERROR_RX_OVERFLOW',['../group__CO__driver.html#gga1cb2d3466eb0c6d267f3b5ff1a0d9532ab0746d0801a99639077b325594b347b5',1,'CO_driver.h']]], + ['co_5ferror_5frx_5fpdo_5flength_319',['CO_ERROR_RX_PDO_LENGTH',['../group__CO__driver.html#gga1cb2d3466eb0c6d267f3b5ff1a0d9532a4829a460cbba557a2ff7f52bd912aa65',1,'CO_driver.h']]], + ['co_5ferror_5frx_5fpdo_5foverflow_320',['CO_ERROR_RX_PDO_OVERFLOW',['../group__CO__driver.html#gga1cb2d3466eb0c6d267f3b5ff1a0d9532a7e4cefeb35775754c87cf6a559f1bbd9',1,'CO_driver.h']]], + ['co_5ferror_5fsyscall_321',['CO_ERROR_SYSCALL',['../group__CO__driver.html#gga1cb2d3466eb0c6d267f3b5ff1a0d9532a46d81c922ecbaf8f136626725ad8399d',1,'CO_driver.h']]], + ['co_5ferror_5ftimeout_322',['CO_ERROR_TIMEOUT',['../group__CO__driver.html#gga1cb2d3466eb0c6d267f3b5ff1a0d9532a129c7141c52ae59d37a2fff163fec600',1,'CO_driver.h']]], + ['co_5ferror_5ftx_5fbusy_323',['CO_ERROR_TX_BUSY',['../group__CO__driver.html#gga1cb2d3466eb0c6d267f3b5ff1a0d9532ab47ecc1b08463eb986fd29c187096343',1,'CO_driver.h']]], + ['co_5ferror_5ftx_5foverflow_324',['CO_ERROR_TX_OVERFLOW',['../group__CO__driver.html#gga1cb2d3466eb0c6d267f3b5ff1a0d9532a757b4d226a3e00a9e2543cf21833d46f',1,'CO_driver.h']]], + ['co_5ferror_5ftx_5fpdo_5fwindow_325',['CO_ERROR_TX_PDO_WINDOW',['../group__CO__driver.html#gga1cb2d3466eb0c6d267f3b5ff1a0d9532a48993dc18698738d168071b4f4c3d244',1,'CO_driver.h']]], + ['co_5ferror_5ftx_5funconfigured_326',['CO_ERROR_TX_UNCONFIGURED',['../group__CO__driver.html#gga1cb2d3466eb0c6d267f3b5ff1a0d9532a6f07e7c21acde035b561edc7f55edb89',1,'CO_driver.h']]], + ['co_5ferror_5fwrong_5fnmt_5fstate_327',['CO_ERROR_WRONG_NMT_STATE',['../group__CO__driver.html#gga1cb2d3466eb0c6d267f3b5ff1a0d9532a07457c9c6eda89fc0260e6d7e431424c',1,'CO_driver.h']]], + ['co_5ferrorregister_5ft_328',['CO_errorRegister_t',['../group__CO__Emergency.html#ga2cfc261cce03577083ee3f1a31d5e03c',1,'CO_Emergency.h']]], + ['co_5ferrorreport_329',['CO_errorReport',['../group__CO__Emergency.html#gab66d4a6daa5f7492704b56a46b135f71',1,'CO_Emergency.h']]], + ['co_5ferrorreset_330',['CO_errorReset',['../group__CO__Emergency.html#ga24e2a9311cf704ec6ed43b0ea730c4a3',1,'CO_Emergency.h']]], + ['co_5ffifo_2eh_331',['CO_fifo.h',['../CO__fifo_8h.html',1,'']]], + ['co_5ffifo_5faltbegin_332',['CO_fifo_altBegin',['../group__CO__CANopen__301__fifo.html#ga55c186098b2d860b116ddeb637a27b5a',1,'CO_fifo.h']]], + ['co_5ffifo_5faltfinish_333',['CO_fifo_altFinish',['../group__CO__CANopen__301__fifo.html#ga7c534dd50959814b40c3027aa85f1c55',1,'CO_fifo.h']]], + ['co_5ffifo_5faltgetoccupied_334',['CO_fifo_altGetOccupied',['../group__CO__CANopen__301__fifo.html#ga2ee45a0cab19a212022d82987de2a955',1,'CO_fifo.h']]], + ['co_5ffifo_5faltread_335',['CO_fifo_altRead',['../group__CO__CANopen__301__fifo.html#gaa28339101c4ac5a2c44004276075c759',1,'CO_fifo.h']]], + ['co_5ffifo_5fcommsearch_336',['CO_fifo_CommSearch',['../group__CO__CANopen__301__fifo.html#ga0dbef46e369e659bab7e29742971134c',1,'CO_fifo.h']]], + ['co_5ffifo_5fcpytok2b64_337',['CO_fifo_cpyTok2B64',['../group__CO__CANopen__301__fifo.html#gaf11cf0cccc01e86f341e2b31607e90c7',1,'CO_fifo.h']]], + ['co_5ffifo_5fcpytok2hex_338',['CO_fifo_cpyTok2Hex',['../group__CO__CANopen__301__fifo.html#gab3c4acf458a13bd6c4e5b0bc2adca10b',1,'CO_fifo.h']]], + ['co_5ffifo_5fcpytok2i16_339',['CO_fifo_cpyTok2I16',['../group__CO__CANopen__301__fifo.html#ga306913df146db90f7209b89080d51640',1,'CO_fifo.h']]], + ['co_5ffifo_5fcpytok2i32_340',['CO_fifo_cpyTok2I32',['../group__CO__CANopen__301__fifo.html#ga4f5b25af39c5a78faf04e54cb3cc90f7',1,'CO_fifo.h']]], + ['co_5ffifo_5fcpytok2i64_341',['CO_fifo_cpyTok2I64',['../group__CO__CANopen__301__fifo.html#ga6a9bd6e8d07d20ded9aeca531f412d50',1,'CO_fifo.h']]], + ['co_5ffifo_5fcpytok2i8_342',['CO_fifo_cpyTok2I8',['../group__CO__CANopen__301__fifo.html#gae038564001c68ab242a6f33756668ac5',1,'CO_fifo.h']]], + ['co_5ffifo_5fcpytok2r32_343',['CO_fifo_cpyTok2R32',['../group__CO__CANopen__301__fifo.html#ga853847c375425a55fc2007880d94c484',1,'CO_fifo.h']]], + ['co_5ffifo_5fcpytok2r64_344',['CO_fifo_cpyTok2R64',['../group__CO__CANopen__301__fifo.html#ga8b17d4f3c0d272bb73d82a79cd51cd3d',1,'CO_fifo.h']]], + ['co_5ffifo_5fcpytok2u16_345',['CO_fifo_cpyTok2U16',['../group__CO__CANopen__301__fifo.html#gaf19bdbd45626578afc8a19de43698e0b',1,'CO_fifo.h']]], + ['co_5ffifo_5fcpytok2u32_346',['CO_fifo_cpyTok2U32',['../group__CO__CANopen__301__fifo.html#gae2816b287cc2091d2382aaa6ac56e422',1,'CO_fifo.h']]], + ['co_5ffifo_5fcpytok2u64_347',['CO_fifo_cpyTok2U64',['../group__CO__CANopen__301__fifo.html#gacfd3fdfda66e1feb6982f68377a8b7e2',1,'CO_fifo.h']]], + ['co_5ffifo_5fcpytok2u8_348',['CO_fifo_cpyTok2U8',['../group__CO__CANopen__301__fifo.html#ga1788f69639bcc6bfa59bf85e57e5e13a',1,'CO_fifo.h']]], + ['co_5ffifo_5fcpytok2vs_349',['CO_fifo_cpyTok2Vs',['../group__CO__CANopen__301__fifo.html#ga8cb3d7032cd46dfeb3b6c0bbd5ed1575',1,'CO_fifo.h']]], + ['co_5ffifo_5fgetc_350',['CO_fifo_getc',['../group__CO__CANopen__301__fifo.html#ga4c1237aa63123d0ab8e9e16132a5c40a',1,'CO_fifo.h']]], + ['co_5ffifo_5fgetoccupied_351',['CO_fifo_getOccupied',['../group__CO__CANopen__301__fifo.html#ga40ef4fc5dc184ef3c0c2f37eca1bc507',1,'CO_fifo.h']]], + ['co_5ffifo_5fgetspace_352',['CO_fifo_getSpace',['../group__CO__CANopen__301__fifo.html#ga0d456e83af18cce9db9157b5a30fac21',1,'CO_fifo.h']]], + ['co_5ffifo_5finit_353',['CO_fifo_init',['../group__CO__CANopen__301__fifo.html#ga44e8f83feb2dd463b2fbb399dcd4de4d',1,'CO_fifo.h']]], + ['co_5ffifo_5fpurge_354',['CO_fifo_purge',['../group__CO__CANopen__301__fifo.html#ga15a27a4871e680475f680c574050844a',1,'CO_fifo.h']]], + ['co_5ffifo_5fputc_355',['CO_fifo_putc',['../group__CO__CANopen__301__fifo.html#ga7c2deb5abee499ad2f1b5175a205bc5b',1,'CO_fifo.h']]], + ['co_5ffifo_5fputc_5fov_356',['CO_fifo_putc_ov',['../group__CO__CANopen__301__fifo.html#gae9351e31e5d74e738fe5e1514f99f2dd',1,'CO_fifo.h']]], + ['co_5ffifo_5fread_357',['CO_fifo_read',['../group__CO__CANopen__301__fifo.html#gad0ba8be2a601030a374913e4fa94e6cb',1,'CO_fifo.h']]], + ['co_5ffifo_5freadb642a_358',['CO_fifo_readB642a',['../group__CO__CANopen__301__fifo.html#ga7b9dcd98906837e82fa914d10028cf33',1,'CO_fifo.h']]], + ['co_5ffifo_5freadhex2a_359',['CO_fifo_readHex2a',['../group__CO__CANopen__301__fifo.html#gae9bfcb5f9c52de5f7239ab133fe326ec',1,'CO_fifo.h']]], + ['co_5ffifo_5freadi162a_360',['CO_fifo_readI162a',['../group__CO__CANopen__301__fifo.html#ga01a1765094d97ca3d0c5bbf98d1b8b1f',1,'CO_fifo.h']]], + ['co_5ffifo_5freadi322a_361',['CO_fifo_readI322a',['../group__CO__CANopen__301__fifo.html#ga6a7a2ec338f735cdddf7ea7f7c0fd3a5',1,'CO_fifo.h']]], + ['co_5ffifo_5freadi642a_362',['CO_fifo_readI642a',['../group__CO__CANopen__301__fifo.html#gaf959a6b823cc6db1d2107fd1faa40472',1,'CO_fifo.h']]], + ['co_5ffifo_5freadi82a_363',['CO_fifo_readI82a',['../group__CO__CANopen__301__fifo.html#ga00fe8ce9f04846e8c756a09aad22cf03',1,'CO_fifo.h']]], + ['co_5ffifo_5freadr322a_364',['CO_fifo_readR322a',['../group__CO__CANopen__301__fifo.html#ga74aad9b161d3953221a99d4b238f3c1f',1,'CO_fifo.h']]], + ['co_5ffifo_5freadr642a_365',['CO_fifo_readR642a',['../group__CO__CANopen__301__fifo.html#gadc55fa15874eaf757acf83921d0156c4',1,'CO_fifo.h']]], + ['co_5ffifo_5freadtoken_366',['CO_fifo_readToken',['../group__CO__CANopen__301__fifo.html#ga87a199708c95d3ca02e2fcc4ca4a6319',1,'CO_fifo.h']]], + ['co_5ffifo_5freadu162a_367',['CO_fifo_readU162a',['../group__CO__CANopen__301__fifo.html#ga99072bd50ad3b92c291136e458885ca2',1,'CO_fifo.h']]], + ['co_5ffifo_5freadu322a_368',['CO_fifo_readU322a',['../group__CO__CANopen__301__fifo.html#ga9a2d7d07570a8f552a81e8c72327f43c',1,'CO_fifo.h']]], + ['co_5ffifo_5freadu642a_369',['CO_fifo_readU642a',['../group__CO__CANopen__301__fifo.html#ga8805e75a32e9f8672259f2274816a0b8',1,'CO_fifo.h']]], + ['co_5ffifo_5freadu82a_370',['CO_fifo_readU82a',['../group__CO__CANopen__301__fifo.html#ga50b3075a0cfab3ab8608a4ba4977ecd6',1,'CO_fifo.h']]], + ['co_5ffifo_5freadvs2a_371',['CO_fifo_readVs2a',['../group__CO__CANopen__301__fifo.html#ga8000ba92f2023a88b5f7d0399373b206',1,'CO_fifo.h']]], + ['co_5ffifo_5freadx162a_372',['CO_fifo_readX162a',['../group__CO__CANopen__301__fifo.html#ga3202ac32250b599f5a5719c52264578a',1,'CO_fifo.h']]], + ['co_5ffifo_5freadx322a_373',['CO_fifo_readX322a',['../group__CO__CANopen__301__fifo.html#ga8dcf1f06de7b744d95c6a65721828a5a',1,'CO_fifo.h']]], + ['co_5ffifo_5freadx642a_374',['CO_fifo_readX642a',['../group__CO__CANopen__301__fifo.html#ga387acd7fdbd61389a42935d452dbd409',1,'CO_fifo.h']]], + ['co_5ffifo_5freadx82a_375',['CO_fifo_readX82a',['../group__CO__CANopen__301__fifo.html#ga7ed37d0c4caea872cfe15a0c2e86fe0e',1,'CO_fifo.h']]], + ['co_5ffifo_5freset_376',['CO_fifo_reset',['../group__CO__CANopen__301__fifo.html#ga6817a4baeaee87a578291e9de820b126',1,'CO_fifo.h']]], + ['co_5ffifo_5fst_377',['CO_fifo_st',['../group__CO__CANopen__301__fifo.html#ga2c7db7d527e4055a5dde62b74dfc2818',1,'CO_fifo.h']]], + ['co_5ffifo_5fst_5fclosed_378',['CO_fifo_st_closed',['../group__CO__CANopen__301__fifo.html#gga2c7db7d527e4055a5dde62b74dfc2818a1f111c3f5a9396da6ced33132a2341f9',1,'CO_fifo.h']]], + ['co_5ffifo_5fst_5ferrbuf_379',['CO_fifo_st_errBuf',['../group__CO__CANopen__301__fifo.html#gga2c7db7d527e4055a5dde62b74dfc2818a51c064e77b0e2bf3741719b57de65141',1,'CO_fifo.h']]], + ['co_5ffifo_5fst_5ferrint_380',['CO_fifo_st_errInt',['../group__CO__CANopen__301__fifo.html#gga2c7db7d527e4055a5dde62b74dfc2818a07a7d8d483365e3afdcf9dc5b2dd2143',1,'CO_fifo.h']]], + ['co_5ffifo_5fst_5ferrmask_381',['CO_fifo_st_errMask',['../group__CO__CANopen__301__fifo.html#gga2c7db7d527e4055a5dde62b74dfc2818a9e587fa1d802fbe939f6da9d44aba389',1,'CO_fifo.h']]], + ['co_5ffifo_5fst_5ferrtok_382',['CO_fifo_st_errTok',['../group__CO__CANopen__301__fifo.html#gga2c7db7d527e4055a5dde62b74dfc2818a70060b411fcbfa0696ca453d7f4348d4',1,'CO_fifo.h']]], + ['co_5ffifo_5fst_5ferrval_383',['CO_fifo_st_errVal',['../group__CO__CANopen__301__fifo.html#gga2c7db7d527e4055a5dde62b74dfc2818a9c41b295a487450017cc3b1ce0bcedfb',1,'CO_fifo.h']]], + ['co_5ffifo_5fst_5fpartial_384',['CO_fifo_st_partial',['../group__CO__CANopen__301__fifo.html#gga2c7db7d527e4055a5dde62b74dfc2818a7bf88ff5b78ef6a25a4391bc6423c1f7',1,'CO_fifo.h']]], + ['co_5ffifo_5ft_385',['CO_fifo_t',['../structCO__fifo__t.html',1,'']]], + ['co_5ffifo_5ftrimspaces_386',['CO_fifo_trimSpaces',['../group__CO__CANopen__301__fifo.html#gac01a59d65d275bd9f9a7fb0ea04fb915',1,'CO_fifo.h']]], + ['co_5ffifo_5fwrite_387',['CO_fifo_write',['../group__CO__CANopen__301__fifo.html#ga715cb5e1feacd2f3af5bc8195bbe69d3',1,'CO_fifo.h']]], + ['co_5fflag_5fclear_388',['CO_FLAG_CLEAR',['../group__CO__critical__sections.html#ga044da4253aeed15c3e0bb7fce13664af',1,'CO_driver.h']]], + ['co_5fflag_5fread_389',['CO_FLAG_READ',['../group__CO__critical__sections.html#ga577a6ebcf246087f084c75d9ae25eeb7',1,'CO_driver.h']]], + ['co_5fflag_5fset_390',['CO_FLAG_SET',['../group__CO__critical__sections.html#gac54b5e4f680aa8b0177f0df5d5be2e88',1,'CO_driver.h']]], + ['co_5fgateway_5fascii_2eh_391',['CO_gateway_ascii.h',['../CO__gateway__ascii_8h.html',1,'']]], + ['co_5fgeterrorregister_392',['CO_getErrorRegister',['../group__CO__Emergency.html#gaf0c47186d9e51fb91d48385a9f6bad6b',1,'CO_Emergency.h']]], + ['co_5fgetuint16_393',['CO_getUint16',['../group__CO__driver.html#ga6590fe7a05ecb4b81ee3d7e233274ea4',1,'CO_driver.h']]], + ['co_5fgetuint32_394',['CO_getUint32',['../group__CO__driver.html#ga21cd9e2391f276b908bcde5769e967ed',1,'CO_driver.h']]], + ['co_5fgetuint8_395',['CO_getUint8',['../group__CO__driver.html#ga27a4052f87c60cf2df472378689c2ef9',1,'CO_driver.h']]], + ['co_5fgfc_2ec_396',['CO_GFC.c',['../CO__GFC_8c.html',1,'']]], + ['co_5fgfc_2eh_397',['CO_GFC.h',['../CO__GFC_8h.html',1,'']]], + ['co_5fgfc_5finit_398',['CO_GFC_init',['../group__CO__GFC.html#ga23d83d03ef1b9ad5ffe68103a627026c',1,'CO_GFC.h']]], + ['co_5fgfc_5finitcallbackentersafestate_399',['CO_GFC_initCallbackEnterSafeState',['../group__CO__GFC.html#gaa7cf845381bf150a5816bc068ab9218f',1,'CO_GFC.h']]], + ['co_5fgfc_5ft_400',['CO_GFC_t',['../structCO__GFC__t.html',1,'']]], + ['co_5fgfcsend_401',['CO_GFCsend',['../group__CO__GFC.html#ga64a30ac6c275d166a2f4117050b12c8c',1,'CO_GFC.h']]], + ['co_5fgtwa_5finit_402',['CO_GTWA_init',['../group__CO__CANopen__309__3.html#gabc95dab4fb09bcb18948502f922520ee',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5finitread_403',['CO_GTWA_initRead',['../group__CO__CANopen__309__3.html#ga2093c35b83b096e01bd0c65ae9374e30',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5flog_5fprint_404',['CO_GTWA_log_print',['../group__CO__CANopen__309__3.html#ga20523907b832d55b47b855dd92409996',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5fprocess_405',['CO_GTWA_process',['../group__CO__CANopen__309__3.html#ga4a82ef2ebdd5d5f9d8a7efe84048493d',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5fresp_5fbuf_5fsize_406',['CO_GTWA_RESP_BUF_SIZE',['../group__CO__CANopen__309__3.html#ga52919223e5f43323f15c6a382913653d',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5fresperrorbootup_407',['CO_GTWA_respErrorBootUp',['../group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fbaa4c5257721f3b64c231f67dccacd2ee7',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5fresperrorbusoff_408',['CO_GTWA_respErrorBusOff',['../group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fbabb407ab8220e0d3f0ebc06f867ecac9c',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5fresperrorcanactive_409',['CO_GTWA_respErrorCANactive',['../group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fba06e779c446aae0f7c6021dd0b955ffe8',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5fresperrorcanbufferoverflow_410',['CO_GTWA_respErrorCANbufferOverflow',['../group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fba8aa993a6c57d8db7d6355260a091e4a4',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5fresperrorcaninit_411',['CO_GTWA_respErrorCANinit',['../group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fba97d5b22449aadae1226679986d654bbc',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5fresperrorcode_5ft_412',['CO_GTWA_respErrorCode_t',['../group__CO__CANopen__309__3.html#ga92e67dec9b5e29cdd67a28651db237fb',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5fresperrorerrorpassive_413',['CO_GTWA_respErrorErrorPassive',['../group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fba6f9a335886d942ef028878832551afe0',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5fresperrorheartbeatlost_414',['CO_GTWA_respErrorHeartbeatLost',['../group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fbaba7c053af330ec3ffdc8c2c298db6c93',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5fresperrorheartbeatstarted_415',['CO_GTWA_respErrorHeartbeatStarted',['../group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fbaf4ed1e079359d86cee1ad431ed745886',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5fresperrorinternalstate_416',['CO_GTWA_respErrorInternalState',['../group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fba1d990943991355e276f31533c841cb81',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5fresperrorlostconnection_417',['CO_GTWA_respErrorLostConnection',['../group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fba9cc980ac35af4c1f9ad1d96719f05425',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5fresperrorlostguardingmessage_418',['CO_GTWA_respErrorLostGuardingMessage',['../group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fba1111be206d1ebfb62caa217d6231092c',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5fresperrorlssbitratenotsupported_419',['CO_GTWA_respErrorLSSbitRateNotSupported',['../group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fbad8ff0b3e8970d010eb084a703fa794db',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5fresperrorlssmanufacturer_420',['CO_GTWA_respErrorLSSmanufacturer',['../group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fba7e3209f49a5ae7bad0b197110aa78512',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5fresperrorlssmediaerror_421',['CO_GTWA_respErrorLSSmediaError',['../group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fbacc7ec1f2040bebb15becb067673e40e5',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5fresperrorlssnodeidnotsupported_422',['CO_GTWA_respErrorLSSnodeIdNotSupported',['../group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fba6e164941b6f3a18ef6e366e7c53073b6',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5fresperrorlssparameterstoringfailed_423',['CO_GTWA_respErrorLSSparameterStoringFailed',['../group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fba315295e9ea4cdaba0955e6d8920df5a5',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5fresperrornodefaultnetset_424',['CO_GTWA_respErrorNoDefaultNetSet',['../group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fbad05b7717a0f79d333eed37ced2e758f9',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5fresperrornodefaultnodeset_425',['CO_GTWA_respErrorNoDefaultNodeSet',['../group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fbac668fafa372a0cee1d50ceb0f5ed7b4a',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5fresperrornone_426',['CO_GTWA_respErrorNone',['../group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fba46a2114a5c4d9b43babecebb11573c66',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5fresperrorpdoalreadyused_427',['CO_GTWA_respErrorPDOalreadyUsed',['../group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fbace2f0ef57b11e716d7ca58369ff5eede',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5fresperrorpdolengthexceeded_428',['CO_GTWA_respErrorPDOlengthExceeded',['../group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fbadd3dbd3197e1756102508208e394d72f',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5fresperrorreqnotsupported_429',['CO_GTWA_respErrorReqNotSupported',['../group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fba04540349402c75b73af33951db285904',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5fresperrorrunningoutofmemory_430',['CO_GTWA_respErrorRunningOutOfMemory',['../group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fba99443a03d7ba443b9beb07c6d475b350',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5fresperrorsyntax_431',['CO_GTWA_respErrorSyntax',['../group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fba541a3546ac6be179c728272409259c98',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5fresperrortimeout_432',['CO_GTWA_respErrorTimeOut',['../group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fba81131bf73d1ad39163a8b28e5ecf92fd',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5fresperrorunsupportednet_433',['CO_GTWA_respErrorUnsupportedNet',['../group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fba291ec5267c935d6277c27c767e2dd178',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5fresperrorunsupportednode_434',['CO_GTWA_respErrorUnsupportedNode',['../group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fbaf65eee2744df8e528312139a54a853d5',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5fresperrorwrongnmtstate_435',['CO_GTWA_respErrorWrongNMTstate',['../group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fbad007ed28e48b2fd7c287de3e6755a604',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5fst_5f_5flss_5ffastscan_436',['CO_GTWA_ST__LSS_FASTSCAN',['../group__CO__CANopen__309__3.html#ggae809d7b5adbc7a4fb1f2fce527b30954ab8c2946ce5d3581ecf5640bd1f5667b7',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5fst_5fhelp_437',['CO_GTWA_ST_HELP',['../group__CO__CANopen__309__3.html#ggae809d7b5adbc7a4fb1f2fce527b30954af2f0ce738128675b98926aba680884d1',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5fst_5fidle_438',['CO_GTWA_ST_IDLE',['../group__CO__CANopen__309__3.html#ggae809d7b5adbc7a4fb1f2fce527b30954a98a36dabc8934b9c2d37b13999e3c393',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5fst_5fled_439',['CO_GTWA_ST_LED',['../group__CO__CANopen__309__3.html#ggae809d7b5adbc7a4fb1f2fce527b30954a1a467cf33b8d3a2c0e8e31f87d81f05c',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5fst_5flog_440',['CO_GTWA_ST_LOG',['../group__CO__CANopen__309__3.html#ggae809d7b5adbc7a4fb1f2fce527b30954a829f096eca2ccd152069e97d9c70022f',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5fst_5flss_5fallnodes_441',['CO_GTWA_ST_LSS_ALLNODES',['../group__CO__CANopen__309__3.html#ggae809d7b5adbc7a4fb1f2fce527b30954a338a75317f0bf276f1c786316d6b9ec7',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5fst_5flss_5fconf_5fbitrate_442',['CO_GTWA_ST_LSS_CONF_BITRATE',['../group__CO__CANopen__309__3.html#ggae809d7b5adbc7a4fb1f2fce527b30954a6e18980d6fbcbd7dd871af5c98c4755e',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5fst_5flss_5finquire_443',['CO_GTWA_ST_LSS_INQUIRE',['../group__CO__CANopen__309__3.html#ggae809d7b5adbc7a4fb1f2fce527b30954abc4877f6d72e218cbd4959279a537b94',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5fst_5flss_5finquire_5faddr_5fall_444',['CO_GTWA_ST_LSS_INQUIRE_ADDR_ALL',['../group__CO__CANopen__309__3.html#ggae809d7b5adbc7a4fb1f2fce527b30954a17254161b141cc0c424c3649655f4df2',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5fst_5flss_5fset_5fnode_445',['CO_GTWA_ST_LSS_SET_NODE',['../group__CO__CANopen__309__3.html#ggae809d7b5adbc7a4fb1f2fce527b30954acbc1d88036a086ae9d9dcbed1742330e',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5fst_5flss_5fstore_446',['CO_GTWA_ST_LSS_STORE',['../group__CO__CANopen__309__3.html#ggae809d7b5adbc7a4fb1f2fce527b30954a63e0705eddd22fb9d476c1b370394fc4',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5fst_5flss_5fswitch_5fglob_447',['CO_GTWA_ST_LSS_SWITCH_GLOB',['../group__CO__CANopen__309__3.html#ggae809d7b5adbc7a4fb1f2fce527b30954afcc0bdbc08aa70d401f122c73055223e',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5fst_5flss_5fswitch_5fsel_448',['CO_GTWA_ST_LSS_SWITCH_SEL',['../group__CO__CANopen__309__3.html#ggae809d7b5adbc7a4fb1f2fce527b30954a19bb6359e2efba37226a5dcf27d4a0e3',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5fst_5fread_449',['CO_GTWA_ST_READ',['../group__CO__CANopen__309__3.html#ggae809d7b5adbc7a4fb1f2fce527b30954aaca55c8223aa1ba8f18031196178ea58',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5fst_5fwrite_450',['CO_GTWA_ST_WRITE',['../group__CO__CANopen__309__3.html#ggae809d7b5adbc7a4fb1f2fce527b30954a959c96b4eb3d948a977f66679423baa2',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5fst_5fwrite_5faborted_451',['CO_GTWA_ST_WRITE_ABORTED',['../group__CO__CANopen__309__3.html#ggae809d7b5adbc7a4fb1f2fce527b30954ab10b68af4a8bc47f96abf552c7baa3b9',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5fstate_5ft_452',['CO_GTWA_state_t',['../group__CO__CANopen__309__3.html#gae809d7b5adbc7a4fb1f2fce527b30954',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5fstate_5ftimeout_5ftime_5fus_453',['CO_GTWA_STATE_TIMEOUT_TIME_US',['../group__CO__CANopen__309__3.html#gaa631d47f972204f26502d65894694cfb',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5ft_454',['CO_GTWA_t',['../structCO__GTWA__t.html',1,'']]], + ['co_5fgtwa_5fwrite_455',['CO_GTWA_write',['../group__CO__CANopen__309__3.html#gaae1f01d444be975aa3cb3b2c9ebded3d',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5fwrite_5fgetspace_456',['CO_GTWA_write_getSpace',['../group__CO__CANopen__309__3.html#gad614da9bb171d995c02ffe1940fe7e64',1,'CO_gateway_ascii.h']]], + ['co_5fhbconsnode_5ft_457',['CO_HBconsNode_t',['../structCO__HBconsNode__t.html',1,'']]], + ['co_5fhbconsumer_2eh_458',['CO_HBconsumer.h',['../CO__HBconsumer_8h.html',1,'']]], + ['co_5fhbconsumer_5factive_459',['CO_HBconsumer_ACTIVE',['../group__CO__HBconsumer.html#gga7658e41b7c045b7b612e4ef8a2b663f3ad80c78b38e6d28927bf3d1b1464b36e9',1,'CO_HBconsumer.h']]], + ['co_5fhbconsumer_5fgetidxbynodeid_460',['CO_HBconsumer_getIdxByNodeId',['../group__CO__HBconsumer.html#ga041b92d6feb1774cb7eb87fba842fdf2',1,'CO_HBconsumer.h']]], + ['co_5fhbconsumer_5fgetnmtstate_461',['CO_HBconsumer_getNmtState',['../group__CO__HBconsumer.html#ga1731e3860fce5ca5d341d9b7fc32d8d6',1,'CO_HBconsumer.h']]], + ['co_5fhbconsumer_5fgetstate_462',['CO_HBconsumer_getState',['../group__CO__HBconsumer.html#ga7c5d4eccbcb0f1f8965a336fde04e765',1,'CO_HBconsumer.h']]], + ['co_5fhbconsumer_5finit_463',['CO_HBconsumer_init',['../group__CO__HBconsumer.html#gacc31c4848a14c9c367505b20e4a6a496',1,'CO_HBconsumer.h']]], + ['co_5fhbconsumer_5finitcallbackheartbeatstarted_464',['CO_HBconsumer_initCallbackHeartbeatStarted',['../group__CO__HBconsumer.html#ga6c9bd0df815428719b9f9429ed4476a9',1,'CO_HBconsumer.h']]], + ['co_5fhbconsumer_5finitcallbacknmtchanged_465',['CO_HBconsumer_initCallbackNmtChanged',['../group__CO__HBconsumer.html#gabab4b2dd74f6e341fe8b683f7a6d56f3',1,'CO_HBconsumer.h']]], + ['co_5fhbconsumer_5finitcallbackpre_466',['CO_HBconsumer_initCallbackPre',['../group__CO__HBconsumer.html#ga2faa596dcebbec8f486788a791d638be',1,'CO_HBconsumer.h']]], + ['co_5fhbconsumer_5finitcallbackremotereset_467',['CO_HBconsumer_initCallbackRemoteReset',['../group__CO__HBconsumer.html#ga8758a7bd92aa458b90d5da9221cc694f',1,'CO_HBconsumer.h']]], + ['co_5fhbconsumer_5finitcallbacktimeout_468',['CO_HBconsumer_initCallbackTimeout',['../group__CO__HBconsumer.html#gaef359610a0cdd1331da266be9c55c2d2',1,'CO_HBconsumer.h']]], + ['co_5fhbconsumer_5finitentry_469',['CO_HBconsumer_initEntry',['../group__CO__HBconsumer.html#gaf4bfa2bbd2b7d70f25b6bf173932170a',1,'CO_HBconsumer.h']]], + ['co_5fhbconsumer_5fprocess_470',['CO_HBconsumer_process',['../group__CO__HBconsumer.html#ga29e01b5fe6392ce688e8ac57d966258f',1,'CO_HBconsumer.h']]], + ['co_5fhbconsumer_5fstate_5ft_471',['CO_HBconsumer_state_t',['../group__CO__HBconsumer.html#ga7658e41b7c045b7b612e4ef8a2b663f3',1,'CO_HBconsumer.h']]], + ['co_5fhbconsumer_5ft_472',['CO_HBconsumer_t',['../structCO__HBconsumer__t.html',1,'']]], + ['co_5fhbconsumer_5ftimeout_473',['CO_HBconsumer_TIMEOUT',['../group__CO__HBconsumer.html#gga7658e41b7c045b7b612e4ef8a2b663f3a8a7ac49e5c809994ee65f365a7a75f22',1,'CO_HBconsumer.h']]], + ['co_5fhbconsumer_5funconfigured_474',['CO_HBconsumer_UNCONFIGURED',['../group__CO__HBconsumer.html#gga7658e41b7c045b7b612e4ef8a2b663f3a4c481c1ba58fb71b9870e8b355351211',1,'CO_HBconsumer.h']]], + ['co_5fhbconsumer_5funknown_475',['CO_HBconsumer_UNKNOWN',['../group__CO__HBconsumer.html#gga7658e41b7c045b7b612e4ef8a2b663f3af36bc5f46fd11044dc2d1d995ad8f28b',1,'CO_HBconsumer.h']]], + ['co_5finterface_5factive_476',['CO_INTERFACE_ACTIVE',['../group__CO__socketCAN__ERROR.html#gga7bf6a56766c008531d32b4218a5a9061ad22fcb069e808548dee28e4aae580c1a',1,'CO_error.h']]], + ['co_5finterface_5fbus_5foff_477',['CO_INTERFACE_BUS_OFF',['../group__CO__socketCAN__ERROR.html#gga7bf6a56766c008531d32b4218a5a9061a8e51897ec66a9b37865659bbc348e212',1,'CO_error.h']]], + ['co_5finterface_5flisten_5fonly_478',['CO_INTERFACE_LISTEN_ONLY',['../group__CO__socketCAN__ERROR.html#gga7bf6a56766c008531d32b4218a5a9061a9b74584a321fe8a89cf74e087b094581',1,'CO_error.h']]], + ['co_5fiserror_479',['CO_isError',['../group__CO__Emergency.html#ga8e9bae71814a3e7bbd8d59d721174c2b',1,'CO_Emergency.h']]], + ['co_5fislssslaveenabled_480',['CO_isLSSslaveEnabled',['../group__CO__CANopen.html#ga4c9143f717e8df279034fb897e39b517',1,'CANopen.h']]], + ['co_5fled_5fbitfield_5ft_481',['CO_LED_BITFIELD_t',['../group__CO__LEDs.html#ga366de3822a3da8478e97b248bed641fb',1,'CO_LEDs.h']]], + ['co_5fled_5fblink_482',['CO_LED_blink',['../group__CO__LEDs.html#gga366de3822a3da8478e97b248bed641fbaa24d4647d37adc17e1d3c242a42f6b68',1,'CO_LEDs.h']]], + ['co_5fled_5fcanopen_483',['CO_LED_CANopen',['../group__CO__LEDs.html#gga366de3822a3da8478e97b248bed641fbaea338dad48dda75ef1ebac0948093148',1,'CO_LEDs.h']]], + ['co_5fled_5fflash_5f1_484',['CO_LED_flash_1',['../group__CO__LEDs.html#gga366de3822a3da8478e97b248bed641fba88e905d94927b3c626b50a48537c7b73',1,'CO_LEDs.h']]], + ['co_5fled_5fflash_5f2_485',['CO_LED_flash_2',['../group__CO__LEDs.html#gga366de3822a3da8478e97b248bed641fbae751e14d72d829b2b7f9a9c1e98e0612',1,'CO_LEDs.h']]], + ['co_5fled_5fflash_5f3_486',['CO_LED_flash_3',['../group__CO__LEDs.html#gga366de3822a3da8478e97b248bed641fba250a08a71c4bbca9761f0dfa54d37938',1,'CO_LEDs.h']]], + ['co_5fled_5fflash_5f4_487',['CO_LED_flash_4',['../group__CO__LEDs.html#gga366de3822a3da8478e97b248bed641fbadd3ec0da4d999e5ffc107d7891c26667',1,'CO_LEDs.h']]], + ['co_5fled_5fflicker_488',['CO_LED_flicker',['../group__CO__LEDs.html#gga366de3822a3da8478e97b248bed641fba9838518b974c263a401a089901cdcf54',1,'CO_LEDs.h']]], + ['co_5fled_5fgreen_489',['CO_LED_GREEN',['../group__CO__LEDs.html#ga3e01e6ec590d6d5c7b887b48557498f4',1,'CO_LEDs.h']]], + ['co_5fled_5fred_490',['CO_LED_RED',['../group__CO__LEDs.html#ga38a415372f20b9444f254a205aa511e8',1,'CO_LEDs.h']]], + ['co_5fleds_2eh_491',['CO_LEDs.h',['../CO__LEDs_8h.html',1,'']]], + ['co_5fleds_5finit_492',['CO_LEDs_init',['../group__CO__LEDs.html#gadafdaf5de4c227a3df17cbcf1d81be69',1,'CO_LEDs.h']]], + ['co_5fleds_5fprocess_493',['CO_LEDs_process',['../group__CO__LEDs.html#gac008ef501f913c9df1ee79c4b071ad80',1,'CO_LEDs.h']]], + ['co_5fleds_5ft_494',['CO_LEDs_t',['../structCO__LEDs__t.html',1,'']]], + ['co_5flittle_5fendian_495',['CO_LITTLE_ENDIAN',['../group__CO__dataTypes.html#gaed3e1bffaf912485092fc20193705f35',1,'CO_driver.h']]], + ['co_5flock_5fcan_5fsend_496',['CO_LOCK_CAN_SEND',['../group__CO__critical__sections.html#ga7566ee901bbf1a0d76d771d72d2f826f',1,'CO_driver.h']]], + ['co_5flock_5femcy_497',['CO_LOCK_EMCY',['../group__CO__critical__sections.html#ga3052a84235f56d535a14705e0cfda799',1,'CO_driver.h']]], + ['co_5flock_5fod_498',['CO_LOCK_OD',['../group__CO__critical__sections.html#ga3850830931ced2bd3d7e15821572bbcc',1,'CO_driver.h']]], + ['co_5flss_2eh_499',['CO_LSS.h',['../CO__LSS_8h.html',1,'']]], + ['co_5flss_5faddress_5fequal_500',['CO_LSS_ADDRESS_EQUAL',['../group__CO__LSS.html#ga341d156c334f2a3c2523f03eb24f4710',1,'CO_LSS.h']]], + ['co_5flss_5faddress_5ft_501',['CO_LSS_address_t',['../unionCO__LSS__address__t.html',1,'']]], + ['co_5flss_5fbit_5ftiming_5f10_502',['CO_LSS_BIT_TIMING_10',['../group__CO__LSS.html#ggacb4c13e75306153eafd535e55ba0ca2ca7a3c4e05623e7f75fe3b622a9b0185c8',1,'CO_LSS.h']]], + ['co_5flss_5fbit_5ftiming_5f1000_503',['CO_LSS_BIT_TIMING_1000',['../group__CO__LSS.html#ggacb4c13e75306153eafd535e55ba0ca2ca4cc0aa1f074ddaa5db4fdc0ed437a0b7',1,'CO_LSS.h']]], + ['co_5flss_5fbit_5ftiming_5f125_504',['CO_LSS_BIT_TIMING_125',['../group__CO__LSS.html#ggacb4c13e75306153eafd535e55ba0ca2ca1d93a0699e4269d809404c40565133cc',1,'CO_LSS.h']]], + ['co_5flss_5fbit_5ftiming_5f20_505',['CO_LSS_BIT_TIMING_20',['../group__CO__LSS.html#ggacb4c13e75306153eafd535e55ba0ca2ca08fb8c70b5d185521959415ef731de82',1,'CO_LSS.h']]], + ['co_5flss_5fbit_5ftiming_5f250_506',['CO_LSS_BIT_TIMING_250',['../group__CO__LSS.html#ggacb4c13e75306153eafd535e55ba0ca2ca3fadf15163c4f45300045e3645bba3ea',1,'CO_LSS.h']]], + ['co_5flss_5fbit_5ftiming_5f50_507',['CO_LSS_BIT_TIMING_50',['../group__CO__LSS.html#ggacb4c13e75306153eafd535e55ba0ca2caf0e90aeacc6461bd2ace73be51fd5383',1,'CO_LSS.h']]], + ['co_5flss_5fbit_5ftiming_5f500_508',['CO_LSS_BIT_TIMING_500',['../group__CO__LSS.html#ggacb4c13e75306153eafd535e55ba0ca2caaa4d4f1a766516c6d97a81483001e8ef',1,'CO_LSS.h']]], + ['co_5flss_5fbit_5ftiming_5f800_509',['CO_LSS_BIT_TIMING_800',['../group__CO__LSS.html#ggacb4c13e75306153eafd535e55ba0ca2ca51413e9c20e1eb60f5cc160a4d30cffa',1,'CO_LSS.h']]], + ['co_5flss_5fbit_5ftiming_5fauto_510',['CO_LSS_BIT_TIMING_AUTO',['../group__CO__LSS.html#ggacb4c13e75306153eafd535e55ba0ca2ca12ca7fd4a3604cb8281fa7d58d1137e2',1,'CO_LSS.h']]], + ['co_5flss_5fbit_5ftiming_5fvalid_511',['CO_LSS_BIT_TIMING_VALID',['../group__CO__LSS.html#gaf1a3d7df8dcd93e4a0e0b29aa6b003a1',1,'CO_LSS.h']]], + ['co_5flss_5fbittimingtable_5ft_512',['CO_LSS_bitTimingTable_t',['../group__CO__LSS.html#gacb4c13e75306153eafd535e55ba0ca2c',1,'CO_LSS.h']]], + ['co_5flss_5fbittimingtablelookup_513',['CO_LSS_bitTimingTableLookup',['../group__CO__LSS.html#ga61bf679482f7d425ceb521ea80a0cc18',1,'CO_LSS.h']]], + ['co_5flss_5fcfg_5factivate_5fbit_5ftiming_514',['CO_LSS_CFG_ACTIVATE_BIT_TIMING',['../group__CO__LSS.html#ggacc7cba1fb1f1f595506751d6af385964a1046863405c6de85d0d86088d9c034cc',1,'CO_LSS.h']]], + ['co_5flss_5fcfg_5fbit_5ftiming_515',['CO_LSS_CFG_BIT_TIMING',['../group__CO__LSS.html#ggacc7cba1fb1f1f595506751d6af385964afd4b839204b66547e5ebb0e4ff9c4481',1,'CO_LSS.h']]], + ['co_5flss_5fcfg_5fbit_5ftiming_5fmanufacturer_516',['CO_LSS_CFG_BIT_TIMING_MANUFACTURER',['../group__CO__LSS.html#gga28b8651550d1719c38cd307f4ef0a8aca3f5cbbebba617a9c12a7ed919a541255',1,'CO_LSS.h']]], + ['co_5flss_5fcfg_5fbit_5ftiming_5fok_517',['CO_LSS_CFG_BIT_TIMING_OK',['../group__CO__LSS.html#gga28b8651550d1719c38cd307f4ef0a8aca028ded96599022c4416e3d8c0798456a',1,'CO_LSS.h']]], + ['co_5flss_5fcfg_5fbit_5ftiming_5fout_5fof_5frange_518',['CO_LSS_CFG_BIT_TIMING_OUT_OF_RANGE',['../group__CO__LSS.html#gga28b8651550d1719c38cd307f4ef0a8acac089dd862b289dfc3f6bae0f30409625',1,'CO_LSS.h']]], + ['co_5flss_5fcfg_5fnode_5fid_519',['CO_LSS_CFG_NODE_ID',['../group__CO__LSS.html#ggacc7cba1fb1f1f595506751d6af385964a777432f5b616a250a9db8bf7328b0a59',1,'CO_LSS.h']]], + ['co_5flss_5fcfg_5fnode_5fid_5fmanufacturer_520',['CO_LSS_CFG_NODE_ID_MANUFACTURER',['../group__CO__LSS.html#ggaf8a13f567f8f405e4aae68268ba5d0a5aa23e0ca77dfb47ff4a1d48ddfaebc98e',1,'CO_LSS.h']]], + ['co_5flss_5fcfg_5fnode_5fid_5fok_521',['CO_LSS_CFG_NODE_ID_OK',['../group__CO__LSS.html#ggaf8a13f567f8f405e4aae68268ba5d0a5abe91f1d0e99fa890fe69a3e60aab6c2b',1,'CO_LSS.h']]], + ['co_5flss_5fcfg_5fnode_5fid_5fout_5fof_5frange_522',['CO_LSS_CFG_NODE_ID_OUT_OF_RANGE',['../group__CO__LSS.html#ggaf8a13f567f8f405e4aae68268ba5d0a5a79c95a7e63e6ff09fcbe5494ef59eed5',1,'CO_LSS.h']]], + ['co_5flss_5fcfg_5fstore_523',['CO_LSS_CFG_STORE',['../group__CO__LSS.html#ggacc7cba1fb1f1f595506751d6af385964ab0a136e255e8c2c32984881487b414d9',1,'CO_LSS.h']]], + ['co_5flss_5fcfg_5fstore_5ffailed_524',['CO_LSS_CFG_STORE_FAILED',['../group__CO__LSS.html#gga1e4e8c43143125ebe8912de81464bd9fad7f12cd5d1125e97d7b9bacac4b80d69',1,'CO_LSS.h']]], + ['co_5flss_5fcfg_5fstore_5fmanufacturer_525',['CO_LSS_CFG_STORE_MANUFACTURER',['../group__CO__LSS.html#gga1e4e8c43143125ebe8912de81464bd9fa7ece37a5aabe812068efe6a2780f31cc',1,'CO_LSS.h']]], + ['co_5flss_5fcfg_5fstore_5fnot_5fsupported_526',['CO_LSS_CFG_STORE_NOT_SUPPORTED',['../group__CO__LSS.html#gga1e4e8c43143125ebe8912de81464bd9faf78b03384da05bedcc45016d10dc0c3b',1,'CO_LSS.h']]], + ['co_5flss_5fcfg_5fstore_5fok_527',['CO_LSS_CFG_STORE_OK',['../group__CO__LSS.html#gga1e4e8c43143125ebe8912de81464bd9fa0f0407dee97a1e5e5d26cc4c717103cc',1,'CO_LSS.h']]], + ['co_5flss_5fcfgbittiming_5ft_528',['CO_LSS_cfgBitTiming_t',['../group__CO__LSS.html#ga28b8651550d1719c38cd307f4ef0a8ac',1,'CO_LSS.h']]], + ['co_5flss_5fcfgnodeid_5ft_529',['CO_LSS_cfgNodeId_t',['../group__CO__LSS.html#gaf8a13f567f8f405e4aae68268ba5d0a5',1,'CO_LSS.h']]], + ['co_5flss_5fcfgstore_5ft_530',['CO_LSS_cfgStore_t',['../group__CO__LSS.html#ga1e4e8c43143125ebe8912de81464bd9f',1,'CO_LSS.h']]], + ['co_5flss_5fcs_5ft_531',['CO_LSS_cs_t',['../group__CO__LSS.html#gacc7cba1fb1f1f595506751d6af385964',1,'CO_LSS.h']]], + ['co_5flss_5ffastscan_5fbit0_532',['CO_LSS_FASTSCAN_BIT0',['../group__CO__LSS.html#gga65751e78ae5f2674cc7205e13967f7c0a2db8b358b4954e5989a347e1e308eb20',1,'CO_LSS.h']]], + ['co_5flss_5ffastscan_5fbit31_533',['CO_LSS_FASTSCAN_BIT31',['../group__CO__LSS.html#gga65751e78ae5f2674cc7205e13967f7c0a48f5e26f67114198d945f37f8f713979',1,'CO_LSS.h']]], + ['co_5flss_5ffastscan_5fbitcheck_534',['CO_LSS_fastscan_bitcheck',['../group__CO__LSS.html#ga65751e78ae5f2674cc7205e13967f7c0',1,'CO_LSS.h']]], + ['co_5flss_5ffastscan_5fconfirm_535',['CO_LSS_FASTSCAN_CONFIRM',['../group__CO__LSS.html#gga65751e78ae5f2674cc7205e13967f7c0a72f67194903c03b688373ef859b66a0f',1,'CO_LSS.h']]], + ['co_5flss_5ffastscan_5flss_5fsub_5fnext_536',['CO_LSS_fastscan_lss_sub_next',['../group__CO__LSS.html#ga1ce707d287b285e7d148f37f93e0f02a',1,'CO_LSS.h']]], + ['co_5flss_5ffastscan_5fproduct_537',['CO_LSS_FASTSCAN_PRODUCT',['../group__CO__LSS.html#gga1ce707d287b285e7d148f37f93e0f02aa6514ca82752d5496904388a0589da209',1,'CO_LSS.h']]], + ['co_5flss_5ffastscan_5frev_538',['CO_LSS_FASTSCAN_REV',['../group__CO__LSS.html#gga1ce707d287b285e7d148f37f93e0f02aa0e153eebb470156f5d0a27caac2bc71f',1,'CO_LSS.h']]], + ['co_5flss_5ffastscan_5fserial_539',['CO_LSS_FASTSCAN_SERIAL',['../group__CO__LSS.html#gga1ce707d287b285e7d148f37f93e0f02aabe98c25d444fa5971bc81f775cd6bb35',1,'CO_LSS.h']]], + ['co_5flss_5ffastscan_5fvendor_5fid_540',['CO_LSS_FASTSCAN_VENDOR_ID',['../group__CO__LSS.html#gga1ce707d287b285e7d148f37f93e0f02aa4eb5786c488953cdb2d5ffbb25c15298',1,'CO_LSS.h']]], + ['co_5flss_5fident_5ffastscan_541',['CO_LSS_IDENT_FASTSCAN',['../group__CO__LSS.html#ggacc7cba1fb1f1f595506751d6af385964a9e4bd7f4a726aee66157ac9aac446ddc',1,'CO_LSS.h']]], + ['co_5flss_5fident_5fslave_542',['CO_LSS_IDENT_SLAVE',['../group__CO__LSS.html#ggacc7cba1fb1f1f595506751d6af385964ad0ddd8e3d472f4d85de8613e7f35902a',1,'CO_LSS.h']]], + ['co_5flss_5finquire_5fnode_5fid_543',['CO_LSS_INQUIRE_NODE_ID',['../group__CO__LSS.html#ggacc7cba1fb1f1f595506751d6af385964af5d369fc4d3d860dc43de041b9fd59f5',1,'CO_LSS.h']]], + ['co_5flss_5finquire_5fproduct_544',['CO_LSS_INQUIRE_PRODUCT',['../group__CO__LSS.html#ggacc7cba1fb1f1f595506751d6af385964a5fba5f8bd0f6a91b45fda117556b994c',1,'CO_LSS.h']]], + ['co_5flss_5finquire_5frev_545',['CO_LSS_INQUIRE_REV',['../group__CO__LSS.html#ggacc7cba1fb1f1f595506751d6af385964a6f412845cd8cd4ab62a54a988ccc384c',1,'CO_LSS.h']]], + ['co_5flss_5finquire_5fserial_546',['CO_LSS_INQUIRE_SERIAL',['../group__CO__LSS.html#ggacc7cba1fb1f1f595506751d6af385964a55f0698aa15abf17c41cd344df055184',1,'CO_LSS.h']]], + ['co_5flss_5finquire_5fvendor_547',['CO_LSS_INQUIRE_VENDOR',['../group__CO__LSS.html#ggacc7cba1fb1f1f595506751d6af385964ae28351cd1b60bbdd045a9f79cb506023',1,'CO_LSS.h']]], + ['co_5flss_5fnode_5fid_5fassignment_548',['CO_LSS_NODE_ID_ASSIGNMENT',['../group__CO__LSS.html#ga02771497ab59dd86f2dbe59cd1fb04b1',1,'CO_LSS.h']]], + ['co_5flss_5fnode_5fid_5fvalid_549',['CO_LSS_NODE_ID_VALID',['../group__CO__LSS.html#ga939b17fdd44126fb758db46b9cadf79c',1,'CO_LSS.h']]], + ['co_5flss_5fstate_5fconfiguration_550',['CO_LSS_STATE_CONFIGURATION',['../group__CO__LSS.html#ggaaa9a270e40ea09850e1661e5aeb080ada69cc3fe20e50dcd6f7bad8c0b887ff89',1,'CO_LSS.h']]], + ['co_5flss_5fstate_5ft_551',['CO_LSS_state_t',['../group__CO__LSS.html#gaaa9a270e40ea09850e1661e5aeb080ad',1,'CO_LSS.h']]], + ['co_5flss_5fstate_5fwaiting_552',['CO_LSS_STATE_WAITING',['../group__CO__LSS.html#ggaaa9a270e40ea09850e1661e5aeb080adac688ea4e90b7dfac437a44536f2af8db',1,'CO_LSS.h']]], + ['co_5flss_5fswitch_5fstate_5fglobal_553',['CO_LSS_SWITCH_STATE_GLOBAL',['../group__CO__LSS.html#ggacc7cba1fb1f1f595506751d6af385964a6c9f8aaef024d11e50c007b881208113',1,'CO_LSS.h']]], + ['co_5flss_5fswitch_5fstate_5fsel_554',['CO_LSS_SWITCH_STATE_SEL',['../group__CO__LSS.html#ggacc7cba1fb1f1f595506751d6af385964a97495fe55645d498550e0c05417e2c22',1,'CO_LSS.h']]], + ['co_5flss_5fswitch_5fstate_5fsel_5fproduct_555',['CO_LSS_SWITCH_STATE_SEL_PRODUCT',['../group__CO__LSS.html#ggacc7cba1fb1f1f595506751d6af385964a9caf25d2d4a28e279b1bc364d303ee7d',1,'CO_LSS.h']]], + ['co_5flss_5fswitch_5fstate_5fsel_5frev_556',['CO_LSS_SWITCH_STATE_SEL_REV',['../group__CO__LSS.html#ggacc7cba1fb1f1f595506751d6af385964ae02d526a2c1170babeccffd00d477db5',1,'CO_LSS.h']]], + ['co_5flss_5fswitch_5fstate_5fsel_5fserial_557',['CO_LSS_SWITCH_STATE_SEL_SERIAL',['../group__CO__LSS.html#ggacc7cba1fb1f1f595506751d6af385964add363bec1d5ff239c847425f8b94718d',1,'CO_LSS.h']]], + ['co_5flss_5fswitch_5fstate_5fsel_5fvendor_558',['CO_LSS_SWITCH_STATE_SEL_VENDOR',['../group__CO__LSS.html#ggacc7cba1fb1f1f595506751d6af385964a620895a76069780eb5df8188b6c8a2de',1,'CO_LSS.h']]], + ['co_5flssinit_559',['CO_LSSinit',['../group__CO__CANopen.html#ga0e11332fd28af597bc47b21327cdc8a3',1,'CANopen.h']]], + ['co_5flssmaster_2eh_560',['CO_LSSmaster.h',['../CO__LSSmaster_8h.html',1,'']]], + ['co_5flssmaster_5factivatebit_561',['CO_LSSmaster_ActivateBit',['../group__CO__LSSmaster.html#gaa016c0f3dc4dd021801b6139765657ab',1,'CO_LSSmaster.h']]], + ['co_5flssmaster_5fchangetimeout_562',['CO_LSSmaster_changeTimeout',['../group__CO__LSSmaster.html#gae22758aff11b796cfaed979c5f2808c0',1,'CO_LSSmaster.h']]], + ['co_5flssmaster_5fconfigurebittiming_563',['CO_LSSmaster_configureBitTiming',['../group__CO__LSSmaster.html#ga71a5d90e569ee7e88763a541c286e240',1,'CO_LSSmaster.h']]], + ['co_5flssmaster_5fconfigurenodeid_564',['CO_LSSmaster_configureNodeId',['../group__CO__LSSmaster.html#ga2cdba08d9a564c4a61ebbcd0d10342fd',1,'CO_LSSmaster.h']]], + ['co_5flssmaster_5fconfigurestore_565',['CO_LSSmaster_configureStore',['../group__CO__LSSmaster.html#gacea091d379a5338f13963eb745b25b16',1,'CO_LSSmaster.h']]], + ['co_5flssmaster_5fdefault_5ftimeout_566',['CO_LSSmaster_DEFAULT_TIMEOUT',['../group__CO__LSSmaster.html#ga76adbab914fff5b18982dad449e0a00a',1,'CO_LSSmaster.h']]], + ['co_5flssmaster_5ffastscan_5ft_567',['CO_LSSmaster_fastscan_t',['../structCO__LSSmaster__fastscan__t.html',1,'']]], + ['co_5flssmaster_5ffs_5fmatch_568',['CO_LSSmaster_FS_MATCH',['../group__CO__LSSmaster.html#gga6e3f0d07f0712c371fb81cbf4a3dbcb1ad5a936fd00345aea75e8917f99df4ab3',1,'CO_LSSmaster.h']]], + ['co_5flssmaster_5ffs_5fscan_569',['CO_LSSmaster_FS_SCAN',['../group__CO__LSSmaster.html#gga6e3f0d07f0712c371fb81cbf4a3dbcb1a5666c53e2fc02e214294bd3210146c90',1,'CO_LSSmaster.h']]], + ['co_5flssmaster_5ffs_5fskip_570',['CO_LSSmaster_FS_SKIP',['../group__CO__LSSmaster.html#gga6e3f0d07f0712c371fb81cbf4a3dbcb1ae0e6e92e760129fc173a0a4f19a0cf07',1,'CO_LSSmaster.h']]], + ['co_5flssmaster_5fidentifyfastscan_571',['CO_LSSmaster_IdentifyFastscan',['../group__CO__LSSmaster.html#gad01ce178ea43b1843f541d4dd488f90e',1,'CO_LSSmaster.h']]], + ['co_5flssmaster_5fillegal_5fargument_572',['CO_LSSmaster_ILLEGAL_ARGUMENT',['../group__CO__LSSmaster.html#ggae848ff3ff649c8a23b96053efaea4985a9bd04e1c9923416d1d1ecb1ded6bc7b9',1,'CO_LSSmaster.h']]], + ['co_5flssmaster_5finit_573',['CO_LSSmaster_init',['../group__CO__LSSmaster.html#ga0675297a7e7e1f472ad2e88d6b7408e7',1,'CO_LSSmaster.h']]], + ['co_5flssmaster_5finitcallbackpre_574',['CO_LSSmaster_initCallbackPre',['../group__CO__LSSmaster.html#gabfeb7e75d88b76bb00c1740381c7b53f',1,'CO_LSSmaster.h']]], + ['co_5flssmaster_5finquire_575',['CO_LSSmaster_Inquire',['../group__CO__LSSmaster.html#ga22414a7184ca0c9d371dd67e9990d820',1,'CO_LSSmaster.h']]], + ['co_5flssmaster_5finquirelssaddress_576',['CO_LSSmaster_InquireLssAddress',['../group__CO__LSSmaster.html#ga1b0a5c9e27e046736c6ec55a0256ed77',1,'CO_LSSmaster.h']]], + ['co_5flssmaster_5finvalid_5fstate_577',['CO_LSSmaster_INVALID_STATE',['../group__CO__LSSmaster.html#ggae848ff3ff649c8a23b96053efaea4985a1e99145e94c3a6fb8cc7c48150ed5b60',1,'CO_LSSmaster.h']]], + ['co_5flssmaster_5fok_578',['CO_LSSmaster_OK',['../group__CO__LSSmaster.html#ggae848ff3ff649c8a23b96053efaea4985a1be85610f29d8e6cecebac9db2da3099',1,'CO_LSSmaster.h']]], + ['co_5flssmaster_5fok_5fillegal_5fargument_579',['CO_LSSmaster_OK_ILLEGAL_ARGUMENT',['../group__CO__LSSmaster.html#ggae848ff3ff649c8a23b96053efaea4985ab6985b2cbcb24653ec1f2ae46d3c09cd',1,'CO_LSSmaster.h']]], + ['co_5flssmaster_5fok_5fmanufacturer_580',['CO_LSSmaster_OK_MANUFACTURER',['../group__CO__LSSmaster.html#ggae848ff3ff649c8a23b96053efaea4985a51083a5cf04e03a2f623eddb1d7324c7',1,'CO_LSSmaster.h']]], + ['co_5flssmaster_5freturn_5ft_581',['CO_LSSmaster_return_t',['../group__CO__LSSmaster.html#gae848ff3ff649c8a23b96053efaea4985',1,'CO_LSSmaster.h']]], + ['co_5flssmaster_5fscan_5ffailed_582',['CO_LSSmaster_SCAN_FAILED',['../group__CO__LSSmaster.html#ggae848ff3ff649c8a23b96053efaea4985af84e0db6bd9aeebffc4266618145a8ea',1,'CO_LSSmaster.h']]], + ['co_5flssmaster_5fscan_5ffinished_583',['CO_LSSmaster_SCAN_FINISHED',['../group__CO__LSSmaster.html#ggae848ff3ff649c8a23b96053efaea4985a0f527d1ce01820fa184ccae2510505c7',1,'CO_LSSmaster.h']]], + ['co_5flssmaster_5fscan_5fnoack_584',['CO_LSSmaster_SCAN_NOACK',['../group__CO__LSSmaster.html#ggae848ff3ff649c8a23b96053efaea4985ae9eb103f25dbe694143a64e7bb2c29d9',1,'CO_LSSmaster.h']]], + ['co_5flssmaster_5fscantype_5ft_585',['CO_LSSmaster_scantype_t',['../group__CO__LSSmaster.html#ga6e3f0d07f0712c371fb81cbf4a3dbcb1',1,'CO_LSSmaster.h']]], + ['co_5flssmaster_5fswitchstatedeselect_586',['CO_LSSmaster_switchStateDeselect',['../group__CO__LSSmaster.html#gac0e13ec42e1fd85da5ddef6f654ef1a4',1,'CO_LSSmaster.h']]], + ['co_5flssmaster_5fswitchstateselect_587',['CO_LSSmaster_switchStateSelect',['../group__CO__LSSmaster.html#ga41b4288c03af394261541b9a8395e8f3',1,'CO_LSSmaster.h']]], + ['co_5flssmaster_5ft_588',['CO_LSSmaster_t',['../structCO__LSSmaster__t.html',1,'']]], + ['co_5flssmaster_5ftimeout_589',['CO_LSSmaster_TIMEOUT',['../group__CO__LSSmaster.html#ggae848ff3ff649c8a23b96053efaea4985a3a61073ef2c8ef7c5be946618b95d42d',1,'CO_LSSmaster.h']]], + ['co_5flssmaster_5fwait_5fslave_590',['CO_LSSmaster_WAIT_SLAVE',['../group__CO__LSSmaster.html#ggae848ff3ff649c8a23b96053efaea4985a20fbe514e36bd92534141bb75e68eb34',1,'CO_LSSmaster.h']]], + ['co_5flssslave_2eh_591',['CO_LSSslave.h',['../CO__LSSslave_8h.html',1,'']]], + ['co_5flssslave_5fgetstate_592',['CO_LSSslave_getState',['../group__CO__LSSslave.html#ga2692ff0d6837db494c029a3bef735ee7',1,'CO_LSSslave.h']]], + ['co_5flssslave_5finit_593',['CO_LSSslave_init',['../group__CO__LSSslave.html#gaaba1fafcd0024609f8a72be4810baf66',1,'CO_LSSslave.h']]], + ['co_5flssslave_5finitactivatebitratecallback_594',['CO_LSSslave_initActivateBitRateCallback',['../group__CO__LSSslave.html#ga0253fffcb36ab6b850563328784a8a5a',1,'CO_LSSslave.h']]], + ['co_5flssslave_5finitcallbackpre_595',['CO_LSSslave_initCallbackPre',['../group__CO__LSSslave.html#ga64c23178117a707046eb15ebc6506429',1,'CO_LSSslave.h']]], + ['co_5flssslave_5finitcfgstorecallback_596',['CO_LSSslave_initCfgStoreCallback',['../group__CO__LSSslave.html#gadc6187357904293da0a35317f15d0666',1,'CO_LSSslave.h']]], + ['co_5flssslave_5finitcheckbitratecallback_597',['CO_LSSslave_initCheckBitRateCallback',['../group__CO__LSSslave.html#ga665f147d6fae6db71173c4a8d602495c',1,'CO_LSSslave.h']]], + ['co_5flssslave_5fprocess_598',['CO_LSSslave_process',['../group__CO__LSSslave.html#gae19d7ad84333f1a3f40ecbdbf639e017',1,'CO_LSSslave.h']]], + ['co_5flssslave_5ft_599',['CO_LSSslave_t',['../structCO__LSSslave__t.html',1,'']]], + ['co_5fmultiple_5fod_600',['CO_MULTIPLE_OD',['../group__CO__CANopen.html#gae3927c69d6937caf2b67ac49c5e41982',1,'CANopen.h']]], + ['co_5fnew_601',['CO_new',['../group__CO__CANopen.html#gaf6ef29daa2063de90b4799ae795c7027',1,'CANopen.h']]], + ['co_5fnmt_5fcommand_5ft_602',['CO_NMT_command_t',['../group__CO__NMT__Heartbeat.html#gac396242e2e12ef6b0b22ff48636bc4eb',1,'CO_NMT_Heartbeat.h']]], + ['co_5fnmt_5fcontrol_5ft_603',['CO_NMT_control_t',['../group__CO__NMT__Heartbeat.html#gaf92cf5943801e5dda84654345cc3d67f',1,'CO_NMT_Heartbeat.h']]], + ['co_5fnmt_5fenter_5foperational_604',['CO_NMT_ENTER_OPERATIONAL',['../group__CO__NMT__Heartbeat.html#ggac396242e2e12ef6b0b22ff48636bc4eba6c7cdae79e5939cf321cdf82aa6b4146',1,'CO_NMT_Heartbeat.h']]], + ['co_5fnmt_5fenter_5fpre_5foperational_605',['CO_NMT_ENTER_PRE_OPERATIONAL',['../group__CO__NMT__Heartbeat.html#ggac396242e2e12ef6b0b22ff48636bc4eba575ed4f3386e8a63498921e659aa7ba4',1,'CO_NMT_Heartbeat.h']]], + ['co_5fnmt_5fenter_5fstopped_606',['CO_NMT_ENTER_STOPPED',['../group__CO__NMT__Heartbeat.html#ggac396242e2e12ef6b0b22ff48636bc4eba8a2911a2a576a1bd83b7f4a247cd77c9',1,'CO_NMT_Heartbeat.h']]], + ['co_5fnmt_5ferr_5ffree_5fto_5foperational_607',['CO_NMT_ERR_FREE_TO_OPERATIONAL',['../group__CO__NMT__Heartbeat.html#ggaf92cf5943801e5dda84654345cc3d67fa6e79d137c725417786035982cd0e2687',1,'CO_NMT_Heartbeat.h']]], + ['co_5fnmt_5ferr_5fon_5fbusoff_5fhb_608',['CO_NMT_ERR_ON_BUSOFF_HB',['../group__CO__NMT__Heartbeat.html#ggaf92cf5943801e5dda84654345cc3d67faf151918ecf07fc43ba30489d31ca275a',1,'CO_NMT_Heartbeat.h']]], + ['co_5fnmt_5ferr_5fon_5ferr_5freg_609',['CO_NMT_ERR_ON_ERR_REG',['../group__CO__NMT__Heartbeat.html#ggaf92cf5943801e5dda84654345cc3d67faab1cd82001a087114a5361910e206c74',1,'CO_NMT_Heartbeat.h']]], + ['co_5fnmt_5ferr_5freg_5fmask_610',['CO_NMT_ERR_REG_MASK',['../group__CO__NMT__Heartbeat.html#ggaf92cf5943801e5dda84654345cc3d67faaa5ab06e2047e51ac7d2767e8a821d80',1,'CO_NMT_Heartbeat.h']]], + ['co_5fnmt_5ferr_5fto_5fstopped_611',['CO_NMT_ERR_TO_STOPPED',['../group__CO__NMT__Heartbeat.html#ggaf92cf5943801e5dda84654345cc3d67fa9498a5af4e0557a2c0284c04b26b3eb5',1,'CO_NMT_Heartbeat.h']]], + ['co_5fnmt_5fgetinternalstate_612',['CO_NMT_getInternalState',['../group__CO__NMT__Heartbeat.html#ga2e3f0d579e321d73b53bf469d49b0ce3',1,'CO_NMT_Heartbeat.h']]], + ['co_5fnmt_5fheartbeat_2eh_613',['CO_NMT_Heartbeat.h',['../CO__NMT__Heartbeat_8h.html',1,'']]], + ['co_5fnmt_5finit_614',['CO_NMT_init',['../group__CO__NMT__Heartbeat.html#ga53c92521fff0fab405f8a372702c7259',1,'CO_NMT_Heartbeat.h']]], + ['co_5fnmt_5finitcallbackchanged_615',['CO_NMT_initCallbackChanged',['../group__CO__NMT__Heartbeat.html#ga87d153e39b32586b67ee6dd47dfce787',1,'CO_NMT_Heartbeat.h']]], + ['co_5fnmt_5finitcallbackpre_616',['CO_NMT_initCallbackPre',['../group__CO__NMT__Heartbeat.html#ga58b6123938e950b5e8c38ec0b9caeec4',1,'CO_NMT_Heartbeat.h']]], + ['co_5fnmt_5finitializing_617',['CO_NMT_INITIALIZING',['../group__CO__NMT__Heartbeat.html#gga1e8c2a6c0fd4a33183503d25a7c6d744a672c48c0e9bd5cb070bd86e1dcdc085c',1,'CO_NMT_Heartbeat.h']]], + ['co_5fnmt_5finternalstate_5ft_618',['CO_NMT_internalState_t',['../group__CO__NMT__Heartbeat.html#ga1e8c2a6c0fd4a33183503d25a7c6d744',1,'CO_NMT_Heartbeat.h']]], + ['co_5fnmt_5foperational_619',['CO_NMT_OPERATIONAL',['../group__CO__NMT__Heartbeat.html#gga1e8c2a6c0fd4a33183503d25a7c6d744a3f9d07a10f2781201b85e9de9ba1c9a5',1,'CO_NMT_Heartbeat.h']]], + ['co_5fnmt_5fpre_5foperational_620',['CO_NMT_PRE_OPERATIONAL',['../group__CO__NMT__Heartbeat.html#gga1e8c2a6c0fd4a33183503d25a7c6d744ae41d4ebcb7e26e165c8685b5809871a9',1,'CO_NMT_Heartbeat.h']]], + ['co_5fnmt_5fprocess_621',['CO_NMT_process',['../group__CO__NMT__Heartbeat.html#ga724ff5b1d9cfee955914f365514deda0',1,'CO_NMT_process(CO_NMT_t *NMT, CO_NMT_internalState_t *NMTstate, uint32_t timeDifference_us, uint32_t *timerNext_us): CO_NMT_Heartbeat.c'],['../group__CO__NMT__Heartbeat.html#ga724ff5b1d9cfee955914f365514deda0',1,'CO_NMT_process(CO_NMT_t *NMT, CO_NMT_internalState_t *NMTstate, uint32_t timeDifference_us, uint32_t *timerNext_us): CO_NMT_Heartbeat.c']]], + ['co_5fnmt_5freset_5fcmd_5ft_622',['CO_NMT_reset_cmd_t',['../group__CO__NMT__Heartbeat.html#gaf42f056a571b8e17a2d74428d1a49674',1,'CO_NMT_Heartbeat.h']]], + ['co_5fnmt_5freset_5fcommunication_623',['CO_NMT_RESET_COMMUNICATION',['../group__CO__NMT__Heartbeat.html#ggac396242e2e12ef6b0b22ff48636bc4ebabc6aef9d84c816eecaea38f9fe2cbd9c',1,'CO_NMT_Heartbeat.h']]], + ['co_5fnmt_5freset_5fnode_624',['CO_NMT_RESET_NODE',['../group__CO__NMT__Heartbeat.html#ggac396242e2e12ef6b0b22ff48636bc4ebaa8558d18a80148a598e6295ef41cdf97',1,'CO_NMT_Heartbeat.h']]], + ['co_5fnmt_5fsendcommand_625',['CO_NMT_sendCommand',['../group__CO__NMT__Heartbeat.html#ga9d557708be1bfb9b169718abea90b663',1,'CO_NMT_Heartbeat.h']]], + ['co_5fnmt_5fsetinternalstate_626',['CO_NMT_setInternalState',['../group__CO__NMT__Heartbeat.html#ga91b7041eda0487bbc0588d3200e0e868',1,'CO_NMT_Heartbeat.h']]], + ['co_5fnmt_5fstartup_5fto_5foperational_627',['CO_NMT_STARTUP_TO_OPERATIONAL',['../group__CO__NMT__Heartbeat.html#ggaf92cf5943801e5dda84654345cc3d67faf4da9fa5d805e0f535abf95a175312e8',1,'CO_NMT_Heartbeat.h']]], + ['co_5fnmt_5fstopped_628',['CO_NMT_STOPPED',['../group__CO__NMT__Heartbeat.html#gga1e8c2a6c0fd4a33183503d25a7c6d744aba34c0b4b2c6a0c11b01236cbe949ed5',1,'CO_NMT_Heartbeat.h']]], + ['co_5fnmt_5ft_629',['CO_NMT_t',['../structCO__NMT__t.html',1,'']]], + ['co_5fnmt_5funknown_630',['CO_NMT_UNKNOWN',['../group__CO__NMT__Heartbeat.html#gga1e8c2a6c0fd4a33183503d25a7c6d744a52d111d4262b514df4433ae07969cc3c',1,'CO_NMT_Heartbeat.h']]], + ['co_5fod_5fstorage_2eh_631',['CO_OD_storage.h',['../CO__OD__storage_8h.html',1,'']]], + ['co_5fod_5fstorage_5fautosave_632',['CO_OD_storage_autoSave',['../group__CO__socketCAN__OD__storage.html#ga9381d4f670cef9068efaf7d42097de2f',1,'CO_OD_storage_autoSave(CO_OD_storage_t *odStor, uint32_t timer1usDiff, uint32_t delay_us): CO_OD_storage.c'],['../group__CO__socketCAN__OD__storage.html#ga9381d4f670cef9068efaf7d42097de2f',1,'CO_OD_storage_autoSave(CO_OD_storage_t *odStor, uint32_t timer1usDiff, uint32_t delay_us): CO_OD_storage.c']]], + ['co_5fod_5fstorage_5fautosaveclose_633',['CO_OD_storage_autoSaveClose',['../group__CO__socketCAN__OD__storage.html#ga6c51a86516ff7e128873f6cf1fdef5eb',1,'CO_OD_storage_autoSaveClose(CO_OD_storage_t *odStor): CO_OD_storage.c'],['../group__CO__socketCAN__OD__storage.html#ga6c51a86516ff7e128873f6cf1fdef5eb',1,'CO_OD_storage_autoSaveClose(CO_OD_storage_t *odStor): CO_OD_storage.c']]], + ['co_5fod_5fstorage_5finit_634',['CO_OD_storage_init',['../group__CO__socketCAN__OD__storage.html#ga5a26b63e7222b058c6024e54c6e0d5cd',1,'CO_OD_storage_init(CO_OD_storage_t *odStor, uint8_t *odAddress, uint32_t odSize, char *filename): CO_OD_storage.c'],['../group__CO__socketCAN__OD__storage.html#ga5a26b63e7222b058c6024e54c6e0d5cd',1,'CO_OD_storage_init(CO_OD_storage_t *odStor, uint8_t *odAddress, uint32_t odSize, char *filename): CO_OD_storage.c']]], + ['co_5fod_5fstorage_5frestoresecure_635',['CO_OD_storage_restoreSecure',['../group__CO__socketCAN__OD__storage.html#ga63b392fa7eb2bdc92ecd2f1ff6f4ced0',1,'CO_OD_storage_restoreSecure(char *filename): CO_OD_storage.c'],['../group__CO__socketCAN__OD__storage.html#ga63b392fa7eb2bdc92ecd2f1ff6f4ced0',1,'CO_OD_storage_restoreSecure(char *filename): CO_OD_storage.c']]], + ['co_5fod_5fstorage_5fsavesecure_636',['CO_OD_storage_saveSecure',['../group__CO__socketCAN__OD__storage.html#ga7f6124c9079807bc2f8f3d860571ccec',1,'CO_OD_storage_saveSecure(uint8_t *odAddress, uint32_t odSize, char *filename): CO_OD_storage.c'],['../group__CO__socketCAN__OD__storage.html#ga7f6124c9079807bc2f8f3d860571ccec',1,'CO_OD_storage_saveSecure(uint8_t *odAddress, uint32_t odSize, char *filename): CO_OD_storage.c']]], + ['co_5fod_5fstorage_5ft_637',['CO_OD_storage_t',['../structCO__OD__storage__t.html',1,'']]], + ['co_5fodf_5f1010_638',['CO_ODF_1010',['../group__CO__socketCAN__OD__storage.html#ga4a5e807a83eeab172bb3b0aeb6fa92c2',1,'CO_ODF_1010(CO_ODF_arg_t *ODF_arg): CO_OD_storage.c'],['../group__CO__socketCAN__OD__storage.html#ga4a5e807a83eeab172bb3b0aeb6fa92c2',1,'CO_ODF_1010(CO_ODF_arg_t *ODF_arg): CO_OD_storage.c']]], + ['co_5fodf_5f1011_639',['CO_ODF_1011',['../group__CO__socketCAN__OD__storage.html#ga059fcd46d8b15caf86c57d541a09576a',1,'CO_ODF_1011(CO_ODF_arg_t *ODF_arg): CO_OD_storage.c'],['../group__CO__socketCAN__OD__storage.html#ga059fcd46d8b15caf86c57d541a09576a',1,'CO_ODF_1011(CO_ODF_arg_t *ODF_arg): CO_OD_storage.c']]], + ['co_5fodinterface_2eh_640',['CO_ODinterface.h',['../CO__ODinterface_8h.html',1,'']]], + ['co_5fpdo_2eh_641',['CO_PDO.h',['../CO__PDO_8h.html',1,'']]], + ['co_5fprocess_642',['CO_process',['../group__CO__CANopen.html#ga895d7fad40b60aacdac3cb0615729b5e',1,'CANopen.h']]], + ['co_5fprocess_5frpdo_643',['CO_process_RPDO',['../group__CO__CANopen.html#ga4318848921c35e8bb5a7d97dca5668a0',1,'CANopen.h']]], + ['co_5fprocess_5fsrdo_644',['CO_process_SRDO',['../group__CO__CANopen.html#gab76d7283fe5190d3a0009b423a9ba8b1',1,'CANopen.h']]], + ['co_5fprocess_5fsync_645',['CO_process_SYNC',['../group__CO__CANopen.html#gaad5c15c3ca475912661f512d37413b12',1,'CANopen.h']]], + ['co_5fprocess_5ftpdo_646',['CO_process_TPDO',['../group__CO__CANopen.html#ga8c62a2afd2762d99e9c9be13a3d9a7a8',1,'CANopen.h']]], + ['co_5freset_5fapp_647',['CO_RESET_APP',['../group__CO__NMT__Heartbeat.html#ggaf42f056a571b8e17a2d74428d1a49674a2ab187bf1daccd4bc31685c08ca93bf1',1,'CO_NMT_Heartbeat.h']]], + ['co_5freset_5fcomm_648',['CO_RESET_COMM',['../group__CO__NMT__Heartbeat.html#ggaf42f056a571b8e17a2d74428d1a49674a2a48dc2be7c9ffa7874eb08a1ba39f6f',1,'CO_NMT_Heartbeat.h']]], + ['co_5freset_5fnot_649',['CO_RESET_NOT',['../group__CO__NMT__Heartbeat.html#ggaf42f056a571b8e17a2d74428d1a49674a1e75deb437b2e06b2c1f353bf76d8807',1,'CO_NMT_Heartbeat.h']]], + ['co_5freset_5fquit_650',['CO_RESET_QUIT',['../group__CO__NMT__Heartbeat.html#ggaf42f056a571b8e17a2d74428d1a49674ae2c7805e42ce9b7c893018922b24a102',1,'CO_NMT_Heartbeat.h']]], + ['co_5freturnerror_5ft_651',['CO_ReturnError_t',['../group__CO__driver.html#ga1cb2d3466eb0c6d267f3b5ff1a0d9532',1,'CO_driver.h']]], + ['co_5frpdo_5finit_652',['CO_RPDO_init',['../group__CO__PDO.html#ga92c484ada2ad240c1b8c891c88d56901',1,'CO_PDO.h']]], + ['co_5frpdo_5finitcallbackpre_653',['CO_RPDO_initCallbackPre',['../group__CO__PDO.html#ga34532746ccf88ccfa835716e89369478',1,'CO_PDO.h']]], + ['co_5frpdo_5fprocess_654',['CO_RPDO_process',['../group__CO__PDO.html#gad77bfd4c7f64e75e7ddee5c926477e66',1,'CO_PDO.h']]], + ['co_5frpdo_5ft_655',['CO_RPDO_t',['../structCO__RPDO__t.html',1,'']]], + ['co_5frpdocommpar_5ft_656',['CO_RPDOCommPar_t',['../structCO__RPDOCommPar__t.html',1,'']]], + ['co_5frpdomappar_5ft_657',['CO_RPDOMapPar_t',['../structCO__RPDOMapPar__t.html',1,'']]], + ['co_5fsdo_5fab_5fblock_5fsize_658',['CO_SDO_AB_BLOCK_SIZE',['../group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979ac86b70b71d601658c93a1dd270a902b0',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fab_5fcmd_659',['CO_SDO_AB_CMD',['../group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979a26b4e2680c16ce6a09d3e3a8293472ce',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fab_5fcrc_660',['CO_SDO_AB_CRC',['../group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979aee7fcab60a6fde6e41d999f5a2b10aa5',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fab_5fdata_5fdev_5fstate_661',['CO_SDO_AB_DATA_DEV_STATE',['../group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979ac92ccaa16d833cac6d2f6d8c2836d886',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fab_5fdata_5floc_5fctrl_662',['CO_SDO_AB_DATA_LOC_CTRL',['../group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979ac489bb77a98f65008932861924bc4bbf',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fab_5fdata_5flong_663',['CO_SDO_AB_DATA_LONG',['../group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979a50d373f7a7ba976dc2277a2111cf56c3',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fab_5fdata_5fod_664',['CO_SDO_AB_DATA_OD',['../group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979aec1840b00621e92f27da2d0705ddab63',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fab_5fdata_5fshort_665',['CO_SDO_AB_DATA_SHORT',['../group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979a481537e4c170066ca31b167fa598bb54',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fab_5fdata_5ftransf_666',['CO_SDO_AB_DATA_TRANSF',['../group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979a631a043a79c7eef4ddb2f874365c6660',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fab_5fdevice_5fincompat_667',['CO_SDO_AB_DEVICE_INCOMPAT',['../group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979ad07acc06f76122627412a71f2f2e39fc',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fab_5fgeneral_668',['CO_SDO_AB_GENERAL',['../group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979a58d6be7d156bbe576b8438a6fd5b446d',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fab_5fhw_669',['CO_SDO_AB_HW',['../group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979a070f096bb09f5a6235643702b5a40759',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fab_5finvalid_5fvalue_670',['CO_SDO_AB_INVALID_VALUE',['../group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979afff1ec491c628031e65672383f3e3c76',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fab_5fmap_5flen_671',['CO_SDO_AB_MAP_LEN',['../group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979a07edee9ce8ec5cd01cfd3cfbff48b96c',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fab_5fmax_5fless_5fmin_672',['CO_SDO_AB_MAX_LESS_MIN',['../group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979a15d49829c0d15f8cb9995f07617d874f',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fab_5fno_5fdata_673',['CO_SDO_AB_NO_DATA',['../group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979a3e007eeec7538b5dbe7e78240632b415',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fab_5fno_5fmap_674',['CO_SDO_AB_NO_MAP',['../group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979a729452df9557e4acbda8691efb4da310',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fab_5fno_5fresource_675',['CO_SDO_AB_NO_RESOURCE',['../group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979ab54dd042727804cd8f310a04fd4575f7',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fab_5fnone_676',['CO_SDO_AB_NONE',['../group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979a5fc84558a4ca47e067189a14543691b6',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fab_5fnot_5fexist_677',['CO_SDO_AB_NOT_EXIST',['../group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979a6ef5b921ac0f299f34e9860eb82e332e',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fab_5fout_5fof_5fmem_678',['CO_SDO_AB_OUT_OF_MEM',['../group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979adc021e79ace03edbd279a3c492853c7f',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fab_5fpram_5fincompat_679',['CO_SDO_AB_PRAM_INCOMPAT',['../group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979acaedcf71c4638efb40fc6debfa9dba67',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fab_5freadonly_680',['CO_SDO_AB_READONLY',['../group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979a48c8a5f4939372564a17b31f992b82a4',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fab_5fseq_5fnum_681',['CO_SDO_AB_SEQ_NUM',['../group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979a778ef6b5751cb8ba10b67436409c3fd2',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fab_5fsub_5funknown_682',['CO_SDO_AB_SUB_UNKNOWN',['../group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979a3e48e535fddeaa78a4059c2f91f9bb8e',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fab_5ftimeout_683',['CO_SDO_AB_TIMEOUT',['../group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979a036d0be874d10f66aa6601d76a9aa2f0',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fab_5ftoggle_5fbit_684',['CO_SDO_AB_TOGGLE_BIT',['../group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979ad4e9214eab1d034e9c10eb6c7638e592',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fab_5ftype_5fmismatch_685',['CO_SDO_AB_TYPE_MISMATCH',['../group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979a838c274eaa14626514da8f7a8ac043c3',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fab_5funsupported_5faccess_686',['CO_SDO_AB_UNSUPPORTED_ACCESS',['../group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979a370ff72a5bddee5760ba0930c3b13ba0',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fab_5fvalue_5fhigh_687',['CO_SDO_AB_VALUE_HIGH',['../group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979a4983bce8e9503f9e7a720a44528036ad',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fab_5fvalue_5flow_688',['CO_SDO_AB_VALUE_LOW',['../group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979ab402816165086fbad21a130e9f488d52',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fab_5fwriteonly_689',['CO_SDO_AB_WRITEONLY',['../group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979a457e80af0f952c272fa90ebd45cdb8cd',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fabortcode_5ft_690',['CO_SDO_abortCode_t',['../group__CO__SDOserver.html#ga7587ddcf798747fe6d97d03bf1bf2979',1,'CO_SDOserver.h']]], + ['co_5fsdo_5freturn_5ft_691',['CO_SDO_return_t',['../group__CO__SDOserver.html#ga7f729ab203285c7623df493916f22a73',1,'CO_SDOserver.h']]], + ['co_5fsdo_5frt_5fblockdownldinprogress_692',['CO_SDO_RT_blockDownldInProgress',['../group__CO__SDOserver.html#gga7f729ab203285c7623df493916f22a73aa8036db7f41e8958c057da0d4ab24f8f',1,'CO_SDOserver.h']]], + ['co_5fsdo_5frt_5fblockuploadinprogress_693',['CO_SDO_RT_blockUploadInProgress',['../group__CO__SDOserver.html#gga7f729ab203285c7623df493916f22a73ad73a50f4a1d7ef69797cbf7c930293f2',1,'CO_SDOserver.h']]], + ['co_5fsdo_5frt_5fendedwithclientabort_694',['CO_SDO_RT_endedWithClientAbort',['../group__CO__SDOserver.html#gga7f729ab203285c7623df493916f22a73a9aafefd96d032c1b65cb6c23bc53f0aa',1,'CO_SDOserver.h']]], + ['co_5fsdo_5frt_5fendedwithserverabort_695',['CO_SDO_RT_endedWithServerAbort',['../group__CO__SDOserver.html#gga7f729ab203285c7623df493916f22a73ae2fce3f477766eb188502886705dc177',1,'CO_SDOserver.h']]], + ['co_5fsdo_5frt_5fok_5fcommunicationend_696',['CO_SDO_RT_ok_communicationEnd',['../group__CO__SDOserver.html#gga7f729ab203285c7623df493916f22a73a2d0d1d8d1bc297205b3e87174642199c',1,'CO_SDOserver.h']]], + ['co_5fsdo_5frt_5ftransmittbufferfull_697',['CO_SDO_RT_transmittBufferFull',['../group__CO__SDOserver.html#gga7f729ab203285c7623df493916f22a73ad62e2421dcee78ba0477fb379a6e7e4e',1,'CO_SDOserver.h']]], + ['co_5fsdo_5frt_5fuploaddatabufferfull_698',['CO_SDO_RT_uploadDataBufferFull',['../group__CO__SDOserver.html#gga7f729ab203285c7623df493916f22a73ada069dad6b1e0bec180600b1d34758d2',1,'CO_SDOserver.h']]], + ['co_5fsdo_5frt_5fwaitinglocaltransfer_699',['CO_SDO_RT_waitingLocalTransfer',['../group__CO__SDOserver.html#gga7f729ab203285c7623df493916f22a73ab9191f8a57b840a81457591f0fbd8a76',1,'CO_SDOserver.h']]], + ['co_5fsdo_5frt_5fwaitingresponse_700',['CO_SDO_RT_waitingResponse',['../group__CO__SDOserver.html#gga7f729ab203285c7623df493916f22a73a15d85fc411d0c6e69888c2ec9d641eb5',1,'CO_SDOserver.h']]], + ['co_5fsdo_5frt_5fwrongarguments_701',['CO_SDO_RT_wrongArguments',['../group__CO__SDOserver.html#gga7f729ab203285c7623df493916f22a73af1dc6a56b2b38fb5f4c878661173decc',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fst_5fabort_702',['CO_SDO_ST_ABORT',['../group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11ac40cb6c0b2f2eb1877aee3963dc1927d',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fst_5fdownload_5fblk_5fend_5freq_703',['CO_SDO_ST_DOWNLOAD_BLK_END_REQ',['../group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11af955593bb966b324bfda361b0364d15b',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fst_5fdownload_5fblk_5fend_5frsp_704',['CO_SDO_ST_DOWNLOAD_BLK_END_RSP',['../group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11af511c26db1fb7ba18d6054255b560be7',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fst_5fdownload_5fblk_5finitiate_5freq_705',['CO_SDO_ST_DOWNLOAD_BLK_INITIATE_REQ',['../group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11a5d4ead9d3f06962987b6af8c073b6a2e',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fst_5fdownload_5fblk_5finitiate_5frsp_706',['CO_SDO_ST_DOWNLOAD_BLK_INITIATE_RSP',['../group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11af25ee4e636a98dd72fe4c5bef9bcecf2',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fst_5fdownload_5fblk_5fsubblock_5freq_707',['CO_SDO_ST_DOWNLOAD_BLK_SUBBLOCK_REQ',['../group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11a320cc9749db35473265b5203c547bbf8',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fst_5fdownload_5fblk_5fsubblock_5frsp_708',['CO_SDO_ST_DOWNLOAD_BLK_SUBBLOCK_RSP',['../group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11a70e97f34a6a98014bef1d2eeb3b5247c',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fst_5fdownload_5finitiate_5freq_709',['CO_SDO_ST_DOWNLOAD_INITIATE_REQ',['../group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11ac07432ccfaa6be8730cc8c306b3e42bb',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fst_5fdownload_5finitiate_5frsp_710',['CO_SDO_ST_DOWNLOAD_INITIATE_RSP',['../group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11a49b060ebf39c4bfb498b8691c16bb882',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fst_5fdownload_5flocal_5ftransfer_711',['CO_SDO_ST_DOWNLOAD_LOCAL_TRANSFER',['../group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11a8f685c4d233c35defb423fda8ff5544c',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fst_5fdownload_5fsegment_5freq_712',['CO_SDO_ST_DOWNLOAD_SEGMENT_REQ',['../group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11a6b44777e7e209313612baab5f83745ff',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fst_5fdownload_5fsegment_5frsp_713',['CO_SDO_ST_DOWNLOAD_SEGMENT_RSP',['../group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11ae5b55aec51372cbc2a6e32ce1456c11c',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fst_5fflag_5fdownload_714',['CO_SDO_ST_FLAG_DOWNLOAD',['../group__CO__SDOserver.html#ga84d9afbba1769aada5c52c81b7f5c3f4',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fst_5fidle_715',['CO_SDO_ST_IDLE',['../group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11a2eee38ba2a2d52890281ae54b12d50b3',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fst_5fupload_5fblk_5fend_5fcrsp_716',['CO_SDO_ST_UPLOAD_BLK_END_CRSP',['../group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11ab75a573a45778a0c4bea2c50402be03e',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fst_5fupload_5fblk_5fend_5fsreq_717',['CO_SDO_ST_UPLOAD_BLK_END_SREQ',['../group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11a98896138e97542e659051fff33b1a692',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fst_5fupload_5fblk_5finitiate_5freq_718',['CO_SDO_ST_UPLOAD_BLK_INITIATE_REQ',['../group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11a49b5c39c9e5d025c85eedffa28aa22ed',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fst_5fupload_5fblk_5finitiate_5freq2_719',['CO_SDO_ST_UPLOAD_BLK_INITIATE_REQ2',['../group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11acc4e87ad1ad20eddd19a60d9592bbada',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fst_5fupload_5fblk_5finitiate_5frsp_720',['CO_SDO_ST_UPLOAD_BLK_INITIATE_RSP',['../group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11ae9be0eeb0711890d1b9c5cbfbd204ed8',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fst_5fupload_5fblk_5fsubblock_5fcrsp_721',['CO_SDO_ST_UPLOAD_BLK_SUBBLOCK_CRSP',['../group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11af762eb5a985cf79a3e7423a39b29b328',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fst_5fupload_5fblk_5fsubblock_5fsreq_722',['CO_SDO_ST_UPLOAD_BLK_SUBBLOCK_SREQ',['../group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11a39f1cb5426ee3c3689ed833cb66e231c',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fst_5fupload_5finitiate_5freq_723',['CO_SDO_ST_UPLOAD_INITIATE_REQ',['../group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11aa8a8b5050c6528fdaa19bbb429d8e4f4',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fst_5fupload_5finitiate_5frsp_724',['CO_SDO_ST_UPLOAD_INITIATE_RSP',['../group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11aa096d10c9eb891cfedddc16276f58aaf',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fst_5fupload_5flocal_5ftransfer_725',['CO_SDO_ST_UPLOAD_LOCAL_TRANSFER',['../group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11aa07fe53d69ec7e0d56db39111867f8ce',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fst_5fupload_5fsegment_5freq_726',['CO_SDO_ST_UPLOAD_SEGMENT_REQ',['../group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11ad610c289b85192d70c835b033b49b3fb',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fst_5fupload_5fsegment_5frsp_727',['CO_SDO_ST_UPLOAD_SEGMENT_RSP',['../group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11a210a3eb6acfdb055bb72a59d8e24a6b6',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fstate_5ft_728',['CO_SDO_state_t',['../group__CO__SDOserver.html#ga0b0e614dadcc1c005185b8bc9a7fec11',1,'CO_SDOserver.h']]], + ['co_5fsdoclient_2eh_729',['CO_SDOclient.h',['../CO__SDOclient_8h.html',1,'']]], + ['co_5fsdoclient_5finit_730',['CO_SDOclient_init',['../group__CO__SDOclient.html#ga754160e34089aea70f84d22b06eaff9e',1,'CO_SDOclient.h']]], + ['co_5fsdoclient_5finitcallbackpre_731',['CO_SDOclient_initCallbackPre',['../group__CO__SDOclient.html#ga4377eaecc3bd0a8320a2bbe1ef0ef776',1,'CO_SDOclient.h']]], + ['co_5fsdoclient_5fsetup_732',['CO_SDOclient_setup',['../group__CO__SDOclient.html#gade3bf4e3249aa4c611570ec43563a08d',1,'CO_SDOclient.h']]], + ['co_5fsdoclient_5ft_733',['CO_SDOclient_t',['../structCO__SDOclient__t.html',1,'']]], + ['co_5fsdoclientclose_734',['CO_SDOclientClose',['../group__CO__SDOclient.html#ga9b98ea2c36f864f1a589c842528b12ab',1,'CO_SDOclient.h']]], + ['co_5fsdoclientdownload_735',['CO_SDOclientDownload',['../group__CO__SDOclient.html#gaab262f0a8d08ba023639a2c197d0943a',1,'CO_SDOclient.h']]], + ['co_5fsdoclientdownloadbufwrite_736',['CO_SDOclientDownloadBufWrite',['../group__CO__SDOclient.html#ga958d0568bd47d9a3152f9ea8d104b5f5',1,'CO_SDOclient.h']]], + ['co_5fsdoclientdownloadinitiate_737',['CO_SDOclientDownloadInitiate',['../group__CO__SDOclient.html#ga40f6e79592e1d587d02bbb4eaefa9489',1,'CO_SDOclient.h']]], + ['co_5fsdoclientdownloadinitiatesize_738',['CO_SDOclientDownloadInitiateSize',['../group__CO__SDOclient.html#gaf58b7731b4285538c26a0c7c49ab24b6',1,'CO_SDOclient.h']]], + ['co_5fsdoclientupload_739',['CO_SDOclientUpload',['../group__CO__SDOclient.html#gabd3a3be7e3d1649adfdd253c979ec21f',1,'CO_SDOclient.h']]], + ['co_5fsdoclientuploadbufread_740',['CO_SDOclientUploadBufRead',['../group__CO__SDOclient.html#gaf5cd4e009476b15a2cd995a9841fb175',1,'CO_SDOclient.h']]], + ['co_5fsdoclientuploadinitiate_741',['CO_SDOclientUploadInitiate',['../group__CO__SDOclient.html#ga3180f82563b3ed768fe7d3bd34fe1886',1,'CO_SDOclient.h']]], + ['co_5fsdoserver_2eh_742',['CO_SDOserver.h',['../CO__SDOserver_8h.html',1,'']]], + ['co_5fsdoserver_5finit_743',['CO_SDOserver_init',['../group__CO__SDOserver.html#gac989ba60f25fd2bc48bca6df0c0c1dde',1,'CO_SDOserver_init(CO_SDOserver_t *SDO, const OD_t *OD, const OD_entry_t *OD_1200_SDOsrvPar, uint8_t nodeId, uint16_t SDOtimeoutTime_ms, CO_CANmodule_t *CANdevRx, uint16_t CANdevRxIdx, CO_CANmodule_t *CANdevTx, uint16_t CANdevTxIdx): CO_SDOserver.c'],['../group__CO__SDOserver.html#gac989ba60f25fd2bc48bca6df0c0c1dde',1,'CO_SDOserver_init(CO_SDOserver_t *SDO, const OD_t *OD, const OD_entry_t *OD_1200_SDOsrvPar, uint8_t nodeId, uint16_t SDOtimeoutTime_ms, CO_CANmodule_t *CANdevRx, uint16_t CANdevRxIdx, CO_CANmodule_t *CANdevTx, uint16_t CANdevTxIdx): CO_SDOserver.c']]], + ['co_5fsdoserver_5finitcallbackpre_744',['CO_SDOserver_initCallbackPre',['../group__CO__SDOserver.html#ga3eeea49e2fb36da22dc754c62b03a423',1,'CO_SDOserver.h']]], + ['co_5fsdoserver_5fprocess_745',['CO_SDOserver_process',['../group__CO__SDOserver.html#ga360bc6aa1659a5572d4d1077d787433a',1,'CO_SDOserver_process(CO_SDOserver_t *SDO, bool_t NMTisPreOrOperational, uint32_t timeDifference_us, uint32_t *timerNext_us): CO_SDOserver.c'],['../group__CO__SDOserver.html#ga360bc6aa1659a5572d4d1077d787433a',1,'CO_SDOserver_process(CO_SDOserver_t *SDO, bool_t NMTisPreOrOperational, uint32_t timeDifference_us, uint32_t *timerNext_us): CO_SDOserver.c']]], + ['co_5fsdoserver_5ft_746',['CO_SDOserver_t',['../structCO__SDOserver__t.html',1,'']]], + ['co_5fsetuint16_747',['CO_setUint16',['../group__CO__driver.html#gaf74a6b7343bc9d6efd5ec5d9d9f965fb',1,'CO_driver.h']]], + ['co_5fsetuint32_748',['CO_setUint32',['../group__CO__driver.html#ga9dbc7193fbd875503e0b3b34ea22434e',1,'CO_driver.h']]], + ['co_5fsetuint8_749',['CO_setUint8',['../group__CO__driver.html#ga61de0908305223a0a7949e184cc1d644',1,'CO_driver.h']]], + ['co_5fdriver_5ftarget_2eh_750',['CO_driver_target.h',['../group__CO__socketCAN__driver__target.html',1,'']]], + ['can_20errors_20_26_20log_751',['CAN errors & Log',['../group__CO__socketCAN__ERROR.html',1,'']]], + ['co_5fsrdo_2ec_752',['CO_SRDO.c',['../CO__SRDO_8c.html',1,'']]], + ['co_5fsrdo_2eh_753',['CO_SRDO.h',['../CO__SRDO_8h.html',1,'']]], + ['co_5fsrdo_5finit_754',['CO_SRDO_init',['../group__CO__SRDO.html#ga608e6d48e97f1b4da316b36f10e389c6',1,'CO_SRDO.h']]], + ['co_5fsrdo_5finitcallbackentersafestate_755',['CO_SRDO_initCallbackEnterSafeState',['../group__CO__SRDO.html#ga5303601f6f94c83530b5e165f54b54bb',1,'CO_SRDO.h']]], + ['co_5fsrdo_5finitcallbackpre_756',['CO_SRDO_initCallbackPre',['../group__CO__SRDO.html#gac066166b35bcb3d0b01adfc0e2eb9bb1',1,'CO_SRDO.h']]], + ['co_5fsrdo_5fprocess_757',['CO_SRDO_process',['../group__CO__SRDO.html#gacb94aa4f279a4476c193ee50c408dfbb',1,'CO_SRDO.h']]], + ['co_5fsrdo_5frequestsend_758',['CO_SRDO_requestSend',['../group__CO__SRDO.html#gac9a21725c4bb0373ea18a414019cf339',1,'CO_SRDO.h']]], + ['co_5fsrdo_5ft_759',['CO_SRDO_t',['../structCO__SRDO__t.html',1,'']]], + ['co_5fsrdocommpar_5ft_760',['CO_SRDOCommPar_t',['../structCO__SRDOCommPar__t.html',1,'']]], + ['co_5fsrdoguard_5finit_761',['CO_SRDOGuard_init',['../group__CO__SRDO.html#gacd578e8a5a4af024f8e6f8aef87cbd14',1,'CO_SRDO.h']]], + ['co_5fsrdoguard_5fprocess_762',['CO_SRDOGuard_process',['../group__CO__SRDO.html#ga0201fa4da8b37a18f864a9fd7c826a6c',1,'CO_SRDO.h']]], + ['co_5fsrdoguard_5ft_763',['CO_SRDOGuard_t',['../structCO__SRDOGuard__t.html',1,'']]], + ['common_20definitions_764',['Common definitions',['../group__CO__STACK__CONFIG__COMMON.html',1,'']]], + ['crc_2016_20calculation_765',['CRC 16 calculation',['../group__CO__STACK__CONFIG__CRC16.html',1,'']]], + ['canopen_20gateway_766',['CANopen gateway',['../group__CO__STACK__CONFIG__GATEWAY.html',1,'']]], + ['canopen_20led_20diodes_767',['CANopen LED diodes',['../group__CO__STACK__CONFIG__LEDS.html',1,'']]], + ['co_5fswap_5f16_768',['CO_SWAP_16',['../group__CO__dataTypes.html#ga1717fcaabbe2cd6cd9fc0bc0cb917a6c',1,'CO_driver.h']]], + ['co_5fswap_5f32_769',['CO_SWAP_32',['../group__CO__dataTypes.html#gadf87da54942e3a0ff159688c5e0e267b',1,'CO_driver.h']]], + ['co_5fswap_5f64_770',['CO_SWAP_64',['../group__CO__dataTypes.html#gaec0fc209357883f42d66cd2cdaa7236f',1,'CO_driver.h']]], + ['co_5fsync_2eh_771',['CO_SYNC.h',['../CO__SYNC_8h.html',1,'']]], + ['co_5fsync_5finit_772',['CO_SYNC_init',['../group__CO__SYNC.html#ga3047b679d5e3261eedf1980ea87b5135',1,'CO_SYNC.h']]], + ['co_5fsync_5finitcallbackpre_773',['CO_SYNC_initCallbackPre',['../group__CO__SYNC.html#gaf1005766c7f1588262b018fe04960777',1,'CO_SYNC.h']]], + ['co_5fsync_5fnone_774',['CO_SYNC_NONE',['../group__CO__SYNC.html#gga121ede6e0c90c66076a7ed950db38517aeff7846423b9eb92cd2c69df745ea429',1,'CO_SYNC.h']]], + ['co_5fsync_5foutside_5fwindow_775',['CO_SYNC_OUTSIDE_WINDOW',['../group__CO__SYNC.html#gga121ede6e0c90c66076a7ed950db38517a5b112bec6cb10119879703a6313de41e',1,'CO_SYNC.h']]], + ['co_5fsync_5fprocess_776',['CO_SYNC_process',['../group__CO__SYNC.html#ga66b8f42fd430daa2a57ff15aa49c814c',1,'CO_SYNC.h']]], + ['co_5fsync_5freceived_777',['CO_SYNC_RECEIVED',['../group__CO__SYNC.html#gga121ede6e0c90c66076a7ed950db38517aa03c21a78b503a4adebb2d9d7aa655bf',1,'CO_SYNC.h']]], + ['co_5fsync_5fstatus_5ft_778',['CO_SYNC_status_t',['../group__CO__SYNC.html#ga121ede6e0c90c66076a7ed950db38517',1,'CO_SYNC.h']]], + ['co_5fsync_5ft_779',['CO_SYNC_t',['../structCO__SYNC__t.html',1,'']]], + ['co_5fsyncsend_780',['CO_SYNCsend',['../group__CO__SYNC.html#gabbc8625d068d19d8b632d034f396a1ff',1,'CO_SYNC.h']]], + ['co_5ft_781',['CO_t',['../structCO__t.html',1,'']]], + ['co_5ftime_2eh_782',['CO_TIME.h',['../CO__TIME_8h.html',1,'']]], + ['co_5ftime_5finit_783',['CO_TIME_init',['../group__CO__TIME.html#ga9e02651c1662d3da13a9a071c73347bb',1,'CO_TIME.h']]], + ['co_5ftime_5finitcallbackpre_784',['CO_TIME_initCallbackPre',['../group__CO__TIME.html#gae2f03663f1477cdc551b61cf5689cd6b',1,'CO_TIME.h']]], + ['co_5ftime_5fprocess_785',['CO_TIME_process',['../group__CO__TIME.html#ga39f71192db2b40da6dcf8f4fceac9bb6',1,'CO_TIME.h']]], + ['co_5ftime_5ft_786',['CO_TIME_t',['../structCO__TIME__t.html',1,'']]], + ['co_5ftpdo_5finit_787',['CO_TPDO_init',['../group__CO__PDO.html#ga8fb100744dc91f84b236c55ee37200a1',1,'CO_PDO.h']]], + ['co_5ftpdo_5fprocess_788',['CO_TPDO_process',['../group__CO__PDO.html#ga0bb0d1b09d37ca19e01d47d8d0004f6b',1,'CO_PDO.h']]], + ['co_5ftpdo_5ft_789',['CO_TPDO_t',['../structCO__TPDO__t.html',1,'']]], + ['co_5ftpdocommpar_5ft_790',['CO_TPDOCommPar_t',['../structCO__TPDOCommPar__t.html',1,'']]], + ['co_5ftpdoiscos_791',['CO_TPDOisCOS',['../group__CO__PDO.html#gafec3eb12b93146a3706cbf03d3770a8d',1,'CO_PDO.h']]], + ['co_5ftpdomappar_5ft_792',['CO_TPDOMapPar_t',['../structCO__TPDOMapPar__t.html',1,'']]], + ['co_5ftpdosend_793',['CO_TPDOsend',['../group__CO__PDO.html#ga9b2c8692f74f6a6a389ef88bf9c682a5',1,'CO_PDO.h']]], + ['co_5ftrace_2eh_794',['CO_trace.h',['../CO__trace_8h.html',1,'']]], + ['co_5ftrace_5fdatatype_5ft_795',['CO_trace_dataType_t',['../structCO__trace__dataType__t.html',1,'']]], + ['co_5ftrace_5finit_796',['CO_trace_init',['../group__CO__trace.html#ga50621e5b8e28349f7caf260b6cf9962e',1,'CO_trace.h']]], + ['co_5ftrace_5fprocess_797',['CO_trace_process',['../group__CO__trace.html#ga5b8d9460d7a42920325cf66eb8b725ec',1,'CO_trace.h']]], + ['co_5ftrace_5ft_798',['CO_trace_t',['../structCO__trace__t.html',1,'']]], + ['co_5funlock_5fcan_5fsend_799',['CO_UNLOCK_CAN_SEND',['../group__CO__critical__sections.html#ga511a5a0bf905c2207d5c9e26d35fe3cc',1,'CO_driver.h']]], + ['co_5funlock_5femcy_800',['CO_UNLOCK_EMCY',['../group__CO__critical__sections.html#ga720a798f2bf7fe20d9c95a212b4df417',1,'CO_driver.h']]], + ['co_5funlock_5fod_801',['CO_UNLOCK_OD',['../group__CO__critical__sections.html#ga2477f5d24fd31a9f4052cf451b87809f',1,'CO_driver.h']]], + ['co_5fuse_5fglobals_802',['CO_USE_GLOBALS',['../group__CO__CANopen.html#gaab00eab90dbe59885c98831e2d819e56',1,'CANopen.h']]], + ['co_5fversion_5fmajor_803',['CO_VERSION_MAJOR',['../group__CO__driver.html#ga0e351c2972f6d8f2e08fb5ac21a833b8',1,'CO_driver.h']]], + ['co_5fversion_5fminor_804',['CO_VERSION_MINOR',['../group__CO__driver.html#gaac3f110c1dd3cfc2b994b5c20d1c6ace',1,'CO_driver.h']]], + ['cob_5fid_805',['COB_ID',['../structCO__SYNC__t.html#af528cdc487bdaee3dfc0a3baa3f7c7cc',1,'CO_SYNC_t::COB_ID()'],['../structCO__TIME__t.html#a6bcf872834e3a76869943108fd55ffc3',1,'CO_TIME_t::COB_ID()']]], + ['cob_5fid1_5fnormal_806',['COB_ID1_normal',['../structCO__SRDOCommPar__t.html#a89d3762612ef971aecaae43ce94141cc',1,'CO_SRDOCommPar_t']]], + ['cob_5fid2_5finverted_807',['COB_ID2_inverted',['../structCO__SRDOCommPar__t.html#a574ade8ff753ae742f6a3b51b32a11fe',1,'CO_SRDOCommPar_t']]], + ['cob_5fidclienttoserver_808',['COB_IDClientToServer',['../structCO__SDOclient__t.html#a8752f1ad790cc61af17ff8b93b80c7ef',1,'CO_SDOclient_t::COB_IDClientToServer()'],['../structCO__SDOserver__t.html#a7c9113f146613eec4b76888bd8d0f2fd',1,'CO_SDOserver_t::COB_IDClientToServer()']]], + ['cob_5fidservertoclient_809',['COB_IDServerToClient',['../structCO__SDOclient__t.html#a7b29a785240ca994c7dafd07d5a396e4',1,'CO_SDOclient_t::COB_IDServerToClient()'],['../structCO__SDOserver__t.html#a47cf7cde974f0ff8fd20f7a5363857cf',1,'CO_SDOserver_t::COB_IDServerToClient()']]], + ['cob_5fidusedbyrpdo_810',['COB_IDUsedByRPDO',['../structCO__RPDOCommPar__t.html#a798b8c59ea8f8b627c307f1246b6ee78',1,'CO_RPDOCommPar_t']]], + ['cob_5fidusedbytpdo_811',['COB_IDUsedByTPDO',['../structCO__TPDOCommPar__t.html#a65ee2e80b1078e84479ab749012f94cc',1,'CO_TPDOCommPar_t']]], + ['command_812',['command',['../structCO__LSSmaster__t.html#a745a126919856c94e7f93186830e6940',1,'CO_LSSmaster_t']]], + ['commandinterface_813',['commandInterface',['../structCO__epoll__gtw__t.html#a96544bbc848d4627f3147496cc40d9f4',1,'CO_epoll_gtw_t']]], + ['commbuf_814',['commBuf',['../structCO__GTWA__t.html#a51fd91cf468da15e5f943131fa696266',1,'CO_GTWA_t']]], + ['commfifo_815',['commFifo',['../structCO__GTWA__t.html#a74241ff1c68a8fc05f0b2be601dcf960',1,'CO_GTWA_t']]], + ['compatibilityentry_816',['compatibilityEntry',['../structCO__TPDOCommPar__t.html#acba30527976f508b43b3348846f2e657',1,'CO_TPDOCommPar_t']]], + ['config_817',['config',['../structCO__t.html#a522a9a56f1256dac8cd305313b3913f1',1,'CO_t']]], + ['configurationvalid_818',['configurationValid',['../structCO__SRDOGuard__t.html#aa4c8b5e7d9d8fa54d91ad0797ea9b39b',1,'CO_SRDOGuard_t']]], + ['counter_819',['counter',['../structCO__SYNC__t.html#a0977c2f09f69755e4b53df68bb1fee0b',1,'CO_SYNC_t']]], + ['counteroverflowvalue_820',['counterOverflowValue',['../structCO__SYNC__t.html#aee5eb3e245e54e509d2f2cc05cf4a664',1,'CO_SYNC_t']]], + ['crc16_2dccitt_2eh_821',['crc16-ccitt.h',['../crc16-ccitt_8h.html',1,'']]], + ['crc16_5fccitt_822',['crc16_ccitt',['../group__CO__crc16__ccitt.html#gab03185ec096eb8792b52d657ed6cbea1',1,'crc16-ccitt.h']]], + ['crc16_5fccitt_5fsingle_823',['crc16_ccitt_single',['../group__CO__crc16__ccitt.html#gad0ef7bb8f47a7eb3ff71d603beba7f92',1,'crc16-ccitt.h']]], + ['curentsynctimeisinsidewindow_824',['curentSyncTimeIsInsideWindow',['../structCO__SYNC__t.html#adbc85ba8f5f4cbca0caa645216204a88',1,'CO_SYNC_t']]], + ['canopennode_825',['CANopenNode',['../index.html',1,'']]], + ['change_20log_826',['Change Log',['../md_doc_CHANGELOG.html',1,'']]], + ['canopen_20documentation_827',['CANopen documentation',['../md_example_DS301_profile.html',1,'']]] +]; diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/all_3.html b/Devices/Libraries/Systems/CANopenSocket/docs/search/all_3.html new file mode 100755 index 0000000..b61b96f --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/all_3.html @@ -0,0 +1,30 @@ + + + + + + + + + +
+
Loading...
+
+ +
Searching...
+
No Matches
+ +
+ + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/all_3.js b/Devices/Libraries/Systems/CANopenSocket/docs/search/all_3.js new file mode 100755 index 0000000..ae336e7 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/all_3.js @@ -0,0 +1,17 @@ +var searchData= +[ + ['driver_828',['Driver',['../group__CO__driver.html',1,'']]], + ['debug_20messages_829',['Debug messages',['../group__CO__STACK__CONFIG__DEBUG.html',1,'']]], + ['data_830',['data',['../structCO__CANtx__t.html#aae5bcdc2296a5d1d53a3105e86dbb66d',1,'CO_CANtx_t::data()'],['../structOD__obj__var__t.html#a7c15865c69e0dc0a09f6d21bd890d062',1,'OD_obj_var_t::data()'],['../structOD__obj__array__t.html#a009bcf700d8e27c93e886ad7ff7fb2eb',1,'OD_obj_array_t::data()'],['../structOD__obj__record__t.html#a490d4818eaa6ca94d2c6fc598647dbd1',1,'OD_obj_record_t::data()']]], + ['data0_831',['data0',['../structOD__obj__array__t.html#a9a4ca22f014061ca9d926ab43036bc1f',1,'OD_obj_array_t']]], + ['dataelementlength_832',['dataElementLength',['../structOD__obj__array__t.html#a985fb68eba74f9e8fb76a4c5d85e96e9',1,'OD_obj_array_t']]], + ['dataelementsizeof_833',['dataElementSizeof',['../structOD__obj__array__t.html#ad310fa351ebb2a44f66451dd12675bf9',1,'OD_obj_array_t']]], + ['datalength_834',['dataLength',['../structOD__stream__t.html#a60c4499678a5db84a7f7285b934ce75a',1,'OD_stream_t::dataLength()'],['../structOD__obj__var__t.html#a385af11ed619b78de9b2f1ae6528a870',1,'OD_obj_var_t::dataLength()'],['../structOD__obj__record__t.html#a1302f14c20e976ac884196d4c3e201c1',1,'OD_obj_record_t::dataLength()'],['../structCO__RPDO__t.html#af742fd80b982822c3e06770ab0877cc3',1,'CO_RPDO_t::dataLength()'],['../structCO__TPDO__t.html#a223e60deb77d4ef8a78da37e4d9cdf85',1,'CO_TPDO_t::dataLength()'],['../structCO__SRDO__t.html#ae994e87a71e85342f3a5b3c046d8a47f',1,'CO_SRDO_t::dataLength()']]], + ['dataobjectoriginal_835',['dataObjectOriginal',['../structOD__stream__t.html#aad2551a7bf0da6396e6b909adf487b01',1,'OD_stream_t']]], + ['dataoffset_836',['dataOffset',['../structOD__stream__t.html#a97799b896ee689504771a9274575bcdc',1,'OD_stream_t']]], + ['defaultcob_5fid_837',['defaultCOB_ID',['../structCO__RPDO__t.html#a9e327dba172ebbd3112097e5085eea2f',1,'CO_RPDO_t::defaultCOB_ID()'],['../structCO__TPDO__t.html#a95e95dc4668b41de0b8eb8504e83c944',1,'CO_TPDO_t::defaultCOB_ID()'],['../structCO__SRDO__t.html#a14da12ca5af61cd6cc6e442a0baa5c26',1,'CO_SRDO_t::defaultCOB_ID()']]], + ['dlc_838',['DLC',['../structCO__CANtx__t.html#a9bb96d60314283061f7619e36d870fa0',1,'CO_CANtx_t']]], + ['domain_5ft_839',['domain_t',['../group__CO__dataTypes.html#gadc433a2a90dacd3b2b3801dd9431c254',1,'CO_driver.h']]], + ['dt_840',['dt',['../structCO__trace__t.html#a31e42b2511450377294475fcf0189d89',1,'CO_trace_t']]], + ['device_20support_841',['Device Support',['../md_doc_deviceSupport.html',1,'']]] +]; diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/all_4.html b/Devices/Libraries/Systems/CANopenSocket/docs/search/all_4.html new file mode 100755 index 0000000..06de155 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/all_4.html @@ -0,0 +1,30 @@ + + + + + + + + + +
+
Loading...
+
+ +
Searching...
+
No Matches
+ +
+ + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/all_4.js b/Devices/Libraries/Systems/CANopenSocket/docs/search/all_4.js new file mode 100755 index 0000000..f00c4a5 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/all_4.js @@ -0,0 +1,40 @@ +var searchData= +[ + ['emergency_842',['Emergency',['../group__CO__Emergency.html',1,'']]], + ['epoll_20interface_843',['Epoll interface',['../group__CO__epoll__interface.html',1,'']]], + ['emergency_20producer_2fconsumer_844',['Emergency producer/consumer',['../group__CO__STACK__CONFIG__EMERGENCY.html',1,'']]], + ['em_845',['em',['../structCO__t.html#a6e4c80d975a5a2207530b72bdbf530b7',1,'CO_t::em()'],['../structCO__HBconsumer__t.html#aae5e363ccc6a6fd3b17a35e0430add2a',1,'CO_HBconsumer_t::em()'],['../structCO__NMT__t.html#a60848ed23fb775dc4412be0e7ff9bc4b',1,'CO_NMT_t::em()'],['../structCO__RPDO__t.html#a5261e898fc67ecba19f0fc146e4a13ef',1,'CO_RPDO_t::em()'],['../structCO__TPDO__t.html#ac7ce3386549e212300bb85bfc5e88f2e',1,'CO_TPDO_t::em()'],['../structCO__SYNC__t.html#acf987cc40eb5f005a92f10210353ea1f',1,'CO_SYNC_t::em()'],['../structCO__TIME__t.html#ad38fd490eece812c27048a0ffde75c9e',1,'CO_TIME_t::em()'],['../structCO__SRDO__t.html#a1779ab0170d7b604948a9396681212c8',1,'CO_SRDO_t::em()']]], + ['enabled_846',['enabled',['../structCO__trace__t.html#aefa5e9934aaac1f00d0a1866100dae50',1,'CO_trace_t']]], + ['entry_5fh1001_847',['ENTRY_H1001',['../structCO__config__t.html#a6a6c19e816fb76882e85b2c07c0d8f42',1,'CO_config_t']]], + ['entry_5fh1003_848',['ENTRY_H1003',['../structCO__config__t.html#a7e320b309714b7f623c2006d45fee929',1,'CO_config_t']]], + ['entry_5fh1005_849',['ENTRY_H1005',['../structCO__config__t.html#a02a4992f47db72816753ff2aa1964318',1,'CO_config_t']]], + ['entry_5fh1006_850',['ENTRY_H1006',['../structCO__config__t.html#aa9befdebbaaa22f309b9a1b115612071',1,'CO_config_t']]], + ['entry_5fh1007_851',['ENTRY_H1007',['../structCO__config__t.html#ad51ab63ca8b5836bf0dd8543f02db544',1,'CO_config_t']]], + ['entry_5fh1012_852',['ENTRY_H1012',['../structCO__config__t.html#abac6be7122af1a8a4f9ae3ff5912d490',1,'CO_config_t']]], + ['entry_5fh1014_853',['ENTRY_H1014',['../structCO__config__t.html#a4827d94f6152cc12d86bd21312ae86e4',1,'CO_config_t']]], + ['entry_5fh1015_854',['ENTRY_H1015',['../structCO__config__t.html#a141f21b4d1730206d1af823fd6b13a01',1,'CO_config_t']]], + ['entry_5fh1016_855',['ENTRY_H1016',['../structCO__config__t.html#a0af4cf7d0355861e7f60206d794d6a91',1,'CO_config_t']]], + ['entry_5fh1017_856',['ENTRY_H1017',['../structCO__config__t.html#ad17f77b55de3d90ec983fcac49eeab6d',1,'CO_config_t']]], + ['entry_5fh1019_857',['ENTRY_H1019',['../structCO__config__t.html#a468c82f6a0afd757a6b78ce33532c0d2',1,'CO_config_t']]], + ['entry_5fh1200_858',['ENTRY_H1200',['../structCO__config__t.html#a05ab8adad4517850e31e5542895f7cc5',1,'CO_config_t']]], + ['entry_5fh1280_859',['ENTRY_H1280',['../structCO__config__t.html#a9f871c4ec753e8414cdb47eb78c3e09d',1,'CO_config_t']]], + ['entry_5fh1300_860',['ENTRY_H1300',['../structCO__config__t.html#a91c9f3ddb67231854af39224a9597e20',1,'CO_config_t']]], + ['entry_5fh1301_861',['ENTRY_H1301',['../structCO__config__t.html#a87076cb1f9282d9720c21d395ff4e541',1,'CO_config_t']]], + ['entry_5fh1381_862',['ENTRY_H1381',['../structCO__config__t.html#a7b3172b29ce8751adcab9e4351dcc31e',1,'CO_config_t']]], + ['entry_5fh13fe_863',['ENTRY_H13FE',['../structCO__config__t.html#a03fcaca5a8e0e71b86086908cae75f3d',1,'CO_config_t']]], + ['entry_5fh13ff_864',['ENTRY_H13FF',['../structCO__config__t.html#aa4cb9674209b83e7f0e48b01feaa04ef',1,'CO_config_t']]], + ['entry_5fh1400_865',['ENTRY_H1400',['../structCO__config__t.html#a5e0984d93183493d587523888465eaa7',1,'CO_config_t']]], + ['entry_5fh1600_866',['ENTRY_H1600',['../structCO__config__t.html#ab2ddc9943fd8c89f3b852d7ac9508d21',1,'CO_config_t']]], + ['entry_5fh1800_867',['ENTRY_H1800',['../structCO__config__t.html#a29b98c08edfe0fba2e46c7af7a9edf6f',1,'CO_config_t']]], + ['entry_5fh1a00_868',['ENTRY_H1A00',['../structCO__config__t.html#a43fd6a448c91910c603f2c7756610432',1,'CO_config_t']]], + ['epoll_5ffd_869',['epoll_fd',['../structCO__epoll__t.html#a6e4aeb640b634a49a290aa9b4bc3b517',1,'CO_epoll_t::epoll_fd()'],['../structCO__epoll__gtw__t.html#a7804e63fe1f7f5f6df68b32d8c25da2d',1,'CO_epoll_gtw_t::epoll_fd()']]], + ['epoll_5fnew_870',['epoll_new',['../structCO__epoll__t.html#ac4113f19873fc3217e1e997a28f5bdd4',1,'CO_epoll_t']]], + ['errinfo_871',['errinfo',['../structCO__CANmodule__t.html#aead6585dcb9b8214dd77e8f74ca2aea1',1,'CO_CANmodule_t']]], + ['errold_872',['errOld',['../structCO__CANmodule__t.html#aa0627988ddedc3a4ec3bbfcf3b818d06',1,'CO_CANmodule_t']]], + ['errorregister_873',['errorRegister',['../structCO__EM__t.html#ae18bb84c6235afbdf3745d9da337c2d3',1,'CO_EM_t']]], + ['errorstatusbits_874',['errorStatusBits',['../structCO__EM__t.html#a377eb478f0af20e6e1e23bd77186dcfd',1,'CO_EM_t']]], + ['ev_875',['ev',['../structCO__epoll__t.html#a5ace69fd90506eae4f3f538fcb160c01',1,'CO_epoll_t']]], + ['event_5ffd_876',['event_fd',['../structCO__epoll__t.html#af0e85776b360999b0830c943eb4505eb',1,'CO_epoll_t']]], + ['eventtimer_877',['eventTimer',['../structCO__TPDOCommPar__t.html#ab01f44570dca08c910c17b14fc664414',1,'CO_TPDOCommPar_t::eventTimer()'],['../structCO__TPDO__t.html#ae71e875f41d8f14f02e757dbc3b2d0c5',1,'CO_TPDO_t::eventTimer()']]], + ['extio_878',['extIO',['../structOD__obj__extended__t.html#a6ccf59c770c3887233ab7b850e77e375',1,'OD_obj_extended_t']]] +]; diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/all_5.html b/Devices/Libraries/Systems/CANopenSocket/docs/search/all_5.html new file mode 100755 index 0000000..2544c4e --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/all_5.html @@ -0,0 +1,30 @@ + + + + + + + + + +
+
Loading...
+
+ +
Searching...
+
No Matches
+ +
+ + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/all_5.js b/Devices/Libraries/Systems/CANopenSocket/docs/search/all_5.js new file mode 100755 index 0000000..e0659a8 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/all_5.js @@ -0,0 +1,33 @@ +var searchData= +[ + ['fifo_20circular_20buffer_879',['FIFO circular buffer',['../group__CO__CANopen__301__fifo.html',1,'']]], + ['fifo_20buffer_880',['FIFO buffer',['../group__CO__STACK__CONFIG__FIFO.html',1,'']]], + ['false_881',['false',['../group__CO__dataTypes.html#ga65e9886d74aaee76545e83dd09011727',1,'CO_driver.h']]], + ['fastscanpos_882',['fastscanPos',['../structCO__LSSslave__t.html#af163113cf94a4a2c74521ffe194522b2',1,'CO_LSSslave_t']]], + ['fd_883',['fd',['../structCO__CANinterfaceErrorhandler__t.html#a06e667f1f90ad62495aa2172d97305b8',1,'CO_CANinterfaceErrorhandler_t']]], + ['fifo_884',['fifo',['../structCO__EM__t.html#ac84d3cf89e04ee48fff85d59bb91f3d9',1,'CO_EM_t']]], + ['fifocount_885',['fifoCount',['../structCO__EM__t.html#a99ffd8be6baebaf6c598b02c7ce31518',1,'CO_EM_t']]], + ['fifooverflow_886',['fifoOverflow',['../structCO__EM__t.html#a52c37c126d1f761b0a9cf608123a5976',1,'CO_EM_t']]], + ['fifoppptr_887',['fifoPpPtr',['../structCO__EM__t.html#a1cf16a27db5ccf065c68b39e8e2d401f',1,'CO_EM_t']]], + ['fifowrptr_888',['fifoWrPtr',['../structCO__EM__t.html#aa790e927251322c852c26bdb21853647',1,'CO_EM_t']]], + ['filename_889',['filename',['../structCO__OD__storage__t.html#afb3dd08b01bf20f251754c32b116d8fe',1,'CO_OD_storage_t']]], + ['finished_890',['finished',['../structCO__SDOserver__t.html#a90997c7a119e97d2cb8b9bbc5fb5145f',1,'CO_SDOserver_t']]], + ['firstcantxmessage_891',['firstCANtxMessage',['../structCO__CANmodule__t.html#ac12bd37971b89b5796fec0de71be0f07',1,'CO_CANmodule_t']]], + ['flagspdo_892',['flagsPDO',['../structOD__subEntry__t.html#ad6a4eb8da2f84f7b84164eac01115dd1',1,'OD_subEntry_t::flagsPDO()'],['../structOD__obj__extended__t.html#a80e8186d50cb6ca8ebf447d43815e566',1,'OD_obj_extended_t::flagsPDO()']]], + ['float32_5ft_893',['float32_t',['../group__CO__dataTypes.html#ga4611b605e45ab401f02cab15c5e38715',1,'CO_driver.h']]], + ['float64_5ft_894',['float64_t',['../group__CO__dataTypes.html#gac55f3ae81b5bc9053760baacf57e47f4',1,'CO_driver.h']]], + ['format_895',['format',['../structCO__trace__t.html#a3ea91e63f2c2f6c34b3ee1ae0f84e939',1,'CO_trace_t']]], + ['found_896',['found',['../structCO__LSSmaster__fastscan__t.html#a9c34a389339a46da81f2234443f20fb9',1,'CO_LSSmaster_fastscan_t']]], + ['fp_897',['fp',['../structCO__OD__storage__t.html#a8976cb73fd4a2baadc4689fdb8b876a1',1,'CO_OD_storage_t']]], + ['freshcommand_898',['freshCommand',['../structCO__epoll__gtw__t.html#ae529dc4279406811f0b97593816e863c',1,'CO_epoll_gtw_t']]], + ['fsbitchecked_899',['fsBitChecked',['../structCO__LSSmaster__t.html#ab71abc74d5d92f149ca991c9bb47ed99',1,'CO_LSSmaster_t']]], + ['fsidnumber_900',['fsIdNumber',['../structCO__LSSmaster__t.html#aca7e7be8c297c612b2200b866ed7b248',1,'CO_LSSmaster_t']]], + ['fslsssub_901',['fsLssSub',['../structCO__LSSmaster__t.html#a9962ae690930bf58a447a47bf708c5f2',1,'CO_LSSmaster_t']]], + ['fsstate_902',['fsState',['../structCO__LSSmaster__t.html#ad7ee57af199cfd615f6caa07358f0ce7',1,'CO_LSSmaster_t']]], + ['functsignalobject_903',['functSignalObject',['../structCO__SDOclient__t.html#a582292a9d5db2b3bf09bb089c788ecd0',1,'CO_SDOclient_t::functSignalObject()'],['../structCO__LSSmaster__t.html#aafce10b9e126c5c157cb137e8b65b27c',1,'CO_LSSmaster_t::functSignalObject()']]], + ['functsignalobjecthbstarted_904',['functSignalObjectHbStarted',['../structCO__HBconsNode__t.html#a50ed8caa11fce685dfbba13d11ada0ef',1,'CO_HBconsNode_t']]], + ['functsignalobjectpre_905',['functSignalObjectPre',['../structCO__EM__t.html#ac1dec593fd20fbf7ccc5e8287e27b2d8',1,'CO_EM_t::functSignalObjectPre()'],['../structCO__HBconsNode__t.html#afd6a4fcc8bc4cc9c647e9ef3d91b2431',1,'CO_HBconsNode_t::functSignalObjectPre()'],['../structCO__NMT__t.html#a9ae2c962bc4aaa477555920dc0323302',1,'CO_NMT_t::functSignalObjectPre()'],['../structCO__RPDO__t.html#aff7dd123460cc50708f28d40d851f8dd',1,'CO_RPDO_t::functSignalObjectPre()'],['../structCO__SDOserver__t.html#adac867a846309166bca3f5dd0b550e65',1,'CO_SDOserver_t::functSignalObjectPre()'],['../structCO__SYNC__t.html#a6a219b1fd9d3382131878af1f016b83d',1,'CO_SYNC_t::functSignalObjectPre()'],['../structCO__TIME__t.html#a91e41ac57bb634b53b6698b58efc4616',1,'CO_TIME_t::functSignalObjectPre()'],['../structCO__SRDO__t.html#a5807d03b19a7e2ae7c4250dd4e61f235',1,'CO_SRDO_t::functSignalObjectPre()'],['../structCO__LSSslave__t.html#a346a28ca3adef3050f6cc29ec182059d',1,'CO_LSSslave_t::functSignalObjectPre()']]], + ['functsignalobjectremotereset_906',['functSignalObjectRemoteReset',['../structCO__HBconsNode__t.html#a94b3c3b2b5b24f5ea3ecc1bbc14b79dd',1,'CO_HBconsNode_t']]], + ['functsignalobjectsafe_907',['functSignalObjectSafe',['../structCO__GFC__t.html#a890395b6bf1b77055c52ad150356ea33',1,'CO_GFC_t::functSignalObjectSafe()'],['../structCO__SRDO__t.html#a037ba4d3ca1e33c6187219ba4766b2a8',1,'CO_SRDO_t::functSignalObjectSafe()']]], + ['functsignalobjecttimeout_908',['functSignalObjectTimeout',['../structCO__HBconsNode__t.html#aaf6cc300976931c02e3d46ec2b75cc2e',1,'CO_HBconsNode_t']]] +]; diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/all_6.html b/Devices/Libraries/Systems/CANopenSocket/docs/search/all_6.html new file mode 100755 index 0000000..43f14ea --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/all_6.html @@ -0,0 +1,30 @@ + + + + + + + + + +
+
Loading...
+
+ +
Searching...
+
No Matches
+ +
+ + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/all_6.js b/Devices/Libraries/Systems/CANopenSocket/docs/search/all_6.js new file mode 100755 index 0000000..d2382e1 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/all_6.js @@ -0,0 +1,11 @@ +var searchData= +[ + ['gateway_20ascii_20mapping_909',['Gateway ASCII mapping',['../group__CO__CANopen__309__3.html',1,'']]], + ['gfc_910',['GFC',['../group__CO__GFC.html',1,'']]], + ['getters_20and_20setters_911',['Getters and setters',['../group__CO__ODgetSetters.html',1,'']]], + ['gfc_912',['GFC',['../structCO__t.html#a9c6e7b29436b05c8b659502c6fae2a6a',1,'CO_t']]], + ['gtwa_913',['gtwa',['../structCO__t.html#afa0e937046492a26af9bb5e03c3aab94',1,'CO_t']]], + ['gtwa_5ffd_914',['gtwa_fd',['../structCO__epoll__gtw__t.html#a79b9c968b1f44ad5a4c55c36174d6898',1,'CO_epoll_gtw_t']]], + ['gtwa_5ffdsocket_915',['gtwa_fdSocket',['../structCO__epoll__gtw__t.html#a418eccf136c9f8e081d61f98289e0759',1,'CO_epoll_gtw_t']]], + ['getting_20started_916',['Getting Started',['../md_doc_gettingStarted.html',1,'']]] +]; diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/all_7.html b/Devices/Libraries/Systems/CANopenSocket/docs/search/all_7.html new file mode 100755 index 0000000..af52f82 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/all_7.html @@ -0,0 +1,30 @@ + + + + + + + + + +
+
Loading...
+
+ +
Searching...
+
No Matches
+ +
+ + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/all_7.js b/Devices/Libraries/Systems/CANopenSocket/docs/search/all_7.js new file mode 100755 index 0000000..a34224c --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/all_7.js @@ -0,0 +1,12 @@ +var searchData= +[ + ['heartbeat_20consumer_917',['Heartbeat consumer',['../group__CO__HBconsumer.html',1,'']]], + ['hb_5fcandevtx_918',['HB_CANdevTx',['../structCO__NMT__t.html#aedcf17c643f41370a978794301f00169',1,'CO_NMT_t']]], + ['hb_5ftxbuff_919',['HB_TXbuff',['../structCO__NMT__t.html#aa9fb1bb36758a1ff27b44c20fd6a0192',1,'CO_NMT_t']]], + ['hbcons_920',['HBcons',['../structCO__t.html#a5eea4e2b8390e1f0ec531248e229cd72',1,'CO_t']]], + ['hbconstime_921',['HBconsTime',['../structCO__HBconsumer__t.html#a1cd314f387357f2ce13d4093f477fff5',1,'CO_HBconsumer_t']]], + ['hbproducertime_5fus_922',['HBproducerTime_us',['../structCO__NMT__t.html#a983a4ba9a004d216e32414c0a38736c4',1,'CO_NMT_t']]], + ['hbproducertimer_923',['HBproducerTimer',['../structCO__NMT__t.html#a0babc82bd6dfde07511b87167941b4f0',1,'CO_NMT_t']]], + ['hbstate_924',['HBstate',['../structCO__HBconsNode__t.html#a6d16bde174d37094149343fcc7025e3c',1,'CO_HBconsNode_t']]], + ['helpstring_925',['helpString',['../structCO__GTWA__t.html#a5ce9a4cef511904ad4038c0b1443d3f6',1,'CO_GTWA_t']]] +]; diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/all_8.html b/Devices/Libraries/Systems/CANopenSocket/docs/search/all_8.html new file mode 100755 index 0000000..cf2b5df --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/all_8.html @@ -0,0 +1,30 @@ + + + + + + + + + +
+
Loading...
+
+ +
Searching...
+
No Matches
+ +
+ + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/all_8.js b/Devices/Libraries/Systems/CANopenSocket/docs/search/all_8.js new file mode 100755 index 0000000..a0dc64e --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/all_8.js @@ -0,0 +1,17 @@ +var searchData= +[ + ['ident_926',['ident',['../structCO__CANrx__t.html#a8595c238cf0364bde995dee97d321909',1,'CO_CANrx_t::ident()'],['../structCO__CANtx__t.html#a9cc2687eb11da14d4c0aa167352c635c',1,'CO_CANtx_t::ident()']]], + ['ifname_927',['ifName',['../structCO__CANinterfaceErrorhandler__t.html#a58a5219f8dad7dc1c98db2b463e6e005',1,'CO_CANinterfaceErrorhandler_t']]], + ['index_928',['index',['../structOD__subEntry__t.html#a4e10db7bdf91d721ecc7d97f4dda67ff',1,'OD_subEntry_t::index()'],['../structOD__entry__t.html#ac27d9d9ac18705e84d64f5226a6e352c',1,'OD_entry_t::index()'],['../structCO__SDOclient__t.html#ad4fc4deee415a621f3558266ba447455',1,'CO_SDOclient_t::index()'],['../structCO__SDOserver__t.html#ae0b1720a88d948fbf6d8e20b333abb17',1,'CO_SDOserver_t::index()']]], + ['informationdirection_929',['informationDirection',['../structCO__SRDOCommPar__t.html#ac8f865699090f666910e66dabf53b339',1,'CO_SRDOCommPar_t']]], + ['inhibitemtime_5fus_930',['inhibitEmTime_us',['../structCO__EM__t.html#a82db41fc720e2f2551207bb0d2ba1ae4',1,'CO_EM_t']]], + ['inhibittime_931',['inhibitTime',['../structCO__TPDOCommPar__t.html#ad53403c65582d166898546e329ea9587',1,'CO_TPDOCommPar_t']]], + ['inhibittimer_932',['inhibitTimer',['../structCO__TPDO__t.html#a9277687ef658353801435638c8aa2bee',1,'CO_TPDO_t']]], + ['int16_5ft_933',['int16_t',['../group__CO__dataTypes.html#ga932e6ccc3d54c58f761c1aead83bd6d7',1,'CO_driver.h']]], + ['int32_5ft_934',['int32_t',['../group__CO__dataTypes.html#gadb828ef50c2dbb783109824e94cf6c47',1,'CO_driver.h']]], + ['int64_5ft_935',['int64_t',['../group__CO__dataTypes.html#ga831d6234342279926bb11bad3a37add9',1,'CO_driver.h']]], + ['int8_5ft_936',['int8_t',['../group__CO__dataTypes.html#gaef44329758059c91c76d334e8fc09700',1,'CO_driver.h']]], + ['internalcommand_937',['internalCommand',['../structCO__NMT__t.html#a7a56fcf387bd2922ece9106538d986ef',1,'CO_NMT_t']]], + ['isconsumer_938',['isConsumer',['../structCO__TIME__t.html#aeea1ecdf76e04f37c1d760eee72a1395',1,'CO_TIME_t']]], + ['isproducer_939',['isProducer',['../structCO__SYNC__t.html#af37a656db91d31a8187e0350f472ea36',1,'CO_SYNC_t::isProducer()'],['../structCO__TIME__t.html#a691d02cb2128a81f4af345165146b761',1,'CO_TIME_t::isProducer()']]] +]; diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/all_9.html b/Devices/Libraries/Systems/CANopenSocket/docs/search/all_9.html new file mode 100755 index 0000000..690785a --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/all_9.html @@ -0,0 +1,30 @@ + + + + + + + + + +
+
Loading...
+
+ +
Searching...
+
No Matches
+ +
+ + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/all_9.js b/Devices/Libraries/Systems/CANopenSocket/docs/search/all_9.js new file mode 100755 index 0000000..dada6bd --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/all_9.js @@ -0,0 +1,39 @@ +var searchData= +[ + ['led_20indicators_940',['LED indicators',['../group__CO__LEDs.html',1,'']]], + ['lss_941',['LSS',['../group__CO__LSS.html',1,'']]], + ['lss_20master_942',['LSS Master',['../group__CO__LSSmaster.html',1,'']]], + ['lss_20slave_943',['LSS Slave',['../group__CO__LSSslave.html',1,'']]], + ['lss_20master_2fslave_944',['LSS master/slave',['../group__CO__STACK__CONFIG__LSS.html',1,'']]], + ['lastsavedus_945',['lastSavedUs',['../structCO__OD__storage__t.html#a94d80f0c140485ab426891839b356347',1,'CO_OD_storage_t']]], + ['lasttimestamp_946',['lastTimeStamp',['../structCO__trace__t.html#af3ffa370f4b25a3c3fd27f9ce75379c0',1,'CO_trace_t']]], + ['ledgreen_947',['LEDgreen',['../structCO__LEDs__t.html#ac2b4eb053725681f1935c8a6e4184f85',1,'CO_LEDs_t']]], + ['ledred_948',['LEDred',['../structCO__LEDs__t.html#ad8624f0baa6186e523e072e6101c7a84',1,'CO_LEDs_t']]], + ['leds_949',['LEDs',['../structCO__t.html#a4837a9caa7daa219a8ecff5c64bc08ba',1,'CO_t::LEDs()'],['../structCO__GTWA__t.html#a90ec5c0f770fd1a3ec2ea4f33059357a',1,'CO_GTWA_t::LEDs()']]], + ['ledtmr200ms_950',['LEDtmr200ms',['../structCO__LEDs__t.html#a2690fe669ce01322e14de1b9f559a147',1,'CO_LEDs_t']]], + ['ledtmr50ms_951',['LEDtmr50ms',['../structCO__LEDs__t.html#ae27f4bbc313e08e25b9b1e1d515ea9e4',1,'CO_LEDs_t']]], + ['ledtmrflash_5f1_952',['LEDtmrflash_1',['../structCO__LEDs__t.html#a2431d9736416e1b5b6c57f61f029dc04',1,'CO_LEDs_t']]], + ['ledtmrflash_5f2_953',['LEDtmrflash_2',['../structCO__LEDs__t.html#ae6aa2a97a9496abc67774d94caa0ea1a',1,'CO_LEDs_t']]], + ['ledtmrflash_5f3_954',['LEDtmrflash_3',['../structCO__LEDs__t.html#ad3af9ed6c2c43330cc3eadb3b76de2c4',1,'CO_LEDs_t']]], + ['ledtmrflash_5f4_955',['LEDtmrflash_4',['../structCO__LEDs__t.html#ae33491a6eed2761893fea47dd65c5a78',1,'CO_LEDs_t']]], + ['list_956',['list',['../structOD__t.html#a935673d88e82c7a72be76766467d74b4',1,'OD_t']]], + ['listenonly_957',['listenOnly',['../structCO__CANinterfaceErrorhandler__t.html#a96ddaefd75e680898f93d4891f5fc195',1,'CO_CANinterfaceErrorhandler_t']]], + ['localsocketpath_958',['localSocketPath',['../structCO__epoll__gtw__t.html#a5f11979c2bc301d6b8cc7ddce8d45688',1,'CO_epoll_gtw_t']]], + ['log_5fprintf_959',['log_printf',['../group__CO__socketCAN__ERROR.html#gac9aeec86e89e5525b4e13e3b1e21866d',1,'log_printf(int priority, const char *format,...): CO_main_basic.c'],['../group__CO__socketCAN__ERROR.html#gac9aeec86e89e5525b4e13e3b1e21866d',1,'log_printf(int priority, const char *format,...): CO_main_basic.c']]], + ['logbuf_960',['logBuf',['../structCO__GTWA__t.html#a7400a0dcf3d8ad25b8ad9820237d4f3c',1,'CO_GTWA_t']]], + ['logfifo_961',['logFifo',['../structCO__GTWA__t.html#af84217848a2f2e4d3f840f4978d4e2ac',1,'CO_GTWA_t']]], + ['lssaddress_962',['lssAddress',['../structCO__LSSslave__t.html#ae18023e6feb33ea4e9ddf26796a99fe0',1,'CO_LSSslave_t::lssAddress()'],['../structCO__GTWA__t.html#aae702db8c811f7e85f3fd8984bde8f9d',1,'CO_GTWA_t::lssAddress()']]], + ['lssbitrate_963',['lssBitrate',['../structCO__GTWA__t.html#ae404009d2987f589cc52b9958d688917',1,'CO_GTWA_t']]], + ['lssfastscan_964',['lssFastscan',['../structCO__LSSslave__t.html#a433215fe4d5f9764427a5e0dc57d60a5',1,'CO_LSSslave_t::lssFastscan()'],['../structCO__GTWA__t.html#aa39bf29226ecaa52d20a6a92b61cf4fa',1,'CO_GTWA_t::lssFastscan()']]], + ['lssinquirecs_965',['lssInquireCs',['../structCO__GTWA__t.html#a74892b830cb6064d31190ff6bac6a3c9',1,'CO_GTWA_t']]], + ['lssmaster_966',['LSSmaster',['../structCO__t.html#af586261baf229fe624fb72b712208c86',1,'CO_t::LSSmaster()'],['../structCO__GTWA__t.html#ae2897d681afe5bd45db8306b52734318',1,'CO_GTWA_t::LSSmaster()']]], + ['lssnid_967',['lssNID',['../structCO__GTWA__t.html#af49dacd6548ca791a09e78727aeeabad',1,'CO_GTWA_t']]], + ['lssnodecount_968',['lssNodeCount',['../structCO__GTWA__t.html#a16f5893d54bce3d2741d8b732ec6c29e',1,'CO_GTWA_t']]], + ['lssselect_969',['lssSelect',['../structCO__LSSslave__t.html#a38d6e43b3a1c3b0ce20bdb9108f8ab91',1,'CO_LSSslave_t']]], + ['lssslave_970',['LSSslave',['../structCO__t.html#a0c6ecabcb18ed2bd9eb881f9156482b6',1,'CO_t']]], + ['lssstate_971',['lssState',['../structCO__LSSslave__t.html#a514565a5cda1630165c3640ef223aa28',1,'CO_LSSslave_t']]], + ['lssstore_972',['lssStore',['../structCO__GTWA__t.html#a47ec165dd4d545a6ae52067644dbbaab',1,'CO_GTWA_t']]], + ['lsssubstate_973',['lssSubState',['../structCO__GTWA__t.html#a7f55184f433e48dd4d4fb52aebed6da6',1,'CO_GTWA_t']]], + ['lsstimeout_5fms_974',['lssTimeout_ms',['../structCO__GTWA__t.html#a78c3d1a9ebb44a1db4e6eb100ed6c6a7',1,'CO_GTWA_t']]], + ['lss_20usage_975',['LSS usage',['../md_doc_LSSusage.html',1,'']]] +]; diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/all_a.html b/Devices/Libraries/Systems/CANopenSocket/docs/search/all_a.html new file mode 100755 index 0000000..f2f3d3a --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/all_a.html @@ -0,0 +1,30 @@ + + + + + + + + + +
+
Loading...
+
+ +
Searching...
+
No Matches
+ +
+ + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/all_a.js b/Devices/Libraries/Systems/CANopenSocket/docs/search/all_a.js new file mode 100755 index 0000000..0c45794 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/all_a.js @@ -0,0 +1,19 @@ +var searchData= +[ + ['map_976',['map',['../structCO__trace__t.html#ad6e329507a29f46b41ea6bef210b6502',1,'CO_trace_t']]], + ['mappedobject1_977',['mappedObject1',['../structCO__RPDOMapPar__t.html#a0a161a9792479c07aaf12447fbac2499',1,'CO_RPDOMapPar_t::mappedObject1()'],['../structCO__TPDOMapPar__t.html#a6869485bc0705188069c37d6b6ee1694',1,'CO_TPDOMapPar_t::mappedObject1()']]], + ['mappedobject2_978',['mappedObject2',['../structCO__RPDOMapPar__t.html#a63f9b87f27d30e6f4faf65a56b458ffb',1,'CO_RPDOMapPar_t::mappedObject2()'],['../structCO__TPDOMapPar__t.html#a496d610062c09a1667e7541659339eea',1,'CO_TPDOMapPar_t::mappedObject2()']]], + ['mappedobject3_979',['mappedObject3',['../structCO__RPDOMapPar__t.html#a3ac0b7cd18683c45d3dcd0e18b13810b',1,'CO_RPDOMapPar_t::mappedObject3()'],['../structCO__TPDOMapPar__t.html#a8e79bb51d865cae1bab20621db898ddb',1,'CO_TPDOMapPar_t::mappedObject3()']]], + ['mappedobject4_980',['mappedObject4',['../structCO__RPDOMapPar__t.html#a0a3ef7ae329b91ec73d05301e870ad75',1,'CO_RPDOMapPar_t::mappedObject4()'],['../structCO__TPDOMapPar__t.html#aa8a046a95b3ac151f5d8d8cb4415851e',1,'CO_TPDOMapPar_t::mappedObject4()']]], + ['mappedobject5_981',['mappedObject5',['../structCO__RPDOMapPar__t.html#af532c96971699321f04aad14dbee52a4',1,'CO_RPDOMapPar_t::mappedObject5()'],['../structCO__TPDOMapPar__t.html#a0fbdd6c39635c8288771867bdc061d6f',1,'CO_TPDOMapPar_t::mappedObject5()']]], + ['mappedobject6_982',['mappedObject6',['../structCO__RPDOMapPar__t.html#a1082b9de3f132363f78c19004be017fb',1,'CO_RPDOMapPar_t::mappedObject6()'],['../structCO__TPDOMapPar__t.html#ad8f8f0e0629b1c0467599ea8b138e3eb',1,'CO_TPDOMapPar_t::mappedObject6()']]], + ['mappedobject7_983',['mappedObject7',['../structCO__RPDOMapPar__t.html#ad66f07a873d2ceb0b10bdad242925984',1,'CO_RPDOMapPar_t::mappedObject7()'],['../structCO__TPDOMapPar__t.html#a1fb19d7423d2fd74bc8575aff1657552',1,'CO_TPDOMapPar_t::mappedObject7()']]], + ['mappedobject8_984',['mappedObject8',['../structCO__RPDOMapPar__t.html#a2f1467c9ba8a91e4f2742c08ed8551f6',1,'CO_RPDOMapPar_t::mappedObject8()'],['../structCO__TPDOMapPar__t.html#aa482b092dd475bac21193c671dabdb49',1,'CO_TPDOMapPar_t::mappedObject8()']]], + ['mappointer_985',['mapPointer',['../structCO__RPDO__t.html#a998b83bc1cbf11aa4e9170ce03c9d203',1,'CO_RPDO_t::mapPointer()'],['../structCO__TPDO__t.html#acb6fa2c4037b1e41afdf6195cd6e93ec',1,'CO_TPDO_t::mapPointer()'],['../structCO__SRDO__t.html#a3e7570a1aeef89502702175eccb93500',1,'CO_SRDO_t::mapPointer()']]], + ['mask_986',['mask',['../structCO__CANrx__t.html#af7a48dd4ac895a19c4031038e2c1222d',1,'CO_CANrx_t']]], + ['match_987',['match',['../structCO__LSSmaster__fastscan__t.html#a69540f77885162e936803a9526d3c342',1,'CO_LSSmaster_fastscan_t']]], + ['maxsubindex_988',['maxSubIndex',['../structCO__RPDOCommPar__t.html#ac49d1bc96c31ec32015628128a6e60a7',1,'CO_RPDOCommPar_t::maxSubIndex()'],['../structCO__TPDOCommPar__t.html#a74a8681177fabbb55ebf5be843f12fc5',1,'CO_TPDOCommPar_t::maxSubIndex()'],['../structCO__SRDOCommPar__t.html#af156b61e6d278b014466e860f073cf05',1,'CO_SRDOCommPar_t::maxSubIndex()']]], + ['maxvalue_989',['maxValue',['../structCO__trace__t.html#a7b41b99eb65d49fe522c25d08127f81d',1,'CO_trace_t']]], + ['minvalue_990',['minValue',['../structCO__trace__t.html#a0f1b287dae5b6a30b43f481d2bee41ab',1,'CO_trace_t']]], + ['monitorednodes_991',['monitoredNodes',['../structCO__HBconsumer__t.html#a737b37c544a28eff8de0b03b51cbeec8',1,'CO_HBconsumer_t']]] +]; diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/all_b.html b/Devices/Libraries/Systems/CANopenSocket/docs/search/all_b.html new file mode 100755 index 0000000..14f3403 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/all_b.html @@ -0,0 +1,30 @@ + + + + + + + + + +
+
Loading...
+
+ +
Searching...
+
No Matches
+ +
+ + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/all_b.js b/Devices/Libraries/Systems/CANopenSocket/docs/search/all_b.js new file mode 100755 index 0000000..452de47 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/all_b.js @@ -0,0 +1,23 @@ +var searchData= +[ + ['nmt_20and_20heartbeat_992',['NMT and Heartbeat',['../group__CO__NMT__Heartbeat.html',1,'']]], + ['nmt_20master_2fslave_20and_20hb_20producer_2fconsumer_993',['NMT master/slave and HB producer/consumer',['../group__CO__STACK__CONFIG__NMT__HB.html',1,'']]], + ['net_994',['net',['../structCO__GTWA__t.html#a8df8a3f47d967e4fb0a56e491db0f9e9',1,'CO_GTWA_t']]], + ['net_5fdefault_995',['net_default',['../structCO__GTWA__t.html#aec9a1ffe0ce40572452d3f1e36e51c1b',1,'CO_GTWA_t']]], + ['nmt_996',['NMT',['../structCO__t.html#a6fddf777eec75e0cc8fc510a5c4ec8e5',1,'CO_t::NMT()'],['../structCO__GTWA__t.html#a6aa019a1583f8ba56fada7c5ed8ec191',1,'CO_GTWA_t::NMT()']]], + ['nmt_5fcandevtx_997',['NMT_CANdevTx',['../structCO__NMT__t.html#aaa1a7f9278595d82eaddbee8ac434ca9',1,'CO_NMT_t']]], + ['nmt_5ftxbuff_998',['NMT_TXbuff',['../structCO__NMT__t.html#a6c9ca6315daa74257699ff657165d409',1,'CO_NMT_t']]], + ['nmtcontrol_999',['NMTcontrol',['../structCO__NMT__t.html#a0c28cae6c3c7319c8fdfe694a3b4d9a7',1,'CO_NMT_t']]], + ['nmtispreoroperationalprev_1000',['NMTisPreOrOperationalPrev',['../structCO__HBconsumer__t.html#a2fe3d81e2124918d0d5947e6891a060e',1,'CO_HBconsumer_t']]], + ['nmtstate_1001',['NMTstate',['../structCO__HBconsNode__t.html#aca87186237691cc315da47d5bcc8ad31',1,'CO_HBconsNode_t']]], + ['nmtstateprev_1002',['NMTstatePrev',['../structCO__HBconsNode__t.html#a7fd2636d9f46b7ff47676f716f6f00a4',1,'CO_HBconsNode_t']]], + ['noackcounter_1003',['noackCounter',['../structCO__CANinterfaceErrorhandler__t.html#a6d17a248ec3ce1f6c53f4315c0cb9282',1,'CO_CANinterfaceErrorhandler_t']]], + ['node_1004',['node',['../structCO__GTWA__t.html#a38f5c9325dc69820d831688282a63a10',1,'CO_GTWA_t']]], + ['node_5fdefault_1005',['node_default',['../structCO__GTWA__t.html#a2464fa84713d31811e8872b4557d50d1',1,'CO_GTWA_t']]], + ['nodeid_1006',['nodeId',['../structCO__EM__t.html#ac5522470ed7ea0f5e91520a2563b9abc',1,'CO_EM_t::nodeId()'],['../structCO__HBconsNode__t.html#a180aca37057c670be35bbdd89f72b812',1,'CO_HBconsNode_t::nodeId()'],['../structCO__NMT__t.html#a6cf0441aff58a3e208d1ed221a26709c',1,'CO_NMT_t::nodeId()'],['../structCO__RPDO__t.html#a15e1425101d92521ad219695036b1cd2',1,'CO_RPDO_t::nodeId()'],['../structCO__TPDO__t.html#a4ef8ced15f6fffb56a0ec4aeb48d4551',1,'CO_TPDO_t::nodeId()'],['../structCO__SDOclient__t.html#a13de9457791eecf17051e405665bfa4a',1,'CO_SDOclient_t::nodeId()'],['../structCO__SDOserver__t.html#a38d0b70cb37d6be927208e3662105c6c',1,'CO_SDOserver_t::nodeId()'],['../structCO__SRDO__t.html#ac4cc841f24894e41a5bbdbd386e62a0e',1,'CO_SRDO_t::nodeId()']]], + ['nodeidofthesdoserver_1007',['nodeIDOfTheSDOServer',['../structCO__SDOclient__t.html#a9e3c564cd4d027c5bd93bd25293ebacc',1,'CO_SDOclient_t']]], + ['nodeidunconfigured_1008',['nodeIdUnconfigured',['../structCO__t.html#a139e71d4b3c9f2c072df13e6ac0dbac4',1,'CO_t']]], + ['null_1009',['NULL',['../group__CO__dataTypes.html#ga070d2ce7b6bb7e5c05602aa8c308d0c4',1,'CO_driver.h']]], + ['numberofmappedobjects_1010',['numberOfMappedObjects',['../structCO__RPDOMapPar__t.html#a479613deae0d06607897093d617edb1d',1,'CO_RPDOMapPar_t::numberOfMappedObjects()'],['../structCO__TPDOMapPar__t.html#a10718cf84dc6a975509d327efb8403de',1,'CO_TPDOMapPar_t::numberOfMappedObjects()']]], + ['numberofmonitorednodes_1011',['numberOfMonitoredNodes',['../structCO__HBconsumer__t.html#a5b944043074d42017be3b76320030542',1,'CO_HBconsumer_t']]] +]; diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/all_c.html b/Devices/Libraries/Systems/CANopenSocket/docs/search/all_c.html new file mode 100755 index 0000000..da60ab8 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/all_c.html @@ -0,0 +1,30 @@ + + + + + + + + + +
+
Loading...
+
+ +
Searching...
+
No Matches
+ +
+ + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/all_c.js b/Devices/Libraries/Systems/CANopenSocket/docs/search/all_c.js new file mode 100755 index 0000000..932d260 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/all_c.js @@ -0,0 +1,172 @@ +var searchData= +[ + ['od_20definition_20objects_1012',['OD definition objects',['../group__CO__ODdefinition.html',1,'']]], + ['od_20interface_1013',['OD interface',['../group__CO__ODinterface.html',1,'']]], + ['od_20storage_1014',['OD storage',['../group__CO__socketCAN__OD__storage.html',1,'']]], + ['object_20dictionary_1015',['Object Dictionary',['../md_doc_objectDictionary.html',1,'']]], + ['object_1016',['object',['../structCO__CANrx__t.html#a957a1ce67cd1d9010889d557bf0c5770',1,'CO_CANrx_t::object()'],['../structOD__stream__t.html#af3c8ded429eefa8646317207c0b3ff97',1,'OD_stream_t::object()'],['../structOD__extensionIO__t.html#a929dace9c5bf5f1e3090f3fbff40f9b8',1,'OD_extensionIO_t::object()']]], + ['ochar_5ft_1017',['oChar_t',['../group__CO__dataTypes.html#ga00f664c467579d7b2839d6926b6f33a6',1,'CO_driver.h']]], + ['od_1018',['OD',['../structCO__SDOclient__t.html#a89334ad9cc4ac6751f4a8225f1645923',1,'CO_SDOclient_t::OD()'],['../structCO__SDOserver__t.html#a8c104c076f57ffe039e0e41a1b12377e',1,'CO_SDOserver_t::OD()']]], + ['od_5fattr_5ft_1019',['OD_attr_t',['../group__CO__ODinterface.html#ga8d459f95307815637e41edc4df71a725',1,'CO_ODinterface.h']]], + ['od_5fattributes_5ft_1020',['OD_attributes_t',['../group__CO__ODinterface.html#ga47b0d204aaf1ea64b4f826aaf8f5c151',1,'CO_ODinterface.h']]], + ['od_5fentry_5ft_1021',['OD_entry_t',['../structOD__entry__t.html',1,'']]], + ['od_5fextensionio_5finit_1022',['OD_extensionIO_init',['../group__CO__ODinterface.html#gac07bbe54fbfecc6bc8da2e10b2c0f7e8',1,'OD_extensionIO_init(const OD_entry_t *entry, void *object, OD_size_t(*read)(OD_stream_t *stream, uint8_t subIndex, void *buf, OD_size_t count, ODR_t *returnCode), OD_size_t(*write)(OD_stream_t *stream, uint8_t subIndex, const void *buf, OD_size_t count, ODR_t *returnCode)): CO_ODinterface.c'],['../group__CO__ODinterface.html#gac07bbe54fbfecc6bc8da2e10b2c0f7e8',1,'OD_extensionIO_init(const OD_entry_t *entry, void *object, OD_size_t(*read)(OD_stream_t *stream, uint8_t subIndex, void *buf, OD_size_t count, ODR_t *returnCode), OD_size_t(*write)(OD_stream_t *stream, uint8_t subIndex, const void *buf, OD_size_t count, ODR_t *returnCode)): CO_ODinterface.c']]], + ['od_5fextensionio_5ft_1023',['OD_extensionIO_t',['../structOD__extensionIO__t.html',1,'']]], + ['od_5ffind_1024',['OD_find',['../group__CO__ODinterface.html#gaaacaadfc28bfaf485cefc8bff64310f4',1,'OD_find(const OD_t *od, uint16_t index): CO_ODinterface.c'],['../group__CO__ODinterface.html#gaaacaadfc28bfaf485cefc8bff64310f4',1,'OD_find(const OD_t *od, uint16_t index): CO_ODinterface.c']]], + ['od_5fflagspdo_5ft_1025',['OD_flagsPDO_t',['../group__CO__ODinterface.html#ga69f6e1121545e5669098f49e95ce4e47',1,'CO_ODinterface.h']]], + ['od_5fget_5fi16_1026',['OD_get_i16',['../group__CO__ODgetSetters.html#ga4bcc220e0cc4f8c6bfaf4d5cf31da448',1,'OD_get_i16(const OD_entry_t *entry, uint8_t subIndex, int16_t *val, bool_t odOrig): CO_ODinterface.c'],['../group__CO__ODgetSetters.html#ga4bcc220e0cc4f8c6bfaf4d5cf31da448',1,'OD_get_i16(const OD_entry_t *entry, uint8_t subIndex, int16_t *val, bool_t odOrig): CO_ODinterface.c']]], + ['od_5fget_5fi32_1027',['OD_get_i32',['../group__CO__ODgetSetters.html#ga042737cecbf248d2cc6c874022d06e22',1,'OD_get_i32(const OD_entry_t *entry, uint8_t subIndex, int32_t *val, bool_t odOrig): CO_ODinterface.c'],['../group__CO__ODgetSetters.html#ga042737cecbf248d2cc6c874022d06e22',1,'OD_get_i32(const OD_entry_t *entry, uint8_t subIndex, int32_t *val, bool_t odOrig): CO_ODinterface.c']]], + ['od_5fget_5fi64_1028',['OD_get_i64',['../group__CO__ODgetSetters.html#gabfb67bc3602d9b5d901ac13e5271ccd0',1,'OD_get_i64(const OD_entry_t *entry, uint8_t subIndex, int64_t *val, bool_t odOrig): CO_ODinterface.c'],['../group__CO__ODgetSetters.html#gabfb67bc3602d9b5d901ac13e5271ccd0',1,'OD_get_i64(const OD_entry_t *entry, uint8_t subIndex, int64_t *val, bool_t odOrig): CO_ODinterface.c']]], + ['od_5fget_5fi8_1029',['OD_get_i8',['../group__CO__ODgetSetters.html#gac174f05be716b7d169e0d7b7393e512c',1,'OD_get_i8(const OD_entry_t *entry, uint8_t subIndex, int8_t *val, bool_t odOrig): CO_ODinterface.c'],['../group__CO__ODgetSetters.html#gac174f05be716b7d169e0d7b7393e512c',1,'OD_get_i8(const OD_entry_t *entry, uint8_t subIndex, int8_t *val, bool_t odOrig): CO_ODinterface.c']]], + ['od_5fget_5fr32_1030',['OD_get_r32',['../group__CO__ODgetSetters.html#gabbba108fa7a92cb48d31833bb804baa6',1,'OD_get_r32(const OD_entry_t *entry, uint8_t subIndex, float32_t *val, bool_t odOrig): CO_ODinterface.c'],['../group__CO__ODgetSetters.html#gabbba108fa7a92cb48d31833bb804baa6',1,'OD_get_r32(const OD_entry_t *entry, uint8_t subIndex, float32_t *val, bool_t odOrig): CO_ODinterface.c']]], + ['od_5fget_5fr64_1031',['OD_get_r64',['../group__CO__ODgetSetters.html#gadf78189c35343bcd2ca1737491b55684',1,'OD_get_r64(const OD_entry_t *entry, uint8_t subIndex, float64_t *val, bool_t odOrig): CO_ODinterface.c'],['../group__CO__ODgetSetters.html#gadf78189c35343bcd2ca1737491b55684',1,'OD_get_r64(const OD_entry_t *entry, uint8_t subIndex, float64_t *val, bool_t odOrig): CO_ODinterface.c']]], + ['od_5fget_5fu16_1032',['OD_get_u16',['../group__CO__ODgetSetters.html#gaa9cba6642facf33cdbe2e0155a90d571',1,'OD_get_u16(const OD_entry_t *entry, uint8_t subIndex, uint16_t *val, bool_t odOrig): CO_ODinterface.c'],['../group__CO__ODgetSetters.html#gaa9cba6642facf33cdbe2e0155a90d571',1,'OD_get_u16(const OD_entry_t *entry, uint8_t subIndex, uint16_t *val, bool_t odOrig): CO_ODinterface.c']]], + ['od_5fget_5fu32_1033',['OD_get_u32',['../group__CO__ODgetSetters.html#ga10e1975b6177b92e8da4b2b6a19533be',1,'OD_get_u32(const OD_entry_t *entry, uint8_t subIndex, uint32_t *val, bool_t odOrig): CO_ODinterface.c'],['../group__CO__ODgetSetters.html#ga10e1975b6177b92e8da4b2b6a19533be',1,'OD_get_u32(const OD_entry_t *entry, uint8_t subIndex, uint32_t *val, bool_t odOrig): CO_ODinterface.c']]], + ['od_5fget_5fu64_1034',['OD_get_u64',['../group__CO__ODgetSetters.html#ga2ef84e594a6733f7efeaeff55bfc9c9a',1,'OD_get_u64(const OD_entry_t *entry, uint8_t subIndex, uint64_t *val, bool_t odOrig): CO_ODinterface.c'],['../group__CO__ODgetSetters.html#ga2ef84e594a6733f7efeaeff55bfc9c9a',1,'OD_get_u64(const OD_entry_t *entry, uint8_t subIndex, uint64_t *val, bool_t odOrig): CO_ODinterface.c']]], + ['od_5fget_5fu8_1035',['OD_get_u8',['../group__CO__ODgetSetters.html#gad8fd318804b9f1ded265bfd07c6cdcf2',1,'OD_get_u8(const OD_entry_t *entry, uint8_t subIndex, uint8_t *val, bool_t odOrig): CO_ODinterface.c'],['../group__CO__ODgetSetters.html#gad8fd318804b9f1ded265bfd07c6cdcf2',1,'OD_get_u8(const OD_entry_t *entry, uint8_t subIndex, uint8_t *val, bool_t odOrig): CO_ODinterface.c']]], + ['od_5fgetindex_1036',['OD_getIndex',['../group__CO__ODinterface.html#gac84e7390f50e7e5c5e8ba42714e51aaf',1,'CO_ODinterface.h']]], + ['od_5fgetptr_5fi16_1037',['OD_getPtr_i16',['../group__CO__ODgetSetters.html#gaddbad04c274a68f3fabfbbdd13e83cfc',1,'OD_getPtr_i16(const OD_entry_t *entry, uint8_t subIndex, int16_t **val): CO_ODinterface.c'],['../group__CO__ODgetSetters.html#gaddbad04c274a68f3fabfbbdd13e83cfc',1,'OD_getPtr_i16(const OD_entry_t *entry, uint8_t subIndex, int16_t **val): CO_ODinterface.c']]], + ['od_5fgetptr_5fi32_1038',['OD_getPtr_i32',['../group__CO__ODgetSetters.html#ga085db308dd11ea7047c815d214f12a32',1,'OD_getPtr_i32(const OD_entry_t *entry, uint8_t subIndex, int32_t **val): CO_ODinterface.c'],['../group__CO__ODgetSetters.html#ga085db308dd11ea7047c815d214f12a32',1,'OD_getPtr_i32(const OD_entry_t *entry, uint8_t subIndex, int32_t **val): CO_ODinterface.c']]], + ['od_5fgetptr_5fi64_1039',['OD_getPtr_i64',['../group__CO__ODgetSetters.html#gae7e11e1bdf04006dcc6a9f91287c459b',1,'OD_getPtr_i64(const OD_entry_t *entry, uint8_t subIndex, int64_t **val): CO_ODinterface.c'],['../group__CO__ODgetSetters.html#gae7e11e1bdf04006dcc6a9f91287c459b',1,'OD_getPtr_i64(const OD_entry_t *entry, uint8_t subIndex, int64_t **val): CO_ODinterface.c']]], + ['od_5fgetptr_5fi8_1040',['OD_getPtr_i8',['../group__CO__ODgetSetters.html#gaa4e6671855056e5ae13d198acdfab664',1,'OD_getPtr_i8(const OD_entry_t *entry, uint8_t subIndex, int8_t **val): CO_ODinterface.c'],['../group__CO__ODgetSetters.html#gaa4e6671855056e5ae13d198acdfab664',1,'OD_getPtr_i8(const OD_entry_t *entry, uint8_t subIndex, int8_t **val): CO_ODinterface.c']]], + ['od_5fgetptr_5fos_1041',['OD_getPtr_os',['../group__CO__ODgetSetters.html#ga06361976dce86f180de76e1fced9a212',1,'OD_getPtr_os(const OD_entry_t *entry, uint8_t subIndex, uint8_t **val, OD_size_t *dataLength): CO_ODinterface.c'],['../group__CO__ODgetSetters.html#ga06361976dce86f180de76e1fced9a212',1,'OD_getPtr_os(const OD_entry_t *entry, uint8_t subIndex, uint8_t **val, OD_size_t *dataLength): CO_ODinterface.c']]], + ['od_5fgetptr_5fr32_1042',['OD_getPtr_r32',['../group__CO__ODgetSetters.html#gaf4e3f15fc0fbe2b24e88aeb53c9daa61',1,'OD_getPtr_r32(const OD_entry_t *entry, uint8_t subIndex, float32_t **val): CO_ODinterface.c'],['../group__CO__ODgetSetters.html#gaf4e3f15fc0fbe2b24e88aeb53c9daa61',1,'OD_getPtr_r32(const OD_entry_t *entry, uint8_t subIndex, float32_t **val): CO_ODinterface.c']]], + ['od_5fgetptr_5fr64_1043',['OD_getPtr_r64',['../group__CO__ODgetSetters.html#ga87a34e3e04dd2faa851d2d9c0a24e945',1,'OD_getPtr_r64(const OD_entry_t *entry, uint8_t subIndex, float64_t **val): CO_ODinterface.c'],['../group__CO__ODgetSetters.html#ga87a34e3e04dd2faa851d2d9c0a24e945',1,'OD_getPtr_r64(const OD_entry_t *entry, uint8_t subIndex, float64_t **val): CO_ODinterface.c']]], + ['od_5fgetptr_5fu16_1044',['OD_getPtr_u16',['../group__CO__ODgetSetters.html#gad1e334a640dc30553ece27e32e270bbb',1,'OD_getPtr_u16(const OD_entry_t *entry, uint8_t subIndex, uint16_t **val): CO_ODinterface.c'],['../group__CO__ODgetSetters.html#gad1e334a640dc30553ece27e32e270bbb',1,'OD_getPtr_u16(const OD_entry_t *entry, uint8_t subIndex, uint16_t **val): CO_ODinterface.c']]], + ['od_5fgetptr_5fu32_1045',['OD_getPtr_u32',['../group__CO__ODgetSetters.html#ga92f57ed14ed69ac00c78d48f1e479bb0',1,'OD_getPtr_u32(const OD_entry_t *entry, uint8_t subIndex, uint32_t **val): CO_ODinterface.c'],['../group__CO__ODgetSetters.html#ga92f57ed14ed69ac00c78d48f1e479bb0',1,'OD_getPtr_u32(const OD_entry_t *entry, uint8_t subIndex, uint32_t **val): CO_ODinterface.c']]], + ['od_5fgetptr_5fu64_1046',['OD_getPtr_u64',['../group__CO__ODgetSetters.html#gad192b1efde6a5bd6fa62fc5a4c484d84',1,'OD_getPtr_u64(const OD_entry_t *entry, uint8_t subIndex, uint64_t **val): CO_ODinterface.c'],['../group__CO__ODgetSetters.html#gad192b1efde6a5bd6fa62fc5a4c484d84',1,'OD_getPtr_u64(const OD_entry_t *entry, uint8_t subIndex, uint64_t **val): CO_ODinterface.c']]], + ['od_5fgetptr_5fu8_1047',['OD_getPtr_u8',['../group__CO__ODgetSetters.html#gaaa9f06494001462d32a74d11438d3157',1,'OD_getPtr_u8(const OD_entry_t *entry, uint8_t subIndex, uint8_t **val): CO_ODinterface.c'],['../group__CO__ODgetSetters.html#gaaa9f06494001462d32a74d11438d3157',1,'OD_getPtr_u8(const OD_entry_t *entry, uint8_t subIndex, uint8_t **val): CO_ODinterface.c']]], + ['od_5fgetptr_5fus_1048',['OD_getPtr_us',['../group__CO__ODgetSetters.html#gaf3efb5f4a50ff7ab17b37bf9c0fcf030',1,'OD_getPtr_us(const OD_entry_t *entry, uint8_t subIndex, uint16_t **val, OD_size_t *dataLength): CO_ODinterface.c'],['../group__CO__ODgetSetters.html#gaf3efb5f4a50ff7ab17b37bf9c0fcf030',1,'OD_getPtr_us(const OD_entry_t *entry, uint8_t subIndex, uint16_t **val, OD_size_t *dataLength): CO_ODinterface.c']]], + ['od_5fgetptr_5fvs_1049',['OD_getPtr_vs',['../group__CO__ODgetSetters.html#gaad4d755515e8dccf3616f14fe3914bde',1,'OD_getPtr_vs(const OD_entry_t *entry, uint8_t subIndex, char **val, OD_size_t *dataLength): CO_ODinterface.c'],['../group__CO__ODgetSetters.html#gaad4d755515e8dccf3616f14fe3914bde',1,'OD_getPtr_vs(const OD_entry_t *entry, uint8_t subIndex, char **val, OD_size_t *dataLength): CO_ODinterface.c']]], + ['od_5fgetsdoabcode_1050',['OD_getSDOabCode',['../group__CO__ODinterface.html#ga7c24b06bb9365d41b8e60acb4eaecc6c',1,'OD_getSDOabCode(ODR_t returnCode): CO_ODinterface.c'],['../group__CO__ODinterface.html#ga7c24b06bb9365d41b8e60acb4eaecc6c',1,'OD_getSDOabCode(ODR_t returnCode): CO_ODinterface.c']]], + ['od_5fgetsub_1051',['OD_getSub',['../group__CO__ODinterface.html#gaf1f736d4b4d6754d971f0c0a63655bcf',1,'OD_getSub(const OD_entry_t *entry, uint8_t subIndex, OD_subEntry_t *subEntry, OD_IO_t *io, bool_t odOrig): CO_ODinterface.c'],['../group__CO__ODinterface.html#gaf1f736d4b4d6754d971f0c0a63655bcf',1,'OD_getSub(const OD_entry_t *entry, uint8_t subIndex, OD_subEntry_t *subEntry, OD_IO_t *io, bool_t odOrig): CO_ODinterface.c']]], + ['od_5fh1000_5fdev_5ftype_1052',['OD_H1000_DEV_TYPE',['../group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886af1e65ef6eb730b9302540e0ba44852b1',1,'CO_ODinterface.h']]], + ['od_5fh1001_5ferr_5freg_1053',['OD_H1001_ERR_REG',['../group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886af4eb4e0204ae9696f935af5d4fdcff7e',1,'CO_ODinterface.h']]], + ['od_5fh1002_5fmanuf_5fstatus_5freg_1054',['OD_H1002_MANUF_STATUS_REG',['../group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a2cca52f61d70db5ca2aaa168b32f3aaf',1,'CO_ODinterface.h']]], + ['od_5fh1003_5fpredef_5ferr_5ffield_1055',['OD_H1003_PREDEF_ERR_FIELD',['../group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886adfc213af5af80cf037231621132013fb',1,'CO_ODinterface.h']]], + ['od_5fh1004_5frsv_1056',['OD_H1004_RSV',['../group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886ad7d53fa95504566811bdf0683f645ccd',1,'CO_ODinterface.h']]], + ['od_5fh1005_5fcobid_5fsync_1057',['OD_H1005_COBID_SYNC',['../group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886adcbc9ec0c547b00db2b0403708becb97',1,'CO_ODinterface.h']]], + ['od_5fh1006_5fcomm_5fcycl_5fperiod_1058',['OD_H1006_COMM_CYCL_PERIOD',['../group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a463dab19c27811dd6de51fcc082b565b',1,'CO_ODinterface.h']]], + ['od_5fh1007_5fsync_5fwindow_5flen_1059',['OD_H1007_SYNC_WINDOW_LEN',['../group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a41f7f0f96dbea4fb0d6bd2bbbd2d59dc',1,'CO_ODinterface.h']]], + ['od_5fh1008_5fmanuf_5fdev_5fname_1060',['OD_H1008_MANUF_DEV_NAME',['../group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886aa282ef78e8a64ff527c79218d23168f0',1,'CO_ODinterface.h']]], + ['od_5fh1009_5fmanuf_5fhw_5fversion_1061',['OD_H1009_MANUF_HW_VERSION',['../group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886ad91534dd5cc5f382287b9a392c744948',1,'CO_ODinterface.h']]], + ['od_5fh100a_5fmanuf_5fsw_5fversion_1062',['OD_H100A_MANUF_SW_VERSION',['../group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a8bd48d9bb18d1c291249050818c82a57',1,'CO_ODinterface.h']]], + ['od_5fh100b_5frsv_1063',['OD_H100B_RSV',['../group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a561260a506655cf0b7df9d684a08b5be',1,'CO_ODinterface.h']]], + ['od_5fh100c_5fguard_5ftime_1064',['OD_H100C_GUARD_TIME',['../group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886adf00d5b448274a91940cac15b8e22fc5',1,'CO_ODinterface.h']]], + ['od_5fh100d_5flifetime_5ffactor_1065',['OD_H100D_LIFETIME_FACTOR',['../group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a4bfbb36a82606125d52fbe4daff6b5fb',1,'CO_ODinterface.h']]], + ['od_5fh100e_5frsv_1066',['OD_H100E_RSV',['../group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a4d831b2a36d679d31982e35ca38f8f6e',1,'CO_ODinterface.h']]], + ['od_5fh100f_5frsv_1067',['OD_H100F_RSV',['../group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a7d6a3f1ca8f72bf808ee5fe341f2acca',1,'CO_ODinterface.h']]], + ['od_5fh1010_5fstore_5fparam_5ffunc_1068',['OD_H1010_STORE_PARAM_FUNC',['../group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a0f8c3db5a62d5e4df59d83253b69b0f2',1,'CO_ODinterface.h']]], + ['od_5fh1011_5frest_5fparam_5ffunc_1069',['OD_H1011_REST_PARAM_FUNC',['../group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886adad8c0ea18f674f3eb61b43e8259395c',1,'CO_ODinterface.h']]], + ['od_5fh1012_5fcobid_5ftime_1070',['OD_H1012_COBID_TIME',['../group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a5781f519d9ec08fd4389c4761754a4e6',1,'CO_ODinterface.h']]], + ['od_5fh1013_5fhigh_5fres_5ftimestamp_1071',['OD_H1013_HIGH_RES_TIMESTAMP',['../group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a7aa32b2b6df7c4d4354599ef2fd2ca29',1,'CO_ODinterface.h']]], + ['od_5fh1014_5fcobid_5femergency_1072',['OD_H1014_COBID_EMERGENCY',['../group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a5cae036f3dd0bc1861dcea7c9a83c6d5',1,'CO_ODinterface.h']]], + ['od_5fh1015_5finhibit_5ftime_5femcy_1073',['OD_H1015_INHIBIT_TIME_EMCY',['../group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886adbcd0d36ab781fc60b05a94288b8f019',1,'CO_ODinterface.h']]], + ['od_5fh1016_5fconsumer_5fhb_5ftime_1074',['OD_H1016_CONSUMER_HB_TIME',['../group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a996952cb963ce6e2783a6fa915d85612',1,'CO_ODinterface.h']]], + ['od_5fh1017_5fproducer_5fhb_5ftime_1075',['OD_H1017_PRODUCER_HB_TIME',['../group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886ab2f428825c1127b286f5b8ace5e881b2',1,'CO_ODinterface.h']]], + ['od_5fh1018_5fidentity_5fobject_1076',['OD_H1018_IDENTITY_OBJECT',['../group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886aabb7a688852e453c5535f663be6298d2',1,'CO_ODinterface.h']]], + ['od_5fh1019_5fsync_5fcnt_5foverflow_1077',['OD_H1019_SYNC_CNT_OVERFLOW',['../group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886aa966d2e020222331b18c5b08261acbf0',1,'CO_ODinterface.h']]], + ['od_5fh1020_5fverify_5fconfig_1078',['OD_H1020_VERIFY_CONFIG',['../group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a8c740bbdd0cb98200d402ec6272d7e8b',1,'CO_ODinterface.h']]], + ['od_5fh1021_5fstore_5feds_1079',['OD_H1021_STORE_EDS',['../group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a259737f7e9ef239d85cb9e7bdeda550b',1,'CO_ODinterface.h']]], + ['od_5fh1022_5fstore_5fformat_1080',['OD_H1022_STORE_FORMAT',['../group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a4061d54d1c1583fd178566a3915bcefe',1,'CO_ODinterface.h']]], + ['od_5fh1023_5fos_5fcmd_1081',['OD_H1023_OS_CMD',['../group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a12ba9d8cdfc20b9ff66167a8d1e5b21c',1,'CO_ODinterface.h']]], + ['od_5fh1024_5fos_5fcmd_5fmode_1082',['OD_H1024_OS_CMD_MODE',['../group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886ad7e64256615fcda5b531063eeaa346de',1,'CO_ODinterface.h']]], + ['od_5fh1025_5fos_5fdbg_5finterface_1083',['OD_H1025_OS_DBG_INTERFACE',['../group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886aaf890d86468408b0dbe8353a3b270156',1,'CO_ODinterface.h']]], + ['od_5fh1026_5fos_5fprompt_1084',['OD_H1026_OS_PROMPT',['../group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a6e3b80d148d22f129ed388fad9aaf398',1,'CO_ODinterface.h']]], + ['od_5fh1027_5fmodule_5flist_1085',['OD_H1027_MODULE_LIST',['../group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a668cdf3b0102b753858b9bfeb7efdc1c',1,'CO_ODinterface.h']]], + ['od_5fh1028_5femcy_5fconsumer_1086',['OD_H1028_EMCY_CONSUMER',['../group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a6f093f4fdeaac7b723305fd8d2ce40c1',1,'CO_ODinterface.h']]], + ['od_5fh1029_5ferr_5fbehavior_1087',['OD_H1029_ERR_BEHAVIOR',['../group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a67d50722cc629ee8c2a90a123ee41fa3',1,'CO_ODinterface.h']]], + ['od_5fh1200_5fsdo_5fserver_5f1_5fparam_1088',['OD_H1200_SDO_SERVER_1_PARAM',['../group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a7e351a37cb53541b510e97711e167450',1,'CO_ODinterface.h']]], + ['od_5fh1280_5fsdo_5fclient_5f1_5fparam_1089',['OD_H1280_SDO_CLIENT_1_PARAM',['../group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a3e0e0989da485b0511d7e7cadcdb6dbb',1,'CO_ODinterface.h']]], + ['od_5fh1300_5fgfc_5fparam_1090',['OD_H1300_GFC_PARAM',['../group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886afa584e69f73b7a80cd708010fc0ae64f',1,'CO_ODinterface.h']]], + ['od_5fh1301_5fsrdo_5f1_5fparam_1091',['OD_H1301_SRDO_1_PARAM',['../group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886afbb7d48d240c7584d461712685e62945',1,'CO_ODinterface.h']]], + ['od_5fh1381_5fsrdo_5f1_5fmapping_1092',['OD_H1381_SRDO_1_MAPPING',['../group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a8effa6834289b482e5ac3319ccf2c17b',1,'CO_ODinterface.h']]], + ['od_5fh13fe_5fsrdo_5fvalid_1093',['OD_H13FE_SRDO_VALID',['../group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886aa66d7204dd265d19276113ef7177ce68',1,'CO_ODinterface.h']]], + ['od_5fh13ff_5fsrdo_5fchecksum_1094',['OD_H13FF_SRDO_CHECKSUM',['../group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a92c6bed78bc6bbbd182c16fe8890dcbf',1,'CO_ODinterface.h']]], + ['od_5fh1400_5frxpdo_5f1_5fparam_1095',['OD_H1400_RXPDO_1_PARAM',['../group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886ae64bf5b7166b6adf46b8e965d43150a0',1,'CO_ODinterface.h']]], + ['od_5fh1600_5frxpdo_5f1_5fmapping_1096',['OD_H1600_RXPDO_1_MAPPING',['../group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a7ed53d283e4719920b233b9094b18f9c',1,'CO_ODinterface.h']]], + ['od_5fh1800_5ftxpdo_5f1_5fparam_1097',['OD_H1800_TXPDO_1_PARAM',['../group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886aac35e48e4b717eef309ebb57876d30f1',1,'CO_ODinterface.h']]], + ['od_5fh1a00_5ftxpdo_5f1_5fmapping_1098',['OD_H1A00_TXPDO_1_MAPPING',['../group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a2a2f1c4cc58d29ccf43e105afd57bc14',1,'CO_ODinterface.h']]], + ['od_5findex_5ftrace_5fconfig_1099',['OD_INDEX_TRACE_CONFIG',['../group__CO__trace.html#ga4edaf4c29180028f5f128f0aed2a417d',1,'CO_trace.h']]], + ['od_5fio_1100',['OD_IO',['../structCO__SDOclient__t.html#a16fa659bc3098a3fbd60d0f6efde937a',1,'CO_SDOclient_t::OD_IO()'],['../structCO__SDOserver__t.html#a19e7e8afc09ced5629e3a1e04b83aa9f',1,'CO_SDOserver_t::OD_IO()']]], + ['od_5fio_5ft_1101',['OD_IO_t',['../structOD__IO__t.html',1,'']]], + ['od_5fobj_5farray_5ft_1102',['OD_obj_array_t',['../structOD__obj__array__t.html',1,'']]], + ['od_5fobj_5fextended_5ft_1103',['OD_obj_extended_t',['../structOD__obj__extended__t.html',1,'']]], + ['od_5fobj_5frecord_5ft_1104',['OD_obj_record_t',['../structOD__obj__record__t.html',1,'']]], + ['od_5fobj_5fvar_5ft_1105',['OD_obj_var_t',['../structOD__obj__var__t.html',1,'']]], + ['od_5fobjdicid_5f30x_5ft_1106',['OD_ObjDicId_30x_t',['../group__CO__ODinterface.html#gade8960f241ee3b728eac09288a694886',1,'CO_ODinterface.h']]], + ['od_5fobjecttypes_5ft_1107',['OD_objectTypes_t',['../group__CO__ODdefinition.html#gaae426e9d66ec1bacfef2d93f096d7805',1,'CO_ODinterface.h']]], + ['od_5freadoriginal_1108',['OD_readOriginal',['../group__CO__ODinterface.html#gadf9ac60f94e1f9fc21b7f10a0d254503',1,'OD_readOriginal(OD_stream_t *stream, uint8_t subIndex, void *buf, OD_size_t count, ODR_t *returnCode): CO_ODinterface.c'],['../group__CO__ODinterface.html#gadf9ac60f94e1f9fc21b7f10a0d254503',1,'OD_readOriginal(OD_stream_t *stream, uint8_t subIndex, void *buf, OD_size_t count, ODR_t *returnCode): CO_ODinterface.c']]], + ['od_5frwrestart_1109',['OD_rwRestart',['../group__CO__ODinterface.html#ga3715e0a6b15bdf45659e1e01f9fc4e65',1,'CO_ODinterface.h']]], + ['od_5fset_5fi16_1110',['OD_set_i16',['../group__CO__ODgetSetters.html#gaf207e188609f742be3c12f248f45138c',1,'OD_set_i16(const OD_entry_t *entry, uint8_t subIndex, int16_t val, bool_t odOrig): CO_ODinterface.c'],['../group__CO__ODgetSetters.html#gaf207e188609f742be3c12f248f45138c',1,'OD_set_i16(const OD_entry_t *entry, uint8_t subIndex, int16_t val, bool_t odOrig): CO_ODinterface.c']]], + ['od_5fset_5fi32_1111',['OD_set_i32',['../group__CO__ODgetSetters.html#ga101037295ea6af8488b499097f0d1ef1',1,'OD_set_i32(const OD_entry_t *entry, uint8_t subIndex, int32_t val, bool_t odOrig): CO_ODinterface.c'],['../group__CO__ODgetSetters.html#ga101037295ea6af8488b499097f0d1ef1',1,'OD_set_i32(const OD_entry_t *entry, uint8_t subIndex, int32_t val, bool_t odOrig): CO_ODinterface.c']]], + ['od_5fset_5fi64_1112',['OD_set_i64',['../group__CO__ODgetSetters.html#ga5c19e8e101d06a0203d13f8d3e997d16',1,'OD_set_i64(const OD_entry_t *entry, uint8_t subIndex, int64_t val, bool_t odOrig): CO_ODinterface.c'],['../group__CO__ODgetSetters.html#ga5c19e8e101d06a0203d13f8d3e997d16',1,'OD_set_i64(const OD_entry_t *entry, uint8_t subIndex, int64_t val, bool_t odOrig): CO_ODinterface.c']]], + ['od_5fset_5fi8_1113',['OD_set_i8',['../group__CO__ODgetSetters.html#ga46df632d54b48714cf50e4a3a92b4e98',1,'OD_set_i8(const OD_entry_t *entry, uint8_t subIndex, int8_t val, bool_t odOrig): CO_ODinterface.c'],['../group__CO__ODgetSetters.html#ga46df632d54b48714cf50e4a3a92b4e98',1,'OD_set_i8(const OD_entry_t *entry, uint8_t subIndex, int8_t val, bool_t odOrig): CO_ODinterface.c']]], + ['od_5fset_5fr32_1114',['OD_set_r32',['../group__CO__ODgetSetters.html#ga032219231c5e488969d02787265ad7ab',1,'OD_set_r32(const OD_entry_t *entry, uint8_t subIndex, float32_t val, bool_t odOrig): CO_ODinterface.c'],['../group__CO__ODgetSetters.html#ga032219231c5e488969d02787265ad7ab',1,'OD_set_r32(const OD_entry_t *entry, uint8_t subIndex, float32_t val, bool_t odOrig): CO_ODinterface.c']]], + ['od_5fset_5fr64_1115',['OD_set_r64',['../group__CO__ODgetSetters.html#ga75c5dd2daa6f0352bf079d18c9e90708',1,'OD_set_r64(const OD_entry_t *entry, uint8_t subIndex, float64_t val, bool_t odOrig): CO_ODinterface.c'],['../group__CO__ODgetSetters.html#ga75c5dd2daa6f0352bf079d18c9e90708',1,'OD_set_r64(const OD_entry_t *entry, uint8_t subIndex, float64_t val, bool_t odOrig): CO_ODinterface.c']]], + ['od_5fset_5fu16_1116',['OD_set_u16',['../group__CO__ODgetSetters.html#gac63795511b7decfbbc46ddb7c2b59cfd',1,'OD_set_u16(const OD_entry_t *entry, uint8_t subIndex, uint16_t val, bool_t odOrig): CO_ODinterface.c'],['../group__CO__ODgetSetters.html#gac63795511b7decfbbc46ddb7c2b59cfd',1,'OD_set_u16(const OD_entry_t *entry, uint8_t subIndex, uint16_t val, bool_t odOrig): CO_ODinterface.c']]], + ['od_5fset_5fu32_1117',['OD_set_u32',['../group__CO__ODgetSetters.html#ga5e5e943b89a2385f41a075131f47b5d5',1,'OD_set_u32(const OD_entry_t *entry, uint8_t subIndex, uint32_t val, bool_t odOrig): CO_ODinterface.c'],['../group__CO__ODgetSetters.html#ga5e5e943b89a2385f41a075131f47b5d5',1,'OD_set_u32(const OD_entry_t *entry, uint8_t subIndex, uint32_t val, bool_t odOrig): CO_ODinterface.c']]], + ['od_5fset_5fu64_1118',['OD_set_u64',['../group__CO__ODgetSetters.html#ga85570b44cc9d0af5b1084f42aeaf5e9f',1,'OD_set_u64(const OD_entry_t *entry, uint8_t subIndex, uint64_t val, bool_t odOrig): CO_ODinterface.c'],['../group__CO__ODgetSetters.html#ga85570b44cc9d0af5b1084f42aeaf5e9f',1,'OD_set_u64(const OD_entry_t *entry, uint8_t subIndex, uint64_t val, bool_t odOrig): CO_ODinterface.c']]], + ['od_5fset_5fu8_1119',['OD_set_u8',['../group__CO__ODgetSetters.html#ga3f56347af1f0f9b8bf46463777df87d0',1,'OD_set_u8(const OD_entry_t *entry, uint8_t subIndex, uint8_t val, bool_t odOrig): CO_ODinterface.c'],['../group__CO__ODgetSetters.html#ga3f56347af1f0f9b8bf46463777df87d0',1,'OD_set_u8(const OD_entry_t *entry, uint8_t subIndex, uint8_t val, bool_t odOrig): CO_ODinterface.c']]], + ['od_5fsize_5ft_1120',['OD_size_t',['../group__CO__ODinterface.html#gaef984c993ddbf6a0500391e97f05d08e',1,'CO_ODinterface.h']]], + ['od_5fstream_5ft_1121',['OD_stream_t',['../structOD__stream__t.html',1,'']]], + ['od_5fsubentry_5ft_1122',['OD_subEntry_t',['../structOD__subEntry__t.html',1,'']]], + ['od_5ft_1123',['OD_t',['../structOD__t.html',1,'']]], + ['od_5fvariable_1124',['OD_variable',['../structCO__trace__t.html#a2d71398a641edb4fe095aabb6e81f834',1,'CO_trace_t']]], + ['od_5fwriteoriginal_1125',['OD_writeOriginal',['../group__CO__ODinterface.html#ga648f0b0bfabde2d377149bf84e937422',1,'OD_writeOriginal(OD_stream_t *stream, uint8_t subIndex, const void *buf, OD_size_t count, ODR_t *returnCode): CO_ODinterface.c'],['../group__CO__ODinterface.html#ga648f0b0bfabde2d377149bf84e937422',1,'OD_writeOriginal(OD_stream_t *stream, uint8_t subIndex, const void *buf, OD_size_t count, ODR_t *returnCode): CO_ODinterface.c']]], + ['oda_5fmb_1126',['ODA_MB',['../group__CO__ODinterface.html#gga47b0d204aaf1ea64b4f826aaf8f5c151ae02b665e7e8d8bd84f341c9ad040d367',1,'CO_ODinterface.h']]], + ['oda_5frpdo_1127',['ODA_RPDO',['../group__CO__ODinterface.html#gga47b0d204aaf1ea64b4f826aaf8f5c151ae4930aa0efbc2249563613b5107bb107',1,'CO_ODinterface.h']]], + ['oda_5frsrdo_1128',['ODA_RSRDO',['../group__CO__ODinterface.html#gga47b0d204aaf1ea64b4f826aaf8f5c151a6c6261d5bea91588b2851f5c11faae02',1,'CO_ODinterface.h']]], + ['oda_5fsdo_5fr_1129',['ODA_SDO_R',['../group__CO__ODinterface.html#gga47b0d204aaf1ea64b4f826aaf8f5c151ada3eb985961ffdd9cf655d1a7a7d9485',1,'CO_ODinterface.h']]], + ['oda_5fsdo_5frw_1130',['ODA_SDO_RW',['../group__CO__ODinterface.html#gga47b0d204aaf1ea64b4f826aaf8f5c151a2c60ba85cbe4f25d5511ffab3dcd7486',1,'CO_ODinterface.h']]], + ['oda_5fsdo_5fw_1131',['ODA_SDO_W',['../group__CO__ODinterface.html#gga47b0d204aaf1ea64b4f826aaf8f5c151a9245e7a557f32ab863aef41412df9eb5',1,'CO_ODinterface.h']]], + ['oda_5fstr_1132',['ODA_STR',['../group__CO__ODinterface.html#gga47b0d204aaf1ea64b4f826aaf8f5c151a9527d9c21ba4159d653771e1dedad81d',1,'CO_ODinterface.h']]], + ['oda_5ftpdo_1133',['ODA_TPDO',['../group__CO__ODinterface.html#gga47b0d204aaf1ea64b4f826aaf8f5c151ad106aacb6b181ab7dac0f6dbc8c50321',1,'CO_ODinterface.h']]], + ['oda_5ftrpdo_1134',['ODA_TRPDO',['../group__CO__ODinterface.html#gga47b0d204aaf1ea64b4f826aaf8f5c151af750832681daa8d5e44ed8908c4ec552',1,'CO_ODinterface.h']]], + ['oda_5ftrsrdo_1135',['ODA_TRSRDO',['../group__CO__ODinterface.html#gga47b0d204aaf1ea64b4f826aaf8f5c151af887f38d83f35c28d478f1a4b08d1be9',1,'CO_ODinterface.h']]], + ['oda_5ftsrdo_1136',['ODA_TSRDO',['../group__CO__ODinterface.html#gga47b0d204aaf1ea64b4f826aaf8f5c151a3a1b8c2ed54565e89d6e6d3a043bdcfe',1,'CO_ODinterface.h']]], + ['odaddress_1137',['odAddress',['../structCO__OD__storage__t.html#ac434d6480330c026761fe8a82e32839b',1,'CO_OD_storage_t']]], + ['odobject_1138',['odObject',['../structOD__entry__t.html#a8667e5896b5b001270103e59c92bb181',1,'OD_entry_t']]], + ['odobjectoriginal_1139',['odObjectOriginal',['../structOD__obj__extended__t.html#abbc62d96e2ecafc99bb1eaf1210f816a',1,'OD_obj_extended_t']]], + ['odobjecttype_1140',['odObjectType',['../structOD__entry__t.html#a86a662107a24527872d92cdb13cad29b',1,'OD_entry_t']]], + ['odr_5fcount_1141',['ODR_COUNT',['../group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869cac43414be729ea2b701380c4400658c37',1,'CO_ODinterface.h']]], + ['odr_5fdata_5fdev_5fstate_1142',['ODR_DATA_DEV_STATE',['../group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869ca16bfe0c712aaea3841ae2b250331b276',1,'CO_ODinterface.h']]], + ['odr_5fdata_5floc_5fctrl_1143',['ODR_DATA_LOC_CTRL',['../group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869ca28b28d8f091eefb7e8fac92bbdae82bb',1,'CO_ODinterface.h']]], + ['odr_5fdata_5flong_1144',['ODR_DATA_LONG',['../group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869ca3f93f54b7f130bb7b266b2d36c24caec',1,'CO_ODinterface.h']]], + ['odr_5fdata_5fshort_1145',['ODR_DATA_SHORT',['../group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869cae0b103160b23ff40047fcf85225121d2',1,'CO_ODinterface.h']]], + ['odr_5fdata_5ftransf_1146',['ODR_DATA_TRANSF',['../group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869ca341104b9ac43168d0c668586b8f750bb',1,'CO_ODinterface.h']]], + ['odr_5fdev_5fincompat_1147',['ODR_DEV_INCOMPAT',['../group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869ca3d259ef030c2a10afecb253b532a0323',1,'CO_ODinterface.h']]], + ['odr_5fgeneral_1148',['ODR_GENERAL',['../group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869ca968d2a28cb866cacf7ef8b8cd0b76e2c',1,'CO_ODinterface.h']]], + ['odr_5fhw_1149',['ODR_HW',['../group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869caeaac26c680e626185429468dda9c2433',1,'CO_ODinterface.h']]], + ['odr_5fidx_5fnot_5fexist_1150',['ODR_IDX_NOT_EXIST',['../group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869ca64ea80d1baebf136382d53d5580fbc85',1,'CO_ODinterface.h']]], + ['odr_5finvalid_5fvalue_1151',['ODR_INVALID_VALUE',['../group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869ca0646f592124a9b4d2835c9c0296c6a0c',1,'CO_ODinterface.h']]], + ['odr_5fmap_5flen_1152',['ODR_MAP_LEN',['../group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869cad6c92203fa86ee8ff7d7271bf81e7d9e',1,'CO_ODinterface.h']]], + ['odr_5fmax_5fless_5fmin_1153',['ODR_MAX_LESS_MIN',['../group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869cac9c8eda20bfc7dfffe17f170c377d646',1,'CO_ODinterface.h']]], + ['odr_5fno_5fdata_1154',['ODR_NO_DATA',['../group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869caff503d4f6cc55429913680d071ca3c4d',1,'CO_ODinterface.h']]], + ['odr_5fno_5fmap_1155',['ODR_NO_MAP',['../group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869ca853242f74446c58773099bdef9835a94',1,'CO_ODinterface.h']]], + ['odr_5fno_5fresource_1156',['ODR_NO_RESOURCE',['../group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869ca15fe8d8b791b90373e59bc5bc5d3f8c8',1,'CO_ODinterface.h']]], + ['odr_5fod_5fmissing_1157',['ODR_OD_MISSING',['../group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869caeae1e00129ff22708ffdcb2c8b3f083b',1,'CO_ODinterface.h']]], + ['odr_5fok_1158',['ODR_OK',['../group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869ca15f7f20e27f1c5f174bdeecfeef45cc2',1,'CO_ODinterface.h']]], + ['odr_5fout_5fof_5fmem_1159',['ODR_OUT_OF_MEM',['../group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869ca118a63a81ef2fd802c925bf4c79975fa',1,'CO_ODinterface.h']]], + ['odr_5fpar_5fincompat_1160',['ODR_PAR_INCOMPAT',['../group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869ca36edb0ad1c8c5c0d804bb88274bfe165',1,'CO_ODinterface.h']]], + ['odr_5fpartial_1161',['ODR_PARTIAL',['../group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869caf7595749473065bfc81cfa6709370fee',1,'CO_ODinterface.h']]], + ['odr_5freadonly_1162',['ODR_READONLY',['../group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869caa119384effe499c9bffb874219c6433a',1,'CO_ODinterface.h']]], + ['odr_5fsub_5fnot_5fexist_1163',['ODR_SUB_NOT_EXIST',['../group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869ca4f76fa87ea446616ff2f6195e7bec67c',1,'CO_ODinterface.h']]], + ['odr_5ft_1164',['ODR_t',['../group__CO__ODinterface.html#ga0e9afd8ad27de0920d1fe0738834869c',1,'CO_ODinterface.h']]], + ['odr_5ftype_5fmismatch_1165',['ODR_TYPE_MISMATCH',['../group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869cad197c1462f472a21be2e3ed5c5880aa4',1,'CO_ODinterface.h']]], + ['odr_5funsupp_5faccess_1166',['ODR_UNSUPP_ACCESS',['../group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869ca17d694adc9471112cbb2740f7f45a2d0',1,'CO_ODinterface.h']]], + ['odr_5fvalue_5fhigh_1167',['ODR_VALUE_HIGH',['../group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869ca95291309267a732c380e13ad6c17a986',1,'CO_ODinterface.h']]], + ['odr_5fvalue_5flow_1168',['ODR_VALUE_LOW',['../group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869ca4db02b8575a8a10786959a5472f1c0f4',1,'CO_ODinterface.h']]], + ['odr_5fwriteonly_1169',['ODR_WRITEONLY',['../group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869ca44ab94bfc7547122b96498c781291df6',1,'CO_ODinterface.h']]], + ['odsize_1170',['odSize',['../structCO__OD__storage__t.html#aefd1cb33fa031c1592b20643bb38bfbb',1,'CO_OD_storage_t']]], + ['odt_5farr_1171',['ODT_ARR',['../group__CO__ODdefinition.html#ggaae426e9d66ec1bacfef2d93f096d7805a1ad5763beafe79c42ca223297c832ff4',1,'CO_ODinterface.h']]], + ['odt_5fearr_1172',['ODT_EARR',['../group__CO__ODdefinition.html#ggaae426e9d66ec1bacfef2d93f096d7805a1ae954b4709b24d93bdcac69957c8e40',1,'CO_ODinterface.h']]], + ['odt_5ferec_1173',['ODT_EREC',['../group__CO__ODdefinition.html#ggaae426e9d66ec1bacfef2d93f096d7805ae176b06a942e47815d2e4c51a8f9b7f8',1,'CO_ODinterface.h']]], + ['odt_5fevar_1174',['ODT_EVAR',['../group__CO__ODdefinition.html#ggaae426e9d66ec1bacfef2d93f096d7805adb58a7faa735918d39b8bbcd3a6ad594',1,'CO_ODinterface.h']]], + ['odt_5fextension_5fmask_1175',['ODT_EXTENSION_MASK',['../group__CO__ODdefinition.html#ggaae426e9d66ec1bacfef2d93f096d7805a3dd8bc41ec11c475d487b877fdd0a879',1,'CO_ODinterface.h']]], + ['odt_5frec_1176',['ODT_REC',['../group__CO__ODdefinition.html#ggaae426e9d66ec1bacfef2d93f096d7805a9376cface357f03bec8a651a307f33b9',1,'CO_ODinterface.h']]], + ['odt_5ftype_5fmask_1177',['ODT_TYPE_MASK',['../group__CO__ODdefinition.html#ggaae426e9d66ec1bacfef2d93f096d7805ac9e24bde0d37c35c3065dbaa541e1acb',1,'CO_ODinterface.h']]], + ['odt_5fvar_1178',['ODT_VAR',['../group__CO__ODdefinition.html#ggaae426e9d66ec1bacfef2d93f096d7805a829f1df882410efc0ea0e05b3435c820',1,'CO_ODinterface.h']]], + ['operatingstate_1179',['operatingState',['../structCO__NMT__t.html#abebf973c819c48fa36c48ba2eb84818a',1,'CO_NMT_t::operatingState()'],['../structCO__RPDO__t.html#a85583dccb8f2d0515288888e56065e70',1,'CO_RPDO_t::operatingState()'],['../structCO__TPDO__t.html#a7ba73f70490869a38ff561aa6c32489f',1,'CO_TPDO_t::operatingState()'],['../structCO__SYNC__t.html#a70fea8996ebfbe7d163bae11201dac6e',1,'CO_SYNC_t::operatingState()'],['../structCO__TIME__t.html#a2afb9ad74a04332b644669c7fdb187ac',1,'CO_TIME_t::operatingState()'],['../structCO__SRDOGuard__t.html#a5e80afb9ddd0debd94eaafa9c6f4ad36',1,'CO_SRDOGuard_t::operatingState()']]], + ['operatingstateprev_1180',['operatingStatePrev',['../structCO__NMT__t.html#a544df8108d0853dd6d2938fa253ef018',1,'CO_NMT_t::operatingStatePrev()'],['../structCO__SRDOGuard__t.html#ac6f6a3b1465360e035656933470d6b4d',1,'CO_SRDOGuard_t::operatingStatePrev()']]] +]; diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/all_d.html b/Devices/Libraries/Systems/CANopenSocket/docs/search/all_d.html new file mode 100755 index 0000000..bc376fe --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/all_d.html @@ -0,0 +1,30 @@ + + + + + + + + + +
+
Loading...
+
+ +
Searching...
+
No Matches
+ +
+ + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/all_d.js b/Devices/Libraries/Systems/CANopenSocket/docs/search/all_d.js new file mode 100755 index 0000000..7ad95e2 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/all_d.js @@ -0,0 +1,29 @@ +var searchData= +[ + ['pdo_1181',['PDO',['../group__CO__PDO.html',1,'']]], + ['pcanrx_5fcallback_1182',['pCANrx_callback',['../structCO__CANrx__t.html#a8e4668eec8326bb9ac08d67afc3060c7',1,'CO_CANrx_t']]], + ['pendingbitrate_1183',['pendingBitRate',['../structCO__LSSslave__t.html#a90b0fabc76e788404da520a7a5e90e48',1,'CO_LSSslave_t']]], + ['pendingnodeid_1184',['pendingNodeID',['../structCO__LSSslave__t.html#a9a632a4655fb5adfd91bd3e26ed43f0b',1,'CO_LSSslave_t']]], + ['periodtime_1185',['periodTime',['../structCO__SYNC__t.html#a15337541e9b3defa2dbae2df3dd01e72',1,'CO_SYNC_t::periodTime()'],['../structCO__TIME__t.html#a7c0090cb2e1e3af1fab308640c3d25a5',1,'CO_TIME_t::periodTime()']]], + ['periodtimeouttime_1186',['periodTimeoutTime',['../structCO__SYNC__t.html#a9f612a7f9d691edeef11d8df9cf166a7',1,'CO_SYNC_t::periodTimeoutTime()'],['../structCO__TIME__t.html#a9f2d24892e5b318f5eea5844f3b29a4a',1,'CO_TIME_t::periodTimeoutTime()']]], + ['pfunctlssactivatebitrate_1187',['pFunctLSSactivateBitRate',['../structCO__LSSslave__t.html#a8c2bfab9e6225cd65b48149333d9e1dd',1,'CO_LSSslave_t']]], + ['pfunctlsscfgstore_1188',['pFunctLSScfgStore',['../structCO__LSSslave__t.html#a53b9620f867ad5f534c99955a2479f16',1,'CO_LSSslave_t']]], + ['pfunctlsscheckbitrate_1189',['pFunctLSScheckBitRate',['../structCO__LSSslave__t.html#a1482f4cef4b3ac4afbdf569306e32aab',1,'CO_LSSslave_t']]], + ['pfunctnmt_1190',['pFunctNMT',['../structCO__NMT__t.html#a64bc9d74c871bf560d8b5db85e4d051e',1,'CO_NMT_t']]], + ['pfunctsignal_1191',['pFunctSignal',['../structCO__SDOclient__t.html#ae7bbefdb0854addfa2ae45dcfc39d738',1,'CO_SDOclient_t::pFunctSignal()'],['../structCO__LSSmaster__t.html#a9b8376cf89bba5a3650f82da61ffee5b',1,'CO_LSSmaster_t::pFunctSignal()']]], + ['pfunctsignalhbstarted_1192',['pFunctSignalHbStarted',['../structCO__HBconsNode__t.html#ac44d2a232ca2b352fd717ee8e6f28e90',1,'CO_HBconsNode_t']]], + ['pfunctsignalnmtchanged_1193',['pFunctSignalNmtChanged',['../structCO__HBconsNode__t.html#a3f4ec7dfe9e47a3e7a9bdad382fd7f56',1,'CO_HBconsNode_t::pFunctSignalNmtChanged()'],['../structCO__HBconsumer__t.html#a58314f7d35efc2dbeae14ba4be76dec1',1,'CO_HBconsumer_t::pFunctSignalNmtChanged()']]], + ['pfunctsignalobjectnmtchanged_1194',['pFunctSignalObjectNmtChanged',['../structCO__HBconsNode__t.html#a2a1c5abd88c2ecd451e2a1ca65d3dc93',1,'CO_HBconsNode_t::pFunctSignalObjectNmtChanged()'],['../structCO__HBconsumer__t.html#ad583c93f4e59f98669cd18f263aee45a',1,'CO_HBconsumer_t::pFunctSignalObjectNmtChanged()']]], + ['pfunctsignalpre_1195',['pFunctSignalPre',['../structCO__EM__t.html#a124a5d8fb51bb600618a9427b14663c4',1,'CO_EM_t::pFunctSignalPre()'],['../structCO__HBconsNode__t.html#a61c753c38666dda8a4d4d870fc593ae5',1,'CO_HBconsNode_t::pFunctSignalPre()'],['../structCO__NMT__t.html#a31b37e4e64737bb3ef4331d607adbed6',1,'CO_NMT_t::pFunctSignalPre()'],['../structCO__RPDO__t.html#a3c0573cc4e76a601bbbd710f19b9615f',1,'CO_RPDO_t::pFunctSignalPre()'],['../structCO__SDOserver__t.html#ace57036f1dfd1ffe35f4b13e50fc4a41',1,'CO_SDOserver_t::pFunctSignalPre()'],['../structCO__SYNC__t.html#ac56a3411d0cd312e51d905937c628b6f',1,'CO_SYNC_t::pFunctSignalPre()'],['../structCO__TIME__t.html#aed7685145a3b3cfb80e8865aa3e8df6d',1,'CO_TIME_t::pFunctSignalPre()'],['../structCO__SRDO__t.html#a5c9c5a746066607b6052c66bcf07190e',1,'CO_SRDO_t::pFunctSignalPre()'],['../structCO__LSSslave__t.html#a9a6db85a8e5a14652e3b1fb7026790a1',1,'CO_LSSslave_t::pFunctSignalPre()']]], + ['pfunctsignalremotereset_1196',['pFunctSignalRemoteReset',['../structCO__HBconsNode__t.html#a7f7ccf80c31d4c764db24b70f9111e7b',1,'CO_HBconsNode_t']]], + ['pfunctsignalrx_1197',['pFunctSignalRx',['../structCO__EM__t.html#a71ba138e5c1814446c210ac7d7f2aa02',1,'CO_EM_t']]], + ['pfunctsignalsafe_1198',['pFunctSignalSafe',['../structCO__GFC__t.html#af7ffb43e3b2a682941404bc5c23512e5',1,'CO_GFC_t::pFunctSignalSafe()'],['../structCO__SRDO__t.html#ab000fd951d5b72615a3b3086335a6add',1,'CO_SRDO_t::pFunctSignalSafe()']]], + ['pfunctsignaltimeout_1199',['pFunctSignalTimeout',['../structCO__HBconsNode__t.html#ab6bbeee344ebd1d657bbabc02f51e597',1,'CO_HBconsNode_t']]], + ['pgetvalue_1200',['pGetValue',['../structCO__trace__dataType__t.html#a8ac5cd02c39b591354149f8c5d6357f7',1,'CO_trace_dataType_t']]], + ['previoustime_5fus_1201',['previousTime_us',['../structCO__epoll__t.html#aa047c5d68bb87515a1089cf5c75d847f',1,'CO_epoll_t']]], + ['printpoint_1202',['printPoint',['../structCO__trace__dataType__t.html#aef2341fd443f3aa33a11db1b429b7dc1',1,'CO_trace_dataType_t']]], + ['printpointend_1203',['printPointEnd',['../structCO__trace__dataType__t.html#a48ec81e33885a9114f2ea0be237a0059',1,'CO_trace_dataType_t']]], + ['printpointstart_1204',['printPointStart',['../structCO__trace__dataType__t.html#a751d9b94aba8f663de5b5f7c7f074893',1,'CO_trace_dataType_t']]], + ['producercanid_1205',['producerCanId',['../structCO__EM__t.html#a490ef24607a0b637d9bc3a60f616b41e',1,'CO_EM_t']]], + ['producerenabled_1206',['producerEnabled',['../structCO__EM__t.html#ad54685d2f0e7ac934edbe0cf5c0b4baf',1,'CO_EM_t']]] +]; diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/all_e.html b/Devices/Libraries/Systems/CANopenSocket/docs/search/all_e.html new file mode 100755 index 0000000..2e3c74d --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/all_e.html @@ -0,0 +1,30 @@ + + + + + + + + + +
+
Loading...
+
+ +
Searching...
+
No Matches
+ +
+ + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/all_e.js b/Devices/Libraries/Systems/CANopenSocket/docs/search/all_e.js new file mode 100755 index 0000000..703ac18 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/all_e.js @@ -0,0 +1,31 @@ +var searchData= +[ + ['reception_20of_20can_20messages_1207',['Reception of CAN messages',['../group__CO__CAN__Message__reception.html',1,'']]], + ['read_1208',['read',['../structOD__IO__t.html#ab0ed3186d15d20f80f877a5f087fdebc',1,'OD_IO_t::read()'],['../structOD__extensionIO__t.html#af4c210e173adb94e297ad26eacb2b678',1,'OD_extensionIO_t::read()']]], + ['readcallback_1209',['readCallback',['../structCO__GTWA__t.html#a036c4a3a89b8171baba8039fc50876ba',1,'CO_GTWA_t']]], + ['readcallbackobject_1210',['readCallbackObject',['../structCO__GTWA__t.html#ae1b9a86d7020ac21713a9b658a08495b',1,'CO_GTWA_t']]], + ['readptr_1211',['readPtr',['../structCO__fifo__t.html#a4b56b7d217aa2b02eaf1b3592c5c26e6',1,'CO_fifo_t::readPtr()'],['../structCO__trace__t.html#af439eee35bf45f6ec14a32270577ff85',1,'CO_trace_t::readPtr()']]], + ['receiveerror_1212',['receiveError',['../structCO__SYNC__t.html#ae108ac75f1fb7c797393646cc6c494af',1,'CO_SYNC_t::receiveError()'],['../structCO__TIME__t.html#abbff3c5ce159992f4b45ec042849cbc0',1,'CO_TIME_t::receiveError()']]], + ['respbuf_1213',['respBuf',['../structCO__GTWA__t.html#a987d9431f47a10272cf9c81b0d0159d1',1,'CO_GTWA_t']]], + ['respbufcount_1214',['respBufCount',['../structCO__GTWA__t.html#a67770af170976d4d904fbc044d347376',1,'CO_GTWA_t']]], + ['respbufoffset_1215',['respBufOffset',['../structCO__GTWA__t.html#a605bfa0c99f4a0235980de0603a050ca',1,'CO_GTWA_t']]], + ['resphold_1216',['respHold',['../structCO__GTWA__t.html#aef556bb4c595944ebf3de22a2c9d5007',1,'CO_GTWA_t']]], + ['restrictionflags_1217',['restrictionFlags',['../structCO__RPDO__t.html#a1759ebaef816a352d37e717c9360458a',1,'CO_RPDO_t::restrictionFlags()'],['../structCO__TPDO__t.html#aa0d1d4b71933c7bac438d97ca280fe56',1,'CO_TPDO_t::restrictionFlags()']]], + ['rpdo_1218',['RPDO',['../structCO__t.html#a332fa4512032d94a20af69b5a103e017',1,'CO_t']]], + ['rpdocommpar_1219',['RPDOCommPar',['../structCO__RPDO__t.html#a1aaaf9abb01030dbd732985d07ed8c33',1,'CO_RPDO_t']]], + ['rpdomappar_1220',['RPDOMapPar',['../structCO__RPDO__t.html#a69ad9068dfcee1d12697430856e62d10',1,'CO_RPDO_t']]], + ['rx_5fidx_5fem_5fcons_1221',['RX_IDX_EM_CONS',['../structCO__t.html#a47df99673e8da6e48c6c17cbf204c16d',1,'CO_t']]], + ['rx_5fidx_5fgfc_1222',['RX_IDX_GFC',['../structCO__t.html#ae689666dc05116ce0694d5ea505a2e79',1,'CO_t']]], + ['rx_5fidx_5fhb_5fcons_1223',['RX_IDX_HB_CONS',['../structCO__t.html#a788f815f4d31a4aa11d171114173573e',1,'CO_t']]], + ['rx_5fidx_5flss_5fmst_1224',['RX_IDX_LSS_MST',['../structCO__t.html#aa0c0c6b387e406c67d3b988a744b3b36',1,'CO_t']]], + ['rx_5fidx_5flss_5fslv_1225',['RX_IDX_LSS_SLV',['../structCO__t.html#a2c230c531fc2a2a09ad33edef582041e',1,'CO_t']]], + ['rx_5fidx_5fnmt_5fslv_1226',['RX_IDX_NMT_SLV',['../structCO__t.html#af693786fa6c0923d4c934d09a00820d3',1,'CO_t']]], + ['rx_5fidx_5frpdo_1227',['RX_IDX_RPDO',['../structCO__t.html#a7a01e7792c600f5dc1eb2e7676e1e01d',1,'CO_t']]], + ['rx_5fidx_5fsdo_5fcli_1228',['RX_IDX_SDO_CLI',['../structCO__t.html#a85a2007d87b4fb3608859e251cf8653e',1,'CO_t']]], + ['rx_5fidx_5fsdo_5fsrv_1229',['RX_IDX_SDO_SRV',['../structCO__t.html#a8e63b9c217cf4cd7fc8fbc0900bdb01c',1,'CO_t']]], + ['rx_5fidx_5fsrdo_1230',['RX_IDX_SRDO',['../structCO__t.html#a8fec2730008c19f6fbc6192937776739',1,'CO_t']]], + ['rx_5fidx_5fsync_1231',['RX_IDX_SYNC',['../structCO__t.html#a72a378c1e806978f8399bf3f3af7c002',1,'CO_t']]], + ['rx_5fidx_5ftime_1232',['RX_IDX_TIME',['../structCO__t.html#a039a63c4b10f91814775f9d1cef43aad',1,'CO_t']]], + ['rxarray_1233',['rxArray',['../structCO__CANmodule__t.html#a747e694e15cca5a5579c2c4397a6da39',1,'CO_CANmodule_t']]], + ['rxsize_1234',['rxSize',['../structCO__CANmodule__t.html#a88edfd32c2bff5b9f29a2bccd1ac96f3',1,'CO_CANmodule_t']]] +]; diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/all_f.html b/Devices/Libraries/Systems/CANopenSocket/docs/search/all_f.html new file mode 100755 index 0000000..246f8ab --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/all_f.html @@ -0,0 +1,30 @@ + + + + + + + + + +
+
Loading...
+
+ +
Searching...
+
No Matches
+ +
+ + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/all_f.js b/Devices/Libraries/Systems/CANopenSocket/docs/search/all_f.js new file mode 100755 index 0000000..b877fd1 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/all_f.js @@ -0,0 +1,49 @@ +var searchData= +[ + ['sdo_20client_1235',['SDO client',['../group__CO__SDOclient.html',1,'']]], + ['sdo_20server_1236',['SDO server',['../group__CO__SDOserver.html',1,'']]], + ['socketcan_1237',['socketCAN',['../group__CO__socketCAN.html',1,'']]], + ['srdo_1238',['SRDO',['../group__CO__SRDO.html',1,'']]], + ['stack_20configuration_1239',['Stack configuration',['../group__CO__STACK__CONFIG.html',1,'']]], + ['sdo_20server_2fclient_1240',['SDO server/client',['../group__CO__STACK__CONFIG__SDO.html',1,'']]], + ['safety_20related_20data_20objects_20_28srdo_29_1241',['Safety Related Data Objects (SRDO)',['../group__CO__STACK__CONFIG__SRDO.html',1,'']]], + ['sync_20and_20pdo_20producer_2fconsumer_1242',['SYNC and PDO producer/consumer',['../group__CO__STACK__CONFIG__SYNC__PDO.html',1,'']]], + ['sync_1243',['SYNC',['../group__CO__SYNC.html',1,'']]], + ['safetycycletime_1244',['safetyCycleTime',['../structCO__SRDOCommPar__t.html#a257a5534889060efc7477be7fc0adbad',1,'CO_SRDOCommPar_t']]], + ['safetyrelatedvalidationtime_1245',['safetyRelatedValidationTime',['../structCO__SRDOCommPar__t.html#a5c02007d0e4f1b1cfd8a07da93b954cb',1,'CO_SRDOCommPar_t']]], + ['scan_1246',['scan',['../structCO__LSSmaster__fastscan__t.html#a42853c0091c96d7fc7e763c2be3b6e8a',1,'CO_LSSmaster_fastscan_t']]], + ['sdo_1247',['SDO',['../structCO__RPDO__t.html#abcf8134da148073ec8e3f1dd6f0c8da1',1,'CO_RPDO_t::SDO()'],['../structCO__TPDO__t.html#a52c1e56f282549b0949721049bcc7e73',1,'CO_TPDO_t::SDO()'],['../structCO__SRDO__t.html#af8121f4d1f0879eba793afaee5c6c804',1,'CO_SRDO_t::SDO()'],['../structCO__trace__t.html#a33616f410feb93ddb6a2ac238027996b',1,'CO_trace_t::SDO()']]], + ['sdo_5fc_1248',['SDO_C',['../structCO__GTWA__t.html#a274945dbaacfd975f86a58566a884769',1,'CO_GTWA_t']]], + ['sdoblocktransferenable_1249',['SDOblockTransferEnable',['../structCO__GTWA__t.html#a0b05c1b89fe8b104b019d12679d4edcf',1,'CO_GTWA_t']]], + ['sdoclient_1250',['SDOclient',['../structCO__t.html#ae9548e1a85b750b039dcc12bd43bd972',1,'CO_t']]], + ['sdodatacopystatus_1251',['SDOdataCopyStatus',['../structCO__GTWA__t.html#aae981f9a446cfaad8a5450e77adc32ec',1,'CO_GTWA_t']]], + ['sdodatatype_1252',['SDOdataType',['../structCO__GTWA__t.html#a7b9bcb2113286454a273b9b43f4e1548',1,'CO_GTWA_t']]], + ['sdoserver_1253',['SDOserver',['../structCO__t.html#aa9fe002954a6b902e594fc47a9f1917b',1,'CO_t']]], + ['sdotimeouttime_1254',['SDOtimeoutTime',['../structCO__GTWA__t.html#a9fa3fe5ce1806296cf36ef5d8c3ecae2',1,'CO_GTWA_t']]], + ['sdotimeouttime_5fus_1255',['SDOtimeoutTime_us',['../structCO__SDOclient__t.html#ab2f8be2e90734e78f4e78bdabbb13488',1,'CO_SDOclient_t::SDOtimeoutTime_us()'],['../structCO__SDOserver__t.html#a09381f3b6885d0a3bc7d1fa96805eb5d',1,'CO_SDOserver_t::SDOtimeoutTime_us()']]], + ['sendifcosflags_1256',['sendIfCOSFlags',['../structCO__TPDO__t.html#a0f5dbb51df08261c466b079c08119a04',1,'CO_TPDO_t']]], + ['sendrequest_1257',['sendRequest',['../structCO__TPDO__t.html#afc375e06e5931cb8bae7886f2f5f0f7a',1,'CO_TPDO_t']]], + ['sendresponse_1258',['sendResponse',['../structCO__LSSslave__t.html#af9fb29812cd548f34223cc6d16de8121',1,'CO_LSSslave_t']]], + ['sequence_1259',['sequence',['../structCO__GTWA__t.html#a31b7ae3a5da107dfb5432f5a95f9faee',1,'CO_GTWA_t']]], + ['service_1260',['service',['../structCO__LSSslave__t.html#acdffe4100d9031c885ba5b5b995e471c',1,'CO_LSSslave_t']]], + ['size_1261',['size',['../structOD__t.html#ab52b946b5f0127fe618c6f5bbe65a698',1,'OD_t']]], + ['sizeind_1262',['sizeInd',['../structCO__SDOclient__t.html#a1b9dc211b9b90dac825097757f985583',1,'CO_SDOclient_t::sizeInd()'],['../structCO__SDOserver__t.html#a95be5bcb7257f818a294c9b03b0d1386',1,'CO_SDOserver_t::sizeInd()']]], + ['sizetran_1263',['sizeTran',['../structCO__SDOclient__t.html#aa7a9ddc5dbf035644cf59857dcfa83db',1,'CO_SDOclient_t::sizeTran()'],['../structCO__SDOserver__t.html#a86eaba6efabaa335b769924edd80f1ae',1,'CO_SDOserver_t::sizeTran()']]], + ['sockettimeout_5fus_1264',['socketTimeout_us',['../structCO__epoll__gtw__t.html#a876c2b8ae6e9585155c5cf035c876c91',1,'CO_epoll_gtw_t']]], + ['sockettimeouttmr_5fus_1265',['socketTimeoutTmr_us',['../structCO__epoll__gtw__t.html#a94ff0ff20f321b749202d9f3aa92a66d',1,'CO_epoll_gtw_t']]], + ['srdo_1266',['SRDO',['../structCO__t.html#a2a65c843c89da67fc049aa285e03243c',1,'CO_t']]], + ['srdocommpar_1267',['SRDOCommPar',['../structCO__SRDO__t.html#aba2d09de18da68fb01ab23696de452ac',1,'CO_SRDO_t']]], + ['srdoguard_1268',['SRDOGuard',['../structCO__t.html#a007c96db5b54e91145571feb06e9e683',1,'CO_t::SRDOGuard()'],['../structCO__SRDO__t.html#ab96c524d0b0243527a6b2d3ee89de302',1,'CO_SRDO_t::SRDOGuard()']]], + ['srdomappar_1269',['SRDOMapPar',['../structCO__SRDO__t.html#a876184e5c0b2809fc4b412cc2accb43e',1,'CO_SRDO_t']]], + ['started_1270',['started',['../structCO__fifo__t.html#ab754d03636ff0ef17950d4167429a77f',1,'CO_fifo_t']]], + ['state_1271',['state',['../structCO__SDOclient__t.html#a511a3981c4664be192b19714b95995c0',1,'CO_SDOclient_t::state()'],['../structCO__SDOserver__t.html#a0b457d35679acb51d11d21cd9f3660fd',1,'CO_SDOserver_t::state()'],['../structCO__LSSmaster__t.html#a1fb90a878809a6690dd4493444795e30',1,'CO_LSSmaster_t::state()'],['../structCO__GTWA__t.html#a5c37389f4a985950708e2fed036daf1f',1,'CO_GTWA_t::state()']]], + ['statetimeouttmr_1272',['stateTimeoutTmr',['../structCO__GTWA__t.html#ac5fc932142ed17d04393ae30819fb021',1,'CO_GTWA_t']]], + ['stream_1273',['stream',['../structOD__IO__t.html#a7f31cae6c859c2a2eaf2610b65fd6626',1,'OD_IO_t']]], + ['subentriescount_1274',['subEntriesCount',['../structOD__subEntry__t.html#a49750c447be0b3773e8aa1814d5fa2de',1,'OD_subEntry_t::subEntriesCount()'],['../structOD__entry__t.html#a2f8698a482c2994602a24d39d924acc6',1,'OD_entry_t::subEntriesCount()']]], + ['subindex_1275',['subIndex',['../structOD__subEntry__t.html#a2c39cbf86fc3b384b017ed6261d379bd',1,'OD_subEntry_t::subIndex()'],['../structOD__obj__record__t.html#a7cfa0012ae3aec7bbfc98e5e8301782c',1,'OD_obj_record_t::subIndex()'],['../structCO__SDOclient__t.html#a7f36981af65dc318dd69213c408b1b62',1,'CO_SDOclient_t::subIndex()'],['../structCO__SDOserver__t.html#a5051d5aeaa97e5d40ccfb39461706f10',1,'CO_SDOserver_t::subIndex()']]], + ['sync_1276',['SYNC',['../structCO__t.html#a9c704cab995029fd216076523c70d283',1,'CO_t::SYNC()'],['../structCO__RPDO__t.html#a2545c12748b54a1ef3e5660c82e1adf8',1,'CO_RPDO_t::SYNC()'],['../structCO__TPDO__t.html#ae6c014bdb855d57d4c53bc702734b51f',1,'CO_TPDO_t::SYNC()']]], + ['synccounter_1277',['syncCounter',['../structCO__TPDO__t.html#a5bf0fa473c5eb92e3b56fc1f17222976',1,'CO_TPDO_t']]], + ['syncflag_1278',['syncFlag',['../structCO__CANtx__t.html#a79c19597a51351b9d6ed1c9bdfd051b3',1,'CO_CANtx_t']]], + ['synchronous_1279',['synchronous',['../structCO__RPDO__t.html#a08c58c120349e150e4188666a5113916',1,'CO_RPDO_t']]], + ['syncstartvalue_1280',['SYNCStartValue',['../structCO__TPDOCommPar__t.html#a597bd93f097550c3d869307e733cd198',1,'CO_TPDOCommPar_t']]] +]; diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/classes_0.html b/Devices/Libraries/Systems/CANopenSocket/docs/search/classes_0.html new file mode 100755 index 0000000..f7e4c14 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/classes_0.html @@ -0,0 +1,30 @@ + + + + + + + + + +
+
Loading...
+
+ +
Searching...
+
No Matches
+ +
+ + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/classes_0.js b/Devices/Libraries/Systems/CANopenSocket/docs/search/classes_0.js new file mode 100755 index 0000000..084f0af --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/classes_0.js @@ -0,0 +1,39 @@ +var searchData= +[ + ['co_5fcaninterfaceerrorhandler_5ft_1338',['CO_CANinterfaceErrorhandler_t',['../structCO__CANinterfaceErrorhandler__t.html',1,'']]], + ['co_5fcanmodule_5ft_1339',['CO_CANmodule_t',['../structCO__CANmodule__t.html',1,'']]], + ['co_5fcanrx_5ft_1340',['CO_CANrx_t',['../structCO__CANrx__t.html',1,'']]], + ['co_5fcantx_5ft_1341',['CO_CANtx_t',['../structCO__CANtx__t.html',1,'']]], + ['co_5fconfig_5ft_1342',['CO_config_t',['../structCO__config__t.html',1,'']]], + ['co_5fem_5ft_1343',['CO_EM_t',['../structCO__EM__t.html',1,'']]], + ['co_5fepoll_5fgtw_5ft_1344',['CO_epoll_gtw_t',['../structCO__epoll__gtw__t.html',1,'']]], + ['co_5fepoll_5ft_1345',['CO_epoll_t',['../structCO__epoll__t.html',1,'']]], + ['co_5ffifo_5ft_1346',['CO_fifo_t',['../structCO__fifo__t.html',1,'']]], + ['co_5fgfc_5ft_1347',['CO_GFC_t',['../structCO__GFC__t.html',1,'']]], + ['co_5fgtwa_5ft_1348',['CO_GTWA_t',['../structCO__GTWA__t.html',1,'']]], + ['co_5fhbconsnode_5ft_1349',['CO_HBconsNode_t',['../structCO__HBconsNode__t.html',1,'']]], + ['co_5fhbconsumer_5ft_1350',['CO_HBconsumer_t',['../structCO__HBconsumer__t.html',1,'']]], + ['co_5fleds_5ft_1351',['CO_LEDs_t',['../structCO__LEDs__t.html',1,'']]], + ['co_5flss_5faddress_5ft_1352',['CO_LSS_address_t',['../unionCO__LSS__address__t.html',1,'']]], + ['co_5flssmaster_5ffastscan_5ft_1353',['CO_LSSmaster_fastscan_t',['../structCO__LSSmaster__fastscan__t.html',1,'']]], + ['co_5flssmaster_5ft_1354',['CO_LSSmaster_t',['../structCO__LSSmaster__t.html',1,'']]], + ['co_5flssslave_5ft_1355',['CO_LSSslave_t',['../structCO__LSSslave__t.html',1,'']]], + ['co_5fnmt_5ft_1356',['CO_NMT_t',['../structCO__NMT__t.html',1,'']]], + ['co_5fod_5fstorage_5ft_1357',['CO_OD_storage_t',['../structCO__OD__storage__t.html',1,'']]], + ['co_5frpdo_5ft_1358',['CO_RPDO_t',['../structCO__RPDO__t.html',1,'']]], + ['co_5frpdocommpar_5ft_1359',['CO_RPDOCommPar_t',['../structCO__RPDOCommPar__t.html',1,'']]], + ['co_5frpdomappar_5ft_1360',['CO_RPDOMapPar_t',['../structCO__RPDOMapPar__t.html',1,'']]], + ['co_5fsdoclient_5ft_1361',['CO_SDOclient_t',['../structCO__SDOclient__t.html',1,'']]], + ['co_5fsdoserver_5ft_1362',['CO_SDOserver_t',['../structCO__SDOserver__t.html',1,'']]], + ['co_5fsrdo_5ft_1363',['CO_SRDO_t',['../structCO__SRDO__t.html',1,'']]], + ['co_5fsrdocommpar_5ft_1364',['CO_SRDOCommPar_t',['../structCO__SRDOCommPar__t.html',1,'']]], + ['co_5fsrdoguard_5ft_1365',['CO_SRDOGuard_t',['../structCO__SRDOGuard__t.html',1,'']]], + ['co_5fsync_5ft_1366',['CO_SYNC_t',['../structCO__SYNC__t.html',1,'']]], + ['co_5ft_1367',['CO_t',['../structCO__t.html',1,'']]], + ['co_5ftime_5ft_1368',['CO_TIME_t',['../structCO__TIME__t.html',1,'']]], + ['co_5ftpdo_5ft_1369',['CO_TPDO_t',['../structCO__TPDO__t.html',1,'']]], + ['co_5ftpdocommpar_5ft_1370',['CO_TPDOCommPar_t',['../structCO__TPDOCommPar__t.html',1,'']]], + ['co_5ftpdomappar_5ft_1371',['CO_TPDOMapPar_t',['../structCO__TPDOMapPar__t.html',1,'']]], + ['co_5ftrace_5fdatatype_5ft_1372',['CO_trace_dataType_t',['../structCO__trace__dataType__t.html',1,'']]], + ['co_5ftrace_5ft_1373',['CO_trace_t',['../structCO__trace__t.html',1,'']]] +]; diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/classes_1.html b/Devices/Libraries/Systems/CANopenSocket/docs/search/classes_1.html new file mode 100755 index 0000000..c7ff4b3 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/classes_1.html @@ -0,0 +1,30 @@ + + + + + + + + + +
+
Loading...
+
+ +
Searching...
+
No Matches
+ +
+ + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/classes_1.js b/Devices/Libraries/Systems/CANopenSocket/docs/search/classes_1.js new file mode 100755 index 0000000..5ed01eb --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/classes_1.js @@ -0,0 +1,13 @@ +var searchData= +[ + ['od_5fentry_5ft_1374',['OD_entry_t',['../structOD__entry__t.html',1,'']]], + ['od_5fextensionio_5ft_1375',['OD_extensionIO_t',['../structOD__extensionIO__t.html',1,'']]], + ['od_5fio_5ft_1376',['OD_IO_t',['../structOD__IO__t.html',1,'']]], + ['od_5fobj_5farray_5ft_1377',['OD_obj_array_t',['../structOD__obj__array__t.html',1,'']]], + ['od_5fobj_5fextended_5ft_1378',['OD_obj_extended_t',['../structOD__obj__extended__t.html',1,'']]], + ['od_5fobj_5frecord_5ft_1379',['OD_obj_record_t',['../structOD__obj__record__t.html',1,'']]], + ['od_5fobj_5fvar_5ft_1380',['OD_obj_var_t',['../structOD__obj__var__t.html',1,'']]], + ['od_5fstream_5ft_1381',['OD_stream_t',['../structOD__stream__t.html',1,'']]], + ['od_5fsubentry_5ft_1382',['OD_subEntry_t',['../structOD__subEntry__t.html',1,'']]], + ['od_5ft_1383',['OD_t',['../structOD__t.html',1,'']]] +]; diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/close.png b/Devices/Libraries/Systems/CANopenSocket/docs/search/close.png new file mode 100755 index 0000000..9342d3d Binary files /dev/null and b/Devices/Libraries/Systems/CANopenSocket/docs/search/close.png differ diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/enums_0.html b/Devices/Libraries/Systems/CANopenSocket/docs/search/enums_0.html new file mode 100755 index 0000000..9669700 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/enums_0.html @@ -0,0 +1,30 @@ + + + + + + + + + +
+
Loading...
+
+ +
Searching...
+
No Matches
+ +
+ + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/enums_0.js b/Devices/Libraries/Systems/CANopenSocket/docs/search/enums_0.js new file mode 100755 index 0000000..7dfed02 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/enums_0.js @@ -0,0 +1,34 @@ +var searchData= +[ + ['co_5fcan_5ferr_5fstatus_5ft_2044',['CO_CAN_ERR_status_t',['../group__CO__driver.html#ga6c5539afb3a95af43f5477d904607426',1,'CO_driver.h']]], + ['co_5fcaninterfacestate_5ft_2045',['CO_CANinterfaceState_t',['../group__CO__socketCAN__ERROR.html#ga7bf6a56766c008531d32b4218a5a9061',1,'CO_error.h']]], + ['co_5fcommandinterface_5ft_2046',['CO_commandInterface_t',['../group__CO__epoll__interface.html#gad5a4218d5775fd7cda81a8015e1ee6f0',1,'CO_epoll_interface.h']]], + ['co_5fdefault_5fcan_5fid_5ft_2047',['CO_Default_CAN_ID_t',['../group__CO__driver.html#ga01dd35ae53fd2209ceccabdc8bf8dd06',1,'CO_driver.h']]], + ['co_5fem_5ferrorcode_5ft_2048',['CO_EM_errorCode_t',['../group__CO__Emergency.html#ga0653c307fd6bc5238babf48e01c9fa02',1,'CO_Emergency.h']]], + ['co_5fem_5ferrorstatusbits_5ft_2049',['CO_EM_errorStatusBits_t',['../group__CO__Emergency.html#ga587034df9d350c8e121c253f1d4eeacc',1,'CO_Emergency.h']]], + ['co_5ferrorregister_5ft_2050',['CO_errorRegister_t',['../group__CO__Emergency.html#ga2cfc261cce03577083ee3f1a31d5e03c',1,'CO_Emergency.h']]], + ['co_5ffifo_5fst_2051',['CO_fifo_st',['../group__CO__CANopen__301__fifo.html#ga2c7db7d527e4055a5dde62b74dfc2818',1,'CO_fifo.h']]], + ['co_5fgtwa_5fresperrorcode_5ft_2052',['CO_GTWA_respErrorCode_t',['../group__CO__CANopen__309__3.html#ga92e67dec9b5e29cdd67a28651db237fb',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5fstate_5ft_2053',['CO_GTWA_state_t',['../group__CO__CANopen__309__3.html#gae809d7b5adbc7a4fb1f2fce527b30954',1,'CO_gateway_ascii.h']]], + ['co_5fhbconsumer_5fstate_5ft_2054',['CO_HBconsumer_state_t',['../group__CO__HBconsumer.html#ga7658e41b7c045b7b612e4ef8a2b663f3',1,'CO_HBconsumer.h']]], + ['co_5fled_5fbitfield_5ft_2055',['CO_LED_BITFIELD_t',['../group__CO__LEDs.html#ga366de3822a3da8478e97b248bed641fb',1,'CO_LEDs.h']]], + ['co_5flss_5fbittimingtable_5ft_2056',['CO_LSS_bitTimingTable_t',['../group__CO__LSS.html#gacb4c13e75306153eafd535e55ba0ca2c',1,'CO_LSS.h']]], + ['co_5flss_5fcfgbittiming_5ft_2057',['CO_LSS_cfgBitTiming_t',['../group__CO__LSS.html#ga28b8651550d1719c38cd307f4ef0a8ac',1,'CO_LSS.h']]], + ['co_5flss_5fcfgnodeid_5ft_2058',['CO_LSS_cfgNodeId_t',['../group__CO__LSS.html#gaf8a13f567f8f405e4aae68268ba5d0a5',1,'CO_LSS.h']]], + ['co_5flss_5fcfgstore_5ft_2059',['CO_LSS_cfgStore_t',['../group__CO__LSS.html#ga1e4e8c43143125ebe8912de81464bd9f',1,'CO_LSS.h']]], + ['co_5flss_5fcs_5ft_2060',['CO_LSS_cs_t',['../group__CO__LSS.html#gacc7cba1fb1f1f595506751d6af385964',1,'CO_LSS.h']]], + ['co_5flss_5ffastscan_5fbitcheck_2061',['CO_LSS_fastscan_bitcheck',['../group__CO__LSS.html#ga65751e78ae5f2674cc7205e13967f7c0',1,'CO_LSS.h']]], + ['co_5flss_5ffastscan_5flss_5fsub_5fnext_2062',['CO_LSS_fastscan_lss_sub_next',['../group__CO__LSS.html#ga1ce707d287b285e7d148f37f93e0f02a',1,'CO_LSS.h']]], + ['co_5flss_5fstate_5ft_2063',['CO_LSS_state_t',['../group__CO__LSS.html#gaaa9a270e40ea09850e1661e5aeb080ad',1,'CO_LSS.h']]], + ['co_5flssmaster_5freturn_5ft_2064',['CO_LSSmaster_return_t',['../group__CO__LSSmaster.html#gae848ff3ff649c8a23b96053efaea4985',1,'CO_LSSmaster.h']]], + ['co_5flssmaster_5fscantype_5ft_2065',['CO_LSSmaster_scantype_t',['../group__CO__LSSmaster.html#ga6e3f0d07f0712c371fb81cbf4a3dbcb1',1,'CO_LSSmaster.h']]], + ['co_5fnmt_5fcommand_5ft_2066',['CO_NMT_command_t',['../group__CO__NMT__Heartbeat.html#gac396242e2e12ef6b0b22ff48636bc4eb',1,'CO_NMT_Heartbeat.h']]], + ['co_5fnmt_5fcontrol_5ft_2067',['CO_NMT_control_t',['../group__CO__NMT__Heartbeat.html#gaf92cf5943801e5dda84654345cc3d67f',1,'CO_NMT_Heartbeat.h']]], + ['co_5fnmt_5finternalstate_5ft_2068',['CO_NMT_internalState_t',['../group__CO__NMT__Heartbeat.html#ga1e8c2a6c0fd4a33183503d25a7c6d744',1,'CO_NMT_Heartbeat.h']]], + ['co_5fnmt_5freset_5fcmd_5ft_2069',['CO_NMT_reset_cmd_t',['../group__CO__NMT__Heartbeat.html#gaf42f056a571b8e17a2d74428d1a49674',1,'CO_NMT_Heartbeat.h']]], + ['co_5freturnerror_5ft_2070',['CO_ReturnError_t',['../group__CO__driver.html#ga1cb2d3466eb0c6d267f3b5ff1a0d9532',1,'CO_driver.h']]], + ['co_5fsdo_5fabortcode_5ft_2071',['CO_SDO_abortCode_t',['../group__CO__SDOserver.html#ga7587ddcf798747fe6d97d03bf1bf2979',1,'CO_SDOserver.h']]], + ['co_5fsdo_5freturn_5ft_2072',['CO_SDO_return_t',['../group__CO__SDOserver.html#ga7f729ab203285c7623df493916f22a73',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fstate_5ft_2073',['CO_SDO_state_t',['../group__CO__SDOserver.html#ga0b0e614dadcc1c005185b8bc9a7fec11',1,'CO_SDOserver.h']]], + ['co_5fsync_5fstatus_5ft_2074',['CO_SYNC_status_t',['../group__CO__SYNC.html#ga121ede6e0c90c66076a7ed950db38517',1,'CO_SYNC.h']]] +]; diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/enums_1.html b/Devices/Libraries/Systems/CANopenSocket/docs/search/enums_1.html new file mode 100755 index 0000000..dfec174 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/enums_1.html @@ -0,0 +1,30 @@ + + + + + + + + + +
+
Loading...
+
+ +
Searching...
+
No Matches
+ +
+ + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/enums_1.js b/Devices/Libraries/Systems/CANopenSocket/docs/search/enums_1.js new file mode 100755 index 0000000..5286152 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/enums_1.js @@ -0,0 +1,7 @@ +var searchData= +[ + ['od_5fattributes_5ft_2075',['OD_attributes_t',['../group__CO__ODinterface.html#ga47b0d204aaf1ea64b4f826aaf8f5c151',1,'CO_ODinterface.h']]], + ['od_5fobjdicid_5f30x_5ft_2076',['OD_ObjDicId_30x_t',['../group__CO__ODinterface.html#gade8960f241ee3b728eac09288a694886',1,'CO_ODinterface.h']]], + ['od_5fobjecttypes_5ft_2077',['OD_objectTypes_t',['../group__CO__ODdefinition.html#gaae426e9d66ec1bacfef2d93f096d7805',1,'CO_ODinterface.h']]], + ['odr_5ft_2078',['ODR_t',['../group__CO__ODinterface.html#ga0e9afd8ad27de0920d1fe0738834869c',1,'CO_ODinterface.h']]] +]; diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/enumvalues_0.html b/Devices/Libraries/Systems/CANopenSocket/docs/search/enumvalues_0.html new file mode 100755 index 0000000..9286248 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/enumvalues_0.html @@ -0,0 +1,30 @@ + + + + + + + + + +
+
Loading...
+
+ +
Searching...
+
No Matches
+ +
+ + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/enumvalues_0.js b/Devices/Libraries/Systems/CANopenSocket/docs/search/enumvalues_0.js new file mode 100755 index 0000000..16c6e11 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/enumvalues_0.js @@ -0,0 +1,367 @@ +var searchData= +[ + ['co_5fcan_5ferr_5fwarn_5fpassive_2079',['CO_CAN_ERR_WARN_PASSIVE',['../group__CO__driver.html#gga6c5539afb3a95af43f5477d904607426a5ad44f86d5691f2bc809f722364516e0',1,'CO_driver.h']]], + ['co_5fcan_5ferrrx_5foverflow_2080',['CO_CAN_ERRRX_OVERFLOW',['../group__CO__driver.html#gga6c5539afb3a95af43f5477d904607426a714a0b9c0978ffde5f138a81880a1fdd',1,'CO_driver.h']]], + ['co_5fcan_5ferrrx_5fpassive_2081',['CO_CAN_ERRRX_PASSIVE',['../group__CO__driver.html#gga6c5539afb3a95af43f5477d904607426a51494ad0df1e2de6d9395f1803c4b233',1,'CO_driver.h']]], + ['co_5fcan_5ferrrx_5fwarning_2082',['CO_CAN_ERRRX_WARNING',['../group__CO__driver.html#gga6c5539afb3a95af43f5477d904607426a6df88a8a296eb4addc12d9136c0566b0',1,'CO_driver.h']]], + ['co_5fcan_5ferrtx_5fbus_5foff_2083',['CO_CAN_ERRTX_BUS_OFF',['../group__CO__driver.html#gga6c5539afb3a95af43f5477d904607426ae58aa7d0eb5d13630f858a3869f0ee7d',1,'CO_driver.h']]], + ['co_5fcan_5ferrtx_5foverflow_2084',['CO_CAN_ERRTX_OVERFLOW',['../group__CO__driver.html#gga6c5539afb3a95af43f5477d904607426ad6fba8a27d82774f53812d3a49655d12',1,'CO_driver.h']]], + ['co_5fcan_5ferrtx_5fpassive_2085',['CO_CAN_ERRTX_PASSIVE',['../group__CO__driver.html#gga6c5539afb3a95af43f5477d904607426a85d05e861dc03e256dccf977ae16ad6e',1,'CO_driver.h']]], + ['co_5fcan_5ferrtx_5fpdo_5flate_2086',['CO_CAN_ERRTX_PDO_LATE',['../group__CO__driver.html#gga6c5539afb3a95af43f5477d904607426ac0b993c7786f41a8c73ad0339124881b',1,'CO_driver.h']]], + ['co_5fcan_5ferrtx_5fwarning_2087',['CO_CAN_ERRTX_WARNING',['../group__CO__driver.html#gga6c5539afb3a95af43f5477d904607426a725e4fe057c2f9d222850686a76c724d',1,'CO_driver.h']]], + ['co_5fcan_5fid_5femergency_2088',['CO_CAN_ID_EMERGENCY',['../group__CO__driver.html#gga01dd35ae53fd2209ceccabdc8bf8dd06a94ffef8babcef5b807c5f8c865ef7666',1,'CO_driver.h']]], + ['co_5fcan_5fid_5fgfc_2089',['CO_CAN_ID_GFC',['../group__CO__driver.html#gga01dd35ae53fd2209ceccabdc8bf8dd06a0ba8a628aa1a873a21820070261c2783',1,'CO_driver.h']]], + ['co_5fcan_5fid_5fheartbeat_2090',['CO_CAN_ID_HEARTBEAT',['../group__CO__driver.html#gga01dd35ae53fd2209ceccabdc8bf8dd06a0cfd21623475a1a8522b30b8b16d9874',1,'CO_driver.h']]], + ['co_5fcan_5fid_5flss_5fmst_2091',['CO_CAN_ID_LSS_MST',['../group__CO__driver.html#gga01dd35ae53fd2209ceccabdc8bf8dd06ad7d16ed89e513b035104e4b2634ce287',1,'CO_driver.h']]], + ['co_5fcan_5fid_5flss_5fslv_2092',['CO_CAN_ID_LSS_SLV',['../group__CO__driver.html#gga01dd35ae53fd2209ceccabdc8bf8dd06a4a62af7fb0b8768e57945a558a0ceee4',1,'CO_driver.h']]], + ['co_5fcan_5fid_5fnmt_5fservice_2093',['CO_CAN_ID_NMT_SERVICE',['../group__CO__driver.html#gga01dd35ae53fd2209ceccabdc8bf8dd06a335d0f6204819d267ba396b715f66ead',1,'CO_driver.h']]], + ['co_5fcan_5fid_5frpdo_5f1_2094',['CO_CAN_ID_RPDO_1',['../group__CO__driver.html#gga01dd35ae53fd2209ceccabdc8bf8dd06a0ab4be02961987ad817a99a4ef379517',1,'CO_driver.h']]], + ['co_5fcan_5fid_5frpdo_5f2_2095',['CO_CAN_ID_RPDO_2',['../group__CO__driver.html#gga01dd35ae53fd2209ceccabdc8bf8dd06a60081a7b09921c6bfce3762a3dd4e49f',1,'CO_driver.h']]], + ['co_5fcan_5fid_5frpdo_5f3_2096',['CO_CAN_ID_RPDO_3',['../group__CO__driver.html#gga01dd35ae53fd2209ceccabdc8bf8dd06ad634b89f227db86bc8c633dda327e5fb',1,'CO_driver.h']]], + ['co_5fcan_5fid_5frpdo_5f4_2097',['CO_CAN_ID_RPDO_4',['../group__CO__driver.html#gga01dd35ae53fd2209ceccabdc8bf8dd06afec9dfa33a34beef50c434e5cde68c6b',1,'CO_driver.h']]], + ['co_5fcan_5fid_5fsdo_5fcli_2098',['CO_CAN_ID_SDO_CLI',['../group__CO__driver.html#gga01dd35ae53fd2209ceccabdc8bf8dd06acfe8640033d9668fafc63aa81d68ede5',1,'CO_driver.h']]], + ['co_5fcan_5fid_5fsdo_5fsrv_2099',['CO_CAN_ID_SDO_SRV',['../group__CO__driver.html#gga01dd35ae53fd2209ceccabdc8bf8dd06a3c84d92ad004cfc04e398193b742d30c',1,'CO_driver.h']]], + ['co_5fcan_5fid_5fsrdo_5f1_2100',['CO_CAN_ID_SRDO_1',['../group__CO__driver.html#gga01dd35ae53fd2209ceccabdc8bf8dd06aefe4dd6630902d36173b81c106a813bc',1,'CO_driver.h']]], + ['co_5fcan_5fid_5fsync_2101',['CO_CAN_ID_SYNC',['../group__CO__driver.html#gga01dd35ae53fd2209ceccabdc8bf8dd06a7a8486aaf2f35eb83c6ca690d0cdce06',1,'CO_driver.h']]], + ['co_5fcan_5fid_5ftime_2102',['CO_CAN_ID_TIME',['../group__CO__driver.html#gga01dd35ae53fd2209ceccabdc8bf8dd06ab2e20e54189f5cb565e80b05eb8c4931',1,'CO_driver.h']]], + ['co_5fcan_5fid_5ftpdo_5f1_2103',['CO_CAN_ID_TPDO_1',['../group__CO__driver.html#gga01dd35ae53fd2209ceccabdc8bf8dd06a848f2bdb085bc3a342400a6b43c37f82',1,'CO_driver.h']]], + ['co_5fcan_5fid_5ftpdo_5f2_2104',['CO_CAN_ID_TPDO_2',['../group__CO__driver.html#gga01dd35ae53fd2209ceccabdc8bf8dd06acb90f0dab2e31982df1bebae6dd02e4b',1,'CO_driver.h']]], + ['co_5fcan_5fid_5ftpdo_5f3_2105',['CO_CAN_ID_TPDO_3',['../group__CO__driver.html#gga01dd35ae53fd2209ceccabdc8bf8dd06a83b73730655607582d1dabc8f78f7ca4',1,'CO_driver.h']]], + ['co_5fcan_5fid_5ftpdo_5f4_2106',['CO_CAN_ID_TPDO_4',['../group__CO__driver.html#gga01dd35ae53fd2209ceccabdc8bf8dd06abe92c9f7938ad6566e8aa010ab6f5cae',1,'CO_driver.h']]], + ['co_5fem_5f0b_5funused_2107',['CO_EM_0B_unused',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca8bf6fb0db21e29e477b38304279bed5e',1,'CO_Emergency.h']]], + ['co_5fem_5f0c_5funused_2108',['CO_EM_0C_unused',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeaccaa724f4fdeff7043b0d4f454613a96992',1,'CO_Emergency.h']]], + ['co_5fem_5f0d_5funused_2109',['CO_EM_0D_unused',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca933c070fd08c1223462a3a331b016c99',1,'CO_Emergency.h']]], + ['co_5fem_5f0e_5funused_2110',['CO_EM_0E_unused',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca0cabb2e45202f938cfdafe8e7871f4f7',1,'CO_Emergency.h']]], + ['co_5fem_5f0f_5funused_2111',['CO_EM_0F_unused',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeaccac6469cc3e6176136f69e549c4a4f5b71',1,'CO_Emergency.h']]], + ['co_5fem_5f10_5funused_2112',['CO_EM_10_unused',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca274f46ac0760c4c340f48d1de884f2fe',1,'CO_Emergency.h']]], + ['co_5fem_5f11_5funused_2113',['CO_EM_11_unused',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca9d8abe2f426ed071febf85a932c1df98',1,'CO_Emergency.h']]], + ['co_5fem_5f16_5funused_2114',['CO_EM_16_unused',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca865160ae9fdac8fcba1e5335b31c2f9f',1,'CO_Emergency.h']]], + ['co_5fem_5f17_5funused_2115',['CO_EM_17_unused',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeaccad5bb584bb3c85ca0ba0313367aa75a9b',1,'CO_Emergency.h']]], + ['co_5fem_5f1d_5funused_2116',['CO_EM_1D_unused',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca6d0bf9c926241ec8f67c477928300761',1,'CO_Emergency.h']]], + ['co_5fem_5f1e_5funused_2117',['CO_EM_1E_unused',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeaccab2f3562c4e1f8e25a7837627dc1721db',1,'CO_Emergency.h']]], + ['co_5fem_5f1f_5funused_2118',['CO_EM_1F_unused',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeaccadba4afb9dac78f8eb0c5f494926568b1',1,'CO_Emergency.h']]], + ['co_5fem_5f21_5funused_2119',['CO_EM_21_unused',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeaccadbce7bd9d5a0ee681104914092b21d8d',1,'CO_Emergency.h']]], + ['co_5fem_5f23_5funused_2120',['CO_EM_23_unused',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeaccaa0c8857afdd8455b30fd0179e98599fb',1,'CO_Emergency.h']]], + ['co_5fem_5f24_5funused_2121',['CO_EM_24_unused',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca9cf88b48355b3cc43fe9a8360b8470df',1,'CO_Emergency.h']]], + ['co_5fem_5f25_5funused_2122',['CO_EM_25_unused',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca0a8abc6fcd7b0d5469b469c2cf370a82',1,'CO_Emergency.h']]], + ['co_5fem_5f26_5funused_2123',['CO_EM_26_unused',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca0398195eafec5f8d60a76f677ce2a714',1,'CO_Emergency.h']]], + ['co_5fem_5f27_5funused_2124',['CO_EM_27_unused',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca95ca6848349affc579fff2c2a62e87d7',1,'CO_Emergency.h']]], + ['co_5fem_5fcalculation_5fof_5fparameters_2125',['CO_EM_CALCULATION_OF_PARAMETERS',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca5544a90d3047bc08186ea7412528dc93',1,'CO_Emergency.h']]], + ['co_5fem_5fcan_5fbus_5fwarning_2126',['CO_EM_CAN_BUS_WARNING',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca27ebb7f155d4b72618c34dd6aa496aac',1,'CO_Emergency.h']]], + ['co_5fem_5fcan_5frx_5fbus_5fpassive_2127',['CO_EM_CAN_RX_BUS_PASSIVE',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeaccaab5efa11cefb2cd6125cec3ec1c570e1',1,'CO_Emergency.h']]], + ['co_5fem_5fcan_5frxb_5foverflow_2128',['CO_EM_CAN_RXB_OVERFLOW',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeaccaf9a86c6c3b87763593dd14be6b0bef29',1,'CO_Emergency.h']]], + ['co_5fem_5fcan_5ftx_5fbus_5foff_2129',['CO_EM_CAN_TX_BUS_OFF',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeaccae59f8e20795915a0929861809ed42e7c',1,'CO_Emergency.h']]], + ['co_5fem_5fcan_5ftx_5fbus_5fpassive_2130',['CO_EM_CAN_TX_BUS_PASSIVE',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeaccadb8502da626d80a8c423e94e1c76d0cb',1,'CO_Emergency.h']]], + ['co_5fem_5fcan_5ftx_5foverflow_2131',['CO_EM_CAN_TX_OVERFLOW',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca2dbceee7b6deae231bb40a96f8f748a9',1,'CO_Emergency.h']]], + ['co_5fem_5femergency_5fbuffer_5ffull_2132',['CO_EM_EMERGENCY_BUFFER_FULL',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeaccabd1935c51679f70f509ffd60e28c02b1',1,'CO_Emergency.h']]], + ['co_5fem_5fgeneric_5ferror_2133',['CO_EM_GENERIC_ERROR',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca21648a2863590d3cccb469f8ef759267',1,'CO_Emergency.h']]], + ['co_5fem_5fgeneric_5fsoftware_5ferror_2134',['CO_EM_GENERIC_SOFTWARE_ERROR',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca6c3e7fff310443f05815ea2b7ac6b289',1,'CO_Emergency.h']]], + ['co_5fem_5fhb_5fconsumer_5fremote_5freset_2135',['CO_EM_HB_CONSUMER_REMOTE_RESET',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca0b6698662476cc622661fb5a5a75ec31',1,'CO_Emergency.h']]], + ['co_5fem_5fheartbeat_5fconsumer_2136',['CO_EM_HEARTBEAT_CONSUMER',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca6478d414ea45f6a9129e68a9d57e11b7',1,'CO_Emergency.h']]], + ['co_5fem_5finconsistent_5fobject_5fdict_2137',['CO_EM_INCONSISTENT_OBJECT_DICT',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca2c6a108cabca6f03b1400065f2ad4887',1,'CO_Emergency.h']]], + ['co_5fem_5fisr_5ftimer_5foverflow_2138',['CO_EM_ISR_TIMER_OVERFLOW',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeaccadbf7382f537c9f59f965ce38be464e46',1,'CO_Emergency.h']]], + ['co_5fem_5fmanufacturer_5fend_2139',['CO_EM_MANUFACTURER_END',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca5d3c6fdb77551b3f4aaf993ae1dfb414',1,'CO_Emergency.h']]], + ['co_5fem_5fmanufacturer_5fstart_2140',['CO_EM_MANUFACTURER_START',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeaccaf850a661aadde65b10b22715cf24942c',1,'CO_Emergency.h']]], + ['co_5fem_5fmemory_5fallocation_5ferror_2141',['CO_EM_MEMORY_ALLOCATION_ERROR',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca2575dac21ff9ac9c4c4e5ca63d34fdbc',1,'CO_Emergency.h']]], + ['co_5fem_5fmicrocontroller_5freset_2142',['CO_EM_MICROCONTROLLER_RESET',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeaccacb69eecc08e72c56aec215fa55e27e16',1,'CO_Emergency.h']]], + ['co_5fem_5fnmt_5fwrong_5fcommand_2143',['CO_EM_NMT_WRONG_COMMAND',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeaccac5f82aeeda52c83eee0025c8b387ac5d',1,'CO_Emergency.h']]], + ['co_5fem_5fno_5ferror_2144',['CO_EM_NO_ERROR',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeaccafb7b06b4b1d4fb2f9fa8661fdbaf8b01',1,'CO_Emergency.h']]], + ['co_5fem_5fnon_5fvolatile_5fmemory_2145',['CO_EM_NON_VOLATILE_MEMORY',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeaccac019532cacaa8310f9ca413a2f599af3',1,'CO_Emergency.h']]], + ['co_5fem_5fpdo_5fwrong_5fmapping_2146',['CO_EM_PDO_WRONG_MAPPING',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca7308b487766b8feca60ef0c1b873f167',1,'CO_Emergency.h']]], + ['co_5fem_5frpdo_5foverflow_2147',['CO_EM_RPDO_OVERFLOW',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca73426de91d49273d023b5084a0cea8e0',1,'CO_Emergency.h']]], + ['co_5fem_5frpdo_5fwrong_5flength_2148',['CO_EM_RPDO_WRONG_LENGTH',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca2a36480d4dd6a24f1f8bb66d79441a8d',1,'CO_Emergency.h']]], + ['co_5fem_5frxmsg_5foverflow_2149',['CO_EM_RXMSG_OVERFLOW',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca0b17027ee0097065d92e6c0981e3face',1,'CO_Emergency.h']]], + ['co_5fem_5frxmsg_5fwrong_5flength_2150',['CO_EM_RXMSG_WRONG_LENGTH',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeaccae1e45de61059459a6f1f6e500962f287',1,'CO_Emergency.h']]], + ['co_5fem_5fsync_5flength_2151',['CO_EM_SYNC_LENGTH',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca09a521bfc9ea08ed340cfa29952a471c',1,'CO_Emergency.h']]], + ['co_5fem_5fsync_5ftime_5fout_2152',['CO_EM_SYNC_TIME_OUT',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeaccafd760392f4d4d6358896486c5b5d7d82',1,'CO_Emergency.h']]], + ['co_5fem_5ftime_5flength_2153',['CO_EM_TIME_LENGTH',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca3af179820ed2aa88e2c22b7961de71f8',1,'CO_Emergency.h']]], + ['co_5fem_5ftime_5ftimeout_2154',['CO_EM_TIME_TIMEOUT',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca5c1a6209ebe6167bbf13f565b6fd994d',1,'CO_Emergency.h']]], + ['co_5fem_5ftpdo_5foutside_5fwindow_2155',['CO_EM_TPDO_OUTSIDE_WINDOW',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeaccaea141284cd85126a9b3e7b0605a26a94',1,'CO_Emergency.h']]], + ['co_5fem_5fwrong_5ferror_5freport_2156',['CO_EM_WRONG_ERROR_REPORT',['../group__CO__Emergency.html#gga587034df9d350c8e121c253f1d4eeacca2d7776243205bc75e6c448e13e697480',1,'CO_Emergency.h']]], + ['co_5femc401_5fin_5fvolt_5fhi_2157',['CO_EMC401_IN_VOLT_HI',['../group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02aacdc3517e800b037b46c1b54f454b562',1,'CO_Emergency.h']]], + ['co_5femc401_5fin_5fvolt_5flow_2158',['CO_EMC401_IN_VOLT_LOW',['../group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a867eb16fce01ade3c728df7c7527e311',1,'CO_Emergency.h']]], + ['co_5femc401_5fintern_5fvolt_5fhi_2159',['CO_EMC401_INTERN_VOLT_HI',['../group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a9cd0f1a897f40a3a43198ba05de4a11b',1,'CO_Emergency.h']]], + ['co_5femc401_5fintern_5fvolt_5flo_2160',['CO_EMC401_INTERN_VOLT_LO',['../group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02ac122203ed5c6a71749ace599b13ac594',1,'CO_Emergency.h']]], + ['co_5femc401_5fout_5fcur_5fhi_2161',['CO_EMC401_OUT_CUR_HI',['../group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02aa432d9c66bb0f6eecc38d720cae6c32e',1,'CO_Emergency.h']]], + ['co_5femc401_5fout_5fload_5fdump_2162',['CO_EMC401_OUT_LOAD_DUMP',['../group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a21cd31a1455c9dc379796798f0eecd32',1,'CO_Emergency.h']]], + ['co_5femc401_5fout_5fshorted_2163',['CO_EMC401_OUT_SHORTED',['../group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a182a0c7afc0cb1c30af42a05430da353',1,'CO_Emergency.h']]], + ['co_5femc401_5fout_5fvolt_5fhigh_2164',['CO_EMC401_OUT_VOLT_HIGH',['../group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02abf5b6a68120351c2fa52146b45798ed2',1,'CO_Emergency.h']]], + ['co_5femc401_5fout_5fvolt_5flow_2165',['CO_EMC401_OUT_VOLT_LOW',['../group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a5f262e622db7482b7230055e5b27c902',1,'CO_Emergency.h']]], + ['co_5femc_5fadditional_5ffunc_2166',['CO_EMC_ADDITIONAL_FUNC',['../group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02af5f9446049023ceae559562998172278',1,'CO_Emergency.h']]], + ['co_5femc_5fadditional_5fmodul_2167',['CO_EMC_ADDITIONAL_MODUL',['../group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02ae210dc1069c7b046527f7d7903ef82cb',1,'CO_Emergency.h']]], + ['co_5femc_5fbus_5foff_5frecovered_2168',['CO_EMC_BUS_OFF_RECOVERED',['../group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a2fd717ed311007b4dd6fe92443f134b0',1,'CO_Emergency.h']]], + ['co_5femc_5fcan_5fid_5fcollision_2169',['CO_EMC_CAN_ID_COLLISION',['../group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a683bff5350b0cbab24aef2fc8eac363a',1,'CO_Emergency.h']]], + ['co_5femc_5fcan_5foverrun_2170',['CO_EMC_CAN_OVERRUN',['../group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a1f04b4ffe9cc1d8f2b294261909dec4e',1,'CO_Emergency.h']]], + ['co_5femc_5fcan_5fpassive_2171',['CO_EMC_CAN_PASSIVE',['../group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02aa024c00c21f705474355b9ca7d7ce948',1,'CO_Emergency.h']]], + ['co_5femc_5fcommunication_2172',['CO_EMC_COMMUNICATION',['../group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02aab2946bf800f551bcae55dd299ff315b',1,'CO_Emergency.h']]], + ['co_5femc_5fcurrent_2173',['CO_EMC_CURRENT',['../group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02abad9ca04a37cc43cacabfef9483699cf',1,'CO_Emergency.h']]], + ['co_5femc_5fcurrent_5finput_2174',['CO_EMC_CURRENT_INPUT',['../group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02ab792c971a569d1175666b3fff9ffbe70',1,'CO_Emergency.h']]], + ['co_5femc_5fcurrent_5finside_2175',['CO_EMC_CURRENT_INSIDE',['../group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a3ded1c05cbf37d2d7d286af97e833e65',1,'CO_Emergency.h']]], + ['co_5femc_5fcurrent_5foutput_2176',['CO_EMC_CURRENT_OUTPUT',['../group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02ad42e8ab666fd3da75d1fa3a7b8708efc',1,'CO_Emergency.h']]], + ['co_5femc_5fdam_5fmpdo_2177',['CO_EMC_DAM_MPDO',['../group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02ab58df03302ab06710f7455d37039dea3',1,'CO_Emergency.h']]], + ['co_5femc_5fdata_5fset_2178',['CO_EMC_DATA_SET',['../group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02ad22329fc3e44867a365401458e691ddc',1,'CO_Emergency.h']]], + ['co_5femc_5fdevice_5fspecific_2179',['CO_EMC_DEVICE_SPECIFIC',['../group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02ad7b895b5e7d0f3fa7ff422157ac36c70',1,'CO_Emergency.h']]], + ['co_5femc_5fexternal_5ferror_2180',['CO_EMC_EXTERNAL_ERROR',['../group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a1d76eff88ebd6050377c393533aebc8d',1,'CO_Emergency.h']]], + ['co_5femc_5fgeneric_2181',['CO_EMC_GENERIC',['../group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a2eaf45ca12b32b7bcc58df91becda767',1,'CO_Emergency.h']]], + ['co_5femc_5fhardware_2182',['CO_EMC_HARDWARE',['../group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a33344d49b9667151d86aef28a73e6f66',1,'CO_Emergency.h']]], + ['co_5femc_5fheartbeat_2183',['CO_EMC_HEARTBEAT',['../group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02aff47b500e2e760355ca653b247e4b93f',1,'CO_Emergency.h']]], + ['co_5femc_5fmonitoring_2184',['CO_EMC_MONITORING',['../group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a8ecd7e45af52d83d986e3de8e957a986',1,'CO_Emergency.h']]], + ['co_5femc_5fno_5ferror_2185',['CO_EMC_NO_ERROR',['../group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02aa60e1333102cbe544eccbaad8e77f6f7',1,'CO_Emergency.h']]], + ['co_5femc_5fpdo_5flength_2186',['CO_EMC_PDO_LENGTH',['../group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a80fedd7bbb98ddf1ec26d4b31ed6d749',1,'CO_Emergency.h']]], + ['co_5femc_5fpdo_5flength_5fexc_2187',['CO_EMC_PDO_LENGTH_EXC',['../group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a88bac871b7539a579fba73825a2e240a',1,'CO_Emergency.h']]], + ['co_5femc_5fprotocol_5ferror_2188',['CO_EMC_PROTOCOL_ERROR',['../group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02ab884b23b23af9671d99cca5865549e5a',1,'CO_Emergency.h']]], + ['co_5femc_5frpdo_5ftimeout_2189',['CO_EMC_RPDO_TIMEOUT',['../group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a4ca48c8d1be6a42ac0c13e551e12b230',1,'CO_Emergency.h']]], + ['co_5femc_5fsoftware_5fdevice_2190',['CO_EMC_SOFTWARE_DEVICE',['../group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a6d0b3c0c31228e0bc57fc080c754fefa',1,'CO_Emergency.h']]], + ['co_5femc_5fsoftware_5finternal_2191',['CO_EMC_SOFTWARE_INTERNAL',['../group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a62e0949639733e85c2b6d4c8b099d467',1,'CO_Emergency.h']]], + ['co_5femc_5fsoftware_5fuser_2192',['CO_EMC_SOFTWARE_USER',['../group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a7b6ae38c015688128890bfe42b0271e5',1,'CO_Emergency.h']]], + ['co_5femc_5fsync_5fdata_5flength_2193',['CO_EMC_SYNC_DATA_LENGTH',['../group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a81aa2a66727d1fe29720067dc4e20879',1,'CO_Emergency.h']]], + ['co_5femc_5ftemp_5fambient_2194',['CO_EMC_TEMP_AMBIENT',['../group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02ae5256d8178374a48750537c3d04c0a30',1,'CO_Emergency.h']]], + ['co_5femc_5ftemp_5fdevice_2195',['CO_EMC_TEMP_DEVICE',['../group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a6c29a4b49fae39b45c5c0e553ef6668f',1,'CO_Emergency.h']]], + ['co_5femc_5ftemperature_2196',['CO_EMC_TEMPERATURE',['../group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02aa24dfa4c6948187f62d3e8182285d4a3',1,'CO_Emergency.h']]], + ['co_5femc_5ftime_5fdata_5flength_2197',['CO_EMC_TIME_DATA_LENGTH',['../group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a018485be8125a3515ecd127a08e2e2f1',1,'CO_Emergency.h']]], + ['co_5femc_5fvoltage_2198',['CO_EMC_VOLTAGE',['../group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a84a8f0dfb97e0ec13be9a4cdb0d71233',1,'CO_Emergency.h']]], + ['co_5femc_5fvoltage_5finside_2199',['CO_EMC_VOLTAGE_INSIDE',['../group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a78dfa1d496a010ae7ae2e8b6edc1362a',1,'CO_Emergency.h']]], + ['co_5femc_5fvoltage_5fmains_2200',['CO_EMC_VOLTAGE_MAINS',['../group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02ab4b095d1d9e7e7f5150bc2ecd83bc140',1,'CO_Emergency.h']]], + ['co_5femc_5fvoltage_5foutput_2201',['CO_EMC_VOLTAGE_OUTPUT',['../group__CO__Emergency.html#gga0653c307fd6bc5238babf48e01c9fa02a9c5becd591c91bb3e255badf0a308c2d',1,'CO_Emergency.h']]], + ['co_5ferr_5freg_5fcommunication_2202',['CO_ERR_REG_COMMUNICATION',['../group__CO__Emergency.html#gga2cfc261cce03577083ee3f1a31d5e03ca2f3b7aeac7282281c1d17895406c006a',1,'CO_Emergency.h']]], + ['co_5ferr_5freg_5fcurrent_2203',['CO_ERR_REG_CURRENT',['../group__CO__Emergency.html#gga2cfc261cce03577083ee3f1a31d5e03ca61eded29fb0fcd95b2f66c2682de0f2b',1,'CO_Emergency.h']]], + ['co_5ferr_5freg_5fdev_5fprofile_2204',['CO_ERR_REG_DEV_PROFILE',['../group__CO__Emergency.html#gga2cfc261cce03577083ee3f1a31d5e03cab137d1705e9ab20e2caeb22f57dd4716',1,'CO_Emergency.h']]], + ['co_5ferr_5freg_5fgeneric_5ferr_2205',['CO_ERR_REG_GENERIC_ERR',['../group__CO__Emergency.html#gga2cfc261cce03577083ee3f1a31d5e03ca92a7e121ae04a022fc2fe604eb1c148e',1,'CO_Emergency.h']]], + ['co_5ferr_5freg_5fmanufacturer_2206',['CO_ERR_REG_MANUFACTURER',['../group__CO__Emergency.html#gga2cfc261cce03577083ee3f1a31d5e03ca207eee1d9638f61166bc395ee71b84a3',1,'CO_Emergency.h']]], + ['co_5ferr_5freg_5freserved_2207',['CO_ERR_REG_RESERVED',['../group__CO__Emergency.html#gga2cfc261cce03577083ee3f1a31d5e03caffdf7f5d9f9ae52fa1bf97a3fb3d848b',1,'CO_Emergency.h']]], + ['co_5ferr_5freg_5ftemperature_2208',['CO_ERR_REG_TEMPERATURE',['../group__CO__Emergency.html#gga2cfc261cce03577083ee3f1a31d5e03cab12f2b419af0aeb8aae83a13d5c8b7bf',1,'CO_Emergency.h']]], + ['co_5ferr_5freg_5fvoltage_2209',['CO_ERR_REG_VOLTAGE',['../group__CO__Emergency.html#gga2cfc261cce03577083ee3f1a31d5e03ca360c75e04303d1c55e2bc8528407cb87',1,'CO_Emergency.h']]], + ['co_5ferror_5fcrc_2210',['CO_ERROR_CRC',['../group__CO__driver.html#gga1cb2d3466eb0c6d267f3b5ff1a0d9532a25eaff6474c6bc2aae67b1e7c7a35109',1,'CO_driver.h']]], + ['co_5ferror_5fdata_5fcorrupt_2211',['CO_ERROR_DATA_CORRUPT',['../group__CO__driver.html#gga1cb2d3466eb0c6d267f3b5ff1a0d9532aa053301213d13670e9af7d31abc1ee48',1,'CO_driver.h']]], + ['co_5ferror_5fillegal_5fargument_2212',['CO_ERROR_ILLEGAL_ARGUMENT',['../group__CO__driver.html#gga1cb2d3466eb0c6d267f3b5ff1a0d9532a3e78d588c0e21630c9cb7b43bfda3dae',1,'CO_driver.h']]], + ['co_5ferror_5fillegal_5fbaudrate_2213',['CO_ERROR_ILLEGAL_BAUDRATE',['../group__CO__driver.html#gga1cb2d3466eb0c6d267f3b5ff1a0d9532a0214308865e83b8c21de7317b3070097',1,'CO_driver.h']]], + ['co_5ferror_5finvalid_5fstate_2214',['CO_ERROR_INVALID_STATE',['../group__CO__driver.html#gga1cb2d3466eb0c6d267f3b5ff1a0d9532a57f1c4d2d960ab2550669a1c5ebbf4e1',1,'CO_driver.h']]], + ['co_5ferror_5fno_2215',['CO_ERROR_NO',['../group__CO__driver.html#gga1cb2d3466eb0c6d267f3b5ff1a0d9532aff1c3e9fd4bf65e6757b020f752cdac8',1,'CO_driver.h']]], + ['co_5ferror_5fnode_5fid_5funconfigured_5flss_2216',['CO_ERROR_NODE_ID_UNCONFIGURED_LSS',['../group__CO__driver.html#gga1cb2d3466eb0c6d267f3b5ff1a0d9532ac7a6f0554ae52997b88a97b46a16e5a3',1,'CO_driver.h']]], + ['co_5ferror_5fod_5fparameters_2217',['CO_ERROR_OD_PARAMETERS',['../group__CO__driver.html#gga1cb2d3466eb0c6d267f3b5ff1a0d9532a737e67c9f8d3ca882a3cba852153c1f3',1,'CO_driver.h']]], + ['co_5ferror_5fout_5fof_5fmemory_2218',['CO_ERROR_OUT_OF_MEMORY',['../group__CO__driver.html#gga1cb2d3466eb0c6d267f3b5ff1a0d9532a2f07295c4c6539c2b390db2d7c351267',1,'CO_driver.h']]], + ['co_5ferror_5frx_5fmsg_5flength_2219',['CO_ERROR_RX_MSG_LENGTH',['../group__CO__driver.html#gga1cb2d3466eb0c6d267f3b5ff1a0d9532a3e40e8440a8480c413d3ff724b875de4',1,'CO_driver.h']]], + ['co_5ferror_5frx_5foverflow_2220',['CO_ERROR_RX_OVERFLOW',['../group__CO__driver.html#gga1cb2d3466eb0c6d267f3b5ff1a0d9532ab0746d0801a99639077b325594b347b5',1,'CO_driver.h']]], + ['co_5ferror_5frx_5fpdo_5flength_2221',['CO_ERROR_RX_PDO_LENGTH',['../group__CO__driver.html#gga1cb2d3466eb0c6d267f3b5ff1a0d9532a4829a460cbba557a2ff7f52bd912aa65',1,'CO_driver.h']]], + ['co_5ferror_5frx_5fpdo_5foverflow_2222',['CO_ERROR_RX_PDO_OVERFLOW',['../group__CO__driver.html#gga1cb2d3466eb0c6d267f3b5ff1a0d9532a7e4cefeb35775754c87cf6a559f1bbd9',1,'CO_driver.h']]], + ['co_5ferror_5fsyscall_2223',['CO_ERROR_SYSCALL',['../group__CO__driver.html#gga1cb2d3466eb0c6d267f3b5ff1a0d9532a46d81c922ecbaf8f136626725ad8399d',1,'CO_driver.h']]], + ['co_5ferror_5ftimeout_2224',['CO_ERROR_TIMEOUT',['../group__CO__driver.html#gga1cb2d3466eb0c6d267f3b5ff1a0d9532a129c7141c52ae59d37a2fff163fec600',1,'CO_driver.h']]], + ['co_5ferror_5ftx_5fbusy_2225',['CO_ERROR_TX_BUSY',['../group__CO__driver.html#gga1cb2d3466eb0c6d267f3b5ff1a0d9532ab47ecc1b08463eb986fd29c187096343',1,'CO_driver.h']]], + ['co_5ferror_5ftx_5foverflow_2226',['CO_ERROR_TX_OVERFLOW',['../group__CO__driver.html#gga1cb2d3466eb0c6d267f3b5ff1a0d9532a757b4d226a3e00a9e2543cf21833d46f',1,'CO_driver.h']]], + ['co_5ferror_5ftx_5fpdo_5fwindow_2227',['CO_ERROR_TX_PDO_WINDOW',['../group__CO__driver.html#gga1cb2d3466eb0c6d267f3b5ff1a0d9532a48993dc18698738d168071b4f4c3d244',1,'CO_driver.h']]], + ['co_5ferror_5ftx_5funconfigured_2228',['CO_ERROR_TX_UNCONFIGURED',['../group__CO__driver.html#gga1cb2d3466eb0c6d267f3b5ff1a0d9532a6f07e7c21acde035b561edc7f55edb89',1,'CO_driver.h']]], + ['co_5ferror_5fwrong_5fnmt_5fstate_2229',['CO_ERROR_WRONG_NMT_STATE',['../group__CO__driver.html#gga1cb2d3466eb0c6d267f3b5ff1a0d9532a07457c9c6eda89fc0260e6d7e431424c',1,'CO_driver.h']]], + ['co_5ffifo_5fst_5fclosed_2230',['CO_fifo_st_closed',['../group__CO__CANopen__301__fifo.html#gga2c7db7d527e4055a5dde62b74dfc2818a1f111c3f5a9396da6ced33132a2341f9',1,'CO_fifo.h']]], + ['co_5ffifo_5fst_5ferrbuf_2231',['CO_fifo_st_errBuf',['../group__CO__CANopen__301__fifo.html#gga2c7db7d527e4055a5dde62b74dfc2818a51c064e77b0e2bf3741719b57de65141',1,'CO_fifo.h']]], + ['co_5ffifo_5fst_5ferrint_2232',['CO_fifo_st_errInt',['../group__CO__CANopen__301__fifo.html#gga2c7db7d527e4055a5dde62b74dfc2818a07a7d8d483365e3afdcf9dc5b2dd2143',1,'CO_fifo.h']]], + ['co_5ffifo_5fst_5ferrmask_2233',['CO_fifo_st_errMask',['../group__CO__CANopen__301__fifo.html#gga2c7db7d527e4055a5dde62b74dfc2818a9e587fa1d802fbe939f6da9d44aba389',1,'CO_fifo.h']]], + ['co_5ffifo_5fst_5ferrtok_2234',['CO_fifo_st_errTok',['../group__CO__CANopen__301__fifo.html#gga2c7db7d527e4055a5dde62b74dfc2818a70060b411fcbfa0696ca453d7f4348d4',1,'CO_fifo.h']]], + ['co_5ffifo_5fst_5ferrval_2235',['CO_fifo_st_errVal',['../group__CO__CANopen__301__fifo.html#gga2c7db7d527e4055a5dde62b74dfc2818a9c41b295a487450017cc3b1ce0bcedfb',1,'CO_fifo.h']]], + ['co_5ffifo_5fst_5fpartial_2236',['CO_fifo_st_partial',['../group__CO__CANopen__301__fifo.html#gga2c7db7d527e4055a5dde62b74dfc2818a7bf88ff5b78ef6a25a4391bc6423c1f7',1,'CO_fifo.h']]], + ['co_5fgtwa_5fresperrorbootup_2237',['CO_GTWA_respErrorBootUp',['../group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fbaa4c5257721f3b64c231f67dccacd2ee7',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5fresperrorbusoff_2238',['CO_GTWA_respErrorBusOff',['../group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fbabb407ab8220e0d3f0ebc06f867ecac9c',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5fresperrorcanactive_2239',['CO_GTWA_respErrorCANactive',['../group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fba06e779c446aae0f7c6021dd0b955ffe8',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5fresperrorcanbufferoverflow_2240',['CO_GTWA_respErrorCANbufferOverflow',['../group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fba8aa993a6c57d8db7d6355260a091e4a4',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5fresperrorcaninit_2241',['CO_GTWA_respErrorCANinit',['../group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fba97d5b22449aadae1226679986d654bbc',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5fresperrorerrorpassive_2242',['CO_GTWA_respErrorErrorPassive',['../group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fba6f9a335886d942ef028878832551afe0',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5fresperrorheartbeatlost_2243',['CO_GTWA_respErrorHeartbeatLost',['../group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fbaba7c053af330ec3ffdc8c2c298db6c93',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5fresperrorheartbeatstarted_2244',['CO_GTWA_respErrorHeartbeatStarted',['../group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fbaf4ed1e079359d86cee1ad431ed745886',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5fresperrorinternalstate_2245',['CO_GTWA_respErrorInternalState',['../group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fba1d990943991355e276f31533c841cb81',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5fresperrorlostconnection_2246',['CO_GTWA_respErrorLostConnection',['../group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fba9cc980ac35af4c1f9ad1d96719f05425',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5fresperrorlostguardingmessage_2247',['CO_GTWA_respErrorLostGuardingMessage',['../group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fba1111be206d1ebfb62caa217d6231092c',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5fresperrorlssbitratenotsupported_2248',['CO_GTWA_respErrorLSSbitRateNotSupported',['../group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fbad8ff0b3e8970d010eb084a703fa794db',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5fresperrorlssmanufacturer_2249',['CO_GTWA_respErrorLSSmanufacturer',['../group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fba7e3209f49a5ae7bad0b197110aa78512',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5fresperrorlssmediaerror_2250',['CO_GTWA_respErrorLSSmediaError',['../group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fbacc7ec1f2040bebb15becb067673e40e5',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5fresperrorlssnodeidnotsupported_2251',['CO_GTWA_respErrorLSSnodeIdNotSupported',['../group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fba6e164941b6f3a18ef6e366e7c53073b6',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5fresperrorlssparameterstoringfailed_2252',['CO_GTWA_respErrorLSSparameterStoringFailed',['../group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fba315295e9ea4cdaba0955e6d8920df5a5',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5fresperrornodefaultnetset_2253',['CO_GTWA_respErrorNoDefaultNetSet',['../group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fbad05b7717a0f79d333eed37ced2e758f9',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5fresperrornodefaultnodeset_2254',['CO_GTWA_respErrorNoDefaultNodeSet',['../group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fbac668fafa372a0cee1d50ceb0f5ed7b4a',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5fresperrornone_2255',['CO_GTWA_respErrorNone',['../group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fba46a2114a5c4d9b43babecebb11573c66',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5fresperrorpdoalreadyused_2256',['CO_GTWA_respErrorPDOalreadyUsed',['../group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fbace2f0ef57b11e716d7ca58369ff5eede',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5fresperrorpdolengthexceeded_2257',['CO_GTWA_respErrorPDOlengthExceeded',['../group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fbadd3dbd3197e1756102508208e394d72f',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5fresperrorreqnotsupported_2258',['CO_GTWA_respErrorReqNotSupported',['../group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fba04540349402c75b73af33951db285904',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5fresperrorrunningoutofmemory_2259',['CO_GTWA_respErrorRunningOutOfMemory',['../group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fba99443a03d7ba443b9beb07c6d475b350',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5fresperrorsyntax_2260',['CO_GTWA_respErrorSyntax',['../group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fba541a3546ac6be179c728272409259c98',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5fresperrortimeout_2261',['CO_GTWA_respErrorTimeOut',['../group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fba81131bf73d1ad39163a8b28e5ecf92fd',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5fresperrorunsupportednet_2262',['CO_GTWA_respErrorUnsupportedNet',['../group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fba291ec5267c935d6277c27c767e2dd178',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5fresperrorunsupportednode_2263',['CO_GTWA_respErrorUnsupportedNode',['../group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fbaf65eee2744df8e528312139a54a853d5',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5fresperrorwrongnmtstate_2264',['CO_GTWA_respErrorWrongNMTstate',['../group__CO__CANopen__309__3.html#gga92e67dec9b5e29cdd67a28651db237fbad007ed28e48b2fd7c287de3e6755a604',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5fst_5f_5flss_5ffastscan_2265',['CO_GTWA_ST__LSS_FASTSCAN',['../group__CO__CANopen__309__3.html#ggae809d7b5adbc7a4fb1f2fce527b30954ab8c2946ce5d3581ecf5640bd1f5667b7',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5fst_5fhelp_2266',['CO_GTWA_ST_HELP',['../group__CO__CANopen__309__3.html#ggae809d7b5adbc7a4fb1f2fce527b30954af2f0ce738128675b98926aba680884d1',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5fst_5fidle_2267',['CO_GTWA_ST_IDLE',['../group__CO__CANopen__309__3.html#ggae809d7b5adbc7a4fb1f2fce527b30954a98a36dabc8934b9c2d37b13999e3c393',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5fst_5fled_2268',['CO_GTWA_ST_LED',['../group__CO__CANopen__309__3.html#ggae809d7b5adbc7a4fb1f2fce527b30954a1a467cf33b8d3a2c0e8e31f87d81f05c',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5fst_5flog_2269',['CO_GTWA_ST_LOG',['../group__CO__CANopen__309__3.html#ggae809d7b5adbc7a4fb1f2fce527b30954a829f096eca2ccd152069e97d9c70022f',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5fst_5flss_5fallnodes_2270',['CO_GTWA_ST_LSS_ALLNODES',['../group__CO__CANopen__309__3.html#ggae809d7b5adbc7a4fb1f2fce527b30954a338a75317f0bf276f1c786316d6b9ec7',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5fst_5flss_5fconf_5fbitrate_2271',['CO_GTWA_ST_LSS_CONF_BITRATE',['../group__CO__CANopen__309__3.html#ggae809d7b5adbc7a4fb1f2fce527b30954a6e18980d6fbcbd7dd871af5c98c4755e',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5fst_5flss_5finquire_2272',['CO_GTWA_ST_LSS_INQUIRE',['../group__CO__CANopen__309__3.html#ggae809d7b5adbc7a4fb1f2fce527b30954abc4877f6d72e218cbd4959279a537b94',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5fst_5flss_5finquire_5faddr_5fall_2273',['CO_GTWA_ST_LSS_INQUIRE_ADDR_ALL',['../group__CO__CANopen__309__3.html#ggae809d7b5adbc7a4fb1f2fce527b30954a17254161b141cc0c424c3649655f4df2',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5fst_5flss_5fset_5fnode_2274',['CO_GTWA_ST_LSS_SET_NODE',['../group__CO__CANopen__309__3.html#ggae809d7b5adbc7a4fb1f2fce527b30954acbc1d88036a086ae9d9dcbed1742330e',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5fst_5flss_5fstore_2275',['CO_GTWA_ST_LSS_STORE',['../group__CO__CANopen__309__3.html#ggae809d7b5adbc7a4fb1f2fce527b30954a63e0705eddd22fb9d476c1b370394fc4',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5fst_5flss_5fswitch_5fglob_2276',['CO_GTWA_ST_LSS_SWITCH_GLOB',['../group__CO__CANopen__309__3.html#ggae809d7b5adbc7a4fb1f2fce527b30954afcc0bdbc08aa70d401f122c73055223e',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5fst_5flss_5fswitch_5fsel_2277',['CO_GTWA_ST_LSS_SWITCH_SEL',['../group__CO__CANopen__309__3.html#ggae809d7b5adbc7a4fb1f2fce527b30954a19bb6359e2efba37226a5dcf27d4a0e3',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5fst_5fread_2278',['CO_GTWA_ST_READ',['../group__CO__CANopen__309__3.html#ggae809d7b5adbc7a4fb1f2fce527b30954aaca55c8223aa1ba8f18031196178ea58',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5fst_5fwrite_2279',['CO_GTWA_ST_WRITE',['../group__CO__CANopen__309__3.html#ggae809d7b5adbc7a4fb1f2fce527b30954a959c96b4eb3d948a977f66679423baa2',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5fst_5fwrite_5faborted_2280',['CO_GTWA_ST_WRITE_ABORTED',['../group__CO__CANopen__309__3.html#ggae809d7b5adbc7a4fb1f2fce527b30954ab10b68af4a8bc47f96abf552c7baa3b9',1,'CO_gateway_ascii.h']]], + ['co_5fhbconsumer_5factive_2281',['CO_HBconsumer_ACTIVE',['../group__CO__HBconsumer.html#gga7658e41b7c045b7b612e4ef8a2b663f3ad80c78b38e6d28927bf3d1b1464b36e9',1,'CO_HBconsumer.h']]], + ['co_5fhbconsumer_5ftimeout_2282',['CO_HBconsumer_TIMEOUT',['../group__CO__HBconsumer.html#gga7658e41b7c045b7b612e4ef8a2b663f3a8a7ac49e5c809994ee65f365a7a75f22',1,'CO_HBconsumer.h']]], + ['co_5fhbconsumer_5funconfigured_2283',['CO_HBconsumer_UNCONFIGURED',['../group__CO__HBconsumer.html#gga7658e41b7c045b7b612e4ef8a2b663f3a4c481c1ba58fb71b9870e8b355351211',1,'CO_HBconsumer.h']]], + ['co_5fhbconsumer_5funknown_2284',['CO_HBconsumer_UNKNOWN',['../group__CO__HBconsumer.html#gga7658e41b7c045b7b612e4ef8a2b663f3af36bc5f46fd11044dc2d1d995ad8f28b',1,'CO_HBconsumer.h']]], + ['co_5finterface_5factive_2285',['CO_INTERFACE_ACTIVE',['../group__CO__socketCAN__ERROR.html#gga7bf6a56766c008531d32b4218a5a9061ad22fcb069e808548dee28e4aae580c1a',1,'CO_error.h']]], + ['co_5finterface_5fbus_5foff_2286',['CO_INTERFACE_BUS_OFF',['../group__CO__socketCAN__ERROR.html#gga7bf6a56766c008531d32b4218a5a9061a8e51897ec66a9b37865659bbc348e212',1,'CO_error.h']]], + ['co_5finterface_5flisten_5fonly_2287',['CO_INTERFACE_LISTEN_ONLY',['../group__CO__socketCAN__ERROR.html#gga7bf6a56766c008531d32b4218a5a9061a9b74584a321fe8a89cf74e087b094581',1,'CO_error.h']]], + ['co_5fled_5fblink_2288',['CO_LED_blink',['../group__CO__LEDs.html#gga366de3822a3da8478e97b248bed641fbaa24d4647d37adc17e1d3c242a42f6b68',1,'CO_LEDs.h']]], + ['co_5fled_5fcanopen_2289',['CO_LED_CANopen',['../group__CO__LEDs.html#gga366de3822a3da8478e97b248bed641fbaea338dad48dda75ef1ebac0948093148',1,'CO_LEDs.h']]], + ['co_5fled_5fflash_5f1_2290',['CO_LED_flash_1',['../group__CO__LEDs.html#gga366de3822a3da8478e97b248bed641fba88e905d94927b3c626b50a48537c7b73',1,'CO_LEDs.h']]], + ['co_5fled_5fflash_5f2_2291',['CO_LED_flash_2',['../group__CO__LEDs.html#gga366de3822a3da8478e97b248bed641fbae751e14d72d829b2b7f9a9c1e98e0612',1,'CO_LEDs.h']]], + ['co_5fled_5fflash_5f3_2292',['CO_LED_flash_3',['../group__CO__LEDs.html#gga366de3822a3da8478e97b248bed641fba250a08a71c4bbca9761f0dfa54d37938',1,'CO_LEDs.h']]], + ['co_5fled_5fflash_5f4_2293',['CO_LED_flash_4',['../group__CO__LEDs.html#gga366de3822a3da8478e97b248bed641fbadd3ec0da4d999e5ffc107d7891c26667',1,'CO_LEDs.h']]], + ['co_5fled_5fflicker_2294',['CO_LED_flicker',['../group__CO__LEDs.html#gga366de3822a3da8478e97b248bed641fba9838518b974c263a401a089901cdcf54',1,'CO_LEDs.h']]], + ['co_5flss_5fbit_5ftiming_5f10_2295',['CO_LSS_BIT_TIMING_10',['../group__CO__LSS.html#ggacb4c13e75306153eafd535e55ba0ca2ca7a3c4e05623e7f75fe3b622a9b0185c8',1,'CO_LSS.h']]], + ['co_5flss_5fbit_5ftiming_5f1000_2296',['CO_LSS_BIT_TIMING_1000',['../group__CO__LSS.html#ggacb4c13e75306153eafd535e55ba0ca2ca4cc0aa1f074ddaa5db4fdc0ed437a0b7',1,'CO_LSS.h']]], + ['co_5flss_5fbit_5ftiming_5f125_2297',['CO_LSS_BIT_TIMING_125',['../group__CO__LSS.html#ggacb4c13e75306153eafd535e55ba0ca2ca1d93a0699e4269d809404c40565133cc',1,'CO_LSS.h']]], + ['co_5flss_5fbit_5ftiming_5f20_2298',['CO_LSS_BIT_TIMING_20',['../group__CO__LSS.html#ggacb4c13e75306153eafd535e55ba0ca2ca08fb8c70b5d185521959415ef731de82',1,'CO_LSS.h']]], + ['co_5flss_5fbit_5ftiming_5f250_2299',['CO_LSS_BIT_TIMING_250',['../group__CO__LSS.html#ggacb4c13e75306153eafd535e55ba0ca2ca3fadf15163c4f45300045e3645bba3ea',1,'CO_LSS.h']]], + ['co_5flss_5fbit_5ftiming_5f50_2300',['CO_LSS_BIT_TIMING_50',['../group__CO__LSS.html#ggacb4c13e75306153eafd535e55ba0ca2caf0e90aeacc6461bd2ace73be51fd5383',1,'CO_LSS.h']]], + ['co_5flss_5fbit_5ftiming_5f500_2301',['CO_LSS_BIT_TIMING_500',['../group__CO__LSS.html#ggacb4c13e75306153eafd535e55ba0ca2caaa4d4f1a766516c6d97a81483001e8ef',1,'CO_LSS.h']]], + ['co_5flss_5fbit_5ftiming_5f800_2302',['CO_LSS_BIT_TIMING_800',['../group__CO__LSS.html#ggacb4c13e75306153eafd535e55ba0ca2ca51413e9c20e1eb60f5cc160a4d30cffa',1,'CO_LSS.h']]], + ['co_5flss_5fbit_5ftiming_5fauto_2303',['CO_LSS_BIT_TIMING_AUTO',['../group__CO__LSS.html#ggacb4c13e75306153eafd535e55ba0ca2ca12ca7fd4a3604cb8281fa7d58d1137e2',1,'CO_LSS.h']]], + ['co_5flss_5fcfg_5factivate_5fbit_5ftiming_2304',['CO_LSS_CFG_ACTIVATE_BIT_TIMING',['../group__CO__LSS.html#ggacc7cba1fb1f1f595506751d6af385964a1046863405c6de85d0d86088d9c034cc',1,'CO_LSS.h']]], + ['co_5flss_5fcfg_5fbit_5ftiming_2305',['CO_LSS_CFG_BIT_TIMING',['../group__CO__LSS.html#ggacc7cba1fb1f1f595506751d6af385964afd4b839204b66547e5ebb0e4ff9c4481',1,'CO_LSS.h']]], + ['co_5flss_5fcfg_5fbit_5ftiming_5fmanufacturer_2306',['CO_LSS_CFG_BIT_TIMING_MANUFACTURER',['../group__CO__LSS.html#gga28b8651550d1719c38cd307f4ef0a8aca3f5cbbebba617a9c12a7ed919a541255',1,'CO_LSS.h']]], + ['co_5flss_5fcfg_5fbit_5ftiming_5fok_2307',['CO_LSS_CFG_BIT_TIMING_OK',['../group__CO__LSS.html#gga28b8651550d1719c38cd307f4ef0a8aca028ded96599022c4416e3d8c0798456a',1,'CO_LSS.h']]], + ['co_5flss_5fcfg_5fbit_5ftiming_5fout_5fof_5frange_2308',['CO_LSS_CFG_BIT_TIMING_OUT_OF_RANGE',['../group__CO__LSS.html#gga28b8651550d1719c38cd307f4ef0a8acac089dd862b289dfc3f6bae0f30409625',1,'CO_LSS.h']]], + ['co_5flss_5fcfg_5fnode_5fid_2309',['CO_LSS_CFG_NODE_ID',['../group__CO__LSS.html#ggacc7cba1fb1f1f595506751d6af385964a777432f5b616a250a9db8bf7328b0a59',1,'CO_LSS.h']]], + ['co_5flss_5fcfg_5fnode_5fid_5fmanufacturer_2310',['CO_LSS_CFG_NODE_ID_MANUFACTURER',['../group__CO__LSS.html#ggaf8a13f567f8f405e4aae68268ba5d0a5aa23e0ca77dfb47ff4a1d48ddfaebc98e',1,'CO_LSS.h']]], + ['co_5flss_5fcfg_5fnode_5fid_5fok_2311',['CO_LSS_CFG_NODE_ID_OK',['../group__CO__LSS.html#ggaf8a13f567f8f405e4aae68268ba5d0a5abe91f1d0e99fa890fe69a3e60aab6c2b',1,'CO_LSS.h']]], + ['co_5flss_5fcfg_5fnode_5fid_5fout_5fof_5frange_2312',['CO_LSS_CFG_NODE_ID_OUT_OF_RANGE',['../group__CO__LSS.html#ggaf8a13f567f8f405e4aae68268ba5d0a5a79c95a7e63e6ff09fcbe5494ef59eed5',1,'CO_LSS.h']]], + ['co_5flss_5fcfg_5fstore_2313',['CO_LSS_CFG_STORE',['../group__CO__LSS.html#ggacc7cba1fb1f1f595506751d6af385964ab0a136e255e8c2c32984881487b414d9',1,'CO_LSS.h']]], + ['co_5flss_5fcfg_5fstore_5ffailed_2314',['CO_LSS_CFG_STORE_FAILED',['../group__CO__LSS.html#gga1e4e8c43143125ebe8912de81464bd9fad7f12cd5d1125e97d7b9bacac4b80d69',1,'CO_LSS.h']]], + ['co_5flss_5fcfg_5fstore_5fmanufacturer_2315',['CO_LSS_CFG_STORE_MANUFACTURER',['../group__CO__LSS.html#gga1e4e8c43143125ebe8912de81464bd9fa7ece37a5aabe812068efe6a2780f31cc',1,'CO_LSS.h']]], + ['co_5flss_5fcfg_5fstore_5fnot_5fsupported_2316',['CO_LSS_CFG_STORE_NOT_SUPPORTED',['../group__CO__LSS.html#gga1e4e8c43143125ebe8912de81464bd9faf78b03384da05bedcc45016d10dc0c3b',1,'CO_LSS.h']]], + ['co_5flss_5fcfg_5fstore_5fok_2317',['CO_LSS_CFG_STORE_OK',['../group__CO__LSS.html#gga1e4e8c43143125ebe8912de81464bd9fa0f0407dee97a1e5e5d26cc4c717103cc',1,'CO_LSS.h']]], + ['co_5flss_5ffastscan_5fbit0_2318',['CO_LSS_FASTSCAN_BIT0',['../group__CO__LSS.html#gga65751e78ae5f2674cc7205e13967f7c0a2db8b358b4954e5989a347e1e308eb20',1,'CO_LSS.h']]], + ['co_5flss_5ffastscan_5fbit31_2319',['CO_LSS_FASTSCAN_BIT31',['../group__CO__LSS.html#gga65751e78ae5f2674cc7205e13967f7c0a48f5e26f67114198d945f37f8f713979',1,'CO_LSS.h']]], + ['co_5flss_5ffastscan_5fconfirm_2320',['CO_LSS_FASTSCAN_CONFIRM',['../group__CO__LSS.html#gga65751e78ae5f2674cc7205e13967f7c0a72f67194903c03b688373ef859b66a0f',1,'CO_LSS.h']]], + ['co_5flss_5ffastscan_5fproduct_2321',['CO_LSS_FASTSCAN_PRODUCT',['../group__CO__LSS.html#gga1ce707d287b285e7d148f37f93e0f02aa6514ca82752d5496904388a0589da209',1,'CO_LSS.h']]], + ['co_5flss_5ffastscan_5frev_2322',['CO_LSS_FASTSCAN_REV',['../group__CO__LSS.html#gga1ce707d287b285e7d148f37f93e0f02aa0e153eebb470156f5d0a27caac2bc71f',1,'CO_LSS.h']]], + ['co_5flss_5ffastscan_5fserial_2323',['CO_LSS_FASTSCAN_SERIAL',['../group__CO__LSS.html#gga1ce707d287b285e7d148f37f93e0f02aabe98c25d444fa5971bc81f775cd6bb35',1,'CO_LSS.h']]], + ['co_5flss_5ffastscan_5fvendor_5fid_2324',['CO_LSS_FASTSCAN_VENDOR_ID',['../group__CO__LSS.html#gga1ce707d287b285e7d148f37f93e0f02aa4eb5786c488953cdb2d5ffbb25c15298',1,'CO_LSS.h']]], + ['co_5flss_5fident_5ffastscan_2325',['CO_LSS_IDENT_FASTSCAN',['../group__CO__LSS.html#ggacc7cba1fb1f1f595506751d6af385964a9e4bd7f4a726aee66157ac9aac446ddc',1,'CO_LSS.h']]], + ['co_5flss_5fident_5fslave_2326',['CO_LSS_IDENT_SLAVE',['../group__CO__LSS.html#ggacc7cba1fb1f1f595506751d6af385964ad0ddd8e3d472f4d85de8613e7f35902a',1,'CO_LSS.h']]], + ['co_5flss_5finquire_5fnode_5fid_2327',['CO_LSS_INQUIRE_NODE_ID',['../group__CO__LSS.html#ggacc7cba1fb1f1f595506751d6af385964af5d369fc4d3d860dc43de041b9fd59f5',1,'CO_LSS.h']]], + ['co_5flss_5finquire_5fproduct_2328',['CO_LSS_INQUIRE_PRODUCT',['../group__CO__LSS.html#ggacc7cba1fb1f1f595506751d6af385964a5fba5f8bd0f6a91b45fda117556b994c',1,'CO_LSS.h']]], + ['co_5flss_5finquire_5frev_2329',['CO_LSS_INQUIRE_REV',['../group__CO__LSS.html#ggacc7cba1fb1f1f595506751d6af385964a6f412845cd8cd4ab62a54a988ccc384c',1,'CO_LSS.h']]], + ['co_5flss_5finquire_5fserial_2330',['CO_LSS_INQUIRE_SERIAL',['../group__CO__LSS.html#ggacc7cba1fb1f1f595506751d6af385964a55f0698aa15abf17c41cd344df055184',1,'CO_LSS.h']]], + ['co_5flss_5finquire_5fvendor_2331',['CO_LSS_INQUIRE_VENDOR',['../group__CO__LSS.html#ggacc7cba1fb1f1f595506751d6af385964ae28351cd1b60bbdd045a9f79cb506023',1,'CO_LSS.h']]], + ['co_5flss_5fstate_5fconfiguration_2332',['CO_LSS_STATE_CONFIGURATION',['../group__CO__LSS.html#ggaaa9a270e40ea09850e1661e5aeb080ada69cc3fe20e50dcd6f7bad8c0b887ff89',1,'CO_LSS.h']]], + ['co_5flss_5fstate_5fwaiting_2333',['CO_LSS_STATE_WAITING',['../group__CO__LSS.html#ggaaa9a270e40ea09850e1661e5aeb080adac688ea4e90b7dfac437a44536f2af8db',1,'CO_LSS.h']]], + ['co_5flss_5fswitch_5fstate_5fglobal_2334',['CO_LSS_SWITCH_STATE_GLOBAL',['../group__CO__LSS.html#ggacc7cba1fb1f1f595506751d6af385964a6c9f8aaef024d11e50c007b881208113',1,'CO_LSS.h']]], + ['co_5flss_5fswitch_5fstate_5fsel_2335',['CO_LSS_SWITCH_STATE_SEL',['../group__CO__LSS.html#ggacc7cba1fb1f1f595506751d6af385964a97495fe55645d498550e0c05417e2c22',1,'CO_LSS.h']]], + ['co_5flss_5fswitch_5fstate_5fsel_5fproduct_2336',['CO_LSS_SWITCH_STATE_SEL_PRODUCT',['../group__CO__LSS.html#ggacc7cba1fb1f1f595506751d6af385964a9caf25d2d4a28e279b1bc364d303ee7d',1,'CO_LSS.h']]], + ['co_5flss_5fswitch_5fstate_5fsel_5frev_2337',['CO_LSS_SWITCH_STATE_SEL_REV',['../group__CO__LSS.html#ggacc7cba1fb1f1f595506751d6af385964ae02d526a2c1170babeccffd00d477db5',1,'CO_LSS.h']]], + ['co_5flss_5fswitch_5fstate_5fsel_5fserial_2338',['CO_LSS_SWITCH_STATE_SEL_SERIAL',['../group__CO__LSS.html#ggacc7cba1fb1f1f595506751d6af385964add363bec1d5ff239c847425f8b94718d',1,'CO_LSS.h']]], + ['co_5flss_5fswitch_5fstate_5fsel_5fvendor_2339',['CO_LSS_SWITCH_STATE_SEL_VENDOR',['../group__CO__LSS.html#ggacc7cba1fb1f1f595506751d6af385964a620895a76069780eb5df8188b6c8a2de',1,'CO_LSS.h']]], + ['co_5flssmaster_5ffs_5fmatch_2340',['CO_LSSmaster_FS_MATCH',['../group__CO__LSSmaster.html#gga6e3f0d07f0712c371fb81cbf4a3dbcb1ad5a936fd00345aea75e8917f99df4ab3',1,'CO_LSSmaster.h']]], + ['co_5flssmaster_5ffs_5fscan_2341',['CO_LSSmaster_FS_SCAN',['../group__CO__LSSmaster.html#gga6e3f0d07f0712c371fb81cbf4a3dbcb1a5666c53e2fc02e214294bd3210146c90',1,'CO_LSSmaster.h']]], + ['co_5flssmaster_5ffs_5fskip_2342',['CO_LSSmaster_FS_SKIP',['../group__CO__LSSmaster.html#gga6e3f0d07f0712c371fb81cbf4a3dbcb1ae0e6e92e760129fc173a0a4f19a0cf07',1,'CO_LSSmaster.h']]], + ['co_5flssmaster_5fillegal_5fargument_2343',['CO_LSSmaster_ILLEGAL_ARGUMENT',['../group__CO__LSSmaster.html#ggae848ff3ff649c8a23b96053efaea4985a9bd04e1c9923416d1d1ecb1ded6bc7b9',1,'CO_LSSmaster.h']]], + ['co_5flssmaster_5finvalid_5fstate_2344',['CO_LSSmaster_INVALID_STATE',['../group__CO__LSSmaster.html#ggae848ff3ff649c8a23b96053efaea4985a1e99145e94c3a6fb8cc7c48150ed5b60',1,'CO_LSSmaster.h']]], + ['co_5flssmaster_5fok_2345',['CO_LSSmaster_OK',['../group__CO__LSSmaster.html#ggae848ff3ff649c8a23b96053efaea4985a1be85610f29d8e6cecebac9db2da3099',1,'CO_LSSmaster.h']]], + ['co_5flssmaster_5fok_5fillegal_5fargument_2346',['CO_LSSmaster_OK_ILLEGAL_ARGUMENT',['../group__CO__LSSmaster.html#ggae848ff3ff649c8a23b96053efaea4985ab6985b2cbcb24653ec1f2ae46d3c09cd',1,'CO_LSSmaster.h']]], + ['co_5flssmaster_5fok_5fmanufacturer_2347',['CO_LSSmaster_OK_MANUFACTURER',['../group__CO__LSSmaster.html#ggae848ff3ff649c8a23b96053efaea4985a51083a5cf04e03a2f623eddb1d7324c7',1,'CO_LSSmaster.h']]], + ['co_5flssmaster_5fscan_5ffailed_2348',['CO_LSSmaster_SCAN_FAILED',['../group__CO__LSSmaster.html#ggae848ff3ff649c8a23b96053efaea4985af84e0db6bd9aeebffc4266618145a8ea',1,'CO_LSSmaster.h']]], + ['co_5flssmaster_5fscan_5ffinished_2349',['CO_LSSmaster_SCAN_FINISHED',['../group__CO__LSSmaster.html#ggae848ff3ff649c8a23b96053efaea4985a0f527d1ce01820fa184ccae2510505c7',1,'CO_LSSmaster.h']]], + ['co_5flssmaster_5fscan_5fnoack_2350',['CO_LSSmaster_SCAN_NOACK',['../group__CO__LSSmaster.html#ggae848ff3ff649c8a23b96053efaea4985ae9eb103f25dbe694143a64e7bb2c29d9',1,'CO_LSSmaster.h']]], + ['co_5flssmaster_5ftimeout_2351',['CO_LSSmaster_TIMEOUT',['../group__CO__LSSmaster.html#ggae848ff3ff649c8a23b96053efaea4985a3a61073ef2c8ef7c5be946618b95d42d',1,'CO_LSSmaster.h']]], + ['co_5flssmaster_5fwait_5fslave_2352',['CO_LSSmaster_WAIT_SLAVE',['../group__CO__LSSmaster.html#ggae848ff3ff649c8a23b96053efaea4985a20fbe514e36bd92534141bb75e68eb34',1,'CO_LSSmaster.h']]], + ['co_5fnmt_5fenter_5foperational_2353',['CO_NMT_ENTER_OPERATIONAL',['../group__CO__NMT__Heartbeat.html#ggac396242e2e12ef6b0b22ff48636bc4eba6c7cdae79e5939cf321cdf82aa6b4146',1,'CO_NMT_Heartbeat.h']]], + ['co_5fnmt_5fenter_5fpre_5foperational_2354',['CO_NMT_ENTER_PRE_OPERATIONAL',['../group__CO__NMT__Heartbeat.html#ggac396242e2e12ef6b0b22ff48636bc4eba575ed4f3386e8a63498921e659aa7ba4',1,'CO_NMT_Heartbeat.h']]], + ['co_5fnmt_5fenter_5fstopped_2355',['CO_NMT_ENTER_STOPPED',['../group__CO__NMT__Heartbeat.html#ggac396242e2e12ef6b0b22ff48636bc4eba8a2911a2a576a1bd83b7f4a247cd77c9',1,'CO_NMT_Heartbeat.h']]], + ['co_5fnmt_5ferr_5ffree_5fto_5foperational_2356',['CO_NMT_ERR_FREE_TO_OPERATIONAL',['../group__CO__NMT__Heartbeat.html#ggaf92cf5943801e5dda84654345cc3d67fa6e79d137c725417786035982cd0e2687',1,'CO_NMT_Heartbeat.h']]], + ['co_5fnmt_5ferr_5fon_5fbusoff_5fhb_2357',['CO_NMT_ERR_ON_BUSOFF_HB',['../group__CO__NMT__Heartbeat.html#ggaf92cf5943801e5dda84654345cc3d67faf151918ecf07fc43ba30489d31ca275a',1,'CO_NMT_Heartbeat.h']]], + ['co_5fnmt_5ferr_5fon_5ferr_5freg_2358',['CO_NMT_ERR_ON_ERR_REG',['../group__CO__NMT__Heartbeat.html#ggaf92cf5943801e5dda84654345cc3d67faab1cd82001a087114a5361910e206c74',1,'CO_NMT_Heartbeat.h']]], + ['co_5fnmt_5ferr_5freg_5fmask_2359',['CO_NMT_ERR_REG_MASK',['../group__CO__NMT__Heartbeat.html#ggaf92cf5943801e5dda84654345cc3d67faaa5ab06e2047e51ac7d2767e8a821d80',1,'CO_NMT_Heartbeat.h']]], + ['co_5fnmt_5ferr_5fto_5fstopped_2360',['CO_NMT_ERR_TO_STOPPED',['../group__CO__NMT__Heartbeat.html#ggaf92cf5943801e5dda84654345cc3d67fa9498a5af4e0557a2c0284c04b26b3eb5',1,'CO_NMT_Heartbeat.h']]], + ['co_5fnmt_5finitializing_2361',['CO_NMT_INITIALIZING',['../group__CO__NMT__Heartbeat.html#gga1e8c2a6c0fd4a33183503d25a7c6d744a672c48c0e9bd5cb070bd86e1dcdc085c',1,'CO_NMT_Heartbeat.h']]], + ['co_5fnmt_5foperational_2362',['CO_NMT_OPERATIONAL',['../group__CO__NMT__Heartbeat.html#gga1e8c2a6c0fd4a33183503d25a7c6d744a3f9d07a10f2781201b85e9de9ba1c9a5',1,'CO_NMT_Heartbeat.h']]], + ['co_5fnmt_5fpre_5foperational_2363',['CO_NMT_PRE_OPERATIONAL',['../group__CO__NMT__Heartbeat.html#gga1e8c2a6c0fd4a33183503d25a7c6d744ae41d4ebcb7e26e165c8685b5809871a9',1,'CO_NMT_Heartbeat.h']]], + ['co_5fnmt_5freset_5fcommunication_2364',['CO_NMT_RESET_COMMUNICATION',['../group__CO__NMT__Heartbeat.html#ggac396242e2e12ef6b0b22ff48636bc4ebabc6aef9d84c816eecaea38f9fe2cbd9c',1,'CO_NMT_Heartbeat.h']]], + ['co_5fnmt_5freset_5fnode_2365',['CO_NMT_RESET_NODE',['../group__CO__NMT__Heartbeat.html#ggac396242e2e12ef6b0b22ff48636bc4ebaa8558d18a80148a598e6295ef41cdf97',1,'CO_NMT_Heartbeat.h']]], + ['co_5fnmt_5fstartup_5fto_5foperational_2366',['CO_NMT_STARTUP_TO_OPERATIONAL',['../group__CO__NMT__Heartbeat.html#ggaf92cf5943801e5dda84654345cc3d67faf4da9fa5d805e0f535abf95a175312e8',1,'CO_NMT_Heartbeat.h']]], + ['co_5fnmt_5fstopped_2367',['CO_NMT_STOPPED',['../group__CO__NMT__Heartbeat.html#gga1e8c2a6c0fd4a33183503d25a7c6d744aba34c0b4b2c6a0c11b01236cbe949ed5',1,'CO_NMT_Heartbeat.h']]], + ['co_5fnmt_5funknown_2368',['CO_NMT_UNKNOWN',['../group__CO__NMT__Heartbeat.html#gga1e8c2a6c0fd4a33183503d25a7c6d744a52d111d4262b514df4433ae07969cc3c',1,'CO_NMT_Heartbeat.h']]], + ['co_5freset_5fapp_2369',['CO_RESET_APP',['../group__CO__NMT__Heartbeat.html#ggaf42f056a571b8e17a2d74428d1a49674a2ab187bf1daccd4bc31685c08ca93bf1',1,'CO_NMT_Heartbeat.h']]], + ['co_5freset_5fcomm_2370',['CO_RESET_COMM',['../group__CO__NMT__Heartbeat.html#ggaf42f056a571b8e17a2d74428d1a49674a2a48dc2be7c9ffa7874eb08a1ba39f6f',1,'CO_NMT_Heartbeat.h']]], + ['co_5freset_5fnot_2371',['CO_RESET_NOT',['../group__CO__NMT__Heartbeat.html#ggaf42f056a571b8e17a2d74428d1a49674a1e75deb437b2e06b2c1f353bf76d8807',1,'CO_NMT_Heartbeat.h']]], + ['co_5freset_5fquit_2372',['CO_RESET_QUIT',['../group__CO__NMT__Heartbeat.html#ggaf42f056a571b8e17a2d74428d1a49674ae2c7805e42ce9b7c893018922b24a102',1,'CO_NMT_Heartbeat.h']]], + ['co_5fsdo_5fab_5fblock_5fsize_2373',['CO_SDO_AB_BLOCK_SIZE',['../group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979ac86b70b71d601658c93a1dd270a902b0',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fab_5fcmd_2374',['CO_SDO_AB_CMD',['../group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979a26b4e2680c16ce6a09d3e3a8293472ce',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fab_5fcrc_2375',['CO_SDO_AB_CRC',['../group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979aee7fcab60a6fde6e41d999f5a2b10aa5',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fab_5fdata_5fdev_5fstate_2376',['CO_SDO_AB_DATA_DEV_STATE',['../group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979ac92ccaa16d833cac6d2f6d8c2836d886',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fab_5fdata_5floc_5fctrl_2377',['CO_SDO_AB_DATA_LOC_CTRL',['../group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979ac489bb77a98f65008932861924bc4bbf',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fab_5fdata_5flong_2378',['CO_SDO_AB_DATA_LONG',['../group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979a50d373f7a7ba976dc2277a2111cf56c3',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fab_5fdata_5fod_2379',['CO_SDO_AB_DATA_OD',['../group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979aec1840b00621e92f27da2d0705ddab63',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fab_5fdata_5fshort_2380',['CO_SDO_AB_DATA_SHORT',['../group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979a481537e4c170066ca31b167fa598bb54',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fab_5fdata_5ftransf_2381',['CO_SDO_AB_DATA_TRANSF',['../group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979a631a043a79c7eef4ddb2f874365c6660',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fab_5fdevice_5fincompat_2382',['CO_SDO_AB_DEVICE_INCOMPAT',['../group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979ad07acc06f76122627412a71f2f2e39fc',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fab_5fgeneral_2383',['CO_SDO_AB_GENERAL',['../group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979a58d6be7d156bbe576b8438a6fd5b446d',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fab_5fhw_2384',['CO_SDO_AB_HW',['../group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979a070f096bb09f5a6235643702b5a40759',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fab_5finvalid_5fvalue_2385',['CO_SDO_AB_INVALID_VALUE',['../group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979afff1ec491c628031e65672383f3e3c76',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fab_5fmap_5flen_2386',['CO_SDO_AB_MAP_LEN',['../group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979a07edee9ce8ec5cd01cfd3cfbff48b96c',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fab_5fmax_5fless_5fmin_2387',['CO_SDO_AB_MAX_LESS_MIN',['../group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979a15d49829c0d15f8cb9995f07617d874f',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fab_5fno_5fdata_2388',['CO_SDO_AB_NO_DATA',['../group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979a3e007eeec7538b5dbe7e78240632b415',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fab_5fno_5fmap_2389',['CO_SDO_AB_NO_MAP',['../group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979a729452df9557e4acbda8691efb4da310',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fab_5fno_5fresource_2390',['CO_SDO_AB_NO_RESOURCE',['../group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979ab54dd042727804cd8f310a04fd4575f7',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fab_5fnone_2391',['CO_SDO_AB_NONE',['../group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979a5fc84558a4ca47e067189a14543691b6',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fab_5fnot_5fexist_2392',['CO_SDO_AB_NOT_EXIST',['../group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979a6ef5b921ac0f299f34e9860eb82e332e',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fab_5fout_5fof_5fmem_2393',['CO_SDO_AB_OUT_OF_MEM',['../group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979adc021e79ace03edbd279a3c492853c7f',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fab_5fpram_5fincompat_2394',['CO_SDO_AB_PRAM_INCOMPAT',['../group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979acaedcf71c4638efb40fc6debfa9dba67',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fab_5freadonly_2395',['CO_SDO_AB_READONLY',['../group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979a48c8a5f4939372564a17b31f992b82a4',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fab_5fseq_5fnum_2396',['CO_SDO_AB_SEQ_NUM',['../group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979a778ef6b5751cb8ba10b67436409c3fd2',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fab_5fsub_5funknown_2397',['CO_SDO_AB_SUB_UNKNOWN',['../group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979a3e48e535fddeaa78a4059c2f91f9bb8e',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fab_5ftimeout_2398',['CO_SDO_AB_TIMEOUT',['../group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979a036d0be874d10f66aa6601d76a9aa2f0',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fab_5ftoggle_5fbit_2399',['CO_SDO_AB_TOGGLE_BIT',['../group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979ad4e9214eab1d034e9c10eb6c7638e592',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fab_5ftype_5fmismatch_2400',['CO_SDO_AB_TYPE_MISMATCH',['../group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979a838c274eaa14626514da8f7a8ac043c3',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fab_5funsupported_5faccess_2401',['CO_SDO_AB_UNSUPPORTED_ACCESS',['../group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979a370ff72a5bddee5760ba0930c3b13ba0',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fab_5fvalue_5fhigh_2402',['CO_SDO_AB_VALUE_HIGH',['../group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979a4983bce8e9503f9e7a720a44528036ad',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fab_5fvalue_5flow_2403',['CO_SDO_AB_VALUE_LOW',['../group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979ab402816165086fbad21a130e9f488d52',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fab_5fwriteonly_2404',['CO_SDO_AB_WRITEONLY',['../group__CO__SDOserver.html#gga7587ddcf798747fe6d97d03bf1bf2979a457e80af0f952c272fa90ebd45cdb8cd',1,'CO_SDOserver.h']]], + ['co_5fsdo_5frt_5fblockdownldinprogress_2405',['CO_SDO_RT_blockDownldInProgress',['../group__CO__SDOserver.html#gga7f729ab203285c7623df493916f22a73aa8036db7f41e8958c057da0d4ab24f8f',1,'CO_SDOserver.h']]], + ['co_5fsdo_5frt_5fblockuploadinprogress_2406',['CO_SDO_RT_blockUploadInProgress',['../group__CO__SDOserver.html#gga7f729ab203285c7623df493916f22a73ad73a50f4a1d7ef69797cbf7c930293f2',1,'CO_SDOserver.h']]], + ['co_5fsdo_5frt_5fendedwithclientabort_2407',['CO_SDO_RT_endedWithClientAbort',['../group__CO__SDOserver.html#gga7f729ab203285c7623df493916f22a73a9aafefd96d032c1b65cb6c23bc53f0aa',1,'CO_SDOserver.h']]], + ['co_5fsdo_5frt_5fendedwithserverabort_2408',['CO_SDO_RT_endedWithServerAbort',['../group__CO__SDOserver.html#gga7f729ab203285c7623df493916f22a73ae2fce3f477766eb188502886705dc177',1,'CO_SDOserver.h']]], + ['co_5fsdo_5frt_5fok_5fcommunicationend_2409',['CO_SDO_RT_ok_communicationEnd',['../group__CO__SDOserver.html#gga7f729ab203285c7623df493916f22a73a2d0d1d8d1bc297205b3e87174642199c',1,'CO_SDOserver.h']]], + ['co_5fsdo_5frt_5ftransmittbufferfull_2410',['CO_SDO_RT_transmittBufferFull',['../group__CO__SDOserver.html#gga7f729ab203285c7623df493916f22a73ad62e2421dcee78ba0477fb379a6e7e4e',1,'CO_SDOserver.h']]], + ['co_5fsdo_5frt_5fuploaddatabufferfull_2411',['CO_SDO_RT_uploadDataBufferFull',['../group__CO__SDOserver.html#gga7f729ab203285c7623df493916f22a73ada069dad6b1e0bec180600b1d34758d2',1,'CO_SDOserver.h']]], + ['co_5fsdo_5frt_5fwaitinglocaltransfer_2412',['CO_SDO_RT_waitingLocalTransfer',['../group__CO__SDOserver.html#gga7f729ab203285c7623df493916f22a73ab9191f8a57b840a81457591f0fbd8a76',1,'CO_SDOserver.h']]], + ['co_5fsdo_5frt_5fwaitingresponse_2413',['CO_SDO_RT_waitingResponse',['../group__CO__SDOserver.html#gga7f729ab203285c7623df493916f22a73a15d85fc411d0c6e69888c2ec9d641eb5',1,'CO_SDOserver.h']]], + ['co_5fsdo_5frt_5fwrongarguments_2414',['CO_SDO_RT_wrongArguments',['../group__CO__SDOserver.html#gga7f729ab203285c7623df493916f22a73af1dc6a56b2b38fb5f4c878661173decc',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fst_5fabort_2415',['CO_SDO_ST_ABORT',['../group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11ac40cb6c0b2f2eb1877aee3963dc1927d',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fst_5fdownload_5fblk_5fend_5freq_2416',['CO_SDO_ST_DOWNLOAD_BLK_END_REQ',['../group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11af955593bb966b324bfda361b0364d15b',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fst_5fdownload_5fblk_5fend_5frsp_2417',['CO_SDO_ST_DOWNLOAD_BLK_END_RSP',['../group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11af511c26db1fb7ba18d6054255b560be7',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fst_5fdownload_5fblk_5finitiate_5freq_2418',['CO_SDO_ST_DOWNLOAD_BLK_INITIATE_REQ',['../group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11a5d4ead9d3f06962987b6af8c073b6a2e',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fst_5fdownload_5fblk_5finitiate_5frsp_2419',['CO_SDO_ST_DOWNLOAD_BLK_INITIATE_RSP',['../group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11af25ee4e636a98dd72fe4c5bef9bcecf2',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fst_5fdownload_5fblk_5fsubblock_5freq_2420',['CO_SDO_ST_DOWNLOAD_BLK_SUBBLOCK_REQ',['../group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11a320cc9749db35473265b5203c547bbf8',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fst_5fdownload_5fblk_5fsubblock_5frsp_2421',['CO_SDO_ST_DOWNLOAD_BLK_SUBBLOCK_RSP',['../group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11a70e97f34a6a98014bef1d2eeb3b5247c',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fst_5fdownload_5finitiate_5freq_2422',['CO_SDO_ST_DOWNLOAD_INITIATE_REQ',['../group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11ac07432ccfaa6be8730cc8c306b3e42bb',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fst_5fdownload_5finitiate_5frsp_2423',['CO_SDO_ST_DOWNLOAD_INITIATE_RSP',['../group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11a49b060ebf39c4bfb498b8691c16bb882',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fst_5fdownload_5flocal_5ftransfer_2424',['CO_SDO_ST_DOWNLOAD_LOCAL_TRANSFER',['../group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11a8f685c4d233c35defb423fda8ff5544c',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fst_5fdownload_5fsegment_5freq_2425',['CO_SDO_ST_DOWNLOAD_SEGMENT_REQ',['../group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11a6b44777e7e209313612baab5f83745ff',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fst_5fdownload_5fsegment_5frsp_2426',['CO_SDO_ST_DOWNLOAD_SEGMENT_RSP',['../group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11ae5b55aec51372cbc2a6e32ce1456c11c',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fst_5fidle_2427',['CO_SDO_ST_IDLE',['../group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11a2eee38ba2a2d52890281ae54b12d50b3',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fst_5fupload_5fblk_5fend_5fcrsp_2428',['CO_SDO_ST_UPLOAD_BLK_END_CRSP',['../group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11ab75a573a45778a0c4bea2c50402be03e',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fst_5fupload_5fblk_5fend_5fsreq_2429',['CO_SDO_ST_UPLOAD_BLK_END_SREQ',['../group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11a98896138e97542e659051fff33b1a692',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fst_5fupload_5fblk_5finitiate_5freq_2430',['CO_SDO_ST_UPLOAD_BLK_INITIATE_REQ',['../group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11a49b5c39c9e5d025c85eedffa28aa22ed',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fst_5fupload_5fblk_5finitiate_5freq2_2431',['CO_SDO_ST_UPLOAD_BLK_INITIATE_REQ2',['../group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11acc4e87ad1ad20eddd19a60d9592bbada',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fst_5fupload_5fblk_5finitiate_5frsp_2432',['CO_SDO_ST_UPLOAD_BLK_INITIATE_RSP',['../group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11ae9be0eeb0711890d1b9c5cbfbd204ed8',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fst_5fupload_5fblk_5fsubblock_5fcrsp_2433',['CO_SDO_ST_UPLOAD_BLK_SUBBLOCK_CRSP',['../group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11af762eb5a985cf79a3e7423a39b29b328',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fst_5fupload_5fblk_5fsubblock_5fsreq_2434',['CO_SDO_ST_UPLOAD_BLK_SUBBLOCK_SREQ',['../group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11a39f1cb5426ee3c3689ed833cb66e231c',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fst_5fupload_5finitiate_5freq_2435',['CO_SDO_ST_UPLOAD_INITIATE_REQ',['../group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11aa8a8b5050c6528fdaa19bbb429d8e4f4',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fst_5fupload_5finitiate_5frsp_2436',['CO_SDO_ST_UPLOAD_INITIATE_RSP',['../group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11aa096d10c9eb891cfedddc16276f58aaf',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fst_5fupload_5flocal_5ftransfer_2437',['CO_SDO_ST_UPLOAD_LOCAL_TRANSFER',['../group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11aa07fe53d69ec7e0d56db39111867f8ce',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fst_5fupload_5fsegment_5freq_2438',['CO_SDO_ST_UPLOAD_SEGMENT_REQ',['../group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11ad610c289b85192d70c835b033b49b3fb',1,'CO_SDOserver.h']]], + ['co_5fsdo_5fst_5fupload_5fsegment_5frsp_2439',['CO_SDO_ST_UPLOAD_SEGMENT_RSP',['../group__CO__SDOserver.html#gga0b0e614dadcc1c005185b8bc9a7fec11a210a3eb6acfdb055bb72a59d8e24a6b6',1,'CO_SDOserver.h']]], + ['co_5fsync_5fnone_2440',['CO_SYNC_NONE',['../group__CO__SYNC.html#gga121ede6e0c90c66076a7ed950db38517aeff7846423b9eb92cd2c69df745ea429',1,'CO_SYNC.h']]], + ['co_5fsync_5foutside_5fwindow_2441',['CO_SYNC_OUTSIDE_WINDOW',['../group__CO__SYNC.html#gga121ede6e0c90c66076a7ed950db38517a5b112bec6cb10119879703a6313de41e',1,'CO_SYNC.h']]], + ['co_5fsync_5freceived_2442',['CO_SYNC_RECEIVED',['../group__CO__SYNC.html#gga121ede6e0c90c66076a7ed950db38517aa03c21a78b503a4adebb2d9d7aa655bf',1,'CO_SYNC.h']]] +]; diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/enumvalues_1.html b/Devices/Libraries/Systems/CANopenSocket/docs/search/enumvalues_1.html new file mode 100755 index 0000000..e22a79f --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/enumvalues_1.html @@ -0,0 +1,30 @@ + + + + + + + + + +
+
Loading...
+
+ +
Searching...
+
No Matches
+ +
+ + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/enumvalues_1.js b/Devices/Libraries/Systems/CANopenSocket/docs/search/enumvalues_1.js new file mode 100755 index 0000000..1ab7d3a --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/enumvalues_1.js @@ -0,0 +1,97 @@ +var searchData= +[ + ['od_5fh1000_5fdev_5ftype_2443',['OD_H1000_DEV_TYPE',['../group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886af1e65ef6eb730b9302540e0ba44852b1',1,'CO_ODinterface.h']]], + ['od_5fh1001_5ferr_5freg_2444',['OD_H1001_ERR_REG',['../group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886af4eb4e0204ae9696f935af5d4fdcff7e',1,'CO_ODinterface.h']]], + ['od_5fh1002_5fmanuf_5fstatus_5freg_2445',['OD_H1002_MANUF_STATUS_REG',['../group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a2cca52f61d70db5ca2aaa168b32f3aaf',1,'CO_ODinterface.h']]], + ['od_5fh1003_5fpredef_5ferr_5ffield_2446',['OD_H1003_PREDEF_ERR_FIELD',['../group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886adfc213af5af80cf037231621132013fb',1,'CO_ODinterface.h']]], + ['od_5fh1004_5frsv_2447',['OD_H1004_RSV',['../group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886ad7d53fa95504566811bdf0683f645ccd',1,'CO_ODinterface.h']]], + ['od_5fh1005_5fcobid_5fsync_2448',['OD_H1005_COBID_SYNC',['../group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886adcbc9ec0c547b00db2b0403708becb97',1,'CO_ODinterface.h']]], + ['od_5fh1006_5fcomm_5fcycl_5fperiod_2449',['OD_H1006_COMM_CYCL_PERIOD',['../group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a463dab19c27811dd6de51fcc082b565b',1,'CO_ODinterface.h']]], + ['od_5fh1007_5fsync_5fwindow_5flen_2450',['OD_H1007_SYNC_WINDOW_LEN',['../group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a41f7f0f96dbea4fb0d6bd2bbbd2d59dc',1,'CO_ODinterface.h']]], + ['od_5fh1008_5fmanuf_5fdev_5fname_2451',['OD_H1008_MANUF_DEV_NAME',['../group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886aa282ef78e8a64ff527c79218d23168f0',1,'CO_ODinterface.h']]], + ['od_5fh1009_5fmanuf_5fhw_5fversion_2452',['OD_H1009_MANUF_HW_VERSION',['../group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886ad91534dd5cc5f382287b9a392c744948',1,'CO_ODinterface.h']]], + ['od_5fh100a_5fmanuf_5fsw_5fversion_2453',['OD_H100A_MANUF_SW_VERSION',['../group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a8bd48d9bb18d1c291249050818c82a57',1,'CO_ODinterface.h']]], + ['od_5fh100b_5frsv_2454',['OD_H100B_RSV',['../group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a561260a506655cf0b7df9d684a08b5be',1,'CO_ODinterface.h']]], + ['od_5fh100c_5fguard_5ftime_2455',['OD_H100C_GUARD_TIME',['../group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886adf00d5b448274a91940cac15b8e22fc5',1,'CO_ODinterface.h']]], + ['od_5fh100d_5flifetime_5ffactor_2456',['OD_H100D_LIFETIME_FACTOR',['../group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a4bfbb36a82606125d52fbe4daff6b5fb',1,'CO_ODinterface.h']]], + ['od_5fh100e_5frsv_2457',['OD_H100E_RSV',['../group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a4d831b2a36d679d31982e35ca38f8f6e',1,'CO_ODinterface.h']]], + ['od_5fh100f_5frsv_2458',['OD_H100F_RSV',['../group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a7d6a3f1ca8f72bf808ee5fe341f2acca',1,'CO_ODinterface.h']]], + ['od_5fh1010_5fstore_5fparam_5ffunc_2459',['OD_H1010_STORE_PARAM_FUNC',['../group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a0f8c3db5a62d5e4df59d83253b69b0f2',1,'CO_ODinterface.h']]], + ['od_5fh1011_5frest_5fparam_5ffunc_2460',['OD_H1011_REST_PARAM_FUNC',['../group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886adad8c0ea18f674f3eb61b43e8259395c',1,'CO_ODinterface.h']]], + ['od_5fh1012_5fcobid_5ftime_2461',['OD_H1012_COBID_TIME',['../group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a5781f519d9ec08fd4389c4761754a4e6',1,'CO_ODinterface.h']]], + ['od_5fh1013_5fhigh_5fres_5ftimestamp_2462',['OD_H1013_HIGH_RES_TIMESTAMP',['../group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a7aa32b2b6df7c4d4354599ef2fd2ca29',1,'CO_ODinterface.h']]], + ['od_5fh1014_5fcobid_5femergency_2463',['OD_H1014_COBID_EMERGENCY',['../group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a5cae036f3dd0bc1861dcea7c9a83c6d5',1,'CO_ODinterface.h']]], + ['od_5fh1015_5finhibit_5ftime_5femcy_2464',['OD_H1015_INHIBIT_TIME_EMCY',['../group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886adbcd0d36ab781fc60b05a94288b8f019',1,'CO_ODinterface.h']]], + ['od_5fh1016_5fconsumer_5fhb_5ftime_2465',['OD_H1016_CONSUMER_HB_TIME',['../group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a996952cb963ce6e2783a6fa915d85612',1,'CO_ODinterface.h']]], + ['od_5fh1017_5fproducer_5fhb_5ftime_2466',['OD_H1017_PRODUCER_HB_TIME',['../group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886ab2f428825c1127b286f5b8ace5e881b2',1,'CO_ODinterface.h']]], + ['od_5fh1018_5fidentity_5fobject_2467',['OD_H1018_IDENTITY_OBJECT',['../group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886aabb7a688852e453c5535f663be6298d2',1,'CO_ODinterface.h']]], + ['od_5fh1019_5fsync_5fcnt_5foverflow_2468',['OD_H1019_SYNC_CNT_OVERFLOW',['../group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886aa966d2e020222331b18c5b08261acbf0',1,'CO_ODinterface.h']]], + ['od_5fh1020_5fverify_5fconfig_2469',['OD_H1020_VERIFY_CONFIG',['../group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a8c740bbdd0cb98200d402ec6272d7e8b',1,'CO_ODinterface.h']]], + ['od_5fh1021_5fstore_5feds_2470',['OD_H1021_STORE_EDS',['../group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a259737f7e9ef239d85cb9e7bdeda550b',1,'CO_ODinterface.h']]], + ['od_5fh1022_5fstore_5fformat_2471',['OD_H1022_STORE_FORMAT',['../group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a4061d54d1c1583fd178566a3915bcefe',1,'CO_ODinterface.h']]], + ['od_5fh1023_5fos_5fcmd_2472',['OD_H1023_OS_CMD',['../group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a12ba9d8cdfc20b9ff66167a8d1e5b21c',1,'CO_ODinterface.h']]], + ['od_5fh1024_5fos_5fcmd_5fmode_2473',['OD_H1024_OS_CMD_MODE',['../group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886ad7e64256615fcda5b531063eeaa346de',1,'CO_ODinterface.h']]], + ['od_5fh1025_5fos_5fdbg_5finterface_2474',['OD_H1025_OS_DBG_INTERFACE',['../group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886aaf890d86468408b0dbe8353a3b270156',1,'CO_ODinterface.h']]], + ['od_5fh1026_5fos_5fprompt_2475',['OD_H1026_OS_PROMPT',['../group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a6e3b80d148d22f129ed388fad9aaf398',1,'CO_ODinterface.h']]], + ['od_5fh1027_5fmodule_5flist_2476',['OD_H1027_MODULE_LIST',['../group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a668cdf3b0102b753858b9bfeb7efdc1c',1,'CO_ODinterface.h']]], + ['od_5fh1028_5femcy_5fconsumer_2477',['OD_H1028_EMCY_CONSUMER',['../group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a6f093f4fdeaac7b723305fd8d2ce40c1',1,'CO_ODinterface.h']]], + ['od_5fh1029_5ferr_5fbehavior_2478',['OD_H1029_ERR_BEHAVIOR',['../group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a67d50722cc629ee8c2a90a123ee41fa3',1,'CO_ODinterface.h']]], + ['od_5fh1200_5fsdo_5fserver_5f1_5fparam_2479',['OD_H1200_SDO_SERVER_1_PARAM',['../group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a7e351a37cb53541b510e97711e167450',1,'CO_ODinterface.h']]], + ['od_5fh1280_5fsdo_5fclient_5f1_5fparam_2480',['OD_H1280_SDO_CLIENT_1_PARAM',['../group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a3e0e0989da485b0511d7e7cadcdb6dbb',1,'CO_ODinterface.h']]], + ['od_5fh1300_5fgfc_5fparam_2481',['OD_H1300_GFC_PARAM',['../group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886afa584e69f73b7a80cd708010fc0ae64f',1,'CO_ODinterface.h']]], + ['od_5fh1301_5fsrdo_5f1_5fparam_2482',['OD_H1301_SRDO_1_PARAM',['../group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886afbb7d48d240c7584d461712685e62945',1,'CO_ODinterface.h']]], + ['od_5fh1381_5fsrdo_5f1_5fmapping_2483',['OD_H1381_SRDO_1_MAPPING',['../group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a8effa6834289b482e5ac3319ccf2c17b',1,'CO_ODinterface.h']]], + ['od_5fh13fe_5fsrdo_5fvalid_2484',['OD_H13FE_SRDO_VALID',['../group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886aa66d7204dd265d19276113ef7177ce68',1,'CO_ODinterface.h']]], + ['od_5fh13ff_5fsrdo_5fchecksum_2485',['OD_H13FF_SRDO_CHECKSUM',['../group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a92c6bed78bc6bbbd182c16fe8890dcbf',1,'CO_ODinterface.h']]], + ['od_5fh1400_5frxpdo_5f1_5fparam_2486',['OD_H1400_RXPDO_1_PARAM',['../group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886ae64bf5b7166b6adf46b8e965d43150a0',1,'CO_ODinterface.h']]], + ['od_5fh1600_5frxpdo_5f1_5fmapping_2487',['OD_H1600_RXPDO_1_MAPPING',['../group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a7ed53d283e4719920b233b9094b18f9c',1,'CO_ODinterface.h']]], + ['od_5fh1800_5ftxpdo_5f1_5fparam_2488',['OD_H1800_TXPDO_1_PARAM',['../group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886aac35e48e4b717eef309ebb57876d30f1',1,'CO_ODinterface.h']]], + ['od_5fh1a00_5ftxpdo_5f1_5fmapping_2489',['OD_H1A00_TXPDO_1_MAPPING',['../group__CO__ODinterface.html#ggade8960f241ee3b728eac09288a694886a2a2f1c4cc58d29ccf43e105afd57bc14',1,'CO_ODinterface.h']]], + ['oda_5fmb_2490',['ODA_MB',['../group__CO__ODinterface.html#gga47b0d204aaf1ea64b4f826aaf8f5c151ae02b665e7e8d8bd84f341c9ad040d367',1,'CO_ODinterface.h']]], + ['oda_5frpdo_2491',['ODA_RPDO',['../group__CO__ODinterface.html#gga47b0d204aaf1ea64b4f826aaf8f5c151ae4930aa0efbc2249563613b5107bb107',1,'CO_ODinterface.h']]], + ['oda_5frsrdo_2492',['ODA_RSRDO',['../group__CO__ODinterface.html#gga47b0d204aaf1ea64b4f826aaf8f5c151a6c6261d5bea91588b2851f5c11faae02',1,'CO_ODinterface.h']]], + ['oda_5fsdo_5fr_2493',['ODA_SDO_R',['../group__CO__ODinterface.html#gga47b0d204aaf1ea64b4f826aaf8f5c151ada3eb985961ffdd9cf655d1a7a7d9485',1,'CO_ODinterface.h']]], + ['oda_5fsdo_5frw_2494',['ODA_SDO_RW',['../group__CO__ODinterface.html#gga47b0d204aaf1ea64b4f826aaf8f5c151a2c60ba85cbe4f25d5511ffab3dcd7486',1,'CO_ODinterface.h']]], + ['oda_5fsdo_5fw_2495',['ODA_SDO_W',['../group__CO__ODinterface.html#gga47b0d204aaf1ea64b4f826aaf8f5c151a9245e7a557f32ab863aef41412df9eb5',1,'CO_ODinterface.h']]], + ['oda_5fstr_2496',['ODA_STR',['../group__CO__ODinterface.html#gga47b0d204aaf1ea64b4f826aaf8f5c151a9527d9c21ba4159d653771e1dedad81d',1,'CO_ODinterface.h']]], + ['oda_5ftpdo_2497',['ODA_TPDO',['../group__CO__ODinterface.html#gga47b0d204aaf1ea64b4f826aaf8f5c151ad106aacb6b181ab7dac0f6dbc8c50321',1,'CO_ODinterface.h']]], + ['oda_5ftrpdo_2498',['ODA_TRPDO',['../group__CO__ODinterface.html#gga47b0d204aaf1ea64b4f826aaf8f5c151af750832681daa8d5e44ed8908c4ec552',1,'CO_ODinterface.h']]], + ['oda_5ftrsrdo_2499',['ODA_TRSRDO',['../group__CO__ODinterface.html#gga47b0d204aaf1ea64b4f826aaf8f5c151af887f38d83f35c28d478f1a4b08d1be9',1,'CO_ODinterface.h']]], + ['oda_5ftsrdo_2500',['ODA_TSRDO',['../group__CO__ODinterface.html#gga47b0d204aaf1ea64b4f826aaf8f5c151a3a1b8c2ed54565e89d6e6d3a043bdcfe',1,'CO_ODinterface.h']]], + ['odr_5fcount_2501',['ODR_COUNT',['../group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869cac43414be729ea2b701380c4400658c37',1,'CO_ODinterface.h']]], + ['odr_5fdata_5fdev_5fstate_2502',['ODR_DATA_DEV_STATE',['../group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869ca16bfe0c712aaea3841ae2b250331b276',1,'CO_ODinterface.h']]], + ['odr_5fdata_5floc_5fctrl_2503',['ODR_DATA_LOC_CTRL',['../group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869ca28b28d8f091eefb7e8fac92bbdae82bb',1,'CO_ODinterface.h']]], + ['odr_5fdata_5flong_2504',['ODR_DATA_LONG',['../group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869ca3f93f54b7f130bb7b266b2d36c24caec',1,'CO_ODinterface.h']]], + ['odr_5fdata_5fshort_2505',['ODR_DATA_SHORT',['../group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869cae0b103160b23ff40047fcf85225121d2',1,'CO_ODinterface.h']]], + ['odr_5fdata_5ftransf_2506',['ODR_DATA_TRANSF',['../group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869ca341104b9ac43168d0c668586b8f750bb',1,'CO_ODinterface.h']]], + ['odr_5fdev_5fincompat_2507',['ODR_DEV_INCOMPAT',['../group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869ca3d259ef030c2a10afecb253b532a0323',1,'CO_ODinterface.h']]], + ['odr_5fgeneral_2508',['ODR_GENERAL',['../group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869ca968d2a28cb866cacf7ef8b8cd0b76e2c',1,'CO_ODinterface.h']]], + ['odr_5fhw_2509',['ODR_HW',['../group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869caeaac26c680e626185429468dda9c2433',1,'CO_ODinterface.h']]], + ['odr_5fidx_5fnot_5fexist_2510',['ODR_IDX_NOT_EXIST',['../group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869ca64ea80d1baebf136382d53d5580fbc85',1,'CO_ODinterface.h']]], + ['odr_5finvalid_5fvalue_2511',['ODR_INVALID_VALUE',['../group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869ca0646f592124a9b4d2835c9c0296c6a0c',1,'CO_ODinterface.h']]], + ['odr_5fmap_5flen_2512',['ODR_MAP_LEN',['../group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869cad6c92203fa86ee8ff7d7271bf81e7d9e',1,'CO_ODinterface.h']]], + ['odr_5fmax_5fless_5fmin_2513',['ODR_MAX_LESS_MIN',['../group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869cac9c8eda20bfc7dfffe17f170c377d646',1,'CO_ODinterface.h']]], + ['odr_5fno_5fdata_2514',['ODR_NO_DATA',['../group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869caff503d4f6cc55429913680d071ca3c4d',1,'CO_ODinterface.h']]], + ['odr_5fno_5fmap_2515',['ODR_NO_MAP',['../group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869ca853242f74446c58773099bdef9835a94',1,'CO_ODinterface.h']]], + ['odr_5fno_5fresource_2516',['ODR_NO_RESOURCE',['../group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869ca15fe8d8b791b90373e59bc5bc5d3f8c8',1,'CO_ODinterface.h']]], + ['odr_5fod_5fmissing_2517',['ODR_OD_MISSING',['../group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869caeae1e00129ff22708ffdcb2c8b3f083b',1,'CO_ODinterface.h']]], + ['odr_5fok_2518',['ODR_OK',['../group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869ca15f7f20e27f1c5f174bdeecfeef45cc2',1,'CO_ODinterface.h']]], + ['odr_5fout_5fof_5fmem_2519',['ODR_OUT_OF_MEM',['../group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869ca118a63a81ef2fd802c925bf4c79975fa',1,'CO_ODinterface.h']]], + ['odr_5fpar_5fincompat_2520',['ODR_PAR_INCOMPAT',['../group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869ca36edb0ad1c8c5c0d804bb88274bfe165',1,'CO_ODinterface.h']]], + ['odr_5fpartial_2521',['ODR_PARTIAL',['../group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869caf7595749473065bfc81cfa6709370fee',1,'CO_ODinterface.h']]], + ['odr_5freadonly_2522',['ODR_READONLY',['../group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869caa119384effe499c9bffb874219c6433a',1,'CO_ODinterface.h']]], + ['odr_5fsub_5fnot_5fexist_2523',['ODR_SUB_NOT_EXIST',['../group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869ca4f76fa87ea446616ff2f6195e7bec67c',1,'CO_ODinterface.h']]], + ['odr_5ftype_5fmismatch_2524',['ODR_TYPE_MISMATCH',['../group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869cad197c1462f472a21be2e3ed5c5880aa4',1,'CO_ODinterface.h']]], + ['odr_5funsupp_5faccess_2525',['ODR_UNSUPP_ACCESS',['../group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869ca17d694adc9471112cbb2740f7f45a2d0',1,'CO_ODinterface.h']]], + ['odr_5fvalue_5fhigh_2526',['ODR_VALUE_HIGH',['../group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869ca95291309267a732c380e13ad6c17a986',1,'CO_ODinterface.h']]], + ['odr_5fvalue_5flow_2527',['ODR_VALUE_LOW',['../group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869ca4db02b8575a8a10786959a5472f1c0f4',1,'CO_ODinterface.h']]], + ['odr_5fwriteonly_2528',['ODR_WRITEONLY',['../group__CO__ODinterface.html#gga0e9afd8ad27de0920d1fe0738834869ca44ab94bfc7547122b96498c781291df6',1,'CO_ODinterface.h']]], + ['odt_5farr_2529',['ODT_ARR',['../group__CO__ODdefinition.html#ggaae426e9d66ec1bacfef2d93f096d7805a1ad5763beafe79c42ca223297c832ff4',1,'CO_ODinterface.h']]], + ['odt_5fearr_2530',['ODT_EARR',['../group__CO__ODdefinition.html#ggaae426e9d66ec1bacfef2d93f096d7805a1ae954b4709b24d93bdcac69957c8e40',1,'CO_ODinterface.h']]], + ['odt_5ferec_2531',['ODT_EREC',['../group__CO__ODdefinition.html#ggaae426e9d66ec1bacfef2d93f096d7805ae176b06a942e47815d2e4c51a8f9b7f8',1,'CO_ODinterface.h']]], + ['odt_5fevar_2532',['ODT_EVAR',['../group__CO__ODdefinition.html#ggaae426e9d66ec1bacfef2d93f096d7805adb58a7faa735918d39b8bbcd3a6ad594',1,'CO_ODinterface.h']]], + ['odt_5fextension_5fmask_2533',['ODT_EXTENSION_MASK',['../group__CO__ODdefinition.html#ggaae426e9d66ec1bacfef2d93f096d7805a3dd8bc41ec11c475d487b877fdd0a879',1,'CO_ODinterface.h']]], + ['odt_5frec_2534',['ODT_REC',['../group__CO__ODdefinition.html#ggaae426e9d66ec1bacfef2d93f096d7805a9376cface357f03bec8a651a307f33b9',1,'CO_ODinterface.h']]], + ['odt_5ftype_5fmask_2535',['ODT_TYPE_MASK',['../group__CO__ODdefinition.html#ggaae426e9d66ec1bacfef2d93f096d7805ac9e24bde0d37c35c3065dbaa541e1acb',1,'CO_ODinterface.h']]], + ['odt_5fvar_2536',['ODT_VAR',['../group__CO__ODdefinition.html#ggaae426e9d66ec1bacfef2d93f096d7805a829f1df882410efc0ea0e05b3435c820',1,'CO_ODinterface.h']]] +]; diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/files_0.html b/Devices/Libraries/Systems/CANopenSocket/docs/search/files_0.html new file mode 100755 index 0000000..737608e --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/files_0.html @@ -0,0 +1,30 @@ + + + + + + + + + +
+
Loading...
+
+ +
Searching...
+
No Matches
+ +
+ + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/files_0.js b/Devices/Libraries/Systems/CANopenSocket/docs/search/files_0.js new file mode 100755 index 0000000..b6539db --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/files_0.js @@ -0,0 +1,31 @@ +var searchData= +[ + ['canopen_2eh_1384',['CANopen.h',['../CANopen_8h.html',1,'']]], + ['co_5fconfig_2eh_1385',['CO_config.h',['../CO__config_8h.html',1,'']]], + ['co_5fdriver_2eh_1386',['CO_driver.h',['../CO__driver_8h.html',1,'']]], + ['co_5fdriver_5ftarget_2eh_1387',['CO_driver_target.h',['../CO__driver__target_8h.html',1,'']]], + ['co_5femergency_2eh_1388',['CO_Emergency.h',['../CO__Emergency_8h.html',1,'']]], + ['co_5fepoll_5finterface_2eh_1389',['CO_epoll_interface.h',['../CO__epoll__interface_8h.html',1,'']]], + ['co_5ferror_2eh_1390',['CO_error.h',['../CO__error_8h.html',1,'']]], + ['co_5ffifo_2eh_1391',['CO_fifo.h',['../CO__fifo_8h.html',1,'']]], + ['co_5fgateway_5fascii_2eh_1392',['CO_gateway_ascii.h',['../CO__gateway__ascii_8h.html',1,'']]], + ['co_5fgfc_2ec_1393',['CO_GFC.c',['../CO__GFC_8c.html',1,'']]], + ['co_5fgfc_2eh_1394',['CO_GFC.h',['../CO__GFC_8h.html',1,'']]], + ['co_5fhbconsumer_2eh_1395',['CO_HBconsumer.h',['../CO__HBconsumer_8h.html',1,'']]], + ['co_5fleds_2eh_1396',['CO_LEDs.h',['../CO__LEDs_8h.html',1,'']]], + ['co_5flss_2eh_1397',['CO_LSS.h',['../CO__LSS_8h.html',1,'']]], + ['co_5flssmaster_2eh_1398',['CO_LSSmaster.h',['../CO__LSSmaster_8h.html',1,'']]], + ['co_5flssslave_2eh_1399',['CO_LSSslave.h',['../CO__LSSslave_8h.html',1,'']]], + ['co_5fnmt_5fheartbeat_2eh_1400',['CO_NMT_Heartbeat.h',['../CO__NMT__Heartbeat_8h.html',1,'']]], + ['co_5fod_5fstorage_2eh_1401',['CO_OD_storage.h',['../CO__OD__storage_8h.html',1,'']]], + ['co_5fodinterface_2eh_1402',['CO_ODinterface.h',['../CO__ODinterface_8h.html',1,'']]], + ['co_5fpdo_2eh_1403',['CO_PDO.h',['../CO__PDO_8h.html',1,'']]], + ['co_5fsdoclient_2eh_1404',['CO_SDOclient.h',['../CO__SDOclient_8h.html',1,'']]], + ['co_5fsdoserver_2eh_1405',['CO_SDOserver.h',['../CO__SDOserver_8h.html',1,'']]], + ['co_5fsrdo_2ec_1406',['CO_SRDO.c',['../CO__SRDO_8c.html',1,'']]], + ['co_5fsrdo_2eh_1407',['CO_SRDO.h',['../CO__SRDO_8h.html',1,'']]], + ['co_5fsync_2eh_1408',['CO_SYNC.h',['../CO__SYNC_8h.html',1,'']]], + ['co_5ftime_2eh_1409',['CO_TIME.h',['../CO__TIME_8h.html',1,'']]], + ['co_5ftrace_2eh_1410',['CO_trace.h',['../CO__trace_8h.html',1,'']]], + ['crc16_2dccitt_2eh_1411',['crc16-ccitt.h',['../crc16-ccitt_8h.html',1,'']]] +]; diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/functions_0.html b/Devices/Libraries/Systems/CANopenSocket/docs/search/functions_0.html new file mode 100755 index 0000000..e17c711 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/functions_0.html @@ -0,0 +1,30 @@ + + + + + + + + + +
+
Loading...
+
+ +
Searching...
+
No Matches
+ +
+ + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/functions_0.js b/Devices/Libraries/Systems/CANopenSocket/docs/search/functions_0.js new file mode 100755 index 0000000..3b6d97a --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/functions_0.js @@ -0,0 +1,201 @@ +var searchData= +[ + ['canrx_5fcallback_1412',['CANrx_callback',['../group__CO__CAN__Message__reception.html#ga23168979123a5ca8a87d49307eb2990e',1,'CO_driver.h']]], + ['co_5fcanclearpendingsyncpdos_1413',['CO_CANclearPendingSyncPDOs',['../group__CO__driver.html#gabbeac85cbf513162b11fc3d0717b0753',1,'CO_CANclearPendingSyncPDOs(CO_CANmodule_t *CANmodule): CO_driver.c'],['../group__CO__driver.html#gabbeac85cbf513162b11fc3d0717b0753',1,'CO_CANclearPendingSyncPDOs(CO_CANmodule_t *CANmodule): CO_driver.c']]], + ['co_5fcanerror_5fdisable_1414',['CO_CANerror_disable',['../group__CO__socketCAN__ERROR.html#ga6a74902a35f8a260cdc3956b83c17c77',1,'CO_CANerror_disable(CO_CANinterfaceErrorhandler_t *CANerrorhandler): CO_error.c'],['../group__CO__socketCAN__ERROR.html#ga6a74902a35f8a260cdc3956b83c17c77',1,'CO_CANerror_disable(CO_CANinterfaceErrorhandler_t *CANerrorhandler): CO_error.c']]], + ['co_5fcanerror_5finit_1415',['CO_CANerror_init',['../group__CO__socketCAN__ERROR.html#ga30c3cb98d37aedf45d49643064fee4cd',1,'CO_CANerror_init(CO_CANinterfaceErrorhandler_t *CANerrorhandler, int fd, const char *ifName): CO_error.c'],['../group__CO__socketCAN__ERROR.html#ga30c3cb98d37aedf45d49643064fee4cd',1,'CO_CANerror_init(CO_CANinterfaceErrorhandler_t *CANerrorhandler, int fd, const char *ifname): CO_error.c']]], + ['co_5fcanerror_5frxmsg_1416',['CO_CANerror_rxMsg',['../group__CO__socketCAN__ERROR.html#ga5d5ee1aac31cf0334108cd147b9c9038',1,'CO_CANerror_rxMsg(CO_CANinterfaceErrorhandler_t *CANerrorhandler): CO_error.c'],['../group__CO__socketCAN__ERROR.html#ga5d5ee1aac31cf0334108cd147b9c9038',1,'CO_CANerror_rxMsg(CO_CANinterfaceErrorhandler_t *CANerrorhandler): CO_error.c']]], + ['co_5fcanerror_5frxmsgerror_1417',['CO_CANerror_rxMsgError',['../group__CO__socketCAN__ERROR.html#ga975e329593af25c4467f3ddde5cf5f5c',1,'CO_CANerror_rxMsgError(CO_CANinterfaceErrorhandler_t *CANerrorhandler, const struct can_frame *msg): CO_error.c'],['../group__CO__socketCAN__ERROR.html#ga975e329593af25c4467f3ddde5cf5f5c',1,'CO_CANerror_rxMsgError(CO_CANinterfaceErrorhandler_t *CANerrorhandler, const struct can_frame *msg): CO_error.c']]], + ['co_5fcanerror_5ftxmsg_1418',['CO_CANerror_txMsg',['../group__CO__socketCAN__ERROR.html#gac40e1c681a9721c91ed75c49afda913c',1,'CO_CANerror_txMsg(CO_CANinterfaceErrorhandler_t *CANerrorhandler): CO_error.c'],['../group__CO__socketCAN__ERROR.html#gac40e1c681a9721c91ed75c49afda913c',1,'CO_CANerror_txMsg(CO_CANinterfaceErrorhandler_t *CANerrorhandler): CO_error.c']]], + ['co_5fcaninit_1419',['CO_CANinit',['../group__CO__CANopen.html#ga619d9ee70c17464bb819b48b5eddb074',1,'CANopen.h']]], + ['co_5fcanmodule_5faddinterface_1420',['CO_CANmodule_addInterface',['../group__CO__socketCAN__driver__target.html#gaaffa26125993a7a1f6cbfdb468b59333',1,'CO_driver_target.h']]], + ['co_5fcanmodule_5fdisable_1421',['CO_CANmodule_disable',['../group__CO__driver.html#gac6f60f9da27dda0c9b3950c4e96bd687',1,'CO_CANmodule_disable(CO_CANmodule_t *CANmodule): CO_driver.c'],['../group__CO__driver.html#gac6f60f9da27dda0c9b3950c4e96bd687',1,'CO_CANmodule_disable(CO_CANmodule_t *CANmodule): CO_driver.c']]], + ['co_5fcanmodule_5finit_1422',['CO_CANmodule_init',['../group__CO__driver.html#ga3a1131813110529494cee5e27c0bf28d',1,'CO_CANmodule_init(CO_CANmodule_t *CANmodule, void *CANptr, CO_CANrx_t rxArray[], uint16_t rxSize, CO_CANtx_t txArray[], uint16_t txSize, uint16_t CANbitRate): CO_driver.c'],['../group__CO__driver.html#ga3a1131813110529494cee5e27c0bf28d',1,'CO_CANmodule_init(CO_CANmodule_t *CANmodule, void *CANptr, CO_CANrx_t rxArray[], uint16_t rxSize, CO_CANtx_t txArray[], uint16_t txSize, uint16_t CANbitRate): CO_driver.c']]], + ['co_5fcanmodule_5fprocess_1423',['CO_CANmodule_process',['../group__CO__driver.html#ga066c6742f75b2daac585735ffd6c9928',1,'CO_CANmodule_process(CO_CANmodule_t *CANmodule): CO_driver.c'],['../group__CO__driver.html#ga066c6742f75b2daac585735ffd6c9928',1,'CO_CANmodule_process(CO_CANmodule_t *CANmodule): CO_driver.c']]], + ['co_5fcanopeninit_1424',['CO_CANopenInit',['../group__CO__CANopen.html#ga0b64a860299af6e96f5663419aa6d446',1,'CANopen.h']]], + ['co_5fcanrxbuffer_5fgetinterface_1425',['CO_CANrxBuffer_getInterface',['../group__CO__socketCAN__driver__target.html#gaf266a58e21acf104d9f19cd0da704afe',1,'CO_driver_target.h']]], + ['co_5fcanrxbufferinit_1426',['CO_CANrxBufferInit',['../group__CO__driver.html#ga25ee22cd2e3a2f3bb49990e8bc3076b0',1,'CO_CANrxBufferInit(CO_CANmodule_t *CANmodule, uint16_t index, uint16_t ident, uint16_t mask, bool_t rtr, void *object, void(*CANrx_callback)(void *object, void *message)): CO_driver.c'],['../group__CO__driver.html#ga25ee22cd2e3a2f3bb49990e8bc3076b0',1,'CO_CANrxBufferInit(CO_CANmodule_t *CANmodule, uint16_t index, uint16_t ident, uint16_t mask, bool_t rtr, void *object, void(*CANrx_callback)(void *object, void *message)): CO_driver.c']]], + ['co_5fcanrxfromepoll_1427',['CO_CANrxFromEpoll',['../group__CO__socketCAN__driver__target.html#ga072c53260a32d7cd30d9ad1b5bb2c359',1,'CO_CANrxFromEpoll(CO_CANmodule_t *CANmodule, struct epoll_event *ev, CO_CANrxMsg_t *buffer, int32_t *msgIndex): CO_driver.c'],['../group__CO__socketCAN__driver__target.html#ga072c53260a32d7cd30d9ad1b5bb2c359',1,'CO_CANrxFromEpoll(CO_CANmodule_t *CANmodule, struct epoll_event *ev, CO_CANrxMsg_t *buffer, int32_t *msgIndex): CO_driver.c']]], + ['co_5fcanrxmsg_5freaddata_1428',['CO_CANrxMsg_readData',['../group__CO__CAN__Message__reception.html#gab02a5fe910acf9aa5021f97e523697f9',1,'CO_driver.h']]], + ['co_5fcanrxmsg_5freaddlc_1429',['CO_CANrxMsg_readDLC',['../group__CO__CAN__Message__reception.html#gacec1dcf8b7e66ea2e65905f91321b299',1,'CO_driver.h']]], + ['co_5fcanrxmsg_5freadident_1430',['CO_CANrxMsg_readIdent',['../group__CO__CAN__Message__reception.html#ga018e9159b92165b2506a6673113cdc0e',1,'CO_driver.h']]], + ['co_5fcansend_1431',['CO_CANsend',['../group__CO__driver.html#ga4664a9f5d547cb0605a9e929fb079f2e',1,'CO_CANsend(CO_CANmodule_t *CANmodule, CO_CANtx_t *buffer): CO_driver.c'],['../group__CO__driver.html#ga4664a9f5d547cb0605a9e929fb079f2e',1,'CO_CANsend(CO_CANmodule_t *CANmodule, CO_CANtx_t *buffer): CO_driver.c']]], + ['co_5fcansetconfigurationmode_1432',['CO_CANsetConfigurationMode',['../group__CO__driver.html#ga88ef52baae169a80e4c2f2cb93b33747',1,'CO_CANsetConfigurationMode(void *CANptr): CO_driver.c'],['../group__CO__driver.html#ga88ef52baae169a80e4c2f2cb93b33747',1,'CO_CANsetConfigurationMode(void *CANptr): CO_driver.c']]], + ['co_5fcansetnormalmode_1433',['CO_CANsetNormalMode',['../group__CO__driver.html#gad654edfa651bf7b68263913786697200',1,'CO_CANsetNormalMode(CO_CANmodule_t *CANmodule): CO_driver.c'],['../group__CO__driver.html#gad654edfa651bf7b68263913786697200',1,'CO_CANsetNormalMode(CO_CANmodule_t *CANmodule): CO_driver.c']]], + ['co_5fcantxbuffer_5fsetinterface_1434',['CO_CANtxBuffer_setInterface',['../group__CO__socketCAN__driver__target.html#ga0d7a8fdf7a2fafd4c6d2f2dd1e1017b0',1,'CO_driver_target.h']]], + ['co_5fcantxbufferinit_1435',['CO_CANtxBufferInit',['../group__CO__driver.html#ga01e2ee79e7e3a8b321dac831e7e00976',1,'CO_CANtxBufferInit(CO_CANmodule_t *CANmodule, uint16_t index, uint16_t ident, bool_t rtr, uint8_t noOfBytes, bool_t syncFlag): CO_driver.c'],['../group__CO__driver.html#ga01e2ee79e7e3a8b321dac831e7e00976',1,'CO_CANtxBufferInit(CO_CANmodule_t *CANmodule, uint16_t index, uint16_t ident, bool_t rtr, uint8_t noOfBytes, bool_t syncFlag): CO_driver.c']]], + ['co_5fdelete_1436',['CO_delete',['../group__CO__CANopen.html#ga7023592c26cf6649aa67bc2c04dfd95d',1,'CANopen.h']]], + ['co_5fem_5finit_1437',['CO_EM_init',['../group__CO__Emergency.html#ga5b80f59df00b71dca7a5c18c139aa71e',1,'CO_Emergency.h']]], + ['co_5fem_5finitcallbackpre_1438',['CO_EM_initCallbackPre',['../group__CO__Emergency.html#ga94efd78032de3667e2a89780b08aabed',1,'CO_Emergency.h']]], + ['co_5fem_5finitcallbackrx_1439',['CO_EM_initCallbackRx',['../group__CO__Emergency.html#ga583245c954327c3cf7f9fdb97854e76b',1,'CO_Emergency.h']]], + ['co_5fem_5fprocess_1440',['CO_EM_process',['../group__CO__Emergency.html#ga93ae7be6ef966192f5761ce343345d3b',1,'CO_EM_process(CO_EM_t *em, bool_t NMTisPreOrOperational, uint32_t timeDifference_us, uint32_t *timerNext_us): CO_Emergency.c'],['../group__CO__Emergency.html#ga93ae7be6ef966192f5761ce343345d3b',1,'CO_EM_process(CO_EM_t *em, bool_t NMTisPreOrOperational, uint32_t timeDifference_us, uint32_t *timerNext_us): CO_Emergency.c']]], + ['co_5fepoll_5fclose_1441',['CO_epoll_close',['../group__CO__epoll__interface.html#ga72c3ebdede1207e55e0915128f5a2c6a',1,'CO_epoll_close(CO_epoll_t *ep): CO_epoll_interface.c'],['../group__CO__epoll__interface.html#ga72c3ebdede1207e55e0915128f5a2c6a',1,'CO_epoll_close(CO_epoll_t *ep): CO_epoll_interface.c']]], + ['co_5fepoll_5fclosegtw_1442',['CO_epoll_closeGtw',['../group__CO__epoll__interface.html#gab2f3f7bb7d885799c462e95a45563b69',1,'CO_epoll_interface.h']]], + ['co_5fepoll_5fcreate_1443',['CO_epoll_create',['../group__CO__epoll__interface.html#ga9bb687bf26f711ce0573581984b79443',1,'CO_epoll_create(CO_epoll_t *ep, uint32_t timerInterval_us): CO_epoll_interface.c'],['../group__CO__epoll__interface.html#ga9bb687bf26f711ce0573581984b79443',1,'CO_epoll_create(CO_epoll_t *ep, uint32_t timerInterval_us): CO_epoll_interface.c']]], + ['co_5fepoll_5fcreategtw_1444',['CO_epoll_createGtw',['../group__CO__epoll__interface.html#ga7165df8b37ca1ed59476773fa075470c',1,'CO_epoll_interface.h']]], + ['co_5fepoll_5finitcanopengtw_1445',['CO_epoll_initCANopenGtw',['../group__CO__epoll__interface.html#ga07eaf2c954bb09b6420acee62ff207c3',1,'CO_epoll_interface.h']]], + ['co_5fepoll_5finitcanopenmain_1446',['CO_epoll_initCANopenMain',['../group__CO__epoll__interface.html#ga3668c8b600022ffecdd3c2fa643b5e7d',1,'CO_epoll_initCANopenMain(CO_epoll_t *ep, CO_t *co): CO_epoll_interface.c'],['../group__CO__epoll__interface.html#ga3668c8b600022ffecdd3c2fa643b5e7d',1,'CO_epoll_initCANopenMain(CO_epoll_t *ep, CO_t *co): CO_epoll_interface.c']]], + ['co_5fepoll_5fprocessgtw_1447',['CO_epoll_processGtw',['../group__CO__epoll__interface.html#ga97aa0bc09684b0ed78b708198e663407',1,'CO_epoll_interface.h']]], + ['co_5fepoll_5fprocesslast_1448',['CO_epoll_processLast',['../group__CO__epoll__interface.html#ga2aef637d4f2f818a7d95a7bfac251132',1,'CO_epoll_processLast(CO_epoll_t *ep): CO_epoll_interface.c'],['../group__CO__epoll__interface.html#ga2aef637d4f2f818a7d95a7bfac251132',1,'CO_epoll_processLast(CO_epoll_t *ep): CO_epoll_interface.c']]], + ['co_5fepoll_5fprocessmain_1449',['CO_epoll_processMain',['../group__CO__epoll__interface.html#gad79d73fbac0e816f81f75507bd164515',1,'CO_epoll_processMain(CO_epoll_t *ep, CO_t *co, bool_t enableGateway, CO_NMT_reset_cmd_t *reset): CO_epoll_interface.c'],['../group__CO__epoll__interface.html#gad79d73fbac0e816f81f75507bd164515',1,'CO_epoll_processMain(CO_epoll_t *ep, CO_t *co, bool_t enableGateway, CO_NMT_reset_cmd_t *reset): CO_epoll_interface.c']]], + ['co_5fepoll_5fprocessrt_1450',['CO_epoll_processRT',['../group__CO__epoll__interface.html#ga6ed67073bc96e575bec6fceb627b1245',1,'CO_epoll_processRT(CO_epoll_t *ep, CO_t *co, bool_t realtime): CO_epoll_interface.c'],['../group__CO__epoll__interface.html#ga6ed67073bc96e575bec6fceb627b1245',1,'CO_epoll_processRT(CO_epoll_t *ep, CO_t *co, bool_t realtime): CO_epoll_interface.c']]], + ['co_5fepoll_5fwait_1451',['CO_epoll_wait',['../group__CO__epoll__interface.html#ga5cfd1df62494cf9d9e85dbb9fe981a57',1,'CO_epoll_wait(CO_epoll_t *ep): CO_epoll_interface.c'],['../group__CO__epoll__interface.html#ga5cfd1df62494cf9d9e85dbb9fe981a57',1,'CO_epoll_wait(CO_epoll_t *ep): CO_epoll_interface.c']]], + ['co_5ferror_1452',['CO_error',['../group__CO__Emergency.html#ga9221f9f631ead4b6f66cfcff8614ba46',1,'CO_error(CO_EM_t *em, bool_t setError, const uint8_t errorBit, uint16_t errorCode, uint32_t infoCode): CO_Emergency.c'],['../group__CO__Emergency.html#ga9221f9f631ead4b6f66cfcff8614ba46',1,'CO_error(CO_EM_t *em, bool_t setError, const uint8_t errorBit, uint16_t errorCode, uint32_t infoCode): CO_Emergency.c']]], + ['co_5ffifo_5faltbegin_1453',['CO_fifo_altBegin',['../group__CO__CANopen__301__fifo.html#ga55c186098b2d860b116ddeb637a27b5a',1,'CO_fifo.h']]], + ['co_5ffifo_5faltfinish_1454',['CO_fifo_altFinish',['../group__CO__CANopen__301__fifo.html#ga7c534dd50959814b40c3027aa85f1c55',1,'CO_fifo.h']]], + ['co_5ffifo_5faltgetoccupied_1455',['CO_fifo_altGetOccupied',['../group__CO__CANopen__301__fifo.html#ga2ee45a0cab19a212022d82987de2a955',1,'CO_fifo.h']]], + ['co_5ffifo_5faltread_1456',['CO_fifo_altRead',['../group__CO__CANopen__301__fifo.html#gaa28339101c4ac5a2c44004276075c759',1,'CO_fifo.h']]], + ['co_5ffifo_5fcommsearch_1457',['CO_fifo_CommSearch',['../group__CO__CANopen__301__fifo.html#ga0dbef46e369e659bab7e29742971134c',1,'CO_fifo.h']]], + ['co_5ffifo_5fcpytok2b64_1458',['CO_fifo_cpyTok2B64',['../group__CO__CANopen__301__fifo.html#gaf11cf0cccc01e86f341e2b31607e90c7',1,'CO_fifo.h']]], + ['co_5ffifo_5fcpytok2hex_1459',['CO_fifo_cpyTok2Hex',['../group__CO__CANopen__301__fifo.html#gab3c4acf458a13bd6c4e5b0bc2adca10b',1,'CO_fifo.h']]], + ['co_5ffifo_5fcpytok2i16_1460',['CO_fifo_cpyTok2I16',['../group__CO__CANopen__301__fifo.html#ga306913df146db90f7209b89080d51640',1,'CO_fifo.h']]], + ['co_5ffifo_5fcpytok2i32_1461',['CO_fifo_cpyTok2I32',['../group__CO__CANopen__301__fifo.html#ga4f5b25af39c5a78faf04e54cb3cc90f7',1,'CO_fifo.h']]], + ['co_5ffifo_5fcpytok2i64_1462',['CO_fifo_cpyTok2I64',['../group__CO__CANopen__301__fifo.html#ga6a9bd6e8d07d20ded9aeca531f412d50',1,'CO_fifo.h']]], + ['co_5ffifo_5fcpytok2i8_1463',['CO_fifo_cpyTok2I8',['../group__CO__CANopen__301__fifo.html#gae038564001c68ab242a6f33756668ac5',1,'CO_fifo.h']]], + ['co_5ffifo_5fcpytok2r32_1464',['CO_fifo_cpyTok2R32',['../group__CO__CANopen__301__fifo.html#ga853847c375425a55fc2007880d94c484',1,'CO_fifo.h']]], + ['co_5ffifo_5fcpytok2r64_1465',['CO_fifo_cpyTok2R64',['../group__CO__CANopen__301__fifo.html#ga8b17d4f3c0d272bb73d82a79cd51cd3d',1,'CO_fifo.h']]], + ['co_5ffifo_5fcpytok2u16_1466',['CO_fifo_cpyTok2U16',['../group__CO__CANopen__301__fifo.html#gaf19bdbd45626578afc8a19de43698e0b',1,'CO_fifo.h']]], + ['co_5ffifo_5fcpytok2u32_1467',['CO_fifo_cpyTok2U32',['../group__CO__CANopen__301__fifo.html#gae2816b287cc2091d2382aaa6ac56e422',1,'CO_fifo.h']]], + ['co_5ffifo_5fcpytok2u64_1468',['CO_fifo_cpyTok2U64',['../group__CO__CANopen__301__fifo.html#gacfd3fdfda66e1feb6982f68377a8b7e2',1,'CO_fifo.h']]], + ['co_5ffifo_5fcpytok2u8_1469',['CO_fifo_cpyTok2U8',['../group__CO__CANopen__301__fifo.html#ga1788f69639bcc6bfa59bf85e57e5e13a',1,'CO_fifo.h']]], + ['co_5ffifo_5fcpytok2vs_1470',['CO_fifo_cpyTok2Vs',['../group__CO__CANopen__301__fifo.html#ga8cb3d7032cd46dfeb3b6c0bbd5ed1575',1,'CO_fifo.h']]], + ['co_5ffifo_5fgetc_1471',['CO_fifo_getc',['../group__CO__CANopen__301__fifo.html#ga4c1237aa63123d0ab8e9e16132a5c40a',1,'CO_fifo.h']]], + ['co_5ffifo_5fgetoccupied_1472',['CO_fifo_getOccupied',['../group__CO__CANopen__301__fifo.html#ga40ef4fc5dc184ef3c0c2f37eca1bc507',1,'CO_fifo.h']]], + ['co_5ffifo_5fgetspace_1473',['CO_fifo_getSpace',['../group__CO__CANopen__301__fifo.html#ga0d456e83af18cce9db9157b5a30fac21',1,'CO_fifo.h']]], + ['co_5ffifo_5finit_1474',['CO_fifo_init',['../group__CO__CANopen__301__fifo.html#ga44e8f83feb2dd463b2fbb399dcd4de4d',1,'CO_fifo.h']]], + ['co_5ffifo_5fpurge_1475',['CO_fifo_purge',['../group__CO__CANopen__301__fifo.html#ga15a27a4871e680475f680c574050844a',1,'CO_fifo.h']]], + ['co_5ffifo_5fputc_1476',['CO_fifo_putc',['../group__CO__CANopen__301__fifo.html#ga7c2deb5abee499ad2f1b5175a205bc5b',1,'CO_fifo.h']]], + ['co_5ffifo_5fputc_5fov_1477',['CO_fifo_putc_ov',['../group__CO__CANopen__301__fifo.html#gae9351e31e5d74e738fe5e1514f99f2dd',1,'CO_fifo.h']]], + ['co_5ffifo_5fread_1478',['CO_fifo_read',['../group__CO__CANopen__301__fifo.html#gad0ba8be2a601030a374913e4fa94e6cb',1,'CO_fifo.h']]], + ['co_5ffifo_5freadb642a_1479',['CO_fifo_readB642a',['../group__CO__CANopen__301__fifo.html#ga7b9dcd98906837e82fa914d10028cf33',1,'CO_fifo.h']]], + ['co_5ffifo_5freadhex2a_1480',['CO_fifo_readHex2a',['../group__CO__CANopen__301__fifo.html#gae9bfcb5f9c52de5f7239ab133fe326ec',1,'CO_fifo.h']]], + ['co_5ffifo_5freadi162a_1481',['CO_fifo_readI162a',['../group__CO__CANopen__301__fifo.html#ga01a1765094d97ca3d0c5bbf98d1b8b1f',1,'CO_fifo.h']]], + ['co_5ffifo_5freadi322a_1482',['CO_fifo_readI322a',['../group__CO__CANopen__301__fifo.html#ga6a7a2ec338f735cdddf7ea7f7c0fd3a5',1,'CO_fifo.h']]], + ['co_5ffifo_5freadi642a_1483',['CO_fifo_readI642a',['../group__CO__CANopen__301__fifo.html#gaf959a6b823cc6db1d2107fd1faa40472',1,'CO_fifo.h']]], + ['co_5ffifo_5freadi82a_1484',['CO_fifo_readI82a',['../group__CO__CANopen__301__fifo.html#ga00fe8ce9f04846e8c756a09aad22cf03',1,'CO_fifo.h']]], + ['co_5ffifo_5freadr322a_1485',['CO_fifo_readR322a',['../group__CO__CANopen__301__fifo.html#ga74aad9b161d3953221a99d4b238f3c1f',1,'CO_fifo.h']]], + ['co_5ffifo_5freadr642a_1486',['CO_fifo_readR642a',['../group__CO__CANopen__301__fifo.html#gadc55fa15874eaf757acf83921d0156c4',1,'CO_fifo.h']]], + ['co_5ffifo_5freadtoken_1487',['CO_fifo_readToken',['../group__CO__CANopen__301__fifo.html#ga87a199708c95d3ca02e2fcc4ca4a6319',1,'CO_fifo.h']]], + ['co_5ffifo_5freadu162a_1488',['CO_fifo_readU162a',['../group__CO__CANopen__301__fifo.html#ga99072bd50ad3b92c291136e458885ca2',1,'CO_fifo.h']]], + ['co_5ffifo_5freadu322a_1489',['CO_fifo_readU322a',['../group__CO__CANopen__301__fifo.html#ga9a2d7d07570a8f552a81e8c72327f43c',1,'CO_fifo.h']]], + ['co_5ffifo_5freadu642a_1490',['CO_fifo_readU642a',['../group__CO__CANopen__301__fifo.html#ga8805e75a32e9f8672259f2274816a0b8',1,'CO_fifo.h']]], + ['co_5ffifo_5freadu82a_1491',['CO_fifo_readU82a',['../group__CO__CANopen__301__fifo.html#ga50b3075a0cfab3ab8608a4ba4977ecd6',1,'CO_fifo.h']]], + ['co_5ffifo_5freadvs2a_1492',['CO_fifo_readVs2a',['../group__CO__CANopen__301__fifo.html#ga8000ba92f2023a88b5f7d0399373b206',1,'CO_fifo.h']]], + ['co_5ffifo_5freadx162a_1493',['CO_fifo_readX162a',['../group__CO__CANopen__301__fifo.html#ga3202ac32250b599f5a5719c52264578a',1,'CO_fifo.h']]], + ['co_5ffifo_5freadx322a_1494',['CO_fifo_readX322a',['../group__CO__CANopen__301__fifo.html#ga8dcf1f06de7b744d95c6a65721828a5a',1,'CO_fifo.h']]], + ['co_5ffifo_5freadx642a_1495',['CO_fifo_readX642a',['../group__CO__CANopen__301__fifo.html#ga387acd7fdbd61389a42935d452dbd409',1,'CO_fifo.h']]], + ['co_5ffifo_5freadx82a_1496',['CO_fifo_readX82a',['../group__CO__CANopen__301__fifo.html#ga7ed37d0c4caea872cfe15a0c2e86fe0e',1,'CO_fifo.h']]], + ['co_5ffifo_5freset_1497',['CO_fifo_reset',['../group__CO__CANopen__301__fifo.html#ga6817a4baeaee87a578291e9de820b126',1,'CO_fifo.h']]], + ['co_5ffifo_5ftrimspaces_1498',['CO_fifo_trimSpaces',['../group__CO__CANopen__301__fifo.html#gac01a59d65d275bd9f9a7fb0ea04fb915',1,'CO_fifo.h']]], + ['co_5ffifo_5fwrite_1499',['CO_fifo_write',['../group__CO__CANopen__301__fifo.html#ga715cb5e1feacd2f3af5bc8195bbe69d3',1,'CO_fifo.h']]], + ['co_5fgeterrorregister_1500',['CO_getErrorRegister',['../group__CO__Emergency.html#gaf0c47186d9e51fb91d48385a9f6bad6b',1,'CO_Emergency.h']]], + ['co_5fgetuint16_1501',['CO_getUint16',['../group__CO__driver.html#ga6590fe7a05ecb4b81ee3d7e233274ea4',1,'CO_driver.h']]], + ['co_5fgetuint32_1502',['CO_getUint32',['../group__CO__driver.html#ga21cd9e2391f276b908bcde5769e967ed',1,'CO_driver.h']]], + ['co_5fgetuint8_1503',['CO_getUint8',['../group__CO__driver.html#ga27a4052f87c60cf2df472378689c2ef9',1,'CO_driver.h']]], + ['co_5fgfc_5finit_1504',['CO_GFC_init',['../group__CO__GFC.html#ga23d83d03ef1b9ad5ffe68103a627026c',1,'CO_GFC.h']]], + ['co_5fgfc_5finitcallbackentersafestate_1505',['CO_GFC_initCallbackEnterSafeState',['../group__CO__GFC.html#gaa7cf845381bf150a5816bc068ab9218f',1,'CO_GFC.h']]], + ['co_5fgfcsend_1506',['CO_GFCsend',['../group__CO__GFC.html#ga64a30ac6c275d166a2f4117050b12c8c',1,'CO_GFC.h']]], + ['co_5fgtwa_5finit_1507',['CO_GTWA_init',['../group__CO__CANopen__309__3.html#gabc95dab4fb09bcb18948502f922520ee',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5finitread_1508',['CO_GTWA_initRead',['../group__CO__CANopen__309__3.html#ga2093c35b83b096e01bd0c65ae9374e30',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5flog_5fprint_1509',['CO_GTWA_log_print',['../group__CO__CANopen__309__3.html#ga20523907b832d55b47b855dd92409996',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5fprocess_1510',['CO_GTWA_process',['../group__CO__CANopen__309__3.html#ga4a82ef2ebdd5d5f9d8a7efe84048493d',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5fwrite_1511',['CO_GTWA_write',['../group__CO__CANopen__309__3.html#gaae1f01d444be975aa3cb3b2c9ebded3d',1,'CO_gateway_ascii.h']]], + ['co_5fgtwa_5fwrite_5fgetspace_1512',['CO_GTWA_write_getSpace',['../group__CO__CANopen__309__3.html#gad614da9bb171d995c02ffe1940fe7e64',1,'CO_gateway_ascii.h']]], + ['co_5fhbconsumer_5fgetidxbynodeid_1513',['CO_HBconsumer_getIdxByNodeId',['../group__CO__HBconsumer.html#ga041b92d6feb1774cb7eb87fba842fdf2',1,'CO_HBconsumer.h']]], + ['co_5fhbconsumer_5fgetnmtstate_1514',['CO_HBconsumer_getNmtState',['../group__CO__HBconsumer.html#ga1731e3860fce5ca5d341d9b7fc32d8d6',1,'CO_HBconsumer.h']]], + ['co_5fhbconsumer_5fgetstate_1515',['CO_HBconsumer_getState',['../group__CO__HBconsumer.html#ga7c5d4eccbcb0f1f8965a336fde04e765',1,'CO_HBconsumer.h']]], + ['co_5fhbconsumer_5finit_1516',['CO_HBconsumer_init',['../group__CO__HBconsumer.html#gacc31c4848a14c9c367505b20e4a6a496',1,'CO_HBconsumer.h']]], + ['co_5fhbconsumer_5finitcallbackheartbeatstarted_1517',['CO_HBconsumer_initCallbackHeartbeatStarted',['../group__CO__HBconsumer.html#ga6c9bd0df815428719b9f9429ed4476a9',1,'CO_HBconsumer.h']]], + ['co_5fhbconsumer_5finitcallbacknmtchanged_1518',['CO_HBconsumer_initCallbackNmtChanged',['../group__CO__HBconsumer.html#gabab4b2dd74f6e341fe8b683f7a6d56f3',1,'CO_HBconsumer.h']]], + ['co_5fhbconsumer_5finitcallbackpre_1519',['CO_HBconsumer_initCallbackPre',['../group__CO__HBconsumer.html#ga2faa596dcebbec8f486788a791d638be',1,'CO_HBconsumer.h']]], + ['co_5fhbconsumer_5finitcallbackremotereset_1520',['CO_HBconsumer_initCallbackRemoteReset',['../group__CO__HBconsumer.html#ga8758a7bd92aa458b90d5da9221cc694f',1,'CO_HBconsumer.h']]], + ['co_5fhbconsumer_5finitcallbacktimeout_1521',['CO_HBconsumer_initCallbackTimeout',['../group__CO__HBconsumer.html#gaef359610a0cdd1331da266be9c55c2d2',1,'CO_HBconsumer.h']]], + ['co_5fhbconsumer_5finitentry_1522',['CO_HBconsumer_initEntry',['../group__CO__HBconsumer.html#gaf4bfa2bbd2b7d70f25b6bf173932170a',1,'CO_HBconsumer.h']]], + ['co_5fhbconsumer_5fprocess_1523',['CO_HBconsumer_process',['../group__CO__HBconsumer.html#ga29e01b5fe6392ce688e8ac57d966258f',1,'CO_HBconsumer.h']]], + ['co_5fiserror_1524',['CO_isError',['../group__CO__Emergency.html#ga8e9bae71814a3e7bbd8d59d721174c2b',1,'CO_Emergency.h']]], + ['co_5fislssslaveenabled_1525',['CO_isLSSslaveEnabled',['../group__CO__CANopen.html#ga4c9143f717e8df279034fb897e39b517',1,'CANopen.h']]], + ['co_5fleds_5finit_1526',['CO_LEDs_init',['../group__CO__LEDs.html#gadafdaf5de4c227a3df17cbcf1d81be69',1,'CO_LEDs.h']]], + ['co_5fleds_5fprocess_1527',['CO_LEDs_process',['../group__CO__LEDs.html#gac008ef501f913c9df1ee79c4b071ad80',1,'CO_LEDs.h']]], + ['co_5flssinit_1528',['CO_LSSinit',['../group__CO__CANopen.html#ga0e11332fd28af597bc47b21327cdc8a3',1,'CANopen.h']]], + ['co_5flssmaster_5factivatebit_1529',['CO_LSSmaster_ActivateBit',['../group__CO__LSSmaster.html#gaa016c0f3dc4dd021801b6139765657ab',1,'CO_LSSmaster.h']]], + ['co_5flssmaster_5fchangetimeout_1530',['CO_LSSmaster_changeTimeout',['../group__CO__LSSmaster.html#gae22758aff11b796cfaed979c5f2808c0',1,'CO_LSSmaster.h']]], + ['co_5flssmaster_5fconfigurebittiming_1531',['CO_LSSmaster_configureBitTiming',['../group__CO__LSSmaster.html#ga71a5d90e569ee7e88763a541c286e240',1,'CO_LSSmaster.h']]], + ['co_5flssmaster_5fconfigurenodeid_1532',['CO_LSSmaster_configureNodeId',['../group__CO__LSSmaster.html#ga2cdba08d9a564c4a61ebbcd0d10342fd',1,'CO_LSSmaster.h']]], + ['co_5flssmaster_5fconfigurestore_1533',['CO_LSSmaster_configureStore',['../group__CO__LSSmaster.html#gacea091d379a5338f13963eb745b25b16',1,'CO_LSSmaster.h']]], + ['co_5flssmaster_5fidentifyfastscan_1534',['CO_LSSmaster_IdentifyFastscan',['../group__CO__LSSmaster.html#gad01ce178ea43b1843f541d4dd488f90e',1,'CO_LSSmaster.h']]], + ['co_5flssmaster_5finit_1535',['CO_LSSmaster_init',['../group__CO__LSSmaster.html#ga0675297a7e7e1f472ad2e88d6b7408e7',1,'CO_LSSmaster.h']]], + ['co_5flssmaster_5finitcallbackpre_1536',['CO_LSSmaster_initCallbackPre',['../group__CO__LSSmaster.html#gabfeb7e75d88b76bb00c1740381c7b53f',1,'CO_LSSmaster.h']]], + ['co_5flssmaster_5finquire_1537',['CO_LSSmaster_Inquire',['../group__CO__LSSmaster.html#ga22414a7184ca0c9d371dd67e9990d820',1,'CO_LSSmaster.h']]], + ['co_5flssmaster_5finquirelssaddress_1538',['CO_LSSmaster_InquireLssAddress',['../group__CO__LSSmaster.html#ga1b0a5c9e27e046736c6ec55a0256ed77',1,'CO_LSSmaster.h']]], + ['co_5flssmaster_5fswitchstatedeselect_1539',['CO_LSSmaster_switchStateDeselect',['../group__CO__LSSmaster.html#gac0e13ec42e1fd85da5ddef6f654ef1a4',1,'CO_LSSmaster.h']]], + ['co_5flssmaster_5fswitchstateselect_1540',['CO_LSSmaster_switchStateSelect',['../group__CO__LSSmaster.html#ga41b4288c03af394261541b9a8395e8f3',1,'CO_LSSmaster.h']]], + ['co_5flssslave_5fgetstate_1541',['CO_LSSslave_getState',['../group__CO__LSSslave.html#ga2692ff0d6837db494c029a3bef735ee7',1,'CO_LSSslave.h']]], + ['co_5flssslave_5finit_1542',['CO_LSSslave_init',['../group__CO__LSSslave.html#gaaba1fafcd0024609f8a72be4810baf66',1,'CO_LSSslave.h']]], + ['co_5flssslave_5finitactivatebitratecallback_1543',['CO_LSSslave_initActivateBitRateCallback',['../group__CO__LSSslave.html#ga0253fffcb36ab6b850563328784a8a5a',1,'CO_LSSslave.h']]], + ['co_5flssslave_5finitcallbackpre_1544',['CO_LSSslave_initCallbackPre',['../group__CO__LSSslave.html#ga64c23178117a707046eb15ebc6506429',1,'CO_LSSslave.h']]], + ['co_5flssslave_5finitcfgstorecallback_1545',['CO_LSSslave_initCfgStoreCallback',['../group__CO__LSSslave.html#gadc6187357904293da0a35317f15d0666',1,'CO_LSSslave.h']]], + ['co_5flssslave_5finitcheckbitratecallback_1546',['CO_LSSslave_initCheckBitRateCallback',['../group__CO__LSSslave.html#ga665f147d6fae6db71173c4a8d602495c',1,'CO_LSSslave.h']]], + ['co_5flssslave_5fprocess_1547',['CO_LSSslave_process',['../group__CO__LSSslave.html#gae19d7ad84333f1a3f40ecbdbf639e017',1,'CO_LSSslave.h']]], + ['co_5fnew_1548',['CO_new',['../group__CO__CANopen.html#gaf6ef29daa2063de90b4799ae795c7027',1,'CANopen.h']]], + ['co_5fnmt_5fgetinternalstate_1549',['CO_NMT_getInternalState',['../group__CO__NMT__Heartbeat.html#ga2e3f0d579e321d73b53bf469d49b0ce3',1,'CO_NMT_Heartbeat.h']]], + ['co_5fnmt_5finit_1550',['CO_NMT_init',['../group__CO__NMT__Heartbeat.html#ga53c92521fff0fab405f8a372702c7259',1,'CO_NMT_Heartbeat.h']]], + ['co_5fnmt_5finitcallbackchanged_1551',['CO_NMT_initCallbackChanged',['../group__CO__NMT__Heartbeat.html#ga87d153e39b32586b67ee6dd47dfce787',1,'CO_NMT_Heartbeat.h']]], + ['co_5fnmt_5finitcallbackpre_1552',['CO_NMT_initCallbackPre',['../group__CO__NMT__Heartbeat.html#ga58b6123938e950b5e8c38ec0b9caeec4',1,'CO_NMT_Heartbeat.h']]], + ['co_5fnmt_5fprocess_1553',['CO_NMT_process',['../group__CO__NMT__Heartbeat.html#ga724ff5b1d9cfee955914f365514deda0',1,'CO_NMT_process(CO_NMT_t *NMT, CO_NMT_internalState_t *NMTstate, uint32_t timeDifference_us, uint32_t *timerNext_us): CO_NMT_Heartbeat.c'],['../group__CO__NMT__Heartbeat.html#ga724ff5b1d9cfee955914f365514deda0',1,'CO_NMT_process(CO_NMT_t *NMT, CO_NMT_internalState_t *NMTstate, uint32_t timeDifference_us, uint32_t *timerNext_us): CO_NMT_Heartbeat.c']]], + ['co_5fnmt_5fsendcommand_1554',['CO_NMT_sendCommand',['../group__CO__NMT__Heartbeat.html#ga9d557708be1bfb9b169718abea90b663',1,'CO_NMT_Heartbeat.h']]], + ['co_5fnmt_5fsetinternalstate_1555',['CO_NMT_setInternalState',['../group__CO__NMT__Heartbeat.html#ga91b7041eda0487bbc0588d3200e0e868',1,'CO_NMT_Heartbeat.h']]], + ['co_5fod_5fstorage_5fautosave_1556',['CO_OD_storage_autoSave',['../group__CO__socketCAN__OD__storage.html#ga9381d4f670cef9068efaf7d42097de2f',1,'CO_OD_storage_autoSave(CO_OD_storage_t *odStor, uint32_t timer1usDiff, uint32_t delay_us): CO_OD_storage.c'],['../group__CO__socketCAN__OD__storage.html#ga9381d4f670cef9068efaf7d42097de2f',1,'CO_OD_storage_autoSave(CO_OD_storage_t *odStor, uint32_t timer1usDiff, uint32_t delay_us): CO_OD_storage.c']]], + ['co_5fod_5fstorage_5fautosaveclose_1557',['CO_OD_storage_autoSaveClose',['../group__CO__socketCAN__OD__storage.html#ga6c51a86516ff7e128873f6cf1fdef5eb',1,'CO_OD_storage_autoSaveClose(CO_OD_storage_t *odStor): CO_OD_storage.c'],['../group__CO__socketCAN__OD__storage.html#ga6c51a86516ff7e128873f6cf1fdef5eb',1,'CO_OD_storage_autoSaveClose(CO_OD_storage_t *odStor): CO_OD_storage.c']]], + ['co_5fod_5fstorage_5finit_1558',['CO_OD_storage_init',['../group__CO__socketCAN__OD__storage.html#ga5a26b63e7222b058c6024e54c6e0d5cd',1,'CO_OD_storage_init(CO_OD_storage_t *odStor, uint8_t *odAddress, uint32_t odSize, char *filename): CO_OD_storage.c'],['../group__CO__socketCAN__OD__storage.html#ga5a26b63e7222b058c6024e54c6e0d5cd',1,'CO_OD_storage_init(CO_OD_storage_t *odStor, uint8_t *odAddress, uint32_t odSize, char *filename): CO_OD_storage.c']]], + ['co_5fod_5fstorage_5frestoresecure_1559',['CO_OD_storage_restoreSecure',['../group__CO__socketCAN__OD__storage.html#ga63b392fa7eb2bdc92ecd2f1ff6f4ced0',1,'CO_OD_storage_restoreSecure(char *filename): CO_OD_storage.c'],['../group__CO__socketCAN__OD__storage.html#ga63b392fa7eb2bdc92ecd2f1ff6f4ced0',1,'CO_OD_storage_restoreSecure(char *filename): CO_OD_storage.c']]], + ['co_5fod_5fstorage_5fsavesecure_1560',['CO_OD_storage_saveSecure',['../group__CO__socketCAN__OD__storage.html#ga7f6124c9079807bc2f8f3d860571ccec',1,'CO_OD_storage_saveSecure(uint8_t *odAddress, uint32_t odSize, char *filename): CO_OD_storage.c'],['../group__CO__socketCAN__OD__storage.html#ga7f6124c9079807bc2f8f3d860571ccec',1,'CO_OD_storage_saveSecure(uint8_t *odAddress, uint32_t odSize, char *filename): CO_OD_storage.c']]], + ['co_5fodf_5f1010_1561',['CO_ODF_1010',['../group__CO__socketCAN__OD__storage.html#ga4a5e807a83eeab172bb3b0aeb6fa92c2',1,'CO_ODF_1010(CO_ODF_arg_t *ODF_arg): CO_OD_storage.c'],['../group__CO__socketCAN__OD__storage.html#ga4a5e807a83eeab172bb3b0aeb6fa92c2',1,'CO_ODF_1010(CO_ODF_arg_t *ODF_arg): CO_OD_storage.c']]], + ['co_5fodf_5f1011_1562',['CO_ODF_1011',['../group__CO__socketCAN__OD__storage.html#ga059fcd46d8b15caf86c57d541a09576a',1,'CO_ODF_1011(CO_ODF_arg_t *ODF_arg): CO_OD_storage.c'],['../group__CO__socketCAN__OD__storage.html#ga059fcd46d8b15caf86c57d541a09576a',1,'CO_ODF_1011(CO_ODF_arg_t *ODF_arg): CO_OD_storage.c']]], + ['co_5fprocess_1563',['CO_process',['../group__CO__CANopen.html#ga895d7fad40b60aacdac3cb0615729b5e',1,'CANopen.h']]], + ['co_5fprocess_5frpdo_1564',['CO_process_RPDO',['../group__CO__CANopen.html#ga4318848921c35e8bb5a7d97dca5668a0',1,'CANopen.h']]], + ['co_5fprocess_5fsrdo_1565',['CO_process_SRDO',['../group__CO__CANopen.html#gab76d7283fe5190d3a0009b423a9ba8b1',1,'CANopen.h']]], + ['co_5fprocess_5fsync_1566',['CO_process_SYNC',['../group__CO__CANopen.html#gaad5c15c3ca475912661f512d37413b12',1,'CANopen.h']]], + ['co_5fprocess_5ftpdo_1567',['CO_process_TPDO',['../group__CO__CANopen.html#ga8c62a2afd2762d99e9c9be13a3d9a7a8',1,'CANopen.h']]], + ['co_5frpdo_5finit_1568',['CO_RPDO_init',['../group__CO__PDO.html#ga92c484ada2ad240c1b8c891c88d56901',1,'CO_PDO.h']]], + ['co_5frpdo_5finitcallbackpre_1569',['CO_RPDO_initCallbackPre',['../group__CO__PDO.html#ga34532746ccf88ccfa835716e89369478',1,'CO_PDO.h']]], + ['co_5frpdo_5fprocess_1570',['CO_RPDO_process',['../group__CO__PDO.html#gad77bfd4c7f64e75e7ddee5c926477e66',1,'CO_PDO.h']]], + ['co_5fsdoclient_5finit_1571',['CO_SDOclient_init',['../group__CO__SDOclient.html#ga754160e34089aea70f84d22b06eaff9e',1,'CO_SDOclient.h']]], + ['co_5fsdoclient_5finitcallbackpre_1572',['CO_SDOclient_initCallbackPre',['../group__CO__SDOclient.html#ga4377eaecc3bd0a8320a2bbe1ef0ef776',1,'CO_SDOclient.h']]], + ['co_5fsdoclient_5fsetup_1573',['CO_SDOclient_setup',['../group__CO__SDOclient.html#gade3bf4e3249aa4c611570ec43563a08d',1,'CO_SDOclient.h']]], + ['co_5fsdoclientclose_1574',['CO_SDOclientClose',['../group__CO__SDOclient.html#ga9b98ea2c36f864f1a589c842528b12ab',1,'CO_SDOclient.h']]], + ['co_5fsdoclientdownload_1575',['CO_SDOclientDownload',['../group__CO__SDOclient.html#gaab262f0a8d08ba023639a2c197d0943a',1,'CO_SDOclient.h']]], + ['co_5fsdoclientdownloadbufwrite_1576',['CO_SDOclientDownloadBufWrite',['../group__CO__SDOclient.html#ga958d0568bd47d9a3152f9ea8d104b5f5',1,'CO_SDOclient.h']]], + ['co_5fsdoclientdownloadinitiate_1577',['CO_SDOclientDownloadInitiate',['../group__CO__SDOclient.html#ga40f6e79592e1d587d02bbb4eaefa9489',1,'CO_SDOclient.h']]], + ['co_5fsdoclientdownloadinitiatesize_1578',['CO_SDOclientDownloadInitiateSize',['../group__CO__SDOclient.html#gaf58b7731b4285538c26a0c7c49ab24b6',1,'CO_SDOclient.h']]], + ['co_5fsdoclientupload_1579',['CO_SDOclientUpload',['../group__CO__SDOclient.html#gabd3a3be7e3d1649adfdd253c979ec21f',1,'CO_SDOclient.h']]], + ['co_5fsdoclientuploadbufread_1580',['CO_SDOclientUploadBufRead',['../group__CO__SDOclient.html#gaf5cd4e009476b15a2cd995a9841fb175',1,'CO_SDOclient.h']]], + ['co_5fsdoclientuploadinitiate_1581',['CO_SDOclientUploadInitiate',['../group__CO__SDOclient.html#ga3180f82563b3ed768fe7d3bd34fe1886',1,'CO_SDOclient.h']]], + ['co_5fsdoserver_5finit_1582',['CO_SDOserver_init',['../group__CO__SDOserver.html#gac989ba60f25fd2bc48bca6df0c0c1dde',1,'CO_SDOserver_init(CO_SDOserver_t *SDO, const OD_t *OD, const OD_entry_t *OD_1200_SDOsrvPar, uint8_t nodeId, uint16_t SDOtimeoutTime_ms, CO_CANmodule_t *CANdevRx, uint16_t CANdevRxIdx, CO_CANmodule_t *CANdevTx, uint16_t CANdevTxIdx): CO_SDOserver.c'],['../group__CO__SDOserver.html#gac989ba60f25fd2bc48bca6df0c0c1dde',1,'CO_SDOserver_init(CO_SDOserver_t *SDO, const OD_t *OD, const OD_entry_t *OD_1200_SDOsrvPar, uint8_t nodeId, uint16_t SDOtimeoutTime_ms, CO_CANmodule_t *CANdevRx, uint16_t CANdevRxIdx, CO_CANmodule_t *CANdevTx, uint16_t CANdevTxIdx): CO_SDOserver.c']]], + ['co_5fsdoserver_5finitcallbackpre_1583',['CO_SDOserver_initCallbackPre',['../group__CO__SDOserver.html#ga3eeea49e2fb36da22dc754c62b03a423',1,'CO_SDOserver.h']]], + ['co_5fsdoserver_5fprocess_1584',['CO_SDOserver_process',['../group__CO__SDOserver.html#ga360bc6aa1659a5572d4d1077d787433a',1,'CO_SDOserver_process(CO_SDOserver_t *SDO, bool_t NMTisPreOrOperational, uint32_t timeDifference_us, uint32_t *timerNext_us): CO_SDOserver.c'],['../group__CO__SDOserver.html#ga360bc6aa1659a5572d4d1077d787433a',1,'CO_SDOserver_process(CO_SDOserver_t *SDO, bool_t NMTisPreOrOperational, uint32_t timeDifference_us, uint32_t *timerNext_us): CO_SDOserver.c']]], + ['co_5fsetuint16_1585',['CO_setUint16',['../group__CO__driver.html#gaf74a6b7343bc9d6efd5ec5d9d9f965fb',1,'CO_driver.h']]], + ['co_5fsetuint32_1586',['CO_setUint32',['../group__CO__driver.html#ga9dbc7193fbd875503e0b3b34ea22434e',1,'CO_driver.h']]], + ['co_5fsetuint8_1587',['CO_setUint8',['../group__CO__driver.html#ga61de0908305223a0a7949e184cc1d644',1,'CO_driver.h']]], + ['co_5fsrdo_5finit_1588',['CO_SRDO_init',['../group__CO__SRDO.html#ga608e6d48e97f1b4da316b36f10e389c6',1,'CO_SRDO.h']]], + ['co_5fsrdo_5finitcallbackentersafestate_1589',['CO_SRDO_initCallbackEnterSafeState',['../group__CO__SRDO.html#ga5303601f6f94c83530b5e165f54b54bb',1,'CO_SRDO.h']]], + ['co_5fsrdo_5finitcallbackpre_1590',['CO_SRDO_initCallbackPre',['../group__CO__SRDO.html#gac066166b35bcb3d0b01adfc0e2eb9bb1',1,'CO_SRDO.h']]], + ['co_5fsrdo_5fprocess_1591',['CO_SRDO_process',['../group__CO__SRDO.html#gacb94aa4f279a4476c193ee50c408dfbb',1,'CO_SRDO.h']]], + ['co_5fsrdo_5frequestsend_1592',['CO_SRDO_requestSend',['../group__CO__SRDO.html#gac9a21725c4bb0373ea18a414019cf339',1,'CO_SRDO.h']]], + ['co_5fsrdoguard_5finit_1593',['CO_SRDOGuard_init',['../group__CO__SRDO.html#gacd578e8a5a4af024f8e6f8aef87cbd14',1,'CO_SRDO.h']]], + ['co_5fsrdoguard_5fprocess_1594',['CO_SRDOGuard_process',['../group__CO__SRDO.html#ga0201fa4da8b37a18f864a9fd7c826a6c',1,'CO_SRDO.h']]], + ['co_5fsync_5finit_1595',['CO_SYNC_init',['../group__CO__SYNC.html#ga3047b679d5e3261eedf1980ea87b5135',1,'CO_SYNC.h']]], + ['co_5fsync_5finitcallbackpre_1596',['CO_SYNC_initCallbackPre',['../group__CO__SYNC.html#gaf1005766c7f1588262b018fe04960777',1,'CO_SYNC.h']]], + ['co_5fsync_5fprocess_1597',['CO_SYNC_process',['../group__CO__SYNC.html#ga66b8f42fd430daa2a57ff15aa49c814c',1,'CO_SYNC.h']]], + ['co_5fsyncsend_1598',['CO_SYNCsend',['../group__CO__SYNC.html#gabbc8625d068d19d8b632d034f396a1ff',1,'CO_SYNC.h']]], + ['co_5ftime_5finit_1599',['CO_TIME_init',['../group__CO__TIME.html#ga9e02651c1662d3da13a9a071c73347bb',1,'CO_TIME.h']]], + ['co_5ftime_5finitcallbackpre_1600',['CO_TIME_initCallbackPre',['../group__CO__TIME.html#gae2f03663f1477cdc551b61cf5689cd6b',1,'CO_TIME.h']]], + ['co_5ftime_5fprocess_1601',['CO_TIME_process',['../group__CO__TIME.html#ga39f71192db2b40da6dcf8f4fceac9bb6',1,'CO_TIME.h']]], + ['co_5ftpdo_5finit_1602',['CO_TPDO_init',['../group__CO__PDO.html#ga8fb100744dc91f84b236c55ee37200a1',1,'CO_PDO.h']]], + ['co_5ftpdo_5fprocess_1603',['CO_TPDO_process',['../group__CO__PDO.html#ga0bb0d1b09d37ca19e01d47d8d0004f6b',1,'CO_PDO.h']]], + ['co_5ftpdoiscos_1604',['CO_TPDOisCOS',['../group__CO__PDO.html#gafec3eb12b93146a3706cbf03d3770a8d',1,'CO_PDO.h']]], + ['co_5ftpdosend_1605',['CO_TPDOsend',['../group__CO__PDO.html#ga9b2c8692f74f6a6a389ef88bf9c682a5',1,'CO_PDO.h']]], + ['co_5ftrace_5finit_1606',['CO_trace_init',['../group__CO__trace.html#ga50621e5b8e28349f7caf260b6cf9962e',1,'CO_trace.h']]], + ['co_5ftrace_5fprocess_1607',['CO_trace_process',['../group__CO__trace.html#ga5b8d9460d7a42920325cf66eb8b725ec',1,'CO_trace.h']]], + ['crc16_5fccitt_1608',['crc16_ccitt',['../group__CO__crc16__ccitt.html#gab03185ec096eb8792b52d657ed6cbea1',1,'crc16-ccitt.h']]], + ['crc16_5fccitt_5fsingle_1609',['crc16_ccitt_single',['../group__CO__crc16__ccitt.html#gad0ef7bb8f47a7eb3ff71d603beba7f92',1,'crc16-ccitt.h']]] +]; diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/functions_1.html b/Devices/Libraries/Systems/CANopenSocket/docs/search/functions_1.html new file mode 100755 index 0000000..0ddac0a --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/functions_1.html @@ -0,0 +1,30 @@ + + + + + + + + + +
+
Loading...
+
+ +
Searching...
+
No Matches
+ +
+ + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/functions_1.js b/Devices/Libraries/Systems/CANopenSocket/docs/search/functions_1.js new file mode 100755 index 0000000..b4fde06 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/functions_1.js @@ -0,0 +1,4 @@ +var searchData= +[ + ['log_5fprintf_1610',['log_printf',['../group__CO__socketCAN__ERROR.html#gac9aeec86e89e5525b4e13e3b1e21866d',1,'log_printf(int priority, const char *format,...): CO_main_basic.c'],['../group__CO__socketCAN__ERROR.html#gac9aeec86e89e5525b4e13e3b1e21866d',1,'log_printf(int priority, const char *format,...): CO_main_basic.c']]] +]; diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/functions_2.html b/Devices/Libraries/Systems/CANopenSocket/docs/search/functions_2.html new file mode 100755 index 0000000..2737c5a --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/functions_2.html @@ -0,0 +1,30 @@ + + + + + + + + + +
+
Loading...
+
+ +
Searching...
+
No Matches
+ +
+ + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/functions_2.js b/Devices/Libraries/Systems/CANopenSocket/docs/search/functions_2.js new file mode 100755 index 0000000..6deadbd --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/functions_2.js @@ -0,0 +1,44 @@ +var searchData= +[ + ['od_5fextensionio_5finit_1611',['OD_extensionIO_init',['../group__CO__ODinterface.html#gac07bbe54fbfecc6bc8da2e10b2c0f7e8',1,'OD_extensionIO_init(const OD_entry_t *entry, void *object, OD_size_t(*read)(OD_stream_t *stream, uint8_t subIndex, void *buf, OD_size_t count, ODR_t *returnCode), OD_size_t(*write)(OD_stream_t *stream, uint8_t subIndex, const void *buf, OD_size_t count, ODR_t *returnCode)): CO_ODinterface.c'],['../group__CO__ODinterface.html#gac07bbe54fbfecc6bc8da2e10b2c0f7e8',1,'OD_extensionIO_init(const OD_entry_t *entry, void *object, OD_size_t(*read)(OD_stream_t *stream, uint8_t subIndex, void *buf, OD_size_t count, ODR_t *returnCode), OD_size_t(*write)(OD_stream_t *stream, uint8_t subIndex, const void *buf, OD_size_t count, ODR_t *returnCode)): CO_ODinterface.c']]], + ['od_5ffind_1612',['OD_find',['../group__CO__ODinterface.html#gaaacaadfc28bfaf485cefc8bff64310f4',1,'OD_find(const OD_t *od, uint16_t index): CO_ODinterface.c'],['../group__CO__ODinterface.html#gaaacaadfc28bfaf485cefc8bff64310f4',1,'OD_find(const OD_t *od, uint16_t index): CO_ODinterface.c']]], + ['od_5fget_5fi16_1613',['OD_get_i16',['../group__CO__ODgetSetters.html#ga4bcc220e0cc4f8c6bfaf4d5cf31da448',1,'OD_get_i16(const OD_entry_t *entry, uint8_t subIndex, int16_t *val, bool_t odOrig): CO_ODinterface.c'],['../group__CO__ODgetSetters.html#ga4bcc220e0cc4f8c6bfaf4d5cf31da448',1,'OD_get_i16(const OD_entry_t *entry, uint8_t subIndex, int16_t *val, bool_t odOrig): CO_ODinterface.c']]], + ['od_5fget_5fi32_1614',['OD_get_i32',['../group__CO__ODgetSetters.html#ga042737cecbf248d2cc6c874022d06e22',1,'OD_get_i32(const OD_entry_t *entry, uint8_t subIndex, int32_t *val, bool_t odOrig): CO_ODinterface.c'],['../group__CO__ODgetSetters.html#ga042737cecbf248d2cc6c874022d06e22',1,'OD_get_i32(const OD_entry_t *entry, uint8_t subIndex, int32_t *val, bool_t odOrig): CO_ODinterface.c']]], + ['od_5fget_5fi64_1615',['OD_get_i64',['../group__CO__ODgetSetters.html#gabfb67bc3602d9b5d901ac13e5271ccd0',1,'OD_get_i64(const OD_entry_t *entry, uint8_t subIndex, int64_t *val, bool_t odOrig): CO_ODinterface.c'],['../group__CO__ODgetSetters.html#gabfb67bc3602d9b5d901ac13e5271ccd0',1,'OD_get_i64(const OD_entry_t *entry, uint8_t subIndex, int64_t *val, bool_t odOrig): CO_ODinterface.c']]], + ['od_5fget_5fi8_1616',['OD_get_i8',['../group__CO__ODgetSetters.html#gac174f05be716b7d169e0d7b7393e512c',1,'OD_get_i8(const OD_entry_t *entry, uint8_t subIndex, int8_t *val, bool_t odOrig): CO_ODinterface.c'],['../group__CO__ODgetSetters.html#gac174f05be716b7d169e0d7b7393e512c',1,'OD_get_i8(const OD_entry_t *entry, uint8_t subIndex, int8_t *val, bool_t odOrig): CO_ODinterface.c']]], + ['od_5fget_5fr32_1617',['OD_get_r32',['../group__CO__ODgetSetters.html#gabbba108fa7a92cb48d31833bb804baa6',1,'OD_get_r32(const OD_entry_t *entry, uint8_t subIndex, float32_t *val, bool_t odOrig): CO_ODinterface.c'],['../group__CO__ODgetSetters.html#gabbba108fa7a92cb48d31833bb804baa6',1,'OD_get_r32(const OD_entry_t *entry, uint8_t subIndex, float32_t *val, bool_t odOrig): CO_ODinterface.c']]], + ['od_5fget_5fr64_1618',['OD_get_r64',['../group__CO__ODgetSetters.html#gadf78189c35343bcd2ca1737491b55684',1,'OD_get_r64(const OD_entry_t *entry, uint8_t subIndex, float64_t *val, bool_t odOrig): CO_ODinterface.c'],['../group__CO__ODgetSetters.html#gadf78189c35343bcd2ca1737491b55684',1,'OD_get_r64(const OD_entry_t *entry, uint8_t subIndex, float64_t *val, bool_t odOrig): CO_ODinterface.c']]], + ['od_5fget_5fu16_1619',['OD_get_u16',['../group__CO__ODgetSetters.html#gaa9cba6642facf33cdbe2e0155a90d571',1,'OD_get_u16(const OD_entry_t *entry, uint8_t subIndex, uint16_t *val, bool_t odOrig): CO_ODinterface.c'],['../group__CO__ODgetSetters.html#gaa9cba6642facf33cdbe2e0155a90d571',1,'OD_get_u16(const OD_entry_t *entry, uint8_t subIndex, uint16_t *val, bool_t odOrig): CO_ODinterface.c']]], + ['od_5fget_5fu32_1620',['OD_get_u32',['../group__CO__ODgetSetters.html#ga10e1975b6177b92e8da4b2b6a19533be',1,'OD_get_u32(const OD_entry_t *entry, uint8_t subIndex, uint32_t *val, bool_t odOrig): CO_ODinterface.c'],['../group__CO__ODgetSetters.html#ga10e1975b6177b92e8da4b2b6a19533be',1,'OD_get_u32(const OD_entry_t *entry, uint8_t subIndex, uint32_t *val, bool_t odOrig): CO_ODinterface.c']]], + ['od_5fget_5fu64_1621',['OD_get_u64',['../group__CO__ODgetSetters.html#ga2ef84e594a6733f7efeaeff55bfc9c9a',1,'OD_get_u64(const OD_entry_t *entry, uint8_t subIndex, uint64_t *val, bool_t odOrig): CO_ODinterface.c'],['../group__CO__ODgetSetters.html#ga2ef84e594a6733f7efeaeff55bfc9c9a',1,'OD_get_u64(const OD_entry_t *entry, uint8_t subIndex, uint64_t *val, bool_t odOrig): CO_ODinterface.c']]], + ['od_5fget_5fu8_1622',['OD_get_u8',['../group__CO__ODgetSetters.html#gad8fd318804b9f1ded265bfd07c6cdcf2',1,'OD_get_u8(const OD_entry_t *entry, uint8_t subIndex, uint8_t *val, bool_t odOrig): CO_ODinterface.c'],['../group__CO__ODgetSetters.html#gad8fd318804b9f1ded265bfd07c6cdcf2',1,'OD_get_u8(const OD_entry_t *entry, uint8_t subIndex, uint8_t *val, bool_t odOrig): CO_ODinterface.c']]], + ['od_5fgetindex_1623',['OD_getIndex',['../group__CO__ODinterface.html#gac84e7390f50e7e5c5e8ba42714e51aaf',1,'CO_ODinterface.h']]], + ['od_5fgetptr_5fi16_1624',['OD_getPtr_i16',['../group__CO__ODgetSetters.html#gaddbad04c274a68f3fabfbbdd13e83cfc',1,'OD_getPtr_i16(const OD_entry_t *entry, uint8_t subIndex, int16_t **val): CO_ODinterface.c'],['../group__CO__ODgetSetters.html#gaddbad04c274a68f3fabfbbdd13e83cfc',1,'OD_getPtr_i16(const OD_entry_t *entry, uint8_t subIndex, int16_t **val): CO_ODinterface.c']]], + ['od_5fgetptr_5fi32_1625',['OD_getPtr_i32',['../group__CO__ODgetSetters.html#ga085db308dd11ea7047c815d214f12a32',1,'OD_getPtr_i32(const OD_entry_t *entry, uint8_t subIndex, int32_t **val): CO_ODinterface.c'],['../group__CO__ODgetSetters.html#ga085db308dd11ea7047c815d214f12a32',1,'OD_getPtr_i32(const OD_entry_t *entry, uint8_t subIndex, int32_t **val): CO_ODinterface.c']]], + ['od_5fgetptr_5fi64_1626',['OD_getPtr_i64',['../group__CO__ODgetSetters.html#gae7e11e1bdf04006dcc6a9f91287c459b',1,'OD_getPtr_i64(const OD_entry_t *entry, uint8_t subIndex, int64_t **val): CO_ODinterface.c'],['../group__CO__ODgetSetters.html#gae7e11e1bdf04006dcc6a9f91287c459b',1,'OD_getPtr_i64(const OD_entry_t *entry, uint8_t subIndex, int64_t **val): CO_ODinterface.c']]], + ['od_5fgetptr_5fi8_1627',['OD_getPtr_i8',['../group__CO__ODgetSetters.html#gaa4e6671855056e5ae13d198acdfab664',1,'OD_getPtr_i8(const OD_entry_t *entry, uint8_t subIndex, int8_t **val): CO_ODinterface.c'],['../group__CO__ODgetSetters.html#gaa4e6671855056e5ae13d198acdfab664',1,'OD_getPtr_i8(const OD_entry_t *entry, uint8_t subIndex, int8_t **val): CO_ODinterface.c']]], + ['od_5fgetptr_5fos_1628',['OD_getPtr_os',['../group__CO__ODgetSetters.html#ga06361976dce86f180de76e1fced9a212',1,'OD_getPtr_os(const OD_entry_t *entry, uint8_t subIndex, uint8_t **val, OD_size_t *dataLength): CO_ODinterface.c'],['../group__CO__ODgetSetters.html#ga06361976dce86f180de76e1fced9a212',1,'OD_getPtr_os(const OD_entry_t *entry, uint8_t subIndex, uint8_t **val, OD_size_t *dataLength): CO_ODinterface.c']]], + ['od_5fgetptr_5fr32_1629',['OD_getPtr_r32',['../group__CO__ODgetSetters.html#gaf4e3f15fc0fbe2b24e88aeb53c9daa61',1,'OD_getPtr_r32(const OD_entry_t *entry, uint8_t subIndex, float32_t **val): CO_ODinterface.c'],['../group__CO__ODgetSetters.html#gaf4e3f15fc0fbe2b24e88aeb53c9daa61',1,'OD_getPtr_r32(const OD_entry_t *entry, uint8_t subIndex, float32_t **val): CO_ODinterface.c']]], + ['od_5fgetptr_5fr64_1630',['OD_getPtr_r64',['../group__CO__ODgetSetters.html#ga87a34e3e04dd2faa851d2d9c0a24e945',1,'OD_getPtr_r64(const OD_entry_t *entry, uint8_t subIndex, float64_t **val): CO_ODinterface.c'],['../group__CO__ODgetSetters.html#ga87a34e3e04dd2faa851d2d9c0a24e945',1,'OD_getPtr_r64(const OD_entry_t *entry, uint8_t subIndex, float64_t **val): CO_ODinterface.c']]], + ['od_5fgetptr_5fu16_1631',['OD_getPtr_u16',['../group__CO__ODgetSetters.html#gad1e334a640dc30553ece27e32e270bbb',1,'OD_getPtr_u16(const OD_entry_t *entry, uint8_t subIndex, uint16_t **val): CO_ODinterface.c'],['../group__CO__ODgetSetters.html#gad1e334a640dc30553ece27e32e270bbb',1,'OD_getPtr_u16(const OD_entry_t *entry, uint8_t subIndex, uint16_t **val): CO_ODinterface.c']]], + ['od_5fgetptr_5fu32_1632',['OD_getPtr_u32',['../group__CO__ODgetSetters.html#ga92f57ed14ed69ac00c78d48f1e479bb0',1,'OD_getPtr_u32(const OD_entry_t *entry, uint8_t subIndex, uint32_t **val): CO_ODinterface.c'],['../group__CO__ODgetSetters.html#ga92f57ed14ed69ac00c78d48f1e479bb0',1,'OD_getPtr_u32(const OD_entry_t *entry, uint8_t subIndex, uint32_t **val): CO_ODinterface.c']]], + ['od_5fgetptr_5fu64_1633',['OD_getPtr_u64',['../group__CO__ODgetSetters.html#gad192b1efde6a5bd6fa62fc5a4c484d84',1,'OD_getPtr_u64(const OD_entry_t *entry, uint8_t subIndex, uint64_t **val): CO_ODinterface.c'],['../group__CO__ODgetSetters.html#gad192b1efde6a5bd6fa62fc5a4c484d84',1,'OD_getPtr_u64(const OD_entry_t *entry, uint8_t subIndex, uint64_t **val): CO_ODinterface.c']]], + ['od_5fgetptr_5fu8_1634',['OD_getPtr_u8',['../group__CO__ODgetSetters.html#gaaa9f06494001462d32a74d11438d3157',1,'OD_getPtr_u8(const OD_entry_t *entry, uint8_t subIndex, uint8_t **val): CO_ODinterface.c'],['../group__CO__ODgetSetters.html#gaaa9f06494001462d32a74d11438d3157',1,'OD_getPtr_u8(const OD_entry_t *entry, uint8_t subIndex, uint8_t **val): CO_ODinterface.c']]], + ['od_5fgetptr_5fus_1635',['OD_getPtr_us',['../group__CO__ODgetSetters.html#gaf3efb5f4a50ff7ab17b37bf9c0fcf030',1,'OD_getPtr_us(const OD_entry_t *entry, uint8_t subIndex, uint16_t **val, OD_size_t *dataLength): CO_ODinterface.c'],['../group__CO__ODgetSetters.html#gaf3efb5f4a50ff7ab17b37bf9c0fcf030',1,'OD_getPtr_us(const OD_entry_t *entry, uint8_t subIndex, uint16_t **val, OD_size_t *dataLength): CO_ODinterface.c']]], + ['od_5fgetptr_5fvs_1636',['OD_getPtr_vs',['../group__CO__ODgetSetters.html#gaad4d755515e8dccf3616f14fe3914bde',1,'OD_getPtr_vs(const OD_entry_t *entry, uint8_t subIndex, char **val, OD_size_t *dataLength): CO_ODinterface.c'],['../group__CO__ODgetSetters.html#gaad4d755515e8dccf3616f14fe3914bde',1,'OD_getPtr_vs(const OD_entry_t *entry, uint8_t subIndex, char **val, OD_size_t *dataLength): CO_ODinterface.c']]], + ['od_5fgetsdoabcode_1637',['OD_getSDOabCode',['../group__CO__ODinterface.html#ga7c24b06bb9365d41b8e60acb4eaecc6c',1,'OD_getSDOabCode(ODR_t returnCode): CO_ODinterface.c'],['../group__CO__ODinterface.html#ga7c24b06bb9365d41b8e60acb4eaecc6c',1,'OD_getSDOabCode(ODR_t returnCode): CO_ODinterface.c']]], + ['od_5fgetsub_1638',['OD_getSub',['../group__CO__ODinterface.html#gaf1f736d4b4d6754d971f0c0a63655bcf',1,'OD_getSub(const OD_entry_t *entry, uint8_t subIndex, OD_subEntry_t *subEntry, OD_IO_t *io, bool_t odOrig): CO_ODinterface.c'],['../group__CO__ODinterface.html#gaf1f736d4b4d6754d971f0c0a63655bcf',1,'OD_getSub(const OD_entry_t *entry, uint8_t subIndex, OD_subEntry_t *subEntry, OD_IO_t *io, bool_t odOrig): CO_ODinterface.c']]], + ['od_5freadoriginal_1639',['OD_readOriginal',['../group__CO__ODinterface.html#gadf9ac60f94e1f9fc21b7f10a0d254503',1,'OD_readOriginal(OD_stream_t *stream, uint8_t subIndex, void *buf, OD_size_t count, ODR_t *returnCode): CO_ODinterface.c'],['../group__CO__ODinterface.html#gadf9ac60f94e1f9fc21b7f10a0d254503',1,'OD_readOriginal(OD_stream_t *stream, uint8_t subIndex, void *buf, OD_size_t count, ODR_t *returnCode): CO_ODinterface.c']]], + ['od_5frwrestart_1640',['OD_rwRestart',['../group__CO__ODinterface.html#ga3715e0a6b15bdf45659e1e01f9fc4e65',1,'CO_ODinterface.h']]], + ['od_5fset_5fi16_1641',['OD_set_i16',['../group__CO__ODgetSetters.html#gaf207e188609f742be3c12f248f45138c',1,'OD_set_i16(const OD_entry_t *entry, uint8_t subIndex, int16_t val, bool_t odOrig): CO_ODinterface.c'],['../group__CO__ODgetSetters.html#gaf207e188609f742be3c12f248f45138c',1,'OD_set_i16(const OD_entry_t *entry, uint8_t subIndex, int16_t val, bool_t odOrig): CO_ODinterface.c']]], + ['od_5fset_5fi32_1642',['OD_set_i32',['../group__CO__ODgetSetters.html#ga101037295ea6af8488b499097f0d1ef1',1,'OD_set_i32(const OD_entry_t *entry, uint8_t subIndex, int32_t val, bool_t odOrig): CO_ODinterface.c'],['../group__CO__ODgetSetters.html#ga101037295ea6af8488b499097f0d1ef1',1,'OD_set_i32(const OD_entry_t *entry, uint8_t subIndex, int32_t val, bool_t odOrig): CO_ODinterface.c']]], + ['od_5fset_5fi64_1643',['OD_set_i64',['../group__CO__ODgetSetters.html#ga5c19e8e101d06a0203d13f8d3e997d16',1,'OD_set_i64(const OD_entry_t *entry, uint8_t subIndex, int64_t val, bool_t odOrig): CO_ODinterface.c'],['../group__CO__ODgetSetters.html#ga5c19e8e101d06a0203d13f8d3e997d16',1,'OD_set_i64(const OD_entry_t *entry, uint8_t subIndex, int64_t val, bool_t odOrig): CO_ODinterface.c']]], + ['od_5fset_5fi8_1644',['OD_set_i8',['../group__CO__ODgetSetters.html#ga46df632d54b48714cf50e4a3a92b4e98',1,'OD_set_i8(const OD_entry_t *entry, uint8_t subIndex, int8_t val, bool_t odOrig): CO_ODinterface.c'],['../group__CO__ODgetSetters.html#ga46df632d54b48714cf50e4a3a92b4e98',1,'OD_set_i8(const OD_entry_t *entry, uint8_t subIndex, int8_t val, bool_t odOrig): CO_ODinterface.c']]], + ['od_5fset_5fr32_1645',['OD_set_r32',['../group__CO__ODgetSetters.html#ga032219231c5e488969d02787265ad7ab',1,'OD_set_r32(const OD_entry_t *entry, uint8_t subIndex, float32_t val, bool_t odOrig): CO_ODinterface.c'],['../group__CO__ODgetSetters.html#ga032219231c5e488969d02787265ad7ab',1,'OD_set_r32(const OD_entry_t *entry, uint8_t subIndex, float32_t val, bool_t odOrig): CO_ODinterface.c']]], + ['od_5fset_5fr64_1646',['OD_set_r64',['../group__CO__ODgetSetters.html#ga75c5dd2daa6f0352bf079d18c9e90708',1,'OD_set_r64(const OD_entry_t *entry, uint8_t subIndex, float64_t val, bool_t odOrig): CO_ODinterface.c'],['../group__CO__ODgetSetters.html#ga75c5dd2daa6f0352bf079d18c9e90708',1,'OD_set_r64(const OD_entry_t *entry, uint8_t subIndex, float64_t val, bool_t odOrig): CO_ODinterface.c']]], + ['od_5fset_5fu16_1647',['OD_set_u16',['../group__CO__ODgetSetters.html#gac63795511b7decfbbc46ddb7c2b59cfd',1,'OD_set_u16(const OD_entry_t *entry, uint8_t subIndex, uint16_t val, bool_t odOrig): CO_ODinterface.c'],['../group__CO__ODgetSetters.html#gac63795511b7decfbbc46ddb7c2b59cfd',1,'OD_set_u16(const OD_entry_t *entry, uint8_t subIndex, uint16_t val, bool_t odOrig): CO_ODinterface.c']]], + ['od_5fset_5fu32_1648',['OD_set_u32',['../group__CO__ODgetSetters.html#ga5e5e943b89a2385f41a075131f47b5d5',1,'OD_set_u32(const OD_entry_t *entry, uint8_t subIndex, uint32_t val, bool_t odOrig): CO_ODinterface.c'],['../group__CO__ODgetSetters.html#ga5e5e943b89a2385f41a075131f47b5d5',1,'OD_set_u32(const OD_entry_t *entry, uint8_t subIndex, uint32_t val, bool_t odOrig): CO_ODinterface.c']]], + ['od_5fset_5fu64_1649',['OD_set_u64',['../group__CO__ODgetSetters.html#ga85570b44cc9d0af5b1084f42aeaf5e9f',1,'OD_set_u64(const OD_entry_t *entry, uint8_t subIndex, uint64_t val, bool_t odOrig): CO_ODinterface.c'],['../group__CO__ODgetSetters.html#ga85570b44cc9d0af5b1084f42aeaf5e9f',1,'OD_set_u64(const OD_entry_t *entry, uint8_t subIndex, uint64_t val, bool_t odOrig): CO_ODinterface.c']]], + ['od_5fset_5fu8_1650',['OD_set_u8',['../group__CO__ODgetSetters.html#ga3f56347af1f0f9b8bf46463777df87d0',1,'OD_set_u8(const OD_entry_t *entry, uint8_t subIndex, uint8_t val, bool_t odOrig): CO_ODinterface.c'],['../group__CO__ODgetSetters.html#ga3f56347af1f0f9b8bf46463777df87d0',1,'OD_set_u8(const OD_entry_t *entry, uint8_t subIndex, uint8_t val, bool_t odOrig): CO_ODinterface.c']]], + ['od_5fwriteoriginal_1651',['OD_writeOriginal',['../group__CO__ODinterface.html#ga648f0b0bfabde2d377149bf84e937422',1,'OD_writeOriginal(OD_stream_t *stream, uint8_t subIndex, const void *buf, OD_size_t count, ODR_t *returnCode): CO_ODinterface.c'],['../group__CO__ODinterface.html#ga648f0b0bfabde2d377149bf84e937422',1,'OD_writeOriginal(OD_stream_t *stream, uint8_t subIndex, const void *buf, OD_size_t count, ODR_t *returnCode): CO_ODinterface.c']]] +]; diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/groups_0.html b/Devices/Libraries/Systems/CANopenSocket/docs/search/groups_0.html new file mode 100755 index 0000000..a2d9335 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/groups_0.html @@ -0,0 +1,30 @@ + + + + + + + + + +
+
Loading...
+
+ +
Searching...
+
No Matches
+ +
+ + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/groups_0.js b/Devices/Libraries/Systems/CANopenSocket/docs/search/groups_0.js new file mode 100755 index 0000000..64c934c --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/groups_0.js @@ -0,0 +1,4 @@ +var searchData= +[ + ['basic_20definitions_2537',['Basic definitions',['../group__CO__dataTypes.html',1,'']]] +]; diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/groups_1.html b/Devices/Libraries/Systems/CANopenSocket/docs/search/groups_1.html new file mode 100755 index 0000000..aa06d65 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/groups_1.html @@ -0,0 +1,30 @@ + + + + + + + + + +
+
Loading...
+
+ +
Searching...
+
No Matches
+ +
+ + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/groups_1.js b/Devices/Libraries/Systems/CANopenSocket/docs/search/groups_1.js new file mode 100755 index 0000000..7704d88 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/groups_1.js @@ -0,0 +1,19 @@ +var searchData= +[ + ['canopen_2538',['CANopen',['../group__CO__CANopen.html',1,'']]], + ['canopen_5f301_2539',['CANopen_301',['../group__CO__CANopen__301.html',1,'']]], + ['canopen_5f303_2540',['CANopen_303',['../group__CO__CANopen__303.html',1,'']]], + ['canopen_5f304_2541',['CANopen_304',['../group__CO__CANopen__304.html',1,'']]], + ['canopen_5f305_2542',['CANopen_305',['../group__CO__CANopen__305.html',1,'']]], + ['canopen_5f309_2543',['CANopen_309',['../group__CO__CANopen__309.html',1,'']]], + ['command_20syntax_2544',['Command syntax',['../group__CO__CANopen__309__3__Syntax.html',1,'']]], + ['canopen_5fextra_2545',['CANopen_extra',['../group__CO__CANopen__extra.html',1,'']]], + ['crc_2016_20ccitt_2546',['CRC 16 CCITT',['../group__CO__crc16__ccitt.html',1,'']]], + ['critical_20sections_2547',['Critical sections',['../group__CO__critical__sections.html',1,'']]], + ['co_5fdriver_5ftarget_2eh_2548',['CO_driver_target.h',['../group__CO__socketCAN__driver__target.html',1,'']]], + ['can_20errors_20_26_20log_2549',['CAN errors & Log',['../group__CO__socketCAN__ERROR.html',1,'']]], + ['common_20definitions_2550',['Common definitions',['../group__CO__STACK__CONFIG__COMMON.html',1,'']]], + ['crc_2016_20calculation_2551',['CRC 16 calculation',['../group__CO__STACK__CONFIG__CRC16.html',1,'']]], + ['canopen_20gateway_2552',['CANopen gateway',['../group__CO__STACK__CONFIG__GATEWAY.html',1,'']]], + ['canopen_20led_20diodes_2553',['CANopen LED diodes',['../group__CO__STACK__CONFIG__LEDS.html',1,'']]] +]; diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/groups_2.html b/Devices/Libraries/Systems/CANopenSocket/docs/search/groups_2.html new file mode 100755 index 0000000..a205d30 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/groups_2.html @@ -0,0 +1,30 @@ + + + + + + + + + +
+
Loading...
+
+ +
Searching...
+
No Matches
+ +
+ + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/groups_2.js b/Devices/Libraries/Systems/CANopenSocket/docs/search/groups_2.js new file mode 100755 index 0000000..c5e229d --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/groups_2.js @@ -0,0 +1,5 @@ +var searchData= +[ + ['driver_2554',['Driver',['../group__CO__driver.html',1,'']]], + ['debug_20messages_2555',['Debug messages',['../group__CO__STACK__CONFIG__DEBUG.html',1,'']]] +]; diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/groups_3.html b/Devices/Libraries/Systems/CANopenSocket/docs/search/groups_3.html new file mode 100755 index 0000000..4255bed --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/groups_3.html @@ -0,0 +1,30 @@ + + + + + + + + + +
+
Loading...
+
+ +
Searching...
+
No Matches
+ +
+ + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/groups_3.js b/Devices/Libraries/Systems/CANopenSocket/docs/search/groups_3.js new file mode 100755 index 0000000..55b8c44 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/groups_3.js @@ -0,0 +1,6 @@ +var searchData= +[ + ['emergency_2556',['Emergency',['../group__CO__Emergency.html',1,'']]], + ['epoll_20interface_2557',['Epoll interface',['../group__CO__epoll__interface.html',1,'']]], + ['emergency_20producer_2fconsumer_2558',['Emergency producer/consumer',['../group__CO__STACK__CONFIG__EMERGENCY.html',1,'']]] +]; diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/groups_4.html b/Devices/Libraries/Systems/CANopenSocket/docs/search/groups_4.html new file mode 100755 index 0000000..8644fbe --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/groups_4.html @@ -0,0 +1,30 @@ + + + + + + + + + +
+
Loading...
+
+ +
Searching...
+
No Matches
+ +
+ + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/groups_4.js b/Devices/Libraries/Systems/CANopenSocket/docs/search/groups_4.js new file mode 100755 index 0000000..ad0d503 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/groups_4.js @@ -0,0 +1,5 @@ +var searchData= +[ + ['fifo_20circular_20buffer_2559',['FIFO circular buffer',['../group__CO__CANopen__301__fifo.html',1,'']]], + ['fifo_20buffer_2560',['FIFO buffer',['../group__CO__STACK__CONFIG__FIFO.html',1,'']]] +]; diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/groups_5.html b/Devices/Libraries/Systems/CANopenSocket/docs/search/groups_5.html new file mode 100755 index 0000000..1e9ba85 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/groups_5.html @@ -0,0 +1,30 @@ + + + + + + + + + +
+
Loading...
+
+ +
Searching...
+
No Matches
+ +
+ + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/groups_5.js b/Devices/Libraries/Systems/CANopenSocket/docs/search/groups_5.js new file mode 100755 index 0000000..c9305b9 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/groups_5.js @@ -0,0 +1,6 @@ +var searchData= +[ + ['gateway_20ascii_20mapping_2561',['Gateway ASCII mapping',['../group__CO__CANopen__309__3.html',1,'']]], + ['gfc_2562',['GFC',['../group__CO__GFC.html',1,'']]], + ['getters_20and_20setters_2563',['Getters and setters',['../group__CO__ODgetSetters.html',1,'']]] +]; diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/groups_6.html b/Devices/Libraries/Systems/CANopenSocket/docs/search/groups_6.html new file mode 100755 index 0000000..921827f --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/groups_6.html @@ -0,0 +1,30 @@ + + + + + + + + + +
+
Loading...
+
+ +
Searching...
+
No Matches
+ +
+ + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/groups_6.js b/Devices/Libraries/Systems/CANopenSocket/docs/search/groups_6.js new file mode 100755 index 0000000..5f0a503 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/groups_6.js @@ -0,0 +1,4 @@ +var searchData= +[ + ['heartbeat_20consumer_2564',['Heartbeat consumer',['../group__CO__HBconsumer.html',1,'']]] +]; diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/groups_7.html b/Devices/Libraries/Systems/CANopenSocket/docs/search/groups_7.html new file mode 100755 index 0000000..254468f --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/groups_7.html @@ -0,0 +1,30 @@ + + + + + + + + + +
+
Loading...
+
+ +
Searching...
+
No Matches
+ +
+ + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/groups_7.js b/Devices/Libraries/Systems/CANopenSocket/docs/search/groups_7.js new file mode 100755 index 0000000..7bbf9f3 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/groups_7.js @@ -0,0 +1,8 @@ +var searchData= +[ + ['led_20indicators_2565',['LED indicators',['../group__CO__LEDs.html',1,'']]], + ['lss_2566',['LSS',['../group__CO__LSS.html',1,'']]], + ['lss_20master_2567',['LSS Master',['../group__CO__LSSmaster.html',1,'']]], + ['lss_20slave_2568',['LSS Slave',['../group__CO__LSSslave.html',1,'']]], + ['lss_20master_2fslave_2569',['LSS master/slave',['../group__CO__STACK__CONFIG__LSS.html',1,'']]] +]; diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/groups_8.html b/Devices/Libraries/Systems/CANopenSocket/docs/search/groups_8.html new file mode 100755 index 0000000..726aca2 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/groups_8.html @@ -0,0 +1,30 @@ + + + + + + + + + +
+
Loading...
+
+ +
Searching...
+
No Matches
+ +
+ + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/groups_8.js b/Devices/Libraries/Systems/CANopenSocket/docs/search/groups_8.js new file mode 100755 index 0000000..371f8ed --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/groups_8.js @@ -0,0 +1,5 @@ +var searchData= +[ + ['nmt_20and_20heartbeat_2570',['NMT and Heartbeat',['../group__CO__NMT__Heartbeat.html',1,'']]], + ['nmt_20master_2fslave_20and_20hb_20producer_2fconsumer_2571',['NMT master/slave and HB producer/consumer',['../group__CO__STACK__CONFIG__NMT__HB.html',1,'']]] +]; diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/groups_9.html b/Devices/Libraries/Systems/CANopenSocket/docs/search/groups_9.html new file mode 100755 index 0000000..b5dc589 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/groups_9.html @@ -0,0 +1,30 @@ + + + + + + + + + +
+
Loading...
+
+ +
Searching...
+
No Matches
+ +
+ + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/groups_9.js b/Devices/Libraries/Systems/CANopenSocket/docs/search/groups_9.js new file mode 100755 index 0000000..dcbb5c3 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/groups_9.js @@ -0,0 +1,6 @@ +var searchData= +[ + ['od_20definition_20objects_2572',['OD definition objects',['../group__CO__ODdefinition.html',1,'']]], + ['od_20interface_2573',['OD interface',['../group__CO__ODinterface.html',1,'']]], + ['od_20storage_2574',['OD storage',['../group__CO__socketCAN__OD__storage.html',1,'']]] +]; diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/groups_a.html b/Devices/Libraries/Systems/CANopenSocket/docs/search/groups_a.html new file mode 100755 index 0000000..93fc4d5 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/groups_a.html @@ -0,0 +1,30 @@ + + + + + + + + + +
+
Loading...
+
+ +
Searching...
+
No Matches
+ +
+ + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/groups_a.js b/Devices/Libraries/Systems/CANopenSocket/docs/search/groups_a.js new file mode 100755 index 0000000..6978796 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/groups_a.js @@ -0,0 +1,4 @@ +var searchData= +[ + ['pdo_2575',['PDO',['../group__CO__PDO.html',1,'']]] +]; diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/groups_b.html b/Devices/Libraries/Systems/CANopenSocket/docs/search/groups_b.html new file mode 100755 index 0000000..81b6ddc --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/groups_b.html @@ -0,0 +1,30 @@ + + + + + + + + + +
+
Loading...
+
+ +
Searching...
+
No Matches
+ +
+ + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/groups_b.js b/Devices/Libraries/Systems/CANopenSocket/docs/search/groups_b.js new file mode 100755 index 0000000..7c69b21 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/groups_b.js @@ -0,0 +1,4 @@ +var searchData= +[ + ['reception_20of_20can_20messages_2576',['Reception of CAN messages',['../group__CO__CAN__Message__reception.html',1,'']]] +]; diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/groups_c.html b/Devices/Libraries/Systems/CANopenSocket/docs/search/groups_c.html new file mode 100755 index 0000000..ad0bac9 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/groups_c.html @@ -0,0 +1,30 @@ + + + + + + + + + +
+
Loading...
+
+ +
Searching...
+
No Matches
+ +
+ + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/groups_c.js b/Devices/Libraries/Systems/CANopenSocket/docs/search/groups_c.js new file mode 100755 index 0000000..f4c80d1 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/groups_c.js @@ -0,0 +1,12 @@ +var searchData= +[ + ['sdo_20client_2577',['SDO client',['../group__CO__SDOclient.html',1,'']]], + ['sdo_20server_2578',['SDO server',['../group__CO__SDOserver.html',1,'']]], + ['socketcan_2579',['socketCAN',['../group__CO__socketCAN.html',1,'']]], + ['srdo_2580',['SRDO',['../group__CO__SRDO.html',1,'']]], + ['stack_20configuration_2581',['Stack configuration',['../group__CO__STACK__CONFIG.html',1,'']]], + ['sdo_20server_2fclient_2582',['SDO server/client',['../group__CO__STACK__CONFIG__SDO.html',1,'']]], + ['safety_20related_20data_20objects_20_28srdo_29_2583',['Safety Related Data Objects (SRDO)',['../group__CO__STACK__CONFIG__SRDO.html',1,'']]], + ['sync_20and_20pdo_20producer_2fconsumer_2584',['SYNC and PDO producer/consumer',['../group__CO__STACK__CONFIG__SYNC__PDO.html',1,'']]], + ['sync_2585',['SYNC',['../group__CO__SYNC.html',1,'']]] +]; diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/groups_d.html b/Devices/Libraries/Systems/CANopenSocket/docs/search/groups_d.html new file mode 100755 index 0000000..4a51b91 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/groups_d.html @@ -0,0 +1,30 @@ + + + + + + + + + +
+
Loading...
+
+ +
Searching...
+
No Matches
+ +
+ + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/groups_d.js b/Devices/Libraries/Systems/CANopenSocket/docs/search/groups_d.js new file mode 100755 index 0000000..238a77d --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/groups_d.js @@ -0,0 +1,8 @@ +var searchData= +[ + ['transmission_20of_20can_20messages_2586',['Transmission of CAN messages',['../group__CO__CAN__Message__transmission.html',1,'']]], + ['time_20producer_2fconsumer_2587',['Time producer/consumer',['../group__CO__STACK__CONFIG__TIME.html',1,'']]], + ['trace_20recorder_2588',['Trace recorder',['../group__CO__STACK__CONFIG__TRACE.html',1,'']]], + ['time_2589',['TIME',['../group__CO__TIME.html',1,'']]], + ['trace_2590',['Trace',['../group__CO__trace.html',1,'']]] +]; diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/mag_sel.png b/Devices/Libraries/Systems/CANopenSocket/docs/search/mag_sel.png new file mode 100755 index 0000000..39c0ed5 Binary files /dev/null and b/Devices/Libraries/Systems/CANopenSocket/docs/search/mag_sel.png differ diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/nomatches.html b/Devices/Libraries/Systems/CANopenSocket/docs/search/nomatches.html new file mode 100755 index 0000000..4377320 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/nomatches.html @@ -0,0 +1,12 @@ + + + + + + + +
+
No Matches
+
+ + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/pages_0.html b/Devices/Libraries/Systems/CANopenSocket/docs/search/pages_0.html new file mode 100755 index 0000000..9a6a29a --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/pages_0.html @@ -0,0 +1,30 @@ + + + + + + + + + +
+
Loading...
+
+ +
Searching...
+
No Matches
+ +
+ + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/pages_0.js b/Devices/Libraries/Systems/CANopenSocket/docs/search/pages_0.js new file mode 100755 index 0000000..9386335 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/pages_0.js @@ -0,0 +1,6 @@ +var searchData= +[ + ['canopennode_2591',['CANopenNode',['../index.html',1,'']]], + ['change_20log_2592',['Change Log',['../md_doc_CHANGELOG.html',1,'']]], + ['canopen_20documentation_2593',['CANopen documentation',['../md_example_DS301_profile.html',1,'']]] +]; diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/pages_1.html b/Devices/Libraries/Systems/CANopenSocket/docs/search/pages_1.html new file mode 100755 index 0000000..132ee03 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/pages_1.html @@ -0,0 +1,30 @@ + + + + + + + + + +
+
Loading...
+
+ +
Searching...
+
No Matches
+ +
+ + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/pages_1.js b/Devices/Libraries/Systems/CANopenSocket/docs/search/pages_1.js new file mode 100755 index 0000000..b383c24 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/pages_1.js @@ -0,0 +1,4 @@ +var searchData= +[ + ['device_20support_2594',['Device Support',['../md_doc_deviceSupport.html',1,'']]] +]; diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/pages_2.html b/Devices/Libraries/Systems/CANopenSocket/docs/search/pages_2.html new file mode 100755 index 0000000..6109d47 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/pages_2.html @@ -0,0 +1,30 @@ + + + + + + + + + +
+
Loading...
+
+ +
Searching...
+
No Matches
+ +
+ + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/pages_2.js b/Devices/Libraries/Systems/CANopenSocket/docs/search/pages_2.js new file mode 100755 index 0000000..3944034 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/pages_2.js @@ -0,0 +1,4 @@ +var searchData= +[ + ['getting_20started_2595',['Getting Started',['../md_doc_gettingStarted.html',1,'']]] +]; diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/pages_3.html b/Devices/Libraries/Systems/CANopenSocket/docs/search/pages_3.html new file mode 100755 index 0000000..54e8ba9 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/pages_3.html @@ -0,0 +1,30 @@ + + + + + + + + + +
+
Loading...
+
+ +
Searching...
+
No Matches
+ +
+ + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/pages_3.js b/Devices/Libraries/Systems/CANopenSocket/docs/search/pages_3.js new file mode 100755 index 0000000..ff411c1 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/pages_3.js @@ -0,0 +1,4 @@ +var searchData= +[ + ['lss_20usage_2596',['LSS usage',['../md_doc_LSSusage.html',1,'']]] +]; diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/pages_4.html b/Devices/Libraries/Systems/CANopenSocket/docs/search/pages_4.html new file mode 100755 index 0000000..1ab6c5a --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/pages_4.html @@ -0,0 +1,30 @@ + + + + + + + + + +
+
Loading...
+
+ +
Searching...
+
No Matches
+ +
+ + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/pages_4.js b/Devices/Libraries/Systems/CANopenSocket/docs/search/pages_4.js new file mode 100755 index 0000000..9e7cb7d --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/pages_4.js @@ -0,0 +1,4 @@ +var searchData= +[ + ['object_20dictionary_2597',['Object Dictionary',['../md_doc_objectDictionary.html',1,'']]] +]; diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/pages_5.html b/Devices/Libraries/Systems/CANopenSocket/docs/search/pages_5.html new file mode 100755 index 0000000..b031763 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/pages_5.html @@ -0,0 +1,30 @@ + + + + + + + + + +
+
Loading...
+
+ +
Searching...
+
No Matches
+ +
+ + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/pages_5.js b/Devices/Libraries/Systems/CANopenSocket/docs/search/pages_5.js new file mode 100755 index 0000000..60c2994 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/pages_5.js @@ -0,0 +1,4 @@ +var searchData= +[ + ['trace_20usage_2598',['Trace usage',['../md_doc_traceUsage.html',1,'']]] +]; diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/search.css b/Devices/Libraries/Systems/CANopenSocket/docs/search/search.css new file mode 100755 index 0000000..a0dba44 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/search.css @@ -0,0 +1,273 @@ +/*---------------- Search Box */ + +#FSearchBox { + float: left; +} + +#MSearchBox { + white-space : nowrap; + float: none; + margin-top: 0px; + right: 0px; + width: 170px; + height: 24px; + z-index: 102; + display: inline; + position: absolute; +} + +#MSearchBox .left +{ + display:block; + position:absolute; + left:10px; + width:20px; + height:19px; + background:url('search_l.png') no-repeat; + background-position:right; +} + +#MSearchSelect { + display:block; + position:absolute; + width:20px; + height:19px; +} + +.left #MSearchSelect { + left:4px; +} + +.right #MSearchSelect { + right:5px; +} + +#MSearchField { + display:block; + position:absolute; + height:19px; + background:url('search_m.png') repeat-x; + border:none; + width:111px; + margin-left:20px; + padding-left:4px; + color: #909090; + outline: none; + font: 9pt Arial, Verdana, sans-serif; + -webkit-border-radius: 0px; +} + +#FSearchBox #MSearchField { + margin-left:15px; +} + +#MSearchBox .right { + display:block; + position:absolute; + right:10px; + top:0px; + width:20px; + height:19px; + background:url('search_r.png') no-repeat; + background-position:left; +} + +#MSearchClose { + display: none; + position: absolute; + top: 4px; + background : none; + border: none; + margin: 0px 4px 0px 0px; + padding: 0px 0px; + outline: none; +} + +.left #MSearchClose { + left: 6px; +} + +.right #MSearchClose { + right: 2px; +} + +.MSearchBoxActive #MSearchField { + color: #000000; +} + +/*---------------- Search filter selection */ + +#MSearchSelectWindow { + display: none; + position: absolute; + left: 0; top: 0; + border: 1px solid #90A5CE; + background-color: #F9FAFC; + z-index: 10001; + padding-top: 4px; + padding-bottom: 4px; + -moz-border-radius: 4px; + -webkit-border-top-left-radius: 4px; + -webkit-border-top-right-radius: 4px; + -webkit-border-bottom-left-radius: 4px; + -webkit-border-bottom-right-radius: 4px; + -webkit-box-shadow: 5px 5px 5px rgba(0, 0, 0, 0.15); +} + +.SelectItem { + font: 8pt Arial, Verdana, sans-serif; + padding-left: 2px; + padding-right: 12px; + border: 0px; +} + +span.SelectionMark { + margin-right: 4px; + font-family: monospace; + outline-style: none; + text-decoration: none; +} + +a.SelectItem { + display: block; + outline-style: none; + color: #000000; + text-decoration: none; + padding-left: 6px; + padding-right: 12px; +} + +a.SelectItem:focus, +a.SelectItem:active { + color: #000000; + outline-style: none; + text-decoration: none; +} + +a.SelectItem:hover { + color: #FFFFFF; + background-color: #3D578C; + outline-style: none; + text-decoration: none; + cursor: pointer; + display: block; +} + +/*---------------- Search results window */ + +iframe#MSearchResults { + width: 60ex; + height: 15em; +} + +#MSearchResultsWindow { + display: none; + position: absolute; + left: 0; top: 0; + border: 1px solid #000; + background-color: #EEF1F7; + z-index:10000; +} + +/* ----------------------------------- */ + + +#SRIndex { + clear:both; + padding-bottom: 15px; +} + +.SREntry { + font-size: 10pt; + padding-left: 1ex; +} + +.SRPage .SREntry { + font-size: 8pt; + padding: 1px 5px; +} + +body.SRPage { + margin: 5px 2px; +} + +.SRChildren { + padding-left: 3ex; padding-bottom: .5em +} + +.SRPage .SRChildren { + display: none; +} + +.SRSymbol { + font-weight: bold; + color: #425E97; + font-family: Arial, Verdana, sans-serif; + text-decoration: none; + outline: none; +} + +a.SRScope { + display: block; + color: #425E97; + font-family: Arial, Verdana, sans-serif; + text-decoration: none; + outline: none; +} + +a.SRSymbol:focus, a.SRSymbol:active, +a.SRScope:focus, a.SRScope:active { + text-decoration: underline; +} + +span.SRScope { + padding-left: 4px; +} + +.SRPage .SRStatus { + padding: 2px 5px; + font-size: 8pt; + font-style: italic; +} + +.SRResult { + display: none; +} + +DIV.searchresults { + margin-left: 10px; + margin-right: 10px; +} + +/*---------------- External search page results */ + +.searchresult { + background-color: #F0F3F8; +} + +.pages b { + color: white; + padding: 5px 5px 3px 5px; + background-image: url("../tab_a.png"); + background-repeat: repeat-x; + text-shadow: 0 1px 1px #000000; +} + +.pages { + line-height: 17px; + margin-left: 4px; + text-decoration: none; +} + +.hl { + font-weight: bold; +} + +#searchresults { + margin-bottom: 20px; +} + +.searchpages { + margin-top: 10px; +} + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/search.js b/Devices/Libraries/Systems/CANopenSocket/docs/search/search.js new file mode 100755 index 0000000..a554ab9 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/search.js @@ -0,0 +1,814 @@ +/* + @licstart The following is the entire license notice for the + JavaScript code in this file. + + Copyright (C) 1997-2017 by Dimitri van Heesch + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 2 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License along + with this program; if not, write to the Free Software Foundation, Inc., + 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. + + @licend The above is the entire license notice + for the JavaScript code in this file + */ +function convertToId(search) +{ + var result = ''; + for (i=0;i do a search + { + this.Search(); + } + } + + this.OnSearchSelectKey = function(evt) + { + var e = (evt) ? evt : window.event; // for IE + if (e.keyCode==40 && this.searchIndex0) // Up + { + this.searchIndex--; + this.OnSelectItem(this.searchIndex); + } + else if (e.keyCode==13 || e.keyCode==27) + { + this.OnSelectItem(this.searchIndex); + this.CloseSelectionWindow(); + this.DOMSearchField().focus(); + } + return false; + } + + // --------- Actions + + // Closes the results window. + this.CloseResultsWindow = function() + { + this.DOMPopupSearchResultsWindow().style.display = 'none'; + this.DOMSearchClose().style.display = 'none'; + this.Activate(false); + } + + this.CloseSelectionWindow = function() + { + this.DOMSearchSelectWindow().style.display = 'none'; + } + + // Performs a search. + this.Search = function() + { + this.keyTimeout = 0; + + // strip leading whitespace + var searchValue = this.DOMSearchField().value.replace(/^ +/, ""); + + var code = searchValue.toLowerCase().charCodeAt(0); + var idxChar = searchValue.substr(0, 1).toLowerCase(); + if ( 0xD800 <= code && code <= 0xDBFF && searchValue > 1) // surrogate pair + { + idxChar = searchValue.substr(0, 2); + } + + var resultsPage; + var resultsPageWithSearch; + var hasResultsPage; + + var idx = indexSectionsWithContent[this.searchIndex].indexOf(idxChar); + if (idx!=-1) + { + var hexCode=idx.toString(16); + resultsPage = this.resultsPath + '/' + indexSectionNames[this.searchIndex] + '_' + hexCode + '.html'; + resultsPageWithSearch = resultsPage+'?'+escape(searchValue); + hasResultsPage = true; + } + else // nothing available for this search term + { + resultsPage = this.resultsPath + '/nomatches.html'; + resultsPageWithSearch = resultsPage; + hasResultsPage = false; + } + + window.frames.MSearchResults.location = resultsPageWithSearch; + var domPopupSearchResultsWindow = this.DOMPopupSearchResultsWindow(); + + if (domPopupSearchResultsWindow.style.display!='block') + { + var domSearchBox = this.DOMSearchBox(); + this.DOMSearchClose().style.display = 'inline'; + if (this.insideFrame) + { + var domPopupSearchResults = this.DOMPopupSearchResults(); + domPopupSearchResultsWindow.style.position = 'relative'; + domPopupSearchResultsWindow.style.display = 'block'; + var width = document.body.clientWidth - 8; // the -8 is for IE :-( + domPopupSearchResultsWindow.style.width = width + 'px'; + domPopupSearchResults.style.width = width + 'px'; + } + else + { + var domPopupSearchResults = this.DOMPopupSearchResults(); + var left = getXPos(domSearchBox) + 150; // domSearchBox.offsetWidth; + var top = getYPos(domSearchBox) + 20; // domSearchBox.offsetHeight + 1; + domPopupSearchResultsWindow.style.display = 'block'; + left -= domPopupSearchResults.offsetWidth; + domPopupSearchResultsWindow.style.top = top + 'px'; + domPopupSearchResultsWindow.style.left = left + 'px'; + } + } + + this.lastSearchValue = searchValue; + this.lastResultsPage = resultsPage; + } + + // -------- Activation Functions + + // Activates or deactivates the search panel, resetting things to + // their default values if necessary. + this.Activate = function(isActive) + { + if (isActive || // open it + this.DOMPopupSearchResultsWindow().style.display == 'block' + ) + { + this.DOMSearchBox().className = 'MSearchBoxActive'; + + var searchField = this.DOMSearchField(); + + if (searchField.value == this.searchLabel) // clear "Search" term upon entry + { + searchField.value = ''; + this.searchActive = true; + } + } + else if (!isActive) // directly remove the panel + { + this.DOMSearchBox().className = 'MSearchBoxInactive'; + this.DOMSearchField().value = this.searchLabel; + this.searchActive = false; + this.lastSearchValue = '' + this.lastResultsPage = ''; + } + } +} + +// ----------------------------------------------------------------------- + +// The class that handles everything on the search results page. +function SearchResults(name) +{ + // The number of matches from the last run of . + this.lastMatchCount = 0; + this.lastKey = 0; + this.repeatOn = false; + + // Toggles the visibility of the passed element ID. + this.FindChildElement = function(id) + { + var parentElement = document.getElementById(id); + var element = parentElement.firstChild; + + while (element && element!=parentElement) + { + if (element.nodeName == 'DIV' && element.className == 'SRChildren') + { + return element; + } + + if (element.nodeName == 'DIV' && element.hasChildNodes()) + { + element = element.firstChild; + } + else if (element.nextSibling) + { + element = element.nextSibling; + } + else + { + do + { + element = element.parentNode; + } + while (element && element!=parentElement && !element.nextSibling); + + if (element && element!=parentElement) + { + element = element.nextSibling; + } + } + } + } + + this.Toggle = function(id) + { + var element = this.FindChildElement(id); + if (element) + { + if (element.style.display == 'block') + { + element.style.display = 'none'; + } + else + { + element.style.display = 'block'; + } + } + } + + // Searches for the passed string. If there is no parameter, + // it takes it from the URL query. + // + // Always returns true, since other documents may try to call it + // and that may or may not be possible. + this.Search = function(search) + { + if (!search) // get search word from URL + { + search = window.location.search; + search = search.substring(1); // Remove the leading '?' + search = unescape(search); + } + + search = search.replace(/^ +/, ""); // strip leading spaces + search = search.replace(/ +$/, ""); // strip trailing spaces + search = search.toLowerCase(); + search = convertToId(search); + + var resultRows = document.getElementsByTagName("div"); + var matches = 0; + + var i = 0; + while (i < resultRows.length) + { + var row = resultRows.item(i); + if (row.className == "SRResult") + { + var rowMatchName = row.id.toLowerCase(); + rowMatchName = rowMatchName.replace(/^sr\d*_/, ''); // strip 'sr123_' + + if (search.length<=rowMatchName.length && + rowMatchName.substr(0, search.length)==search) + { + row.style.display = 'block'; + matches++; + } + else + { + row.style.display = 'none'; + } + } + i++; + } + document.getElementById("Searching").style.display='none'; + if (matches == 0) // no results + { + document.getElementById("NoMatches").style.display='block'; + } + else // at least one result + { + document.getElementById("NoMatches").style.display='none'; + } + this.lastMatchCount = matches; + return true; + } + + // return the first item with index index or higher that is visible + this.NavNext = function(index) + { + var focusItem; + while (1) + { + var focusName = 'Item'+index; + focusItem = document.getElementById(focusName); + if (focusItem && focusItem.parentNode.parentNode.style.display=='block') + { + break; + } + else if (!focusItem) // last element + { + break; + } + focusItem=null; + index++; + } + return focusItem; + } + + this.NavPrev = function(index) + { + var focusItem; + while (1) + { + var focusName = 'Item'+index; + focusItem = document.getElementById(focusName); + if (focusItem && focusItem.parentNode.parentNode.style.display=='block') + { + break; + } + else if (!focusItem) // last element + { + break; + } + focusItem=null; + index--; + } + return focusItem; + } + + this.ProcessKeys = function(e) + { + if (e.type == "keydown") + { + this.repeatOn = false; + this.lastKey = e.keyCode; + } + else if (e.type == "keypress") + { + if (!this.repeatOn) + { + if (this.lastKey) this.repeatOn = true; + return false; // ignore first keypress after keydown + } + } + else if (e.type == "keyup") + { + this.lastKey = 0; + this.repeatOn = false; + } + return this.lastKey!=0; + } + + this.Nav = function(evt,itemIndex) + { + var e = (evt) ? evt : window.event; // for IE + if (e.keyCode==13) return true; + if (!this.ProcessKeys(e)) return false; + + if (this.lastKey==38) // Up + { + var newIndex = itemIndex-1; + var focusItem = this.NavPrev(newIndex); + if (focusItem) + { + var child = this.FindChildElement(focusItem.parentNode.parentNode.id); + if (child && child.style.display == 'block') // children visible + { + var n=0; + var tmpElem; + while (1) // search for last child + { + tmpElem = document.getElementById('Item'+newIndex+'_c'+n); + if (tmpElem) + { + focusItem = tmpElem; + } + else // found it! + { + break; + } + n++; + } + } + } + if (focusItem) + { + focusItem.focus(); + } + else // return focus to search field + { + parent.document.getElementById("MSearchField").focus(); + } + } + else if (this.lastKey==40) // Down + { + var newIndex = itemIndex+1; + var focusItem; + var item = document.getElementById('Item'+itemIndex); + var elem = this.FindChildElement(item.parentNode.parentNode.id); + if (elem && elem.style.display == 'block') // children visible + { + focusItem = document.getElementById('Item'+itemIndex+'_c0'); + } + if (!focusItem) focusItem = this.NavNext(newIndex); + if (focusItem) focusItem.focus(); + } + else if (this.lastKey==39) // Right + { + var item = document.getElementById('Item'+itemIndex); + var elem = this.FindChildElement(item.parentNode.parentNode.id); + if (elem) elem.style.display = 'block'; + } + else if (this.lastKey==37) // Left + { + var item = document.getElementById('Item'+itemIndex); + var elem = this.FindChildElement(item.parentNode.parentNode.id); + if (elem) elem.style.display = 'none'; + } + else if (this.lastKey==27) // Escape + { + parent.searchBox.CloseResultsWindow(); + parent.document.getElementById("MSearchField").focus(); + } + else if (this.lastKey==13) // Enter + { + return true; + } + return false; + } + + this.NavChild = function(evt,itemIndex,childIndex) + { + var e = (evt) ? evt : window.event; // for IE + if (e.keyCode==13) return true; + if (!this.ProcessKeys(e)) return false; + + if (this.lastKey==38) // Up + { + if (childIndex>0) + { + var newIndex = childIndex-1; + document.getElementById('Item'+itemIndex+'_c'+newIndex).focus(); + } + else // already at first child, jump to parent + { + document.getElementById('Item'+itemIndex).focus(); + } + } + else if (this.lastKey==40) // Down + { + var newIndex = childIndex+1; + var elem = document.getElementById('Item'+itemIndex+'_c'+newIndex); + if (!elem) // last child, jump to parent next parent + { + elem = this.NavNext(itemIndex+1); + } + if (elem) + { + elem.focus(); + } + } + else if (this.lastKey==27) // Escape + { + parent.searchBox.CloseResultsWindow(); + parent.document.getElementById("MSearchField").focus(); + } + else if (this.lastKey==13) // Enter + { + return true; + } + return false; + } +} + +function setKeyActions(elem,action) +{ + elem.setAttribute('onkeydown',action); + elem.setAttribute('onkeypress',action); + elem.setAttribute('onkeyup',action); +} + +function setClassAttr(elem,attr) +{ + elem.setAttribute('class',attr); + elem.setAttribute('className',attr); +} + +function createResults() +{ + var results = document.getElementById("SRResults"); + for (var e=0; e + + + + + + + + +
+
Loading...
+
+ +
Searching...
+
No Matches
+ +
+ + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/typedefs_0.js b/Devices/Libraries/Systems/CANopenSocket/docs/search/typedefs_0.js new file mode 100755 index 0000000..98ce795 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/typedefs_0.js @@ -0,0 +1,4 @@ +var searchData= +[ + ['bool_5ft_2030',['bool_t',['../group__CO__dataTypes.html#ga449976458a084f880dc8e3d29e7eb6f5',1,'CO_driver.h']]] +]; diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/typedefs_1.html b/Devices/Libraries/Systems/CANopenSocket/docs/search/typedefs_1.html new file mode 100755 index 0000000..9b8bf72 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/typedefs_1.html @@ -0,0 +1,30 @@ + + + + + + + + + +
+
Loading...
+
+ +
Searching...
+
No Matches
+ +
+ + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/typedefs_1.js b/Devices/Libraries/Systems/CANopenSocket/docs/search/typedefs_1.js new file mode 100755 index 0000000..44956bf --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/typedefs_1.js @@ -0,0 +1,4 @@ +var searchData= +[ + ['char_5ft_2031',['char_t',['../group__CO__dataTypes.html#ga40bb5262bf908c328fbcfbe5d29d0201',1,'CO_driver.h']]] +]; diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/typedefs_2.html b/Devices/Libraries/Systems/CANopenSocket/docs/search/typedefs_2.html new file mode 100755 index 0000000..d18982f --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/typedefs_2.html @@ -0,0 +1,30 @@ + + + + + + + + + +
+
Loading...
+
+ +
Searching...
+
No Matches
+ +
+ + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/typedefs_2.js b/Devices/Libraries/Systems/CANopenSocket/docs/search/typedefs_2.js new file mode 100755 index 0000000..402104f --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/typedefs_2.js @@ -0,0 +1,4 @@ +var searchData= +[ + ['domain_5ft_2032',['domain_t',['../group__CO__dataTypes.html#gadc433a2a90dacd3b2b3801dd9431c254',1,'CO_driver.h']]] +]; diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/typedefs_3.html b/Devices/Libraries/Systems/CANopenSocket/docs/search/typedefs_3.html new file mode 100755 index 0000000..8941740 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/typedefs_3.html @@ -0,0 +1,30 @@ + + + + + + + + + +
+
Loading...
+
+ +
Searching...
+
No Matches
+ +
+ + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/typedefs_3.js b/Devices/Libraries/Systems/CANopenSocket/docs/search/typedefs_3.js new file mode 100755 index 0000000..17542b1 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/typedefs_3.js @@ -0,0 +1,5 @@ +var searchData= +[ + ['float32_5ft_2033',['float32_t',['../group__CO__dataTypes.html#ga4611b605e45ab401f02cab15c5e38715',1,'CO_driver.h']]], + ['float64_5ft_2034',['float64_t',['../group__CO__dataTypes.html#gac55f3ae81b5bc9053760baacf57e47f4',1,'CO_driver.h']]] +]; diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/typedefs_4.html b/Devices/Libraries/Systems/CANopenSocket/docs/search/typedefs_4.html new file mode 100755 index 0000000..933bd3b --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/typedefs_4.html @@ -0,0 +1,30 @@ + + + + + + + + + +
+
Loading...
+
+ +
Searching...
+
No Matches
+ +
+ + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/typedefs_4.js b/Devices/Libraries/Systems/CANopenSocket/docs/search/typedefs_4.js new file mode 100755 index 0000000..e7a6b59 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/typedefs_4.js @@ -0,0 +1,7 @@ +var searchData= +[ + ['int16_5ft_2035',['int16_t',['../group__CO__dataTypes.html#ga932e6ccc3d54c58f761c1aead83bd6d7',1,'CO_driver.h']]], + ['int32_5ft_2036',['int32_t',['../group__CO__dataTypes.html#gadb828ef50c2dbb783109824e94cf6c47',1,'CO_driver.h']]], + ['int64_5ft_2037',['int64_t',['../group__CO__dataTypes.html#ga831d6234342279926bb11bad3a37add9',1,'CO_driver.h']]], + ['int8_5ft_2038',['int8_t',['../group__CO__dataTypes.html#gaef44329758059c91c76d334e8fc09700',1,'CO_driver.h']]] +]; diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/typedefs_5.html b/Devices/Libraries/Systems/CANopenSocket/docs/search/typedefs_5.html new file mode 100755 index 0000000..7712e6f --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/typedefs_5.html @@ -0,0 +1,30 @@ + + + + + + + + + +
+
Loading...
+
+ +
Searching...
+
No Matches
+ +
+ + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/typedefs_5.js b/Devices/Libraries/Systems/CANopenSocket/docs/search/typedefs_5.js new file mode 100755 index 0000000..5879ba0 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/typedefs_5.js @@ -0,0 +1,4 @@ +var searchData= +[ + ['ochar_5ft_2039',['oChar_t',['../group__CO__dataTypes.html#ga00f664c467579d7b2839d6926b6f33a6',1,'CO_driver.h']]] +]; diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/typedefs_6.html b/Devices/Libraries/Systems/CANopenSocket/docs/search/typedefs_6.html new file mode 100755 index 0000000..25aa6dd --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/typedefs_6.html @@ -0,0 +1,30 @@ + + + + + + + + + +
+
Loading...
+
+ +
Searching...
+
No Matches
+ +
+ + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/typedefs_6.js b/Devices/Libraries/Systems/CANopenSocket/docs/search/typedefs_6.js new file mode 100755 index 0000000..9e1f8a7 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/typedefs_6.js @@ -0,0 +1,7 @@ +var searchData= +[ + ['uint16_5ft_2040',['uint16_t',['../group__CO__dataTypes.html#ga1f1825b69244eb3ad2c7165ddc99c956',1,'CO_driver.h']]], + ['uint32_5ft_2041',['uint32_t',['../group__CO__dataTypes.html#ga33594304e786b158f3fb30289278f5af',1,'CO_driver.h']]], + ['uint64_5ft_2042',['uint64_t',['../group__CO__dataTypes.html#gad27ed092432b64ff558d2254c278720f',1,'CO_driver.h']]], + ['uint8_5ft_2043',['uint8_t',['../group__CO__dataTypes.html#gaba7bc1797add20fe3efdf37ced1182c5',1,'CO_driver.h']]] +]; diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_0.html b/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_0.html new file mode 100755 index 0000000..bf3eba5 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_0.html @@ -0,0 +1,30 @@ + + + + + + + + + +
+
Loading...
+
+ +
Searching...
+
No Matches
+ +
+ + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_0.js b/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_0.js new file mode 100755 index 0000000..427098f --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_0.js @@ -0,0 +1,10 @@ +var searchData= +[ + ['activenodeid_1652',['activeNodeID',['../structCO__LSSslave__t.html#a91bb370cba5215ddaf52c0883a9bdca2',1,'CO_LSSslave_t']]], + ['allmonitoredactive_1653',['allMonitoredActive',['../structCO__HBconsumer__t.html#aaff60bb59e36a3b0ddd11b45268eaf33',1,'CO_HBconsumer_t']]], + ['allmonitoredoperational_1654',['allMonitoredOperational',['../structCO__HBconsumer__t.html#a9407103796db857229ec5b266c580b37',1,'CO_HBconsumer_t']]], + ['altreadptr_1655',['altReadPtr',['../structCO__fifo__t.html#a4f8eadd2e9b966ce21274cbbceb3adbe',1,'CO_fifo_t']]], + ['attribute_1656',['attribute',['../structOD__subEntry__t.html#ae7d83df4e106219f32cb28d7c510b9d2',1,'OD_subEntry_t::attribute()'],['../structOD__obj__var__t.html#a4662bd6ca12b3ec147f9ffeafb64fe77',1,'OD_obj_var_t::attribute()'],['../structOD__obj__array__t.html#a6af20a410bcd0c8c9f619c4a564b962a',1,'OD_obj_array_t::attribute()'],['../structOD__obj__record__t.html#a42290a19541170f8d108acf029fec171',1,'OD_obj_record_t::attribute()'],['../structCO__SDOclient__t.html#a609088a2005febd6cd1561288cf1b7d1',1,'CO_SDOclient_t::attribute()'],['../structCO__SDOserver__t.html#a3b2febaed4df4921626367a741008400',1,'CO_SDOserver_t::attribute()']]], + ['attribute0_1657',['attribute0',['../structOD__obj__array__t.html#a1cb4802d94112e5bd2f1b0db5e3e5d99',1,'OD_obj_array_t']]], + ['aux_1658',['aux',['../structCO__fifo__t.html#aa255bcb00601a8f4225c97ad6cd854a7',1,'CO_fifo_t']]] +]; diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_1.html b/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_1.html new file mode 100755 index 0000000..49fe59a --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_1.html @@ -0,0 +1,30 @@ + + + + + + + + + +
+
Loading...
+
+ +
Searching...
+
No Matches
+ +
+ + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_1.js b/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_1.js new file mode 100755 index 0000000..aced15e --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_1.js @@ -0,0 +1,19 @@ +var searchData= +[ + ['block_5fblksize_1659',['block_blksize',['../structCO__SDOclient__t.html#a36b10791595b638309a01418d13a745f',1,'CO_SDOclient_t::block_blksize()'],['../structCO__SDOserver__t.html#a76e4c66e15027e78b8ba67bdd3089cc3',1,'CO_SDOserver_t::block_blksize()']]], + ['block_5fcrc_1660',['block_crc',['../structCO__SDOclient__t.html#a51322a623ff85d36a8be60c0fe11430e',1,'CO_SDOclient_t::block_crc()'],['../structCO__SDOserver__t.html#a33c2432ccea06da1d5c33c89c14caf63',1,'CO_SDOserver_t::block_crc()']]], + ['block_5fcrcenabled_1661',['block_crcEnabled',['../structCO__SDOclient__t.html#a8690a5e7ee83fb7e0fa3a76cdec83f3a',1,'CO_SDOclient_t::block_crcEnabled()'],['../structCO__SDOserver__t.html#abdf7205835b75f0f6feef3bc89a86c17',1,'CO_SDOserver_t::block_crcEnabled()']]], + ['block_5fdatauploadlast_1662',['block_dataUploadLast',['../structCO__SDOclient__t.html#ae9e678cb0e461851298658c7eee01334',1,'CO_SDOclient_t']]], + ['block_5fnodata_1663',['block_noData',['../structCO__SDOclient__t.html#a26f9fcf95f47a4f7eeaefdf684e317a1',1,'CO_SDOclient_t::block_noData()'],['../structCO__SDOserver__t.html#a54bac23ac93450234a858c50f0516d05',1,'CO_SDOserver_t::block_noData()']]], + ['block_5fsdotimeouttime_5fus_1664',['block_SDOtimeoutTime_us',['../structCO__SDOclient__t.html#ae86079157706e7db12d9d4817172ba10',1,'CO_SDOclient_t::block_SDOtimeoutTime_us()'],['../structCO__SDOserver__t.html#ae1e16955965dced9464abd0a6bf8c2b2',1,'CO_SDOserver_t::block_SDOtimeoutTime_us()']]], + ['block_5fseqno_1665',['block_seqno',['../structCO__SDOclient__t.html#a628780da4dccceab6ce79ad880989b26',1,'CO_SDOclient_t::block_seqno()'],['../structCO__SDOserver__t.html#aeefd77d5200958a30f63f7e1f4f474fa',1,'CO_SDOserver_t::block_seqno()']]], + ['block_5ftimeouttimer_1666',['block_timeoutTimer',['../structCO__SDOclient__t.html#a8a667736bf5d22e7bb76f8b25d8b0268',1,'CO_SDOclient_t::block_timeoutTimer()'],['../structCO__SDOserver__t.html#ad8748718c76f53347dde5248b1152626',1,'CO_SDOserver_t::block_timeoutTimer()']]], + ['buf_1667',['buf',['../structCO__fifo__t.html#aa4a8bae66b107809c099cd1d2de5c966',1,'CO_fifo_t::buf()'],['../structCO__SDOclient__t.html#aa56b1f115aee473f5c264142053ed0ae',1,'CO_SDOclient_t::buf()'],['../structCO__SDOserver__t.html#ab834e69e77b72e1ee1d39d35e21e0df4',1,'CO_SDOserver_t::buf()']]], + ['bufferfull_1668',['bufferFull',['../structCO__CANtx__t.html#a305f0687a4ed7cd533e7937d6ff7d31b',1,'CO_CANtx_t']]], + ['bufferinhibitflag_1669',['bufferInhibitFlag',['../structCO__CANmodule__t.html#ad7af19de0bf39c8927c926b095cb5292',1,'CO_CANmodule_t']]], + ['buffersize_1670',['bufferSize',['../structCO__trace__t.html#a3dfe996be2bef106f50de085a7f5ca9f',1,'CO_trace_t']]], + ['buffifo_1671',['bufFifo',['../structCO__SDOclient__t.html#a634a9311de3569cf38841d2faaac20b3',1,'CO_SDOclient_t']]], + ['bufoffsetrd_1672',['bufOffsetRd',['../structCO__SDOserver__t.html#a93557d1ee64582bc37c76f4d4680f7a5',1,'CO_SDOserver_t']]], + ['bufoffsetwr_1673',['bufOffsetWr',['../structCO__SDOserver__t.html#a69f52a30b4e7dc0a6e55c88b3126f814',1,'CO_SDOserver_t']]], + ['bufsize_1674',['bufSize',['../structCO__fifo__t.html#a35d9f2fcb8d2ef00794cb305d8344e61',1,'CO_fifo_t']]] +]; diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_10.html b/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_10.html new file mode 100755 index 0000000..92982ac --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_10.html @@ -0,0 +1,30 @@ + + + + + + + + + +
+
Loading...
+
+ +
Searching...
+
No Matches
+ +
+ + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_10.js b/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_10.js new file mode 100755 index 0000000..e41f5b7 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_10.js @@ -0,0 +1,42 @@ +var searchData= +[ + ['threshold_1984',['threshold',['../structCO__trace__t.html#a6400336e1207dcfa5fa8ef2a908fda08',1,'CO_trace_t']]], + ['time_1985',['TIME',['../structCO__t.html#a80052c25c47be2460e02cf88ff91bbb7',1,'CO_t']]], + ['time_5fus_1986',['time_us',['../structCO__HBconsNode__t.html#a489df7aff00a25d8f2a63fe968050b08',1,'CO_HBconsNode_t']]], + ['timebuffer_1987',['timeBuffer',['../structCO__trace__t.html#a83806299bd57a23f5946f81cb05ab41d',1,'CO_trace_t']]], + ['timedifference_5fus_1988',['timeDifference_us',['../structCO__epoll__t.html#a46f38181dc8483ce83af566ee3f8aff3',1,'CO_epoll_t']]], + ['timedifference_5fus_5fcumulative_1989',['timeDifference_us_cumulative',['../structCO__GTWA__t.html#a8ba7809acba0f2de26eda4a890e68160',1,'CO_GTWA_t']]], + ['timeout_5fus_1990',['timeout_us',['../structCO__LSSmaster__t.html#aeac43e30ee9018bb34876a4c2f1a10de',1,'CO_LSSmaster_t']]], + ['timeouttimer_1991',['timeoutTimer',['../structCO__HBconsNode__t.html#ac4ffced8a11aac2b7383244f306c2081',1,'CO_HBconsNode_t::timeoutTimer()'],['../structCO__SDOclient__t.html#a4e2ff087f13ff5a4754b9186c1a2929e',1,'CO_SDOclient_t::timeoutTimer()'],['../structCO__SDOserver__t.html#a69dfa2436ba0bb51c527eb03330b48a7',1,'CO_SDOserver_t::timeoutTimer()'],['../structCO__LSSmaster__t.html#aa706f4a19295e26a862127917ebc9eca',1,'CO_LSSmaster_t::timeoutTimer()']]], + ['timer_1992',['timer',['../structCO__SYNC__t.html#a491f176a5fb70400647474555628bf6f',1,'CO_SYNC_t::timer()'],['../structCO__TIME__t.html#a02fa485a1a5064ad6e0fc07ea3d5d092',1,'CO_TIME_t::timer()'],['../structCO__SRDO__t.html#a1d3553daf7179b3389023ac78287dfbc',1,'CO_SRDO_t::timer()']]], + ['timer_5ffd_1993',['timer_fd',['../structCO__epoll__t.html#aef151da1fe1a293f34f4f546c24bb0f7',1,'CO_epoll_t']]], + ['timerevent_1994',['timerEvent',['../structCO__epoll__t.html#a9fa1c50ecb111049eda7e45132d3dfd1',1,'CO_epoll_t']]], + ['timerinterval_5fus_1995',['timerInterval_us',['../structCO__epoll__t.html#a0595fb48d72a8592e6649e3de7b3477c',1,'CO_epoll_t']]], + ['timernext_5fus_1996',['timerNext_us',['../structCO__epoll__t.html#a0cb262435f594f9d7ea42eb976294262',1,'CO_epoll_t']]], + ['timestamp_1997',['timestamp',['../structCO__CANinterfaceErrorhandler__t.html#a615dfcafdb866ae951296333df35d1c7',1,'CO_CANinterfaceErrorhandler_t']]], + ['tm_1998',['tm',['../structCO__epoll__t.html#a759aff5ae2eb019aa2ce8117f9ce9836',1,'CO_epoll_t']]], + ['toggle_1999',['toggle',['../structCO__SDOclient__t.html#a5962727f5c1830337146c7b2b389b391',1,'CO_SDOclient_t::toggle()'],['../structCO__SDOserver__t.html#a158797aa411b8d3b5c2079907d04ca0d',1,'CO_SDOserver_t::toggle()']]], + ['toogle_2000',['toogle',['../structCO__SRDO__t.html#a79114736807f3aa8ab19e6e88df93050',1,'CO_SRDO_t']]], + ['tpdo_2001',['TPDO',['../structCO__t.html#ab5027ca1447bf64e5e93935c1e5466c2',1,'CO_t']]], + ['tpdocommpar_2002',['TPDOCommPar',['../structCO__TPDO__t.html#ab9bd2bf1a76f1f0e253dd4c4a941ba67',1,'CO_TPDO_t']]], + ['tpdomappar_2003',['TPDOMapPar',['../structCO__TPDO__t.html#a6f6202c2b866f552c512a3513c27be8b',1,'CO_TPDO_t']]], + ['trace_2004',['trace',['../structCO__t.html#a4f7a05e49ea89178cb61e14c4c4575f1',1,'CO_t']]], + ['transmissiontype_2005',['transmissionType',['../structCO__RPDOCommPar__t.html#a09eb4787337cf8579c0ff1d4ae968aba',1,'CO_RPDOCommPar_t::transmissionType()'],['../structCO__TPDOCommPar__t.html#a328398227ff1f167649d54453e44df97',1,'CO_TPDOCommPar_t::transmissionType()'],['../structCO__SRDOCommPar__t.html#a8716aa43cf70db8af86c4125b14cd538',1,'CO_SRDOCommPar_t::transmissionType()']]], + ['trigger_2006',['trigger',['../structCO__trace__t.html#a879f362c9d4a88de9e48d23c4d0c770c',1,'CO_trace_t']]], + ['triggertime_2007',['triggerTime',['../structCO__trace__t.html#a1b7176f0ad48679449a666c34fe45a3f',1,'CO_trace_t']]], + ['tx_5fidx_5fem_5fprod_2008',['TX_IDX_EM_PROD',['../structCO__t.html#a3d2f250c5c3bc972ce45418d22e79caa',1,'CO_t']]], + ['tx_5fidx_5fgfc_2009',['TX_IDX_GFC',['../structCO__t.html#a709e86d5401484bb7d0c5f17e824636d',1,'CO_t']]], + ['tx_5fidx_5fhb_5fprod_2010',['TX_IDX_HB_PROD',['../structCO__t.html#ad2718c137526676467853885d286fc74',1,'CO_t']]], + ['tx_5fidx_5flss_5fmst_2011',['TX_IDX_LSS_MST',['../structCO__t.html#a38e102b879d1dd04f69bacac56266fdf',1,'CO_t']]], + ['tx_5fidx_5flss_5fslv_2012',['TX_IDX_LSS_SLV',['../structCO__t.html#a7d1159efd4a31d0701c497d25343073d',1,'CO_t']]], + ['tx_5fidx_5fnmt_5fmst_2013',['TX_IDX_NMT_MST',['../structCO__t.html#a77bf70da8db7ecfe78f3720bfca12101',1,'CO_t']]], + ['tx_5fidx_5fsdo_5fcli_2014',['TX_IDX_SDO_CLI',['../structCO__t.html#adc86b044f08602394cbb4346b41a9a45',1,'CO_t']]], + ['tx_5fidx_5fsdo_5fsrv_2015',['TX_IDX_SDO_SRV',['../structCO__t.html#a9287e4d62f23f4222fc90c4a6ded2ef0',1,'CO_t']]], + ['tx_5fidx_5fsrdo_2016',['TX_IDX_SRDO',['../structCO__t.html#a3dbc89e76d1627f2b360ddaefd7a7b51',1,'CO_t']]], + ['tx_5fidx_5fsync_2017',['TX_IDX_SYNC',['../structCO__t.html#a2eb3962f37ce7a4b51d8779f96db58bc',1,'CO_t']]], + ['tx_5fidx_5ftime_2018',['TX_IDX_TIME',['../structCO__t.html#ad5cbb43e75d99c5479efb3eddd36ec3d',1,'CO_t']]], + ['tx_5fidx_5ftpdo_2019',['TX_IDX_TPDO',['../structCO__t.html#a9451781c713680d8bab86cda2a1ca570',1,'CO_t']]], + ['txarray_2020',['txArray',['../structCO__CANmodule__t.html#af623f2a045716af77e906b28deee3d3c',1,'CO_CANmodule_t']]], + ['txbuff_2021',['TXbuff',['../structCO__TIME__t.html#aa658a7b219b1dad6b722ec41670da626',1,'CO_TIME_t::TXbuff()'],['../structCO__LSSmaster__t.html#ad915ab10431aea283df0d8e1299876e2',1,'CO_LSSmaster_t::TXbuff()'],['../structCO__LSSslave__t.html#a12b8505b3f2bcf2085b38bbdcda6736f',1,'CO_LSSslave_t::TXbuff()']]], + ['txsize_2022',['txSize',['../structCO__CANmodule__t.html#a6a53d3d12efbe30d1ee08e821d1b4139',1,'CO_CANmodule_t']]] +]; diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_11.html b/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_11.html new file mode 100755 index 0000000..94f1a8c --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_11.html @@ -0,0 +1,30 @@ + + + + + + + + + +
+
Loading...
+
+ +
Searching...
+
No Matches
+ +
+ + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_11.js b/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_11.js new file mode 100755 index 0000000..21abb35 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_11.js @@ -0,0 +1,4 @@ +var searchData= +[ + ['usecanrxfilters_2023',['useCANrxFilters',['../structCO__CANmodule__t.html#a9b28f7a6f02d398b3a0ea6cf70fa64f0',1,'CO_CANmodule_t']]] +]; diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_12.html b/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_12.html new file mode 100755 index 0000000..61c013a --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_12.html @@ -0,0 +1,30 @@ + + + + + + + + + +
+
Loading...
+
+ +
Searching...
+
No Matches
+ +
+ + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_12.js b/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_12.js new file mode 100755 index 0000000..adcdbe2 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_12.js @@ -0,0 +1,7 @@ +var searchData= +[ + ['valid_2024',['valid',['../structCO__RPDO__t.html#a1d9a4be6ad3245309ffe6e3ad5637942',1,'CO_RPDO_t::valid()'],['../structCO__TPDO__t.html#a201c8a0726347a747f6b315915c797fb',1,'CO_TPDO_t::valid()'],['../structCO__GFC__t.html#a775fa3a4f1afda4a4be200f56d6e2b54',1,'CO_GFC_t::valid()'],['../structCO__SRDO__t.html#a6eef41749d7862ef2a29108f4f08185a',1,'CO_SRDO_t::valid()']]], + ['value_2025',['value',['../structCO__trace__t.html#a24fa467aeeb1581c6a3272bd397a2f10',1,'CO_trace_t']]], + ['valuebuffer_2026',['valueBuffer',['../structCO__trace__t.html#abc2f00a5f99453c77dd8515f3526e428',1,'CO_trace_t']]], + ['valueprev_2027',['valuePrev',['../structCO__trace__t.html#abe713136228c54327c1540b99a1a2fd1',1,'CO_trace_t']]] +]; diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_13.html b/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_13.html new file mode 100755 index 0000000..87b7ca6 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_13.html @@ -0,0 +1,30 @@ + + + + + + + + + +
+
Loading...
+
+ +
Searching...
+
No Matches
+ +
+ + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_13.js b/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_13.js new file mode 100755 index 0000000..e9c8d6c --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_13.js @@ -0,0 +1,5 @@ +var searchData= +[ + ['write_2028',['write',['../structOD__IO__t.html#aa296d8e76d99af5c395971602a453b78',1,'OD_IO_t::write()'],['../structOD__extensionIO__t.html#a06573b7740c3c991352734bba25f0fd4',1,'OD_extensionIO_t::write()']]], + ['writeptr_2029',['writePtr',['../structCO__fifo__t.html#a540fbc52344d1205de11625cd81f351d',1,'CO_fifo_t::writePtr()'],['../structCO__trace__t.html#ae3a556a180e38e7247b39b84de609b5d',1,'CO_trace_t::writePtr()']]] +]; diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_2.html b/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_2.html new file mode 100755 index 0000000..0c8a18c --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_2.html @@ -0,0 +1,30 @@ + + + + + + + + + +
+
Loading...
+
+ +
Searching...
+
No Matches
+ +
+ + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_2.js b/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_2.js new file mode 100755 index 0000000..3e1e544 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_2.js @@ -0,0 +1,59 @@ +var searchData= +[ + ['candata_1675',['CANdata',['../structCO__LSSslave__t.html#a80021b6e36da1b495453d360a7001287',1,'CO_LSSslave_t']]], + ['candevrx_1676',['CANdevRx',['../structCO__HBconsumer__t.html#af5d8828478e2b51fe47a63f24dd896b1',1,'CO_HBconsumer_t::CANdevRx()'],['../structCO__RPDO__t.html#a3f153d5326f07ec7c8f3570287547454',1,'CO_RPDO_t::CANdevRx()'],['../structCO__SDOclient__t.html#a3283cda81a28d3b193a6a6bef29fadab',1,'CO_SDOclient_t::CANdevRx()'],['../structCO__SDOserver__t.html#a3e6255db1f66d742debd80ff2ce25012',1,'CO_SDOserver_t::CANdevRx()'],['../structCO__SYNC__t.html#ae799680e430d4d6b05d7a3da216f06be',1,'CO_SYNC_t::CANdevRx()'],['../structCO__TIME__t.html#ad21e78dd36756f1868056bf6638f5ccd',1,'CO_TIME_t::CANdevRx()'],['../structCO__SRDO__t.html#ad5c0cea22d56cef74f42728107748b38',1,'CO_SRDO_t::CANdevRx()']]], + ['candevrxidx_1677',['CANdevRxIdx',['../structCO__RPDO__t.html#a000fe56fcaf727c59b7dc049ec7fc4b1',1,'CO_RPDO_t::CANdevRxIdx()'],['../structCO__SDOclient__t.html#afb8613dbcacfcefb970fabca4106eaeb',1,'CO_SDOclient_t::CANdevRxIdx()'],['../structCO__SDOserver__t.html#a8c5ca24946f34e174fce129a2f5cb38a',1,'CO_SDOserver_t::CANdevRxIdx()'],['../structCO__SYNC__t.html#a1a489d5fd447a8b5e16fb82d957d0667',1,'CO_SYNC_t::CANdevRxIdx()'],['../structCO__TIME__t.html#a4d9335af44c9b5e1224a6691df2f9594',1,'CO_TIME_t::CANdevRxIdx()'],['../structCO__SRDO__t.html#aa1d8ca455950557652636bc8021928a4',1,'CO_SRDO_t::CANdevRxIdx()']]], + ['candevrxidxstart_1678',['CANdevRxIdxStart',['../structCO__HBconsumer__t.html#a00d176c84d169115399c276031b71722',1,'CO_HBconsumer_t']]], + ['candevtx_1679',['CANdevTx',['../structCO__EM__t.html#a5d24b22a05c354937894109a30b1f641',1,'CO_EM_t::CANdevTx()'],['../structCO__TPDO__t.html#a42d9798fca6a122bd06391b7d23ec254',1,'CO_TPDO_t::CANdevTx()'],['../structCO__SDOclient__t.html#ab4f18ed8c085ea333ca165e486f4ead3',1,'CO_SDOclient_t::CANdevTx()'],['../structCO__SDOserver__t.html#add469f75cf702d340d069aadfe8ede14',1,'CO_SDOserver_t::CANdevTx()'],['../structCO__SYNC__t.html#a3b89fbc55cce155f500bbda463b61d8a',1,'CO_SYNC_t::CANdevTx()'],['../structCO__TIME__t.html#ac5fb7127ca474b6aa2afa207f01f8cae',1,'CO_TIME_t::CANdevTx()'],['../structCO__GFC__t.html#a202258a9732622f41d338c22a991f1a5',1,'CO_GFC_t::CANdevTx()'],['../structCO__SRDO__t.html#a2fe71edd01cb39629fe42753e84df1fc',1,'CO_SRDO_t::CANdevTx()'],['../structCO__LSSmaster__t.html#a480123707829720cc82b08b65923abcb',1,'CO_LSSmaster_t::CANdevTx()'],['../structCO__LSSslave__t.html#a6ce85337359f0e4cd948c438f2960015',1,'CO_LSSslave_t::CANdevTx()']]], + ['candevtxidx_1680',['CANdevTxIdx',['../structCO__EM__t.html#a9a56cba0d9fada8b489884ec766aab04',1,'CO_EM_t::CANdevTxIdx()'],['../structCO__TPDO__t.html#ab401d61f30b73d530df3b8d42015407d',1,'CO_TPDO_t::CANdevTxIdx()'],['../structCO__SDOclient__t.html#a2a44e72381604a972f0e289495a41c37',1,'CO_SDOclient_t::CANdevTxIdx()'],['../structCO__SDOserver__t.html#acbb02ed7ddf534c8f0c41acd25478f47',1,'CO_SDOserver_t::CANdevTxIdx()'],['../structCO__SYNC__t.html#ad07d96af0baa18907d4865ecf244d420',1,'CO_SYNC_t::CANdevTxIdx()'],['../structCO__TIME__t.html#a205dfcde7a830d6a15eda16911e328cd',1,'CO_TIME_t::CANdevTxIdx()'],['../structCO__SRDO__t.html#a0f56f71798c4781bf436d03a0f20938b',1,'CO_SRDO_t::CANdevTxIdx()']]], + ['canerrorstatus_1681',['CANerrorStatus',['../structCO__CANmodule__t.html#a5757052c57726cb4fb2ac5cdd8ff744d',1,'CO_CANmodule_t::CANerrorStatus()'],['../structCO__CANinterfaceErrorhandler__t.html#a3f9369fb469c76c66a5e06eb7b909c54',1,'CO_CANinterfaceErrorhandler_t::CANerrorStatus()']]], + ['canerrorstatusold_1682',['CANerrorStatusOld',['../structCO__EM__t.html#a2bbed8454995910f4ac54035a0129b1b',1,'CO_EM_t']]], + ['canmodule_1683',['CANmodule',['../structCO__t.html#a65bea9a6028917db3b0b3a95a60aea18',1,'CO_t']]], + ['cannormal_1684',['CANnormal',['../structCO__CANmodule__t.html#a5555f7d10e09da5815526d7c8b10901d',1,'CO_CANmodule_t']]], + ['canptr_1685',['CANptr',['../structCO__CANmodule__t.html#aad1c18f6d47621e5a611333dc57cb349',1,'CO_CANmodule_t']]], + ['canrx_1686',['CANrx',['../structCO__t.html#a9665dd795f4c5d26910eb1c5b06503e5',1,'CO_t']]], + ['canrxdata_1687',['CANrxData',['../structCO__RPDO__t.html#a514e6d57efc477bdb49cea75e6495a95',1,'CO_RPDO_t::CANrxData()'],['../structCO__SDOclient__t.html#a0bc8e66b7818bca04a4b49ae8210b387',1,'CO_SDOclient_t::CANrxData()'],['../structCO__SDOserver__t.html#abfbff2e51c54be56f0ba090864c7e2f6',1,'CO_SDOserver_t::CANrxData()'],['../structCO__SRDO__t.html#ae6edba4f0446c10b971a90bde01c8a0b',1,'CO_SRDO_t::CANrxData()'],['../structCO__LSSmaster__t.html#a65cc40d755d5b2eaaeb6f1b29332dca8',1,'CO_LSSmaster_t::CANrxData()']]], + ['canrxnew_1688',['CANrxNew',['../structCO__HBconsNode__t.html#a4050f7d0406d85db643410cbca65fd14',1,'CO_HBconsNode_t::CANrxNew()'],['../structCO__RPDO__t.html#a4ce980b6cc3a0e497a0138c9c4a69d41',1,'CO_RPDO_t::CANrxNew()'],['../structCO__SDOclient__t.html#a8020d62a62634a531a2efa43ef4534f5',1,'CO_SDOclient_t::CANrxNew()'],['../structCO__SDOserver__t.html#a48e9b1237bc0dd46945762416d09c5bb',1,'CO_SDOserver_t::CANrxNew()'],['../structCO__SYNC__t.html#afa9120621a777413be033005eb43e771',1,'CO_SYNC_t::CANrxNew()'],['../structCO__TIME__t.html#a893e358443b67c38eee33182c496f2a4',1,'CO_TIME_t::CANrxNew()'],['../structCO__SRDO__t.html#acbf5e7096cf72e911a4391ffb8dc939d',1,'CO_SRDO_t::CANrxNew()'],['../structCO__LSSmaster__t.html#ad5fd09faed937c053f1ded1df54f40cd',1,'CO_LSSmaster_t::CANrxNew()']]], + ['canrxtoggle_1689',['CANrxToggle',['../structCO__SYNC__t.html#a25aeadeddcae8209b6b18034036a0aaf',1,'CO_SYNC_t']]], + ['cantx_1690',['CANtx',['../structCO__t.html#a990b6bd828bc23b455887b5a72f79dca',1,'CO_t']]], + ['cantxbuff_1691',['CANtxBuff',['../structCO__EM__t.html#a81e9d98c9384b573a8adefaacec3fdec',1,'CO_EM_t::CANtxBuff()'],['../structCO__TPDO__t.html#a2a2a10d57e1eeab4e0717caa302a6605',1,'CO_TPDO_t::CANtxBuff()'],['../structCO__SDOclient__t.html#a58efad796664487290b52b79cbdb3ae0',1,'CO_SDOclient_t::CANtxBuff()'],['../structCO__SDOserver__t.html#a36ae3c719d96121b95b77f76d2cce723',1,'CO_SDOserver_t::CANtxBuff()'],['../structCO__SYNC__t.html#a79d41eeaf724741d0ed281ab3b6a95ef',1,'CO_SYNC_t::CANtxBuff()'],['../structCO__GFC__t.html#acdfbeaf134252ffcb5dd5bd3dcf3c784',1,'CO_GFC_t::CANtxBuff()'],['../structCO__SRDO__t.html#a693cc1ce40b882e85359291744d68b99',1,'CO_SRDO_t::CANtxBuff()']]], + ['cantxcount_1692',['CANtxCount',['../structCO__CANmodule__t.html#a269294aa6a15ba56f92a460fae0536ac',1,'CO_CANmodule_t']]], + ['checkcrc_1693',['checkCRC',['../structCO__SRDOGuard__t.html#ad8cd90a85e6e6d551fb04e8fa94feffd',1,'CO_SRDOGuard_t']]], + ['checksum_1694',['checksum',['../structCO__SRDO__t.html#adffd373a96a9570b0cbd4487c50d5359',1,'CO_SRDO_t']]], + ['cnt_5fall_5frx_5fmsgs_1695',['CNT_ALL_RX_MSGS',['../structCO__t.html#a5b9bea68bd5c63af52819c3c4cdb7592',1,'CO_t']]], + ['cnt_5fall_5ftx_5fmsgs_1696',['CNT_ALL_TX_MSGS',['../structCO__t.html#a9e9e7548a182ed9d4aad12e6a41be495',1,'CO_t']]], + ['cnt_5fem_1697',['CNT_EM',['../structCO__config__t.html#a515e08f68835f71a6f145be8f27b510a',1,'CO_config_t']]], + ['cnt_5fgfc_1698',['CNT_GFC',['../structCO__config__t.html#ae282bab830810b61c0b0c3223654d674',1,'CO_config_t']]], + ['cnt_5fgtwa_1699',['CNT_GTWA',['../structCO__config__t.html#a64725014ecce342843f14ffc4b57e2a2',1,'CO_config_t']]], + ['cnt_5fhb_5fcons_1700',['CNT_HB_CONS',['../structCO__config__t.html#a0031fc8f80e95f8480c918dbf8289671',1,'CO_config_t']]], + ['cnt_5fleds_1701',['CNT_LEDS',['../structCO__config__t.html#a642809cc681792bca855906241d891cc',1,'CO_config_t']]], + ['cnt_5flss_5fmst_1702',['CNT_LSS_MST',['../structCO__config__t.html#ac253cae7039090a6c04bc1e385f3ec21',1,'CO_config_t']]], + ['cnt_5flss_5fslv_1703',['CNT_LSS_SLV',['../structCO__config__t.html#a00a7a598b946ed13e3af7696e9f92dcc',1,'CO_config_t']]], + ['cnt_5fnmt_1704',['CNT_NMT',['../structCO__config__t.html#aeef814580eb5ece5156e63bfc1b490c9',1,'CO_config_t']]], + ['cnt_5frpdo_1705',['CNT_RPDO',['../structCO__config__t.html#a7a75302ac077462b67d767b0a11c9f56',1,'CO_config_t']]], + ['cnt_5fsdo_5fcli_1706',['CNT_SDO_CLI',['../structCO__config__t.html#a2fc9606643a7fb4d4237f01812d3a6d2',1,'CO_config_t']]], + ['cnt_5fsdo_5fsrv_1707',['CNT_SDO_SRV',['../structCO__config__t.html#aac83faf556924515cc2aa8003753ab58',1,'CO_config_t']]], + ['cnt_5fsrdo_1708',['CNT_SRDO',['../structCO__config__t.html#ae58a44be57069709af3f6acbd10953e1',1,'CO_config_t']]], + ['cnt_5fsync_1709',['CNT_SYNC',['../structCO__config__t.html#af6dbc7d9f31b4cb050e23af8cff3df33',1,'CO_config_t']]], + ['cnt_5ftime_1710',['CNT_TIME',['../structCO__config__t.html#ada2a43384a544fa2f235de24a874b1e6',1,'CO_config_t']]], + ['cnt_5ftpdo_1711',['CNT_TPDO',['../structCO__config__t.html#a1d830617f50e3235de35a403a1513693',1,'CO_config_t']]], + ['cnt_5ftrace_1712',['CNT_TRACE',['../structCO__config__t.html#aaafb8ffff236b51cd6d4ab16426d460f',1,'CO_config_t']]], + ['co_5flss_5fbittimingtablelookup_1713',['CO_LSS_bitTimingTableLookup',['../group__CO__LSS.html#ga61bf679482f7d425ceb521ea80a0cc18',1,'CO_LSS.h']]], + ['cob_5fid_1714',['COB_ID',['../structCO__SYNC__t.html#af528cdc487bdaee3dfc0a3baa3f7c7cc',1,'CO_SYNC_t::COB_ID()'],['../structCO__TIME__t.html#a6bcf872834e3a76869943108fd55ffc3',1,'CO_TIME_t::COB_ID()']]], + ['cob_5fid1_5fnormal_1715',['COB_ID1_normal',['../structCO__SRDOCommPar__t.html#a89d3762612ef971aecaae43ce94141cc',1,'CO_SRDOCommPar_t']]], + ['cob_5fid2_5finverted_1716',['COB_ID2_inverted',['../structCO__SRDOCommPar__t.html#a574ade8ff753ae742f6a3b51b32a11fe',1,'CO_SRDOCommPar_t']]], + ['cob_5fidclienttoserver_1717',['COB_IDClientToServer',['../structCO__SDOclient__t.html#a8752f1ad790cc61af17ff8b93b80c7ef',1,'CO_SDOclient_t::COB_IDClientToServer()'],['../structCO__SDOserver__t.html#a7c9113f146613eec4b76888bd8d0f2fd',1,'CO_SDOserver_t::COB_IDClientToServer()']]], + ['cob_5fidservertoclient_1718',['COB_IDServerToClient',['../structCO__SDOclient__t.html#a7b29a785240ca994c7dafd07d5a396e4',1,'CO_SDOclient_t::COB_IDServerToClient()'],['../structCO__SDOserver__t.html#a47cf7cde974f0ff8fd20f7a5363857cf',1,'CO_SDOserver_t::COB_IDServerToClient()']]], + ['cob_5fidusedbyrpdo_1719',['COB_IDUsedByRPDO',['../structCO__RPDOCommPar__t.html#a798b8c59ea8f8b627c307f1246b6ee78',1,'CO_RPDOCommPar_t']]], + ['cob_5fidusedbytpdo_1720',['COB_IDUsedByTPDO',['../structCO__TPDOCommPar__t.html#a65ee2e80b1078e84479ab749012f94cc',1,'CO_TPDOCommPar_t']]], + ['command_1721',['command',['../structCO__LSSmaster__t.html#a745a126919856c94e7f93186830e6940',1,'CO_LSSmaster_t']]], + ['commandinterface_1722',['commandInterface',['../structCO__epoll__gtw__t.html#a96544bbc848d4627f3147496cc40d9f4',1,'CO_epoll_gtw_t']]], + ['commbuf_1723',['commBuf',['../structCO__GTWA__t.html#a51fd91cf468da15e5f943131fa696266',1,'CO_GTWA_t']]], + ['commfifo_1724',['commFifo',['../structCO__GTWA__t.html#a74241ff1c68a8fc05f0b2be601dcf960',1,'CO_GTWA_t']]], + ['compatibilityentry_1725',['compatibilityEntry',['../structCO__TPDOCommPar__t.html#acba30527976f508b43b3348846f2e657',1,'CO_TPDOCommPar_t']]], + ['config_1726',['config',['../structCO__t.html#a522a9a56f1256dac8cd305313b3913f1',1,'CO_t']]], + ['configurationvalid_1727',['configurationValid',['../structCO__SRDOGuard__t.html#aa4c8b5e7d9d8fa54d91ad0797ea9b39b',1,'CO_SRDOGuard_t']]], + ['counter_1728',['counter',['../structCO__SYNC__t.html#a0977c2f09f69755e4b53df68bb1fee0b',1,'CO_SYNC_t']]], + ['counteroverflowvalue_1729',['counterOverflowValue',['../structCO__SYNC__t.html#aee5eb3e245e54e509d2f2cc05cf4a664',1,'CO_SYNC_t']]], + ['curentsynctimeisinsidewindow_1730',['curentSyncTimeIsInsideWindow',['../structCO__SYNC__t.html#adbc85ba8f5f4cbca0caa645216204a88',1,'CO_SYNC_t']]] +]; diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_3.html b/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_3.html new file mode 100755 index 0000000..19a31fc --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_3.html @@ -0,0 +1,30 @@ + + + + + + + + + +
+
Loading...
+
+ +
Searching...
+
No Matches
+ +
+ + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_3.js b/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_3.js new file mode 100755 index 0000000..0c3712c --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_3.js @@ -0,0 +1,13 @@ +var searchData= +[ + ['data_1731',['data',['../structCO__CANtx__t.html#aae5bcdc2296a5d1d53a3105e86dbb66d',1,'CO_CANtx_t::data()'],['../structOD__obj__var__t.html#a7c15865c69e0dc0a09f6d21bd890d062',1,'OD_obj_var_t::data()'],['../structOD__obj__array__t.html#a009bcf700d8e27c93e886ad7ff7fb2eb',1,'OD_obj_array_t::data()'],['../structOD__obj__record__t.html#a490d4818eaa6ca94d2c6fc598647dbd1',1,'OD_obj_record_t::data()']]], + ['data0_1732',['data0',['../structOD__obj__array__t.html#a9a4ca22f014061ca9d926ab43036bc1f',1,'OD_obj_array_t']]], + ['dataelementlength_1733',['dataElementLength',['../structOD__obj__array__t.html#a985fb68eba74f9e8fb76a4c5d85e96e9',1,'OD_obj_array_t']]], + ['dataelementsizeof_1734',['dataElementSizeof',['../structOD__obj__array__t.html#ad310fa351ebb2a44f66451dd12675bf9',1,'OD_obj_array_t']]], + ['datalength_1735',['dataLength',['../structOD__stream__t.html#a60c4499678a5db84a7f7285b934ce75a',1,'OD_stream_t::dataLength()'],['../structOD__obj__var__t.html#a385af11ed619b78de9b2f1ae6528a870',1,'OD_obj_var_t::dataLength()'],['../structOD__obj__record__t.html#a1302f14c20e976ac884196d4c3e201c1',1,'OD_obj_record_t::dataLength()'],['../structCO__RPDO__t.html#af742fd80b982822c3e06770ab0877cc3',1,'CO_RPDO_t::dataLength()'],['../structCO__TPDO__t.html#a223e60deb77d4ef8a78da37e4d9cdf85',1,'CO_TPDO_t::dataLength()'],['../structCO__SRDO__t.html#ae994e87a71e85342f3a5b3c046d8a47f',1,'CO_SRDO_t::dataLength()']]], + ['dataobjectoriginal_1736',['dataObjectOriginal',['../structOD__stream__t.html#aad2551a7bf0da6396e6b909adf487b01',1,'OD_stream_t']]], + ['dataoffset_1737',['dataOffset',['../structOD__stream__t.html#a97799b896ee689504771a9274575bcdc',1,'OD_stream_t']]], + ['defaultcob_5fid_1738',['defaultCOB_ID',['../structCO__RPDO__t.html#a9e327dba172ebbd3112097e5085eea2f',1,'CO_RPDO_t::defaultCOB_ID()'],['../structCO__TPDO__t.html#a95e95dc4668b41de0b8eb8504e83c944',1,'CO_TPDO_t::defaultCOB_ID()'],['../structCO__SRDO__t.html#a14da12ca5af61cd6cc6e442a0baa5c26',1,'CO_SRDO_t::defaultCOB_ID()']]], + ['dlc_1739',['DLC',['../structCO__CANtx__t.html#a9bb96d60314283061f7619e36d870fa0',1,'CO_CANtx_t']]], + ['dt_1740',['dt',['../structCO__trace__t.html#a31e42b2511450377294475fcf0189d89',1,'CO_trace_t']]] +]; diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_4.html b/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_4.html new file mode 100755 index 0000000..bdc37be --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_4.html @@ -0,0 +1,30 @@ + + + + + + + + + +
+
Loading...
+
+ +
Searching...
+
No Matches
+ +
+ + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_4.js b/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_4.js new file mode 100755 index 0000000..6e38710 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_4.js @@ -0,0 +1,37 @@ +var searchData= +[ + ['em_1741',['em',['../structCO__t.html#a6e4c80d975a5a2207530b72bdbf530b7',1,'CO_t::em()'],['../structCO__HBconsumer__t.html#aae5e363ccc6a6fd3b17a35e0430add2a',1,'CO_HBconsumer_t::em()'],['../structCO__NMT__t.html#a60848ed23fb775dc4412be0e7ff9bc4b',1,'CO_NMT_t::em()'],['../structCO__RPDO__t.html#a5261e898fc67ecba19f0fc146e4a13ef',1,'CO_RPDO_t::em()'],['../structCO__TPDO__t.html#ac7ce3386549e212300bb85bfc5e88f2e',1,'CO_TPDO_t::em()'],['../structCO__SYNC__t.html#acf987cc40eb5f005a92f10210353ea1f',1,'CO_SYNC_t::em()'],['../structCO__TIME__t.html#ad38fd490eece812c27048a0ffde75c9e',1,'CO_TIME_t::em()'],['../structCO__SRDO__t.html#a1779ab0170d7b604948a9396681212c8',1,'CO_SRDO_t::em()']]], + ['enabled_1742',['enabled',['../structCO__trace__t.html#aefa5e9934aaac1f00d0a1866100dae50',1,'CO_trace_t']]], + ['entry_5fh1001_1743',['ENTRY_H1001',['../structCO__config__t.html#a6a6c19e816fb76882e85b2c07c0d8f42',1,'CO_config_t']]], + ['entry_5fh1003_1744',['ENTRY_H1003',['../structCO__config__t.html#a7e320b309714b7f623c2006d45fee929',1,'CO_config_t']]], + ['entry_5fh1005_1745',['ENTRY_H1005',['../structCO__config__t.html#a02a4992f47db72816753ff2aa1964318',1,'CO_config_t']]], + ['entry_5fh1006_1746',['ENTRY_H1006',['../structCO__config__t.html#aa9befdebbaaa22f309b9a1b115612071',1,'CO_config_t']]], + ['entry_5fh1007_1747',['ENTRY_H1007',['../structCO__config__t.html#ad51ab63ca8b5836bf0dd8543f02db544',1,'CO_config_t']]], + ['entry_5fh1012_1748',['ENTRY_H1012',['../structCO__config__t.html#abac6be7122af1a8a4f9ae3ff5912d490',1,'CO_config_t']]], + ['entry_5fh1014_1749',['ENTRY_H1014',['../structCO__config__t.html#a4827d94f6152cc12d86bd21312ae86e4',1,'CO_config_t']]], + ['entry_5fh1015_1750',['ENTRY_H1015',['../structCO__config__t.html#a141f21b4d1730206d1af823fd6b13a01',1,'CO_config_t']]], + ['entry_5fh1016_1751',['ENTRY_H1016',['../structCO__config__t.html#a0af4cf7d0355861e7f60206d794d6a91',1,'CO_config_t']]], + ['entry_5fh1017_1752',['ENTRY_H1017',['../structCO__config__t.html#ad17f77b55de3d90ec983fcac49eeab6d',1,'CO_config_t']]], + ['entry_5fh1019_1753',['ENTRY_H1019',['../structCO__config__t.html#a468c82f6a0afd757a6b78ce33532c0d2',1,'CO_config_t']]], + ['entry_5fh1200_1754',['ENTRY_H1200',['../structCO__config__t.html#a05ab8adad4517850e31e5542895f7cc5',1,'CO_config_t']]], + ['entry_5fh1280_1755',['ENTRY_H1280',['../structCO__config__t.html#a9f871c4ec753e8414cdb47eb78c3e09d',1,'CO_config_t']]], + ['entry_5fh1300_1756',['ENTRY_H1300',['../structCO__config__t.html#a91c9f3ddb67231854af39224a9597e20',1,'CO_config_t']]], + ['entry_5fh1301_1757',['ENTRY_H1301',['../structCO__config__t.html#a87076cb1f9282d9720c21d395ff4e541',1,'CO_config_t']]], + ['entry_5fh1381_1758',['ENTRY_H1381',['../structCO__config__t.html#a7b3172b29ce8751adcab9e4351dcc31e',1,'CO_config_t']]], + ['entry_5fh13fe_1759',['ENTRY_H13FE',['../structCO__config__t.html#a03fcaca5a8e0e71b86086908cae75f3d',1,'CO_config_t']]], + ['entry_5fh13ff_1760',['ENTRY_H13FF',['../structCO__config__t.html#aa4cb9674209b83e7f0e48b01feaa04ef',1,'CO_config_t']]], + ['entry_5fh1400_1761',['ENTRY_H1400',['../structCO__config__t.html#a5e0984d93183493d587523888465eaa7',1,'CO_config_t']]], + ['entry_5fh1600_1762',['ENTRY_H1600',['../structCO__config__t.html#ab2ddc9943fd8c89f3b852d7ac9508d21',1,'CO_config_t']]], + ['entry_5fh1800_1763',['ENTRY_H1800',['../structCO__config__t.html#a29b98c08edfe0fba2e46c7af7a9edf6f',1,'CO_config_t']]], + ['entry_5fh1a00_1764',['ENTRY_H1A00',['../structCO__config__t.html#a43fd6a448c91910c603f2c7756610432',1,'CO_config_t']]], + ['epoll_5ffd_1765',['epoll_fd',['../structCO__epoll__t.html#a6e4aeb640b634a49a290aa9b4bc3b517',1,'CO_epoll_t::epoll_fd()'],['../structCO__epoll__gtw__t.html#a7804e63fe1f7f5f6df68b32d8c25da2d',1,'CO_epoll_gtw_t::epoll_fd()']]], + ['epoll_5fnew_1766',['epoll_new',['../structCO__epoll__t.html#ac4113f19873fc3217e1e997a28f5bdd4',1,'CO_epoll_t']]], + ['errinfo_1767',['errinfo',['../structCO__CANmodule__t.html#aead6585dcb9b8214dd77e8f74ca2aea1',1,'CO_CANmodule_t']]], + ['errold_1768',['errOld',['../structCO__CANmodule__t.html#aa0627988ddedc3a4ec3bbfcf3b818d06',1,'CO_CANmodule_t']]], + ['errorregister_1769',['errorRegister',['../structCO__EM__t.html#ae18bb84c6235afbdf3745d9da337c2d3',1,'CO_EM_t']]], + ['errorstatusbits_1770',['errorStatusBits',['../structCO__EM__t.html#a377eb478f0af20e6e1e23bd77186dcfd',1,'CO_EM_t']]], + ['ev_1771',['ev',['../structCO__epoll__t.html#a5ace69fd90506eae4f3f538fcb160c01',1,'CO_epoll_t']]], + ['event_5ffd_1772',['event_fd',['../structCO__epoll__t.html#af0e85776b360999b0830c943eb4505eb',1,'CO_epoll_t']]], + ['eventtimer_1773',['eventTimer',['../structCO__TPDOCommPar__t.html#ab01f44570dca08c910c17b14fc664414',1,'CO_TPDOCommPar_t::eventTimer()'],['../structCO__TPDO__t.html#ae71e875f41d8f14f02e757dbc3b2d0c5',1,'CO_TPDO_t::eventTimer()']]], + ['extio_1774',['extIO',['../structOD__obj__extended__t.html#a6ccf59c770c3887233ab7b850e77e375',1,'OD_obj_extended_t']]] +]; diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_5.html b/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_5.html new file mode 100755 index 0000000..6aa2249 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_5.html @@ -0,0 +1,30 @@ + + + + + + + + + +
+
Loading...
+
+ +
Searching...
+
No Matches
+ +
+ + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_5.js b/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_5.js new file mode 100755 index 0000000..b04a54f --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_5.js @@ -0,0 +1,28 @@ +var searchData= +[ + ['fastscanpos_1775',['fastscanPos',['../structCO__LSSslave__t.html#af163113cf94a4a2c74521ffe194522b2',1,'CO_LSSslave_t']]], + ['fd_1776',['fd',['../structCO__CANinterfaceErrorhandler__t.html#a06e667f1f90ad62495aa2172d97305b8',1,'CO_CANinterfaceErrorhandler_t']]], + ['fifo_1777',['fifo',['../structCO__EM__t.html#ac84d3cf89e04ee48fff85d59bb91f3d9',1,'CO_EM_t']]], + ['fifocount_1778',['fifoCount',['../structCO__EM__t.html#a99ffd8be6baebaf6c598b02c7ce31518',1,'CO_EM_t']]], + ['fifooverflow_1779',['fifoOverflow',['../structCO__EM__t.html#a52c37c126d1f761b0a9cf608123a5976',1,'CO_EM_t']]], + ['fifoppptr_1780',['fifoPpPtr',['../structCO__EM__t.html#a1cf16a27db5ccf065c68b39e8e2d401f',1,'CO_EM_t']]], + ['fifowrptr_1781',['fifoWrPtr',['../structCO__EM__t.html#aa790e927251322c852c26bdb21853647',1,'CO_EM_t']]], + ['filename_1782',['filename',['../structCO__OD__storage__t.html#afb3dd08b01bf20f251754c32b116d8fe',1,'CO_OD_storage_t']]], + ['finished_1783',['finished',['../structCO__SDOserver__t.html#a90997c7a119e97d2cb8b9bbc5fb5145f',1,'CO_SDOserver_t']]], + ['firstcantxmessage_1784',['firstCANtxMessage',['../structCO__CANmodule__t.html#ac12bd37971b89b5796fec0de71be0f07',1,'CO_CANmodule_t']]], + ['flagspdo_1785',['flagsPDO',['../structOD__subEntry__t.html#ad6a4eb8da2f84f7b84164eac01115dd1',1,'OD_subEntry_t::flagsPDO()'],['../structOD__obj__extended__t.html#a80e8186d50cb6ca8ebf447d43815e566',1,'OD_obj_extended_t::flagsPDO()']]], + ['format_1786',['format',['../structCO__trace__t.html#a3ea91e63f2c2f6c34b3ee1ae0f84e939',1,'CO_trace_t']]], + ['found_1787',['found',['../structCO__LSSmaster__fastscan__t.html#a9c34a389339a46da81f2234443f20fb9',1,'CO_LSSmaster_fastscan_t']]], + ['fp_1788',['fp',['../structCO__OD__storage__t.html#a8976cb73fd4a2baadc4689fdb8b876a1',1,'CO_OD_storage_t']]], + ['freshcommand_1789',['freshCommand',['../structCO__epoll__gtw__t.html#ae529dc4279406811f0b97593816e863c',1,'CO_epoll_gtw_t']]], + ['fsbitchecked_1790',['fsBitChecked',['../structCO__LSSmaster__t.html#ab71abc74d5d92f149ca991c9bb47ed99',1,'CO_LSSmaster_t']]], + ['fsidnumber_1791',['fsIdNumber',['../structCO__LSSmaster__t.html#aca7e7be8c297c612b2200b866ed7b248',1,'CO_LSSmaster_t']]], + ['fslsssub_1792',['fsLssSub',['../structCO__LSSmaster__t.html#a9962ae690930bf58a447a47bf708c5f2',1,'CO_LSSmaster_t']]], + ['fsstate_1793',['fsState',['../structCO__LSSmaster__t.html#ad7ee57af199cfd615f6caa07358f0ce7',1,'CO_LSSmaster_t']]], + ['functsignalobject_1794',['functSignalObject',['../structCO__SDOclient__t.html#a582292a9d5db2b3bf09bb089c788ecd0',1,'CO_SDOclient_t::functSignalObject()'],['../structCO__LSSmaster__t.html#aafce10b9e126c5c157cb137e8b65b27c',1,'CO_LSSmaster_t::functSignalObject()']]], + ['functsignalobjecthbstarted_1795',['functSignalObjectHbStarted',['../structCO__HBconsNode__t.html#a50ed8caa11fce685dfbba13d11ada0ef',1,'CO_HBconsNode_t']]], + ['functsignalobjectpre_1796',['functSignalObjectPre',['../structCO__EM__t.html#ac1dec593fd20fbf7ccc5e8287e27b2d8',1,'CO_EM_t::functSignalObjectPre()'],['../structCO__HBconsNode__t.html#afd6a4fcc8bc4cc9c647e9ef3d91b2431',1,'CO_HBconsNode_t::functSignalObjectPre()'],['../structCO__NMT__t.html#a9ae2c962bc4aaa477555920dc0323302',1,'CO_NMT_t::functSignalObjectPre()'],['../structCO__RPDO__t.html#aff7dd123460cc50708f28d40d851f8dd',1,'CO_RPDO_t::functSignalObjectPre()'],['../structCO__SDOserver__t.html#adac867a846309166bca3f5dd0b550e65',1,'CO_SDOserver_t::functSignalObjectPre()'],['../structCO__SYNC__t.html#a6a219b1fd9d3382131878af1f016b83d',1,'CO_SYNC_t::functSignalObjectPre()'],['../structCO__TIME__t.html#a91e41ac57bb634b53b6698b58efc4616',1,'CO_TIME_t::functSignalObjectPre()'],['../structCO__SRDO__t.html#a5807d03b19a7e2ae7c4250dd4e61f235',1,'CO_SRDO_t::functSignalObjectPre()'],['../structCO__LSSslave__t.html#a346a28ca3adef3050f6cc29ec182059d',1,'CO_LSSslave_t::functSignalObjectPre()']]], + ['functsignalobjectremotereset_1797',['functSignalObjectRemoteReset',['../structCO__HBconsNode__t.html#a94b3c3b2b5b24f5ea3ecc1bbc14b79dd',1,'CO_HBconsNode_t']]], + ['functsignalobjectsafe_1798',['functSignalObjectSafe',['../structCO__GFC__t.html#a890395b6bf1b77055c52ad150356ea33',1,'CO_GFC_t::functSignalObjectSafe()'],['../structCO__SRDO__t.html#a037ba4d3ca1e33c6187219ba4766b2a8',1,'CO_SRDO_t::functSignalObjectSafe()']]], + ['functsignalobjecttimeout_1799',['functSignalObjectTimeout',['../structCO__HBconsNode__t.html#aaf6cc300976931c02e3d46ec2b75cc2e',1,'CO_HBconsNode_t']]] +]; diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_6.html b/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_6.html new file mode 100755 index 0000000..ce4a906 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_6.html @@ -0,0 +1,30 @@ + + + + + + + + + +
+
Loading...
+
+ +
Searching...
+
No Matches
+ +
+ + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_6.js b/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_6.js new file mode 100755 index 0000000..6b29a96 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_6.js @@ -0,0 +1,7 @@ +var searchData= +[ + ['gfc_1800',['GFC',['../structCO__t.html#a9c6e7b29436b05c8b659502c6fae2a6a',1,'CO_t']]], + ['gtwa_1801',['gtwa',['../structCO__t.html#afa0e937046492a26af9bb5e03c3aab94',1,'CO_t']]], + ['gtwa_5ffd_1802',['gtwa_fd',['../structCO__epoll__gtw__t.html#a79b9c968b1f44ad5a4c55c36174d6898',1,'CO_epoll_gtw_t']]], + ['gtwa_5ffdsocket_1803',['gtwa_fdSocket',['../structCO__epoll__gtw__t.html#a418eccf136c9f8e081d61f98289e0759',1,'CO_epoll_gtw_t']]] +]; diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_7.html b/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_7.html new file mode 100755 index 0000000..39ffd47 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_7.html @@ -0,0 +1,30 @@ + + + + + + + + + +
+
Loading...
+
+ +
Searching...
+
No Matches
+ +
+ + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_7.js b/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_7.js new file mode 100755 index 0000000..b25f62d --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_7.js @@ -0,0 +1,11 @@ +var searchData= +[ + ['hb_5fcandevtx_1804',['HB_CANdevTx',['../structCO__NMT__t.html#aedcf17c643f41370a978794301f00169',1,'CO_NMT_t']]], + ['hb_5ftxbuff_1805',['HB_TXbuff',['../structCO__NMT__t.html#aa9fb1bb36758a1ff27b44c20fd6a0192',1,'CO_NMT_t']]], + ['hbcons_1806',['HBcons',['../structCO__t.html#a5eea4e2b8390e1f0ec531248e229cd72',1,'CO_t']]], + ['hbconstime_1807',['HBconsTime',['../structCO__HBconsumer__t.html#a1cd314f387357f2ce13d4093f477fff5',1,'CO_HBconsumer_t']]], + ['hbproducertime_5fus_1808',['HBproducerTime_us',['../structCO__NMT__t.html#a983a4ba9a004d216e32414c0a38736c4',1,'CO_NMT_t']]], + ['hbproducertimer_1809',['HBproducerTimer',['../structCO__NMT__t.html#a0babc82bd6dfde07511b87167941b4f0',1,'CO_NMT_t']]], + ['hbstate_1810',['HBstate',['../structCO__HBconsNode__t.html#a6d16bde174d37094149343fcc7025e3c',1,'CO_HBconsNode_t']]], + ['helpstring_1811',['helpString',['../structCO__GTWA__t.html#a5ce9a4cef511904ad4038c0b1443d3f6',1,'CO_GTWA_t']]] +]; diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_8.html b/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_8.html new file mode 100755 index 0000000..37a2edd --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_8.html @@ -0,0 +1,30 @@ + + + + + + + + + +
+
Loading...
+
+ +
Searching...
+
No Matches
+ +
+ + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_8.js b/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_8.js new file mode 100755 index 0000000..488ba76 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_8.js @@ -0,0 +1,13 @@ +var searchData= +[ + ['ident_1812',['ident',['../structCO__CANrx__t.html#a8595c238cf0364bde995dee97d321909',1,'CO_CANrx_t::ident()'],['../structCO__CANtx__t.html#a9cc2687eb11da14d4c0aa167352c635c',1,'CO_CANtx_t::ident()']]], + ['ifname_1813',['ifName',['../structCO__CANinterfaceErrorhandler__t.html#a58a5219f8dad7dc1c98db2b463e6e005',1,'CO_CANinterfaceErrorhandler_t']]], + ['index_1814',['index',['../structOD__subEntry__t.html#a4e10db7bdf91d721ecc7d97f4dda67ff',1,'OD_subEntry_t::index()'],['../structOD__entry__t.html#ac27d9d9ac18705e84d64f5226a6e352c',1,'OD_entry_t::index()'],['../structCO__SDOclient__t.html#ad4fc4deee415a621f3558266ba447455',1,'CO_SDOclient_t::index()'],['../structCO__SDOserver__t.html#ae0b1720a88d948fbf6d8e20b333abb17',1,'CO_SDOserver_t::index()']]], + ['informationdirection_1815',['informationDirection',['../structCO__SRDOCommPar__t.html#ac8f865699090f666910e66dabf53b339',1,'CO_SRDOCommPar_t']]], + ['inhibitemtime_5fus_1816',['inhibitEmTime_us',['../structCO__EM__t.html#a82db41fc720e2f2551207bb0d2ba1ae4',1,'CO_EM_t']]], + ['inhibittime_1817',['inhibitTime',['../structCO__TPDOCommPar__t.html#ad53403c65582d166898546e329ea9587',1,'CO_TPDOCommPar_t']]], + ['inhibittimer_1818',['inhibitTimer',['../structCO__TPDO__t.html#a9277687ef658353801435638c8aa2bee',1,'CO_TPDO_t']]], + ['internalcommand_1819',['internalCommand',['../structCO__NMT__t.html#a7a56fcf387bd2922ece9106538d986ef',1,'CO_NMT_t']]], + ['isconsumer_1820',['isConsumer',['../structCO__TIME__t.html#aeea1ecdf76e04f37c1d760eee72a1395',1,'CO_TIME_t']]], + ['isproducer_1821',['isProducer',['../structCO__SYNC__t.html#af37a656db91d31a8187e0350f472ea36',1,'CO_SYNC_t::isProducer()'],['../structCO__TIME__t.html#a691d02cb2128a81f4af345165146b761',1,'CO_TIME_t::isProducer()']]] +]; diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_9.html b/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_9.html new file mode 100755 index 0000000..21e5a4f --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_9.html @@ -0,0 +1,30 @@ + + + + + + + + + +
+
Loading...
+
+ +
Searching...
+
No Matches
+ +
+ + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_9.js b/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_9.js new file mode 100755 index 0000000..47ace9a --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_9.js @@ -0,0 +1,32 @@ +var searchData= +[ + ['lastsavedus_1822',['lastSavedUs',['../structCO__OD__storage__t.html#a94d80f0c140485ab426891839b356347',1,'CO_OD_storage_t']]], + ['lasttimestamp_1823',['lastTimeStamp',['../structCO__trace__t.html#af3ffa370f4b25a3c3fd27f9ce75379c0',1,'CO_trace_t']]], + ['ledgreen_1824',['LEDgreen',['../structCO__LEDs__t.html#ac2b4eb053725681f1935c8a6e4184f85',1,'CO_LEDs_t']]], + ['ledred_1825',['LEDred',['../structCO__LEDs__t.html#ad8624f0baa6186e523e072e6101c7a84',1,'CO_LEDs_t']]], + ['leds_1826',['LEDs',['../structCO__t.html#a4837a9caa7daa219a8ecff5c64bc08ba',1,'CO_t::LEDs()'],['../structCO__GTWA__t.html#a90ec5c0f770fd1a3ec2ea4f33059357a',1,'CO_GTWA_t::LEDs()']]], + ['ledtmr200ms_1827',['LEDtmr200ms',['../structCO__LEDs__t.html#a2690fe669ce01322e14de1b9f559a147',1,'CO_LEDs_t']]], + ['ledtmr50ms_1828',['LEDtmr50ms',['../structCO__LEDs__t.html#ae27f4bbc313e08e25b9b1e1d515ea9e4',1,'CO_LEDs_t']]], + ['ledtmrflash_5f1_1829',['LEDtmrflash_1',['../structCO__LEDs__t.html#a2431d9736416e1b5b6c57f61f029dc04',1,'CO_LEDs_t']]], + ['ledtmrflash_5f2_1830',['LEDtmrflash_2',['../structCO__LEDs__t.html#ae6aa2a97a9496abc67774d94caa0ea1a',1,'CO_LEDs_t']]], + ['ledtmrflash_5f3_1831',['LEDtmrflash_3',['../structCO__LEDs__t.html#ad3af9ed6c2c43330cc3eadb3b76de2c4',1,'CO_LEDs_t']]], + ['ledtmrflash_5f4_1832',['LEDtmrflash_4',['../structCO__LEDs__t.html#ae33491a6eed2761893fea47dd65c5a78',1,'CO_LEDs_t']]], + ['list_1833',['list',['../structOD__t.html#a935673d88e82c7a72be76766467d74b4',1,'OD_t']]], + ['listenonly_1834',['listenOnly',['../structCO__CANinterfaceErrorhandler__t.html#a96ddaefd75e680898f93d4891f5fc195',1,'CO_CANinterfaceErrorhandler_t']]], + ['localsocketpath_1835',['localSocketPath',['../structCO__epoll__gtw__t.html#a5f11979c2bc301d6b8cc7ddce8d45688',1,'CO_epoll_gtw_t']]], + ['logbuf_1836',['logBuf',['../structCO__GTWA__t.html#a7400a0dcf3d8ad25b8ad9820237d4f3c',1,'CO_GTWA_t']]], + ['logfifo_1837',['logFifo',['../structCO__GTWA__t.html#af84217848a2f2e4d3f840f4978d4e2ac',1,'CO_GTWA_t']]], + ['lssaddress_1838',['lssAddress',['../structCO__LSSslave__t.html#ae18023e6feb33ea4e9ddf26796a99fe0',1,'CO_LSSslave_t::lssAddress()'],['../structCO__GTWA__t.html#aae702db8c811f7e85f3fd8984bde8f9d',1,'CO_GTWA_t::lssAddress()']]], + ['lssbitrate_1839',['lssBitrate',['../structCO__GTWA__t.html#ae404009d2987f589cc52b9958d688917',1,'CO_GTWA_t']]], + ['lssfastscan_1840',['lssFastscan',['../structCO__LSSslave__t.html#a433215fe4d5f9764427a5e0dc57d60a5',1,'CO_LSSslave_t::lssFastscan()'],['../structCO__GTWA__t.html#aa39bf29226ecaa52d20a6a92b61cf4fa',1,'CO_GTWA_t::lssFastscan()']]], + ['lssinquirecs_1841',['lssInquireCs',['../structCO__GTWA__t.html#a74892b830cb6064d31190ff6bac6a3c9',1,'CO_GTWA_t']]], + ['lssmaster_1842',['LSSmaster',['../structCO__t.html#af586261baf229fe624fb72b712208c86',1,'CO_t::LSSmaster()'],['../structCO__GTWA__t.html#ae2897d681afe5bd45db8306b52734318',1,'CO_GTWA_t::LSSmaster()']]], + ['lssnid_1843',['lssNID',['../structCO__GTWA__t.html#af49dacd6548ca791a09e78727aeeabad',1,'CO_GTWA_t']]], + ['lssnodecount_1844',['lssNodeCount',['../structCO__GTWA__t.html#a16f5893d54bce3d2741d8b732ec6c29e',1,'CO_GTWA_t']]], + ['lssselect_1845',['lssSelect',['../structCO__LSSslave__t.html#a38d6e43b3a1c3b0ce20bdb9108f8ab91',1,'CO_LSSslave_t']]], + ['lssslave_1846',['LSSslave',['../structCO__t.html#a0c6ecabcb18ed2bd9eb881f9156482b6',1,'CO_t']]], + ['lssstate_1847',['lssState',['../structCO__LSSslave__t.html#a514565a5cda1630165c3640ef223aa28',1,'CO_LSSslave_t']]], + ['lssstore_1848',['lssStore',['../structCO__GTWA__t.html#a47ec165dd4d545a6ae52067644dbbaab',1,'CO_GTWA_t']]], + ['lsssubstate_1849',['lssSubState',['../structCO__GTWA__t.html#a7f55184f433e48dd4d4fb52aebed6da6',1,'CO_GTWA_t']]], + ['lsstimeout_5fms_1850',['lssTimeout_ms',['../structCO__GTWA__t.html#a78c3d1a9ebb44a1db4e6eb100ed6c6a7',1,'CO_GTWA_t']]] +]; diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_a.html b/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_a.html new file mode 100755 index 0000000..1f65055 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_a.html @@ -0,0 +1,30 @@ + + + + + + + + + +
+
Loading...
+
+ +
Searching...
+
No Matches
+ +
+ + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_a.js b/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_a.js new file mode 100755 index 0000000..ad9e95b --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_a.js @@ -0,0 +1,19 @@ +var searchData= +[ + ['map_1851',['map',['../structCO__trace__t.html#ad6e329507a29f46b41ea6bef210b6502',1,'CO_trace_t']]], + ['mappedobject1_1852',['mappedObject1',['../structCO__RPDOMapPar__t.html#a0a161a9792479c07aaf12447fbac2499',1,'CO_RPDOMapPar_t::mappedObject1()'],['../structCO__TPDOMapPar__t.html#a6869485bc0705188069c37d6b6ee1694',1,'CO_TPDOMapPar_t::mappedObject1()']]], + ['mappedobject2_1853',['mappedObject2',['../structCO__RPDOMapPar__t.html#a63f9b87f27d30e6f4faf65a56b458ffb',1,'CO_RPDOMapPar_t::mappedObject2()'],['../structCO__TPDOMapPar__t.html#a496d610062c09a1667e7541659339eea',1,'CO_TPDOMapPar_t::mappedObject2()']]], + ['mappedobject3_1854',['mappedObject3',['../structCO__RPDOMapPar__t.html#a3ac0b7cd18683c45d3dcd0e18b13810b',1,'CO_RPDOMapPar_t::mappedObject3()'],['../structCO__TPDOMapPar__t.html#a8e79bb51d865cae1bab20621db898ddb',1,'CO_TPDOMapPar_t::mappedObject3()']]], + ['mappedobject4_1855',['mappedObject4',['../structCO__RPDOMapPar__t.html#a0a3ef7ae329b91ec73d05301e870ad75',1,'CO_RPDOMapPar_t::mappedObject4()'],['../structCO__TPDOMapPar__t.html#aa8a046a95b3ac151f5d8d8cb4415851e',1,'CO_TPDOMapPar_t::mappedObject4()']]], + ['mappedobject5_1856',['mappedObject5',['../structCO__RPDOMapPar__t.html#af532c96971699321f04aad14dbee52a4',1,'CO_RPDOMapPar_t::mappedObject5()'],['../structCO__TPDOMapPar__t.html#a0fbdd6c39635c8288771867bdc061d6f',1,'CO_TPDOMapPar_t::mappedObject5()']]], + ['mappedobject6_1857',['mappedObject6',['../structCO__RPDOMapPar__t.html#a1082b9de3f132363f78c19004be017fb',1,'CO_RPDOMapPar_t::mappedObject6()'],['../structCO__TPDOMapPar__t.html#ad8f8f0e0629b1c0467599ea8b138e3eb',1,'CO_TPDOMapPar_t::mappedObject6()']]], + ['mappedobject7_1858',['mappedObject7',['../structCO__RPDOMapPar__t.html#ad66f07a873d2ceb0b10bdad242925984',1,'CO_RPDOMapPar_t::mappedObject7()'],['../structCO__TPDOMapPar__t.html#a1fb19d7423d2fd74bc8575aff1657552',1,'CO_TPDOMapPar_t::mappedObject7()']]], + ['mappedobject8_1859',['mappedObject8',['../structCO__RPDOMapPar__t.html#a2f1467c9ba8a91e4f2742c08ed8551f6',1,'CO_RPDOMapPar_t::mappedObject8()'],['../structCO__TPDOMapPar__t.html#aa482b092dd475bac21193c671dabdb49',1,'CO_TPDOMapPar_t::mappedObject8()']]], + ['mappointer_1860',['mapPointer',['../structCO__RPDO__t.html#a998b83bc1cbf11aa4e9170ce03c9d203',1,'CO_RPDO_t::mapPointer()'],['../structCO__TPDO__t.html#acb6fa2c4037b1e41afdf6195cd6e93ec',1,'CO_TPDO_t::mapPointer()'],['../structCO__SRDO__t.html#a3e7570a1aeef89502702175eccb93500',1,'CO_SRDO_t::mapPointer()']]], + ['mask_1861',['mask',['../structCO__CANrx__t.html#af7a48dd4ac895a19c4031038e2c1222d',1,'CO_CANrx_t']]], + ['match_1862',['match',['../structCO__LSSmaster__fastscan__t.html#a69540f77885162e936803a9526d3c342',1,'CO_LSSmaster_fastscan_t']]], + ['maxsubindex_1863',['maxSubIndex',['../structCO__RPDOCommPar__t.html#ac49d1bc96c31ec32015628128a6e60a7',1,'CO_RPDOCommPar_t::maxSubIndex()'],['../structCO__TPDOCommPar__t.html#a74a8681177fabbb55ebf5be843f12fc5',1,'CO_TPDOCommPar_t::maxSubIndex()'],['../structCO__SRDOCommPar__t.html#af156b61e6d278b014466e860f073cf05',1,'CO_SRDOCommPar_t::maxSubIndex()']]], + ['maxvalue_1864',['maxValue',['../structCO__trace__t.html#a7b41b99eb65d49fe522c25d08127f81d',1,'CO_trace_t']]], + ['minvalue_1865',['minValue',['../structCO__trace__t.html#a0f1b287dae5b6a30b43f481d2bee41ab',1,'CO_trace_t']]], + ['monitorednodes_1866',['monitoredNodes',['../structCO__HBconsumer__t.html#a737b37c544a28eff8de0b03b51cbeec8',1,'CO_HBconsumer_t']]] +]; diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_b.html b/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_b.html new file mode 100755 index 0000000..c02d066 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_b.html @@ -0,0 +1,30 @@ + + + + + + + + + +
+
Loading...
+
+ +
Searching...
+
No Matches
+ +
+ + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_b.js b/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_b.js new file mode 100755 index 0000000..3738b37 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_b.js @@ -0,0 +1,20 @@ +var searchData= +[ + ['net_1867',['net',['../structCO__GTWA__t.html#a8df8a3f47d967e4fb0a56e491db0f9e9',1,'CO_GTWA_t']]], + ['net_5fdefault_1868',['net_default',['../structCO__GTWA__t.html#aec9a1ffe0ce40572452d3f1e36e51c1b',1,'CO_GTWA_t']]], + ['nmt_1869',['NMT',['../structCO__t.html#a6fddf777eec75e0cc8fc510a5c4ec8e5',1,'CO_t::NMT()'],['../structCO__GTWA__t.html#a6aa019a1583f8ba56fada7c5ed8ec191',1,'CO_GTWA_t::NMT()']]], + ['nmt_5fcandevtx_1870',['NMT_CANdevTx',['../structCO__NMT__t.html#aaa1a7f9278595d82eaddbee8ac434ca9',1,'CO_NMT_t']]], + ['nmt_5ftxbuff_1871',['NMT_TXbuff',['../structCO__NMT__t.html#a6c9ca6315daa74257699ff657165d409',1,'CO_NMT_t']]], + ['nmtcontrol_1872',['NMTcontrol',['../structCO__NMT__t.html#a0c28cae6c3c7319c8fdfe694a3b4d9a7',1,'CO_NMT_t']]], + ['nmtispreoroperationalprev_1873',['NMTisPreOrOperationalPrev',['../structCO__HBconsumer__t.html#a2fe3d81e2124918d0d5947e6891a060e',1,'CO_HBconsumer_t']]], + ['nmtstate_1874',['NMTstate',['../structCO__HBconsNode__t.html#aca87186237691cc315da47d5bcc8ad31',1,'CO_HBconsNode_t']]], + ['nmtstateprev_1875',['NMTstatePrev',['../structCO__HBconsNode__t.html#a7fd2636d9f46b7ff47676f716f6f00a4',1,'CO_HBconsNode_t']]], + ['noackcounter_1876',['noackCounter',['../structCO__CANinterfaceErrorhandler__t.html#a6d17a248ec3ce1f6c53f4315c0cb9282',1,'CO_CANinterfaceErrorhandler_t']]], + ['node_1877',['node',['../structCO__GTWA__t.html#a38f5c9325dc69820d831688282a63a10',1,'CO_GTWA_t']]], + ['node_5fdefault_1878',['node_default',['../structCO__GTWA__t.html#a2464fa84713d31811e8872b4557d50d1',1,'CO_GTWA_t']]], + ['nodeid_1879',['nodeId',['../structCO__EM__t.html#ac5522470ed7ea0f5e91520a2563b9abc',1,'CO_EM_t::nodeId()'],['../structCO__HBconsNode__t.html#a180aca37057c670be35bbdd89f72b812',1,'CO_HBconsNode_t::nodeId()'],['../structCO__NMT__t.html#a6cf0441aff58a3e208d1ed221a26709c',1,'CO_NMT_t::nodeId()'],['../structCO__RPDO__t.html#a15e1425101d92521ad219695036b1cd2',1,'CO_RPDO_t::nodeId()'],['../structCO__TPDO__t.html#a4ef8ced15f6fffb56a0ec4aeb48d4551',1,'CO_TPDO_t::nodeId()'],['../structCO__SDOclient__t.html#a13de9457791eecf17051e405665bfa4a',1,'CO_SDOclient_t::nodeId()'],['../structCO__SDOserver__t.html#a38d0b70cb37d6be927208e3662105c6c',1,'CO_SDOserver_t::nodeId()'],['../structCO__SRDO__t.html#ac4cc841f24894e41a5bbdbd386e62a0e',1,'CO_SRDO_t::nodeId()']]], + ['nodeidofthesdoserver_1880',['nodeIDOfTheSDOServer',['../structCO__SDOclient__t.html#a9e3c564cd4d027c5bd93bd25293ebacc',1,'CO_SDOclient_t']]], + ['nodeidunconfigured_1881',['nodeIdUnconfigured',['../structCO__t.html#a139e71d4b3c9f2c072df13e6ac0dbac4',1,'CO_t']]], + ['numberofmappedobjects_1882',['numberOfMappedObjects',['../structCO__RPDOMapPar__t.html#a479613deae0d06607897093d617edb1d',1,'CO_RPDOMapPar_t::numberOfMappedObjects()'],['../structCO__TPDOMapPar__t.html#a10718cf84dc6a975509d327efb8403de',1,'CO_TPDOMapPar_t::numberOfMappedObjects()']]], + ['numberofmonitorednodes_1883',['numberOfMonitoredNodes',['../structCO__HBconsumer__t.html#a5b944043074d42017be3b76320030542',1,'CO_HBconsumer_t']]] +]; diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_c.html b/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_c.html new file mode 100755 index 0000000..4b866c6 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_c.html @@ -0,0 +1,30 @@ + + + + + + + + + +
+
Loading...
+
+ +
Searching...
+
No Matches
+ +
+ + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_c.js b/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_c.js new file mode 100755 index 0000000..ff924ac --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_c.js @@ -0,0 +1,14 @@ +var searchData= +[ + ['object_1884',['object',['../structCO__CANrx__t.html#a957a1ce67cd1d9010889d557bf0c5770',1,'CO_CANrx_t::object()'],['../structOD__stream__t.html#af3c8ded429eefa8646317207c0b3ff97',1,'OD_stream_t::object()'],['../structOD__extensionIO__t.html#a929dace9c5bf5f1e3090f3fbff40f9b8',1,'OD_extensionIO_t::object()']]], + ['od_1885',['OD',['../structCO__SDOclient__t.html#a89334ad9cc4ac6751f4a8225f1645923',1,'CO_SDOclient_t::OD()'],['../structCO__SDOserver__t.html#a8c104c076f57ffe039e0e41a1b12377e',1,'CO_SDOserver_t::OD()']]], + ['od_5fio_1886',['OD_IO',['../structCO__SDOclient__t.html#a16fa659bc3098a3fbd60d0f6efde937a',1,'CO_SDOclient_t::OD_IO()'],['../structCO__SDOserver__t.html#a19e7e8afc09ced5629e3a1e04b83aa9f',1,'CO_SDOserver_t::OD_IO()']]], + ['od_5fvariable_1887',['OD_variable',['../structCO__trace__t.html#a2d71398a641edb4fe095aabb6e81f834',1,'CO_trace_t']]], + ['odaddress_1888',['odAddress',['../structCO__OD__storage__t.html#ac434d6480330c026761fe8a82e32839b',1,'CO_OD_storage_t']]], + ['odobject_1889',['odObject',['../structOD__entry__t.html#a8667e5896b5b001270103e59c92bb181',1,'OD_entry_t']]], + ['odobjectoriginal_1890',['odObjectOriginal',['../structOD__obj__extended__t.html#abbc62d96e2ecafc99bb1eaf1210f816a',1,'OD_obj_extended_t']]], + ['odobjecttype_1891',['odObjectType',['../structOD__entry__t.html#a86a662107a24527872d92cdb13cad29b',1,'OD_entry_t']]], + ['odsize_1892',['odSize',['../structCO__OD__storage__t.html#aefd1cb33fa031c1592b20643bb38bfbb',1,'CO_OD_storage_t']]], + ['operatingstate_1893',['operatingState',['../structCO__NMT__t.html#abebf973c819c48fa36c48ba2eb84818a',1,'CO_NMT_t::operatingState()'],['../structCO__RPDO__t.html#a85583dccb8f2d0515288888e56065e70',1,'CO_RPDO_t::operatingState()'],['../structCO__TPDO__t.html#a7ba73f70490869a38ff561aa6c32489f',1,'CO_TPDO_t::operatingState()'],['../structCO__SYNC__t.html#a70fea8996ebfbe7d163bae11201dac6e',1,'CO_SYNC_t::operatingState()'],['../structCO__TIME__t.html#a2afb9ad74a04332b644669c7fdb187ac',1,'CO_TIME_t::operatingState()'],['../structCO__SRDOGuard__t.html#a5e80afb9ddd0debd94eaafa9c6f4ad36',1,'CO_SRDOGuard_t::operatingState()']]], + ['operatingstateprev_1894',['operatingStatePrev',['../structCO__NMT__t.html#a544df8108d0853dd6d2938fa253ef018',1,'CO_NMT_t::operatingStatePrev()'],['../structCO__SRDOGuard__t.html#ac6f6a3b1465360e035656933470d6b4d',1,'CO_SRDOGuard_t::operatingStatePrev()']]] +]; diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_d.html b/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_d.html new file mode 100755 index 0000000..84d878b --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_d.html @@ -0,0 +1,30 @@ + + + + + + + + + +
+
Loading...
+
+ +
Searching...
+
No Matches
+ +
+ + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_d.js b/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_d.js new file mode 100755 index 0000000..68223c0 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_d.js @@ -0,0 +1,28 @@ +var searchData= +[ + ['pcanrx_5fcallback_1895',['pCANrx_callback',['../structCO__CANrx__t.html#a8e4668eec8326bb9ac08d67afc3060c7',1,'CO_CANrx_t']]], + ['pendingbitrate_1896',['pendingBitRate',['../structCO__LSSslave__t.html#a90b0fabc76e788404da520a7a5e90e48',1,'CO_LSSslave_t']]], + ['pendingnodeid_1897',['pendingNodeID',['../structCO__LSSslave__t.html#a9a632a4655fb5adfd91bd3e26ed43f0b',1,'CO_LSSslave_t']]], + ['periodtime_1898',['periodTime',['../structCO__SYNC__t.html#a15337541e9b3defa2dbae2df3dd01e72',1,'CO_SYNC_t::periodTime()'],['../structCO__TIME__t.html#a7c0090cb2e1e3af1fab308640c3d25a5',1,'CO_TIME_t::periodTime()']]], + ['periodtimeouttime_1899',['periodTimeoutTime',['../structCO__SYNC__t.html#a9f612a7f9d691edeef11d8df9cf166a7',1,'CO_SYNC_t::periodTimeoutTime()'],['../structCO__TIME__t.html#a9f2d24892e5b318f5eea5844f3b29a4a',1,'CO_TIME_t::periodTimeoutTime()']]], + ['pfunctlssactivatebitrate_1900',['pFunctLSSactivateBitRate',['../structCO__LSSslave__t.html#a8c2bfab9e6225cd65b48149333d9e1dd',1,'CO_LSSslave_t']]], + ['pfunctlsscfgstore_1901',['pFunctLSScfgStore',['../structCO__LSSslave__t.html#a53b9620f867ad5f534c99955a2479f16',1,'CO_LSSslave_t']]], + ['pfunctlsscheckbitrate_1902',['pFunctLSScheckBitRate',['../structCO__LSSslave__t.html#a1482f4cef4b3ac4afbdf569306e32aab',1,'CO_LSSslave_t']]], + ['pfunctnmt_1903',['pFunctNMT',['../structCO__NMT__t.html#a64bc9d74c871bf560d8b5db85e4d051e',1,'CO_NMT_t']]], + ['pfunctsignal_1904',['pFunctSignal',['../structCO__SDOclient__t.html#ae7bbefdb0854addfa2ae45dcfc39d738',1,'CO_SDOclient_t::pFunctSignal()'],['../structCO__LSSmaster__t.html#a9b8376cf89bba5a3650f82da61ffee5b',1,'CO_LSSmaster_t::pFunctSignal()']]], + ['pfunctsignalhbstarted_1905',['pFunctSignalHbStarted',['../structCO__HBconsNode__t.html#ac44d2a232ca2b352fd717ee8e6f28e90',1,'CO_HBconsNode_t']]], + ['pfunctsignalnmtchanged_1906',['pFunctSignalNmtChanged',['../structCO__HBconsNode__t.html#a3f4ec7dfe9e47a3e7a9bdad382fd7f56',1,'CO_HBconsNode_t::pFunctSignalNmtChanged()'],['../structCO__HBconsumer__t.html#a58314f7d35efc2dbeae14ba4be76dec1',1,'CO_HBconsumer_t::pFunctSignalNmtChanged()']]], + ['pfunctsignalobjectnmtchanged_1907',['pFunctSignalObjectNmtChanged',['../structCO__HBconsNode__t.html#a2a1c5abd88c2ecd451e2a1ca65d3dc93',1,'CO_HBconsNode_t::pFunctSignalObjectNmtChanged()'],['../structCO__HBconsumer__t.html#ad583c93f4e59f98669cd18f263aee45a',1,'CO_HBconsumer_t::pFunctSignalObjectNmtChanged()']]], + ['pfunctsignalpre_1908',['pFunctSignalPre',['../structCO__EM__t.html#a124a5d8fb51bb600618a9427b14663c4',1,'CO_EM_t::pFunctSignalPre()'],['../structCO__HBconsNode__t.html#a61c753c38666dda8a4d4d870fc593ae5',1,'CO_HBconsNode_t::pFunctSignalPre()'],['../structCO__NMT__t.html#a31b37e4e64737bb3ef4331d607adbed6',1,'CO_NMT_t::pFunctSignalPre()'],['../structCO__RPDO__t.html#a3c0573cc4e76a601bbbd710f19b9615f',1,'CO_RPDO_t::pFunctSignalPre()'],['../structCO__SDOserver__t.html#ace57036f1dfd1ffe35f4b13e50fc4a41',1,'CO_SDOserver_t::pFunctSignalPre()'],['../structCO__SYNC__t.html#ac56a3411d0cd312e51d905937c628b6f',1,'CO_SYNC_t::pFunctSignalPre()'],['../structCO__TIME__t.html#aed7685145a3b3cfb80e8865aa3e8df6d',1,'CO_TIME_t::pFunctSignalPre()'],['../structCO__SRDO__t.html#a5c9c5a746066607b6052c66bcf07190e',1,'CO_SRDO_t::pFunctSignalPre()'],['../structCO__LSSslave__t.html#a9a6db85a8e5a14652e3b1fb7026790a1',1,'CO_LSSslave_t::pFunctSignalPre()']]], + ['pfunctsignalremotereset_1909',['pFunctSignalRemoteReset',['../structCO__HBconsNode__t.html#a7f7ccf80c31d4c764db24b70f9111e7b',1,'CO_HBconsNode_t']]], + ['pfunctsignalrx_1910',['pFunctSignalRx',['../structCO__EM__t.html#a71ba138e5c1814446c210ac7d7f2aa02',1,'CO_EM_t']]], + ['pfunctsignalsafe_1911',['pFunctSignalSafe',['../structCO__GFC__t.html#af7ffb43e3b2a682941404bc5c23512e5',1,'CO_GFC_t::pFunctSignalSafe()'],['../structCO__SRDO__t.html#ab000fd951d5b72615a3b3086335a6add',1,'CO_SRDO_t::pFunctSignalSafe()']]], + ['pfunctsignaltimeout_1912',['pFunctSignalTimeout',['../structCO__HBconsNode__t.html#ab6bbeee344ebd1d657bbabc02f51e597',1,'CO_HBconsNode_t']]], + ['pgetvalue_1913',['pGetValue',['../structCO__trace__dataType__t.html#a8ac5cd02c39b591354149f8c5d6357f7',1,'CO_trace_dataType_t']]], + ['previoustime_5fus_1914',['previousTime_us',['../structCO__epoll__t.html#aa047c5d68bb87515a1089cf5c75d847f',1,'CO_epoll_t']]], + ['printpoint_1915',['printPoint',['../structCO__trace__dataType__t.html#aef2341fd443f3aa33a11db1b429b7dc1',1,'CO_trace_dataType_t']]], + ['printpointend_1916',['printPointEnd',['../structCO__trace__dataType__t.html#a48ec81e33885a9114f2ea0be237a0059',1,'CO_trace_dataType_t']]], + ['printpointstart_1917',['printPointStart',['../structCO__trace__dataType__t.html#a751d9b94aba8f663de5b5f7c7f074893',1,'CO_trace_dataType_t']]], + ['producercanid_1918',['producerCanId',['../structCO__EM__t.html#a490ef24607a0b637d9bc3a60f616b41e',1,'CO_EM_t']]], + ['producerenabled_1919',['producerEnabled',['../structCO__EM__t.html#ad54685d2f0e7ac934edbe0cf5c0b4baf',1,'CO_EM_t']]] +]; diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_e.html b/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_e.html new file mode 100755 index 0000000..b0d9b7b --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_e.html @@ -0,0 +1,30 @@ + + + + + + + + + +
+
Loading...
+
+ +
Searching...
+
No Matches
+ +
+ + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_e.js b/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_e.js new file mode 100755 index 0000000..c305107 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_e.js @@ -0,0 +1,30 @@ +var searchData= +[ + ['read_1920',['read',['../structOD__IO__t.html#ab0ed3186d15d20f80f877a5f087fdebc',1,'OD_IO_t::read()'],['../structOD__extensionIO__t.html#af4c210e173adb94e297ad26eacb2b678',1,'OD_extensionIO_t::read()']]], + ['readcallback_1921',['readCallback',['../structCO__GTWA__t.html#a036c4a3a89b8171baba8039fc50876ba',1,'CO_GTWA_t']]], + ['readcallbackobject_1922',['readCallbackObject',['../structCO__GTWA__t.html#ae1b9a86d7020ac21713a9b658a08495b',1,'CO_GTWA_t']]], + ['readptr_1923',['readPtr',['../structCO__fifo__t.html#a4b56b7d217aa2b02eaf1b3592c5c26e6',1,'CO_fifo_t::readPtr()'],['../structCO__trace__t.html#af439eee35bf45f6ec14a32270577ff85',1,'CO_trace_t::readPtr()']]], + ['receiveerror_1924',['receiveError',['../structCO__SYNC__t.html#ae108ac75f1fb7c797393646cc6c494af',1,'CO_SYNC_t::receiveError()'],['../structCO__TIME__t.html#abbff3c5ce159992f4b45ec042849cbc0',1,'CO_TIME_t::receiveError()']]], + ['respbuf_1925',['respBuf',['../structCO__GTWA__t.html#a987d9431f47a10272cf9c81b0d0159d1',1,'CO_GTWA_t']]], + ['respbufcount_1926',['respBufCount',['../structCO__GTWA__t.html#a67770af170976d4d904fbc044d347376',1,'CO_GTWA_t']]], + ['respbufoffset_1927',['respBufOffset',['../structCO__GTWA__t.html#a605bfa0c99f4a0235980de0603a050ca',1,'CO_GTWA_t']]], + ['resphold_1928',['respHold',['../structCO__GTWA__t.html#aef556bb4c595944ebf3de22a2c9d5007',1,'CO_GTWA_t']]], + ['restrictionflags_1929',['restrictionFlags',['../structCO__RPDO__t.html#a1759ebaef816a352d37e717c9360458a',1,'CO_RPDO_t::restrictionFlags()'],['../structCO__TPDO__t.html#aa0d1d4b71933c7bac438d97ca280fe56',1,'CO_TPDO_t::restrictionFlags()']]], + ['rpdo_1930',['RPDO',['../structCO__t.html#a332fa4512032d94a20af69b5a103e017',1,'CO_t']]], + ['rpdocommpar_1931',['RPDOCommPar',['../structCO__RPDO__t.html#a1aaaf9abb01030dbd732985d07ed8c33',1,'CO_RPDO_t']]], + ['rpdomappar_1932',['RPDOMapPar',['../structCO__RPDO__t.html#a69ad9068dfcee1d12697430856e62d10',1,'CO_RPDO_t']]], + ['rx_5fidx_5fem_5fcons_1933',['RX_IDX_EM_CONS',['../structCO__t.html#a47df99673e8da6e48c6c17cbf204c16d',1,'CO_t']]], + ['rx_5fidx_5fgfc_1934',['RX_IDX_GFC',['../structCO__t.html#ae689666dc05116ce0694d5ea505a2e79',1,'CO_t']]], + ['rx_5fidx_5fhb_5fcons_1935',['RX_IDX_HB_CONS',['../structCO__t.html#a788f815f4d31a4aa11d171114173573e',1,'CO_t']]], + ['rx_5fidx_5flss_5fmst_1936',['RX_IDX_LSS_MST',['../structCO__t.html#aa0c0c6b387e406c67d3b988a744b3b36',1,'CO_t']]], + ['rx_5fidx_5flss_5fslv_1937',['RX_IDX_LSS_SLV',['../structCO__t.html#a2c230c531fc2a2a09ad33edef582041e',1,'CO_t']]], + ['rx_5fidx_5fnmt_5fslv_1938',['RX_IDX_NMT_SLV',['../structCO__t.html#af693786fa6c0923d4c934d09a00820d3',1,'CO_t']]], + ['rx_5fidx_5frpdo_1939',['RX_IDX_RPDO',['../structCO__t.html#a7a01e7792c600f5dc1eb2e7676e1e01d',1,'CO_t']]], + ['rx_5fidx_5fsdo_5fcli_1940',['RX_IDX_SDO_CLI',['../structCO__t.html#a85a2007d87b4fb3608859e251cf8653e',1,'CO_t']]], + ['rx_5fidx_5fsdo_5fsrv_1941',['RX_IDX_SDO_SRV',['../structCO__t.html#a8e63b9c217cf4cd7fc8fbc0900bdb01c',1,'CO_t']]], + ['rx_5fidx_5fsrdo_1942',['RX_IDX_SRDO',['../structCO__t.html#a8fec2730008c19f6fbc6192937776739',1,'CO_t']]], + ['rx_5fidx_5fsync_1943',['RX_IDX_SYNC',['../structCO__t.html#a72a378c1e806978f8399bf3f3af7c002',1,'CO_t']]], + ['rx_5fidx_5ftime_1944',['RX_IDX_TIME',['../structCO__t.html#a039a63c4b10f91814775f9d1cef43aad',1,'CO_t']]], + ['rxarray_1945',['rxArray',['../structCO__CANmodule__t.html#a747e694e15cca5a5579c2c4397a6da39',1,'CO_CANmodule_t']]], + ['rxsize_1946',['rxSize',['../structCO__CANmodule__t.html#a88edfd32c2bff5b9f29a2bccd1ac96f3',1,'CO_CANmodule_t']]] +]; diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_f.html b/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_f.html new file mode 100755 index 0000000..a708dbf --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_f.html @@ -0,0 +1,30 @@ + + + + + + + + + +
+
Loading...
+
+ +
Searching...
+
No Matches
+ +
+ + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_f.js b/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_f.js new file mode 100755 index 0000000..e2fc4ac --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/search/variables_f.js @@ -0,0 +1,40 @@ +var searchData= +[ + ['safetycycletime_1947',['safetyCycleTime',['../structCO__SRDOCommPar__t.html#a257a5534889060efc7477be7fc0adbad',1,'CO_SRDOCommPar_t']]], + ['safetyrelatedvalidationtime_1948',['safetyRelatedValidationTime',['../structCO__SRDOCommPar__t.html#a5c02007d0e4f1b1cfd8a07da93b954cb',1,'CO_SRDOCommPar_t']]], + ['scan_1949',['scan',['../structCO__LSSmaster__fastscan__t.html#a42853c0091c96d7fc7e763c2be3b6e8a',1,'CO_LSSmaster_fastscan_t']]], + ['sdo_1950',['SDO',['../structCO__RPDO__t.html#abcf8134da148073ec8e3f1dd6f0c8da1',1,'CO_RPDO_t::SDO()'],['../structCO__TPDO__t.html#a52c1e56f282549b0949721049bcc7e73',1,'CO_TPDO_t::SDO()'],['../structCO__SRDO__t.html#af8121f4d1f0879eba793afaee5c6c804',1,'CO_SRDO_t::SDO()'],['../structCO__trace__t.html#a33616f410feb93ddb6a2ac238027996b',1,'CO_trace_t::SDO()']]], + ['sdo_5fc_1951',['SDO_C',['../structCO__GTWA__t.html#a274945dbaacfd975f86a58566a884769',1,'CO_GTWA_t']]], + ['sdoblocktransferenable_1952',['SDOblockTransferEnable',['../structCO__GTWA__t.html#a0b05c1b89fe8b104b019d12679d4edcf',1,'CO_GTWA_t']]], + ['sdoclient_1953',['SDOclient',['../structCO__t.html#ae9548e1a85b750b039dcc12bd43bd972',1,'CO_t']]], + ['sdodatacopystatus_1954',['SDOdataCopyStatus',['../structCO__GTWA__t.html#aae981f9a446cfaad8a5450e77adc32ec',1,'CO_GTWA_t']]], + ['sdodatatype_1955',['SDOdataType',['../structCO__GTWA__t.html#a7b9bcb2113286454a273b9b43f4e1548',1,'CO_GTWA_t']]], + ['sdoserver_1956',['SDOserver',['../structCO__t.html#aa9fe002954a6b902e594fc47a9f1917b',1,'CO_t']]], + ['sdotimeouttime_1957',['SDOtimeoutTime',['../structCO__GTWA__t.html#a9fa3fe5ce1806296cf36ef5d8c3ecae2',1,'CO_GTWA_t']]], + ['sdotimeouttime_5fus_1958',['SDOtimeoutTime_us',['../structCO__SDOclient__t.html#ab2f8be2e90734e78f4e78bdabbb13488',1,'CO_SDOclient_t::SDOtimeoutTime_us()'],['../structCO__SDOserver__t.html#a09381f3b6885d0a3bc7d1fa96805eb5d',1,'CO_SDOserver_t::SDOtimeoutTime_us()']]], + ['sendifcosflags_1959',['sendIfCOSFlags',['../structCO__TPDO__t.html#a0f5dbb51df08261c466b079c08119a04',1,'CO_TPDO_t']]], + ['sendrequest_1960',['sendRequest',['../structCO__TPDO__t.html#afc375e06e5931cb8bae7886f2f5f0f7a',1,'CO_TPDO_t']]], + ['sendresponse_1961',['sendResponse',['../structCO__LSSslave__t.html#af9fb29812cd548f34223cc6d16de8121',1,'CO_LSSslave_t']]], + ['sequence_1962',['sequence',['../structCO__GTWA__t.html#a31b7ae3a5da107dfb5432f5a95f9faee',1,'CO_GTWA_t']]], + ['service_1963',['service',['../structCO__LSSslave__t.html#acdffe4100d9031c885ba5b5b995e471c',1,'CO_LSSslave_t']]], + ['size_1964',['size',['../structOD__t.html#ab52b946b5f0127fe618c6f5bbe65a698',1,'OD_t']]], + ['sizeind_1965',['sizeInd',['../structCO__SDOclient__t.html#a1b9dc211b9b90dac825097757f985583',1,'CO_SDOclient_t::sizeInd()'],['../structCO__SDOserver__t.html#a95be5bcb7257f818a294c9b03b0d1386',1,'CO_SDOserver_t::sizeInd()']]], + ['sizetran_1966',['sizeTran',['../structCO__SDOclient__t.html#aa7a9ddc5dbf035644cf59857dcfa83db',1,'CO_SDOclient_t::sizeTran()'],['../structCO__SDOserver__t.html#a86eaba6efabaa335b769924edd80f1ae',1,'CO_SDOserver_t::sizeTran()']]], + ['sockettimeout_5fus_1967',['socketTimeout_us',['../structCO__epoll__gtw__t.html#a876c2b8ae6e9585155c5cf035c876c91',1,'CO_epoll_gtw_t']]], + ['sockettimeouttmr_5fus_1968',['socketTimeoutTmr_us',['../structCO__epoll__gtw__t.html#a94ff0ff20f321b749202d9f3aa92a66d',1,'CO_epoll_gtw_t']]], + ['srdo_1969',['SRDO',['../structCO__t.html#a2a65c843c89da67fc049aa285e03243c',1,'CO_t']]], + ['srdocommpar_1970',['SRDOCommPar',['../structCO__SRDO__t.html#aba2d09de18da68fb01ab23696de452ac',1,'CO_SRDO_t']]], + ['srdoguard_1971',['SRDOGuard',['../structCO__t.html#a007c96db5b54e91145571feb06e9e683',1,'CO_t::SRDOGuard()'],['../structCO__SRDO__t.html#ab96c524d0b0243527a6b2d3ee89de302',1,'CO_SRDO_t::SRDOGuard()']]], + ['srdomappar_1972',['SRDOMapPar',['../structCO__SRDO__t.html#a876184e5c0b2809fc4b412cc2accb43e',1,'CO_SRDO_t']]], + ['started_1973',['started',['../structCO__fifo__t.html#ab754d03636ff0ef17950d4167429a77f',1,'CO_fifo_t']]], + ['state_1974',['state',['../structCO__SDOclient__t.html#a511a3981c4664be192b19714b95995c0',1,'CO_SDOclient_t::state()'],['../structCO__SDOserver__t.html#a0b457d35679acb51d11d21cd9f3660fd',1,'CO_SDOserver_t::state()'],['../structCO__LSSmaster__t.html#a1fb90a878809a6690dd4493444795e30',1,'CO_LSSmaster_t::state()'],['../structCO__GTWA__t.html#a5c37389f4a985950708e2fed036daf1f',1,'CO_GTWA_t::state()']]], + ['statetimeouttmr_1975',['stateTimeoutTmr',['../structCO__GTWA__t.html#ac5fc932142ed17d04393ae30819fb021',1,'CO_GTWA_t']]], + ['stream_1976',['stream',['../structOD__IO__t.html#a7f31cae6c859c2a2eaf2610b65fd6626',1,'OD_IO_t']]], + ['subentriescount_1977',['subEntriesCount',['../structOD__subEntry__t.html#a49750c447be0b3773e8aa1814d5fa2de',1,'OD_subEntry_t::subEntriesCount()'],['../structOD__entry__t.html#a2f8698a482c2994602a24d39d924acc6',1,'OD_entry_t::subEntriesCount()']]], + ['subindex_1978',['subIndex',['../structOD__subEntry__t.html#a2c39cbf86fc3b384b017ed6261d379bd',1,'OD_subEntry_t::subIndex()'],['../structOD__obj__record__t.html#a7cfa0012ae3aec7bbfc98e5e8301782c',1,'OD_obj_record_t::subIndex()'],['../structCO__SDOclient__t.html#a7f36981af65dc318dd69213c408b1b62',1,'CO_SDOclient_t::subIndex()'],['../structCO__SDOserver__t.html#a5051d5aeaa97e5d40ccfb39461706f10',1,'CO_SDOserver_t::subIndex()']]], + ['sync_1979',['SYNC',['../structCO__t.html#a9c704cab995029fd216076523c70d283',1,'CO_t::SYNC()'],['../structCO__RPDO__t.html#a2545c12748b54a1ef3e5660c82e1adf8',1,'CO_RPDO_t::SYNC()'],['../structCO__TPDO__t.html#ae6c014bdb855d57d4c53bc702734b51f',1,'CO_TPDO_t::SYNC()']]], + ['synccounter_1980',['syncCounter',['../structCO__TPDO__t.html#a5bf0fa473c5eb92e3b56fc1f17222976',1,'CO_TPDO_t']]], + ['syncflag_1981',['syncFlag',['../structCO__CANtx__t.html#a79c19597a51351b9d6ed1c9bdfd051b3',1,'CO_CANtx_t']]], + ['synchronous_1982',['synchronous',['../structCO__RPDO__t.html#a08c58c120349e150e4188666a5113916',1,'CO_RPDO_t']]], + ['syncstartvalue_1983',['SYNCStartValue',['../structCO__TPDOCommPar__t.html#a597bd93f097550c3d869307e733cd198',1,'CO_TPDOCommPar_t']]] +]; diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/splitbar.png b/Devices/Libraries/Systems/CANopenSocket/docs/splitbar.png new file mode 100755 index 0000000..fe895f2 Binary files /dev/null and b/Devices/Libraries/Systems/CANopenSocket/docs/splitbar.png differ diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/structCO__CANinterfaceErrorhandler__t.html b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__CANinterfaceErrorhandler__t.html new file mode 100755 index 0000000..321e188 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__CANinterfaceErrorhandler__t.html @@ -0,0 +1,150 @@ + + + + + + + +CANopenNode: CO_CANinterfaceErrorhandler_t Struct Reference + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Data Fields
+
+
CO_CANinterfaceErrorhandler_t Struct Reference
socketCAN » CAN errors & Log
+
+
+ +

socketCAN interface error handling + More...

+ +

#include <CO_error.h>

+ + + + + + + + + + + + + + + + + + + + +

+Data Fields

+int fd
 interface FD
 
+char ifName [IFNAMSIZ]
 interface name as string
 
+uint32_t noackCounter
 counts no ACK on CAN transmission
 
+volatile unsigned char listenOnly
 set to listen only mode
 
+struct timespec timestamp
 listen only mode started at this time
 
+uint16_t CANerrorStatus
 CAN error status bitfield, see CO_CAN_ERR_status_t.
 
+

Detailed Description

+

socketCAN interface error handling

+
The documentation for this struct was generated from the following file: +
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/structCO__CANinterfaceErrorhandler__t.js b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__CANinterfaceErrorhandler__t.js new file mode 100755 index 0000000..dbfb0e0 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__CANinterfaceErrorhandler__t.js @@ -0,0 +1,9 @@ +var structCO__CANinterfaceErrorhandler__t = +[ + [ "fd", "structCO__CANinterfaceErrorhandler__t.html#a06e667f1f90ad62495aa2172d97305b8", null ], + [ "ifName", "structCO__CANinterfaceErrorhandler__t.html#a58a5219f8dad7dc1c98db2b463e6e005", null ], + [ "noackCounter", "structCO__CANinterfaceErrorhandler__t.html#a6d17a248ec3ce1f6c53f4315c0cb9282", null ], + [ "listenOnly", "structCO__CANinterfaceErrorhandler__t.html#a96ddaefd75e680898f93d4891f5fc195", null ], + [ "timestamp", "structCO__CANinterfaceErrorhandler__t.html#a615dfcafdb866ae951296333df35d1c7", null ], + [ "CANerrorStatus", "structCO__CANinterfaceErrorhandler__t.html#a3f9369fb469c76c66a5e06eb7b909c54", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/structCO__CANmodule__t.html b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__CANmodule__t.html new file mode 100755 index 0000000..7ed1295 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__CANmodule__t.html @@ -0,0 +1,213 @@ + + + + + + + +CANopenNode: CO_CANmodule_t Struct Reference + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Data Fields
+
+
CO_CANmodule_t Struct Reference
CANopen_301 » Driver
+
+
+ +

Complete CAN module object. + More...

+ +

#include <CO_driver.h>

+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

+Data Fields

+void * CANptr
 From CO_CANmodule_init()
 
+CO_CANrx_t * rxArray
 From CO_CANmodule_init()
 
+uint16_t rxSize
 From CO_CANmodule_init()
 
+CO_CANtx_t * txArray
 From CO_CANmodule_init()
 
+uint16_t txSize
 From CO_CANmodule_init()
 
+uint16_t CANerrorStatus
 CAN error status bitfield, see CO_CAN_ERR_status_t.
 
+volatile bool_t CANnormal
 CAN module is in normal mode.
 
volatile bool_t useCANrxFilters
 Value different than zero indicates, that CAN module hardware filters are used for CAN reception. More...
 
volatile bool_t bufferInhibitFlag
 If flag is true, then message in transmit buffer is synchronous PDO message, which will be aborted, if CO_clearPendingSyncPDOs() function will be called by application. More...
 
+volatile bool_t firstCANtxMessage
 Equal to 1, when the first transmitted message (bootup message) is in CAN TX buffers.
 
+volatile uint16_t CANtxCount
 Number of messages in transmit buffer, which are waiting to be copied to the CAN module.
 
+uint32_t errOld
 Previous state of CAN errors.
 
+int32_t errinfo
 For use with CO_errinfo()
 
+

Detailed Description

+

Complete CAN module object.

+

Must be defined in the CO_driver_target.h file.

+

Usually it has the following data fields, but they may differ for different microcontrollers.

+

Field Documentation

+ +

◆ useCANrxFilters

+ +
+
+ + + + +
volatile bool_t CO_CANmodule_t::useCANrxFilters
+
+ +

Value different than zero indicates, that CAN module hardware filters are used for CAN reception.

+

If there is not enough hardware filters, they won't be used. In this case will be all received CAN messages processed by software.

+ +
+
+ +

◆ bufferInhibitFlag

+ +
+
+ + + + +
volatile bool_t CO_CANmodule_t::bufferInhibitFlag
+
+ +

If flag is true, then message in transmit buffer is synchronous PDO message, which will be aborted, if CO_clearPendingSyncPDOs() function will be called by application.

+

This may be necessary if Synchronous window time was expired.

+ +
+
+
The documentation for this struct was generated from the following file: +
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/structCO__CANmodule__t.js b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__CANmodule__t.js new file mode 100755 index 0000000..57d0c44 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__CANmodule__t.js @@ -0,0 +1,16 @@ +var structCO__CANmodule__t = +[ + [ "CANptr", "structCO__CANmodule__t.html#aad1c18f6d47621e5a611333dc57cb349", null ], + [ "rxArray", "structCO__CANmodule__t.html#a747e694e15cca5a5579c2c4397a6da39", null ], + [ "rxSize", "structCO__CANmodule__t.html#a88edfd32c2bff5b9f29a2bccd1ac96f3", null ], + [ "txArray", "structCO__CANmodule__t.html#af623f2a045716af77e906b28deee3d3c", null ], + [ "txSize", "structCO__CANmodule__t.html#a6a53d3d12efbe30d1ee08e821d1b4139", null ], + [ "CANerrorStatus", "structCO__CANmodule__t.html#a5757052c57726cb4fb2ac5cdd8ff744d", null ], + [ "CANnormal", "structCO__CANmodule__t.html#a5555f7d10e09da5815526d7c8b10901d", null ], + [ "useCANrxFilters", "structCO__CANmodule__t.html#a9b28f7a6f02d398b3a0ea6cf70fa64f0", null ], + [ "bufferInhibitFlag", "structCO__CANmodule__t.html#ad7af19de0bf39c8927c926b095cb5292", null ], + [ "firstCANtxMessage", "structCO__CANmodule__t.html#ac12bd37971b89b5796fec0de71be0f07", null ], + [ "CANtxCount", "structCO__CANmodule__t.html#a269294aa6a15ba56f92a460fae0536ac", null ], + [ "errOld", "structCO__CANmodule__t.html#aa0627988ddedc3a4ec3bbfcf3b818d06", null ], + [ "errinfo", "structCO__CANmodule__t.html#aead6585dcb9b8214dd77e8f74ca2aea1", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/structCO__CANrx__t.html b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__CANrx__t.html new file mode 100755 index 0000000..5ff91db --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__CANrx__t.html @@ -0,0 +1,144 @@ + + + + + + + +CANopenNode: CO_CANrx_t Struct Reference + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Data Fields
+
+
CO_CANrx_t Struct Reference
CANopen_301 » Driver » Reception of CAN messages
+
+
+ +

Configuration object for CAN received message for specific CANopenNode Object. + More...

+ +

#include <CO_driver.h>

+ + + + + + + + + + + + + + +

+Data Fields

+uint16_t ident
 Standard CAN Identifier (bits 0..10) + RTR (bit 11)
 
+uint16_t mask
 Standard CAN Identifier mask with the same alignment as ident.
 
+void * object
 CANopenNode Object initialized in from CO_CANrxBufferInit()
 
+void(* pCANrx_callback )(void *object, void *message)
 Pointer to CANrx_callback() initialized in CO_CANrxBufferInit()
 
+

Detailed Description

+

Configuration object for CAN received message for specific CANopenNode Object.

+

Must be defined in the CO_driver_target.h file.

+

Data fields of this structure are used exclusively by the driver. Usually it has the following data fields, but they may differ for different microcontrollers. Array of multiple CO_CANrx_t objects is included inside CO_CANmodule_t.

+
The documentation for this struct was generated from the following file: +
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/structCO__CANrx__t.js b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__CANrx__t.js new file mode 100755 index 0000000..87e8c12 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__CANrx__t.js @@ -0,0 +1,7 @@ +var structCO__CANrx__t = +[ + [ "ident", "structCO__CANrx__t.html#a8595c238cf0364bde995dee97d321909", null ], + [ "mask", "structCO__CANrx__t.html#af7a48dd4ac895a19c4031038e2c1222d", null ], + [ "object", "structCO__CANrx__t.html#a957a1ce67cd1d9010889d557bf0c5770", null ], + [ "pCANrx_callback", "structCO__CANrx__t.html#a8e4668eec8326bb9ac08d67afc3060c7", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/structCO__CANtx__t.html b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__CANtx__t.html new file mode 100755 index 0000000..e8bed37 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__CANtx__t.html @@ -0,0 +1,165 @@ + + + + + + + +CANopenNode: CO_CANtx_t Struct Reference + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Data Fields
+
+
CO_CANtx_t Struct Reference
CANopen_301 » Driver » Transmission of CAN messages
+
+
+ +

Configuration object for CAN transmit message for specific CANopenNode Object. + More...

+ +

#include <CO_driver.h>

+ + + + + + + + + + + + + + + + + +

+Data Fields

+uint32_t ident
 CAN identifier as aligned in CAN module.
 
+uint8_t DLC
 Length of CAN message.
 
+uint8_t data [8]
 8 data bytes
 
+volatile bool_t bufferFull
 True if previous message is still in the buffer.
 
volatile bool_t syncFlag
 Synchronous PDO messages has this flag set. More...
 
+

Detailed Description

+

Configuration object for CAN transmit message for specific CANopenNode Object.

+

Must be defined in the CO_driver_target.h file.

+

Data fields of this structure are used exclusively by the driver. Usually it has the following data fields, but they may differ for different microcontrollers. Array of multiple CO_CANtx_t objects is included inside CO_CANmodule_t.

+

Field Documentation

+ +

◆ syncFlag

+ +
+
+ + + + +
volatile bool_t CO_CANtx_t::syncFlag
+
+ +

Synchronous PDO messages has this flag set.

+

It prevents them to be sent outside the synchronous window

+ +
+
+
The documentation for this struct was generated from the following file: +
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/structCO__CANtx__t.js b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__CANtx__t.js new file mode 100755 index 0000000..377b3f0 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__CANtx__t.js @@ -0,0 +1,8 @@ +var structCO__CANtx__t = +[ + [ "ident", "structCO__CANtx__t.html#a9cc2687eb11da14d4c0aa167352c635c", null ], + [ "DLC", "structCO__CANtx__t.html#a9bb96d60314283061f7619e36d870fa0", null ], + [ "data", "structCO__CANtx__t.html#aae5bcdc2296a5d1d53a3105e86dbb66d", null ], + [ "bufferFull", "structCO__CANtx__t.html#a305f0687a4ed7cd533e7937d6ff7d31b", null ], + [ "syncFlag", "structCO__CANtx__t.html#a79c19597a51351b9d6ed1c9bdfd051b3", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/structCO__EM__t.html b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__EM__t.html new file mode 100755 index 0000000..45fb410 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__EM__t.html @@ -0,0 +1,247 @@ + + + + + + + +CANopenNode: CO_EM_t Struct Reference + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Data Fields
+
+
CO_EM_t Struct Reference
CANopen_301 » Emergency
+
+
+ +

Emergency object. + More...

+ +

#include <CO_Emergency.h>

+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

+Data Fields

+uint8_t errorStatusBits [CO_CONFIG_EM_ERR_STATUS_BITS_COUNT/8]
 Bitfield for the internal indication of the error condition.
 
+uint8_t * errorRegister
 Pointer to error register in object dictionary at 0x1001,00.
 
+uint16_t CANerrorStatusOld
 Old CAN error status bitfield.
 
uint32_t fifo [CO_CONFIG_EM_BUFFER_SIZE+1][2]
 Internal circular FIFO buffer for storing pre-processed emergency messages. More...
 
+uint8_t fifoWrPtr
 Pointer for the fifo buffer, where next emergency message will be written by CO_error() function.
 
uint8_t fifoPpPtr
 Pointer for the fifo, where next emergency message has to be post-processed by CO_EM_process() function. More...
 
+uint8_t fifoOverflow
 Indication of overflow - messages in buffer are not post-processed.
 
+uint8_t fifoCount
 Count of emergency messages in fifo, used for OD object 0x1003.
 
+bool_t producerEnabled
 True, if emergency producer is enabled, from Object dictionary.
 
+uint8_t nodeId
 Copy of CANopen node ID, from CO_EM_init()
 
+CO_CANmodule_t * CANdevTx
 From CO_EM_init()
 
+CO_CANtx_t * CANtxBuff
 CAN transmit buffer.
 
+uint16_t producerCanId
 COB ID of emergency message, from Object dictionary.
 
+uint16_t CANdevTxIdx
 From CO_EM_init()
 
uint32_t inhibitEmTime_us
 Inhibit time for emergency message, from Object dictionary. More...
 
+void(* pFunctSignalRx )(const uint16_t ident, const uint16_t errorCode, const uint8_t errorRegister, const uint8_t errorBit, const uint32_t infoCode)
 From CO_EM_initCallbackRx() or NULL.
 
+void(* pFunctSignalPre )(void *object)
 From CO_EM_initCallbackPre() or NULL.
 
+void * functSignalObjectPre
 From CO_EM_initCallbackPre() or NULL.
 
+

Detailed Description

+

Emergency object.

+

Field Documentation

+ +

◆ fifo

+ +
+
+ + + + +
uint32_t CO_EM_t::fifo[CO_CONFIG_EM_BUFFER_SIZE+1][2]
+
+ +

Internal circular FIFO buffer for storing pre-processed emergency messages.

+

Messages are added by CO_error() function. All messages are later post-processed by CO_EM_process() function. Fifo is also used for error history - OD object 0x1003, "Pre-defined error field".

+ +
+
+ +

◆ fifoPpPtr

+ +
+
+ + + + +
uint8_t CO_EM_t::fifoPpPtr
+
+ +

Pointer for the fifo, where next emergency message has to be post-processed by CO_EM_process() function.

+

If equal to bufWrPtr, then all messages has been post-processed.

+ +
+
+ +

◆ inhibitEmTime_us

+ +
+
+ + + + +
uint32_t CO_EM_t::inhibitEmTime_us
+
+ +

Inhibit time for emergency message, from Object dictionary.

+

Internal timer for inhibit time

+ +
+
+
The documentation for this struct was generated from the following file: +
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/structCO__EM__t.js b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__EM__t.js new file mode 100755 index 0000000..4f1dd2b --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__EM__t.js @@ -0,0 +1,21 @@ +var structCO__EM__t = +[ + [ "errorStatusBits", "structCO__EM__t.html#a377eb478f0af20e6e1e23bd77186dcfd", null ], + [ "errorRegister", "structCO__EM__t.html#ae18bb84c6235afbdf3745d9da337c2d3", null ], + [ "CANerrorStatusOld", "structCO__EM__t.html#a2bbed8454995910f4ac54035a0129b1b", null ], + [ "fifo", "structCO__EM__t.html#ac84d3cf89e04ee48fff85d59bb91f3d9", null ], + [ "fifoWrPtr", "structCO__EM__t.html#aa790e927251322c852c26bdb21853647", null ], + [ "fifoPpPtr", "structCO__EM__t.html#a1cf16a27db5ccf065c68b39e8e2d401f", null ], + [ "fifoOverflow", "structCO__EM__t.html#a52c37c126d1f761b0a9cf608123a5976", null ], + [ "fifoCount", "structCO__EM__t.html#a99ffd8be6baebaf6c598b02c7ce31518", null ], + [ "producerEnabled", "structCO__EM__t.html#ad54685d2f0e7ac934edbe0cf5c0b4baf", null ], + [ "nodeId", "structCO__EM__t.html#ac5522470ed7ea0f5e91520a2563b9abc", null ], + [ "CANdevTx", "structCO__EM__t.html#a5d24b22a05c354937894109a30b1f641", null ], + [ "CANtxBuff", "structCO__EM__t.html#a81e9d98c9384b573a8adefaacec3fdec", null ], + [ "producerCanId", "structCO__EM__t.html#a490ef24607a0b637d9bc3a60f616b41e", null ], + [ "CANdevTxIdx", "structCO__EM__t.html#a9a56cba0d9fada8b489884ec766aab04", null ], + [ "inhibitEmTime_us", "structCO__EM__t.html#a82db41fc720e2f2551207bb0d2ba1ae4", null ], + [ "pFunctSignalRx", "structCO__EM__t.html#a71ba138e5c1814446c210ac7d7f2aa02", null ], + [ "pFunctSignalPre", "structCO__EM__t.html#a124a5d8fb51bb600618a9427b14663c4", null ], + [ "functSignalObjectPre", "structCO__EM__t.html#ac1dec593fd20fbf7ccc5e8287e27b2d8", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/structCO__GFC__t.html b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__GFC__t.html new file mode 100755 index 0000000..24a9bdd --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__GFC__t.html @@ -0,0 +1,146 @@ + + + + + + + +CANopenNode: CO_GFC_t Struct Reference + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Data Fields
+
+
CO_GFC_t Struct Reference
CANopen_304 » GFC
+
+
+ +

GFC object. + More...

+ +

#include <CO_GFC.h>

+ + + + + + + + + + + + + + + + + +

+Data Fields

+uint8_t * valid
 From CO_GFC_init()
 
+CO_CANmodule_t * CANdevTx
 From CO_GFC_init()
 
+CO_CANtx_t * CANtxBuff
 CAN transmit buffer inside CANdevTx.
 
+void(* pFunctSignalSafe )(void *object)
 From CO_GFC_initCallbackEnterSafeState() or NULL.
 
+void * functSignalObjectSafe
 From CO_GFC_initCallbackEnterSafeState() or NULL.
 
+

Detailed Description

+

GFC object.

+
The documentation for this struct was generated from the following file: +
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/structCO__GFC__t.js b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__GFC__t.js new file mode 100755 index 0000000..d0c4326 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__GFC__t.js @@ -0,0 +1,8 @@ +var structCO__GFC__t = +[ + [ "valid", "structCO__GFC__t.html#a775fa3a4f1afda4a4be200f56d6e2b54", null ], + [ "CANdevTx", "structCO__GFC__t.html#a202258a9732622f41d338c22a991f1a5", null ], + [ "CANtxBuff", "structCO__GFC__t.html#acdfbeaf134252ffcb5dd5bd3dcf3c784", null ], + [ "pFunctSignalSafe", "structCO__GFC__t.html#af7ffb43e3b2a682941404bc5c23512e5", null ], + [ "functSignalObjectSafe", "structCO__GFC__t.html#a890395b6bf1b77055c52ad150356ea33", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/structCO__GTWA__t.html b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__GTWA__t.html new file mode 100755 index 0000000..933fa5a --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__GTWA__t.html @@ -0,0 +1,329 @@ + + + + + + + +CANopenNode: CO_GTWA_t Struct Reference + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Data Fields
+
+
CO_GTWA_t Struct Reference
CANopen_309 » Gateway ASCII mapping
+
+
+ +

CANopen Gateway-ascii object. + More...

+ +

#include <CO_gateway_ascii.h>

+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

+Data Fields

size_t(* readCallback )(void *object, const char *buf, size_t count, uint8_t *connectionOK)
 Pointer to external function for reading response from Gateway-ascii object. More...
 
+void * readCallbackObject
 Pointer to object, which will be used inside readCallback, from CO_GTWA_init()
 
+uint32_t sequence
 Sequence number of the command.
 
+int32_t net_default
 Default CANopen Net number is undefined (-1) at startup.
 
+int16_t node_default
 Default CANopen Node ID number is undefined (-1) at startup.
 
+uint16_t net
 Current CANopen Net number.
 
+uint8_t node
 Current CANopen Node ID.
 
+CO_fifo_t commFifo
 CO_fifo_t object for command (not pointer)
 
+char commBuf [CO_CONFIG_GTWA_COMM_BUF_SIZE+1]
 Command buffer of usable size CO_CONFIG_GTWA_COMM_BUF_SIZE.
 
+char respBuf [CO_GTWA_RESP_BUF_SIZE]
 Response buffer of usable size CO_GTWA_RESP_BUF_SIZE.
 
+size_t respBufCount
 Actual size of data in respBuf.
 
size_t respBufOffset
 If only part of data has been successfully written into external application (with readCallback()), then Gateway-ascii object will stay in current state. More...
 
+bool_t respHold
 See respBufOffset above.
 
+uint32_t timeDifference_us_cumulative
 Sum of time difference from CO_GTWA_process() in case of respHold.
 
+CO_GTWA_state_t state
 Current state of the gateway object.
 
+uint32_t stateTimeoutTmr
 Timeout timer for the current state.
 
+CO_SDOclient_t * SDO_C
 SDO client object from CO_GTWA_init()
 
+uint16_t SDOtimeoutTime
 Timeout time for SDO transfer in milliseconds, if no response.
 
+bool_t SDOblockTransferEnable
 SDO block transfer enabled?
 
bool_t SDOdataCopyStatus
 Indicate status of data copy from / to SDO buffer. More...
 
+const CO_GTWA_dataType_t * SDOdataType
 Data type of variable in current SDO communication.
 
+CO_NMT_t * NMT
 NMT object from CO_GTWA_init()
 
+CO_LSSmaster_t * LSSmaster
 LSSmaster object from CO_GTWA_init()
 
+CO_LSS_address_t lssAddress
 128 bit number, uniquely identifying each node
 
+uint8_t lssNID
 LSS Node-ID parameter.
 
+uint16_t lssBitrate
 LSS bitrate parameter.
 
+CO_LSS_cs_t lssInquireCs
 LSS inquire parameter.
 
+CO_LSSmaster_fastscan_t lssFastscan
 LSS fastscan parameter.
 
+uint8_t lssSubState
 LSS allnodes sub state parameter.
 
+uint8_t lssNodeCount
 LSS allnodes node count parameter.
 
+bool_t lssStore
 LSS allnodes store parameter.
 
+uint16_t lssTimeout_ms
 LSS allnodes timeout parameter.
 
+char logBuf [CO_CONFIG_GTWA_LOG_BUF_SIZE+1]
 Message log buffer of usable size CO_CONFIG_GTWA_LOG_BUF_SIZE.
 
+CO_fifo_t logFifo
 CO_fifo_t object for message log (not pointer)
 
+const char * helpString
 Offset, when printing help text.
 
+CO_LEDs_t * LEDs
 CO_LEDs_t object for CANopen status LEDs imitation from CO_GTWA_init()
 
+

Detailed Description

+

CANopen Gateway-ascii object.

+

Field Documentation

+ +

◆ readCallback

+ +
+
+ + + + +
size_t(* CO_GTWA_t::readCallback) (void *object, const char *buf, size_t count, uint8_t *connectionOK)
+
+ +

Pointer to external function for reading response from Gateway-ascii object.

+

Pointer is initialized in CO_GTWA_initRead().

+
Parameters
+ + + + + +
objectVoid pointer to custom object
bufBuffer from which data can be read
countCount of bytes available inside buffer
[out]connectionOKdifferent than 0 indicates connection is OK.
+
+
+
Returns
Count of bytes actually transferred.
+ +
+
+ +

◆ respBufOffset

+ +
+
+ + + + +
size_t CO_GTWA_t::respBufOffset
+
+ +

If only part of data has been successfully written into external application (with readCallback()), then Gateway-ascii object will stay in current state.

+

This situation is indicated with respHold variable and respBufOffset indicates offset to untransferred data inside respBuf.

+ +
+
+ +

◆ SDOdataCopyStatus

+ +
+
+ + + + +
bool_t CO_GTWA_t::SDOdataCopyStatus
+
+ +

Indicate status of data copy from / to SDO buffer.

+

If reading, true indicates, that response has started. If writing, true indicates, that SDO buffer contains only part of data and more data will follow.

+ +
+
+
The documentation for this struct was generated from the following file: +
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/structCO__GTWA__t.js b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__GTWA__t.js new file mode 100755 index 0000000..a9ff774 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__GTWA__t.js @@ -0,0 +1,39 @@ +var structCO__GTWA__t = +[ + [ "readCallback", "structCO__GTWA__t.html#a036c4a3a89b8171baba8039fc50876ba", null ], + [ "readCallbackObject", "structCO__GTWA__t.html#ae1b9a86d7020ac21713a9b658a08495b", null ], + [ "sequence", "structCO__GTWA__t.html#a31b7ae3a5da107dfb5432f5a95f9faee", null ], + [ "net_default", "structCO__GTWA__t.html#aec9a1ffe0ce40572452d3f1e36e51c1b", null ], + [ "node_default", "structCO__GTWA__t.html#a2464fa84713d31811e8872b4557d50d1", null ], + [ "net", "structCO__GTWA__t.html#a8df8a3f47d967e4fb0a56e491db0f9e9", null ], + [ "node", "structCO__GTWA__t.html#a38f5c9325dc69820d831688282a63a10", null ], + [ "commFifo", "structCO__GTWA__t.html#a74241ff1c68a8fc05f0b2be601dcf960", null ], + [ "commBuf", "structCO__GTWA__t.html#a51fd91cf468da15e5f943131fa696266", null ], + [ "respBuf", "structCO__GTWA__t.html#a987d9431f47a10272cf9c81b0d0159d1", null ], + [ "respBufCount", "structCO__GTWA__t.html#a67770af170976d4d904fbc044d347376", null ], + [ "respBufOffset", "structCO__GTWA__t.html#a605bfa0c99f4a0235980de0603a050ca", null ], + [ "respHold", "structCO__GTWA__t.html#aef556bb4c595944ebf3de22a2c9d5007", null ], + [ "timeDifference_us_cumulative", "structCO__GTWA__t.html#a8ba7809acba0f2de26eda4a890e68160", null ], + [ "state", "structCO__GTWA__t.html#a5c37389f4a985950708e2fed036daf1f", null ], + [ "stateTimeoutTmr", "structCO__GTWA__t.html#ac5fc932142ed17d04393ae30819fb021", null ], + [ "SDO_C", "structCO__GTWA__t.html#a274945dbaacfd975f86a58566a884769", null ], + [ "SDOtimeoutTime", "structCO__GTWA__t.html#a9fa3fe5ce1806296cf36ef5d8c3ecae2", null ], + [ "SDOblockTransferEnable", "structCO__GTWA__t.html#a0b05c1b89fe8b104b019d12679d4edcf", null ], + [ "SDOdataCopyStatus", "structCO__GTWA__t.html#aae981f9a446cfaad8a5450e77adc32ec", null ], + [ "SDOdataType", "structCO__GTWA__t.html#a7b9bcb2113286454a273b9b43f4e1548", null ], + [ "NMT", "structCO__GTWA__t.html#a6aa019a1583f8ba56fada7c5ed8ec191", null ], + [ "LSSmaster", "structCO__GTWA__t.html#ae2897d681afe5bd45db8306b52734318", null ], + [ "lssAddress", "structCO__GTWA__t.html#aae702db8c811f7e85f3fd8984bde8f9d", null ], + [ "lssNID", "structCO__GTWA__t.html#af49dacd6548ca791a09e78727aeeabad", null ], + [ "lssBitrate", "structCO__GTWA__t.html#ae404009d2987f589cc52b9958d688917", null ], + [ "lssInquireCs", "structCO__GTWA__t.html#a74892b830cb6064d31190ff6bac6a3c9", null ], + [ "lssFastscan", "structCO__GTWA__t.html#aa39bf29226ecaa52d20a6a92b61cf4fa", null ], + [ "lssSubState", "structCO__GTWA__t.html#a7f55184f433e48dd4d4fb52aebed6da6", null ], + [ "lssNodeCount", "structCO__GTWA__t.html#a16f5893d54bce3d2741d8b732ec6c29e", null ], + [ "lssStore", "structCO__GTWA__t.html#a47ec165dd4d545a6ae52067644dbbaab", null ], + [ "lssTimeout_ms", "structCO__GTWA__t.html#a78c3d1a9ebb44a1db4e6eb100ed6c6a7", null ], + [ "logBuf", "structCO__GTWA__t.html#a7400a0dcf3d8ad25b8ad9820237d4f3c", null ], + [ "logFifo", "structCO__GTWA__t.html#af84217848a2f2e4d3f840f4978d4e2ac", null ], + [ "helpString", "structCO__GTWA__t.html#a5ce9a4cef511904ad4038c0b1443d3f6", null ], + [ "LEDs", "structCO__GTWA__t.html#a90ec5c0f770fd1a3ec2ea4f33059357a", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/structCO__HBconsNode__t.html b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__HBconsNode__t.html new file mode 100755 index 0000000..a5d0d65 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__HBconsNode__t.html @@ -0,0 +1,259 @@ + + + + + + + +CANopenNode: CO_HBconsNode_t Struct Reference + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Data Fields
+
+
CO_HBconsNode_t Struct Reference
CANopen_301 » Heartbeat consumer
+
+
+ +

One monitored node inside CO_HBconsumer_t. + More...

+ +

#include <CO_HBconsumer.h>

+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

+Data Fields

+uint8_t nodeId
 Node Id of the monitored node.
 
+CO_NMT_internalState_t NMTstate
 Of the remote node (Heartbeat payload)
 
+CO_HBconsumer_state_t HBstate
 Current heartbeat state.
 
+uint32_t timeoutTimer
 Time since last heartbeat received.
 
+uint32_t time_us
 Consumer heartbeat time from OD.
 
+volatile void * CANrxNew
 Indication if new Heartbeat message received from the CAN bus.
 
+void(* pFunctSignalPre )(void *object)
 From CO_HBconsumer_initCallbackPre() or NULL.
 
+void * functSignalObjectPre
 From CO_HBconsumer_initCallbackPre() or NULL.
 
+CO_NMT_internalState_t NMTstatePrev
 Previous value of the remote node (Heartbeat payload)
 
void(* pFunctSignalNmtChanged )(uint8_t nodeId, uint8_t idx, CO_NMT_internalState_t state, void *object)
 Callback for remote NMT changed event. More...
 
+void * pFunctSignalObjectNmtChanged
 Pointer to object.
 
void(* pFunctSignalHbStarted )(uint8_t nodeId, uint8_t idx, void *object)
 Callback for heartbeat state change to active event. More...
 
+void * functSignalObjectHbStarted
 Pointer to object.
 
void(* pFunctSignalTimeout )(uint8_t nodeId, uint8_t idx, void *object)
 Callback for consumer timeout event. More...
 
+void * functSignalObjectTimeout
 Pointer to object.
 
void(* pFunctSignalRemoteReset )(uint8_t nodeId, uint8_t idx, void *object)
 Callback for remote reset event. More...
 
+void * functSignalObjectRemoteReset
 Pointer to object.
 
+

Detailed Description

+

One monitored node inside CO_HBconsumer_t.

+

Field Documentation

+ +

◆ pFunctSignalNmtChanged

+ +
+
+ + + + +
void(* CO_HBconsNode_t::pFunctSignalNmtChanged) (uint8_t nodeId, uint8_t idx, CO_NMT_internalState_t state, void *object)
+
+ +

Callback for remote NMT changed event.

+

From CO_HBconsumer_initCallbackNmtChanged() or NULL.

+ +
+
+ +

◆ pFunctSignalHbStarted

+ +
+
+ + + + +
void(* CO_HBconsNode_t::pFunctSignalHbStarted) (uint8_t nodeId, uint8_t idx, void *object)
+
+ +

Callback for heartbeat state change to active event.

+

From CO_HBconsumer_initCallbackHeartbeatStarted() or NULL.

+ +
+
+ +

◆ pFunctSignalTimeout

+ +
+
+ + + + +
void(* CO_HBconsNode_t::pFunctSignalTimeout) (uint8_t nodeId, uint8_t idx, void *object)
+
+ +

Callback for consumer timeout event.

+

From CO_HBconsumer_initCallbackTimeout() or NULL.

+ +
+
+ +

◆ pFunctSignalRemoteReset

+ +
+
+ + + + +
void(* CO_HBconsNode_t::pFunctSignalRemoteReset) (uint8_t nodeId, uint8_t idx, void *object)
+
+ +

Callback for remote reset event.

+

From CO_HBconsumer_initCallbackRemoteReset() or NULL.

+ +
+
+
The documentation for this struct was generated from the following file: +
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/structCO__HBconsNode__t.js b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__HBconsNode__t.js new file mode 100755 index 0000000..a68587b --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__HBconsNode__t.js @@ -0,0 +1,20 @@ +var structCO__HBconsNode__t = +[ + [ "nodeId", "structCO__HBconsNode__t.html#a180aca37057c670be35bbdd89f72b812", null ], + [ "NMTstate", "structCO__HBconsNode__t.html#aca87186237691cc315da47d5bcc8ad31", null ], + [ "HBstate", "structCO__HBconsNode__t.html#a6d16bde174d37094149343fcc7025e3c", null ], + [ "timeoutTimer", "structCO__HBconsNode__t.html#ac4ffced8a11aac2b7383244f306c2081", null ], + [ "time_us", "structCO__HBconsNode__t.html#a489df7aff00a25d8f2a63fe968050b08", null ], + [ "CANrxNew", "structCO__HBconsNode__t.html#a4050f7d0406d85db643410cbca65fd14", null ], + [ "pFunctSignalPre", "structCO__HBconsNode__t.html#a61c753c38666dda8a4d4d870fc593ae5", null ], + [ "functSignalObjectPre", "structCO__HBconsNode__t.html#afd6a4fcc8bc4cc9c647e9ef3d91b2431", null ], + [ "NMTstatePrev", "structCO__HBconsNode__t.html#a7fd2636d9f46b7ff47676f716f6f00a4", null ], + [ "pFunctSignalNmtChanged", "structCO__HBconsNode__t.html#a3f4ec7dfe9e47a3e7a9bdad382fd7f56", null ], + [ "pFunctSignalObjectNmtChanged", "structCO__HBconsNode__t.html#a2a1c5abd88c2ecd451e2a1ca65d3dc93", null ], + [ "pFunctSignalHbStarted", "structCO__HBconsNode__t.html#ac44d2a232ca2b352fd717ee8e6f28e90", null ], + [ "functSignalObjectHbStarted", "structCO__HBconsNode__t.html#a50ed8caa11fce685dfbba13d11ada0ef", null ], + [ "pFunctSignalTimeout", "structCO__HBconsNode__t.html#ab6bbeee344ebd1d657bbabc02f51e597", null ], + [ "functSignalObjectTimeout", "structCO__HBconsNode__t.html#aaf6cc300976931c02e3d46ec2b75cc2e", null ], + [ "pFunctSignalRemoteReset", "structCO__HBconsNode__t.html#a7f7ccf80c31d4c764db24b70f9111e7b", null ], + [ "functSignalObjectRemoteReset", "structCO__HBconsNode__t.html#a94b3c3b2b5b24f5ea3ecc1bbc14b79dd", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/structCO__HBconsumer__t.html b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__HBconsumer__t.html new file mode 100755 index 0000000..065b92a --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__HBconsumer__t.html @@ -0,0 +1,220 @@ + + + + + + + +CANopenNode: CO_HBconsumer_t Struct Reference + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Data Fields
+
+
CO_HBconsumer_t Struct Reference
CANopen_301 » Heartbeat consumer
+
+
+ +

Heartbeat consumer object. + More...

+ +

#include <CO_HBconsumer.h>

+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

+Data Fields

+CO_EM_t * em
 From CO_HBconsumer_init()
 
+const uint32_t * HBconsTime
 From CO_HBconsumer_init()
 
+CO_HBconsNode_t * monitoredNodes
 From CO_HBconsumer_init()
 
+uint8_t numberOfMonitoredNodes
 From CO_HBconsumer_init()
 
bool_t allMonitoredActive
 True, if all monitored nodes are active or no node is monitored. More...
 
uint8_t allMonitoredOperational
 True, if all monitored nodes are NMT operational or no node is monitored. More...
 
+bool_t NMTisPreOrOperationalPrev
 previous state of var
 
+CO_CANmodule_t * CANdevRx
 From CO_HBconsumer_init()
 
+uint16_t CANdevRxIdxStart
 From CO_HBconsumer_init()
 
void(* pFunctSignalNmtChanged )(uint8_t nodeId, uint8_t idx, CO_NMT_internalState_t state, void *object)
 Callback for remote NMT changed event. More...
 
+void * pFunctSignalObjectNmtChanged
 Pointer to object.
 
+

Detailed Description

+

Heartbeat consumer object.

+

Object is initilaized by CO_HBconsumer_init(). It contains an array of CO_HBconsNode_t objects.

+

Field Documentation

+ +

◆ allMonitoredActive

+ +
+
+ + + + +
bool_t CO_HBconsumer_t::allMonitoredActive
+
+ +

True, if all monitored nodes are active or no node is monitored.

+

Can be read by the application

+ +
+
+ +

◆ allMonitoredOperational

+ +
+
+ + + + +
uint8_t CO_HBconsumer_t::allMonitoredOperational
+
+ +

True, if all monitored nodes are NMT operational or no node is monitored.

+

Can be read by the application

+ +
+
+ +

◆ pFunctSignalNmtChanged

+ +
+
+ + + + +
void(* CO_HBconsumer_t::pFunctSignalNmtChanged) (uint8_t nodeId, uint8_t idx, CO_NMT_internalState_t state, void *object)
+
+ +

Callback for remote NMT changed event.

+

From CO_HBconsumer_initCallbackNmtChanged() or NULL.

+ +
+
+
The documentation for this struct was generated from the following file: +
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/structCO__HBconsumer__t.js b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__HBconsumer__t.js new file mode 100755 index 0000000..be2623e --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__HBconsumer__t.js @@ -0,0 +1,14 @@ +var structCO__HBconsumer__t = +[ + [ "em", "structCO__HBconsumer__t.html#aae5e363ccc6a6fd3b17a35e0430add2a", null ], + [ "HBconsTime", "structCO__HBconsumer__t.html#a1cd314f387357f2ce13d4093f477fff5", null ], + [ "monitoredNodes", "structCO__HBconsumer__t.html#a737b37c544a28eff8de0b03b51cbeec8", null ], + [ "numberOfMonitoredNodes", "structCO__HBconsumer__t.html#a5b944043074d42017be3b76320030542", null ], + [ "allMonitoredActive", "structCO__HBconsumer__t.html#aaff60bb59e36a3b0ddd11b45268eaf33", null ], + [ "allMonitoredOperational", "structCO__HBconsumer__t.html#a9407103796db857229ec5b266c580b37", null ], + [ "NMTisPreOrOperationalPrev", "structCO__HBconsumer__t.html#a2fe3d81e2124918d0d5947e6891a060e", null ], + [ "CANdevRx", "structCO__HBconsumer__t.html#af5d8828478e2b51fe47a63f24dd896b1", null ], + [ "CANdevRxIdxStart", "structCO__HBconsumer__t.html#a00d176c84d169115399c276031b71722", null ], + [ "pFunctSignalNmtChanged", "structCO__HBconsumer__t.html#a58314f7d35efc2dbeae14ba4be76dec1", null ], + [ "pFunctSignalObjectNmtChanged", "structCO__HBconsumer__t.html#ad583c93f4e59f98669cd18f263aee45a", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/structCO__LEDs__t.html b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__LEDs__t.html new file mode 100755 index 0000000..5f22dba --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__LEDs__t.html @@ -0,0 +1,158 @@ + + + + + + + +CANopenNode: CO_LEDs_t Struct Reference + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Data Fields
+
+
CO_LEDs_t Struct Reference
CANopen_303 » LED indicators
+
+
+ +

LEDs object, initialized by CO_LEDs_init() + More...

+ +

#include <CO_LEDs.h>

+ + + + + + + + + + + + + + + + + + + + + + + + + + +

+Data Fields

+uint32_t LEDtmr50ms
 50ms led timer
 
+uint8_t LEDtmr200ms
 200ms led timer
 
+uint8_t LEDtmrflash_1
 single flash led timer
 
+uint8_t LEDtmrflash_2
 double flash led timer
 
+uint8_t LEDtmrflash_3
 triple flash led timer
 
+uint8_t LEDtmrflash_4
 quadruple flash led timer
 
+uint8_t LEDred
 red led CO_LED_BITFIELD_t
 
+uint8_t LEDgreen
 green led CO_LED_BITFIELD_t
 
+

Detailed Description

+

LEDs object, initialized by CO_LEDs_init()

+
The documentation for this struct was generated from the following file: +
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/structCO__LEDs__t.js b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__LEDs__t.js new file mode 100755 index 0000000..a41b534 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__LEDs__t.js @@ -0,0 +1,11 @@ +var structCO__LEDs__t = +[ + [ "LEDtmr50ms", "structCO__LEDs__t.html#ae27f4bbc313e08e25b9b1e1d515ea9e4", null ], + [ "LEDtmr200ms", "structCO__LEDs__t.html#a2690fe669ce01322e14de1b9f559a147", null ], + [ "LEDtmrflash_1", "structCO__LEDs__t.html#a2431d9736416e1b5b6c57f61f029dc04", null ], + [ "LEDtmrflash_2", "structCO__LEDs__t.html#ae6aa2a97a9496abc67774d94caa0ea1a", null ], + [ "LEDtmrflash_3", "structCO__LEDs__t.html#ad3af9ed6c2c43330cc3eadb3b76de2c4", null ], + [ "LEDtmrflash_4", "structCO__LEDs__t.html#ae33491a6eed2761893fea47dd65c5a78", null ], + [ "LEDred", "structCO__LEDs__t.html#ad8624f0baa6186e523e072e6101c7a84", null ], + [ "LEDgreen", "structCO__LEDs__t.html#ac2b4eb053725681f1935c8a6e4184f85", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/structCO__LSSmaster__fastscan__t.html b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__LSSmaster__fastscan__t.html new file mode 100755 index 0000000..32ee498 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__LSSmaster__fastscan__t.html @@ -0,0 +1,138 @@ + + + + + + + +CANopenNode: CO_LSSmaster_fastscan_t Struct Reference + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Data Fields
+
+
CO_LSSmaster_fastscan_t Struct Reference
CANopen_305 » LSS Master
+
+
+ +

Parameters for LSS fastscan CO_LSSmaster_IdentifyFastscan. + More...

+ +

#include <CO_LSSmaster.h>

+ + + + + + + + + + + +

+Data Fields

+CO_LSSmaster_scantype_t scan [4]
 Scan type for each part of the LSS address.
 
+CO_LSS_address_t match
 Value to match in case of CO_LSSmaster_FS_MATCH.
 
+CO_LSS_address_t found
 Scan result.
 
+

Detailed Description

+

Parameters for LSS fastscan CO_LSSmaster_IdentifyFastscan.

+
The documentation for this struct was generated from the following file: +
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/structCO__LSSmaster__fastscan__t.js b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__LSSmaster__fastscan__t.js new file mode 100755 index 0000000..5b88016 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__LSSmaster__fastscan__t.js @@ -0,0 +1,6 @@ +var structCO__LSSmaster__fastscan__t = +[ + [ "scan", "structCO__LSSmaster__fastscan__t.html#a42853c0091c96d7fc7e763c2be3b6e8a", null ], + [ "match", "structCO__LSSmaster__fastscan__t.html#a69540f77885162e936803a9526d3c342", null ], + [ "found", "structCO__LSSmaster__fastscan__t.html#a9c34a389339a46da81f2234443f20fb9", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/structCO__LSSmaster__t.html b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__LSSmaster__t.html new file mode 100755 index 0000000..d1c8ad5 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__LSSmaster__t.html @@ -0,0 +1,199 @@ + + + + + + + +CANopenNode: CO_LSSmaster_t Struct Reference + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Data Fields
+
+
CO_LSSmaster_t Struct Reference
CANopen_305 » LSS Master
+
+
+ +

LSS master object. + More...

+ +

#include <CO_LSSmaster.h>

+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

+Data Fields

+uint32_t timeout_us
 LSS response timeout in us.
 
+uint8_t state
 Node is currently selected.
 
+uint8_t command
 Active command.
 
+uint32_t timeoutTimer
 Timeout timer for LSS communication.
 
+uint8_t fsState
 Current state of fastscan master state machine.
 
+uint8_t fsLssSub
 Current state of node state machine.
 
+uint8_t fsBitChecked
 Current scan bit position.
 
+uint32_t fsIdNumber
 Current scan result.
 
volatile void * CANrxNew
 Indication if new LSS message is received from CAN bus. More...
 
+uint8_t CANrxData [8]
 8 data bytes of the received message
 
+void(* pFunctSignal )(void *object)
 From CO_LSSmaster_initCallbackPre() or NULL.
 
+void * functSignalObject
 Pointer to object.
 
+CO_CANmodule_t * CANdevTx
 From CO_LSSmaster_init()
 
+CO_CANtx_t * TXbuff
 CAN transmit buffer.
 
+

Detailed Description

+

LSS master object.

+

Field Documentation

+ +

◆ CANrxNew

+ +
+
+ + + + +
volatile void* CO_LSSmaster_t::CANrxNew
+
+ +

Indication if new LSS message is received from CAN bus.

+

It needs to be cleared when received message is completely processed.

+ +
+
+
The documentation for this struct was generated from the following file: +
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/structCO__LSSmaster__t.js b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__LSSmaster__t.js new file mode 100755 index 0000000..c7c4aa2 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__LSSmaster__t.js @@ -0,0 +1,17 @@ +var structCO__LSSmaster__t = +[ + [ "timeout_us", "structCO__LSSmaster__t.html#aeac43e30ee9018bb34876a4c2f1a10de", null ], + [ "state", "structCO__LSSmaster__t.html#a1fb90a878809a6690dd4493444795e30", null ], + [ "command", "structCO__LSSmaster__t.html#a745a126919856c94e7f93186830e6940", null ], + [ "timeoutTimer", "structCO__LSSmaster__t.html#aa706f4a19295e26a862127917ebc9eca", null ], + [ "fsState", "structCO__LSSmaster__t.html#ad7ee57af199cfd615f6caa07358f0ce7", null ], + [ "fsLssSub", "structCO__LSSmaster__t.html#a9962ae690930bf58a447a47bf708c5f2", null ], + [ "fsBitChecked", "structCO__LSSmaster__t.html#ab71abc74d5d92f149ca991c9bb47ed99", null ], + [ "fsIdNumber", "structCO__LSSmaster__t.html#aca7e7be8c297c612b2200b866ed7b248", null ], + [ "CANrxNew", "structCO__LSSmaster__t.html#ad5fd09faed937c053f1ded1df54f40cd", null ], + [ "CANrxData", "structCO__LSSmaster__t.html#a65cc40d755d5b2eaaeb6f1b29332dca8", null ], + [ "pFunctSignal", "structCO__LSSmaster__t.html#a9b8376cf89bba5a3650f82da61ffee5b", null ], + [ "functSignalObject", "structCO__LSSmaster__t.html#aafce10b9e126c5c157cb137e8b65b27c", null ], + [ "CANdevTx", "structCO__LSSmaster__t.html#a480123707829720cc82b08b65923abcb", null ], + [ "TXbuff", "structCO__LSSmaster__t.html#ad915ab10431aea283df0d8e1299876e2", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/structCO__LSSslave__t.html b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__LSSslave__t.html new file mode 100755 index 0000000..007fc45 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__LSSslave__t.html @@ -0,0 +1,247 @@ + + + + + + + +CANopenNode: CO_LSSslave_t Struct Reference + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Data Fields
+
+
CO_LSSslave_t Struct Reference
CANopen_305 » LSS Slave
+
+
+ +

LSS slave object. + More...

+ +

#include <CO_LSSslave.h>

+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

+Data Fields

+CO_LSS_address_t lssAddress
 From CO_LSSslave_init.
 
+CO_LSS_state_t lssState
 CO_LSS_state_t
 
+CO_LSS_address_t lssSelect
 Received LSS Address by select.
 
+CO_LSS_address_t lssFastscan
 Received LSS Address by fastscan.
 
+uint8_t fastscanPos
 Current state of fastscan.
 
+uint16_t * pendingBitRate
 Bit rate value that is temporarily configured.
 
+uint8_t * pendingNodeID
 Node ID that is temporarily configured.
 
+uint8_t activeNodeID
 Node ID used at the CAN interface.
 
+volatile void * sendResponse
 Variable indicates, if LSS response has to be sent by mainline processing function.
 
+CO_LSS_cs_t service
 Service, which will have to be processed by mainline processing function.
 
+uint8_t CANdata [8]
 Received CAN data, which will be processed by mainline processing function.
 
+void(* pFunctSignalPre )(void *object)
 From CO_LSSslave_initCallbackPre() or NULL.
 
+void * functSignalObjectPre
 Pointer to object.
 
+bool_t(* pFunctLSScheckBitRate )(void *object, uint16_t bitRate)
 From CO_LSSslave_initCheckBitRateCallback() or NULL.
 
void(* pFunctLSSactivateBitRate )(void *object, uint16_t delay)
 Pointer to object. More...
 
bool_t(* pFunctLSScfgStore )(void *object, uint8_t id, uint16_t bitRate)
 Pointer to object. More...
 
CO_CANmodule_t * CANdevTx
 Pointer to object. More...
 
+CO_CANtx_t * TXbuff
 CAN transmit buffer.
 
+

Detailed Description

+

LSS slave object.

+

Field Documentation

+ +

◆ pFunctLSSactivateBitRate

+ +
+
+ + + + +
void(* CO_LSSslave_t::pFunctLSSactivateBitRate) (void *object, uint16_t delay)
+
+ +

Pointer to object.

+

From CO_LSSslave_initActivateBitRateCallback() or NULL. Delay is in ms

+ +
+
+ +

◆ pFunctLSScfgStore

+ +
+
+ + + + +
bool_t(* CO_LSSslave_t::pFunctLSScfgStore) (void *object, uint8_t id, uint16_t bitRate)
+
+ +

Pointer to object.

+

From CO_LSSslave_initCfgStoreCallback() or NULL

+ +
+
+ +

◆ CANdevTx

+ +
+
+ + + + +
CO_CANmodule_t* CO_LSSslave_t::CANdevTx
+
+ +

Pointer to object.

+

From CO_LSSslave_init()

+ +
+
+
The documentation for this struct was generated from the following file: +
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/structCO__LSSslave__t.js b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__LSSslave__t.js new file mode 100755 index 0000000..06ef33b --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__LSSslave__t.js @@ -0,0 +1,21 @@ +var structCO__LSSslave__t = +[ + [ "lssAddress", "structCO__LSSslave__t.html#ae18023e6feb33ea4e9ddf26796a99fe0", null ], + [ "lssState", "structCO__LSSslave__t.html#a514565a5cda1630165c3640ef223aa28", null ], + [ "lssSelect", "structCO__LSSslave__t.html#a38d6e43b3a1c3b0ce20bdb9108f8ab91", null ], + [ "lssFastscan", "structCO__LSSslave__t.html#a433215fe4d5f9764427a5e0dc57d60a5", null ], + [ "fastscanPos", "structCO__LSSslave__t.html#af163113cf94a4a2c74521ffe194522b2", null ], + [ "pendingBitRate", "structCO__LSSslave__t.html#a90b0fabc76e788404da520a7a5e90e48", null ], + [ "pendingNodeID", "structCO__LSSslave__t.html#a9a632a4655fb5adfd91bd3e26ed43f0b", null ], + [ "activeNodeID", "structCO__LSSslave__t.html#a91bb370cba5215ddaf52c0883a9bdca2", null ], + [ "sendResponse", "structCO__LSSslave__t.html#af9fb29812cd548f34223cc6d16de8121", null ], + [ "service", "structCO__LSSslave__t.html#acdffe4100d9031c885ba5b5b995e471c", null ], + [ "CANdata", "structCO__LSSslave__t.html#a80021b6e36da1b495453d360a7001287", null ], + [ "pFunctSignalPre", "structCO__LSSslave__t.html#a9a6db85a8e5a14652e3b1fb7026790a1", null ], + [ "functSignalObjectPre", "structCO__LSSslave__t.html#a346a28ca3adef3050f6cc29ec182059d", null ], + [ "pFunctLSScheckBitRate", "structCO__LSSslave__t.html#a1482f4cef4b3ac4afbdf569306e32aab", null ], + [ "pFunctLSSactivateBitRate", "structCO__LSSslave__t.html#a8c2bfab9e6225cd65b48149333d9e1dd", null ], + [ "pFunctLSScfgStore", "structCO__LSSslave__t.html#a53b9620f867ad5f534c99955a2479f16", null ], + [ "CANdevTx", "structCO__LSSslave__t.html#a6ce85337359f0e4cd948c438f2960015", null ], + [ "TXbuff", "structCO__LSSslave__t.html#a12b8505b3f2bcf2085b38bbdcda6736f", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/structCO__NMT__t.html b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__NMT__t.html new file mode 100755 index 0000000..ee457ce --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__NMT__t.html @@ -0,0 +1,203 @@ + + + + + + + +CANopenNode: CO_NMT_t Struct Reference + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Data Fields
+
+
CO_NMT_t Struct Reference
CANopen_301 » NMT and Heartbeat
+
+
+ +

NMT consumer and Heartbeat producer object. + More...

+ +

#include <CO_NMT_Heartbeat.h>

+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

+Data Fields

+uint8_t operatingState
 Current NMT operating state.
 
+uint8_t operatingStatePrev
 Previous NMT operating state.
 
uint8_t internalCommand
 NMT internal command from CO_NMT_receive() or CO_NMT_sendCommand(), processed in CO_NMT_process(). More...
 
+uint8_t nodeId
 From CO_NMT_init()
 
+CO_NMT_control_t NMTcontrol
 From CO_NMT_init()
 
+uint32_t HBproducerTime_us
 Producer heartbeat time, calculated from OD 0x1017.
 
+uint32_t HBproducerTimer
 Internal timer for HB producer.
 
+CO_EM_t * em
 From CO_NMT_init()
 
+CO_CANmodule_t * NMT_CANdevTx
 From CO_NMT_init()
 
+CO_CANtx_t * NMT_TXbuff
 CAN transmit buffer for NMT master message.
 
+CO_CANmodule_t * HB_CANdevTx
 From CO_NMT_init()
 
+CO_CANtx_t * HB_TXbuff
 CAN transmit buffer for heartbeat message.
 
+void(* pFunctSignalPre )(void *object)
 From CO_NMT_initCallbackPre() or NULL.
 
+void * functSignalObjectPre
 From CO_NMT_initCallbackPre() or NULL.
 
+void(* pFunctNMT )(CO_NMT_internalState_t state)
 From CO_NMT_initCallbackChanged() or NULL.
 
+

Detailed Description

+

NMT consumer and Heartbeat producer object.

+

Field Documentation

+ +

◆ internalCommand

+ +
+
+ + + + +
uint8_t CO_NMT_t::internalCommand
+
+ +

NMT internal command from CO_NMT_receive() or CO_NMT_sendCommand(), processed in CO_NMT_process().

+

0 if no command or CO_NMT_command_t

+ +
+
+
The documentation for this struct was generated from the following file: +
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/structCO__NMT__t.js b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__NMT__t.js new file mode 100755 index 0000000..43de7cd --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__NMT__t.js @@ -0,0 +1,18 @@ +var structCO__NMT__t = +[ + [ "operatingState", "structCO__NMT__t.html#abebf973c819c48fa36c48ba2eb84818a", null ], + [ "operatingStatePrev", "structCO__NMT__t.html#a544df8108d0853dd6d2938fa253ef018", null ], + [ "internalCommand", "structCO__NMT__t.html#a7a56fcf387bd2922ece9106538d986ef", null ], + [ "nodeId", "structCO__NMT__t.html#a6cf0441aff58a3e208d1ed221a26709c", null ], + [ "NMTcontrol", "structCO__NMT__t.html#a0c28cae6c3c7319c8fdfe694a3b4d9a7", null ], + [ "HBproducerTime_us", "structCO__NMT__t.html#a983a4ba9a004d216e32414c0a38736c4", null ], + [ "HBproducerTimer", "structCO__NMT__t.html#a0babc82bd6dfde07511b87167941b4f0", null ], + [ "em", "structCO__NMT__t.html#a60848ed23fb775dc4412be0e7ff9bc4b", null ], + [ "NMT_CANdevTx", "structCO__NMT__t.html#aaa1a7f9278595d82eaddbee8ac434ca9", null ], + [ "NMT_TXbuff", "structCO__NMT__t.html#a6c9ca6315daa74257699ff657165d409", null ], + [ "HB_CANdevTx", "structCO__NMT__t.html#aedcf17c643f41370a978794301f00169", null ], + [ "HB_TXbuff", "structCO__NMT__t.html#aa9fb1bb36758a1ff27b44c20fd6a0192", null ], + [ "pFunctSignalPre", "structCO__NMT__t.html#a31b37e4e64737bb3ef4331d607adbed6", null ], + [ "functSignalObjectPre", "structCO__NMT__t.html#a9ae2c962bc4aaa477555920dc0323302", null ], + [ "pFunctNMT", "structCO__NMT__t.html#a64bc9d74c871bf560d8b5db85e4d051e", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/structCO__OD__storage__t.html b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__OD__storage__t.html new file mode 100755 index 0000000..1a99638 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__OD__storage__t.html @@ -0,0 +1,147 @@ + + + + + + + +CANopenNode: CO_OD_storage_t Struct Reference + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Data Fields
+
+
CO_OD_storage_t Struct Reference
socketCAN » OD storage
+
+
+ +

Object Dictionary storage object. + More...

+ +

#include <CO_OD_storage.h>

+ + + + + + + + + + + + + + + + + +

+Data Fields

+uint8_t * odAddress
 From CO_OD_storage_init()
 
+uint32_t odSize
 From CO_OD_storage_init()
 
+char * filename
 From CO_OD_storage_init()
 
+FILE * fp
 If CO_OD_storage_autoSave() is used, file stays opened and fp is stored here.
 
+uint32_t lastSavedUs
 used with CO_OD_storage_autoSave.
 
+

Detailed Description

+

Object Dictionary storage object.

+

Object is used with CANopen OD objects at index 1010 and 1011.

+
The documentation for this struct was generated from the following file: +
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/structCO__OD__storage__t.js b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__OD__storage__t.js new file mode 100755 index 0000000..92c5563 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__OD__storage__t.js @@ -0,0 +1,8 @@ +var structCO__OD__storage__t = +[ + [ "odAddress", "structCO__OD__storage__t.html#ac434d6480330c026761fe8a82e32839b", null ], + [ "odSize", "structCO__OD__storage__t.html#aefd1cb33fa031c1592b20643bb38bfbb", null ], + [ "filename", "structCO__OD__storage__t.html#afb3dd08b01bf20f251754c32b116d8fe", null ], + [ "fp", "structCO__OD__storage__t.html#a8976cb73fd4a2baadc4689fdb8b876a1", null ], + [ "lastSavedUs", "structCO__OD__storage__t.html#a94d80f0c140485ab426891839b356347", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/structCO__RPDOCommPar__t.html b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__RPDOCommPar__t.html new file mode 100755 index 0000000..a4189c6 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__RPDOCommPar__t.html @@ -0,0 +1,182 @@ + + + + + + + +CANopenNode: CO_RPDOCommPar_t Struct Reference + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Data Fields
+
+
CO_RPDOCommPar_t Struct Reference
CANopen_301 » PDO
+
+
+ +

RPDO communication parameter. + More...

+ +

#include <CO_PDO.h>

+ + + + + + + + + + + +

+Data Fields

+uint8_t maxSubIndex
 Equal to 2.
 
uint32_t COB_IDUsedByRPDO
 Communication object identifier for message received. More...
 
uint8_t transmissionType
 Transmission type. More...
 
+

Detailed Description

+

RPDO communication parameter.

+

The same as record from Object dictionary (index 0x1400+).

+

Field Documentation

+ +

◆ COB_IDUsedByRPDO

+ +
+
+ + + + +
uint32_t CO_RPDOCommPar_t::COB_IDUsedByRPDO
+
+ +

Communication object identifier for message received.

+

Meaning of the specific bits:

    +
  • Bit 0-10: COB-ID for PDO, to change it bit 31 must be set.
    • +
  • Bit 11-29: set to 0 for 11 bit COB-ID.
    • +
  • Bit 30: If true, rtr are NOT allowed for PDO.
    • +
  • Bit 31: If true, node does NOT use the PDO.
    • +
+ +
+
+ +

◆ transmissionType

+ +
+
+ + + + +
uint8_t CO_RPDOCommPar_t::transmissionType
+
+ +

Transmission type.

+

Values:

    +
  • 0-240: Reciving is synchronous, process after next reception of the SYNC object.
    • +
  • 241-253: Not used.
    • +
  • 254: Manufacturer specific.
    • +
  • 255: Asynchronous.
    • +
+ +
+
+
The documentation for this struct was generated from the following file: +
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/structCO__RPDOCommPar__t.js b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__RPDOCommPar__t.js new file mode 100755 index 0000000..3cdb474 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__RPDOCommPar__t.js @@ -0,0 +1,6 @@ +var structCO__RPDOCommPar__t = +[ + [ "maxSubIndex", "structCO__RPDOCommPar__t.html#ac49d1bc96c31ec32015628128a6e60a7", null ], + [ "COB_IDUsedByRPDO", "structCO__RPDOCommPar__t.html#a798b8c59ea8f8b627c307f1246b6ee78", null ], + [ "transmissionType", "structCO__RPDOCommPar__t.html#a09eb4787337cf8579c0ff1d4ae968aba", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/structCO__RPDOMapPar__t.html b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__RPDOMapPar__t.html new file mode 100755 index 0000000..c5d9564 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__RPDOMapPar__t.html @@ -0,0 +1,200 @@ + + + + + + + +CANopenNode: CO_RPDOMapPar_t Struct Reference + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Data Fields
+
+
CO_RPDOMapPar_t Struct Reference
CANopen_301 » PDO
+
+
+ +

RPDO mapping parameter. + More...

+ +

#include <CO_PDO.h>

+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

+Data Fields

uint8_t numberOfMappedObjects
 Actual number of mapped objects from 0 to 8. More...
 
uint32_t mappedObject1
 Location and size of the mapped object. More...
 
+uint32_t mappedObject2
 Same.
 
+uint32_t mappedObject3
 Same.
 
+uint32_t mappedObject4
 Same.
 
+uint32_t mappedObject5
 Same.
 
+uint32_t mappedObject6
 Same.
 
+uint32_t mappedObject7
 Same.
 
+uint32_t mappedObject8
 Same.
 
+

Detailed Description

+

RPDO mapping parameter.

+

The same as record from Object dictionary (index 0x1600+).

+

Field Documentation

+ +

◆ numberOfMappedObjects

+ +
+
+ + + + +
uint8_t CO_RPDOMapPar_t::numberOfMappedObjects
+
+ +

Actual number of mapped objects from 0 to 8.

+

To change mapped object, this value must be 0.

+ +
+
+ +

◆ mappedObject1

+ +
+
+ + + + +
uint32_t CO_RPDOMapPar_t::mappedObject1
+
+ +

Location and size of the mapped object.

+

Bit meanings 0xIIIISSLL:

    +
  • Bit 0-7: Data Length in bits.
    • +
  • Bit 8-15: Subindex from object distionary.
    • +
  • Bit 16-31: Index from object distionary.
    • +
+ +
+
+
The documentation for this struct was generated from the following file: +
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/structCO__RPDOMapPar__t.js b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__RPDOMapPar__t.js new file mode 100755 index 0000000..bc03e88 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__RPDOMapPar__t.js @@ -0,0 +1,12 @@ +var structCO__RPDOMapPar__t = +[ + [ "numberOfMappedObjects", "structCO__RPDOMapPar__t.html#a479613deae0d06607897093d617edb1d", null ], + [ "mappedObject1", "structCO__RPDOMapPar__t.html#a0a161a9792479c07aaf12447fbac2499", null ], + [ "mappedObject2", "structCO__RPDOMapPar__t.html#a63f9b87f27d30e6f4faf65a56b458ffb", null ], + [ "mappedObject3", "structCO__RPDOMapPar__t.html#a3ac0b7cd18683c45d3dcd0e18b13810b", null ], + [ "mappedObject4", "structCO__RPDOMapPar__t.html#a0a3ef7ae329b91ec73d05301e870ad75", null ], + [ "mappedObject5", "structCO__RPDOMapPar__t.html#af532c96971699321f04aad14dbee52a4", null ], + [ "mappedObject6", "structCO__RPDOMapPar__t.html#a1082b9de3f132363f78c19004be017fb", null ], + [ "mappedObject7", "structCO__RPDOMapPar__t.html#ad66f07a873d2ceb0b10bdad242925984", null ], + [ "mappedObject8", "structCO__RPDOMapPar__t.html#a2f1467c9ba8a91e4f2742c08ed8551f6", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/structCO__RPDO__t.html b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__RPDO__t.html new file mode 100755 index 0000000..4e2d5cd --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__RPDO__t.html @@ -0,0 +1,219 @@ + + + + + + + +CANopenNode: CO_RPDO_t Struct Reference + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Data Fields
+
+
CO_RPDO_t Struct Reference
CANopen_301 » PDO
+
+
+ +

RPDO object. + More...

+ +

#include <CO_PDO.h>

+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

+Data Fields

+CO_EM_t * em
 From CO_RPDO_init()
 
+CO_SDO_t * SDO
 From CO_RPDO_init()
 
+const CO_RPDOCommPar_t * RPDOCommPar
 From CO_RPDO_init()
 
+const CO_RPDOMapPar_t * RPDOMapPar
 From CO_RPDO_init()
 
+CO_NMT_internalState_t * operatingState
 From CO_RPDO_init()
 
+uint8_t nodeId
 From CO_RPDO_init()
 
+uint16_t defaultCOB_ID
 From CO_RPDO_init()
 
+uint8_t restrictionFlags
 From CO_RPDO_init()
 
+bool_t valid
 True, if PDO is enabled and valid.
 
uint8_t dataLength
 Data length of the received PDO message. More...
 
+uint8_t * mapPointer [8]
 Pointers to 8 data objects, where PDO will be copied.
 
+CO_SYNC_t * SYNC
 From CO_RPDO_init()
 
+bool_t synchronous
 True, if PDO synchronous (transmissionType <= 240)
 
+volatile void * CANrxNew [2]
 Variable indicates, if new PDO message received from CAN bus.
 
+uint8_t CANrxData [2][8]
 8 data bytes of the received message.
 
+void(* pFunctSignalPre )(void *object)
 From CO_RPDO_initCallbackPre() or NULL.
 
+void * functSignalObjectPre
 From CO_RPDO_initCallbackPre() or NULL.
 
+CO_CANmodule_t * CANdevRx
 From CO_RPDO_init()
 
+uint16_t CANdevRxIdx
 From CO_RPDO_init()
 
+

Detailed Description

+

RPDO object.

+

Field Documentation

+ +

◆ dataLength

+ +
+
+ + + + +
uint8_t CO_RPDO_t::dataLength
+
+ +

Data length of the received PDO message.

+

Calculated from mapping

+ +
+
+
The documentation for this struct was generated from the following file: +
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/structCO__RPDO__t.js b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__RPDO__t.js new file mode 100755 index 0000000..699bf07 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__RPDO__t.js @@ -0,0 +1,22 @@ +var structCO__RPDO__t = +[ + [ "em", "structCO__RPDO__t.html#a5261e898fc67ecba19f0fc146e4a13ef", null ], + [ "SDO", "structCO__RPDO__t.html#abcf8134da148073ec8e3f1dd6f0c8da1", null ], + [ "RPDOCommPar", "structCO__RPDO__t.html#a1aaaf9abb01030dbd732985d07ed8c33", null ], + [ "RPDOMapPar", "structCO__RPDO__t.html#a69ad9068dfcee1d12697430856e62d10", null ], + [ "operatingState", "structCO__RPDO__t.html#a85583dccb8f2d0515288888e56065e70", null ], + [ "nodeId", "structCO__RPDO__t.html#a15e1425101d92521ad219695036b1cd2", null ], + [ "defaultCOB_ID", "structCO__RPDO__t.html#a9e327dba172ebbd3112097e5085eea2f", null ], + [ "restrictionFlags", "structCO__RPDO__t.html#a1759ebaef816a352d37e717c9360458a", null ], + [ "valid", "structCO__RPDO__t.html#a1d9a4be6ad3245309ffe6e3ad5637942", null ], + [ "dataLength", "structCO__RPDO__t.html#af742fd80b982822c3e06770ab0877cc3", null ], + [ "mapPointer", "structCO__RPDO__t.html#a998b83bc1cbf11aa4e9170ce03c9d203", null ], + [ "SYNC", "structCO__RPDO__t.html#a2545c12748b54a1ef3e5660c82e1adf8", null ], + [ "synchronous", "structCO__RPDO__t.html#a08c58c120349e150e4188666a5113916", null ], + [ "CANrxNew", "structCO__RPDO__t.html#a4ce980b6cc3a0e497a0138c9c4a69d41", null ], + [ "CANrxData", "structCO__RPDO__t.html#a514e6d57efc477bdb49cea75e6495a95", null ], + [ "pFunctSignalPre", "structCO__RPDO__t.html#a3c0573cc4e76a601bbbd710f19b9615f", null ], + [ "functSignalObjectPre", "structCO__RPDO__t.html#aff7dd123460cc50708f28d40d851f8dd", null ], + [ "CANdevRx", "structCO__RPDO__t.html#a3f153d5326f07ec7c8f3570287547454", null ], + [ "CANdevRxIdx", "structCO__RPDO__t.html#a000fe56fcaf727c59b7dc049ec7fc4b1", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/structCO__SDOclient__t.html b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__SDOclient__t.html new file mode 100755 index 0000000..99e40b0 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__SDOclient__t.html @@ -0,0 +1,331 @@ + + + + + + + +CANopenNode: CO_SDOclient_t Struct Reference + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Data Fields
+
+
CO_SDOclient_t Struct Reference
CANopen_301 » SDO client
+
+
+ +

SDO client object. + More...

+ +

#include <CO_SDOclient.h>

+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

+Data Fields

+const OD_t * OD
 From CO_SDOclient_init()
 
+uint8_t nodeId
 From CO_SDOclient_init()
 
+OD_IO_t OD_IO
 Object dictionary interface for locally transferred object.
 
+OD_attr_t attribute
 Attribute for locally transferred OD sub-object, see OD_attributes_t.
 
+CO_CANmodule_t * CANdevRx
 From CO_SDOclient_init()
 
+uint16_t CANdevRxIdx
 From CO_SDOclient_init()
 
+CO_CANmodule_t * CANdevTx
 From CO_SDOclient_init()
 
+uint16_t CANdevTxIdx
 From CO_SDOclient_init()
 
+CO_CANtx_t * CANtxBuff
 CAN transmit buffer inside CANdevTx for CAN tx message.
 
uint32_t COB_IDClientToServer
 Copy of CANopen COB_ID Client -> Server, meaning of the specific bits: More...
 
+uint32_t COB_IDServerToClient
 Copy of CANopen COB_ID Server -> Client, similar as above.
 
+uint8_t nodeIDOfTheSDOServer
 Node-ID of the SDO server.
 
+uint16_t index
 Index of current object in Object Dictionary.
 
+uint8_t subIndex
 Subindex of current object in Object Dictionary.
 
size_t sizeInd
 Size of data, which will be transferred. More...
 
+size_t sizeTran
 Size of data which is actually transferred.
 
+volatile CO_SDO_state_t state
 Internal state of the SDO client.
 
+uint32_t SDOtimeoutTime_us
 Maximum timeout time between request and response in microseconds.
 
+uint32_t timeoutTimer
 Timeout timer for SDO communication.
 
+CO_fifo_t bufFifo
 CO_fifo_t object for data buffer (not pointer)
 
char buf [CO_CONFIG_SDO_CLI_BUFFER_SIZE+1]
 Data buffer of usable size CO_CONFIG_SDO_CLI_BUFFER_SIZE, used inside bufFifo. More...
 
volatile void * CANrxNew
 Indicates, if new SDO message received from CAN bus. More...
 
+uint8_t CANrxData [8]
 8 data bytes of the received message
 
+void(* pFunctSignal )(void *object)
 From CO_SDOclient_initCallbackPre() or NULL.
 
+void * functSignalObject
 From CO_SDOclient_initCallbackPre() or NULL.
 
+uint8_t toggle
 Toggle bit toggled in each segment in segmented transfer.
 
+uint32_t block_SDOtimeoutTime_us
 Timeout time for SDO sub-block upload, half of SDOtimeoutTime_us.
 
+uint32_t block_timeoutTimer
 Timeout timer for SDO sub-block upload.
 
+uint8_t block_seqno
 Sequence number of segment in block, 1..127.
 
+uint8_t block_blksize
 Number of segments per block, 1..127.
 
+uint8_t block_noData
 Number of bytes in last segment that do not contain data.
 
+bool_t block_crcEnabled
 Server CRC support in block transfer.
 
+uint8_t block_dataUploadLast [7]
 Last 7 bytes of data at block upload.
 
+uint16_t block_crc
 Calculated CRC checksum.
 
+

Detailed Description

+

SDO client object.

+

Field Documentation

+ +

◆ COB_IDClientToServer

+ +
+
+ + + + +
uint32_t CO_SDOclient_t::COB_IDClientToServer
+
+ +

Copy of CANopen COB_ID Client -> Server, meaning of the specific bits:

+
    +
  • Bit 0...10: 11-bit CAN identifier.
    • +
  • Bit 11..30: reserved, must be 0.
    • +
  • Bit 31: if 1, SDO client object is not used.
    • +
+ +
+
+ +

◆ sizeInd

+ +
+
+ + + + +
size_t CO_SDOclient_t::sizeInd
+
+ +

Size of data, which will be transferred.

+

It is optionally indicated by client in case of download or by server in case of upload.

+ +
+
+ +

◆ buf

+ +
+
+ + + + +
char CO_SDOclient_t::buf[CO_CONFIG_SDO_CLI_BUFFER_SIZE+1]
+
+ +

Data buffer of usable size CO_CONFIG_SDO_CLI_BUFFER_SIZE, used inside bufFifo.

+

Must be one byte larger for fifo usage.

+ +
+
+ +

◆ CANrxNew

+ +
+
+ + + + +
volatile void* CO_SDOclient_t::CANrxNew
+
+ +

Indicates, if new SDO message received from CAN bus.

+

It is not cleared, until received message is completely processed.

+ +
+
+
The documentation for this struct was generated from the following file: +
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/structCO__SDOclient__t.js b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__SDOclient__t.js new file mode 100755 index 0000000..ca9f1b5 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__SDOclient__t.js @@ -0,0 +1,37 @@ +var structCO__SDOclient__t = +[ + [ "OD", "structCO__SDOclient__t.html#a89334ad9cc4ac6751f4a8225f1645923", null ], + [ "nodeId", "structCO__SDOclient__t.html#a13de9457791eecf17051e405665bfa4a", null ], + [ "OD_IO", "structCO__SDOclient__t.html#a16fa659bc3098a3fbd60d0f6efde937a", null ], + [ "attribute", "structCO__SDOclient__t.html#a609088a2005febd6cd1561288cf1b7d1", null ], + [ "CANdevRx", "structCO__SDOclient__t.html#a3283cda81a28d3b193a6a6bef29fadab", null ], + [ "CANdevRxIdx", "structCO__SDOclient__t.html#afb8613dbcacfcefb970fabca4106eaeb", null ], + [ "CANdevTx", "structCO__SDOclient__t.html#ab4f18ed8c085ea333ca165e486f4ead3", null ], + [ "CANdevTxIdx", "structCO__SDOclient__t.html#a2a44e72381604a972f0e289495a41c37", null ], + [ "CANtxBuff", "structCO__SDOclient__t.html#a58efad796664487290b52b79cbdb3ae0", null ], + [ "COB_IDClientToServer", "structCO__SDOclient__t.html#a8752f1ad790cc61af17ff8b93b80c7ef", null ], + [ "COB_IDServerToClient", "structCO__SDOclient__t.html#a7b29a785240ca994c7dafd07d5a396e4", null ], + [ "nodeIDOfTheSDOServer", "structCO__SDOclient__t.html#a9e3c564cd4d027c5bd93bd25293ebacc", null ], + [ "index", "structCO__SDOclient__t.html#ad4fc4deee415a621f3558266ba447455", null ], + [ "subIndex", "structCO__SDOclient__t.html#a7f36981af65dc318dd69213c408b1b62", null ], + [ "sizeInd", "structCO__SDOclient__t.html#a1b9dc211b9b90dac825097757f985583", null ], + [ "sizeTran", "structCO__SDOclient__t.html#aa7a9ddc5dbf035644cf59857dcfa83db", null ], + [ "state", "structCO__SDOclient__t.html#a511a3981c4664be192b19714b95995c0", null ], + [ "SDOtimeoutTime_us", "structCO__SDOclient__t.html#ab2f8be2e90734e78f4e78bdabbb13488", null ], + [ "timeoutTimer", "structCO__SDOclient__t.html#a4e2ff087f13ff5a4754b9186c1a2929e", null ], + [ "bufFifo", "structCO__SDOclient__t.html#a634a9311de3569cf38841d2faaac20b3", null ], + [ "buf", "structCO__SDOclient__t.html#aa56b1f115aee473f5c264142053ed0ae", null ], + [ "CANrxNew", "structCO__SDOclient__t.html#a8020d62a62634a531a2efa43ef4534f5", null ], + [ "CANrxData", "structCO__SDOclient__t.html#a0bc8e66b7818bca04a4b49ae8210b387", null ], + [ "pFunctSignal", "structCO__SDOclient__t.html#ae7bbefdb0854addfa2ae45dcfc39d738", null ], + [ "functSignalObject", "structCO__SDOclient__t.html#a582292a9d5db2b3bf09bb089c788ecd0", null ], + [ "toggle", "structCO__SDOclient__t.html#a5962727f5c1830337146c7b2b389b391", null ], + [ "block_SDOtimeoutTime_us", "structCO__SDOclient__t.html#ae86079157706e7db12d9d4817172ba10", null ], + [ "block_timeoutTimer", "structCO__SDOclient__t.html#a8a667736bf5d22e7bb76f8b25d8b0268", null ], + [ "block_seqno", "structCO__SDOclient__t.html#a628780da4dccceab6ce79ad880989b26", null ], + [ "block_blksize", "structCO__SDOclient__t.html#a36b10791595b638309a01418d13a745f", null ], + [ "block_noData", "structCO__SDOclient__t.html#a26f9fcf95f47a4f7eeaefdf684e317a1", null ], + [ "block_crcEnabled", "structCO__SDOclient__t.html#a8690a5e7ee83fb7e0fa3a76cdec83f3a", null ], + [ "block_dataUploadLast", "structCO__SDOclient__t.html#ae9e678cb0e461851298658c7eee01334", null ], + [ "block_crc", "structCO__SDOclient__t.html#a51322a623ff85d36a8be60c0fe11430e", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/structCO__SDOserver__t.html b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__SDOserver__t.html new file mode 100755 index 0000000..561ed78 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__SDOserver__t.html @@ -0,0 +1,315 @@ + + + + + + + +CANopenNode: CO_SDOserver_t Struct Reference + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Data Fields
+
+
CO_SDOserver_t Struct Reference
CANopen_301 » SDO server
+
+
+ +

SDO server object. + More...

+ +

#include <CO_SDOserver.h>

+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

+Data Fields

+CO_CANmodule_t * CANdevTx
 From CO_SDOserver_init()
 
+CO_CANtx_t * CANtxBuff
 CAN transmit buffer inside CANdevTx for CAN tx message.
 
+const OD_t * OD
 From CO_SDOserver_init()
 
+uint8_t nodeId
 From CO_SDOserver_init()
 
+volatile CO_SDO_state_t state
 Internal state of the SDO server.
 
+OD_IO_t OD_IO
 Object dictionary interface for current object.
 
+uint16_t index
 Index of the current object in Object Dictionary.
 
+uint8_t subIndex
 Subindex of the current object in Object Dictionary.
 
+OD_attr_t attribute
 Attribute bit-field of the current OD sub-object, see OD_attributes_t.
 
volatile void * CANrxNew
 Indicates, if new SDO message received from CAN bus. More...
 
+uint8_t CANrxData [8]
 8 data bytes of the received message
 
+CO_CANmodule_t * CANdevRx
 From CO_SDOserver_init()
 
+uint16_t CANdevRxIdx
 From CO_SDOserver_init()
 
+uint16_t CANdevTxIdx
 From CO_SDOserver_init()
 
uint32_t COB_IDClientToServer
 Copy of CANopen COB_ID Client -> Server, meaning of the specific bits: More...
 
+uint32_t COB_IDServerToClient
 Copy of CANopen COB_ID Server -> Client, similar as above.
 
OD_size_t sizeInd
 Size of data, which will be transferred. More...
 
+OD_size_t sizeTran
 Size of data which is actually transferred.
 
+uint8_t toggle
 Toggle bit toggled in each segment in segmented transfer.
 
+bool_t finished
 If true, then: data transfer is finished (by download) or read from OD variable is finished (by upload)
 
+uint32_t SDOtimeoutTime_us
 Maximum timeout time between request and response in microseconds.
 
+uint32_t timeoutTimer
 Timeout timer for SDO communication.
 
+char buf [CO_CONFIG_SDO_SRV_BUFFER_SIZE+1]
 Interim data buffer for segmented or block transfer + byte for '\0'.
 
+OD_size_t bufOffsetWr
 Offset of next free data byte available for write in the buffer.
 
+OD_size_t bufOffsetRd
 Offset of first data available for read in the buffer.
 
+uint32_t block_SDOtimeoutTime_us
 Timeout time for SDO sub-block download, half of SDOtimeoutTime_us.
 
+uint32_t block_timeoutTimer
 Timeout timer for SDO sub-block download.
 
+uint8_t block_seqno
 Sequence number of segment in block, 1..127.
 
+uint8_t block_blksize
 Number of segments per block, 1..127.
 
+uint8_t block_noData
 Number of bytes in last segment that do not contain data.
 
+bool_t block_crcEnabled
 Client CRC support in block transfer.
 
+uint16_t block_crc
 Calculated CRC checksum.
 
+void(* pFunctSignalPre )(void *object)
 From CO_SDOserver_initCallbackPre() or NULL.
 
+void * functSignalObjectPre
 From CO_SDOserver_initCallbackPre() or NULL.
 
+

Detailed Description

+

SDO server object.

+

Field Documentation

+ +

◆ CANrxNew

+ +
+
+ + + + +
volatile void* CO_SDOserver_t::CANrxNew
+
+ +

Indicates, if new SDO message received from CAN bus.

+

It is not cleared, until received message is completely processed.

+ +
+
+ +

◆ COB_IDClientToServer

+ +
+
+ + + + +
uint32_t CO_SDOserver_t::COB_IDClientToServer
+
+ +

Copy of CANopen COB_ID Client -> Server, meaning of the specific bits:

+
    +
  • Bit 0...10: 11-bit CAN identifier.
    • +
  • Bit 11..30: reserved, must be 0.
    • +
  • Bit 31: if 1, SDO client object is not used.
    • +
+ +
+
+ +

◆ sizeInd

+ +
+
+ + + + +
OD_size_t CO_SDOserver_t::sizeInd
+
+ +

Size of data, which will be transferred.

+

It is optionally indicated by client in case of download or by server in case of upload.

+ +
+
+
The documentation for this struct was generated from the following file: +
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/structCO__SDOserver__t.js b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__SDOserver__t.js new file mode 100755 index 0000000..5b68ee5 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__SDOserver__t.js @@ -0,0 +1,37 @@ +var structCO__SDOserver__t = +[ + [ "CANdevTx", "structCO__SDOserver__t.html#add469f75cf702d340d069aadfe8ede14", null ], + [ "CANtxBuff", "structCO__SDOserver__t.html#a36ae3c719d96121b95b77f76d2cce723", null ], + [ "OD", "structCO__SDOserver__t.html#a8c104c076f57ffe039e0e41a1b12377e", null ], + [ "nodeId", "structCO__SDOserver__t.html#a38d0b70cb37d6be927208e3662105c6c", null ], + [ "state", "structCO__SDOserver__t.html#a0b457d35679acb51d11d21cd9f3660fd", null ], + [ "OD_IO", "structCO__SDOserver__t.html#a19e7e8afc09ced5629e3a1e04b83aa9f", null ], + [ "index", "structCO__SDOserver__t.html#ae0b1720a88d948fbf6d8e20b333abb17", null ], + [ "subIndex", "structCO__SDOserver__t.html#a5051d5aeaa97e5d40ccfb39461706f10", null ], + [ "attribute", "structCO__SDOserver__t.html#a3b2febaed4df4921626367a741008400", null ], + [ "CANrxNew", "structCO__SDOserver__t.html#a48e9b1237bc0dd46945762416d09c5bb", null ], + [ "CANrxData", "structCO__SDOserver__t.html#abfbff2e51c54be56f0ba090864c7e2f6", null ], + [ "CANdevRx", "structCO__SDOserver__t.html#a3e6255db1f66d742debd80ff2ce25012", null ], + [ "CANdevRxIdx", "structCO__SDOserver__t.html#a8c5ca24946f34e174fce129a2f5cb38a", null ], + [ "CANdevTxIdx", "structCO__SDOserver__t.html#acbb02ed7ddf534c8f0c41acd25478f47", null ], + [ "COB_IDClientToServer", "structCO__SDOserver__t.html#a7c9113f146613eec4b76888bd8d0f2fd", null ], + [ "COB_IDServerToClient", "structCO__SDOserver__t.html#a47cf7cde974f0ff8fd20f7a5363857cf", null ], + [ "sizeInd", "structCO__SDOserver__t.html#a95be5bcb7257f818a294c9b03b0d1386", null ], + [ "sizeTran", "structCO__SDOserver__t.html#a86eaba6efabaa335b769924edd80f1ae", null ], + [ "toggle", "structCO__SDOserver__t.html#a158797aa411b8d3b5c2079907d04ca0d", null ], + [ "finished", "structCO__SDOserver__t.html#a90997c7a119e97d2cb8b9bbc5fb5145f", null ], + [ "SDOtimeoutTime_us", "structCO__SDOserver__t.html#a09381f3b6885d0a3bc7d1fa96805eb5d", null ], + [ "timeoutTimer", "structCO__SDOserver__t.html#a69dfa2436ba0bb51c527eb03330b48a7", null ], + [ "buf", "structCO__SDOserver__t.html#ab834e69e77b72e1ee1d39d35e21e0df4", null ], + [ "bufOffsetWr", "structCO__SDOserver__t.html#a69f52a30b4e7dc0a6e55c88b3126f814", null ], + [ "bufOffsetRd", "structCO__SDOserver__t.html#a93557d1ee64582bc37c76f4d4680f7a5", null ], + [ "block_SDOtimeoutTime_us", "structCO__SDOserver__t.html#ae1e16955965dced9464abd0a6bf8c2b2", null ], + [ "block_timeoutTimer", "structCO__SDOserver__t.html#ad8748718c76f53347dde5248b1152626", null ], + [ "block_seqno", "structCO__SDOserver__t.html#aeefd77d5200958a30f63f7e1f4f474fa", null ], + [ "block_blksize", "structCO__SDOserver__t.html#a76e4c66e15027e78b8ba67bdd3089cc3", null ], + [ "block_noData", "structCO__SDOserver__t.html#a54bac23ac93450234a858c50f0516d05", null ], + [ "block_crcEnabled", "structCO__SDOserver__t.html#abdf7205835b75f0f6feef3bc89a86c17", null ], + [ "block_crc", "structCO__SDOserver__t.html#a33c2432ccea06da1d5c33c89c14caf63", null ], + [ "pFunctSignalPre", "structCO__SDOserver__t.html#ace57036f1dfd1ffe35f4b13e50fc4a41", null ], + [ "functSignalObjectPre", "structCO__SDOserver__t.html#adac867a846309166bca3f5dd0b550e65", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/structCO__SRDOCommPar__t.html b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__SRDOCommPar__t.html new file mode 100755 index 0000000..60bba2d --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__SRDOCommPar__t.html @@ -0,0 +1,270 @@ + + + + + + + +CANopenNode: CO_SRDOCommPar_t Struct Reference + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Data Fields
+
+
CO_SRDOCommPar_t Struct Reference
CANopen_304 » SRDO
+
+
+ +

SRDO communication parameter. + More...

+ +

#include <CO_SRDO.h>

+ + + + + + + + + + + + + + + + + + + + + + + +

+Data Fields

+uint8_t maxSubIndex
 Equal to 6.
 
uint8_t informationDirection
 Direction of the SRDO. More...
 
uint16_t safetyCycleTime
 Refresh-time / SCT. More...
 
uint8_t safetyRelatedValidationTime
 SRVT. More...
 
uint8_t transmissionType
 Transmission type. More...
 
uint32_t COB_ID1_normal
 Communication object identifier for message received. More...
 
uint32_t COB_ID2_inverted
 Communication object identifier for message received. More...
 
+

Detailed Description

+

SRDO communication parameter.

+

The same as record from Object dictionary (index 0x1301-0x1340).

+

Field Documentation

+ +

◆ informationDirection

+ +
+
+ + + + +
uint8_t CO_SRDOCommPar_t::informationDirection
+
+ +

Direction of the SRDO.

+

Values:

    +
  • 0: SRDO is invalid (deleted)
    • +
  • 1: SRDO is transmiting data
    • +
  • 2: SRDO is receive data
    • +
+ +
+
+ +

◆ safetyCycleTime

+ +
+
+ + + + +
uint16_t CO_SRDOCommPar_t::safetyCycleTime
+
+ +

Refresh-time / SCT.

+
    +
  • in tx mode (Refresh-time): transmission interval
    • +
  • in rx mode (SCT): receive timeout between two SRDO
    • +
+ +
+
+ +

◆ safetyRelatedValidationTime

+ +
+
+ + + + +
uint8_t CO_SRDOCommPar_t::safetyRelatedValidationTime
+
+ +

SRVT.

+
    +
  • in tx mode: unsed
    • +
  • in rx mode: receive timeout between first and second SRDO message
    • +
+ +
+
+ +

◆ transmissionType

+ +
+
+ + + + +
uint8_t CO_SRDOCommPar_t::transmissionType
+
+ +

Transmission type.

+

Values:

    +
  • 254: Manufacturer specific.
    • +
+ +
+
+ +

◆ COB_ID1_normal

+ +
+
+ + + + +
uint32_t CO_SRDOCommPar_t::COB_ID1_normal
+
+ +

Communication object identifier for message received.

+

Meaning of the specific bits:

    +
  • Bit 0-10: COB-ID for SRDO
    • +
  • Bit 11-31: set to 0 for 11 bit COB-ID.
    • +
+ +
+
+ +

◆ COB_ID2_inverted

+ +
+
+ + + + +
uint32_t CO_SRDOCommPar_t::COB_ID2_inverted
+
+ +

Communication object identifier for message received.

+

Meaning of the specific bits:

    +
  • Bit 0-10: COB-ID for SRDO
    • +
  • Bit 11-31: set to 0 for 11 bit COB-ID.
    • +
+ +
+
+
The documentation for this struct was generated from the following file: +
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/structCO__SRDOCommPar__t.js b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__SRDOCommPar__t.js new file mode 100755 index 0000000..cef747b --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__SRDOCommPar__t.js @@ -0,0 +1,10 @@ +var structCO__SRDOCommPar__t = +[ + [ "maxSubIndex", "structCO__SRDOCommPar__t.html#af156b61e6d278b014466e860f073cf05", null ], + [ "informationDirection", "structCO__SRDOCommPar__t.html#ac8f865699090f666910e66dabf53b339", null ], + [ "safetyCycleTime", "structCO__SRDOCommPar__t.html#a257a5534889060efc7477be7fc0adbad", null ], + [ "safetyRelatedValidationTime", "structCO__SRDOCommPar__t.html#a5c02007d0e4f1b1cfd8a07da93b954cb", null ], + [ "transmissionType", "structCO__SRDOCommPar__t.html#a8716aa43cf70db8af86c4125b14cd538", null ], + [ "COB_ID1_normal", "structCO__SRDOCommPar__t.html#a89d3762612ef971aecaae43ce94141cc", null ], + [ "COB_ID2_inverted", "structCO__SRDOCommPar__t.html#a574ade8ff753ae742f6a3b51b32a11fe", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/structCO__SRDOGuard__t.html b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__SRDOGuard__t.html new file mode 100755 index 0000000..9dbab24 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__SRDOGuard__t.html @@ -0,0 +1,147 @@ + + + + + + + +CANopenNode: CO_SRDOGuard_t Struct Reference + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Data Fields
+
+
CO_SRDOGuard_t Struct Reference
CANopen_304 » SRDO
+
+
+ +

Gurad Object for SRDO monitors: + More...

+ +

#include <CO_SRDO.h>

+ + + + + + + + + + + + + + +

+Data Fields

+CO_NMT_internalState_t * operatingState
 pointer to current operation state
 
+CO_NMT_internalState_t operatingStatePrev
 last operation state
 
+uint8_t * configurationValid
 pointer to the configuration valid flag in OD
 
+uint8_t checkCRC
 specifies whether a CRC check should be performed
 
+

Detailed Description

+

Gurad Object for SRDO monitors:

+
    +
  • access to CRC objects
    • +
  • access configuration valid flag
    • +
  • change in operation state
    • +
+
The documentation for this struct was generated from the following file: +
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/structCO__SRDOGuard__t.js b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__SRDOGuard__t.js new file mode 100755 index 0000000..41558b4 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__SRDOGuard__t.js @@ -0,0 +1,7 @@ +var structCO__SRDOGuard__t = +[ + [ "operatingState", "structCO__SRDOGuard__t.html#a5e80afb9ddd0debd94eaafa9c6f4ad36", null ], + [ "operatingStatePrev", "structCO__SRDOGuard__t.html#ac6f6a3b1465360e035656933470d6b4d", null ], + [ "configurationValid", "structCO__SRDOGuard__t.html#aa4c8b5e7d9d8fa54d91ad0797ea9b39b", null ], + [ "checkCRC", "structCO__SRDOGuard__t.html#ad8cd90a85e6e6d551fb04e8fa94feffd", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/structCO__SRDO__t.html b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__SRDO__t.html new file mode 100755 index 0000000..ca067c7 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__SRDO__t.html @@ -0,0 +1,239 @@ + + + + + + + +CANopenNode: CO_SRDO_t Struct Reference + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Data Fields
+
+
CO_SRDO_t Struct Reference
CANopen_304 » SRDO
+
+
+ +

SRDO object. + More...

+ +

#include <CO_SRDO.h>

+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

+Data Fields

+CO_EM_t * em
 From CO_SRDO_init()
 
+CO_SDO_t * SDO
 From CO_SRDO_init()
 
+CO_SRDOGuard_t * SRDOGuard
 From CO_SRDO_init()
 
+uint8_t * mapPointer [2][8]
 Pointers to 2*8 data objects, where SRDO will be copied.
 
uint8_t dataLength
 Data length of the received SRDO message. More...
 
+uint8_t nodeId
 From CO_SRDO_init()
 
+uint16_t defaultCOB_ID [2]
 From CO_SRDO_init()
 
+uint8_t valid
 0 - invalid, 1 - tx, 2 - rx
 
+const CO_SRDOCommPar_t * SRDOCommPar
 From CO_SRDO_init()
 
+const CO_SRDOMapPar_t * SRDOMapPar
 From CO_SRDO_init()
 
+const uint16_t * checksum
 From CO_SRDO_init()
 
+CO_CANmodule_t * CANdevRx
 From CO_SRDO_init()
 
+CO_CANmodule_t * CANdevTx
 From CO_SRDO_init()
 
+CO_CANtx_t * CANtxBuff [2]
 CAN transmit buffer inside CANdevTx.
 
+uint16_t CANdevRxIdx [2]
 From CO_SRDO_init()
 
+uint16_t CANdevTxIdx [2]
 From CO_SRDO_init()
 
+uint8_t toogle
 defines the current state
 
+uint32_t timer
 transmit timer and receive timeout
 
+volatile void * CANrxNew [2]
 Variable indicates, if new SRDO message received from CAN bus.
 
+uint8_t CANrxData [2][8]
 2*8 data bytes of the received message.
 
+void(* pFunctSignalSafe )(void *object)
 From CO_SRDO_initCallbackEnterSafeState() or NULL.
 
+void * functSignalObjectSafe
 From CO_SRDO_initCallbackEnterSafeState() or NULL.
 
+void(* pFunctSignalPre )(void *object)
 From CO_SRDO_initCallbackPre() or NULL.
 
+void * functSignalObjectPre
 From CO_SRDO_initCallbackPre() or NULL.
 
+

Detailed Description

+

SRDO object.

+

Field Documentation

+ +

◆ dataLength

+ +
+
+ + + + +
uint8_t CO_SRDO_t::dataLength
+
+ +

Data length of the received SRDO message.

+

Calculated from mapping

+ +
+
+
The documentation for this struct was generated from the following file: +
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/structCO__SRDO__t.js b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__SRDO__t.js new file mode 100755 index 0000000..4551ed1 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__SRDO__t.js @@ -0,0 +1,27 @@ +var structCO__SRDO__t = +[ + [ "em", "structCO__SRDO__t.html#a1779ab0170d7b604948a9396681212c8", null ], + [ "SDO", "structCO__SRDO__t.html#af8121f4d1f0879eba793afaee5c6c804", null ], + [ "SRDOGuard", "structCO__SRDO__t.html#ab96c524d0b0243527a6b2d3ee89de302", null ], + [ "mapPointer", "structCO__SRDO__t.html#a3e7570a1aeef89502702175eccb93500", null ], + [ "dataLength", "structCO__SRDO__t.html#ae994e87a71e85342f3a5b3c046d8a47f", null ], + [ "nodeId", "structCO__SRDO__t.html#ac4cc841f24894e41a5bbdbd386e62a0e", null ], + [ "defaultCOB_ID", "structCO__SRDO__t.html#a14da12ca5af61cd6cc6e442a0baa5c26", null ], + [ "valid", "structCO__SRDO__t.html#a6eef41749d7862ef2a29108f4f08185a", null ], + [ "SRDOCommPar", "structCO__SRDO__t.html#aba2d09de18da68fb01ab23696de452ac", null ], + [ "SRDOMapPar", "structCO__SRDO__t.html#a876184e5c0b2809fc4b412cc2accb43e", null ], + [ "checksum", "structCO__SRDO__t.html#adffd373a96a9570b0cbd4487c50d5359", null ], + [ "CANdevRx", "structCO__SRDO__t.html#ad5c0cea22d56cef74f42728107748b38", null ], + [ "CANdevTx", "structCO__SRDO__t.html#a2fe71edd01cb39629fe42753e84df1fc", null ], + [ "CANtxBuff", "structCO__SRDO__t.html#a693cc1ce40b882e85359291744d68b99", null ], + [ "CANdevRxIdx", "structCO__SRDO__t.html#aa1d8ca455950557652636bc8021928a4", null ], + [ "CANdevTxIdx", "structCO__SRDO__t.html#a0f56f71798c4781bf436d03a0f20938b", null ], + [ "toogle", "structCO__SRDO__t.html#a79114736807f3aa8ab19e6e88df93050", null ], + [ "timer", "structCO__SRDO__t.html#a1d3553daf7179b3389023ac78287dfbc", null ], + [ "CANrxNew", "structCO__SRDO__t.html#acbf5e7096cf72e911a4391ffb8dc939d", null ], + [ "CANrxData", "structCO__SRDO__t.html#ae6edba4f0446c10b971a90bde01c8a0b", null ], + [ "pFunctSignalSafe", "structCO__SRDO__t.html#ab000fd951d5b72615a3b3086335a6add", null ], + [ "functSignalObjectSafe", "structCO__SRDO__t.html#a037ba4d3ca1e33c6187219ba4766b2a8", null ], + [ "pFunctSignalPre", "structCO__SRDO__t.html#a5c9c5a746066607b6052c66bcf07190e", null ], + [ "functSignalObjectPre", "structCO__SRDO__t.html#a5807d03b19a7e2ae7c4250dd4e61f235", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/structCO__SYNC__t.html b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__SYNC__t.html new file mode 100755 index 0000000..0df3b96 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__SYNC__t.html @@ -0,0 +1,303 @@ + + + + + + + +CANopenNode: CO_SYNC_t Struct Reference + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Data Fields
+
+
CO_SYNC_t Struct Reference
CANopen_301 » SYNC
+
+
+ +

SYNC producer and consumer object. + More...

+ +

#include <CO_SYNC.h>

+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

+Data Fields

+CO_EM_t * em
 From CO_SYNC_init()
 
+CO_NMT_internalState_t * operatingState
 From CO_SYNC_init()
 
bool_t isProducer
 True, if device is SYNC producer. More...
 
uint16_t COB_ID
 COB_ID of SYNC message. More...
 
uint32_t periodTime
 Sync period time in [microseconds]. More...
 
uint32_t periodTimeoutTime
 Sync period timeout time in [microseconds]. More...
 
+uint8_t counterOverflowValue
 Value from Synchronous counter overflow value variable from Object dictionary (index 0x1019)
 
bool_t curentSyncTimeIsInsideWindow
 True, if current time is inside synchronous window. More...
 
+volatile void * CANrxNew
 Indicates, if new SYNC message received from CAN bus.
 
+bool_t CANrxToggle
 Variable toggles, if new SYNC message received from CAN bus.
 
+uint8_t counter
 Counter of the SYNC message if counterOverflowValue is different than zero.
 
uint32_t timer
 Timer for the SYNC message in [microseconds]. More...
 
+uint16_t receiveError
 Set to nonzero value, if SYNC with wrong data length is received from CAN.
 
+void(* pFunctSignalPre )(void *object)
 From CO_SYNC_initCallbackPre() or NULL.
 
+void * functSignalObjectPre
 From CO_SYNC_initCallbackPre() or NULL.
 
+CO_CANmodule_t * CANdevRx
 From CO_SYNC_init()
 
+uint16_t CANdevRxIdx
 From CO_SYNC_init()
 
+CO_CANmodule_t * CANdevTx
 From CO_SYNC_init()
 
+CO_CANtx_t * CANtxBuff
 CAN transmit buffer inside CANdevTx.
 
+uint16_t CANdevTxIdx
 From CO_SYNC_init()
 
+

Detailed Description

+

SYNC producer and consumer object.

+

Field Documentation

+ +

◆ isProducer

+ +
+
+ + + + +
bool_t CO_SYNC_t::isProducer
+
+ +

True, if device is SYNC producer.

+

Calculated from COB ID SYNC Message variable from Object dictionary (index 0x1005).

+ +
+
+ +

◆ COB_ID

+ +
+
+ + + + +
uint16_t CO_SYNC_t::COB_ID
+
+ +

COB_ID of SYNC message.

+

Calculated from COB ID SYNC Message variable from Object dictionary (index 0x1005).

+ +
+
+ +

◆ periodTime

+ +
+
+ + + + +
uint32_t CO_SYNC_t::periodTime
+
+ +

Sync period time in [microseconds].

+

Calculated from Communication cycle period variable from Object dictionary (index 0x1006).

+ +
+
+ +

◆ periodTimeoutTime

+ +
+
+ + + + +
uint32_t CO_SYNC_t::periodTimeoutTime
+
+ +

Sync period timeout time in [microseconds].

+

(periodTimeoutTime = periodTime * 1,5)

+ +
+
+ +

◆ curentSyncTimeIsInsideWindow

+ +
+
+ + + + +
bool_t CO_SYNC_t::curentSyncTimeIsInsideWindow
+
+ +

True, if current time is inside synchronous window.

+

In this case synchronous PDO may be sent.

+ +
+
+ +

◆ timer

+ +
+
+ + + + +
uint32_t CO_SYNC_t::timer
+
+ +

Timer for the SYNC message in [microseconds].

+

Set to zero after received or transmitted SYNC message

+ +
+
+
The documentation for this struct was generated from the following file: +
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/structCO__SYNC__t.js b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__SYNC__t.js new file mode 100755 index 0000000..b0d7fe3 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__SYNC__t.js @@ -0,0 +1,23 @@ +var structCO__SYNC__t = +[ + [ "em", "structCO__SYNC__t.html#acf987cc40eb5f005a92f10210353ea1f", null ], + [ "operatingState", "structCO__SYNC__t.html#a70fea8996ebfbe7d163bae11201dac6e", null ], + [ "isProducer", "structCO__SYNC__t.html#af37a656db91d31a8187e0350f472ea36", null ], + [ "COB_ID", "structCO__SYNC__t.html#af528cdc487bdaee3dfc0a3baa3f7c7cc", null ], + [ "periodTime", "structCO__SYNC__t.html#a15337541e9b3defa2dbae2df3dd01e72", null ], + [ "periodTimeoutTime", "structCO__SYNC__t.html#a9f612a7f9d691edeef11d8df9cf166a7", null ], + [ "counterOverflowValue", "structCO__SYNC__t.html#aee5eb3e245e54e509d2f2cc05cf4a664", null ], + [ "curentSyncTimeIsInsideWindow", "structCO__SYNC__t.html#adbc85ba8f5f4cbca0caa645216204a88", null ], + [ "CANrxNew", "structCO__SYNC__t.html#afa9120621a777413be033005eb43e771", null ], + [ "CANrxToggle", "structCO__SYNC__t.html#a25aeadeddcae8209b6b18034036a0aaf", null ], + [ "counter", "structCO__SYNC__t.html#a0977c2f09f69755e4b53df68bb1fee0b", null ], + [ "timer", "structCO__SYNC__t.html#a491f176a5fb70400647474555628bf6f", null ], + [ "receiveError", "structCO__SYNC__t.html#ae108ac75f1fb7c797393646cc6c494af", null ], + [ "pFunctSignalPre", "structCO__SYNC__t.html#ac56a3411d0cd312e51d905937c628b6f", null ], + [ "functSignalObjectPre", "structCO__SYNC__t.html#a6a219b1fd9d3382131878af1f016b83d", null ], + [ "CANdevRx", "structCO__SYNC__t.html#ae799680e430d4d6b05d7a3da216f06be", null ], + [ "CANdevRxIdx", "structCO__SYNC__t.html#a1a489d5fd447a8b5e16fb82d957d0667", null ], + [ "CANdevTx", "structCO__SYNC__t.html#a3b89fbc55cce155f500bbda463b61d8a", null ], + [ "CANtxBuff", "structCO__SYNC__t.html#a79d41eeaf724741d0ed281ab3b6a95ef", null ], + [ "CANdevTxIdx", "structCO__SYNC__t.html#ad07d96af0baa18907d4865ecf244d420", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/structCO__TIME__t.html b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__TIME__t.html new file mode 100755 index 0000000..1373de1 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__TIME__t.html @@ -0,0 +1,275 @@ + + + + + + + +CANopenNode: CO_TIME_t Struct Reference + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Data Fields
+
+
CO_TIME_t Struct Reference
CANopen_301 » TIME
+
+
+ +

TIME producer and consumer object. + More...

+ +

#include <CO_TIME.h>

+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

+Data Fields

+CO_EM_t * em
 From CO_TIME_init()
 
+CO_NMT_internalState_t * operatingState
 From CO_TIME_init()
 
bool_t isConsumer
 True, if device is TIME consumer. More...
 
bool_t isProducer
 True, if device is TIME producer. More...
 
+uint16_t COB_ID
 From CO_TIME_init()
 
uint32_t periodTime
 TIME period time in [milliseconds]. More...
 
uint32_t periodTimeoutTime
 TIME period timeout time in [milliseconds]. More...
 
+volatile void * CANrxNew
 Variable indicates, if new TIME message received from CAN bus.
 
uint32_t timer
 Timer for the TIME message in [microseconds]. More...
 
+uint16_t receiveError
 Set to nonzero value, if TIME with wrong data length is received from CAN.
 
+void(* pFunctSignalPre )(void *object)
 From CO_TIME_initCallbackPre() or NULL.
 
+void * functSignalObjectPre
 From CO_TIME_initCallbackPre() or NULL.
 
+CO_CANmodule_t * CANdevRx
 From CO_TIME_init()
 
+uint16_t CANdevRxIdx
 From CO_TIME_init()
 
+CO_CANmodule_t * CANdevTx
 From CO_TIME_init()
 
+uint16_t CANdevTxIdx
 From CO_TIME_init()
 
+CO_CANtx_t * TXbuff
 CAN transmit buffer.
 
+

Detailed Description

+

TIME producer and consumer object.

+

Field Documentation

+ +

◆ isConsumer

+ +
+
+ + + + +
bool_t CO_TIME_t::isConsumer
+
+ +

True, if device is TIME consumer.

+

Calculated from COB ID TIME Message variable from Object dictionary (index 0x1012).

+ +
+
+ +

◆ isProducer

+ +
+
+ + + + +
bool_t CO_TIME_t::isProducer
+
+ +

True, if device is TIME producer.

+

Calculated from COB ID TIME Message variable from Object dictionary (index 0x1012).

+ +
+
+ +

◆ periodTime

+ +
+
+ + + + +
uint32_t CO_TIME_t::periodTime
+
+ +

TIME period time in [milliseconds].

+

Set to TIME period to enable timeout detection

+ +
+
+ +

◆ periodTimeoutTime

+ +
+
+ + + + +
uint32_t CO_TIME_t::periodTimeoutTime
+
+ +

TIME period timeout time in [milliseconds].

+

(periodTimeoutTime = periodTime * 1,5)

+ +
+
+ +

◆ timer

+ +
+
+ + + + +
uint32_t CO_TIME_t::timer
+
+ +

Timer for the TIME message in [microseconds].

+

Set to zero after received or transmitted TIME message

+ +
+
+
The documentation for this struct was generated from the following file: +
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/structCO__TIME__t.js b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__TIME__t.js new file mode 100755 index 0000000..2840f46 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__TIME__t.js @@ -0,0 +1,20 @@ +var structCO__TIME__t = +[ + [ "em", "structCO__TIME__t.html#ad38fd490eece812c27048a0ffde75c9e", null ], + [ "operatingState", "structCO__TIME__t.html#a2afb9ad74a04332b644669c7fdb187ac", null ], + [ "isConsumer", "structCO__TIME__t.html#aeea1ecdf76e04f37c1d760eee72a1395", null ], + [ "isProducer", "structCO__TIME__t.html#a691d02cb2128a81f4af345165146b761", null ], + [ "COB_ID", "structCO__TIME__t.html#a6bcf872834e3a76869943108fd55ffc3", null ], + [ "periodTime", "structCO__TIME__t.html#a7c0090cb2e1e3af1fab308640c3d25a5", null ], + [ "periodTimeoutTime", "structCO__TIME__t.html#a9f2d24892e5b318f5eea5844f3b29a4a", null ], + [ "CANrxNew", "structCO__TIME__t.html#a893e358443b67c38eee33182c496f2a4", null ], + [ "timer", "structCO__TIME__t.html#a02fa485a1a5064ad6e0fc07ea3d5d092", null ], + [ "receiveError", "structCO__TIME__t.html#abbff3c5ce159992f4b45ec042849cbc0", null ], + [ "pFunctSignalPre", "structCO__TIME__t.html#aed7685145a3b3cfb80e8865aa3e8df6d", null ], + [ "functSignalObjectPre", "structCO__TIME__t.html#a91e41ac57bb634b53b6698b58efc4616", null ], + [ "CANdevRx", "structCO__TIME__t.html#ad21e78dd36756f1868056bf6638f5ccd", null ], + [ "CANdevRxIdx", "structCO__TIME__t.html#a4d9335af44c9b5e1224a6691df2f9594", null ], + [ "CANdevTx", "structCO__TIME__t.html#ac5fb7127ca474b6aa2afa207f01f8cae", null ], + [ "CANdevTxIdx", "structCO__TIME__t.html#a205dfcde7a830d6a15eda16911e328cd", null ], + [ "TXbuff", "structCO__TIME__t.html#aa658a7b219b1dad6b722ec41670da626", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/structCO__TPDOCommPar__t.html b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__TPDOCommPar__t.html new file mode 100755 index 0000000..0f3f203 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__TPDOCommPar__t.html @@ -0,0 +1,251 @@ + + + + + + + +CANopenNode: CO_TPDOCommPar_t Struct Reference + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Data Fields
+
+
CO_TPDOCommPar_t Struct Reference
CANopen_301 » PDO
+
+
+ +

TPDO communication parameter. + More...

+ +

#include <CO_PDO.h>

+ + + + + + + + + + + + + + + + + + + + + + + +

+Data Fields

+uint8_t maxSubIndex
 Equal to 6.
 
uint32_t COB_IDUsedByTPDO
 Communication object identifier for transmitting message. More...
 
uint8_t transmissionType
 Transmission type. More...
 
uint16_t inhibitTime
 Minimum time between transmissions of the PDO in 100micro seconds. More...
 
+uint8_t compatibilityEntry
 Not used.
 
uint16_t eventTimer
 Time between periodic transmissions of the PDO in milliseconds. More...
 
uint8_t SYNCStartValue
 Used with numbered SYNC messages. More...
 
+

Detailed Description

+

TPDO communication parameter.

+

The same as record from Object dictionary (index 0x1800+).

+

Field Documentation

+ +

◆ COB_IDUsedByTPDO

+ +
+
+ + + + +
uint32_t CO_TPDOCommPar_t::COB_IDUsedByTPDO
+
+ +

Communication object identifier for transmitting message.

+

Meaning of the specific bits:

    +
  • Bit 0-10: COB-ID for PDO, to change it bit 31 must be set.
    • +
  • Bit 11-29: set to 0 for 11 bit COB-ID.
    • +
  • Bit 30: If true, rtr are NOT allowed for PDO.
    • +
  • Bit 31: If true, node does NOT use the PDO.
    • +
+ +
+
+ +

◆ transmissionType

+ +
+
+ + + + +
uint8_t CO_TPDOCommPar_t::transmissionType
+
+ +

Transmission type.

+

Values:

    +
  • 0: Transmiting is synchronous, specification in device profile.
    • +
  • 1-240: Transmiting is synchronous after every N-th SYNC object.
    • +
  • 241-251: Not used.
    • +
  • 252-253: Transmited only on reception of Remote Transmission Request.
    • +
  • 254: Manufacturer specific.
    • +
  • 255: Asinchronous, specification in device profile.
    • +
+ +
+
+ +

◆ inhibitTime

+ +
+
+ + + + +
uint16_t CO_TPDOCommPar_t::inhibitTime
+
+ +

Minimum time between transmissions of the PDO in 100micro seconds.

+

Zero disables functionality.

+ +
+
+ +

◆ eventTimer

+ +
+
+ + + + +
uint16_t CO_TPDOCommPar_t::eventTimer
+
+ +

Time between periodic transmissions of the PDO in milliseconds.

+

Zero disables functionality.

+ +
+
+ +

◆ SYNCStartValue

+ +
+
+ + + + +
uint8_t CO_TPDOCommPar_t::SYNCStartValue
+
+ +

Used with numbered SYNC messages.

+

Values:

    +
  • 0: Counter of the SYNC message shall not be processed.
    • +
  • 1-240: The SYNC message with the counter value equal to this value shall be regarded as the first received SYNC message.
    • +
+ +
+
+
The documentation for this struct was generated from the following file: +
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/structCO__TPDOCommPar__t.js b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__TPDOCommPar__t.js new file mode 100755 index 0000000..08009c0 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__TPDOCommPar__t.js @@ -0,0 +1,10 @@ +var structCO__TPDOCommPar__t = +[ + [ "maxSubIndex", "structCO__TPDOCommPar__t.html#a74a8681177fabbb55ebf5be843f12fc5", null ], + [ "COB_IDUsedByTPDO", "structCO__TPDOCommPar__t.html#a65ee2e80b1078e84479ab749012f94cc", null ], + [ "transmissionType", "structCO__TPDOCommPar__t.html#a328398227ff1f167649d54453e44df97", null ], + [ "inhibitTime", "structCO__TPDOCommPar__t.html#ad53403c65582d166898546e329ea9587", null ], + [ "compatibilityEntry", "structCO__TPDOCommPar__t.html#acba30527976f508b43b3348846f2e657", null ], + [ "eventTimer", "structCO__TPDOCommPar__t.html#ab01f44570dca08c910c17b14fc664414", null ], + [ "SYNCStartValue", "structCO__TPDOCommPar__t.html#a597bd93f097550c3d869307e733cd198", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/structCO__TPDOMapPar__t.html b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__TPDOMapPar__t.html new file mode 100755 index 0000000..dc4b8c8 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__TPDOMapPar__t.html @@ -0,0 +1,200 @@ + + + + + + + +CANopenNode: CO_TPDOMapPar_t Struct Reference + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Data Fields
+
+
CO_TPDOMapPar_t Struct Reference
CANopen_301 » PDO
+
+
+ +

TPDO mapping parameter. + More...

+ +

#include <CO_PDO.h>

+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

+Data Fields

uint8_t numberOfMappedObjects
 Actual number of mapped objects from 0 to 8. More...
 
uint32_t mappedObject1
 Location and size of the mapped object. More...
 
+uint32_t mappedObject2
 Same.
 
+uint32_t mappedObject3
 Same.
 
+uint32_t mappedObject4
 Same.
 
+uint32_t mappedObject5
 Same.
 
+uint32_t mappedObject6
 Same.
 
+uint32_t mappedObject7
 Same.
 
+uint32_t mappedObject8
 Same.
 
+

Detailed Description

+

TPDO mapping parameter.

+

The same as record from Object dictionary (index 0x1A00+).

+

Field Documentation

+ +

◆ numberOfMappedObjects

+ +
+
+ + + + +
uint8_t CO_TPDOMapPar_t::numberOfMappedObjects
+
+ +

Actual number of mapped objects from 0 to 8.

+

To change mapped object, this value must be 0.

+ +
+
+ +

◆ mappedObject1

+ +
+
+ + + + +
uint32_t CO_TPDOMapPar_t::mappedObject1
+
+ +

Location and size of the mapped object.

+

Bit meanings 0xIIIISSLL:

    +
  • Bit 0-7: Data Length in bits.
    • +
  • Bit 8-15: Subindex from object distionary.
    • +
  • Bit 16-31: Index from object distionary.
    • +
+ +
+
+
The documentation for this struct was generated from the following file: +
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/structCO__TPDOMapPar__t.js b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__TPDOMapPar__t.js new file mode 100755 index 0000000..840b115 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__TPDOMapPar__t.js @@ -0,0 +1,12 @@ +var structCO__TPDOMapPar__t = +[ + [ "numberOfMappedObjects", "structCO__TPDOMapPar__t.html#a10718cf84dc6a975509d327efb8403de", null ], + [ "mappedObject1", "structCO__TPDOMapPar__t.html#a6869485bc0705188069c37d6b6ee1694", null ], + [ "mappedObject2", "structCO__TPDOMapPar__t.html#a496d610062c09a1667e7541659339eea", null ], + [ "mappedObject3", "structCO__TPDOMapPar__t.html#a8e79bb51d865cae1bab20621db898ddb", null ], + [ "mappedObject4", "structCO__TPDOMapPar__t.html#aa8a046a95b3ac151f5d8d8cb4415851e", null ], + [ "mappedObject5", "structCO__TPDOMapPar__t.html#a0fbdd6c39635c8288771867bdc061d6f", null ], + [ "mappedObject6", "structCO__TPDOMapPar__t.html#ad8f8f0e0629b1c0467599ea8b138e3eb", null ], + [ "mappedObject7", "structCO__TPDOMapPar__t.html#a1fb19d7423d2fd74bc8575aff1657552", null ], + [ "mappedObject8", "structCO__TPDOMapPar__t.html#aa482b092dd475bac21193c671dabdb49", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/structCO__TPDO__t.html b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__TPDO__t.html new file mode 100755 index 0000000..7f1742b --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__TPDO__t.html @@ -0,0 +1,255 @@ + + + + + + + +CANopenNode: CO_TPDO_t Struct Reference + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Data Fields
+
+
CO_TPDO_t Struct Reference
CANopen_301 » PDO
+
+
+ +

TPDO object. + More...

+ +

#include <CO_PDO.h>

+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

+Data Fields

+CO_EM_t * em
 From CO_TPDO_init()
 
+CO_SDO_t * SDO
 From CO_TPDO_init()
 
+const CO_TPDOCommPar_t * TPDOCommPar
 From CO_TPDO_init()
 
+const CO_TPDOMapPar_t * TPDOMapPar
 From CO_TPDO_init()
 
+CO_NMT_internalState_t * operatingState
 From CO_TPDO_init()
 
+uint8_t nodeId
 From CO_TPDO_init()
 
+uint16_t defaultCOB_ID
 From CO_TPDO_init()
 
+uint8_t restrictionFlags
 From CO_TPDO_init()
 
+bool_t valid
 True, if PDO is enabled and valid.
 
uint8_t dataLength
 Data length of the transmitting PDO message. More...
 
uint8_t sendRequest
 If application set this flag, PDO will be later sent by function CO_TPDO_process(). More...
 
+uint8_t * mapPointer [8]
 Pointers to 8 data objects, where PDO will be copied.
 
+uint32_t inhibitTimer
 Inhibit timer used for inhibit PDO sending translated to microseconds.
 
+uint32_t eventTimer
 Event timer used for PDO sending translated to microseconds.
 
uint8_t sendIfCOSFlags
 Each flag bit is connected with one mapPointer. More...
 
+uint8_t syncCounter
 SYNC counter used for PDO sending.
 
+CO_SYNC_t * SYNC
 From CO_TPDO_init()
 
+CO_CANmodule_t * CANdevTx
 From CO_TPDO_init()
 
+CO_CANtx_t * CANtxBuff
 CAN transmit buffer inside CANdev.
 
+uint16_t CANdevTxIdx
 From CO_TPDO_init()
 
+

Detailed Description

+

TPDO object.

+

Field Documentation

+ +

◆ dataLength

+ +
+
+ + + + +
uint8_t CO_TPDO_t::dataLength
+
+ +

Data length of the transmitting PDO message.

+

Calculated from mapping

+ +
+
+ +

◆ sendRequest

+ +
+
+ + + + +
uint8_t CO_TPDO_t::sendRequest
+
+ +

If application set this flag, PDO will be later sent by function CO_TPDO_process().

+

Depends on transmission type.

+ +
+
+ +

◆ sendIfCOSFlags

+ +
+
+ + + + +
uint8_t CO_TPDO_t::sendIfCOSFlags
+
+ +

Each flag bit is connected with one mapPointer.

+

If flag bit is true, CO_TPDO_process() functiuon will send PDO if Change of State is detected on value pointed by that mapPointer

+ +
+
+
The documentation for this struct was generated from the following file: +
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/structCO__TPDO__t.js b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__TPDO__t.js new file mode 100755 index 0000000..52a7b56 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__TPDO__t.js @@ -0,0 +1,23 @@ +var structCO__TPDO__t = +[ + [ "em", "structCO__TPDO__t.html#ac7ce3386549e212300bb85bfc5e88f2e", null ], + [ "SDO", "structCO__TPDO__t.html#a52c1e56f282549b0949721049bcc7e73", null ], + [ "TPDOCommPar", "structCO__TPDO__t.html#ab9bd2bf1a76f1f0e253dd4c4a941ba67", null ], + [ "TPDOMapPar", "structCO__TPDO__t.html#a6f6202c2b866f552c512a3513c27be8b", null ], + [ "operatingState", "structCO__TPDO__t.html#a7ba73f70490869a38ff561aa6c32489f", null ], + [ "nodeId", "structCO__TPDO__t.html#a4ef8ced15f6fffb56a0ec4aeb48d4551", null ], + [ "defaultCOB_ID", "structCO__TPDO__t.html#a95e95dc4668b41de0b8eb8504e83c944", null ], + [ "restrictionFlags", "structCO__TPDO__t.html#aa0d1d4b71933c7bac438d97ca280fe56", null ], + [ "valid", "structCO__TPDO__t.html#a201c8a0726347a747f6b315915c797fb", null ], + [ "dataLength", "structCO__TPDO__t.html#a223e60deb77d4ef8a78da37e4d9cdf85", null ], + [ "sendRequest", "structCO__TPDO__t.html#afc375e06e5931cb8bae7886f2f5f0f7a", null ], + [ "mapPointer", "structCO__TPDO__t.html#acb6fa2c4037b1e41afdf6195cd6e93ec", null ], + [ "inhibitTimer", "structCO__TPDO__t.html#a9277687ef658353801435638c8aa2bee", null ], + [ "eventTimer", "structCO__TPDO__t.html#ae71e875f41d8f14f02e757dbc3b2d0c5", null ], + [ "sendIfCOSFlags", "structCO__TPDO__t.html#a0f5dbb51df08261c466b079c08119a04", null ], + [ "syncCounter", "structCO__TPDO__t.html#a5bf0fa473c5eb92e3b56fc1f17222976", null ], + [ "SYNC", "structCO__TPDO__t.html#ae6c014bdb855d57d4c53bc702734b51f", null ], + [ "CANdevTx", "structCO__TPDO__t.html#a42d9798fca6a122bd06391b7d23ec254", null ], + [ "CANtxBuff", "structCO__TPDO__t.html#a2a2a10d57e1eeab4e0717caa302a6605", null ], + [ "CANdevTxIdx", "structCO__TPDO__t.html#ab401d61f30b73d530df3b8d42015407d", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/structCO__config__t.html b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__config__t.html new file mode 100755 index 0000000..512385b --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__config__t.html @@ -0,0 +1,328 @@ + + + + + + + +CANopenNode: CO_config_t Struct Reference + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Data Fields
+
+
CO_config_t Struct Reference
CANopen
+
+
+ +

CANopen configuration, used with CO_new() + More...

+ +

#include <CANopen.h>

+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

+Data Fields

uint8_t CNT_NMT
 Number of NMT objects, 0 or 1: NMT slave (CANrx) + Heartbeat producer (CANtx) + optional NMT master (CANtx), configurable by CO_CONFIG_NMT. More...
 
+const OD_entry_t * ENTRY_H1017
 OD entry for CO_NMT_init()
 
+uint8_t CNT_HB_CONS
 Number of Heartbeat consumer objects, 0 or 1: object uses from 1 to 127 internal consumers (CANrx), as specified by CO_CONFIG_HB_CONS_SIZE.
 
+const OD_entry_t * ENTRY_H1016
 OD entry for CO_HBconsumer_init()
 
uint8_t CNT_EM
 Number of Emergency objects, 0 or 1: optional producer (CANtx) + optional consumer (CANrx), configurable by CO_CONFIG_EM. More...
 
+const OD_entry_t * ENTRY_H1001
 OD entry for CO_EM_init()
 
+const OD_entry_t * ENTRY_H1014
 OD entry for CO_EM_init()
 
+const OD_entry_t * ENTRY_H1015
 OD entry for CO_EM_init()
 
+const OD_entry_t * ENTRY_H1003
 OD entry for CO_EM_init()
 
uint8_t CNT_SDO_SRV
 Number of SDO server objects, from 0 to 128 (CANrx + CANtx). More...
 
+const OD_entry_t * ENTRY_H1200
 OD entry for CO_SDOserver_init()
 
+uint8_t CNT_SDO_CLI
 Number of SDO client objects, from 0 to 128 (CANrx + CANtx).
 
+const OD_entry_t * ENTRY_H1280
 OD entry for CO_SDOclient_init()
 
+uint8_t CNT_TIME
 Number of TIME objects, 0 or 1: consumer (CANrx) + optional producer (CANtx), configurable by CO_CONFIG_TIME.
 
+const OD_entry_t * ENTRY_H1012
 OD entry for CO_TIME_init()
 
+uint8_t CNT_SYNC
 Number of SYNC objects, 0 or 1: consumer (CANrx) + optional producer (CANtx), configurable by CO_CONFIG_SYNC.
 
+const OD_entry_t * ENTRY_H1005
 OD entry for CO_SYNC_init()
 
+const OD_entry_t * ENTRY_H1006
 OD entry for CO_SYNC_init()
 
+const OD_entry_t * ENTRY_H1007
 OD entry for CO_SYNC_init()
 
+const OD_entry_t * ENTRY_H1019
 OD entry for CO_SYNC_init()
 
+uint16_t CNT_RPDO
 Number of RPDO objects, from 0 to 512 consumers (CANrx)
 
+const OD_entry_t * ENTRY_H1400
 OD entry for CO_RPDO_init()
 
+const OD_entry_t * ENTRY_H1600
 OD entry for CO_RPDO_init()
 
+uint16_t CNT_TPDO
 Number of TPDO objects, from 0 to 512 producers (CANtx)
 
+const OD_entry_t * ENTRY_H1800
 OD entry for CO_TPDO_init()
 
+const OD_entry_t * ENTRY_H1A00
 OD entry for CO_TPDO_init()
 
+uint8_t CNT_LEDS
 Number of LEDs objects, 0 or 1.
 
+uint8_t CNT_GFC
 Number of GFC objects, 0 or 1 (CANrx + CANtx).
 
+const OD_entry_t * ENTRY_H1300
 OD entry for CO_GFC_init()
 
+uint8_t CNT_SRDO
 Number of SRDO objects, from 0 to 64 (2*CANrx + 2*CANtx).
 
+const OD_entry_t * ENTRY_H1301
 OD entry for CO_SRDO_init()
 
+const OD_entry_t * ENTRY_H1381
 OD entry for CO_SRDO_init()
 
+const OD_entry_t * ENTRY_H13FE
 OD entry for CO_SRDOGuard_init()
 
+const OD_entry_t * ENTRY_H13FF
 OD entry for CO_SRDOGuard_init()
 
+uint8_t CNT_LSS_SLV
 Number of LSSslave objects, 0 or 1 (CANrx + CANtx).
 
+uint8_t CNT_LSS_MST
 Number of LSSmaster objects, 0 or 1 (CANrx + CANtx).
 
+uint8_t CNT_GTWA
 Number of gateway ascii objects, 0 or 1.
 
+uint16_t CNT_TRACE
 Number of trace objects, 0 or more.
 
+

Detailed Description

+

CANopen configuration, used with CO_new()

+

This structure is used only, if CO_MULTIPLE_OD is enabled. Otherwise parameters are retrieved from default "OD.h" file.

+

Field Documentation

+ +

◆ CNT_NMT

+ +
+
+ + + + +
uint8_t CO_config_t::CNT_NMT
+
+ +

Number of NMT objects, 0 or 1: NMT slave (CANrx) + Heartbeat producer (CANtx) + optional NMT master (CANtx), configurable by CO_CONFIG_NMT.

+

Start indexes inside CANrx and CANtx are always 0. There must be one NMT object in the device.

+ +
+
+ +

◆ CNT_EM

+ +
+
+ + + + +
uint8_t CO_config_t::CNT_EM
+
+ +

Number of Emergency objects, 0 or 1: optional producer (CANtx) + optional consumer (CANrx), configurable by CO_CONFIG_EM.

+

There must be one Emergency object in the device.

+ +
+
+ +

◆ CNT_SDO_SRV

+ +
+
+ + + + +
uint8_t CO_config_t::CNT_SDO_SRV
+
+ +

Number of SDO server objects, from 0 to 128 (CANrx + CANtx).

+

There must be at least one SDO server object in the device.

+ +
+
+
The documentation for this struct was generated from the following file: +
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/structCO__config__t.js b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__config__t.js new file mode 100755 index 0000000..886549c --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__config__t.js @@ -0,0 +1,41 @@ +var structCO__config__t = +[ + [ "CNT_NMT", "structCO__config__t.html#aeef814580eb5ece5156e63bfc1b490c9", null ], + [ "ENTRY_H1017", "structCO__config__t.html#ad17f77b55de3d90ec983fcac49eeab6d", null ], + [ "CNT_HB_CONS", "structCO__config__t.html#a0031fc8f80e95f8480c918dbf8289671", null ], + [ "ENTRY_H1016", "structCO__config__t.html#a0af4cf7d0355861e7f60206d794d6a91", null ], + [ "CNT_EM", "structCO__config__t.html#a515e08f68835f71a6f145be8f27b510a", null ], + [ "ENTRY_H1001", "structCO__config__t.html#a6a6c19e816fb76882e85b2c07c0d8f42", null ], + [ "ENTRY_H1014", "structCO__config__t.html#a4827d94f6152cc12d86bd21312ae86e4", null ], + [ "ENTRY_H1015", "structCO__config__t.html#a141f21b4d1730206d1af823fd6b13a01", null ], + [ "ENTRY_H1003", "structCO__config__t.html#a7e320b309714b7f623c2006d45fee929", null ], + [ "CNT_SDO_SRV", "structCO__config__t.html#aac83faf556924515cc2aa8003753ab58", null ], + [ "ENTRY_H1200", "structCO__config__t.html#a05ab8adad4517850e31e5542895f7cc5", null ], + [ "CNT_SDO_CLI", "structCO__config__t.html#a2fc9606643a7fb4d4237f01812d3a6d2", null ], + [ "ENTRY_H1280", "structCO__config__t.html#a9f871c4ec753e8414cdb47eb78c3e09d", null ], + [ "CNT_TIME", "structCO__config__t.html#ada2a43384a544fa2f235de24a874b1e6", null ], + [ "ENTRY_H1012", "structCO__config__t.html#abac6be7122af1a8a4f9ae3ff5912d490", null ], + [ "CNT_SYNC", "structCO__config__t.html#af6dbc7d9f31b4cb050e23af8cff3df33", null ], + [ "ENTRY_H1005", "structCO__config__t.html#a02a4992f47db72816753ff2aa1964318", null ], + [ "ENTRY_H1006", "structCO__config__t.html#aa9befdebbaaa22f309b9a1b115612071", null ], + [ "ENTRY_H1007", "structCO__config__t.html#ad51ab63ca8b5836bf0dd8543f02db544", null ], + [ "ENTRY_H1019", "structCO__config__t.html#a468c82f6a0afd757a6b78ce33532c0d2", null ], + [ "CNT_RPDO", "structCO__config__t.html#a7a75302ac077462b67d767b0a11c9f56", null ], + [ "ENTRY_H1400", "structCO__config__t.html#a5e0984d93183493d587523888465eaa7", null ], + [ "ENTRY_H1600", "structCO__config__t.html#ab2ddc9943fd8c89f3b852d7ac9508d21", null ], + [ "CNT_TPDO", "structCO__config__t.html#a1d830617f50e3235de35a403a1513693", null ], + [ "ENTRY_H1800", "structCO__config__t.html#a29b98c08edfe0fba2e46c7af7a9edf6f", null ], + [ "ENTRY_H1A00", "structCO__config__t.html#a43fd6a448c91910c603f2c7756610432", null ], + [ "CNT_LEDS", "structCO__config__t.html#a642809cc681792bca855906241d891cc", null ], + [ "CNT_GFC", "structCO__config__t.html#ae282bab830810b61c0b0c3223654d674", null ], + [ "ENTRY_H1300", "structCO__config__t.html#a91c9f3ddb67231854af39224a9597e20", null ], + [ "CNT_SRDO", "structCO__config__t.html#ae58a44be57069709af3f6acbd10953e1", null ], + [ "ENTRY_H1301", "structCO__config__t.html#a87076cb1f9282d9720c21d395ff4e541", null ], + [ "ENTRY_H1381", "structCO__config__t.html#a7b3172b29ce8751adcab9e4351dcc31e", null ], + [ "ENTRY_H13FE", "structCO__config__t.html#a03fcaca5a8e0e71b86086908cae75f3d", null ], + [ "ENTRY_H13FF", "structCO__config__t.html#aa4cb9674209b83e7f0e48b01feaa04ef", null ], + [ "CNT_LSS_SLV", "structCO__config__t.html#a00a7a598b946ed13e3af7696e9f92dcc", null ], + [ "CNT_LSS_MST", "structCO__config__t.html#ac253cae7039090a6c04bc1e385f3ec21", null ], + [ "CNT_GTWA", "structCO__config__t.html#a64725014ecce342843f14ffc4b57e2a2", null ], + [ "CNT_TRACE", "structCO__config__t.html#aaafb8ffff236b51cd6d4ab16426d460f", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/structCO__epoll__gtw__t.html b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__epoll__gtw__t.html new file mode 100755 index 0000000..b21734a --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__epoll__gtw__t.html @@ -0,0 +1,158 @@ + + + + + + + +CANopenNode: CO_epoll_gtw_t Struct Reference + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Data Fields
+
+
CO_epoll_gtw_t Struct Reference
socketCAN » Epoll interface
+
+
+ +

Object for gateway. + More...

+ +

#include <CO_epoll_interface.h>

+ + + + + + + + + + + + + + + + + + + + + + + + + + +

+Data Fields

+int epoll_fd
 Epoll file descriptor, from CO_epoll_createGtw()
 
+int32_t commandInterface
 Command interface type or tcp port number, see CO_commandInterface_t.
 
+uint32_t socketTimeout_us
 Socket timeout in microseconds.
 
+uint32_t socketTimeoutTmr_us
 Socket timeout timer in microseconds.
 
+char * localSocketPath
 Path in case of local socket.
 
+int gtwa_fdSocket
 Gateway socket file descriptor.
 
+int gtwa_fd
 Gateway io stream file descriptor.
 
+bool_t freshCommand
 Indication of fresh command.
 
+

Detailed Description

+

Object for gateway.

+
The documentation for this struct was generated from the following file: +
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/structCO__epoll__gtw__t.js b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__epoll__gtw__t.js new file mode 100755 index 0000000..707779f --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__epoll__gtw__t.js @@ -0,0 +1,11 @@ +var structCO__epoll__gtw__t = +[ + [ "epoll_fd", "structCO__epoll__gtw__t.html#a7804e63fe1f7f5f6df68b32d8c25da2d", null ], + [ "commandInterface", "structCO__epoll__gtw__t.html#a96544bbc848d4627f3147496cc40d9f4", null ], + [ "socketTimeout_us", "structCO__epoll__gtw__t.html#a876c2b8ae6e9585155c5cf035c876c91", null ], + [ "socketTimeoutTmr_us", "structCO__epoll__gtw__t.html#a94ff0ff20f321b749202d9f3aa92a66d", null ], + [ "localSocketPath", "structCO__epoll__gtw__t.html#a5f11979c2bc301d6b8cc7ddce8d45688", null ], + [ "gtwa_fdSocket", "structCO__epoll__gtw__t.html#a418eccf136c9f8e081d61f98289e0759", null ], + [ "gtwa_fd", "structCO__epoll__gtw__t.html#a79b9c968b1f44ad5a4c55c36174d6898", null ], + [ "freshCommand", "structCO__epoll__gtw__t.html#ae529dc4279406811f0b97593816e863c", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/structCO__epoll__t.html b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__epoll__t.html new file mode 100755 index 0000000..0ae374e --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__epoll__t.html @@ -0,0 +1,170 @@ + + + + + + + +CANopenNode: CO_epoll_t Struct Reference + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Data Fields
+
+
CO_epoll_t Struct Reference
socketCAN » Epoll interface
+
+
+ +

Object for epoll, timer and event API. + More...

+ +

#include <CO_epoll_interface.h>

+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

+Data Fields

+int epoll_fd
 Epoll file descriptor.
 
+int event_fd
 Notification event file descriptor.
 
+int timer_fd
 Interval timer file descriptor.
 
+uint32_t timerInterval_us
 Interval of the timer in microseconds, from CO_epoll_create()
 
+uint32_t timeDifference_us
 Time difference since last CO_epoll_wait() execution in microseconds.
 
+uint32_t timerNext_us
 Timer value in microseconds, which can be changed by application and can shorten time of next CO_epoll_wait() execution.
 
+bool_t timerEvent
 True,if timer event is inside CO_epoll_wait()
 
+uint64_t previousTime_us
 time value from the last process call in microseconds
 
+struct itimerspec tm
 Structure for timerfd.
 
+struct epoll_event ev
 Structure for epoll_wait.
 
+bool_t epoll_new
 true, if new epoll event is necessary to process
 
+

Detailed Description

+

Object for epoll, timer and event API.

+
The documentation for this struct was generated from the following file: +
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/structCO__epoll__t.js b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__epoll__t.js new file mode 100755 index 0000000..e839272 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__epoll__t.js @@ -0,0 +1,14 @@ +var structCO__epoll__t = +[ + [ "epoll_fd", "structCO__epoll__t.html#a6e4aeb640b634a49a290aa9b4bc3b517", null ], + [ "event_fd", "structCO__epoll__t.html#af0e85776b360999b0830c943eb4505eb", null ], + [ "timer_fd", "structCO__epoll__t.html#aef151da1fe1a293f34f4f546c24bb0f7", null ], + [ "timerInterval_us", "structCO__epoll__t.html#a0595fb48d72a8592e6649e3de7b3477c", null ], + [ "timeDifference_us", "structCO__epoll__t.html#a46f38181dc8483ce83af566ee3f8aff3", null ], + [ "timerNext_us", "structCO__epoll__t.html#a0cb262435f594f9d7ea42eb976294262", null ], + [ "timerEvent", "structCO__epoll__t.html#a9fa1c50ecb111049eda7e45132d3dfd1", null ], + [ "previousTime_us", "structCO__epoll__t.html#aa047c5d68bb87515a1089cf5c75d847f", null ], + [ "tm", "structCO__epoll__t.html#a759aff5ae2eb019aa2ce8117f9ce9836", null ], + [ "ev", "structCO__epoll__t.html#a5ace69fd90506eae4f3f538fcb160c01", null ], + [ "epoll_new", "structCO__epoll__t.html#ac4113f19873fc3217e1e997a28f5bdd4", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/structCO__fifo__t.html b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__fifo__t.html new file mode 100755 index 0000000..e6d4f89 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__fifo__t.html @@ -0,0 +1,171 @@ + + + + + + + +CANopenNode: CO_fifo_t Struct Reference + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Data Fields
+
+
CO_fifo_t Struct Reference
CANopen_301 » FIFO circular buffer
+
+
+ +

Fifo object. + More...

+ +

#include <CO_fifo.h>

+ + + + + + + + + + + + + + + + + + + + + + + +

+Data Fields

char * buf
 Buffer of size bufSize. More...
 
+size_t bufSize
 Initialized by CO_fifo_init()
 
+size_t writePtr
 Location in the buffer, which will be next written.
 
+size_t readPtr
 Location in the buffer, which will be next read.
 
+size_t altReadPtr
 Location in the buffer, which will be next read.
 
+bool_t started
 helper variable, set to false in CO_fifo_reset(), used in some functions.
 
+uint32_t aux
 auxiliary variable, used in some functions.
 
+

Detailed Description

+

Fifo object.

+

Field Documentation

+ +

◆ buf

+ +
+
+ + + + +
char* CO_fifo_t::buf
+
+ +

Buffer of size bufSize.

+

Initialized by CO_fifo_init()

+ +
+
+
The documentation for this struct was generated from the following file: +
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/structCO__fifo__t.js b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__fifo__t.js new file mode 100755 index 0000000..1c1453a --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__fifo__t.js @@ -0,0 +1,10 @@ +var structCO__fifo__t = +[ + [ "buf", "structCO__fifo__t.html#aa4a8bae66b107809c099cd1d2de5c966", null ], + [ "bufSize", "structCO__fifo__t.html#a35d9f2fcb8d2ef00794cb305d8344e61", null ], + [ "writePtr", "structCO__fifo__t.html#a540fbc52344d1205de11625cd81f351d", null ], + [ "readPtr", "structCO__fifo__t.html#a4b56b7d217aa2b02eaf1b3592c5c26e6", null ], + [ "altReadPtr", "structCO__fifo__t.html#a4f8eadd2e9b966ce21274cbbceb3adbe", null ], + [ "started", "structCO__fifo__t.html#ab754d03636ff0ef17950d4167429a77f", null ], + [ "aux", "structCO__fifo__t.html#aa255bcb00601a8f4225c97ad6cd854a7", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/structCO__t.html b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__t.html new file mode 100755 index 0000000..0c068f8 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__t.html @@ -0,0 +1,318 @@ + + + + + + + +CANopenNode: CO_t Struct Reference + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Data Fields
+
+
CO_t Struct Reference
CANopen
+
+
+ +

CANopen object - collection of all CANopenNode objects. + More...

+ +

#include <CANopen.h>

+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

+Data Fields

+bool_t nodeIdUnconfigured
 True in un-configured LSS slave.
 
+CO_config_t * config
 Remember the configuration parameters.
 
+CO_CANmodule_t * CANmodule
 One CAN module object, initialised by CO_CANmodule_init()
 
+CO_CANrx_t * CANrx
 CAN receive message objects.
 
+CO_CANtx_t * CANtx
 CAN transmit message objects.
 
+uint16_t CNT_ALL_RX_MSGS
 Number of all CAN receive message objects.
 
+uint16_t CNT_ALL_TX_MSGS
 Number of all CAN transmit message objects.
 
+CO_NMT_t * NMT
 NMT and heartbeat object, initialised by CO_NMT_init()
 
+uint16_t RX_IDX_NMT_SLV
 Start index in CANrx.
 
+uint16_t TX_IDX_NMT_MST
 Start index in CANtx.
 
+uint16_t TX_IDX_HB_PROD
 Start index in CANtx.
 
+CO_HBconsumer_t * HBcons
 Heartbeat consumer object, initialised by CO_HBconsumer_init()
 
+uint16_t RX_IDX_HB_CONS
 Start index in CANrx.
 
+CO_EM_t * em
 Emergency object, initialised by CO_EM_init()
 
+uint16_t RX_IDX_EM_CONS
 Start index in CANrx.
 
+uint16_t TX_IDX_EM_PROD
 Start index in CANtx.
 
+CO_SDOserver_t * SDOserver
 SDO server objects, initialised by CO_SDOserver_init()
 
+uint16_t RX_IDX_SDO_SRV
 Start index in CANrx.
 
+uint16_t TX_IDX_SDO_SRV
 Start index in CANtx.
 
+CO_SDOclient_t * SDOclient
 SDO client objects, initialised by CO_SDOclient_init()
 
+uint16_t RX_IDX_SDO_CLI
 Start index in CANrx.
 
+uint16_t TX_IDX_SDO_CLI
 Start index in CANtx.
 
+CO_TIME_t * TIME
 TIME object, initialised by CO_TIME_init()
 
+uint16_t RX_IDX_TIME
 Start index in CANrx.
 
+uint16_t TX_IDX_TIME
 Start index in CANtx.
 
+CO_SYNC_t * SYNC
 SYNC object, initialised by CO_SYNC_init()
 
+uint16_t RX_IDX_SYNC
 Start index in CANrx.
 
+uint16_t TX_IDX_SYNC
 Start index in CANtx.
 
+CO_RPDO_t * RPDO
 RPDO objects, initialised by CO_RPDO_init()
 
+uint16_t RX_IDX_RPDO
 Start index in CANrx.
 
+CO_TPDO_t * TPDO
 TPDO objects, initialised by CO_TPDO_init()
 
+uint16_t TX_IDX_TPDO
 Start index in CANtx.
 
+CO_LEDs_t * LEDs
 LEDs object, initialised by CO_LEDs_init()
 
+CO_GFC_t * GFC
 GFC object, initialised by CO_GFC_init()
 
+uint16_t RX_IDX_GFC
 Start index in CANrx.
 
+uint16_t TX_IDX_GFC
 Start index in CANtx.
 
+CO_SRDOGuard_t * SRDOGuard
 SRDO object, initialised by CO_SRDOGuard_init(), single SRDOGuard object is included inside all SRDO objects.
 
+CO_SRDO_t * SRDO
 SRDO objects, initialised by CO_SRDO_init()
 
+uint16_t RX_IDX_SRDO
 Start index in CANrx.
 
+uint16_t TX_IDX_SRDO
 Start index in CANtx.
 
+CO_LSSslave_t * LSSslave
 LSS slave object, initialised by CO_LSSslave_init().
 
+uint16_t RX_IDX_LSS_SLV
 Start index in CANrx.
 
+uint16_t TX_IDX_LSS_SLV
 Start index in CANtx.
 
+CO_LSSmaster_t * LSSmaster
 LSS master object, initialised by CO_LSSmaster_init().
 
+uint16_t RX_IDX_LSS_MST
 Start index in CANrx.
 
+uint16_t TX_IDX_LSS_MST
 Start index in CANtx.
 
+CO_GTWA_t * gtwa
 Gateway-ascii object, initialised by CO_GTWA_init().
 
+CO_trace_t * trace
 Trace object, initialised by CO_trace_init().
 
+

Detailed Description

+

CANopen object - collection of all CANopenNode objects.

+
The documentation for this struct was generated from the following file: +
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/structCO__t.js b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__t.js new file mode 100755 index 0000000..25fb2cf --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__t.js @@ -0,0 +1,51 @@ +var structCO__t = +[ + [ "nodeIdUnconfigured", "structCO__t.html#a139e71d4b3c9f2c072df13e6ac0dbac4", null ], + [ "config", "structCO__t.html#a522a9a56f1256dac8cd305313b3913f1", null ], + [ "CANmodule", "structCO__t.html#a65bea9a6028917db3b0b3a95a60aea18", null ], + [ "CANrx", "structCO__t.html#a9665dd795f4c5d26910eb1c5b06503e5", null ], + [ "CANtx", "structCO__t.html#a990b6bd828bc23b455887b5a72f79dca", null ], + [ "CNT_ALL_RX_MSGS", "structCO__t.html#a5b9bea68bd5c63af52819c3c4cdb7592", null ], + [ "CNT_ALL_TX_MSGS", "structCO__t.html#a9e9e7548a182ed9d4aad12e6a41be495", null ], + [ "NMT", "structCO__t.html#a6fddf777eec75e0cc8fc510a5c4ec8e5", null ], + [ "RX_IDX_NMT_SLV", "structCO__t.html#af693786fa6c0923d4c934d09a00820d3", null ], + [ "TX_IDX_NMT_MST", "structCO__t.html#a77bf70da8db7ecfe78f3720bfca12101", null ], + [ "TX_IDX_HB_PROD", "structCO__t.html#ad2718c137526676467853885d286fc74", null ], + [ "HBcons", "structCO__t.html#a5eea4e2b8390e1f0ec531248e229cd72", null ], + [ "RX_IDX_HB_CONS", "structCO__t.html#a788f815f4d31a4aa11d171114173573e", null ], + [ "em", "structCO__t.html#a6e4c80d975a5a2207530b72bdbf530b7", null ], + [ "RX_IDX_EM_CONS", "structCO__t.html#a47df99673e8da6e48c6c17cbf204c16d", null ], + [ "TX_IDX_EM_PROD", "structCO__t.html#a3d2f250c5c3bc972ce45418d22e79caa", null ], + [ "SDOserver", "structCO__t.html#aa9fe002954a6b902e594fc47a9f1917b", null ], + [ "RX_IDX_SDO_SRV", "structCO__t.html#a8e63b9c217cf4cd7fc8fbc0900bdb01c", null ], + [ "TX_IDX_SDO_SRV", "structCO__t.html#a9287e4d62f23f4222fc90c4a6ded2ef0", null ], + [ "SDOclient", "structCO__t.html#ae9548e1a85b750b039dcc12bd43bd972", null ], + [ "RX_IDX_SDO_CLI", "structCO__t.html#a85a2007d87b4fb3608859e251cf8653e", null ], + [ "TX_IDX_SDO_CLI", "structCO__t.html#adc86b044f08602394cbb4346b41a9a45", null ], + [ "TIME", "structCO__t.html#a80052c25c47be2460e02cf88ff91bbb7", null ], + [ "RX_IDX_TIME", "structCO__t.html#a039a63c4b10f91814775f9d1cef43aad", null ], + [ "TX_IDX_TIME", "structCO__t.html#ad5cbb43e75d99c5479efb3eddd36ec3d", null ], + [ "SYNC", "structCO__t.html#a9c704cab995029fd216076523c70d283", null ], + [ "RX_IDX_SYNC", "structCO__t.html#a72a378c1e806978f8399bf3f3af7c002", null ], + [ "TX_IDX_SYNC", "structCO__t.html#a2eb3962f37ce7a4b51d8779f96db58bc", null ], + [ "RPDO", "structCO__t.html#a332fa4512032d94a20af69b5a103e017", null ], + [ "RX_IDX_RPDO", "structCO__t.html#a7a01e7792c600f5dc1eb2e7676e1e01d", null ], + [ "TPDO", "structCO__t.html#ab5027ca1447bf64e5e93935c1e5466c2", null ], + [ "TX_IDX_TPDO", "structCO__t.html#a9451781c713680d8bab86cda2a1ca570", null ], + [ "LEDs", "structCO__t.html#a4837a9caa7daa219a8ecff5c64bc08ba", null ], + [ "GFC", "structCO__t.html#a9c6e7b29436b05c8b659502c6fae2a6a", null ], + [ "RX_IDX_GFC", "structCO__t.html#ae689666dc05116ce0694d5ea505a2e79", null ], + [ "TX_IDX_GFC", "structCO__t.html#a709e86d5401484bb7d0c5f17e824636d", null ], + [ "SRDOGuard", "structCO__t.html#a007c96db5b54e91145571feb06e9e683", null ], + [ "SRDO", "structCO__t.html#a2a65c843c89da67fc049aa285e03243c", null ], + [ "RX_IDX_SRDO", "structCO__t.html#a8fec2730008c19f6fbc6192937776739", null ], + [ "TX_IDX_SRDO", "structCO__t.html#a3dbc89e76d1627f2b360ddaefd7a7b51", null ], + [ "LSSslave", "structCO__t.html#a0c6ecabcb18ed2bd9eb881f9156482b6", null ], + [ "RX_IDX_LSS_SLV", "structCO__t.html#a2c230c531fc2a2a09ad33edef582041e", null ], + [ "TX_IDX_LSS_SLV", "structCO__t.html#a7d1159efd4a31d0701c497d25343073d", null ], + [ "LSSmaster", "structCO__t.html#af586261baf229fe624fb72b712208c86", null ], + [ "RX_IDX_LSS_MST", "structCO__t.html#aa0c0c6b387e406c67d3b988a744b3b36", null ], + [ "TX_IDX_LSS_MST", "structCO__t.html#a38e102b879d1dd04f69bacac56266fdf", null ], + [ "gtwa", "structCO__t.html#afa0e937046492a26af9bb5e03c3aab94", null ], + [ "trace", "structCO__t.html#a4f7a05e49ea89178cb61e14c4c4575f1", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/structCO__trace__dataType__t.html b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__trace__dataType__t.html new file mode 100755 index 0000000..b854f09 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__trace__dataType__t.html @@ -0,0 +1,142 @@ + + + + + + + +CANopenNode: CO_trace_dataType_t Struct Reference + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Data Fields
+
+
CO_trace_dataType_t Struct Reference
CANopen_extra » Trace
+
+
+ +

structure for reading variables and printing points for specific data type. + More...

+ +

#include <CO_trace.h>

+ + + + + + + + + + + + + + +

+Data Fields

+int32_t(* pGetValue )(void *OD_variable)
 Function pointer for getting the value from OD variable.
 
+uint32_t(* printPointStart )(char *s, uint32_t size, uint32_t timeStamp, int32_t value)
 Function pointer for printing the start point to trace.plot.
 
+uint32_t(* printPoint )(char *s, uint32_t size, uint32_t timeStamp, int32_t value)
 Function pointer for printing the point to trace.plot.
 
+uint32_t(* printPointEnd )(char *s, uint32_t size, uint32_t timeStamp, int32_t value)
 Function pointer for printing the end point to trace.plot.
 
+

Detailed Description

+

structure for reading variables and printing points for specific data type.

+
The documentation for this struct was generated from the following file: +
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/structCO__trace__dataType__t.js b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__trace__dataType__t.js new file mode 100755 index 0000000..cad69fe --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__trace__dataType__t.js @@ -0,0 +1,7 @@ +var structCO__trace__dataType__t = +[ + [ "pGetValue", "structCO__trace__dataType__t.html#a8ac5cd02c39b591354149f8c5d6357f7", null ], + [ "printPointStart", "structCO__trace__dataType__t.html#a751d9b94aba8f663de5b5f7c7f074893", null ], + [ "printPoint", "structCO__trace__dataType__t.html#aef2341fd443f3aa33a11db1b429b7dc1", null ], + [ "printPointEnd", "structCO__trace__dataType__t.html#a48ec81e33885a9114f2ea0be237a0059", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/structCO__trace__t.html b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__trace__t.html new file mode 100755 index 0000000..79f98fa --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__trace__t.html @@ -0,0 +1,219 @@ + + + + + + + +CANopenNode: CO_trace_t Struct Reference + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Data Fields
+
+
CO_trace_t Struct Reference
CANopen_extra » Trace
+
+
+ +

Trace object. + More...

+ +

#include <CO_trace.h>

+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

+Data Fields

+bool_t enabled
 True, if trace is enabled.
 
+CO_SDO_t * SDO
 From CO_trace_init().
 
+uint32_t * timeBuffer
 From CO_trace_init().
 
+int32_t * valueBuffer
 From CO_trace_init().
 
+uint32_t bufferSize
 From CO_trace_init().
 
+volatile uint32_t writePtr
 Location in buffer, which will be next written.
 
+volatile uint32_t readPtr
 Location in buffer, which will be next read.
 
uint32_t lastTimeStamp
 Last time stamp. More...
 
+void * OD_variable
 Pointer to variable, which is monitored.
 
+const CO_trace_dataType_t * dt
 Data type specific function pointers.
 
+int32_t valuePrev
 Previous value of value.
 
+uint32_t * map
 From CO_trace_init().
 
+uint8_t * format
 From CO_trace_init().
 
+int32_t * value
 From CO_trace_init().
 
+int32_t * minValue
 From CO_trace_init().
 
+int32_t * maxValue
 From CO_trace_init().
 
+uint32_t * triggerTime
 From CO_trace_init().
 
+uint8_t * trigger
 From CO_trace_init().
 
+int32_t * threshold
 From CO_trace_init().
 
+

Detailed Description

+

Trace object.

+

Field Documentation

+ +

◆ lastTimeStamp

+ +
+
+ + + + +
uint32_t CO_trace_t::lastTimeStamp
+
+ +

Last time stamp.

+

If zero, then last point contains last timestamp.

+ +
+
+
The documentation for this struct was generated from the following file: +
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/structCO__trace__t.js b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__trace__t.js new file mode 100755 index 0000000..e982375 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/structCO__trace__t.js @@ -0,0 +1,22 @@ +var structCO__trace__t = +[ + [ "enabled", "structCO__trace__t.html#aefa5e9934aaac1f00d0a1866100dae50", null ], + [ "SDO", "structCO__trace__t.html#a33616f410feb93ddb6a2ac238027996b", null ], + [ "timeBuffer", "structCO__trace__t.html#a83806299bd57a23f5946f81cb05ab41d", null ], + [ "valueBuffer", "structCO__trace__t.html#abc2f00a5f99453c77dd8515f3526e428", null ], + [ "bufferSize", "structCO__trace__t.html#a3dfe996be2bef106f50de085a7f5ca9f", null ], + [ "writePtr", "structCO__trace__t.html#ae3a556a180e38e7247b39b84de609b5d", null ], + [ "readPtr", "structCO__trace__t.html#af439eee35bf45f6ec14a32270577ff85", null ], + [ "lastTimeStamp", "structCO__trace__t.html#af3ffa370f4b25a3c3fd27f9ce75379c0", null ], + [ "OD_variable", "structCO__trace__t.html#a2d71398a641edb4fe095aabb6e81f834", null ], + [ "dt", "structCO__trace__t.html#a31e42b2511450377294475fcf0189d89", null ], + [ "valuePrev", "structCO__trace__t.html#abe713136228c54327c1540b99a1a2fd1", null ], + [ "map", "structCO__trace__t.html#ad6e329507a29f46b41ea6bef210b6502", null ], + [ "format", "structCO__trace__t.html#a3ea91e63f2c2f6c34b3ee1ae0f84e939", null ], + [ "value", "structCO__trace__t.html#a24fa467aeeb1581c6a3272bd397a2f10", null ], + [ "minValue", "structCO__trace__t.html#a0f1b287dae5b6a30b43f481d2bee41ab", null ], + [ "maxValue", "structCO__trace__t.html#a7b41b99eb65d49fe522c25d08127f81d", null ], + [ "triggerTime", "structCO__trace__t.html#a1b7176f0ad48679449a666c34fe45a3f", null ], + [ "trigger", "structCO__trace__t.html#a879f362c9d4a88de9e48d23c4d0c770c", null ], + [ "threshold", "structCO__trace__t.html#a6400336e1207dcfa5fa8ef2a908fda08", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/structOD__IO__t.html b/Devices/Libraries/Systems/CANopenSocket/docs/structOD__IO__t.html new file mode 100755 index 0000000..47071ad --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/structOD__IO__t.html @@ -0,0 +1,200 @@ + + + + + + + +CANopenNode: OD_IO_t Struct Reference + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Data Fields
+
+
OD_IO_t Struct Reference
CANopen_301 » OD interface
+
+
+ +

Structure for input / output on the OD variable. + More...

+ +

#include <CO_ODinterface.h>

+ + + + + + + + + + + +

+Data Fields

+OD_stream_t stream
 Object Dictionary stream object, passed to read or write.
 
OD_size_t(* read )(OD_stream_t *stream, uint8_t subIndex, void *buf, OD_size_t count, ODR_t *returnCode)
 Function pointer for reading value from specified variable from Object Dictionary. More...
 
OD_size_t(* write )(OD_stream_t *stream, uint8_t subIndex, const void *buf, OD_size_t count, ODR_t *returnCode)
 Function pointer for writing value into specified variable inside Object Dictionary. More...
 
+

Detailed Description

+

Structure for input / output on the OD variable.

+

It is initialized with OD_getSub() function. Access principle to OD variable is via read/write functions operating on stream, similar as standard read/write.

+

Field Documentation

+ +

◆ read

+ +
+
+ + + + +
OD_size_t(* OD_IO_t::read) (OD_stream_t *stream, uint8_t subIndex, void *buf, OD_size_t count, ODR_t *returnCode)
+
+ +

Function pointer for reading value from specified variable from Object Dictionary.

+

If OD variable is larger than buf, then this function must be called several times. After completed successful read function returns 'ODR_OK'. If read is partial, it returns 'ODR_PARTIAL'. In case of errors function returns code similar to SDO abort code.

+

Read can be restarted with OD_rwRestart() function.

+

At the moment, when Object Dictionary is initialized, every variable has assigned the same "read" function. This default function simply copies data from Object Dictionary variable. Application can bind its own "read" function for specific object. In that case application is able to calculate data for reading from own internal state at the moment of "read" function call. For this functionality OD object must have IO extension enabled. OD object must also be initialized with OD_extensionIO_init() function call.

+

"read" function must always copy all own data to buf, except if "buf" is not large enough. ("*returnCode" must not return 'ODR_PARTIAL', if there is still space in "buf".)

+
Parameters
+ + + + + + +
streamObject Dictionary stream object.
subIndexObject Dictionary subIndex of the accessed element.
bufPointer to external buffer, where to data will be copied.
countSize of the external buffer in bytes.
[out]returnCodeReturn value from ODR_t.
+
+
+
Returns
Number of bytes successfully read.
+ +
+
+ +

◆ write

+ +
+
+ + + + +
OD_size_t(* OD_IO_t::write) (OD_stream_t *stream, uint8_t subIndex, const void *buf, OD_size_t count, ODR_t *returnCode)
+
+ +

Function pointer for writing value into specified variable inside Object Dictionary.

+

If OD variable is larger than buf, then this function must be called several times. After completed successful write function returns 'ODR_OK'. If write is partial, it returns 'ODR_PARTIAL'. In case of errors function returns code similar to SDO abort code.

+

Write can be restarted with OD_rwRestart() function.

+

At the moment, when Object Dictionary is initialised, every variable has assigned the same "write" function, which simply copies data to Object Dictionary variable. Application can bind its own "write" function, similar as it can bind "read" function.

+

"write" function must always copy all available data from buf. If OD variable expect more data, then "*returnCode" must return 'ODR_PARTIAL'.

+
Parameters
+ + + + + + +
streamObject Dictionary stream object.
subIndexObject Dictionary subIndex of the accessed element.
bufPointer to external buffer, from where data will be copied.
countSize of the external buffer in bytes.
[out]returnCodeReturn value from ODR_t.
+
+
+
Returns
Number of bytes successfully written, must be equal to count on success or zero on error.
+ +
+
+
The documentation for this struct was generated from the following file: +
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/structOD__IO__t.js b/Devices/Libraries/Systems/CANopenSocket/docs/structOD__IO__t.js new file mode 100755 index 0000000..ad7c29b --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/structOD__IO__t.js @@ -0,0 +1,6 @@ +var structOD__IO__t = +[ + [ "stream", "structOD__IO__t.html#a7f31cae6c859c2a2eaf2610b65fd6626", null ], + [ "read", "structOD__IO__t.html#ab0ed3186d15d20f80f877a5f087fdebc", null ], + [ "write", "structOD__IO__t.html#aa296d8e76d99af5c395971602a453b78", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/structOD__entry__t.html b/Devices/Libraries/Systems/CANopenSocket/docs/structOD__entry__t.html new file mode 100755 index 0000000..05acdbf --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/structOD__entry__t.html @@ -0,0 +1,143 @@ + + + + + + + +CANopenNode: OD_entry_t Struct Reference + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Data Fields
+
+
OD_entry_t Struct Reference
CANopen_301 » OD interface
+
+
+ +

Object Dictionary entry for one OD object. + More...

+ +

#include <CO_ODinterface.h>

+ + + + + + + + + + + + + + +

+Data Fields

+uint16_t index
 Object Dictionary index.
 
+uint8_t subEntriesCount
 Maximum sub-index in the OD object.
 
+uint8_t odObjectType
 Type of the odObject, indicated by OD_objectTypes_t enumerator.
 
+const void * odObject
 OD object of type indicated by odObjectType, from which OD_getSub() fetches the information.
 
+

Detailed Description

+

Object Dictionary entry for one OD object.

+

OD entries are collected inside OD_t as array (list). Each OD entry contains basic information about OD object (index and subEntriesCount) and access function together with a pointer to other details of the OD object.

+
The documentation for this struct was generated from the following file: +
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/structOD__entry__t.js b/Devices/Libraries/Systems/CANopenSocket/docs/structOD__entry__t.js new file mode 100755 index 0000000..7ab04be --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/structOD__entry__t.js @@ -0,0 +1,7 @@ +var structOD__entry__t = +[ + [ "index", "structOD__entry__t.html#ac27d9d9ac18705e84d64f5226a6e352c", null ], + [ "subEntriesCount", "structOD__entry__t.html#a2f8698a482c2994602a24d39d924acc6", null ], + [ "odObjectType", "structOD__entry__t.html#a86a662107a24527872d92cdb13cad29b", null ], + [ "odObject", "structOD__entry__t.html#a8667e5896b5b001270103e59c92bb181", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/structOD__extensionIO__t.html b/Devices/Libraries/Systems/CANopenSocket/docs/structOD__extensionIO__t.html new file mode 100755 index 0000000..7ace5e3 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/structOD__extensionIO__t.html @@ -0,0 +1,138 @@ + + + + + + + +CANopenNode: OD_extensionIO_t Struct Reference + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Data Fields
+
+
OD_extensionIO_t Struct Reference
CANopen_301 » OD interface » OD definition objects
+
+
+ +

Object pointed by OD_obj_extended_t contains application specified parameters for extended OD object. + More...

+ +

#include <CO_ODinterface.h>

+ + + + + + + + + + + +

+Data Fields

+void * object
 Object on which read and write will operate.
 
+OD_size_t(* read )(OD_stream_t *stream, uint8_t subIndex, void *buf, OD_size_t count, ODR_t *returnCode)
 Application specified function pointer, see OD_IO_t.
 
+OD_size_t(* write )(OD_stream_t *stream, uint8_t subIndex, const void *buf, OD_size_t count, ODR_t *returnCode)
 Application specified function pointer, see OD_IO_t.
 
+

Detailed Description

+

Object pointed by OD_obj_extended_t contains application specified parameters for extended OD object.

+
The documentation for this struct was generated from the following file: +
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/structOD__extensionIO__t.js b/Devices/Libraries/Systems/CANopenSocket/docs/structOD__extensionIO__t.js new file mode 100755 index 0000000..c6c3c82 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/structOD__extensionIO__t.js @@ -0,0 +1,6 @@ +var structOD__extensionIO__t = +[ + [ "object", "structOD__extensionIO__t.html#a929dace9c5bf5f1e3090f3fbff40f9b8", null ], + [ "read", "structOD__extensionIO__t.html#af4c210e173adb94e297ad26eacb2b678", null ], + [ "write", "structOD__extensionIO__t.html#a06573b7740c3c991352734bba25f0fd4", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/structOD__obj__array__t.html b/Devices/Libraries/Systems/CANopenSocket/docs/structOD__obj__array__t.html new file mode 100755 index 0000000..15055b3 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/structOD__obj__array__t.html @@ -0,0 +1,150 @@ + + + + + + + +CANopenNode: OD_obj_array_t Struct Reference + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Data Fields
+
+
OD_obj_array_t Struct Reference
CANopen_301 » OD interface » OD definition objects
+
+
+ +

Object for OD array of variables, used for "ARRAY" type OD objects. + More...

+ +

#include <CO_ODinterface.h>

+ + + + + + + + + + + + + + + + + + + + +

+Data Fields

+uint8_t * data0
 Pointer to data for sub-index 0.
 
+void * data
 Pointer to array of data.
 
+OD_attr_t attribute0
 Attribute bitfield for sub-index 0, see OD_attributes_t.
 
+OD_attr_t attribute
 Attribute bitfield for array elements.
 
+OD_size_t dataElementLength
 Data length of array elements in bytes.
 
+OD_size_t dataElementSizeof
 Sizeof one array element in bytes.
 
+

Detailed Description

+

Object for OD array of variables, used for "ARRAY" type OD objects.

+
The documentation for this struct was generated from the following file: +
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/structOD__obj__array__t.js b/Devices/Libraries/Systems/CANopenSocket/docs/structOD__obj__array__t.js new file mode 100755 index 0000000..87d2ee4 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/structOD__obj__array__t.js @@ -0,0 +1,9 @@ +var structOD__obj__array__t = +[ + [ "data0", "structOD__obj__array__t.html#a9a4ca22f014061ca9d926ab43036bc1f", null ], + [ "data", "structOD__obj__array__t.html#a009bcf700d8e27c93e886ad7ff7fb2eb", null ], + [ "attribute0", "structOD__obj__array__t.html#a1cb4802d94112e5bd2f1b0db5e3e5d99", null ], + [ "attribute", "structOD__obj__array__t.html#a6af20a410bcd0c8c9f619c4a564b962a", null ], + [ "dataElementLength", "structOD__obj__array__t.html#a985fb68eba74f9e8fb76a4c5d85e96e9", null ], + [ "dataElementSizeof", "structOD__obj__array__t.html#ad310fa351ebb2a44f66451dd12675bf9", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/structOD__obj__extended__t.html b/Devices/Libraries/Systems/CANopenSocket/docs/structOD__obj__extended__t.html new file mode 100755 index 0000000..93fa164 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/structOD__obj__extended__t.html @@ -0,0 +1,138 @@ + + + + + + + +CANopenNode: OD_obj_extended_t Struct Reference + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Data Fields
+
+
OD_obj_extended_t Struct Reference
CANopen_301 » OD interface » OD definition objects
+
+
+ +

Object for extended type of OD variable, configurable by OD_extensionIO_init() function. + More...

+ +

#include <CO_ODinterface.h>

+ + + + + + + + + + + +

+Data Fields

+OD_extensionIO_t * extIO
 Pointer to application specified IO extension, may be NULL.
 
+OD_flagsPDO_t * flagsPDO
 Pointer to PDO flags bit-field, see OD_subEntry_t, may be NULL.
 
+const void * odObjectOriginal
 Pointer to original odObject, see OD_entry_t.
 
+

Detailed Description

+

Object for extended type of OD variable, configurable by OD_extensionIO_init() function.

+
The documentation for this struct was generated from the following file: +
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/structOD__obj__extended__t.js b/Devices/Libraries/Systems/CANopenSocket/docs/structOD__obj__extended__t.js new file mode 100755 index 0000000..199bc6f --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/structOD__obj__extended__t.js @@ -0,0 +1,6 @@ +var structOD__obj__extended__t = +[ + [ "extIO", "structOD__obj__extended__t.html#a6ccf59c770c3887233ab7b850e77e375", null ], + [ "flagsPDO", "structOD__obj__extended__t.html#a80e8186d50cb6ca8ebf447d43815e566", null ], + [ "odObjectOriginal", "structOD__obj__extended__t.html#abbc62d96e2ecafc99bb1eaf1210f816a", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/structOD__obj__record__t.html b/Devices/Libraries/Systems/CANopenSocket/docs/structOD__obj__record__t.html new file mode 100755 index 0000000..70d7b55 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/structOD__obj__record__t.html @@ -0,0 +1,142 @@ + + + + + + + +CANopenNode: OD_obj_record_t Struct Reference + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Data Fields
+
+
OD_obj_record_t Struct Reference
CANopen_301 » OD interface » OD definition objects
+
+
+ +

Object for OD sub-elements, used in "RECORD" type OD objects. + More...

+ +

#include <CO_ODinterface.h>

+ + + + + + + + + + + + + + +

+Data Fields

+void * data
 Pointer to data.
 
+uint8_t subIndex
 Sub index of element.
 
+OD_attr_t attribute
 Attribute bitfield, see OD_attributes_t.
 
+OD_size_t dataLength
 Data length in bytes.
 
+

Detailed Description

+

Object for OD sub-elements, used in "RECORD" type OD objects.

+
The documentation for this struct was generated from the following file: +
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/structOD__obj__record__t.js b/Devices/Libraries/Systems/CANopenSocket/docs/structOD__obj__record__t.js new file mode 100755 index 0000000..658afb8 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/structOD__obj__record__t.js @@ -0,0 +1,7 @@ +var structOD__obj__record__t = +[ + [ "data", "structOD__obj__record__t.html#a490d4818eaa6ca94d2c6fc598647dbd1", null ], + [ "subIndex", "structOD__obj__record__t.html#a7cfa0012ae3aec7bbfc98e5e8301782c", null ], + [ "attribute", "structOD__obj__record__t.html#a42290a19541170f8d108acf029fec171", null ], + [ "dataLength", "structOD__obj__record__t.html#a1302f14c20e976ac884196d4c3e201c1", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/structOD__obj__var__t.html b/Devices/Libraries/Systems/CANopenSocket/docs/structOD__obj__var__t.html new file mode 100755 index 0000000..2d116ad --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/structOD__obj__var__t.html @@ -0,0 +1,138 @@ + + + + + + + +CANopenNode: OD_obj_var_t Struct Reference + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Data Fields
+
+
OD_obj_var_t Struct Reference
CANopen_301 » OD interface » OD definition objects
+
+
+ +

Object for single OD variable, used for "VAR" type OD objects. + More...

+ +

#include <CO_ODinterface.h>

+ + + + + + + + + + + +

+Data Fields

+void * data
 Pointer to data.
 
+OD_attr_t attribute
 Attribute bitfield, see OD_attributes_t.
 
+OD_size_t dataLength
 Data length in bytes.
 
+

Detailed Description

+

Object for single OD variable, used for "VAR" type OD objects.

+
The documentation for this struct was generated from the following file: +
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/structOD__obj__var__t.js b/Devices/Libraries/Systems/CANopenSocket/docs/structOD__obj__var__t.js new file mode 100755 index 0000000..71a4bc1 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/structOD__obj__var__t.js @@ -0,0 +1,6 @@ +var structOD__obj__var__t = +[ + [ "data", "structOD__obj__var__t.html#a7c15865c69e0dc0a09f6d21bd890d062", null ], + [ "attribute", "structOD__obj__var__t.html#a4662bd6ca12b3ec147f9ffeafb64fe77", null ], + [ "dataLength", "structOD__obj__var__t.html#a385af11ed619b78de9b2f1ae6528a870", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/structOD__stream__t.html b/Devices/Libraries/Systems/CANopenSocket/docs/structOD__stream__t.html new file mode 100755 index 0000000..41e2066 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/structOD__stream__t.html @@ -0,0 +1,175 @@ + + + + + + + +CANopenNode: OD_stream_t Struct Reference + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Data Fields
+
+
OD_stream_t Struct Reference
CANopen_301 » OD interface
+
+
+ +

IO stream structure, used for read/write access to OD variable, part of OD_IO_t. + More...

+ +

#include <CO_ODinterface.h>

+ + + + + + + + + + + + + + +

+Data Fields

void * dataObjectOriginal
 Pointer to original data object, defined by Object Dictionary. More...
 
void * object
 Pointer to object, passed by OD_extensionIO_init(). More...
 
+OD_size_t dataLength
 Data length in bytes or 0, if length is not specified.
 
+OD_size_t dataOffset
 In case of large data, dataOffset indicates position of already transferred data.
 
+

Detailed Description

+

IO stream structure, used for read/write access to OD variable, part of OD_IO_t.

+

Field Documentation

+ +

◆ dataObjectOriginal

+ +
+
+ + + + +
void* OD_stream_t::dataObjectOriginal
+
+ +

Pointer to original data object, defined by Object Dictionary.

+

Default read/write functions operate on it. If memory for data object is not specified by Object Dictionary, then dataObjectOriginal is NULL.

+ +
+
+ +

◆ object

+ +
+
+ + + + +
void* OD_stream_t::object
+
+ +

Pointer to object, passed by OD_extensionIO_init().

+

Can be used inside read / write functions from IO extension.

+ +
+
+
The documentation for this struct was generated from the following file: +
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/structOD__stream__t.js b/Devices/Libraries/Systems/CANopenSocket/docs/structOD__stream__t.js new file mode 100755 index 0000000..198fba7 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/structOD__stream__t.js @@ -0,0 +1,7 @@ +var structOD__stream__t = +[ + [ "dataObjectOriginal", "structOD__stream__t.html#aad2551a7bf0da6396e6b909adf487b01", null ], + [ "object", "structOD__stream__t.html#af3c8ded429eefa8646317207c0b3ff97", null ], + [ "dataLength", "structOD__stream__t.html#a60c4499678a5db84a7f7285b934ce75a", null ], + [ "dataOffset", "structOD__stream__t.html#a97799b896ee689504771a9274575bcdc", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/structOD__subEntry__t.html b/Devices/Libraries/Systems/CANopenSocket/docs/structOD__subEntry__t.html new file mode 100755 index 0000000..eb59028 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/structOD__subEntry__t.html @@ -0,0 +1,183 @@ + + + + + + + +CANopenNode: OD_subEntry_t Struct Reference + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Data Fields
+
+
OD_subEntry_t Struct Reference
CANopen_301 » OD interface
+
+
+ +

Structure describing properties of a variable, located in specific index and sub-index inside the Object Dictionary. + More...

+ +

#include <CO_ODinterface.h>

+ + + + + + + + + + + + + + + + + +

+Data Fields

+uint16_t index
 Object Dictionary index.
 
+uint8_t subIndex
 Object Dictionary sub-index.
 
uint8_t subEntriesCount
 Number of sub-entries in OD object. More...
 
+OD_attr_t attribute
 Attribute bit-field of the OD sub-object, see OD_attributes_t.
 
OD_flagsPDO_t * flagsPDO
 Pointer to PDO flags bit-field. More...
 
+

Detailed Description

+

Structure describing properties of a variable, located in specific index and sub-index inside the Object Dictionary.

+

Structure is initialized with OD_getSub() function.

+

Field Documentation

+ +

◆ subEntriesCount

+ +
+
+ + + + +
uint8_t OD_subEntry_t::subEntriesCount
+
+ +

Number of sub-entries in OD object.

+

For VAR is 1, for ARRAY is maxSubIndex + 1, for RECORD maxSubIndex may be larger, if there is a gap between sub-indexes.

+ +
+
+ +

◆ flagsPDO

+ +
+
+ + + + +
OD_flagsPDO_t* OD_subEntry_t::flagsPDO
+
+ +

Pointer to PDO flags bit-field.

+

This is optional extension of OD object. If OD object has enabled this extension, then each sub-element is coupled with own flagsPDO variable of size 8 to 64 bits (size is configurable by OD_flagsPDO_t). Flag is useful, when variable is mapped to RPDO or TPDO.

+

If sub-element is mapped to RPDO, then bit0 is set to 1 each time, when any RPDO writes new data into variable. Application may clear bit0.

+

If sub-element is mapped to TPDO, then TPDO will set one bit on the time, it is sent. First TPDO will set bit1, second TPDO will set bit2, etc. Up to 63 TPDOs can use flagsPDO.

+

Another functionality is with asynchronous TPDOs, to which variable may be mapped. If corresponding bit is 0, TPDO will be sent. This means, that if application sets variable pointed by flagsPDO to zero, it will trigger sending all asynchronous TPDOs (up to first 63), to which variable is mapped.

+ +
+
+
The documentation for this struct was generated from the following file: +
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/structOD__subEntry__t.js b/Devices/Libraries/Systems/CANopenSocket/docs/structOD__subEntry__t.js new file mode 100755 index 0000000..b9fab64 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/structOD__subEntry__t.js @@ -0,0 +1,8 @@ +var structOD__subEntry__t = +[ + [ "index", "structOD__subEntry__t.html#a4e10db7bdf91d721ecc7d97f4dda67ff", null ], + [ "subIndex", "structOD__subEntry__t.html#a2c39cbf86fc3b384b017ed6261d379bd", null ], + [ "subEntriesCount", "structOD__subEntry__t.html#a49750c447be0b3773e8aa1814d5fa2de", null ], + [ "attribute", "structOD__subEntry__t.html#ae7d83df4e106219f32cb28d7c510b9d2", null ], + [ "flagsPDO", "structOD__subEntry__t.html#ad6a4eb8da2f84f7b84164eac01115dd1", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/structOD__t.html b/Devices/Libraries/Systems/CANopenSocket/docs/structOD__t.html new file mode 100755 index 0000000..f9b12cd --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/structOD__t.html @@ -0,0 +1,134 @@ + + + + + + + +CANopenNode: OD_t Struct Reference + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+Data Fields
+
+
OD_t Struct Reference
CANopen_301 » OD interface
+
+
+ +

Object Dictionary. + More...

+ +

#include <CO_ODinterface.h>

+ + + + + + + + +

+Data Fields

+uint16_t size
 Number of elements in the list, without last element, which is blank.
 
+const OD_entry_t * list
 List OD entries (table of contents), ordered by index.
 
+

Detailed Description

+

Object Dictionary.

+
The documentation for this struct was generated from the following file: +
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/structOD__t.js b/Devices/Libraries/Systems/CANopenSocket/docs/structOD__t.js new file mode 100755 index 0000000..fd44e93 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/structOD__t.js @@ -0,0 +1,5 @@ +var structOD__t = +[ + [ "size", "structOD__t.html#ab52b946b5f0127fe618c6f5bbe65a698", null ], + [ "list", "structOD__t.html#a935673d88e82c7a72be76766467d74b4", null ] +]; \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/sync_off.png b/Devices/Libraries/Systems/CANopenSocket/docs/sync_off.png new file mode 100755 index 0000000..3b443fc Binary files /dev/null and b/Devices/Libraries/Systems/CANopenSocket/docs/sync_off.png differ diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/sync_on.png b/Devices/Libraries/Systems/CANopenSocket/docs/sync_on.png new file mode 100755 index 0000000..e08320f Binary files /dev/null and b/Devices/Libraries/Systems/CANopenSocket/docs/sync_on.png differ diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/tab_a.png b/Devices/Libraries/Systems/CANopenSocket/docs/tab_a.png new file mode 100755 index 0000000..3b725c4 Binary files /dev/null and b/Devices/Libraries/Systems/CANopenSocket/docs/tab_a.png differ diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/tab_b.png b/Devices/Libraries/Systems/CANopenSocket/docs/tab_b.png new file mode 100755 index 0000000..e2b4a86 Binary files /dev/null and b/Devices/Libraries/Systems/CANopenSocket/docs/tab_b.png differ diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/tab_h.png b/Devices/Libraries/Systems/CANopenSocket/docs/tab_h.png new file mode 100755 index 0000000..fd5cb70 Binary files /dev/null and b/Devices/Libraries/Systems/CANopenSocket/docs/tab_h.png differ diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/tab_s.png b/Devices/Libraries/Systems/CANopenSocket/docs/tab_s.png new file mode 100755 index 0000000..ab478c9 Binary files /dev/null and b/Devices/Libraries/Systems/CANopenSocket/docs/tab_s.png differ diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/tabs.css b/Devices/Libraries/Systems/CANopenSocket/docs/tabs.css new file mode 100755 index 0000000..7d45d36 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/tabs.css @@ -0,0 +1 @@ +.sm{position:relative;z-index:9999}.sm,.sm ul,.sm li{display:block;list-style:none;margin:0;padding:0;line-height:normal;direction:ltr;text-align:left;-webkit-tap-highlight-color:rgba(0,0,0,0)}.sm-rtl,.sm-rtl ul,.sm-rtl li{direction:rtl;text-align:right}.sm>li>h1,.sm>li>h2,.sm>li>h3,.sm>li>h4,.sm>li>h5,.sm>li>h6{margin:0;padding:0}.sm ul{display:none}.sm li,.sm a{position:relative}.sm a{display:block}.sm a.disabled{cursor:not-allowed}.sm:after{content:"\00a0";display:block;height:0;font:0px/0 serif;clear:both;visibility:hidden;overflow:hidden}.sm,.sm *,.sm *:before,.sm *:after{-moz-box-sizing:border-box;-webkit-box-sizing:border-box;box-sizing:border-box}.sm-dox{background-image:url("tab_b.png")}.sm-dox a,.sm-dox a:focus,.sm-dox a:hover,.sm-dox a:active{padding:0px 12px;padding-right:43px;font-family:"Lucida Grande","Geneva","Helvetica",Arial,sans-serif;font-size:13px;font-weight:bold;line-height:36px;text-decoration:none;text-shadow:0px 1px 1px rgba(255,255,255,0.9);color:#283A5D;outline:none}.sm-dox a:hover{background-image:url("tab_a.png");background-repeat:repeat-x;color:#fff;text-shadow:0px 1px 1px #000}.sm-dox a.current{color:#D23600}.sm-dox a.disabled{color:#bbb}.sm-dox a span.sub-arrow{position:absolute;top:50%;margin-top:-14px;left:auto;right:3px;width:28px;height:28px;overflow:hidden;font:bold 12px/28px monospace !important;text-align:center;text-shadow:none;background:rgba(255,255,255,0.5);border-radius:5px}.sm-dox a.highlighted span.sub-arrow:before{display:block;content:'-'}.sm-dox>li:first-child>a,.sm-dox>li:first-child>:not(ul) a{border-radius:5px 5px 0 0}.sm-dox>li:last-child>a,.sm-dox>li:last-child>*:not(ul) a,.sm-dox>li:last-child>ul,.sm-dox>li:last-child>ul>li:last-child>a,.sm-dox>li:last-child>ul>li:last-child>*:not(ul) a,.sm-dox>li:last-child>ul>li:last-child>ul,.sm-dox>li:last-child>ul>li:last-child>ul>li:last-child>a,.sm-dox>li:last-child>ul>li:last-child>ul>li:last-child>*:not(ul) a,.sm-dox>li:last-child>ul>li:last-child>ul>li:last-child>ul,.sm-dox>li:last-child>ul>li:last-child>ul>li:last-child>ul>li:last-child>a,.sm-dox>li:last-child>ul>li:last-child>ul>li:last-child>ul>li:last-child>*:not(ul) a,.sm-dox>li:last-child>ul>li:last-child>ul>li:last-child>ul>li:last-child>ul,.sm-dox>li:last-child>ul>li:last-child>ul>li:last-child>ul>li:last-child>ul>li:last-child>a,.sm-dox>li:last-child>ul>li:last-child>ul>li:last-child>ul>li:last-child>ul>li:last-child>*:not(ul) a,.sm-dox>li:last-child>ul>li:last-child>ul>li:last-child>ul>li:last-child>ul>li:last-child>ul{border-radius:0 0 5px 5px}.sm-dox>li:last-child>a.highlighted,.sm-dox>li:last-child>*:not(ul) a.highlighted,.sm-dox>li:last-child>ul>li:last-child>a.highlighted,.sm-dox>li:last-child>ul>li:last-child>*:not(ul) a.highlighted,.sm-dox>li:last-child>ul>li:last-child>ul>li:last-child>a.highlighted,.sm-dox>li:last-child>ul>li:last-child>ul>li:last-child>*:not(ul) a.highlighted,.sm-dox>li:last-child>ul>li:last-child>ul>li:last-child>ul>li:last-child>a.highlighted,.sm-dox>li:last-child>ul>li:last-child>ul>li:last-child>ul>li:last-child>*:not(ul) a.highlighted,.sm-dox>li:last-child>ul>li:last-child>ul>li:last-child>ul>li:last-child>ul>li:last-child>a.highlighted,.sm-dox>li:last-child>ul>li:last-child>ul>li:last-child>ul>li:last-child>ul>li:last-child>*:not(ul) a.highlighted{border-radius:0}.sm-dox ul{background:rgba(162,162,162,0.1)}.sm-dox ul a,.sm-dox ul a:focus,.sm-dox ul a:hover,.sm-dox ul a:active{font-size:12px;border-left:8px solid transparent;line-height:36px;text-shadow:none;background-color:white;background-image:none}.sm-dox ul a:hover{background-image:url("tab_a.png");background-repeat:repeat-x;color:#fff;text-shadow:0px 1px 1px #000}.sm-dox ul ul a,.sm-dox ul ul a:hover,.sm-dox ul ul a:focus,.sm-dox ul ul a:active{border-left:16px solid transparent}.sm-dox ul ul ul a,.sm-dox ul ul ul a:hover,.sm-dox ul ul ul a:focus,.sm-dox ul ul ul a:active{border-left:24px solid transparent}.sm-dox ul ul ul ul a,.sm-dox ul ul ul ul a:hover,.sm-dox ul ul ul ul a:focus,.sm-dox ul ul ul ul a:active{border-left:32px solid transparent}.sm-dox ul ul ul ul ul a,.sm-dox ul ul ul ul ul a:hover,.sm-dox ul ul ul ul ul a:focus,.sm-dox ul ul ul ul ul a:active{border-left:40px solid transparent}@media (min-width: 768px){.sm-dox ul{position:absolute;width:12em}.sm-dox li{float:left}.sm-dox.sm-rtl li{float:right}.sm-dox ul li,.sm-dox.sm-rtl ul li,.sm-dox.sm-vertical li{float:none}.sm-dox a{white-space:nowrap}.sm-dox ul a,.sm-dox.sm-vertical a{white-space:normal}.sm-dox .sm-nowrap>li>a,.sm-dox .sm-nowrap>li>:not(ul) a{white-space:nowrap}.sm-dox{padding:0 10px;background-image:url("tab_b.png");line-height:36px}.sm-dox a span.sub-arrow{top:50%;margin-top:-2px;right:12px;width:0;height:0;border-width:4px;border-style:solid dashed dashed dashed;border-color:#283A5D transparent transparent transparent;background:transparent;border-radius:0}.sm-dox a,.sm-dox a:focus,.sm-dox a:active,.sm-dox a:hover,.sm-dox a.highlighted{padding:0px 12px;background-image:url("tab_s.png");background-repeat:no-repeat;background-position:right;border-radius:0 !important}.sm-dox a:hover{background-image:url("tab_a.png");background-repeat:repeat-x;color:#fff;text-shadow:0px 1px 1px #000}.sm-dox a:hover span.sub-arrow{border-color:#fff transparent transparent transparent}.sm-dox a.has-submenu{padding-right:24px}.sm-dox li{border-top:0}.sm-dox>li>ul:before,.sm-dox>li>ul:after{content:'';position:absolute;top:-18px;left:30px;width:0;height:0;overflow:hidden;border-width:9px;border-style:dashed dashed solid dashed;border-color:transparent transparent #bbb transparent}.sm-dox>li>ul:after{top:-16px;left:31px;border-width:8px;border-color:transparent transparent #fff transparent}.sm-dox ul{border:1px solid #bbb;padding:5px 0;background:#fff;border-radius:5px !important;box-shadow:0 5px 9px rgba(0,0,0,0.2)}.sm-dox ul a span.sub-arrow{right:8px;top:50%;margin-top:-5px;border-width:5px;border-color:transparent transparent transparent #555;border-style:dashed dashed dashed solid}.sm-dox ul a,.sm-dox ul a:hover,.sm-dox ul a:focus,.sm-dox ul a:active,.sm-dox ul a.highlighted{color:#555;background-image:none;border:0 !important;color:#555;background-image:none}.sm-dox ul a:hover{background-image:url("tab_a.png");background-repeat:repeat-x;color:#fff;text-shadow:0px 1px 1px #000}.sm-dox ul a:hover span.sub-arrow{border-color:transparent transparent transparent #fff}.sm-dox span.scroll-up,.sm-dox span.scroll-down{position:absolute;display:none;visibility:hidden;overflow:hidden;background:#fff;height:36px}.sm-dox span.scroll-up:hover,.sm-dox span.scroll-down:hover{background:#eee}.sm-dox span.scroll-up:hover span.scroll-up-arrow,.sm-dox span.scroll-up:hover span.scroll-down-arrow{border-color:transparent transparent #D23600 transparent}.sm-dox span.scroll-down:hover span.scroll-down-arrow{border-color:#D23600 transparent transparent transparent}.sm-dox span.scroll-up-arrow,.sm-dox span.scroll-down-arrow{position:absolute;top:0;left:50%;margin-left:-6px;width:0;height:0;overflow:hidden;border-width:6px;border-style:dashed dashed solid dashed;border-color:transparent transparent #555 transparent}.sm-dox span.scroll-down-arrow{top:8px;border-style:solid dashed dashed dashed;border-color:#555 transparent transparent transparent}.sm-dox.sm-rtl a.has-submenu{padding-right:12px;padding-left:24px}.sm-dox.sm-rtl a span.sub-arrow{right:auto;left:12px}.sm-dox.sm-rtl.sm-vertical a.has-submenu{padding:10px 20px}.sm-dox.sm-rtl.sm-vertical a span.sub-arrow{right:auto;left:8px;border-style:dashed solid dashed dashed;border-color:transparent #555 transparent transparent}.sm-dox.sm-rtl>li>ul:before{left:auto;right:30px}.sm-dox.sm-rtl>li>ul:after{left:auto;right:31px}.sm-dox.sm-rtl ul a.has-submenu{padding:10px 20px !important}.sm-dox.sm-rtl ul a span.sub-arrow{right:auto;left:8px;border-style:dashed solid dashed dashed;border-color:transparent #555 transparent transparent}.sm-dox.sm-vertical{padding:10px 0;border-radius:5px}.sm-dox.sm-vertical a{padding:10px 20px}.sm-dox.sm-vertical a:hover,.sm-dox.sm-vertical a:focus,.sm-dox.sm-vertical a:active,.sm-dox.sm-vertical a.highlighted{background:#fff}.sm-dox.sm-vertical a.disabled{background-image:url("tab_b.png")}.sm-dox.sm-vertical a span.sub-arrow{right:8px;top:50%;margin-top:-5px;border-width:5px;border-style:dashed dashed dashed solid;border-color:transparent transparent transparent #555}.sm-dox.sm-vertical>li>ul:before,.sm-dox.sm-vertical>li>ul:after{display:none}.sm-dox.sm-vertical ul a{padding:10px 20px}.sm-dox.sm-vertical ul a:hover,.sm-dox.sm-vertical ul a:focus,.sm-dox.sm-vertical ul a:active,.sm-dox.sm-vertical ul a.highlighted{background:#eee}.sm-dox.sm-vertical ul a.disabled{background:#fff}} diff --git a/Devices/Libraries/Systems/CANopenSocket/docs/unionCO__LSS__address__t.html b/Devices/Libraries/Systems/CANopenSocket/docs/unionCO__LSS__address__t.html new file mode 100755 index 0000000..dfefbbd --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/docs/unionCO__LSS__address__t.html @@ -0,0 +1,121 @@ + + + + + + + +CANopenNode: CO_LSS_address_t Union Reference + + + + + + + + + + + + + + +
+
+ + + + + + + +
+
CANopenNode +
+
+ + + + + + +
+
+
+ + + +
+
+ +
+
+
+ +
+ +
+
+ + +
+ +
+ +
+
+
CO_LSS_address_t Union Reference
CANopen_305 » LSS
+
+
+ +

The LSS address is a 128 bit number, uniquely identifying each node. + More...

+ +

#include <CO_LSS.h>

+

Detailed Description

+

The LSS address is a 128 bit number, uniquely identifying each node.

+

It consists of the values in object 0x1018.

+
The documentation for this union was generated from the following file: +
+
+ + + + diff --git a/Devices/Libraries/Systems/CANopenSocket/examples/basicDevice/.gitignore b/Devices/Libraries/Systems/CANopenSocket/examples/basicDevice/.gitignore new file mode 100755 index 0000000..2689fb4 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/examples/basicDevice/.gitignore @@ -0,0 +1,5 @@ +/basicDevice +/basicDeviceWithGateway +CO_version.h +*.persist +*.persist.old diff --git a/Devices/Libraries/Systems/CANopenSocket/examples/basicDevice/CO_application.c b/Devices/Libraries/Systems/CANopenSocket/examples/basicDevice/CO_application.c new file mode 100755 index 0000000..85b3721 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/examples/basicDevice/CO_application.c @@ -0,0 +1,130 @@ +/* + * Application interface for CANopenNode. + * + * @file CO_application.c + * @author Janez Paternoster + * @copyright 2020 Janez Paternoster + * + * This file is part of CANopenNode, an opensource CANopen Stack. + * Project home page is . + * For more information on CANopen see . + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + + +#include "CO_application.h" +#include "CO_version.h" +#include "OD.h" +#include "testingVariables.h" +#include + + +/* Define object for testingVariables */ +testingVariables_t testVar; +/* Extension for OD object */ +OD_extension_t OD_version_extension; + + +/* + * Custom function for reading OD object _Version_ + * + * For more information see file CO_ODinterface.h, OD_IO_t. + */ +static ODR_t OD_read_version(OD_stream_t *stream, void *buf, + OD_size_t count, OD_size_t *countRead) +{ + if (stream == NULL || buf == NULL || countRead == NULL) { + return ODR_DEV_INCOMPAT; + } + + switch (stream->subIndex) { + case 1: { + OD_size_t len = strlen(CO_VERSION_CANOPENNODE); + if (len > count) len = count; + memcpy(buf, CO_VERSION_CANOPENNODE, len); + + *countRead = stream->dataLength = len; + return ODR_OK; + } + case 2: { + OD_size_t len = strlen(CO_VERSION_APPLICATION); + if (len > count) len = count; + memcpy(buf, CO_VERSION_APPLICATION, len); + + *countRead = stream->dataLength = len; + return ODR_OK; + } + default: + return OD_readOriginal(stream, buf, count, countRead); + } +} + + +/******************************************************************************/ +CO_ReturnError_t app_programStart(bool_t CANopenConfigured, uint32_t *errInfo) { + (void) CANopenConfigured; + CO_ReturnError_t err = CO_ERROR_NO; + + /* increment auto-storage variable */ + OD_PERSIST_TEST_AUTO.x2106_power_onCounter ++; + + /* Initialize custom read-only OD object "Version" */ + OD_version_extension.object = NULL; + OD_version_extension.read = OD_read_version; + OD_version_extension.write = NULL; + OD_extension_init(OD_ENTRY_H2105_version, &OD_version_extension); + + /* Initialize more advanced object, which operates with testing variables + * OD_ENTRY_H2120_testingVariables is constant defined in OD.h. More + * flexible alternative for third argument is 'OD_find(&OD, 0x2120)' */ + err = testingVariables_init(&testVar, + errInfo, + OD_ENTRY_H2120_testingVariables); + + return err; +} + + +/******************************************************************************/ +void app_communicationReset(bool_t CANopenConfigured) { + + /* example printouts */ + if (CANopenConfigured) + printf("CANopen Node-ID is configured and all services will work.\n"); + else + printf("CANopen Node-ID is unconfigured, so only LSS slave will work.\n"); + + printf("Printing 'OD_PERSIST_APP.x2120_testingVariables.stringLong':\n%s\n", + OD_PERSIST_TEST.x2120_testingVariables.stringLong); + + fflush(stdout); +} + + +/******************************************************************************/ +void app_programEnd() { + +} + + +/******************************************************************************/ +void app_programAsync(bool_t CANopenConfigured, uint32_t timer1usDiff) { + (void) CANopenConfigured; (void) timer1usDiff; /* unused */ +} + + +/******************************************************************************/ +void app_program1ms(bool_t CANopenConfigured, uint32_t timer1usDiff) { + (void) CANopenConfigured; (void) timer1usDiff; /* unused */ +} diff --git a/Devices/Libraries/Systems/CANopenSocket/examples/basicDevice/CO_application.h b/Devices/Libraries/Systems/CANopenSocket/examples/basicDevice/CO_application.h new file mode 100755 index 0000000..931137c --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/examples/basicDevice/CO_application.h @@ -0,0 +1,88 @@ +/** + * Application interface for CANopenNode. + * + * @file CO_application.h + * @author Janez Paternoster + * @copyright 2020 Janez Paternoster + * + * This file is part of CANopenNode, an opensource CANopen Stack. + * Project home page is . + * For more information on CANopen see . + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + + +#ifndef CO_APPLICATION_H +#define CO_APPLICATION_H + + +#include "CANopen.h" + + +/* Optional OD entry for CO_CANopenInit() -> CO_EM_init() */ +#define OD_STATUS_BITS OD_ENTRY_H2100_errorStatusBits + + +/** + * Function is called on program startup. + * + * @param CANopenConfigured True, if CANopen Node-Id is known and all CANopen + * objects are configured. + * @param [out] errInfo Variable may indicate additional information for some + * types of errors. + * + * @return @ref CO_ReturnError_t CO_ERROR_NO in case of success. + */ +CO_ReturnError_t app_programStart(bool_t CANopenConfigured, uint32_t *errInfo); + + +/** + * Function is called after CANopen communication reset. + * + * @param CANopenConfigured True, if CANopen Node-Id is known and all CANopen + * objects are configured. + */ +void app_communicationReset(bool_t CANopenConfigured); + + +/** + * Function is called just before program ends. + */ +void app_programEnd(); + + +/** + * Function is called cyclically from main. + * + * @param CANopenConfigured True, if CANopen Node-Id is known and all CANopen + * objects are configured. + * @param timer1usDiff Time difference since last call in microseconds + */ +void app_programAsync(bool_t CANopenConfigured, uint32_t timer1usDiff); + + +/** + * Function is called cyclically from realtime thread at constant intervals. + * + * Code inside this function must be executed fast. Take care on race conditions + * with app_programAsync. + * + * @param CANopenConfigured True, if CANopen Node-Id is known and all CANopen + * objects are configured. + * @param timer1usDiff Time difference since last call in microseconds + */ +void app_program1ms(bool_t CANopenConfigured, uint32_t timer1usDiff); + + +#endif /* CO_APPLICATION_H */ diff --git a/Devices/Libraries/Systems/CANopenSocket/examples/basicDevice/CO_driver_custom.h b/Devices/Libraries/Systems/CANopenSocket/examples/basicDevice/CO_driver_custom.h new file mode 100755 index 0000000..509efd1 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/examples/basicDevice/CO_driver_custom.h @@ -0,0 +1,78 @@ +/* + * Custom definitions for CANopenNode. + * + * @file CO_driver_custom.h + * @author Janez Paternoster + * @copyright 2020 Janez Paternoster + * + * This file is part of CANopenNode, an opensource CANopen Stack. + * Project home page is . + * For more information on CANopen see . + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + + +#ifndef CO_DRIVER_CUSTOM_H +#define CO_DRIVER_CUSTOM_H + + +#define CO_USE_APPLICATION + + +/* Stack configuration override default values. + * For more information see file CO_config.h. */ + +/* Gateway functions are enabled by default in CO_driver_target.h */ +#ifndef CO_GATEWAY +/* without CO_CONFIG_EM_CONSUMER */ +#define CO_CONFIG_EM (CO_CONFIG_EM_PRODUCER | \ + CO_CONFIG_EM_PROD_CONFIGURABLE | \ + CO_CONFIG_EM_PROD_INHIBIT | \ + CO_CONFIG_EM_HISTORY | \ + CO_CONFIG_EM_STATUS_BITS | \ + CO_CONFIG_FLAG_CALLBACK_PRE | \ + CO_CONFIG_FLAG_TIMERNEXT) + +/* Gateway command interface, SDO client and LSS master are disabled. */ +#define CO_CONFIG_SDO_CLI (0) + +/* without CO_CONFIG_LSS_MASTER */ +#define CO_CONFIG_LSS (CO_CONFIG_LSS_SLAVE | \ + CO_CONFIG_LSS_SLAVE_FASTSCAN_DIRECT_RESPOND | \ + CO_CONFIG_FLAG_CALLBACK_PRE) + +#define CO_CONFIG_GTW (0) + +#define CO_CONFIG_FIFO (0) +#endif /* CO_GATEWAY */ + +#define CO_STORAGE_APPLICATION \ + { \ + .addr = &OD_PERSIST_TEST, \ + .len = sizeof(OD_PERSIST_TEST), \ + .subIndexOD = 5, \ + .attr = CO_storage_cmd | CO_storage_restore, \ + .filename = {'o','d','_','t','e','s','t', \ + '.','p','e','r','s','i','s','t','\0'} \ + }, \ + { \ + .addr = &OD_PERSIST_TEST_AUTO, \ + .len = sizeof(OD_PERSIST_TEST_AUTO), \ + .subIndexOD = 6, \ + .attr = CO_storage_cmd | CO_storage_auto | CO_storage_restore, \ + .filename = {'o','d','_','t','e','s','t','_','a','u','t','o', \ + '.','p','e','r','s','i','s','t','\0'} \ + } + +#endif /* CO_DRIVER_CUSTOM_H */ diff --git a/Devices/Libraries/Systems/CANopenSocket/examples/basicDevice/EDSEditor.png b/Devices/Libraries/Systems/CANopenSocket/examples/basicDevice/EDSEditor.png new file mode 100755 index 0000000..9d85b7b Binary files /dev/null and b/Devices/Libraries/Systems/CANopenSocket/examples/basicDevice/EDSEditor.png differ diff --git a/Devices/Libraries/Systems/CANopenSocket/examples/basicDevice/Makefile b/Devices/Libraries/Systems/CANopenSocket/examples/basicDevice/Makefile new file mode 100755 index 0000000..2d510f7 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/examples/basicDevice/Makefile @@ -0,0 +1,76 @@ +# Makefile for application with CANopenNode and Linux socketCAN + + +DRV_SRC = ../../CANopenNode/socketCAN +CANOPEN_SRC = ../../CANopenNode +APPL_SRC = . + + +LINK_TARGET = basicDevice +VERSION_FILE = CO_version.h + + +INCLUDE_DIRS = \ + -I$(DRV_SRC) \ + -I$(CANOPEN_SRC) \ + -I$(APPL_SRC) + + +SOURCES = \ + $(DRV_SRC)/CO_driver.c \ + $(DRV_SRC)/CO_error.c \ + $(DRV_SRC)/CO_epoll_interface.c \ + $(DRV_SRC)/CO_storageLinux.c \ + $(CANOPEN_SRC)/301/CO_ODinterface.c \ + $(CANOPEN_SRC)/301/CO_NMT_Heartbeat.c \ + $(CANOPEN_SRC)/301/CO_HBconsumer.c \ + $(CANOPEN_SRC)/301/CO_Emergency.c \ + $(CANOPEN_SRC)/301/CO_SDOserver.c \ + $(CANOPEN_SRC)/301/CO_TIME.c \ + $(CANOPEN_SRC)/301/CO_SYNC.c \ + $(CANOPEN_SRC)/301/CO_PDO.c \ + $(CANOPEN_SRC)/301/crc16-ccitt.c \ + $(CANOPEN_SRC)/301/CO_storage.c \ + $(CANOPEN_SRC)/303/CO_LEDs.c \ + $(CANOPEN_SRC)/305/CO_LSSslave.c \ + $(CANOPEN_SRC)/CANopen.c \ + $(APPL_SRC)/CO_application.c \ + $(APPL_SRC)/OD.c \ + $(APPL_SRC)/testingVariables.c \ + $(DRV_SRC)/CO_main_basic.c + + +OBJS = $(SOURCES:%.c=%.o) +CC ?= gcc +OPT = +OPT += -g +#OPT += -O2 +#OPT += -DCO_SINGLE_THREAD +#OPT += -DCO_CONFIG_DEBUG=0xFFFF +#OPT += -Wextra -Wshadow -pedantic -fanalyzer +#OPT += -DCO_USE_GLOBALS +#OPT += -DCO_MULTIPLE_OD +CFLAGS = -Wall -DCO_DRIVER_CUSTOM $(OPT) $(INCLUDE_DIRS) +LDFLAGS = +LDFLAGS += -g +LDFLAGS += -pthread + +#Options can be also passed via make: 'make OPT="-g" LDFLAGS="-pthread"' + + +.PHONY: all clean + +all: clean version $(LINK_TARGET) + +clean: + rm -f $(OBJS) $(LINK_TARGET) $(VERSION_FILE) + +%.o: %.c + $(CC) $(CFLAGS) -c $< -o $@ + +$(LINK_TARGET): $(OBJS) + $(CC) $(LDFLAGS) $^ -o $@ + +version: + echo "#define CO_VERSION_CANOPENNODE \"$(shell git -C $(CANOPEN_SRC) describe --tags --long --dirty)\"" > $(VERSION_FILE) + echo "#define CO_VERSION_APPLICATION \"$(shell git describe --tags --long --dirty)\"" >> $(VERSION_FILE) diff --git a/Devices/Libraries/Systems/CANopenSocket/examples/basicDevice/OD.c b/Devices/Libraries/Systems/CANopenSocket/examples/basicDevice/OD.c new file mode 100755 index 0000000..e31c8ed --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/examples/basicDevice/OD.c @@ -0,0 +1,1390 @@ +/******************************************************************************* + CANopen Object Dictionary definition for CANopenNode V4 + + This file was automatically generated with + libedssharp Object Dictionary Editor v0.8-122-g6c02323 + + https://github.com/CANopenNode/CANopenNode + https://github.com/robincornelius/libedssharp + + DON'T EDIT THIS FILE MANUALLY, UNLESS YOU KNOW WHAT YOU ARE DOING !!!! +*******************************************************************************/ + +#define OD_DEFINITION +#include "301/CO_ODinterface.h" +#include "OD.h" + +#if CO_VERSION_MAJOR < 4 +#error This Object dictionary is compatible with CANopenNode V4.0 and above! +#endif + +/******************************************************************************* + OD data initialization of all groups +*******************************************************************************/ +OD_PERSIST_COMM_t OD_PERSIST_COMM = { + .x1000_deviceType = 0x000F0191, + .x1005_COB_ID_SYNCMessage = 0x00000080, + .x1006_communicationCyclePeriod = 0x00000000, + .x1007_synchronousWindowLength = 0x00000000, + .x1012_COB_IDTimeStampObject = 0x00000100, + .x1014_COB_ID_EMCY = 0x00000080, + .x1015_inhibitTimeEMCY = 0x0000, + .x1016_consumerHeartbeatTime_sub0 = 0x08, + .x1016_consumerHeartbeatTime = {0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000}, + .x1017_producerHeartbeatTime = 0x0000, + .x1018_identity = { + .highestSub_indexSupported = 0x04, + .vendor_ID = 0x00000000, + .productCode = 0x00000000, + .revisionNumber = 0x00000000, + .serialNumber = 0x00000000 + }, + .x1019_synchronousCounterOverflowValue = 0x00, + .x1280_SDOClientParameter = { + .highestSub_indexSupported = 0x03, + .COB_IDClientToServerTx = 0x80000000, + .COB_IDServerToClientRx = 0x80000000, + .node_IDOfTheSDOServer = 0x01 + }, + .x1400_RPDOCommunicationParameter = { + .highestSub_indexSupported = 0x05, + .COB_IDUsedByRPDO = 0x00000200, + .transmissionType = 0xFF, + .eventTimer = 0x0000 + }, + .x1401_RPDOCommunicationParameter = { + .highestSub_indexSupported = 0x05, + .COB_IDUsedByRPDO = 0x00000300, + .transmissionType = 0xFF, + .eventTimer = 0x0000 + }, + .x1402_RPDOCommunicationParameter = { + .highestSub_indexSupported = 0x05, + .COB_IDUsedByRPDO = 0x80000400, + .transmissionType = 0xFE, + .eventTimer = 0x0000 + }, + .x1403_RPDOCommunicationParameter = { + .highestSub_indexSupported = 0x05, + .COB_IDUsedByRPDO = 0x80000500, + .transmissionType = 0xFE, + .eventTimer = 0x0000 + }, + .x1600_RPDOMappingParameter = { + .numberOfMappedApplicationObjectsInPDO = 0x02, + .applicationObject_1 = 0x62000108, + .applicationObject_2 = 0x62000208, + .applicationObject_3 = 0x62000308, + .applicationObject_4 = 0x62000408, + .applicationObject_5 = 0x62000508, + .applicationObject_6 = 0x62000608, + .applicationObject_7 = 0x62000708, + .applicationObject_8 = 0x62000808 + }, + .x1601_RPDOMappingParameter = { + .numberOfMappedApplicationObjectsInPDO = 0x04, + .applicationObject_1 = 0x64110110, + .applicationObject_2 = 0x64110210, + .applicationObject_3 = 0x64110310, + .applicationObject_4 = 0x64110410, + .applicationObject_5 = 0x00000000, + .applicationObject_6 = 0x00000000, + .applicationObject_7 = 0x00000000, + .applicationObject_8 = 0x00000000 + }, + .x1602_RPDOMappingParameter = { + .numberOfMappedApplicationObjectsInPDO = 0x00, + .applicationObject_1 = 0x00000000, + .applicationObject_2 = 0x00000000, + .applicationObject_3 = 0x00000000, + .applicationObject_4 = 0x00000000, + .applicationObject_5 = 0x00000000, + .applicationObject_6 = 0x00000000, + .applicationObject_7 = 0x00000000, + .applicationObject_8 = 0x00000000 + }, + .x1603_RPDOMappingParameter = { + .numberOfMappedApplicationObjectsInPDO = 0x00, + .applicationObject_1 = 0x00000000, + .applicationObject_2 = 0x00000000, + .applicationObject_3 = 0x00000000, + .applicationObject_4 = 0x00000000, + .applicationObject_5 = 0x00000000, + .applicationObject_6 = 0x00000000, + .applicationObject_7 = 0x00000000, + .applicationObject_8 = 0x00000000 + }, + .x1800_TPDOCommunicationParameter = { + .highestSub_indexSupported = 0x06, + .COB_IDUsedByTPDO = 0x40000180, + .transmissionType = 0xFF, + .inhibitTime = 0x0000, + .eventTimer = 0x0000, + .SYNCStartValue = 0x00 + }, + .x1801_TPDOCommunicationParameter = { + .highestSub_indexSupported = 0x06, + .COB_IDUsedByTPDO = 0x40000280, + .transmissionType = 0xFF, + .inhibitTime = 0x0000, + .eventTimer = 0x0000, + .SYNCStartValue = 0x00 + }, + .x1802_TPDOCommunicationParameter = { + .highestSub_indexSupported = 0x06, + .COB_IDUsedByTPDO = 0xC0000380, + .transmissionType = 0xFE, + .inhibitTime = 0x0000, + .eventTimer = 0x0000, + .SYNCStartValue = 0x00 + }, + .x1803_TPDOCommunicationParameter = { + .highestSub_indexSupported = 0x06, + .COB_IDUsedByTPDO = 0xC0000480, + .transmissionType = 0xFE, + .inhibitTime = 0x0000, + .eventTimer = 0x0000, + .SYNCStartValue = 0x00 + }, + .x1A00_TPDOMappingParameter = { + .numberOfMappedApplicationObjectsInPDO = 0x02, + .applicationObject_1 = 0x60000108, + .applicationObject_2 = 0x60000208, + .applicationObject_3 = 0x60000308, + .applicationObject_4 = 0x60000408, + .applicationObject_5 = 0x60000508, + .applicationObject_6 = 0x60000608, + .applicationObject_7 = 0x60000708, + .applicationObject_8 = 0x60000808 + }, + .x1A01_TPDOMappingParameter = { + .numberOfMappedApplicationObjectsInPDO = 0x04, + .applicationObject_1 = 0x64010110, + .applicationObject_2 = 0x64010210, + .applicationObject_3 = 0x64010310, + .applicationObject_4 = 0x64010410, + .applicationObject_5 = 0x00000000, + .applicationObject_6 = 0x00000000, + .applicationObject_7 = 0x00000000, + .applicationObject_8 = 0x00000000 + }, + .x1A02_TPDOMappingParameter = { + .numberOfMappedApplicationObjectsInPDO = 0x00, + .applicationObject_1 = 0x00000000, + .applicationObject_2 = 0x00000000, + .applicationObject_3 = 0x00000000, + .applicationObject_4 = 0x00000000, + .applicationObject_5 = 0x00000000, + .applicationObject_6 = 0x00000000, + .applicationObject_7 = 0x00000000, + .applicationObject_8 = 0x00000000 + }, + .x1A03_TPDOMappingParameter = { + .numberOfMappedApplicationObjectsInPDO = 0x00, + .applicationObject_1 = 0x00000000, + .applicationObject_2 = 0x00000000, + .applicationObject_3 = 0x00000000, + .applicationObject_4 = 0x00000000, + .applicationObject_5 = 0x00000000, + .applicationObject_6 = 0x00000000, + .applicationObject_7 = 0x00000000, + .applicationObject_8 = 0x00000000 + } +}; + +OD_RAM_t OD_RAM = { + .x1001_errorRegister = 0x00, + .x1003_pre_definedErrorField_sub0 = 0x00, + .x1003_pre_definedErrorField = {0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000}, + .x1010_storeParameters_sub0 = 0x06, + .x1010_storeParameters = {0x00000003, 0x00000001, 0x00000001, 0x00000003, 0x00000001, 0x00000003}, + .x1011_restoreDefaultParameters_sub0 = 0x06, + .x1011_restoreDefaultParameters = {0x00000001, 0x00000001, 0x00000001, 0x00000001, 0x00000001, 0x00000001}, + .x1200_SDOServerParameter = { + .highestSub_indexSupported = 0x02, + .COB_IDClientToServerRx = 0x00000600, + .COB_IDServerToClientTx = 0x00000580 + }, + .x2100_errorStatusBits = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}, + .x2105_version = { + .highestSub_indexSupported = 0x02 + }, + .x2110_variableInt32_sub0 = 0x10, + .x2110_variableInt32 = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + .x6000_readDigitalInput_8_bit_sub0 = 0x08, + .x6000_readDigitalInput_8_bit = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}, + .x6200_writeDigitalOutput_8_bit_sub0 = 0x08, + .x6200_writeDigitalOutput_8_bit = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}, + .x6401_readAnalogInput_16_bit_sub0 = 0x10, + .x6401_readAnalogInput_16_bit = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + .x6411_writeAnalogOutput_16_bit_sub0 = 0x08, + .x6411_writeAnalogOutput_16_bit = {0, 0, 0, 0, 0, 0, 0, 0} +}; + +OD_PERSIST_TEST_AUTO_t OD_PERSIST_TEST_AUTO = { + .x2106_power_onCounter = 0x00000000, + .x2112_variableNV_Int32_autoSave_sub0 = 0x10, + .x2112_variableNV_Int32_autoSave = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0} +}; + +OD_PERSIST_TEST_t OD_PERSIST_TEST = { + .x2111_variableInt32_save_sub0 = 0x10, + .x2111_variableInt32_save = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + .x2120_testingVariables = { + .highestSub_indexSupported = 0x0C, + .I64 = -1234567890123456789, + .U64 = 0x1234567890ABCDEF, + .R32 = 12.345, + .R64 = 456.789, + .stringShort = {'s', 't', 'r', 0}, + .stringLong = {'E', 'x', 'a', 'm', 'p', 'l', 'e', ' ', 's', 't', 'r', 'i', 'n', 'g', ' ', 'w', 'i', 't', 'h', ' ', '1', '0', '0', '0', ' ', 'b', 'y', 't', 'e', 's', ' ', 'c', 'a', 'p', 'a', 'c', 'i', 't', 'y', '.', ' ', 'I', 't', ' ', 'm', 'a', 'y', ' ', 'c', 'o', 'n', 't', 'a', 'i', 'n', ' ', 'U', 'T', 'F', '-', '8', ' ', 'c', 'h', 'a', 'r', 'a', 'c', 't', 'e', 'r', 's', ',', ' ', 'l', 'i', 'k', 'e', ' ', '\'', (char)0xE2, (char)0x82, (char)0xAC, '\'', ',', ' ', 't', 'a', 'b', 's', ' ', '\'', 0x09, '\'', ',', ' ', 'n', 'e', 'w', 'l', 'i', 'n', 'e', 's', ',', ' ', 'e', 't', 'c', '.', 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + .octetString = {0xC8, 0x3D, 0xBB, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}, + .parameterWithDefaultValue = 0x1234, + .domainFileNameRead = {'b', 'a', 's', 'i', 'c', 'D', 'e', 'v', 'i', 'c', 'e', '.', 'm', 'd', 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + .domainFileNameWrite = {'f', 'i', 'l', 'e', 'W', 'r', 'i', 't', 't', 'e', 'n', 'B', 'y', 'D', 'o', 'm', 'a', 'i', 'n', 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0} + } +}; + + + +/******************************************************************************* + All OD objects (constant definitions) +*******************************************************************************/ +typedef struct { + OD_obj_var_t o_1000_deviceType; + OD_obj_var_t o_1001_errorRegister; + OD_obj_array_t o_1003_pre_definedErrorField; + OD_obj_var_t o_1005_COB_ID_SYNCMessage; + OD_obj_var_t o_1006_communicationCyclePeriod; + OD_obj_var_t o_1007_synchronousWindowLength; + OD_obj_array_t o_1010_storeParameters; + OD_obj_array_t o_1011_restoreDefaultParameters; + OD_obj_var_t o_1012_COB_IDTimeStampObject; + OD_obj_var_t o_1014_COB_ID_EMCY; + OD_obj_var_t o_1015_inhibitTimeEMCY; + OD_obj_array_t o_1016_consumerHeartbeatTime; + OD_obj_var_t o_1017_producerHeartbeatTime; + OD_obj_record_t o_1018_identity[5]; + OD_obj_var_t o_1019_synchronousCounterOverflowValue; + OD_obj_record_t o_1200_SDOServerParameter[3]; + OD_obj_record_t o_1280_SDOClientParameter[4]; + OD_obj_record_t o_1400_RPDOCommunicationParameter[4]; + OD_obj_record_t o_1401_RPDOCommunicationParameter[4]; + OD_obj_record_t o_1402_RPDOCommunicationParameter[4]; + OD_obj_record_t o_1403_RPDOCommunicationParameter[4]; + OD_obj_record_t o_1600_RPDOMappingParameter[9]; + OD_obj_record_t o_1601_RPDOMappingParameter[9]; + OD_obj_record_t o_1602_RPDOMappingParameter[9]; + OD_obj_record_t o_1603_RPDOMappingParameter[9]; + OD_obj_record_t o_1800_TPDOCommunicationParameter[6]; + OD_obj_record_t o_1801_TPDOCommunicationParameter[6]; + OD_obj_record_t o_1802_TPDOCommunicationParameter[6]; + OD_obj_record_t o_1803_TPDOCommunicationParameter[6]; + OD_obj_record_t o_1A00_TPDOMappingParameter[9]; + OD_obj_record_t o_1A01_TPDOMappingParameter[9]; + OD_obj_record_t o_1A02_TPDOMappingParameter[9]; + OD_obj_record_t o_1A03_TPDOMappingParameter[9]; + OD_obj_var_t o_2100_errorStatusBits; + OD_obj_record_t o_2105_version[3]; + OD_obj_var_t o_2106_power_onCounter; + OD_obj_array_t o_2110_variableInt32; + OD_obj_array_t o_2111_variableInt32_save; + OD_obj_array_t o_2112_variableNV_Int32_autoSave; + OD_obj_record_t o_2120_testingVariables[13]; + OD_obj_array_t o_6000_readDigitalInput_8_bit; + OD_obj_array_t o_6200_writeDigitalOutput_8_bit; + OD_obj_array_t o_6401_readAnalogInput_16_bit; + OD_obj_array_t o_6411_writeAnalogOutput_16_bit; +} ODObjs_t; + +static CO_PROGMEM ODObjs_t ODObjs = { + .o_1000_deviceType = { + .dataOrig = &OD_PERSIST_COMM.x1000_deviceType, + .attribute = ODA_SDO_R | ODA_MB, + .dataLength = 4 + }, + .o_1001_errorRegister = { + .dataOrig = &OD_RAM.x1001_errorRegister, + .attribute = ODA_SDO_R | ODA_TRPDO, + .dataLength = 1 + }, + .o_1003_pre_definedErrorField = { + .dataOrig0 = &OD_RAM.x1003_pre_definedErrorField_sub0, + .dataOrig = &OD_RAM.x1003_pre_definedErrorField[0], + .attribute0 = ODA_SDO_RW, + .attribute = ODA_SDO_R | ODA_MB, + .dataElementLength = 4, + .dataElementSizeof = sizeof(uint32_t) + }, + .o_1005_COB_ID_SYNCMessage = { + .dataOrig = &OD_PERSIST_COMM.x1005_COB_ID_SYNCMessage, + .attribute = ODA_SDO_RW | ODA_MB, + .dataLength = 4 + }, + .o_1006_communicationCyclePeriod = { + .dataOrig = &OD_PERSIST_COMM.x1006_communicationCyclePeriod, + .attribute = ODA_SDO_RW | ODA_MB, + .dataLength = 4 + }, + .o_1007_synchronousWindowLength = { + .dataOrig = &OD_PERSIST_COMM.x1007_synchronousWindowLength, + .attribute = ODA_SDO_RW | ODA_MB, + .dataLength = 4 + }, + .o_1010_storeParameters = { + .dataOrig0 = &OD_RAM.x1010_storeParameters_sub0, + .dataOrig = &OD_RAM.x1010_storeParameters[0], + .attribute0 = ODA_SDO_R, + .attribute = ODA_SDO_RW | ODA_MB, + .dataElementLength = 4, + .dataElementSizeof = sizeof(uint32_t) + }, + .o_1011_restoreDefaultParameters = { + .dataOrig0 = &OD_RAM.x1011_restoreDefaultParameters_sub0, + .dataOrig = &OD_RAM.x1011_restoreDefaultParameters[0], + .attribute0 = ODA_SDO_R, + .attribute = ODA_SDO_RW | ODA_MB, + .dataElementLength = 4, + .dataElementSizeof = sizeof(uint32_t) + }, + .o_1012_COB_IDTimeStampObject = { + .dataOrig = &OD_PERSIST_COMM.x1012_COB_IDTimeStampObject, + .attribute = ODA_SDO_RW | ODA_MB, + .dataLength = 4 + }, + .o_1014_COB_ID_EMCY = { + .dataOrig = &OD_PERSIST_COMM.x1014_COB_ID_EMCY, + .attribute = ODA_SDO_RW | ODA_MB, + .dataLength = 4 + }, + .o_1015_inhibitTimeEMCY = { + .dataOrig = &OD_PERSIST_COMM.x1015_inhibitTimeEMCY, + .attribute = ODA_SDO_RW | ODA_MB, + .dataLength = 2 + }, + .o_1016_consumerHeartbeatTime = { + .dataOrig0 = &OD_PERSIST_COMM.x1016_consumerHeartbeatTime_sub0, + .dataOrig = &OD_PERSIST_COMM.x1016_consumerHeartbeatTime[0], + .attribute0 = ODA_SDO_R, + .attribute = ODA_SDO_RW | ODA_MB, + .dataElementLength = 4, + .dataElementSizeof = sizeof(uint32_t) + }, + .o_1017_producerHeartbeatTime = { + .dataOrig = &OD_PERSIST_COMM.x1017_producerHeartbeatTime, + .attribute = ODA_SDO_RW | ODA_MB, + .dataLength = 2 + }, + .o_1018_identity = { + { + .dataOrig = &OD_PERSIST_COMM.x1018_identity.highestSub_indexSupported, + .subIndex = 0, + .attribute = ODA_SDO_R, + .dataLength = 1 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1018_identity.vendor_ID, + .subIndex = 1, + .attribute = ODA_SDO_R | ODA_MB, + .dataLength = 4 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1018_identity.productCode, + .subIndex = 2, + .attribute = ODA_SDO_R | ODA_MB, + .dataLength = 4 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1018_identity.revisionNumber, + .subIndex = 3, + .attribute = ODA_SDO_R | ODA_MB, + .dataLength = 4 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1018_identity.serialNumber, + .subIndex = 4, + .attribute = ODA_SDO_R | ODA_MB, + .dataLength = 4 + } + }, + .o_1019_synchronousCounterOverflowValue = { + .dataOrig = &OD_PERSIST_COMM.x1019_synchronousCounterOverflowValue, + .attribute = ODA_SDO_RW, + .dataLength = 1 + }, + .o_1200_SDOServerParameter = { + { + .dataOrig = &OD_RAM.x1200_SDOServerParameter.highestSub_indexSupported, + .subIndex = 0, + .attribute = ODA_SDO_R, + .dataLength = 1 + }, + { + .dataOrig = &OD_RAM.x1200_SDOServerParameter.COB_IDClientToServerRx, + .subIndex = 1, + .attribute = ODA_SDO_R | ODA_TPDO | ODA_MB, + .dataLength = 4 + }, + { + .dataOrig = &OD_RAM.x1200_SDOServerParameter.COB_IDServerToClientTx, + .subIndex = 2, + .attribute = ODA_SDO_R | ODA_TPDO | ODA_MB, + .dataLength = 4 + } + }, + .o_1280_SDOClientParameter = { + { + .dataOrig = &OD_PERSIST_COMM.x1280_SDOClientParameter.highestSub_indexSupported, + .subIndex = 0, + .attribute = ODA_SDO_R, + .dataLength = 1 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1280_SDOClientParameter.COB_IDClientToServerTx, + .subIndex = 1, + .attribute = ODA_SDO_RW | ODA_TRPDO | ODA_MB, + .dataLength = 4 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1280_SDOClientParameter.COB_IDServerToClientRx, + .subIndex = 2, + .attribute = ODA_SDO_RW | ODA_TRPDO | ODA_MB, + .dataLength = 4 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1280_SDOClientParameter.node_IDOfTheSDOServer, + .subIndex = 3, + .attribute = ODA_SDO_RW, + .dataLength = 1 + } + }, + .o_1400_RPDOCommunicationParameter = { + { + .dataOrig = &OD_PERSIST_COMM.x1400_RPDOCommunicationParameter.highestSub_indexSupported, + .subIndex = 0, + .attribute = ODA_SDO_R, + .dataLength = 1 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1400_RPDOCommunicationParameter.COB_IDUsedByRPDO, + .subIndex = 1, + .attribute = ODA_SDO_RW | ODA_MB, + .dataLength = 4 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1400_RPDOCommunicationParameter.transmissionType, + .subIndex = 2, + .attribute = ODA_SDO_RW, + .dataLength = 1 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1400_RPDOCommunicationParameter.eventTimer, + .subIndex = 5, + .attribute = ODA_SDO_RW | ODA_MB, + .dataLength = 2 + } + }, + .o_1401_RPDOCommunicationParameter = { + { + .dataOrig = &OD_PERSIST_COMM.x1401_RPDOCommunicationParameter.highestSub_indexSupported, + .subIndex = 0, + .attribute = ODA_SDO_R, + .dataLength = 1 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1401_RPDOCommunicationParameter.COB_IDUsedByRPDO, + .subIndex = 1, + .attribute = ODA_SDO_RW | ODA_MB, + .dataLength = 4 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1401_RPDOCommunicationParameter.transmissionType, + .subIndex = 2, + .attribute = ODA_SDO_RW, + .dataLength = 1 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1401_RPDOCommunicationParameter.eventTimer, + .subIndex = 5, + .attribute = ODA_SDO_RW | ODA_MB, + .dataLength = 2 + } + }, + .o_1402_RPDOCommunicationParameter = { + { + .dataOrig = &OD_PERSIST_COMM.x1402_RPDOCommunicationParameter.highestSub_indexSupported, + .subIndex = 0, + .attribute = ODA_SDO_R, + .dataLength = 1 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1402_RPDOCommunicationParameter.COB_IDUsedByRPDO, + .subIndex = 1, + .attribute = ODA_SDO_RW | ODA_MB, + .dataLength = 4 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1402_RPDOCommunicationParameter.transmissionType, + .subIndex = 2, + .attribute = ODA_SDO_RW, + .dataLength = 1 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1402_RPDOCommunicationParameter.eventTimer, + .subIndex = 5, + .attribute = ODA_SDO_RW | ODA_MB, + .dataLength = 2 + } + }, + .o_1403_RPDOCommunicationParameter = { + { + .dataOrig = &OD_PERSIST_COMM.x1403_RPDOCommunicationParameter.highestSub_indexSupported, + .subIndex = 0, + .attribute = ODA_SDO_R, + .dataLength = 1 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1403_RPDOCommunicationParameter.COB_IDUsedByRPDO, + .subIndex = 1, + .attribute = ODA_SDO_RW | ODA_MB, + .dataLength = 4 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1403_RPDOCommunicationParameter.transmissionType, + .subIndex = 2, + .attribute = ODA_SDO_RW, + .dataLength = 1 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1403_RPDOCommunicationParameter.eventTimer, + .subIndex = 5, + .attribute = ODA_SDO_RW | ODA_MB, + .dataLength = 2 + } + }, + .o_1600_RPDOMappingParameter = { + { + .dataOrig = &OD_PERSIST_COMM.x1600_RPDOMappingParameter.numberOfMappedApplicationObjectsInPDO, + .subIndex = 0, + .attribute = ODA_SDO_RW, + .dataLength = 1 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1600_RPDOMappingParameter.applicationObject_1, + .subIndex = 1, + .attribute = ODA_SDO_RW | ODA_MB, + .dataLength = 4 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1600_RPDOMappingParameter.applicationObject_2, + .subIndex = 2, + .attribute = ODA_SDO_RW | ODA_MB, + .dataLength = 4 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1600_RPDOMappingParameter.applicationObject_3, + .subIndex = 3, + .attribute = ODA_SDO_RW | ODA_MB, + .dataLength = 4 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1600_RPDOMappingParameter.applicationObject_4, + .subIndex = 4, + .attribute = ODA_SDO_RW | ODA_MB, + .dataLength = 4 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1600_RPDOMappingParameter.applicationObject_5, + .subIndex = 5, + .attribute = ODA_SDO_RW | ODA_MB, + .dataLength = 4 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1600_RPDOMappingParameter.applicationObject_6, + .subIndex = 6, + .attribute = ODA_SDO_RW | ODA_MB, + .dataLength = 4 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1600_RPDOMappingParameter.applicationObject_7, + .subIndex = 7, + .attribute = ODA_SDO_RW | ODA_MB, + .dataLength = 4 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1600_RPDOMappingParameter.applicationObject_8, + .subIndex = 8, + .attribute = ODA_SDO_RW | ODA_MB, + .dataLength = 4 + } + }, + .o_1601_RPDOMappingParameter = { + { + .dataOrig = &OD_PERSIST_COMM.x1601_RPDOMappingParameter.numberOfMappedApplicationObjectsInPDO, + .subIndex = 0, + .attribute = ODA_SDO_RW, + .dataLength = 1 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1601_RPDOMappingParameter.applicationObject_1, + .subIndex = 1, + .attribute = ODA_SDO_RW | ODA_MB, + .dataLength = 4 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1601_RPDOMappingParameter.applicationObject_2, + .subIndex = 2, + .attribute = ODA_SDO_RW | ODA_MB, + .dataLength = 4 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1601_RPDOMappingParameter.applicationObject_3, + .subIndex = 3, + .attribute = ODA_SDO_RW | ODA_MB, + .dataLength = 4 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1601_RPDOMappingParameter.applicationObject_4, + .subIndex = 4, + .attribute = ODA_SDO_RW | ODA_MB, + .dataLength = 4 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1601_RPDOMappingParameter.applicationObject_5, + .subIndex = 5, + .attribute = ODA_SDO_RW | ODA_MB, + .dataLength = 4 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1601_RPDOMappingParameter.applicationObject_6, + .subIndex = 6, + .attribute = ODA_SDO_RW | ODA_MB, + .dataLength = 4 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1601_RPDOMappingParameter.applicationObject_7, + .subIndex = 7, + .attribute = ODA_SDO_RW | ODA_MB, + .dataLength = 4 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1601_RPDOMappingParameter.applicationObject_8, + .subIndex = 8, + .attribute = ODA_SDO_RW | ODA_MB, + .dataLength = 4 + } + }, + .o_1602_RPDOMappingParameter = { + { + .dataOrig = &OD_PERSIST_COMM.x1602_RPDOMappingParameter.numberOfMappedApplicationObjectsInPDO, + .subIndex = 0, + .attribute = ODA_SDO_RW, + .dataLength = 1 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1602_RPDOMappingParameter.applicationObject_1, + .subIndex = 1, + .attribute = ODA_SDO_RW | ODA_MB, + .dataLength = 4 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1602_RPDOMappingParameter.applicationObject_2, + .subIndex = 2, + .attribute = ODA_SDO_RW | ODA_MB, + .dataLength = 4 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1602_RPDOMappingParameter.applicationObject_3, + .subIndex = 3, + .attribute = ODA_SDO_RW | ODA_MB, + .dataLength = 4 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1602_RPDOMappingParameter.applicationObject_4, + .subIndex = 4, + .attribute = ODA_SDO_RW | ODA_MB, + .dataLength = 4 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1602_RPDOMappingParameter.applicationObject_5, + .subIndex = 5, + .attribute = ODA_SDO_RW | ODA_MB, + .dataLength = 4 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1602_RPDOMappingParameter.applicationObject_6, + .subIndex = 6, + .attribute = ODA_SDO_RW | ODA_MB, + .dataLength = 4 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1602_RPDOMappingParameter.applicationObject_7, + .subIndex = 7, + .attribute = ODA_SDO_RW | ODA_MB, + .dataLength = 4 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1602_RPDOMappingParameter.applicationObject_8, + .subIndex = 8, + .attribute = ODA_SDO_RW | ODA_MB, + .dataLength = 4 + } + }, + .o_1603_RPDOMappingParameter = { + { + .dataOrig = &OD_PERSIST_COMM.x1603_RPDOMappingParameter.numberOfMappedApplicationObjectsInPDO, + .subIndex = 0, + .attribute = ODA_SDO_RW, + .dataLength = 1 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1603_RPDOMappingParameter.applicationObject_1, + .subIndex = 1, + .attribute = ODA_SDO_RW | ODA_MB, + .dataLength = 4 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1603_RPDOMappingParameter.applicationObject_2, + .subIndex = 2, + .attribute = ODA_SDO_RW | ODA_MB, + .dataLength = 4 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1603_RPDOMappingParameter.applicationObject_3, + .subIndex = 3, + .attribute = ODA_SDO_RW | ODA_MB, + .dataLength = 4 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1603_RPDOMappingParameter.applicationObject_4, + .subIndex = 4, + .attribute = ODA_SDO_RW | ODA_MB, + .dataLength = 4 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1603_RPDOMappingParameter.applicationObject_5, + .subIndex = 5, + .attribute = ODA_SDO_RW | ODA_MB, + .dataLength = 4 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1603_RPDOMappingParameter.applicationObject_6, + .subIndex = 6, + .attribute = ODA_SDO_RW | ODA_MB, + .dataLength = 4 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1603_RPDOMappingParameter.applicationObject_7, + .subIndex = 7, + .attribute = ODA_SDO_RW | ODA_MB, + .dataLength = 4 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1603_RPDOMappingParameter.applicationObject_8, + .subIndex = 8, + .attribute = ODA_SDO_RW | ODA_MB, + .dataLength = 4 + } + }, + .o_1800_TPDOCommunicationParameter = { + { + .dataOrig = &OD_PERSIST_COMM.x1800_TPDOCommunicationParameter.highestSub_indexSupported, + .subIndex = 0, + .attribute = ODA_SDO_R, + .dataLength = 1 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1800_TPDOCommunicationParameter.COB_IDUsedByTPDO, + .subIndex = 1, + .attribute = ODA_SDO_RW | ODA_MB, + .dataLength = 4 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1800_TPDOCommunicationParameter.transmissionType, + .subIndex = 2, + .attribute = ODA_SDO_RW, + .dataLength = 1 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1800_TPDOCommunicationParameter.inhibitTime, + .subIndex = 3, + .attribute = ODA_SDO_RW | ODA_MB, + .dataLength = 2 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1800_TPDOCommunicationParameter.eventTimer, + .subIndex = 5, + .attribute = ODA_SDO_RW | ODA_MB, + .dataLength = 2 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1800_TPDOCommunicationParameter.SYNCStartValue, + .subIndex = 6, + .attribute = ODA_SDO_RW, + .dataLength = 1 + } + }, + .o_1801_TPDOCommunicationParameter = { + { + .dataOrig = &OD_PERSIST_COMM.x1801_TPDOCommunicationParameter.highestSub_indexSupported, + .subIndex = 0, + .attribute = ODA_SDO_R, + .dataLength = 1 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1801_TPDOCommunicationParameter.COB_IDUsedByTPDO, + .subIndex = 1, + .attribute = ODA_SDO_RW | ODA_MB, + .dataLength = 4 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1801_TPDOCommunicationParameter.transmissionType, + .subIndex = 2, + .attribute = ODA_SDO_RW, + .dataLength = 1 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1801_TPDOCommunicationParameter.inhibitTime, + .subIndex = 3, + .attribute = ODA_SDO_RW | ODA_MB, + .dataLength = 2 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1801_TPDOCommunicationParameter.eventTimer, + .subIndex = 5, + .attribute = ODA_SDO_RW | ODA_MB, + .dataLength = 2 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1801_TPDOCommunicationParameter.SYNCStartValue, + .subIndex = 6, + .attribute = ODA_SDO_RW, + .dataLength = 1 + } + }, + .o_1802_TPDOCommunicationParameter = { + { + .dataOrig = &OD_PERSIST_COMM.x1802_TPDOCommunicationParameter.highestSub_indexSupported, + .subIndex = 0, + .attribute = ODA_SDO_R, + .dataLength = 1 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1802_TPDOCommunicationParameter.COB_IDUsedByTPDO, + .subIndex = 1, + .attribute = ODA_SDO_RW | ODA_MB, + .dataLength = 4 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1802_TPDOCommunicationParameter.transmissionType, + .subIndex = 2, + .attribute = ODA_SDO_RW, + .dataLength = 1 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1802_TPDOCommunicationParameter.inhibitTime, + .subIndex = 3, + .attribute = ODA_SDO_RW | ODA_MB, + .dataLength = 2 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1802_TPDOCommunicationParameter.eventTimer, + .subIndex = 5, + .attribute = ODA_SDO_RW | ODA_MB, + .dataLength = 2 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1802_TPDOCommunicationParameter.SYNCStartValue, + .subIndex = 6, + .attribute = ODA_SDO_RW, + .dataLength = 1 + } + }, + .o_1803_TPDOCommunicationParameter = { + { + .dataOrig = &OD_PERSIST_COMM.x1803_TPDOCommunicationParameter.highestSub_indexSupported, + .subIndex = 0, + .attribute = ODA_SDO_R, + .dataLength = 1 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1803_TPDOCommunicationParameter.COB_IDUsedByTPDO, + .subIndex = 1, + .attribute = ODA_SDO_RW | ODA_MB, + .dataLength = 4 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1803_TPDOCommunicationParameter.transmissionType, + .subIndex = 2, + .attribute = ODA_SDO_RW, + .dataLength = 1 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1803_TPDOCommunicationParameter.inhibitTime, + .subIndex = 3, + .attribute = ODA_SDO_RW | ODA_MB, + .dataLength = 2 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1803_TPDOCommunicationParameter.eventTimer, + .subIndex = 5, + .attribute = ODA_SDO_RW | ODA_MB, + .dataLength = 2 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1803_TPDOCommunicationParameter.SYNCStartValue, + .subIndex = 6, + .attribute = ODA_SDO_RW, + .dataLength = 1 + } + }, + .o_1A00_TPDOMappingParameter = { + { + .dataOrig = &OD_PERSIST_COMM.x1A00_TPDOMappingParameter.numberOfMappedApplicationObjectsInPDO, + .subIndex = 0, + .attribute = ODA_SDO_RW, + .dataLength = 1 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1A00_TPDOMappingParameter.applicationObject_1, + .subIndex = 1, + .attribute = ODA_SDO_RW | ODA_MB, + .dataLength = 4 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1A00_TPDOMappingParameter.applicationObject_2, + .subIndex = 2, + .attribute = ODA_SDO_RW | ODA_MB, + .dataLength = 4 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1A00_TPDOMappingParameter.applicationObject_3, + .subIndex = 3, + .attribute = ODA_SDO_RW | ODA_MB, + .dataLength = 4 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1A00_TPDOMappingParameter.applicationObject_4, + .subIndex = 4, + .attribute = ODA_SDO_RW | ODA_MB, + .dataLength = 4 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1A00_TPDOMappingParameter.applicationObject_5, + .subIndex = 5, + .attribute = ODA_SDO_RW | ODA_MB, + .dataLength = 4 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1A00_TPDOMappingParameter.applicationObject_6, + .subIndex = 6, + .attribute = ODA_SDO_RW | ODA_MB, + .dataLength = 4 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1A00_TPDOMappingParameter.applicationObject_7, + .subIndex = 7, + .attribute = ODA_SDO_RW | ODA_MB, + .dataLength = 4 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1A00_TPDOMappingParameter.applicationObject_8, + .subIndex = 8, + .attribute = ODA_SDO_RW | ODA_MB, + .dataLength = 4 + } + }, + .o_1A01_TPDOMappingParameter = { + { + .dataOrig = &OD_PERSIST_COMM.x1A01_TPDOMappingParameter.numberOfMappedApplicationObjectsInPDO, + .subIndex = 0, + .attribute = ODA_SDO_RW, + .dataLength = 1 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1A01_TPDOMappingParameter.applicationObject_1, + .subIndex = 1, + .attribute = ODA_SDO_RW | ODA_MB, + .dataLength = 4 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1A01_TPDOMappingParameter.applicationObject_2, + .subIndex = 2, + .attribute = ODA_SDO_RW | ODA_MB, + .dataLength = 4 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1A01_TPDOMappingParameter.applicationObject_3, + .subIndex = 3, + .attribute = ODA_SDO_RW | ODA_MB, + .dataLength = 4 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1A01_TPDOMappingParameter.applicationObject_4, + .subIndex = 4, + .attribute = ODA_SDO_RW | ODA_MB, + .dataLength = 4 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1A01_TPDOMappingParameter.applicationObject_5, + .subIndex = 5, + .attribute = ODA_SDO_RW | ODA_MB, + .dataLength = 4 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1A01_TPDOMappingParameter.applicationObject_6, + .subIndex = 6, + .attribute = ODA_SDO_RW | ODA_MB, + .dataLength = 4 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1A01_TPDOMappingParameter.applicationObject_7, + .subIndex = 7, + .attribute = ODA_SDO_RW | ODA_MB, + .dataLength = 4 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1A01_TPDOMappingParameter.applicationObject_8, + .subIndex = 8, + .attribute = ODA_SDO_RW | ODA_MB, + .dataLength = 4 + } + }, + .o_1A02_TPDOMappingParameter = { + { + .dataOrig = &OD_PERSIST_COMM.x1A02_TPDOMappingParameter.numberOfMappedApplicationObjectsInPDO, + .subIndex = 0, + .attribute = ODA_SDO_RW, + .dataLength = 1 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1A02_TPDOMappingParameter.applicationObject_1, + .subIndex = 1, + .attribute = ODA_SDO_RW | ODA_MB, + .dataLength = 4 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1A02_TPDOMappingParameter.applicationObject_2, + .subIndex = 2, + .attribute = ODA_SDO_RW | ODA_MB, + .dataLength = 4 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1A02_TPDOMappingParameter.applicationObject_3, + .subIndex = 3, + .attribute = ODA_SDO_RW | ODA_MB, + .dataLength = 4 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1A02_TPDOMappingParameter.applicationObject_4, + .subIndex = 4, + .attribute = ODA_SDO_RW | ODA_MB, + .dataLength = 4 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1A02_TPDOMappingParameter.applicationObject_5, + .subIndex = 5, + .attribute = ODA_SDO_RW | ODA_MB, + .dataLength = 4 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1A02_TPDOMappingParameter.applicationObject_6, + .subIndex = 6, + .attribute = ODA_SDO_RW | ODA_MB, + .dataLength = 4 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1A02_TPDOMappingParameter.applicationObject_7, + .subIndex = 7, + .attribute = ODA_SDO_RW | ODA_MB, + .dataLength = 4 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1A02_TPDOMappingParameter.applicationObject_8, + .subIndex = 8, + .attribute = ODA_SDO_RW | ODA_MB, + .dataLength = 4 + } + }, + .o_1A03_TPDOMappingParameter = { + { + .dataOrig = &OD_PERSIST_COMM.x1A03_TPDOMappingParameter.numberOfMappedApplicationObjectsInPDO, + .subIndex = 0, + .attribute = ODA_SDO_RW, + .dataLength = 1 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1A03_TPDOMappingParameter.applicationObject_1, + .subIndex = 1, + .attribute = ODA_SDO_RW | ODA_MB, + .dataLength = 4 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1A03_TPDOMappingParameter.applicationObject_2, + .subIndex = 2, + .attribute = ODA_SDO_RW | ODA_MB, + .dataLength = 4 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1A03_TPDOMappingParameter.applicationObject_3, + .subIndex = 3, + .attribute = ODA_SDO_RW | ODA_MB, + .dataLength = 4 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1A03_TPDOMappingParameter.applicationObject_4, + .subIndex = 4, + .attribute = ODA_SDO_RW | ODA_MB, + .dataLength = 4 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1A03_TPDOMappingParameter.applicationObject_5, + .subIndex = 5, + .attribute = ODA_SDO_RW | ODA_MB, + .dataLength = 4 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1A03_TPDOMappingParameter.applicationObject_6, + .subIndex = 6, + .attribute = ODA_SDO_RW | ODA_MB, + .dataLength = 4 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1A03_TPDOMappingParameter.applicationObject_7, + .subIndex = 7, + .attribute = ODA_SDO_RW | ODA_MB, + .dataLength = 4 + }, + { + .dataOrig = &OD_PERSIST_COMM.x1A03_TPDOMappingParameter.applicationObject_8, + .subIndex = 8, + .attribute = ODA_SDO_RW | ODA_MB, + .dataLength = 4 + } + }, + .o_2100_errorStatusBits = { + .dataOrig = &OD_RAM.x2100_errorStatusBits[0], + .attribute = ODA_SDO_R | ODA_TPDO, + .dataLength = 10 + }, + .o_2105_version = { + { + .dataOrig = &OD_RAM.x2105_version.highestSub_indexSupported, + .subIndex = 0, + .attribute = ODA_SDO_R, + .dataLength = 1 + }, + { + .dataOrig = NULL, + .subIndex = 1, + .attribute = ODA_SDO_R | ODA_STR, + .dataLength = 0 + }, + { + .dataOrig = NULL, + .subIndex = 2, + .attribute = ODA_SDO_R | ODA_STR, + .dataLength = 0 + } + }, + .o_2106_power_onCounter = { + .dataOrig = &OD_PERSIST_TEST_AUTO.x2106_power_onCounter, + .attribute = ODA_SDO_R | ODA_MB, + .dataLength = 4 + }, + .o_2110_variableInt32 = { + .dataOrig0 = &OD_RAM.x2110_variableInt32_sub0, + .dataOrig = &OD_RAM.x2110_variableInt32[0], + .attribute0 = ODA_SDO_R | ODA_TRPDO, + .attribute = ODA_SDO_RW | ODA_TRPDO | ODA_MB, + .dataElementLength = 4, + .dataElementSizeof = sizeof(int32_t) + }, + .o_2111_variableInt32_save = { + .dataOrig0 = &OD_PERSIST_TEST.x2111_variableInt32_save_sub0, + .dataOrig = &OD_PERSIST_TEST.x2111_variableInt32_save[0], + .attribute0 = ODA_SDO_R | ODA_TRPDO, + .attribute = ODA_SDO_RW | ODA_MB, + .dataElementLength = 4, + .dataElementSizeof = sizeof(int32_t) + }, + .o_2112_variableNV_Int32_autoSave = { + .dataOrig0 = &OD_PERSIST_TEST_AUTO.x2112_variableNV_Int32_autoSave_sub0, + .dataOrig = &OD_PERSIST_TEST_AUTO.x2112_variableNV_Int32_autoSave[0], + .attribute0 = ODA_SDO_R | ODA_TRPDO, + .attribute = ODA_SDO_RW | ODA_MB, + .dataElementLength = 4, + .dataElementSizeof = sizeof(int32_t) + }, + .o_2120_testingVariables = { + { + .dataOrig = &OD_PERSIST_TEST.x2120_testingVariables.highestSub_indexSupported, + .subIndex = 0, + .attribute = ODA_SDO_R, + .dataLength = 1 + }, + { + .dataOrig = &OD_PERSIST_TEST.x2120_testingVariables.I64, + .subIndex = 1, + .attribute = ODA_SDO_RW | ODA_TRPDO | ODA_MB, + .dataLength = 8 + }, + { + .dataOrig = &OD_PERSIST_TEST.x2120_testingVariables.U64, + .subIndex = 2, + .attribute = ODA_SDO_RW | ODA_TRPDO | ODA_MB, + .dataLength = 8 + }, + { + .dataOrig = &OD_PERSIST_TEST.x2120_testingVariables.R32, + .subIndex = 3, + .attribute = ODA_SDO_RW | ODA_TRPDO | ODA_MB, + .dataLength = 4 + }, + { + .dataOrig = &OD_PERSIST_TEST.x2120_testingVariables.R64, + .subIndex = 4, + .attribute = ODA_SDO_RW | ODA_TRPDO | ODA_MB, + .dataLength = 8 + }, + { + .dataOrig = NULL, + .subIndex = 5, + .attribute = ODA_SDO_R | ODA_TPDO | ODA_MB, + .dataLength = 8 + }, + { + .dataOrig = &OD_PERSIST_TEST.x2120_testingVariables.stringShort[0], + .subIndex = 6, + .attribute = ODA_SDO_RW | ODA_STR, + .dataLength = 3 + }, + { + .dataOrig = &OD_PERSIST_TEST.x2120_testingVariables.stringLong[0], + .subIndex = 7, + .attribute = ODA_SDO_RW | ODA_STR, + .dataLength = 1000 + }, + { + .dataOrig = &OD_PERSIST_TEST.x2120_testingVariables.octetString[0], + .subIndex = 8, + .attribute = ODA_SDO_RW, + .dataLength = 10 + }, + { + .dataOrig = &OD_PERSIST_TEST.x2120_testingVariables.parameterWithDefaultValue, + .subIndex = 9, + .attribute = ODA_SDO_RW | ODA_MB, + .dataLength = 2 + }, + { + .dataOrig = NULL, + .subIndex = 10, + .attribute = ODA_SDO_RW, + .dataLength = 0 + }, + { + .dataOrig = &OD_PERSIST_TEST.x2120_testingVariables.domainFileNameRead[0], + .subIndex = 11, + .attribute = ODA_SDO_RW | ODA_STR, + .dataLength = 100 + }, + { + .dataOrig = &OD_PERSIST_TEST.x2120_testingVariables.domainFileNameWrite[0], + .subIndex = 12, + .attribute = ODA_SDO_RW | ODA_STR, + .dataLength = 100 + } + }, + .o_6000_readDigitalInput_8_bit = { + .dataOrig0 = &OD_RAM.x6000_readDigitalInput_8_bit_sub0, + .dataOrig = &OD_RAM.x6000_readDigitalInput_8_bit[0], + .attribute0 = ODA_SDO_R, + .attribute = ODA_SDO_R | ODA_TPDO, + .dataElementLength = 1, + .dataElementSizeof = sizeof(uint8_t) + }, + .o_6200_writeDigitalOutput_8_bit = { + .dataOrig0 = &OD_RAM.x6200_writeDigitalOutput_8_bit_sub0, + .dataOrig = &OD_RAM.x6200_writeDigitalOutput_8_bit[0], + .attribute0 = ODA_SDO_R | ODA_TRPDO, + .attribute = ODA_SDO_RW | ODA_TRPDO, + .dataElementLength = 1, + .dataElementSizeof = sizeof(uint8_t) + }, + .o_6401_readAnalogInput_16_bit = { + .dataOrig0 = &OD_RAM.x6401_readAnalogInput_16_bit_sub0, + .dataOrig = &OD_RAM.x6401_readAnalogInput_16_bit[0], + .attribute0 = ODA_SDO_R | ODA_TRPDO, + .attribute = ODA_SDO_R | ODA_TRPDO | ODA_MB, + .dataElementLength = 2, + .dataElementSizeof = sizeof(int16_t) + }, + .o_6411_writeAnalogOutput_16_bit = { + .dataOrig0 = &OD_RAM.x6411_writeAnalogOutput_16_bit_sub0, + .dataOrig = &OD_RAM.x6411_writeAnalogOutput_16_bit[0], + .attribute0 = ODA_SDO_R | ODA_TRPDO, + .attribute = ODA_SDO_RW | ODA_TRPDO | ODA_MB, + .dataElementLength = 2, + .dataElementSizeof = sizeof(int16_t) + } +}; + + +/******************************************************************************* + Object dictionary +*******************************************************************************/ +static OD_entry_t ODList[] = { + {0x1000, 0x01, ODT_VAR, &ODObjs.o_1000_deviceType, NULL}, + {0x1001, 0x01, ODT_VAR, &ODObjs.o_1001_errorRegister, NULL}, + {0x1003, 0x09, ODT_ARR, &ODObjs.o_1003_pre_definedErrorField, NULL}, + {0x1005, 0x01, ODT_VAR, &ODObjs.o_1005_COB_ID_SYNCMessage, NULL}, + {0x1006, 0x01, ODT_VAR, &ODObjs.o_1006_communicationCyclePeriod, NULL}, + {0x1007, 0x01, ODT_VAR, &ODObjs.o_1007_synchronousWindowLength, NULL}, + {0x1010, 0x07, ODT_ARR, &ODObjs.o_1010_storeParameters, NULL}, + {0x1011, 0x07, ODT_ARR, &ODObjs.o_1011_restoreDefaultParameters, NULL}, + {0x1012, 0x01, ODT_VAR, &ODObjs.o_1012_COB_IDTimeStampObject, NULL}, + {0x1014, 0x01, ODT_VAR, &ODObjs.o_1014_COB_ID_EMCY, NULL}, + {0x1015, 0x01, ODT_VAR, &ODObjs.o_1015_inhibitTimeEMCY, NULL}, + {0x1016, 0x09, ODT_ARR, &ODObjs.o_1016_consumerHeartbeatTime, NULL}, + {0x1017, 0x01, ODT_VAR, &ODObjs.o_1017_producerHeartbeatTime, NULL}, + {0x1018, 0x05, ODT_REC, &ODObjs.o_1018_identity, NULL}, + {0x1019, 0x01, ODT_VAR, &ODObjs.o_1019_synchronousCounterOverflowValue, NULL}, + {0x1200, 0x03, ODT_REC, &ODObjs.o_1200_SDOServerParameter, NULL}, + {0x1280, 0x04, ODT_REC, &ODObjs.o_1280_SDOClientParameter, NULL}, + {0x1400, 0x04, ODT_REC, &ODObjs.o_1400_RPDOCommunicationParameter, NULL}, + {0x1401, 0x04, ODT_REC, &ODObjs.o_1401_RPDOCommunicationParameter, NULL}, + {0x1402, 0x04, ODT_REC, &ODObjs.o_1402_RPDOCommunicationParameter, NULL}, + {0x1403, 0x04, ODT_REC, &ODObjs.o_1403_RPDOCommunicationParameter, NULL}, + {0x1600, 0x09, ODT_REC, &ODObjs.o_1600_RPDOMappingParameter, NULL}, + {0x1601, 0x09, ODT_REC, &ODObjs.o_1601_RPDOMappingParameter, NULL}, + {0x1602, 0x09, ODT_REC, &ODObjs.o_1602_RPDOMappingParameter, NULL}, + {0x1603, 0x09, ODT_REC, &ODObjs.o_1603_RPDOMappingParameter, NULL}, + {0x1800, 0x06, ODT_REC, &ODObjs.o_1800_TPDOCommunicationParameter, NULL}, + {0x1801, 0x06, ODT_REC, &ODObjs.o_1801_TPDOCommunicationParameter, NULL}, + {0x1802, 0x06, ODT_REC, &ODObjs.o_1802_TPDOCommunicationParameter, NULL}, + {0x1803, 0x06, ODT_REC, &ODObjs.o_1803_TPDOCommunicationParameter, NULL}, + {0x1A00, 0x09, ODT_REC, &ODObjs.o_1A00_TPDOMappingParameter, NULL}, + {0x1A01, 0x09, ODT_REC, &ODObjs.o_1A01_TPDOMappingParameter, NULL}, + {0x1A02, 0x09, ODT_REC, &ODObjs.o_1A02_TPDOMappingParameter, NULL}, + {0x1A03, 0x09, ODT_REC, &ODObjs.o_1A03_TPDOMappingParameter, NULL}, + {0x2100, 0x01, ODT_VAR, &ODObjs.o_2100_errorStatusBits, NULL}, + {0x2105, 0x03, ODT_REC, &ODObjs.o_2105_version, NULL}, + {0x2106, 0x01, ODT_VAR, &ODObjs.o_2106_power_onCounter, NULL}, + {0x2110, 0x11, ODT_ARR, &ODObjs.o_2110_variableInt32, NULL}, + {0x2111, 0x11, ODT_ARR, &ODObjs.o_2111_variableInt32_save, NULL}, + {0x2112, 0x11, ODT_ARR, &ODObjs.o_2112_variableNV_Int32_autoSave, NULL}, + {0x2120, 0x0D, ODT_REC, &ODObjs.o_2120_testingVariables, NULL}, + {0x6000, 0x09, ODT_ARR, &ODObjs.o_6000_readDigitalInput_8_bit, NULL}, + {0x6200, 0x09, ODT_ARR, &ODObjs.o_6200_writeDigitalOutput_8_bit, NULL}, + {0x6401, 0x11, ODT_ARR, &ODObjs.o_6401_readAnalogInput_16_bit, NULL}, + {0x6411, 0x09, ODT_ARR, &ODObjs.o_6411_writeAnalogOutput_16_bit, NULL}, + {0x0000, 0x00, 0, NULL, NULL} +}; + +static OD_t _OD = { + (sizeof(ODList) / sizeof(ODList[0])) - 1, + &ODList[0] +}; + +OD_t *OD = &_OD; diff --git a/Devices/Libraries/Systems/CANopenSocket/examples/basicDevice/OD.h b/Devices/Libraries/Systems/CANopenSocket/examples/basicDevice/OD.h new file mode 100755 index 0000000..1df4cdd --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/examples/basicDevice/OD.h @@ -0,0 +1,383 @@ +/******************************************************************************* + CANopen Object Dictionary definition for CANopenNode V4 + + This file was automatically generated with + libedssharp Object Dictionary Editor v0.8-122-g6c02323 + + https://github.com/CANopenNode/CANopenNode + https://github.com/robincornelius/libedssharp + + DON'T EDIT THIS FILE MANUALLY !!!! +******************************************************************************** + + File info: + File Names: OD.h; OD.c + Project File: basicDevice.xdd + File Version: 1 + + Created: 28. 11. 2020 12:37:54 + Created By: Janez Paternoster + Modified: 16. 03. 2021 19:31:59 + Modified By: Janez Paternoster + + Device Info: + Vendor Name: + Vendor ID: + Product Name: Basic device + Product ID: + + Description: Basic CANopen device with example usage. +*******************************************************************************/ + +#ifndef OD_H +#define OD_H +/******************************************************************************* + Counters of OD objects +*******************************************************************************/ +#define OD_CNT_NMT 1 +#define OD_CNT_EM 1 +#define OD_CNT_SYNC 1 +#define OD_CNT_SYNC_PROD 1 +#define OD_CNT_STORAGE 1 +#define OD_CNT_TIME 1 +#define OD_CNT_EM_PROD 1 +#define OD_CNT_HB_CONS 1 +#define OD_CNT_HB_PROD 1 +#define OD_CNT_SDO_SRV 1 +#define OD_CNT_SDO_CLI 1 +#define OD_CNT_RPDO 4 +#define OD_CNT_TPDO 4 + + +/******************************************************************************* + OD data declaration of all groups +*******************************************************************************/ +typedef struct { + uint32_t x1000_deviceType; + uint32_t x1005_COB_ID_SYNCMessage; + uint32_t x1006_communicationCyclePeriod; + uint32_t x1007_synchronousWindowLength; + uint32_t x1012_COB_IDTimeStampObject; + uint32_t x1014_COB_ID_EMCY; + uint16_t x1015_inhibitTimeEMCY; + uint8_t x1016_consumerHeartbeatTime_sub0; + uint32_t x1016_consumerHeartbeatTime[8]; + uint16_t x1017_producerHeartbeatTime; + struct { + uint8_t highestSub_indexSupported; + uint32_t vendor_ID; + uint32_t productCode; + uint32_t revisionNumber; + uint32_t serialNumber; + } x1018_identity; + uint8_t x1019_synchronousCounterOverflowValue; + struct { + uint8_t highestSub_indexSupported; + uint32_t COB_IDClientToServerTx; + uint32_t COB_IDServerToClientRx; + uint8_t node_IDOfTheSDOServer; + } x1280_SDOClientParameter; + struct { + uint8_t highestSub_indexSupported; + uint32_t COB_IDUsedByRPDO; + uint8_t transmissionType; + uint16_t eventTimer; + } x1400_RPDOCommunicationParameter; + struct { + uint8_t highestSub_indexSupported; + uint32_t COB_IDUsedByRPDO; + uint8_t transmissionType; + uint16_t eventTimer; + } x1401_RPDOCommunicationParameter; + struct { + uint8_t highestSub_indexSupported; + uint32_t COB_IDUsedByRPDO; + uint8_t transmissionType; + uint16_t eventTimer; + } x1402_RPDOCommunicationParameter; + struct { + uint8_t highestSub_indexSupported; + uint32_t COB_IDUsedByRPDO; + uint8_t transmissionType; + uint16_t eventTimer; + } x1403_RPDOCommunicationParameter; + struct { + uint8_t numberOfMappedApplicationObjectsInPDO; + uint32_t applicationObject_1; + uint32_t applicationObject_2; + uint32_t applicationObject_3; + uint32_t applicationObject_4; + uint32_t applicationObject_5; + uint32_t applicationObject_6; + uint32_t applicationObject_7; + uint32_t applicationObject_8; + } x1600_RPDOMappingParameter; + struct { + uint8_t numberOfMappedApplicationObjectsInPDO; + uint32_t applicationObject_1; + uint32_t applicationObject_2; + uint32_t applicationObject_3; + uint32_t applicationObject_4; + uint32_t applicationObject_5; + uint32_t applicationObject_6; + uint32_t applicationObject_7; + uint32_t applicationObject_8; + } x1601_RPDOMappingParameter; + struct { + uint8_t numberOfMappedApplicationObjectsInPDO; + uint32_t applicationObject_1; + uint32_t applicationObject_2; + uint32_t applicationObject_3; + uint32_t applicationObject_4; + uint32_t applicationObject_5; + uint32_t applicationObject_6; + uint32_t applicationObject_7; + uint32_t applicationObject_8; + } x1602_RPDOMappingParameter; + struct { + uint8_t numberOfMappedApplicationObjectsInPDO; + uint32_t applicationObject_1; + uint32_t applicationObject_2; + uint32_t applicationObject_3; + uint32_t applicationObject_4; + uint32_t applicationObject_5; + uint32_t applicationObject_6; + uint32_t applicationObject_7; + uint32_t applicationObject_8; + } x1603_RPDOMappingParameter; + struct { + uint8_t highestSub_indexSupported; + uint32_t COB_IDUsedByTPDO; + uint8_t transmissionType; + uint16_t inhibitTime; + uint16_t eventTimer; + uint8_t SYNCStartValue; + } x1800_TPDOCommunicationParameter; + struct { + uint8_t highestSub_indexSupported; + uint32_t COB_IDUsedByTPDO; + uint8_t transmissionType; + uint16_t inhibitTime; + uint16_t eventTimer; + uint8_t SYNCStartValue; + } x1801_TPDOCommunicationParameter; + struct { + uint8_t highestSub_indexSupported; + uint32_t COB_IDUsedByTPDO; + uint8_t transmissionType; + uint16_t inhibitTime; + uint16_t eventTimer; + uint8_t SYNCStartValue; + } x1802_TPDOCommunicationParameter; + struct { + uint8_t highestSub_indexSupported; + uint32_t COB_IDUsedByTPDO; + uint8_t transmissionType; + uint16_t inhibitTime; + uint16_t eventTimer; + uint8_t SYNCStartValue; + } x1803_TPDOCommunicationParameter; + struct { + uint8_t numberOfMappedApplicationObjectsInPDO; + uint32_t applicationObject_1; + uint32_t applicationObject_2; + uint32_t applicationObject_3; + uint32_t applicationObject_4; + uint32_t applicationObject_5; + uint32_t applicationObject_6; + uint32_t applicationObject_7; + uint32_t applicationObject_8; + } x1A00_TPDOMappingParameter; + struct { + uint8_t numberOfMappedApplicationObjectsInPDO; + uint32_t applicationObject_1; + uint32_t applicationObject_2; + uint32_t applicationObject_3; + uint32_t applicationObject_4; + uint32_t applicationObject_5; + uint32_t applicationObject_6; + uint32_t applicationObject_7; + uint32_t applicationObject_8; + } x1A01_TPDOMappingParameter; + struct { + uint8_t numberOfMappedApplicationObjectsInPDO; + uint32_t applicationObject_1; + uint32_t applicationObject_2; + uint32_t applicationObject_3; + uint32_t applicationObject_4; + uint32_t applicationObject_5; + uint32_t applicationObject_6; + uint32_t applicationObject_7; + uint32_t applicationObject_8; + } x1A02_TPDOMappingParameter; + struct { + uint8_t numberOfMappedApplicationObjectsInPDO; + uint32_t applicationObject_1; + uint32_t applicationObject_2; + uint32_t applicationObject_3; + uint32_t applicationObject_4; + uint32_t applicationObject_5; + uint32_t applicationObject_6; + uint32_t applicationObject_7; + uint32_t applicationObject_8; + } x1A03_TPDOMappingParameter; +} OD_PERSIST_COMM_t; + +typedef struct { + uint8_t x1001_errorRegister; + uint8_t x1003_pre_definedErrorField_sub0; + uint32_t x1003_pre_definedErrorField[8]; + uint8_t x1010_storeParameters_sub0; + uint32_t x1010_storeParameters[6]; + uint8_t x1011_restoreDefaultParameters_sub0; + uint32_t x1011_restoreDefaultParameters[6]; + struct { + uint8_t highestSub_indexSupported; + uint32_t COB_IDClientToServerRx; + uint32_t COB_IDServerToClientTx; + } x1200_SDOServerParameter; + uint8_t x2100_errorStatusBits[10]; + struct { + uint8_t highestSub_indexSupported; + } x2105_version; + uint8_t x2110_variableInt32_sub0; + int32_t x2110_variableInt32[16]; + uint8_t x6000_readDigitalInput_8_bit_sub0; + uint8_t x6000_readDigitalInput_8_bit[8]; + uint8_t x6200_writeDigitalOutput_8_bit_sub0; + uint8_t x6200_writeDigitalOutput_8_bit[8]; + uint8_t x6401_readAnalogInput_16_bit_sub0; + int16_t x6401_readAnalogInput_16_bit[16]; + uint8_t x6411_writeAnalogOutput_16_bit_sub0; + int16_t x6411_writeAnalogOutput_16_bit[8]; +} OD_RAM_t; + +typedef struct { + uint32_t x2106_power_onCounter; + uint8_t x2112_variableNV_Int32_autoSave_sub0; + int32_t x2112_variableNV_Int32_autoSave[16]; +} OD_PERSIST_TEST_AUTO_t; + +typedef struct { + uint8_t x2111_variableInt32_save_sub0; + int32_t x2111_variableInt32_save[16]; + struct { + uint8_t highestSub_indexSupported; + int64_t I64; + uint64_t U64; + float32_t R32; + float64_t R64; + char stringShort[4]; + char stringLong[1001]; + uint8_t octetString[10]; + uint16_t parameterWithDefaultValue; + char domainFileNameRead[101]; + char domainFileNameWrite[101]; + } x2120_testingVariables; +} OD_PERSIST_TEST_t; + +extern OD_PERSIST_COMM_t OD_PERSIST_COMM; +extern OD_RAM_t OD_RAM; +extern OD_PERSIST_TEST_AUTO_t OD_PERSIST_TEST_AUTO; +extern OD_PERSIST_TEST_t OD_PERSIST_TEST; +extern OD_t *OD; + + +/******************************************************************************* + Object dictionary entries - shortcuts +*******************************************************************************/ +#define OD_ENTRY_H1000 &OD->list[0] +#define OD_ENTRY_H1001 &OD->list[1] +#define OD_ENTRY_H1003 &OD->list[2] +#define OD_ENTRY_H1005 &OD->list[3] +#define OD_ENTRY_H1006 &OD->list[4] +#define OD_ENTRY_H1007 &OD->list[5] +#define OD_ENTRY_H1010 &OD->list[6] +#define OD_ENTRY_H1011 &OD->list[7] +#define OD_ENTRY_H1012 &OD->list[8] +#define OD_ENTRY_H1014 &OD->list[9] +#define OD_ENTRY_H1015 &OD->list[10] +#define OD_ENTRY_H1016 &OD->list[11] +#define OD_ENTRY_H1017 &OD->list[12] +#define OD_ENTRY_H1018 &OD->list[13] +#define OD_ENTRY_H1019 &OD->list[14] +#define OD_ENTRY_H1200 &OD->list[15] +#define OD_ENTRY_H1280 &OD->list[16] +#define OD_ENTRY_H1400 &OD->list[17] +#define OD_ENTRY_H1401 &OD->list[18] +#define OD_ENTRY_H1402 &OD->list[19] +#define OD_ENTRY_H1403 &OD->list[20] +#define OD_ENTRY_H1600 &OD->list[21] +#define OD_ENTRY_H1601 &OD->list[22] +#define OD_ENTRY_H1602 &OD->list[23] +#define OD_ENTRY_H1603 &OD->list[24] +#define OD_ENTRY_H1800 &OD->list[25] +#define OD_ENTRY_H1801 &OD->list[26] +#define OD_ENTRY_H1802 &OD->list[27] +#define OD_ENTRY_H1803 &OD->list[28] +#define OD_ENTRY_H1A00 &OD->list[29] +#define OD_ENTRY_H1A01 &OD->list[30] +#define OD_ENTRY_H1A02 &OD->list[31] +#define OD_ENTRY_H1A03 &OD->list[32] +#define OD_ENTRY_H2100 &OD->list[33] +#define OD_ENTRY_H2105 &OD->list[34] +#define OD_ENTRY_H2106 &OD->list[35] +#define OD_ENTRY_H2110 &OD->list[36] +#define OD_ENTRY_H2111 &OD->list[37] +#define OD_ENTRY_H2112 &OD->list[38] +#define OD_ENTRY_H2120 &OD->list[39] +#define OD_ENTRY_H6000 &OD->list[40] +#define OD_ENTRY_H6200 &OD->list[41] +#define OD_ENTRY_H6401 &OD->list[42] +#define OD_ENTRY_H6411 &OD->list[43] + + +/******************************************************************************* + Object dictionary entries - shortcuts with names +*******************************************************************************/ +#define OD_ENTRY_H1000_deviceType &OD->list[0] +#define OD_ENTRY_H1001_errorRegister &OD->list[1] +#define OD_ENTRY_H1003_pre_definedErrorField &OD->list[2] +#define OD_ENTRY_H1005_COB_ID_SYNCMessage &OD->list[3] +#define OD_ENTRY_H1006_communicationCyclePeriod &OD->list[4] +#define OD_ENTRY_H1007_synchronousWindowLength &OD->list[5] +#define OD_ENTRY_H1010_storeParameters &OD->list[6] +#define OD_ENTRY_H1011_restoreDefaultParameters &OD->list[7] +#define OD_ENTRY_H1012_COB_IDTimeStampObject &OD->list[8] +#define OD_ENTRY_H1014_COB_ID_EMCY &OD->list[9] +#define OD_ENTRY_H1015_inhibitTimeEMCY &OD->list[10] +#define OD_ENTRY_H1016_consumerHeartbeatTime &OD->list[11] +#define OD_ENTRY_H1017_producerHeartbeatTime &OD->list[12] +#define OD_ENTRY_H1018_identity &OD->list[13] +#define OD_ENTRY_H1019_synchronousCounterOverflowValue &OD->list[14] +#define OD_ENTRY_H1200_SDOServerParameter &OD->list[15] +#define OD_ENTRY_H1280_SDOClientParameter &OD->list[16] +#define OD_ENTRY_H1400_RPDOCommunicationParameter &OD->list[17] +#define OD_ENTRY_H1401_RPDOCommunicationParameter &OD->list[18] +#define OD_ENTRY_H1402_RPDOCommunicationParameter &OD->list[19] +#define OD_ENTRY_H1403_RPDOCommunicationParameter &OD->list[20] +#define OD_ENTRY_H1600_RPDOMappingParameter &OD->list[21] +#define OD_ENTRY_H1601_RPDOMappingParameter &OD->list[22] +#define OD_ENTRY_H1602_RPDOMappingParameter &OD->list[23] +#define OD_ENTRY_H1603_RPDOMappingParameter &OD->list[24] +#define OD_ENTRY_H1800_TPDOCommunicationParameter &OD->list[25] +#define OD_ENTRY_H1801_TPDOCommunicationParameter &OD->list[26] +#define OD_ENTRY_H1802_TPDOCommunicationParameter &OD->list[27] +#define OD_ENTRY_H1803_TPDOCommunicationParameter &OD->list[28] +#define OD_ENTRY_H1A00_TPDOMappingParameter &OD->list[29] +#define OD_ENTRY_H1A01_TPDOMappingParameter &OD->list[30] +#define OD_ENTRY_H1A02_TPDOMappingParameter &OD->list[31] +#define OD_ENTRY_H1A03_TPDOMappingParameter &OD->list[32] +#define OD_ENTRY_H2100_errorStatusBits &OD->list[33] +#define OD_ENTRY_H2105_version &OD->list[34] +#define OD_ENTRY_H2106_power_onCounter &OD->list[35] +#define OD_ENTRY_H2110_variableInt32 &OD->list[36] +#define OD_ENTRY_H2111_variableInt32_save &OD->list[37] +#define OD_ENTRY_H2112_variableNV_Int32_autoSave &OD->list[38] +#define OD_ENTRY_H2120_testingVariables &OD->list[39] +#define OD_ENTRY_H6000_readDigitalInput_8_bit &OD->list[40] +#define OD_ENTRY_H6200_writeDigitalOutput_8_bit &OD->list[41] +#define OD_ENTRY_H6401_readAnalogInput_16_bit &OD->list[42] +#define OD_ENTRY_H6411_writeAnalogOutput_16_bit &OD->list[43] + +#endif /* OD_H */ diff --git a/Devices/Libraries/Systems/CANopenSocket/examples/basicDevice/README.md b/Devices/Libraries/Systems/CANopenSocket/examples/basicDevice/README.md new file mode 100755 index 0000000..ea6ad55 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/examples/basicDevice/README.md @@ -0,0 +1,358 @@ +CANopen basicDevice Example +=========================== + +Example of basic CANopen device, with quite rich functionality. It is based on [CANopenNode](https://github.com/CANopenNode/CANopenNode), which is free and open source CANopen Stack, based on ([CiA301](http://can-cia.org/standardization/technical-documents)) standard. + +Example contains object dictionary with most common communication parameters and some additional manufacturer specific and device profile parameters. See picture below or complete OD documentation in [basicDevice.md](basicDevice.md). Note also project file `basicDevice.xdd`, which can be opened and edited with EDSEditor.exe (Linux or Windows application). EDSEditor can export other files, including OD.h and OD.c source files for this example. `basicDevice.xdd` and `basicDevice.eds` are standard CANopen device description files and can be opened also with other CANopen tools. + +![EDSEditor](EDSEditor.png) + + +Example program +--------------- + +### Makefile +It includes most common CANopen files and options for compilation. + +It also generates `CO_version.h` file with information about git version number of CANopenNode and CANopenSocket, for example: + + #define CO_VERSION_CANOPENNODE "v4.0-12-g09bf926-dirty" + #define CO_VERSION_APPLICATION "v1.2-20-g94add15-dirty" + +This information is available from OD entry 0x2105, 01: + + ./cocomm "4 r 0x2105 1 vs" + [1] "v4.0-12-g09bf926-dirty" + +"v4.0" is latest tag name, "12" is number of commits above the tag, "g" is git, "09bf926" is commit number and optional "dirty" means, git repository has uncommited changes. + +Makefile also defines macro `CO_DRIVER_CUSTOM`, which includes `CO_driver_custom.h` into the foundation of all source files. + +#### CANopenNode program flow + +CANopenNode driver for Linux is written in a way, that program execution is triggered by different events, like reception of CAN message, one of the many timers expiration, other file descriptor based events, etc. All CANopenNode functions are non-blocking. After event occurs, all mainline and/or realtime functions are processed. In simpler, microcontroller based applications, mainline functions are executing in infinite loop and realtime functions are executed precisely each timer interval, 1ms typically. + +CANopenNode driver for Linux offers two options, configurable with makefile: +1. Whole program runs in single thread. This is default for `canopend`. This uses less system resources. +2. Mainline functions are executing in primary thread and realtime functions are executed in own thread, triggered by precise timer. This way it is easier to achieve realtime operation and may be easier to implement a controller with access to peripherals, like digital input/output pins. This is default for `basicDevice`. + + +### CO_application.h and CO_application.c + +Those files contains five functions, which are called from `main()` function inside `CANopenNode/socketCAN/CO_main_basic.c` file. They are central part of our `basicDevice` application program: + + CO_ReturnError_t app_programStart(bool_t CANopenConfigured, uint16_t *errinfo); + void app_communicationReset(bool_t CANopenConfigured); + void app_programEnd(); + void app_programAsync(bool_t CANopenConfigured, uint32_t timer1usDiff); + void app_program1ms(bool_t CANopenConfigured, uint32_t timer1usDiff); + + +### testingVariables.h and testingVariables.c + +Those files contains example usage of OD entry "Testing variables" with multiple different sub entries. + +Entry has IO extension enabled. This means that some entries are accessed with custom read/write functions. See source files and examples below for details. + + +Testing with Linux +------------------ + +You should successfully finish `CANopenNode/doc/gettingStarted.md`, before continuing with this example. Here more advanced and more usable command line application, `cocomm`, will be used. + +### Some background about communication paths, when using cocomm + +`cocomm` is a small command line program, which establishes socket connection with `canopend`. It sends standardized CANopen commands (CiA309-3) to gateway and prints the responses to stdout and stderr. + +1. `canopend` serves a socket connection on `/tmp/CO_command_socket` address. This is local Unix socket, TCP socket can be used also. `canopend` is pure CANopen device with commander functionalities and gateway. It listens for socket connections. +2. When run, `cocomm` tries to connect to `/tmp/CO_command_socket` address (this is default setting). `canopend` accepts the connection. +3. `cocomm` writes the specified ascii command to established socket connection, for example `[1] 4 read 0x1017 0 u16`. +4. Gateway in `canopend` receives the command and decodes it. `read` commands goes internally into `CO_SDOclientUploadInitiate()` and then command is processed with multiple `CO_SDOclientUpload()` function calls. +5. `CO_SDOclientUpload()` now sends a CAN message to our `basicDevice`. (CAN interface in Linux is implemented with CAN sockets. This is the third type of sockets mentioned here.) However, in our example `basicDevice` receives SDO request, asking the value of the variable, located in Object Dictionary at index 0x1017, subindex 0. +6. Our `basicDevice` receives CAN message with CAN ID=0x604. It determines SDO request, so `CO_SDOserver_process()` function gets the message. Function gets the value from internal Object Dictionary and sends the CAN response with CAN ID=0x584. Those messages can be seen in candump terminal. And it is not necessary to understand the details of SDO communication, it may be quite complex. +7. `canopend` receives the CAN message, `CO_SDOclientUpload()` decodes it and sends binary value to gateway. +8. Gateway in `canopend` translates binary value to asciiValue, unsigned16 in our example. It prepares the response, in our case `[1] ` + asciiValue + `\r\n`. Then writes the response text back to `/tmp/CO_command_socket`. +9. `cocomm` reads the response text from local socket and prints it partly to stderr (`[1] \r\n`) and partly to stdout (asciiValue). +10. If there are more commands, step 3 is repeated. Otherwise `cocomm` closes the socket connection and exits. + + +### Testing variables + +#### Prepare the environment +We will use four Linux terminals: + + # First terminal: candump + sudo modprobe vcan + sudo ip link add dev vcan0 type vcan + sudo ip link set up vcan0 + candump -td -a vcan0 + + # second terminal: basicDevice + cd examples/basicDevice + make + ./basicDevice -i4 vcan0 + + # third terminal: canopend (gateway) + cd CANopenNode + make + ./canopend vcan0 -i1 -c "local-/tmp/CO_command_socket" + + # fourth terminal: cocomm (connection with gateway) + cd cocomm + make + +#### basic cocomm usage +It is very similar as in `CANopenNode/doc/gettingStarted.md`, but actually easier. + + ./cocomm --help + ./cocomm "help" + ./cocomm "help datatype" + ./cocomm "4 r 0x1017 0 u16" + ./cocomm "1 r 0x1017 0 u16" "4 r 0x1017 0 u16" + +`cocomm` program can be copied to standard location of executables, so we can run it from any directory, without leading `./`: + + sudo cp cocomm /usr/bin/cocomm + +Parameters to program can be set by program arguments, as described in `cocomm --help`, and can also be changed by environmental variables. For example, to change default socket path in for all next `cocomm` commands in current terminal, type: + + export cocomm_socket="some other path than /tmp/CO_command_socket" + +Commands can be also written into a file, for example create a `commands.txt` file, and for its content enter the commands: + + [1] 1 start + [2] 4 start + +Then make `cocomm` use that file: + + $ cocomm -f commands.txt + [1] OK + [2] OK + +#### Advanced variables and strings +Type commands and see responses in `cocomm` terminal. Optionally watch CAN communication in `candump` terminal. + + ./cocomm "4 r 0x2120 1 i64" "4 r 0x2120 2 x64" "4 r 0x2120 2 u64" "4 r 0x2120 2" + [1] -1234567890123456789 + [2] 0x1234567890ABCDEF + [3] 1311768467294899695 + [4] EF CD AB 90 78 56 34 12 + + ./cocomm "4 r 0x2120 3 r32" "4 r 0x2120 4 r64" + [1] 12.345 + [2] 456.789 + + ./cocomm "4 r 0x2120 5 r64 # Custom read function calculates the average." + [1] 1.93001e+16 + + ./cocomm "4 w 0x2120 3 r32 -7.720058e+16" "4 r 0x2120 5 r64" + [1] OK + [2] 7.8681e+07 + + ./cocomm "4 w 0x2120 6 vs 12" "4 r 0x2120 6 vs" \ + "4 w 0x2120 6 vs 1234" "4 r 0x2120 6 vs" \ + "4 w 0x2120 6 vs \"1 2\"" "4 r 0x2120 6 vs" + [1] OK + [2] "12" + [3] ERROR:0x06070012 #Data type does not match, length of service parameter too high. + [4] "12" + [5] OK + [6] "1 2" + + ./cocomm "4 r 0x2120 7 vs" + [1] "1000 bytes long string buffer. Visible string may contain UTF-8 characters, like this 3 bytes long '€' Euro symbol. It may contain also control characters like tabs ' ' or newLines ' + '." + + ./cocomm "4 w 0x2120 7 vs \"Writing newLines is not possible as visible string, but exotic \"\"ß\"\" characters works.\"" + [1] OK + + ./cocomm "4 r 0x2120 7 os" | base64 -d + [1] + Writing newLines is not possible as visible string, but exotic "ß" characters works. + + ./cocomm "4 r 0x2120 7 os" | base64 -d | hexdump -C + [1]... + + echo -ne "We can encode anything to base64\n\nand transfer data as octet string or domain." | base64 -w0 | ./cocomm -i "4 w 0x2120 7 os" + [1] OK + + ./cocomm "4 r 0x2120 8 hex" "4 r 0x2120 8 os" "4 r 0x2120 8 vs" "4 r 0x2120 8 d" "4 r 0x2120 8" + [1] C8 3D BB 00 00 00 00 00 00 00 + [2] yD27AAAAAAAAAA== + [3] "�=�" + [4] yD27AAAAAAAAAA== + [5] C8 3D BB 00 00 00 00 00 00 00 + + ./cocomm "4 w 0x2120 8 hex 01 02 03" + [1] ERROR:0x06070013 #Data type does not match, length of service parameter too short. + + ./cocomm "4 w 0x2120 8 hex 01 2 30 456789 0A b0 0 ff" + [1] OK + +#### Parameter with default value + +Value is read from Object Dictionary on program initialization and stored to internal variable. Each SDO read will read this internal variable, incremented by 1, but value in OD location will stay unchanged. SDO write will write to internal variable and to OD location. And subsequent SDO write to index 0x1010 (store parameters) will update the default value, valid at startup. Experiment with `cocomm` and see `testingVariables.c` file for implementation of this behaviour. + +#### Domain +Domain enables transfer an arbitrary large block of data in one SDO communication cycle. In our example size is limited only by free space of file system, if we have a 64-bit computer. RAM usage is low. SDO block transfer is used for highest efficiency over CAN. In our example internal virtual CAN network is used, which is very fast. On real CAN networks data transfer rates are significantly lower. However, SDO communication does not disturb higher priority real time data transfer, which is likely present on CAN bus. SDO block transfer is also resistant to disturbances to some degree. If necessary, it re-transmits corrupted data. + +See `testingVariables.c` file for implementation of large data transfer. Our `basicDevice` reads data from file, referenced by Object 0x2120, sub index 11. This is a text file in the same directory as our `basicDevice`. We can safely read the file as visible string. If necessary, transfer can be interrupted by `Ctrl+C`. SDO communication will be closed correctly by abort message. + + ./cocomm "set sdo_block 1" + [1] OK + + ./cocomm "4 r 0x2120 11 vs" + [1] "basicDevice.md" + + ./cocomm "4 r 0x2120 10 vs" + [1] "CANopen documentation + ===================== + **Basic device** + .... + +Above works, but is not very practical. This is better: + + ./cocomm "4 r 0x2120 10 d" | base64 -d | pv > file1 + [1] + ...success + 56,7KiB 0:00:00 [ 600KiB/s] [<=> ] + + cmp file1 ../examples/basicDevice/basicDevice.md + rm file1 + +If there is no response from `cmp` command, files are identical. `pv` is nice Linux command, which displays progress. Object 0x2120, sub index 11 can be changed to point to any other accessible file. + +When we send data to our `basicDevice`, it stores them to file, referenced by Object 0x2120, sub index 12. + + ./cocomm "4 r 0x2120 12 vs" + [1] "fileWrittenByDomain" + + pv cocomm.c | base64 -w0 | ./cocomm -i "4 w 0x2120 10 d" + 18,2KiB 0:00:00 [ 323MiB/s] [========================================>] 100% + [1] OK + + cmp cocomm.c ../examples/basicDevice/fileWrittenByDomain + +It is time to transfer something larger, like 100Mb random binary file: + + openssl rand 100000000 > binaryFile + pv binaryFile | base64 -w0 | ./cocomm -i "4 w 0x2120 10 d" + 95,4MiB 0:01:02 [1,52MiB/s] [========================================>] 100% + [1] OK + + cmp binaryFile ../examples/basicDevice/fileWrittenByDomain + rm binaryFile ../examples/basicDevice/fileWrittenByDomain + +Here is experimental candump output with two independent pairs of `canopend-basicDevice` communicating over one virtual CAN interface (two SDO block transfers interlaced): + + (000.000003) vcan0 604 [8] 6F C3 A9 6E AF 5C 81 32 'o..n.\.2' + (000.000010) vcan0 604 [8] 70 B7 32 8A B2 B0 62 27 'p.2...b'' + (000.000006) vcan0 604 [8] 71 4F 5F 0B F0 40 0B 53 'qO_..@.S' + (000.000003) vcan0 604 [8] 72 CE B9 49 D2 80 BD 55 'r..I...U' + (000.000010) vcan0 603 [8] A2 7F 7F 00 00 00 00 00 '........' < response + (000.000002) vcan0 604 [8] 73 94 A2 9E E2 23 8C 5A 's....#.Z' + (000.000005) vcan0 604 [8] 74 13 90 A2 59 C3 9B 1D 't...Y...' + (000.000003) vcan0 604 [8] 75 5A F4 85 3A 3E B9 10 'uZ..:>..' + (000.000011) vcan0 604 [8] 76 30 81 CC 7D 2E 7E 0F 'v0..}.~.' + (000.000000) vcan0 583 [8] 01 FC 80 1C D4 8F 67 A2 '......g.' + (000.000006) vcan0 604 [8] 77 9B FA 36 21 0F 09 2E 'w..6!...' + (000.000003) vcan0 604 [8] 78 C2 6F 97 DC 1D 97 9E 'x.o.....' + (000.000001) vcan0 583 [8] 02 E0 B9 97 47 8D 37 E6 '....G.7.' + (000.000008) vcan0 583 [8] 03 07 B6 88 68 0B 6A 8C '....h.j.' + (000.000002) vcan0 604 [8] 79 4E 6B 8D 2B 02 F5 9E 'yNk.+...' + (000.000005) vcan0 583 [8] 04 AD AC A3 B5 59 4C 30 '.....YL0' + (000.000001) vcan0 604 [8] 7A 23 97 5B D6 2A 02 F4 'z#.[.*..' + (000.000007) vcan0 583 [8] 05 67 6F 4C 1E AF 18 F3 '.goL....' + (000.000007) vcan0 604 [8] 7C 6E 6A 12 FE 5E 0B 5B '|nj..^.[' + (000.000001) vcan0 583 [8] 06 E0 43 D4 2E D2 98 BA '..C.....' + (000.000005) vcan0 604 [8] 7D 4B BF 69 25 43 98 FF '}K.i%C..' + (000.000010) vcan0 583 [8] 08 52 16 57 0D 11 72 AE '.R.W..r.' + (000.000001) vcan0 604 [8] 7F 63 0C B1 2B 19 72 89 '.c..+.r.' + (000.000007) vcan0 583 [8] 09 67 3D EF B9 C1 32 D3 '.g=...2.' + (000.000012) vcan0 583 [8] 0A 3D 22 7E 79 5B D5 D4 '.="~y[..' + (000.000010) vcan0 584 [8] A2 7F 7F 00 00 00 00 00 '........' < response + (000.000001) vcan0 583 [8] 0B EC 91 5C FF 04 0A D7 '...\....' + (000.000011) vcan0 583 [8] 0C 25 86 30 DC 44 6B 59 '.%.0.DkY' + (000.000006) vcan0 583 [8] 0D 56 8F B0 23 81 41 FF '.V..#.A.' + (000.000000) vcan0 604 [8] 01 EF 91 CC EE E8 0D 9C '........' + (000.000006) vcan0 604 [8] 02 EA EB 00 A8 34 71 3F '.....4q?' + (000.000002) vcan0 583 [8] 0E 03 79 87 32 C6 96 15 '..y.2...' + (000.000006) vcan0 604 [8] 03 C2 A9 61 8E 25 3D 0A '...a.%=.' + ... + (000.000003) vcan0 604 [8] 79 16 F1 A6 12 9D 3D B2 'y.....=.' + (000.000005) vcan0 604 [8] 7A E2 75 5A 16 18 66 37 'z.uZ..f7' + (000.000003) vcan0 583 [8] 6B 72 30 0A 0F C2 2C F9 'kr0...,.' + (000.000000) vcan0 604 [8] 7B 82 AE B2 D8 F0 71 32 '{.....q2' + (000.000010) vcan0 604 [8] 7C 2A 04 DD BE 30 A4 6E '|*...0.n' + (000.000001) vcan0 583 [8] 6C EA 98 F4 F2 DA C6 93 'l.......' + (000.000005) vcan0 604 [8] 7D 68 15 72 4D D0 B5 48 '}h.rM..H' + (000.000005) vcan0 583 [8] 6D 7B 24 4D 6E 6A C0 71 'm{$Mnj.q' + (000.000008) vcan0 604 [8] 7F E3 B8 4A CA BE 20 F1 '...J.. .' + (000.000003) vcan0 583 [8] 6E 11 7B 1F DA 9D 2B B3 'n.{...+.' + (000.000010) vcan0 583 [8] 6F 0A 1F 8B C4 56 F9 37 'o....V.7' + (000.000012) vcan0 583 [8] 70 67 91 36 4F 59 04 C9 'pg.6OY..' + (000.000012) vcan0 583 [8] 71 D4 B8 AC D1 06 7F 63 'q......c' + (000.000001) vcan0 584 [8] A2 7F 7F 00 00 00 00 00 '........' < response + (000.000008) vcan0 583 [8] 72 F3 5A E9 06 B2 A7 42 'r.Z....B' + (000.000009) vcan0 583 [8] 73 3D 99 69 35 9D B9 43 's=.i5..C' + (000.000000) vcan0 604 [8] 01 3B CB D9 00 E5 6A B7 '.;....j.' + (000.000006) vcan0 604 [8] 02 A6 0F 34 3D 07 C1 6A '...4=..j' + (000.000002) vcan0 583 [8] 74 72 CE 65 1B 82 0F FC 'tr.e....' + (000.000001) vcan0 604 [8] 03 0D 35 70 C2 34 C4 7D '..5p.4.}' + (000.000007) vcan0 583 [8] 75 9C 7A 1D 19 FB A0 92 'u.z.....' + (000.000007) vcan0 583 [8] 76 3D 9E B8 2F FF 9A 1B 'v=../...' + (000.000008) vcan0 583 [8] 77 ED 4E 6D D5 BA E2 D6 'w.Nm....' + (000.000008) vcan0 583 [8] 78 BA E2 83 1E 03 03 9B 'x.......' + (000.000005) vcan0 604 [8] 04 DD 6B BB FA 9C 67 BF '..k...g.' + (000.000002) vcan0 583 [8] 79 B7 2E A5 37 28 1E F6 'y...7(..' + (000.000004) vcan0 604 [8] 05 C6 A3 C7 EE 6D 24 C0 '.....m$.' + (000.000004) vcan0 604 [8] 06 5F 4E 41 25 BB D9 44 '._NA%..D' + (000.000000) vcan0 583 [8] 7A DB 34 D2 46 33 33 AE 'z.4.F33.' + (000.000008) vcan0 583 [8] 7B 30 76 56 31 1A A6 5C '{0vV1..\' + (000.000007) vcan0 583 [8] 7C 20 42 44 A5 60 AC 77 '| BD.`.w' + (000.000004) vcan0 604 [8] 07 42 7F 87 4B A5 ED 0E '.B..K...' + (000.000004) vcan0 583 [8] 7D 5D 36 93 F5 27 08 CF '}]6..'..' + (000.000002) vcan0 604 [8] 08 C1 17 36 99 9E 5F 17 '...6.._.' + (000.000003) vcan0 604 [8] 09 C0 3A 4F AF 7F 92 71 '..:O...q' + (000.000006) vcan0 583 [8] 7E 77 05 8E 00 43 29 D3 '~w...C).' + (000.000005) vcan0 604 [8] 0A CF 24 64 25 4D 81 30 '..$d%M.0' + (000.000004) vcan0 583 [8] 7F 98 C3 03 A3 2E AE E7 '........' + (000.000002) vcan0 604 [8] 0B 60 F0 5B 29 3F 69 4B '.`.[)?iK' + (000.000007) vcan0 604 [8] 0C E1 02 DA CF 08 34 0E '......4.' + (000.000007) vcan0 604 [8] 0D 7F 13 89 DE AB AB 16 '........' + (000.000006) vcan0 604 [8] 0E 98 AF 47 B3 30 56 DF '...G.0V.' + (000.000003) vcan0 604 [8] 0F 83 FB 95 9F EB E9 12 '........' + (000.000010) vcan0 604 [8] 10 F8 09 E9 39 10 CE 0F '....9...' + (000.000007) vcan0 604 [8] 11 4C 72 0B DB 34 92 14 '.Lr..4..' + (000.000003) vcan0 604 [8] 12 9D 83 1C 07 66 DC 7E '.....f.~' + (000.000000) vcan0 603 [8] A2 7F 7F 00 00 00 00 00 '........' < response + (000.000008) vcan0 604 [8] 13 DB E4 73 B0 6F 23 CB '...s.o#.' + (000.000006) vcan0 604 [8] 14 CE 1C 70 EC C3 57 99 '...p..W.' + (000.000006) vcan0 604 [8] 15 7C 33 D5 C7 FC 80 9B '.|3.....' + (000.000000) vcan0 583 [8] 01 2D 89 31 D6 1F 36 88 '.-.1..6.' + (000.000010) vcan0 583 [8] 02 B6 56 16 06 64 21 69 '..V..d!i' + + +Create project in your favourite IDE +------------------------------------ +BasicDevice example uses simple `Makefile` to properly compile necessary source files and set correct compile options. This file can be examined, to see, how to configure project files for your favourite IDE. There is also `gateway.Makefile`, which compiles the project with included gateway objects, similar as `canopend`. + +### Create project with [KDevelop](https://www.kdevelop.org/) +- `sudo apt install kdevelop breeze` +- Run KDevelop, select: Project -> open project +- Navigate to directory `examples/basicDevice` and click open. +- KDevelop will recognize `Makefile` and will just use it. Click Finish. +- Open project settings (right click on project on left panel) + - Make: set to 1 thread operation. + - Language support, Includes, add paths to directories: + - `CANopenNode/socketCAN` + - `CANopenNode` + - `examples/basicDevice` + - Language support, Defines, add: + - `CO_DRIVER_CUSTOM` +- Run -> Setup launches -> basicDevice: + - Add Executable file + - Name it `basicDevice_i4_vcan0`, for example. + - Executable file: `examples/basicDevice/basicDevice` + - Arguments: `-i 4 vcan0` +- Build, then Execute or Debug diff --git a/Devices/Libraries/Systems/CANopenSocket/examples/basicDevice/basicDevice.eds b/Devices/Libraries/Systems/CANopenSocket/examples/basicDevice/basicDevice.eds new file mode 100755 index 0000000..4c29b20 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/examples/basicDevice/basicDevice.eds @@ -0,0 +1,2806 @@ +[FileInfo] +FileName=basicDevice.eds +FileVersion=1 +FileRevision=1 +LastEDS= +EDSVersion=4.0 +Description=Basic CANopen device with example usage. +CreationTime=12:37PM +CreationDate=11-28-2020 +CreatedBy=Janez Paternoster +ModificationTime=7:31PM +ModificationDate=03-16-2021 +ModifiedBy=Janez Paternoster + +[DeviceInfo] +VendorName= +VendorNumber= +ProductName=Basic device +ProductNumber= +RevisionNumber=0 +BaudRate_10=1 +BaudRate_20=1 +BaudRate_50=1 +BaudRate_125=1 +BaudRate_250=1 +BaudRate_500=1 +BaudRate_800=1 +BaudRate_1000=1 +SimpleBootUpMaster=0 +SimpleBootUpSlave=0 +Granularity=8 +DynamicChannelsSupported=0 +CompactPDO=0 +GroupMessaging=0 +NrOfRXPDO=4 +NrOfTXPDO=4 +LSS_Supported=1 + +[DummyUsage] +Dummy0001=0 +Dummy0002=1 +Dummy0003=1 +Dummy0004=1 +Dummy0005=1 +Dummy0006=1 +Dummy0007=1 + +[Comments] +Lines=0 + +[MandatoryObjects] +SupportedObjects=3 +1=0x1000 +2=0x1001 +3=0x1018 + +[1000] +ParameterName=Device type +ObjectType=0x7 +;StorageLocation=PERSIST_COMM +DataType=0x0007 +AccessType=ro +DefaultValue=0x000F0191 +PDOMapping=0 + +[1001] +ParameterName=Error register +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0005 +AccessType=ro +DefaultValue=0x00 +PDOMapping=1 + +[1018] +ParameterName=Identity +ObjectType=0x9 +;StorageLocation=PERSIST_COMM +SubNumber=0x5 + +[1018sub0] +ParameterName=Highest sub-index supported +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0005 +AccessType=ro +DefaultValue=0x04 +PDOMapping=0 + +[1018sub1] +ParameterName=Vendor-ID +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=ro +DefaultValue=0x00000000 +PDOMapping=0 + +[1018sub2] +ParameterName=Product code +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=ro +DefaultValue=0x00000000 +PDOMapping=0 + +[1018sub3] +ParameterName=Revision number +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=ro +DefaultValue=0x00000000 +PDOMapping=0 + +[1018sub4] +ParameterName=Serial number +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=ro +DefaultValue=0x00000000 +PDOMapping=0 + +[OptionalObjects] +SupportedObjects=34 +1=0x1003 +2=0x1005 +3=0x1006 +4=0x1007 +5=0x1010 +6=0x1011 +7=0x1012 +8=0x1014 +9=0x1015 +10=0x1016 +11=0x1017 +12=0x1019 +13=0x1200 +14=0x1280 +15=0x1400 +16=0x1401 +17=0x1402 +18=0x1403 +19=0x1600 +20=0x1601 +21=0x1602 +22=0x1603 +23=0x1800 +24=0x1801 +25=0x1802 +26=0x1803 +27=0x1A00 +28=0x1A01 +29=0x1A02 +30=0x1A03 +31=0x6000 +32=0x6200 +33=0x6401 +34=0x6411 + +[1003] +ParameterName=Pre-defined error field +ObjectType=0x8 +;StorageLocation=RAM +SubNumber=0x9 + +[1003sub0] +ParameterName=Number of errors +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0005 +AccessType=rw +DefaultValue=0 +PDOMapping=0 + +[1003sub1] +ParameterName=Standard error field +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=ro +DefaultValue=0x00000000 +PDOMapping=0 + +[1003sub2] +ParameterName=Standard error field +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=ro +DefaultValue=0x00000000 +PDOMapping=0 + +[1003sub3] +ParameterName=Standard error field +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=ro +DefaultValue=0x00000000 +PDOMapping=0 + +[1003sub4] +ParameterName=Standard error field +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=ro +DefaultValue=0x00000000 +PDOMapping=0 + +[1003sub5] +ParameterName=Standard error field +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=ro +DefaultValue=0x00000000 +PDOMapping=0 + +[1003sub6] +ParameterName=Standard error field +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=ro +DefaultValue=0x00000000 +PDOMapping=0 + +[1003sub7] +ParameterName=Standard error field +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=ro +DefaultValue=0x00000000 +PDOMapping=0 + +[1003sub8] +ParameterName=Standard error field +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=ro +DefaultValue=0x00000000 +PDOMapping=0 + +[1005] +ParameterName=COB-ID SYNC message +ObjectType=0x7 +;StorageLocation=PERSIST_COMM +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000080 +PDOMapping=0 + +[1006] +ParameterName=Communication cycle period +ObjectType=0x7 +;StorageLocation=PERSIST_COMM +DataType=0x0007 +AccessType=rw +DefaultValue=0 +PDOMapping=0 + +[1007] +ParameterName=Synchronous window length +ObjectType=0x7 +;StorageLocation=PERSIST_COMM +DataType=0x0007 +AccessType=rw +DefaultValue=0 +PDOMapping=0 + +[1010] +ParameterName=Store parameters +ObjectType=0x8 +;StorageLocation=RAM +SubNumber=0x7 + +[1010sub0] +ParameterName=Highest sub-index supported +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0005 +AccessType=ro +DefaultValue=0x06 +PDOMapping=0 + +[1010sub1] +ParameterName=Save all parameters +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000003 +PDOMapping=0 + +[1010sub2] +ParameterName=Save communication parameters +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000001 +PDOMapping=0 + +[1010sub3] +ParameterName=Save application parameters +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000001 +PDOMapping=0 + +[1010sub4] +ParameterName=Save LSS parameters +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000003 +PDOMapping=0 + +[1010sub5] +ParameterName=Save testing parameters +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000001 +PDOMapping=0 + +[1010sub6] +ParameterName=Save testing auto-store parameters +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000003 +PDOMapping=0 + +[1011] +ParameterName=Restore default parameters +ObjectType=0x8 +;StorageLocation=RAM +SubNumber=0x7 + +[1011sub0] +ParameterName=Highest sub-index supported +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0005 +AccessType=ro +DefaultValue=0x06 +PDOMapping=0 + +[1011sub1] +ParameterName=Restore all default parameters +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000001 +PDOMapping=0 + +[1011sub2] +ParameterName=Restore communication default parameters +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000001 +PDOMapping=0 + +[1011sub3] +ParameterName=Restore application default parameters +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000001 +PDOMapping=0 + +[1011sub4] +ParameterName=Restore LSS default parameters +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000001 +PDOMapping=0 + +[1011sub5] +ParameterName=Restore testing default parameters +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000001 +PDOMapping=0 + +[1011sub6] +ParameterName=Restore testing auto-store default parameters +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000001 +PDOMapping=0 + +[1012] +ParameterName=COB-ID time stamp object +ObjectType=0x7 +;StorageLocation=PERSIST_COMM +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000100 +PDOMapping=0 + +[1014] +ParameterName=COB-ID EMCY +ObjectType=0x7 +;StorageLocation=PERSIST_COMM +DataType=0x0007 +AccessType=rw +DefaultValue=0x80+$NODEID +PDOMapping=0 + +[1015] +ParameterName=Inhibit time EMCY +ObjectType=0x7 +;StorageLocation=PERSIST_COMM +DataType=0x0006 +AccessType=rw +DefaultValue=0 +PDOMapping=0 + +[1016] +ParameterName=Consumer heartbeat time +ObjectType=0x8 +;StorageLocation=PERSIST_COMM +SubNumber=0x9 + +[1016sub0] +ParameterName=Highest sub-index supported +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0005 +AccessType=ro +DefaultValue=0x08 +PDOMapping=0 + +[1016sub1] +ParameterName=Consumer heartbeat time +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1016sub2] +ParameterName=Consumer heartbeat time +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1016sub3] +ParameterName=Consumer heartbeat time +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1016sub4] +ParameterName=Consumer heartbeat time +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1016sub5] +ParameterName=Consumer heartbeat time +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1016sub6] +ParameterName=Consumer heartbeat time +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1016sub7] +ParameterName=Consumer heartbeat time +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1016sub8] +ParameterName=Consumer heartbeat time +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1017] +ParameterName=Producer heartbeat time +ObjectType=0x7 +;StorageLocation=PERSIST_COMM +DataType=0x0006 +AccessType=rw +DefaultValue=0 +PDOMapping=0 + +[1019] +ParameterName=Synchronous counter overflow value +ObjectType=0x7 +;StorageLocation=PERSIST_COMM +DataType=0x0005 +AccessType=rw +DefaultValue=0 +PDOMapping=0 + +[1200] +ParameterName=SDO server parameter +ObjectType=0x9 +;StorageLocation=RAM +SubNumber=0x3 + +[1200sub0] +ParameterName=Highest sub-index supported +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0005 +AccessType=ro +DefaultValue=2 +PDOMapping=0 + +[1200sub1] +ParameterName=COB-ID client to server (rx) +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=ro +DefaultValue=0x600+$NODEID +PDOMapping=1 + +[1200sub2] +ParameterName=COB-ID server to client (tx) +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=ro +DefaultValue=0x580+$NODEID +PDOMapping=1 + +[1280] +ParameterName=SDO client parameter +ObjectType=0x9 +;StorageLocation=PERSIST_COMM +SubNumber=0x4 + +[1280sub0] +ParameterName=Highest sub-index supported +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0005 +AccessType=ro +DefaultValue=0x03 +PDOMapping=0 + +[1280sub1] +ParameterName=COB-ID client to server (tx) +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=rw +DefaultValue=0x80000000 +PDOMapping=1 + +[1280sub2] +ParameterName=COB-ID server to client (rx) +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=rw +DefaultValue=0x80000000 +PDOMapping=1 + +[1280sub3] +ParameterName=Node-ID of the SDO server +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0005 +AccessType=rw +DefaultValue=0x01 +PDOMapping=0 + +[1400] +ParameterName=RPDO communication parameter +ObjectType=0x9 +;StorageLocation=PERSIST_COMM +SubNumber=0x4 + +[1400sub0] +ParameterName=Highest sub-index supported +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0005 +AccessType=ro +DefaultValue=0x05 +PDOMapping=0 + +[1400sub1] +ParameterName=COB-ID used by RPDO +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000200+$NODEID +PDOMapping=0 + +[1400sub2] +ParameterName=Transmission type +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0005 +AccessType=rw +DefaultValue=255 +PDOMapping=0 + +[1400sub5] +ParameterName=Event timer +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0006 +AccessType=rw +DefaultValue=0 +PDOMapping=0 + +[1401] +ParameterName=RPDO communication parameter +ObjectType=0x9 +;StorageLocation=PERSIST_COMM +SubNumber=0x4 + +[1401sub0] +ParameterName=Highest sub-index supported +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0005 +AccessType=ro +DefaultValue=0x05 +PDOMapping=0 + +[1401sub1] +ParameterName=COB-ID used by RPDO +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000300+$NODEID +PDOMapping=0 + +[1401sub2] +ParameterName=Transmission type +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0005 +AccessType=rw +DefaultValue=255 +PDOMapping=0 + +[1401sub5] +ParameterName=Event timer +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0006 +AccessType=rw +DefaultValue=0 +PDOMapping=0 + +[1402] +ParameterName=RPDO communication parameter +ObjectType=0x9 +;StorageLocation=PERSIST_COMM +SubNumber=0x4 + +[1402sub0] +ParameterName=Highest sub-index supported +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0005 +AccessType=ro +DefaultValue=0x05 +PDOMapping=0 + +[1402sub1] +ParameterName=COB-ID used by RPDO +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=rw +DefaultValue=0x80000400+$NODEID +PDOMapping=0 + +[1402sub2] +ParameterName=Transmission type +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0005 +AccessType=rw +DefaultValue=254 +PDOMapping=0 + +[1402sub5] +ParameterName=Event timer +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0006 +AccessType=rw +DefaultValue=0 +PDOMapping=0 + +[1403] +ParameterName=RPDO communication parameter +ObjectType=0x9 +;StorageLocation=PERSIST_COMM +SubNumber=0x4 + +[1403sub0] +ParameterName=Highest sub-index supported +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0005 +AccessType=ro +DefaultValue=0x05 +PDOMapping=0 + +[1403sub1] +ParameterName=COB-ID used by RPDO +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=rw +DefaultValue=0x80000500+$NODEID +PDOMapping=0 + +[1403sub2] +ParameterName=Transmission type +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0005 +AccessType=rw +DefaultValue=254 +PDOMapping=0 + +[1403sub5] +ParameterName=Event timer +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0006 +AccessType=rw +DefaultValue=0 +PDOMapping=0 + +[1600] +ParameterName=RPDO mapping parameter +ObjectType=0x9 +;StorageLocation=PERSIST_COMM +SubNumber=0x9 + +[1600sub0] +ParameterName=Number of mapped application objects in PDO +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0005 +AccessType=rw +DefaultValue=2 +PDOMapping=0 + +[1600sub1] +ParameterName=Application object 1 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=rw +DefaultValue=0x62000108 +PDOMapping=0 + +[1600sub2] +ParameterName=Application object 2 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=rw +DefaultValue=0x62000208 +PDOMapping=0 + +[1600sub3] +ParameterName=Application object 3 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=rw +DefaultValue=0x62000308 +PDOMapping=0 + +[1600sub4] +ParameterName=Application object 4 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=rw +DefaultValue=0x62000408 +PDOMapping=0 + +[1600sub5] +ParameterName=Application object 5 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=rw +DefaultValue=0x62000508 +PDOMapping=0 + +[1600sub6] +ParameterName=Application object 6 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=rw +DefaultValue=0x62000608 +PDOMapping=0 + +[1600sub7] +ParameterName=Application object 7 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=rw +DefaultValue=0x62000708 +PDOMapping=0 + +[1600sub8] +ParameterName=Application object 8 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=rw +DefaultValue=0x62000808 +PDOMapping=0 + +[1601] +ParameterName=RPDO mapping parameter +ObjectType=0x9 +;StorageLocation=PERSIST_COMM +SubNumber=0x9 + +[1601sub0] +ParameterName=Number of mapped application objects in PDO +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0005 +AccessType=rw +DefaultValue=4 +PDOMapping=0 + +[1601sub1] +ParameterName=Application object 1 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=rw +DefaultValue=0x64110110 +PDOMapping=0 + +[1601sub2] +ParameterName=Application object 2 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=rw +DefaultValue=0x64110210 +PDOMapping=0 + +[1601sub3] +ParameterName=Application object 3 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=rw +DefaultValue=0x64110310 +PDOMapping=0 + +[1601sub4] +ParameterName=Application object 4 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=rw +DefaultValue=0x64110410 +PDOMapping=0 + +[1601sub5] +ParameterName=Application object 5 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1601sub6] +ParameterName=Application object 6 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1601sub7] +ParameterName=Application object 7 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1601sub8] +ParameterName=Application object 8 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1602] +ParameterName=RPDO mapping parameter +ObjectType=0x9 +;StorageLocation=PERSIST_COMM +SubNumber=0x9 + +[1602sub0] +ParameterName=Number of mapped application objects in PDO +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0005 +AccessType=rw +DefaultValue=0 +PDOMapping=0 + +[1602sub1] +ParameterName=Application object 1 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1602sub2] +ParameterName=Application object 2 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1602sub3] +ParameterName=Application object 3 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1602sub4] +ParameterName=Application object 4 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1602sub5] +ParameterName=Application object 5 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1602sub6] +ParameterName=Application object 6 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1602sub7] +ParameterName=Application object 7 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1602sub8] +ParameterName=Application object 8 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1603] +ParameterName=RPDO mapping parameter +ObjectType=0x9 +;StorageLocation=PERSIST_COMM +SubNumber=0x9 + +[1603sub0] +ParameterName=Number of mapped application objects in PDO +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0005 +AccessType=rw +DefaultValue=0 +PDOMapping=0 + +[1603sub1] +ParameterName=Application object 1 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1603sub2] +ParameterName=Application object 2 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1603sub3] +ParameterName=Application object 3 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1603sub4] +ParameterName=Application object 4 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1603sub5] +ParameterName=Application object 5 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1603sub6] +ParameterName=Application object 6 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1603sub7] +ParameterName=Application object 7 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1603sub8] +ParameterName=Application object 8 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1800] +ParameterName=TPDO communication parameter +ObjectType=0x9 +;StorageLocation=PERSIST_COMM +SubNumber=0x6 + +[1800sub0] +ParameterName=Highest sub-index supported +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0005 +AccessType=ro +DefaultValue=0x06 +PDOMapping=0 + +[1800sub1] +ParameterName=COB-ID used by TPDO +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=rw +DefaultValue=0x40000180+$NODEID +PDOMapping=0 + +[1800sub2] +ParameterName=Transmission type +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0005 +AccessType=rw +DefaultValue=255 +PDOMapping=0 + +[1800sub3] +ParameterName=Inhibit time +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0006 +AccessType=rw +DefaultValue=0 +PDOMapping=0 + +[1800sub5] +ParameterName=Event timer +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0006 +AccessType=rw +DefaultValue=0 +PDOMapping=0 + +[1800sub6] +ParameterName=SYNC start value +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0005 +AccessType=rw +DefaultValue=0 +PDOMapping=0 + +[1801] +ParameterName=TPDO communication parameter +ObjectType=0x9 +;StorageLocation=PERSIST_COMM +SubNumber=0x6 + +[1801sub0] +ParameterName=Highest sub-index supported +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0005 +AccessType=ro +DefaultValue=0x06 +PDOMapping=0 + +[1801sub1] +ParameterName=COB-ID used by TPDO +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=rw +DefaultValue=0x40000280+$NODEID +PDOMapping=0 + +[1801sub2] +ParameterName=Transmission type +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0005 +AccessType=rw +DefaultValue=255 +PDOMapping=0 + +[1801sub3] +ParameterName=Inhibit time +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0006 +AccessType=rw +DefaultValue=0 +PDOMapping=0 + +[1801sub5] +ParameterName=Event timer +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0006 +AccessType=rw +DefaultValue=0 +PDOMapping=0 + +[1801sub6] +ParameterName=SYNC start value +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0005 +AccessType=rw +DefaultValue=0 +PDOMapping=0 + +[1802] +ParameterName=TPDO communication parameter +ObjectType=0x9 +;StorageLocation=PERSIST_COMM +SubNumber=0x6 + +[1802sub0] +ParameterName=Highest sub-index supported +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0005 +AccessType=ro +DefaultValue=0x06 +PDOMapping=0 + +[1802sub1] +ParameterName=COB-ID used by TPDO +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=rw +DefaultValue=0xC0000380+$NODEID +PDOMapping=0 + +[1802sub2] +ParameterName=Transmission type +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0005 +AccessType=rw +DefaultValue=254 +PDOMapping=0 + +[1802sub3] +ParameterName=Inhibit time +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0006 +AccessType=rw +DefaultValue=0 +PDOMapping=0 + +[1802sub5] +ParameterName=Event timer +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0006 +AccessType=rw +DefaultValue=0 +PDOMapping=0 + +[1802sub6] +ParameterName=SYNC start value +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0005 +AccessType=rw +DefaultValue=0 +PDOMapping=0 + +[1803] +ParameterName=TPDO communication parameter +ObjectType=0x9 +;StorageLocation=PERSIST_COMM +SubNumber=0x6 + +[1803sub0] +ParameterName=Highest sub-index supported +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0005 +AccessType=ro +DefaultValue=0x06 +PDOMapping=0 + +[1803sub1] +ParameterName=COB-ID used by TPDO +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=rw +DefaultValue=0xC0000480+$NODEID +PDOMapping=0 + +[1803sub2] +ParameterName=Transmission type +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0005 +AccessType=rw +DefaultValue=254 +PDOMapping=0 + +[1803sub3] +ParameterName=Inhibit time +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0006 +AccessType=rw +DefaultValue=0 +PDOMapping=0 + +[1803sub5] +ParameterName=Event timer +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0006 +AccessType=rw +DefaultValue=0 +PDOMapping=0 + +[1803sub6] +ParameterName=SYNC start value +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0005 +AccessType=rw +DefaultValue=0 +PDOMapping=0 + +[1A00] +ParameterName=TPDO mapping parameter +ObjectType=0x9 +;StorageLocation=PERSIST_COMM +SubNumber=0x9 + +[1A00sub0] +ParameterName=Number of mapped application objects in PDO +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0005 +AccessType=rw +DefaultValue=2 +PDOMapping=0 + +[1A00sub1] +ParameterName=Application object 1 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=rw +DefaultValue=0x60000108 +PDOMapping=0 + +[1A00sub2] +ParameterName=Application object 2 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=rw +DefaultValue=0x60000208 +PDOMapping=0 + +[1A00sub3] +ParameterName=Application object 3 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=rw +DefaultValue=0x60000308 +PDOMapping=0 + +[1A00sub4] +ParameterName=Application object 4 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=rw +DefaultValue=0x60000408 +PDOMapping=0 + +[1A00sub5] +ParameterName=Application object 5 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=rw +DefaultValue=0x60000508 +PDOMapping=0 + +[1A00sub6] +ParameterName=Application object 6 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=rw +DefaultValue=0x60000608 +PDOMapping=0 + +[1A00sub7] +ParameterName=Application object 7 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=rw +DefaultValue=0x60000708 +PDOMapping=0 + +[1A00sub8] +ParameterName=Application object 8 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=rw +DefaultValue=0x60000808 +PDOMapping=0 + +[1A01] +ParameterName=TPDO mapping parameter +ObjectType=0x9 +;StorageLocation=PERSIST_COMM +SubNumber=0x9 + +[1A01sub0] +ParameterName=Number of mapped application objects in PDO +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0005 +AccessType=rw +DefaultValue=4 +PDOMapping=0 + +[1A01sub1] +ParameterName=Application object 1 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=rw +DefaultValue=0x64010110 +PDOMapping=0 + +[1A01sub2] +ParameterName=Application object 2 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=rw +DefaultValue=0x64010210 +PDOMapping=0 + +[1A01sub3] +ParameterName=Application object 3 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=rw +DefaultValue=0x64010310 +PDOMapping=0 + +[1A01sub4] +ParameterName=Application object 4 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=rw +DefaultValue=0x64010410 +PDOMapping=0 + +[1A01sub5] +ParameterName=Application object 5 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1A01sub6] +ParameterName=Application object 6 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1A01sub7] +ParameterName=Application object 7 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1A01sub8] +ParameterName=Application object 8 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1A02] +ParameterName=TPDO mapping parameter +ObjectType=0x9 +;StorageLocation=PERSIST_COMM +SubNumber=0x9 + +[1A02sub0] +ParameterName=Number of mapped application objects in PDO +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0005 +AccessType=rw +DefaultValue=0 +PDOMapping=0 + +[1A02sub1] +ParameterName=Application object 1 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1A02sub2] +ParameterName=Application object 2 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1A02sub3] +ParameterName=Application object 3 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1A02sub4] +ParameterName=Application object 4 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1A02sub5] +ParameterName=Application object 5 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1A02sub6] +ParameterName=Application object 6 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1A02sub7] +ParameterName=Application object 7 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1A02sub8] +ParameterName=Application object 8 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1A03] +ParameterName=TPDO mapping parameter +ObjectType=0x9 +;StorageLocation=PERSIST_COMM +SubNumber=0x9 + +[1A03sub0] +ParameterName=Number of mapped application objects in PDO +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0005 +AccessType=rw +DefaultValue=0 +PDOMapping=0 + +[1A03sub1] +ParameterName=Application object 1 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1A03sub2] +ParameterName=Application object 2 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1A03sub3] +ParameterName=Application object 3 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1A03sub4] +ParameterName=Application object 4 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1A03sub5] +ParameterName=Application object 5 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1A03sub6] +ParameterName=Application object 6 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1A03sub7] +ParameterName=Application object 7 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1A03sub8] +ParameterName=Application object 8 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[6000] +ParameterName=Read digital input 8-bit +ObjectType=0x8 +;StorageLocation=RAM +SubNumber=0x9 + +[6000sub0] +ParameterName=Highest sub-index supported +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0005 +AccessType=ro +DefaultValue=0x08 +PDOMapping=0 + +[6000sub1] +ParameterName=Input +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0005 +AccessType=ro +DefaultValue=0x00 +PDOMapping=1 + +[6000sub2] +ParameterName=Input +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0005 +AccessType=ro +DefaultValue=0x00 +PDOMapping=1 + +[6000sub3] +ParameterName=Input +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0005 +AccessType=ro +DefaultValue=0x00 +PDOMapping=1 + +[6000sub4] +ParameterName=Input +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0005 +AccessType=ro +DefaultValue=0x00 +PDOMapping=1 + +[6000sub5] +ParameterName=Input +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0005 +AccessType=ro +DefaultValue=0x00 +PDOMapping=1 + +[6000sub6] +ParameterName=Input +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0005 +AccessType=ro +DefaultValue=0x00 +PDOMapping=1 + +[6000sub7] +ParameterName=Input +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0005 +AccessType=ro +DefaultValue=0x00 +PDOMapping=1 + +[6000sub8] +ParameterName=Input +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0005 +AccessType=ro +DefaultValue=0x00 +PDOMapping=1 + +[6200] +ParameterName=Write digital output 8-bit +ObjectType=0x8 +;StorageLocation=RAM +SubNumber=0x9 + +[6200sub0] +ParameterName=Highest sub-index supported +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0005 +AccessType=ro +DefaultValue=0x08 +PDOMapping=1 + +[6200sub1] +ParameterName=Output +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0005 +AccessType=rw +DefaultValue=0x00 +PDOMapping=1 + +[6200sub2] +ParameterName=Output +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0005 +AccessType=rw +DefaultValue=0x00 +PDOMapping=1 + +[6200sub3] +ParameterName=Output +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0005 +AccessType=rw +DefaultValue=0x00 +PDOMapping=1 + +[6200sub4] +ParameterName=Output +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0005 +AccessType=rw +DefaultValue=0x00 +PDOMapping=1 + +[6200sub5] +ParameterName=Output +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0005 +AccessType=rw +DefaultValue=0x00 +PDOMapping=1 + +[6200sub6] +ParameterName=Output +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0005 +AccessType=rw +DefaultValue=0x00 +PDOMapping=1 + +[6200sub7] +ParameterName=Output +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0005 +AccessType=rw +DefaultValue=0x00 +PDOMapping=1 + +[6200sub8] +ParameterName=Output +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0005 +AccessType=rw +DefaultValue=0x00 +PDOMapping=1 + +[6401] +ParameterName=Read analog input 16-bit +ObjectType=0x8 +;StorageLocation=RAM +SubNumber=0x11 + +[6401sub0] +ParameterName=Highest sub-index supported +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0005 +AccessType=ro +DefaultValue=0x10 +PDOMapping=1 + +[6401sub1] +ParameterName=Input +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0003 +AccessType=ro +DefaultValue=0 +PDOMapping=1 + +[6401sub2] +ParameterName=Input +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0003 +AccessType=ro +DefaultValue=0 +PDOMapping=1 + +[6401sub3] +ParameterName=Input +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0003 +AccessType=ro +DefaultValue=0 +PDOMapping=1 + +[6401sub4] +ParameterName=Input +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0003 +AccessType=ro +DefaultValue=0 +PDOMapping=1 + +[6401sub5] +ParameterName=Input +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0003 +AccessType=ro +DefaultValue=0 +PDOMapping=1 + +[6401sub6] +ParameterName=Input +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0003 +AccessType=ro +DefaultValue=0 +PDOMapping=1 + +[6401sub7] +ParameterName=Input +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0003 +AccessType=ro +DefaultValue=0 +PDOMapping=1 + +[6401sub8] +ParameterName=Input +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0003 +AccessType=ro +DefaultValue=0 +PDOMapping=1 + +[6401sub9] +ParameterName=Input +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0003 +AccessType=ro +DefaultValue=0 +PDOMapping=1 + +[6401subA] +ParameterName=Input +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0003 +AccessType=ro +DefaultValue=0 +PDOMapping=1 + +[6401subB] +ParameterName=Input +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0003 +AccessType=ro +DefaultValue=0 +PDOMapping=1 + +[6401subC] +ParameterName=Input +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0003 +AccessType=ro +DefaultValue=0 +PDOMapping=1 + +[6401subD] +ParameterName=Input +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0003 +AccessType=ro +DefaultValue=0 +PDOMapping=1 + +[6401subE] +ParameterName=Input +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0003 +AccessType=ro +DefaultValue=0 +PDOMapping=1 + +[6401subF] +ParameterName=Input +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0003 +AccessType=ro +DefaultValue=0 +PDOMapping=1 + +[6401sub10] +ParameterName=Input +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0003 +AccessType=ro +DefaultValue=0 +PDOMapping=1 + +[6411] +ParameterName=Write analog output 16-bit +ObjectType=0x8 +;StorageLocation=RAM +SubNumber=0x9 + +[6411sub0] +ParameterName=Highest sub-index supported +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0005 +AccessType=ro +DefaultValue=0x08 +PDOMapping=1 + +[6411sub1] +ParameterName=Output +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0003 +AccessType=rw +DefaultValue=0 +PDOMapping=1 + +[6411sub2] +ParameterName=Output +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0003 +AccessType=rw +DefaultValue=0 +PDOMapping=1 + +[6411sub3] +ParameterName=Output +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0003 +AccessType=rw +DefaultValue=0 +PDOMapping=1 + +[6411sub4] +ParameterName=Output +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0003 +AccessType=rw +DefaultValue=0 +PDOMapping=1 + +[6411sub5] +ParameterName=Output +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0003 +AccessType=rw +DefaultValue=0 +PDOMapping=1 + +[6411sub6] +ParameterName=Output +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0003 +AccessType=rw +DefaultValue=0 +PDOMapping=1 + +[6411sub7] +ParameterName=Output +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0003 +AccessType=rw +DefaultValue=0 +PDOMapping=1 + +[6411sub8] +ParameterName=Output +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0003 +AccessType=rw +DefaultValue=0 +PDOMapping=1 + +[ManufacturerObjects] +SupportedObjects=7 +1=0x2100 +2=0x2105 +3=0x2106 +4=0x2110 +5=0x2111 +6=0x2112 +7=0x2120 + +[2100] +ParameterName=Error status bits +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x000A +AccessType=ro +DefaultValue=00000000000000000000 +PDOMapping=1 + +[2105] +ParameterName=Version +ObjectType=0x9 +;StorageLocation=RAM +SubNumber=0x3 + +[2105sub0] +ParameterName=Highest sub-index supported +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0005 +AccessType=ro +DefaultValue=0x02 +PDOMapping=0 + +[2105sub1] +ParameterName=CANopenNode +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0009 +AccessType=ro +DefaultValue= +PDOMapping=0 + +[2105sub2] +ParameterName=Application +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0009 +AccessType=ro +DefaultValue= +PDOMapping=0 + +[2106] +ParameterName=Power-on counter +ObjectType=0x7 +;StorageLocation=PERSIST_TEST_AUTO +DataType=0x0007 +AccessType=ro +DefaultValue=0 +PDOMapping=0 + +[2110] +ParameterName=Variable Int32 +ObjectType=0x8 +;StorageLocation=RAM +SubNumber=0x11 + +[2110sub0] +ParameterName=Highest sub-index supported +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0005 +AccessType=ro +DefaultValue=0x10 +PDOMapping=1 + +[2110sub1] +ParameterName=int32 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0004 +AccessType=rw +DefaultValue=0 +PDOMapping=1 + +[2110sub2] +ParameterName=int32 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0004 +AccessType=rw +DefaultValue=0 +PDOMapping=1 + +[2110sub3] +ParameterName=int32 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0004 +AccessType=rw +DefaultValue=0 +PDOMapping=1 + +[2110sub4] +ParameterName=int32 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0004 +AccessType=rw +DefaultValue=0 +PDOMapping=1 + +[2110sub5] +ParameterName=int32 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0004 +AccessType=rw +DefaultValue=0 +PDOMapping=1 + +[2110sub6] +ParameterName=int32 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0004 +AccessType=rw +DefaultValue=0 +PDOMapping=1 + +[2110sub7] +ParameterName=int32 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0004 +AccessType=rw +DefaultValue=0 +PDOMapping=1 + +[2110sub8] +ParameterName=int32 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0004 +AccessType=rw +DefaultValue=0 +PDOMapping=1 + +[2110sub9] +ParameterName=int32 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0004 +AccessType=rw +DefaultValue=0 +PDOMapping=1 + +[2110subA] +ParameterName=int32 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0004 +AccessType=rw +DefaultValue=0 +PDOMapping=1 + +[2110subB] +ParameterName=int32 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0004 +AccessType=rw +DefaultValue=0 +PDOMapping=1 + +[2110subC] +ParameterName=int32 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0004 +AccessType=rw +DefaultValue=0 +PDOMapping=1 + +[2110subD] +ParameterName=int32 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0004 +AccessType=rw +DefaultValue=0 +PDOMapping=1 + +[2110subE] +ParameterName=int32 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0004 +AccessType=rw +DefaultValue=0 +PDOMapping=1 + +[2110subF] +ParameterName=int32 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0004 +AccessType=rw +DefaultValue=0 +PDOMapping=1 + +[2110sub10] +ParameterName=int32 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0004 +AccessType=rw +DefaultValue=0 +PDOMapping=1 + +[2111] +ParameterName=Variable Int32 save +ObjectType=0x8 +;StorageLocation=PERSIST_TEST +SubNumber=0x11 + +[2111sub0] +ParameterName=Highest sub-index supported +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0005 +AccessType=ro +DefaultValue=0x10 +PDOMapping=1 + +[2111sub1] +ParameterName=int32 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0004 +AccessType=rw +DefaultValue=0 +PDOMapping=0 + +[2111sub2] +ParameterName=int32 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0004 +AccessType=rw +DefaultValue=0 +PDOMapping=0 + +[2111sub3] +ParameterName=int32 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0004 +AccessType=rw +DefaultValue=0 +PDOMapping=0 + +[2111sub4] +ParameterName=int32 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0004 +AccessType=rw +DefaultValue=0 +PDOMapping=0 + +[2111sub5] +ParameterName=int32 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0004 +AccessType=rw +DefaultValue=0 +PDOMapping=0 + +[2111sub6] +ParameterName=int32 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0004 +AccessType=rw +DefaultValue=0 +PDOMapping=0 + +[2111sub7] +ParameterName=int32 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0004 +AccessType=rw +DefaultValue=0 +PDOMapping=0 + +[2111sub8] +ParameterName=int32 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0004 +AccessType=rw +DefaultValue=0 +PDOMapping=0 + +[2111sub9] +ParameterName=int32 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0004 +AccessType=rw +DefaultValue=0 +PDOMapping=0 + +[2111subA] +ParameterName=int32 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0004 +AccessType=rw +DefaultValue=0 +PDOMapping=0 + +[2111subB] +ParameterName=int32 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0004 +AccessType=rw +DefaultValue=0 +PDOMapping=0 + +[2111subC] +ParameterName=int32 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0004 +AccessType=rw +DefaultValue=0 +PDOMapping=0 + +[2111subD] +ParameterName=int32 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0004 +AccessType=rw +DefaultValue=0 +PDOMapping=0 + +[2111subE] +ParameterName=int32 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0004 +AccessType=rw +DefaultValue=0 +PDOMapping=0 + +[2111subF] +ParameterName=int32 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0004 +AccessType=rw +DefaultValue=0 +PDOMapping=0 + +[2111sub10] +ParameterName=int32 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0004 +AccessType=rw +DefaultValue=0 +PDOMapping=0 + +[2112] +ParameterName=Variable NV Int32 auto save +ObjectType=0x8 +;StorageLocation=PERSIST_TEST_AUTO +SubNumber=0x11 + +[2112sub0] +ParameterName=Highest sub-index supported +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0005 +AccessType=ro +DefaultValue=0x10 +PDOMapping=1 + +[2112sub1] +ParameterName=int32 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0004 +AccessType=rw +DefaultValue=0 +PDOMapping=0 + +[2112sub2] +ParameterName=int32 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0004 +AccessType=rw +DefaultValue=0 +PDOMapping=0 + +[2112sub3] +ParameterName=int32 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0004 +AccessType=rw +DefaultValue=0 +PDOMapping=0 + +[2112sub4] +ParameterName=int32 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0004 +AccessType=rw +DefaultValue=0 +PDOMapping=0 + +[2112sub5] +ParameterName=int32 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0004 +AccessType=rw +DefaultValue=0 +PDOMapping=0 + +[2112sub6] +ParameterName=int32 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0004 +AccessType=rw +DefaultValue=0 +PDOMapping=0 + +[2112sub7] +ParameterName=int32 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0004 +AccessType=rw +DefaultValue=0 +PDOMapping=0 + +[2112sub8] +ParameterName=int32 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0004 +AccessType=rw +DefaultValue=0 +PDOMapping=0 + +[2112sub9] +ParameterName=int32 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0004 +AccessType=rw +DefaultValue=0 +PDOMapping=0 + +[2112subA] +ParameterName=int32 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0004 +AccessType=rw +DefaultValue=0 +PDOMapping=0 + +[2112subB] +ParameterName=int32 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0004 +AccessType=rw +DefaultValue=0 +PDOMapping=0 + +[2112subC] +ParameterName=int32 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0004 +AccessType=rw +DefaultValue=0 +PDOMapping=0 + +[2112subD] +ParameterName=int32 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0004 +AccessType=rw +DefaultValue=0 +PDOMapping=0 + +[2112subE] +ParameterName=int32 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0004 +AccessType=rw +DefaultValue=0 +PDOMapping=0 + +[2112subF] +ParameterName=int32 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0004 +AccessType=rw +DefaultValue=0 +PDOMapping=0 + +[2112sub10] +ParameterName=int32 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0004 +AccessType=rw +DefaultValue=0 +PDOMapping=0 + +[2120] +ParameterName=Testing variables +ObjectType=0x9 +;StorageLocation=PERSIST_TEST +SubNumber=0xD + +[2120sub0] +ParameterName=Highest sub-index supported +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0005 +AccessType=ro +DefaultValue=0x0C +PDOMapping=0 + +[2120sub1] +ParameterName=I64 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0015 +AccessType=rw +DefaultValue=-1234567890123456789 +PDOMapping=1 + +[2120sub2] +ParameterName=U64 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x001B +AccessType=rw +DefaultValue=0x1234567890ABCDEF +PDOMapping=1 + +[2120sub3] +ParameterName=R32 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0008 +AccessType=rw +DefaultValue=12.345 +PDOMapping=1 + +[2120sub4] +ParameterName=R64 +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0011 +AccessType=rw +DefaultValue=456.789 +PDOMapping=1 + +[2120sub5] +ParameterName=Average of four numbers +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0011 +AccessType=ro +DefaultValue= +PDOMapping=1 + +[2120sub6] +ParameterName=String short +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0009 +AccessType=rw +DefaultValue=str +PDOMapping=0 + +[2120sub7] +ParameterName=String long +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0009 +AccessType=rw +DefaultValue=Example string with 1000 bytes capacity. It may contain UTF-8 characters, like '€', tabs ' ', newlines, etc. +PDOMapping=0 + +[2120sub8] +ParameterName=Octet string +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x000A +AccessType=rw +DefaultValue=C83DBB +PDOMapping=0 + +[2120sub9] +ParameterName=Parameter with default value +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0006 +AccessType=rw +DefaultValue=0x1234 +PDOMapping=0 + +[2120subA] +ParameterName=Domain +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x000F +AccessType=rw +DefaultValue= +PDOMapping=0 + +[2120subB] +ParameterName=Domain file name read +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0009 +AccessType=rw +DefaultValue=basicDevice.md +PDOMapping=0 + +[2120subC] +ParameterName=Domain file name write +ObjectType=0x7 +;StorageLocation=RAM +DataType=0x0009 +AccessType=rw +DefaultValue=fileWrittenByDomain +PDOMapping=0 + diff --git a/Devices/Libraries/Systems/CANopenSocket/examples/basicDevice/basicDevice.md b/Devices/Libraries/Systems/CANopenSocket/examples/basicDevice/basicDevice.md new file mode 100755 index 0000000..84075cd --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/examples/basicDevice/basicDevice.md @@ -0,0 +1,1012 @@ +CANopen device documentation +============================ +**Basic device** + +Basic CANopen device with example usage. + +| | | +| ------------ | ------------------------------ | +| Project File | basicDevice.xdd | +| File Version | 1 | +| Created | 28. 11. 2020 12:37:54 | +| Created By | Janez Paternoster | +| Modified | 16. 03. 2021 19:31:59 | +| Modified By | Janez Paternoster | + +This file was automatically generated with [libedssharp](https://github.com/robincornelius/libedssharp) Object Dictionary Editor v0.8-122-g6c02323 + +* [Device Information](#device-information) +* [PDO Mapping](#pdo-mapping) +* [Communication Specific Parameters](#communication-specific-parameters) +* [Manufacturer Specific Parameters](#manufacturer-specific-parameters) +* [Device Profile Specific Parameters](#device-profile-specific-parameters) + + +Device Information {#device-information} +---------------------------------------- +| | | +| ------------ | ------------------------------ | +| Vendor Name | | +| Vendor ID | | +| Product Name | Basic device | +| Product ID | | +| Granularity | 8 | +| RPDO count | 4 | +| TPDO count | 4 | +| LSS Slave | True | +| LSS Master | False | + +#### Supported Baud rates +* [x] 10 kBit/s +* [x] 20 kBit/s +* [x] 50 kBit/s +* [x] 125 kBit/s +* [x] 250 kBit/s +* [x] 500 kBit/s +* [x] 800 kBit/s +* [x] 1000 kBit/s +* [ ] auto + + +PDO Mapping {#pdo-mapping} +-------------------------- + +### RPDO 0x1400 +| | | +| ------------ | ------------------------------------------------------------- | +| COB_ID | 0x00000200+$NODEID | +| Transmission | type=255 | +| 0x62000108 | Write digital output 8-bit (Output) | +| 0x62000208 | Write digital output 8-bit (Output) | + + +### RPDO 0x1401 +| | | +| ------------ | ------------------------------------------------------------- | +| COB_ID | 0x00000300+$NODEID | +| Transmission | type=255 | +| 0x64110110 | Write analog output 16-bit (Output) | +| 0x64110210 | Write analog output 16-bit (Output) | +| 0x64110310 | Write analog output 16-bit (Output) | +| 0x64110410 | Write analog output 16-bit (Output) | + + +### TPDO 0x1800 +| | | +| ------------ | ------------------------------------------------------------- | +| COB_ID | 0x40000180+$NODEID | +| Transmission | type=255; inhibit-time=0; event-timer=0 | +| 0x60000108 | Read digital input 8-bit (Input) | +| 0x60000208 | Read digital input 8-bit (Input) | + + +### TPDO 0x1801 +| | | +| ------------ | ------------------------------------------------------------- | +| COB_ID | 0x40000280+$NODEID | +| Transmission | type=255; inhibit-time=0; event-timer=0 | +| 0x64010110 | Read analog input 16-bit (Input) | +| 0x64010210 | Read analog input 16-bit (Input) | +| 0x64010310 | Read analog input 16-bit (Input) | +| 0x64010410 | Read analog input 16-bit (Input) | + + +Communication Specific Parameters {#communication-specific-parameters} +---------------------------------------------------------------------- + +### 0x1000 - Device type +| Object Type | Count Label | Storage Group | +| ----------- | -------------- | -------------- | +| VAR | NMT | PERSIST_COMM | + +| Data Type | SDO | PDO | SRDO | Default Value | +| ----------------------- | --- | --- | ---- | ------------------------------- | +| UNSIGNED32 | ro | no | no | 0x000F0191 | + +* bit 16-31: Additional information +* bit 0-15: Device profile number + +### 0x1001 - Error register +| Object Type | Count Label | Storage Group | +| ----------- | -------------- | -------------- | +| VAR | EM | RAM | + +| Data Type | SDO | PDO | SRDO | Default Value | +| ----------------------- | --- | --- | ---- | ------------------------------- | +| UNSIGNED8 | ro | tr | no | 0x00 | + +* bit 7: manufacturer specific +* bit 6: Reserved (always 0) +* bit 5: device profile specific +* bit 4: communication error (overrun, error state) +* bit 3: temperature +* bit 2: voltage +* bit 1: current +* bit 0: generic error + +### 0x1003 - Pre-defined error field +| Object Type | Count Label | Storage Group | +| ----------- | -------------- | -------------- | +| ARRAY | | RAM | + +| Sub | Name | Data Type | SDO | PDO | SRDO | Default Value | +| ---- | --------------------- | ---------- | --- | --- | ---- | ------------- | +| 0x00 | Number of errors | UNSIGNED8 | rw | no | no | 0 | +| 0x01 | Standard error field | UNSIGNED32 | ro | no | no | 0x00000000 | +| 0x02 | Standard error field | UNSIGNED32 | ro | no | no | 0x00000000 | +| 0x03 | Standard error field | UNSIGNED32 | ro | no | no | 0x00000000 | +| 0x04 | Standard error field | UNSIGNED32 | ro | no | no | 0x00000000 | +| 0x05 | Standard error field | UNSIGNED32 | ro | no | no | 0x00000000 | +| 0x06 | Standard error field | UNSIGNED32 | ro | no | no | 0x00000000 | +| 0x07 | Standard error field | UNSIGNED32 | ro | no | no | 0x00000000 | +| 0x08 | Standard error field | UNSIGNED32 | ro | no | no | 0x00000000 | + +* Sub Index 0: Contains number of actual errors. 0 can be written to clear error history. +* sub-index 1 and above: + * bit 16-31: Manufacturer specific additional information + * bit 0-15: Error code as transmited in the Emergency object + +### 0x1005 - COB-ID SYNC message +| Object Type | Count Label | Storage Group | +| ----------- | -------------- | -------------- | +| VAR | SYNC | PERSIST_COMM | + +| Data Type | SDO | PDO | SRDO | Default Value | +| ----------------------- | --- | --- | ---- | ------------------------------- | +| UNSIGNED32 | rw | no | no | 0x00000080 | + +* bit 31: set to 0 +* bit 30: If set, CANopen device generates SYNC object +* bit 11-29: set to 0 +* bit 0-10: 11-bit CAN-ID + +### 0x1006 - Communication cycle period +| Object Type | Count Label | Storage Group | +| ----------- | -------------- | -------------- | +| VAR | SYNC_PROD | PERSIST_COMM | + +| Data Type | SDO | PDO | SRDO | Default Value | +| ----------------------- | --- | --- | ---- | ------------------------------- | +| UNSIGNED32 | rw | no | no | 0 | + +Period of SYNC transmission in µs (0 = transmission disabled). + +### 0x1007 - Synchronous window length +| Object Type | Count Label | Storage Group | +| ----------- | -------------- | -------------- | +| VAR | | PERSIST_COMM | + +| Data Type | SDO | PDO | SRDO | Default Value | +| ----------------------- | --- | --- | ---- | ------------------------------- | +| UNSIGNED32 | rw | no | no | 0 | + +Synchronous window leghth in µs (0 = not used). All synchronous PDOs must be transmitted within this time window. + +### 0x1010 - Store parameters +| Object Type | Count Label | Storage Group | +| ----------- | -------------- | -------------- | +| ARRAY | STORAGE | RAM | + +| Sub | Name | Data Type | SDO | PDO | SRDO | Default Value | +| ---- | --------------------- | ---------- | --- | --- | ---- | ------------- | +| 0x00 | Highest sub-index supported| UNSIGNED8 | ro | no | no | 0x06 | +| 0x01 | Save all parameters | UNSIGNED32 | rw | no | no | 0x00000003 | +| 0x02 | Save communication parameters| UNSIGNED32 | rw | no | no | 0x00000001 | +| 0x03 | Save application parameters| UNSIGNED32 | rw | no | no | 0x00000001 | +| 0x04 | Save LSS parameters | UNSIGNED32 | rw | no | no | 0x00000003 | +| 0x05 | Save testing parameters| UNSIGNED32 | rw | no | no | 0x00000001 | +| 0x06 | Save testing auto-store parameters| UNSIGNED32 | rw | no | no | 0x00000003 | + +Sub-indexes 1 and above: +* Reading provides information about its storage functionality: + * bit 0: If set, CANopen device saves parameters on command + * bit 1: If set, CANopen device saves parameters autonomously +* Writing value 0x65766173 ('s','a','v','e' from LSB to MSB) stores corresponding data. + +### 0x1011 - Restore default parameters +| Object Type | Count Label | Storage Group | +| ----------- | -------------- | -------------- | +| ARRAY | | RAM | + +| Sub | Name | Data Type | SDO | PDO | SRDO | Default Value | +| ---- | --------------------- | ---------- | --- | --- | ---- | ------------- | +| 0x00 | Highest sub-index supported| UNSIGNED8 | ro | no | no | 0x06 | +| 0x01 | Restore all default parameters| UNSIGNED32 | rw | no | no | 0x00000001 | +| 0x02 | Restore communication default parameters| UNSIGNED32 | rw | no | no | 0x00000001 | +| 0x03 | Restore application default parameters| UNSIGNED32 | rw | no | no | 0x00000001 | +| 0x04 | Restore LSS default parameters| UNSIGNED32 | rw | no | no | 0x00000001 | +| 0x05 | Restore testing default parameters| UNSIGNED32 | rw | no | no | 0x00000001 | +| 0x06 | Restore testing auto-store default parameters| UNSIGNED32 | rw | no | no | 0x00000001 | + +Sub-indexes 1 and above: +* Reading provides information about its restoring capability: + * bit 0: If set, CANopen device restores parameters +* Writing value 0x64616F6C ('l','o','a','d' from LSB to MSB) restores corresponding data. + +### 0x1012 - COB-ID time stamp object +| Object Type | Count Label | Storage Group | +| ----------- | -------------- | -------------- | +| VAR | TIME | PERSIST_COMM | + +| Data Type | SDO | PDO | SRDO | Default Value | +| ----------------------- | --- | --- | ---- | ------------------------------- | +| UNSIGNED32 | rw | no | no | 0x00000100 | + +* bit 31: If set, CANopen device consumes TIME message +* bit 30: If set, CANopen device produces TIME message +* bit 11-29: set to 0 +* bit 0-10: 11-bit CAN-ID + +### 0x1014 - COB-ID EMCY +| Object Type | Count Label | Storage Group | +| ----------- | -------------- | -------------- | +| VAR | EM_PROD | PERSIST_COMM | + +| Data Type | SDO | PDO | SRDO | Default Value | +| ----------------------- | --- | --- | ---- | ------------------------------- | +| UNSIGNED32 | rw | no | no | 0x80+$NODEID | + +* bit 31: If set, EMCY does NOT exist / is NOT valid +* bit 11-30: set to 0 +* bit 0-10: 11-bit CAN-ID + +### 0x1015 - Inhibit time EMCY +| Object Type | Count Label | Storage Group | +| ----------- | -------------- | -------------- | +| VAR | | PERSIST_COMM | + +| Data Type | SDO | PDO | SRDO | Default Value | +| ----------------------- | --- | --- | ---- | ------------------------------- | +| UNSIGNED16 | rw | no | no | 0 | + +Inhibit time of emergency message in multiples of 100µs. The value 0 disables the inhibit time. + +### 0x1016 - Consumer heartbeat time +| Object Type | Count Label | Storage Group | +| ----------- | -------------- | -------------- | +| ARRAY | HB_CONS | PERSIST_COMM | + +| Sub | Name | Data Type | SDO | PDO | SRDO | Default Value | +| ---- | --------------------- | ---------- | --- | --- | ---- | ------------- | +| 0x00 | Highest sub-index supported| UNSIGNED8 | ro | no | no | 0x08 | +| 0x01 | Consumer heartbeat time| UNSIGNED32 | rw | no | no | 0x00000000 | +| 0x02 | Consumer heartbeat time| UNSIGNED32 | rw | no | no | 0x00000000 | +| 0x03 | Consumer heartbeat time| UNSIGNED32 | rw | no | no | 0x00000000 | +| 0x04 | Consumer heartbeat time| UNSIGNED32 | rw | no | no | 0x00000000 | +| 0x05 | Consumer heartbeat time| UNSIGNED32 | rw | no | no | 0x00000000 | +| 0x06 | Consumer heartbeat time| UNSIGNED32 | rw | no | no | 0x00000000 | +| 0x07 | Consumer heartbeat time| UNSIGNED32 | rw | no | no | 0x00000000 | +| 0x08 | Consumer heartbeat time| UNSIGNED32 | rw | no | no | 0x00000000 | + +Consumer Heartbeat Time: + * bit 24-31: set to 0 + * bit 16-23: Node ID of the monitored node. If 0 or greater than 127, sub-entry is not used. + * bit 0-15: Heartbeat time in ms (if 0, sub-intry is not used). Value should be higher than the corresponding producer heartbeat time. + +### 0x1017 - Producer heartbeat time +| Object Type | Count Label | Storage Group | +| ----------- | -------------- | -------------- | +| VAR | HB_PROD | PERSIST_COMM | + +| Data Type | SDO | PDO | SRDO | Default Value | +| ----------------------- | --- | --- | ---- | ------------------------------- | +| UNSIGNED16 | rw | no | no | 0 | + +Heartbeat producer time in ms (0 = disable transmission). + +### 0x1018 - Identity +| Object Type | Count Label | Storage Group | +| ----------- | -------------- | -------------- | +| RECORD | | PERSIST_COMM | + +| Sub | Name | Data Type | SDO | PDO | SRDO | Default Value | +| ---- | --------------------- | ---------- | --- | --- | ---- | ------------- | +| 0x00 | Highest sub-index supported| UNSIGNED8 | ro | no | no | 0x04 | +| 0x01 | Vendor-ID | UNSIGNED32 | ro | no | no | 0x00000000 | +| 0x02 | Product code | UNSIGNED32 | ro | no | no | 0x00000000 | +| 0x03 | Revision number | UNSIGNED32 | ro | no | no | 0x00000000 | +| 0x04 | Serial number | UNSIGNED32 | ro | no | no | 0x00000000 | + +* Vendor-ID, assigned by CiA +* Product code, manufacturer specific +* Revision number: + * bit 16-31: Major revision number (CANopen behavior has changed) + * bit 0-15: Minor revision num. (CANopen behavior has not changed) +* Serial number, manufacturer specific + +### 0x1019 - Synchronous counter overflow value +| Object Type | Count Label | Storage Group | +| ----------- | -------------- | -------------- | +| VAR | | PERSIST_COMM | + +| Data Type | SDO | PDO | SRDO | Default Value | +| ----------------------- | --- | --- | ---- | ------------------------------- | +| UNSIGNED8 | rw | no | no | 0 | + +* Value 0: SYNC message is transmitted with data length 0. +* Value 1: reserved. +* Value 2-240: SYNC message has one data byte, which contains the counter. +* Value 241-255: reserved. + +### 0x1200 - SDO server parameter +| Object Type | Count Label | Storage Group | +| ----------- | -------------- | -------------- | +| RECORD | SDO_SRV | RAM | + +| Sub | Name | Data Type | SDO | PDO | SRDO | Default Value | +| ---- | --------------------- | ---------- | --- | --- | ---- | ------------- | +| 0x00 | Highest sub-index supported| UNSIGNED8 | ro | no | no | 2 | +| 0x01 | COB-ID client to server (rx)| UNSIGNED32 | ro | t | no | 0x600+$NODEID | +| 0x02 | COB-ID server to client (tx)| UNSIGNED32 | ro | t | no | 0x580+$NODEID | + +Sub-indexes 1 and 2: +* bit 11-31: set to 0 +* bit 0-10: 11-bit CAN-ID + +### 0x1280 - SDO client parameter +| Object Type | Count Label | Storage Group | +| ----------- | -------------- | -------------- | +| RECORD | SDO_CLI | PERSIST_COMM | + +| Sub | Name | Data Type | SDO | PDO | SRDO | Default Value | +| ---- | --------------------- | ---------- | --- | --- | ---- | ------------- | +| 0x00 | Highest sub-index supported| UNSIGNED8 | ro | no | no | 0x03 | +| 0x01 | COB-ID client to server (tx)| UNSIGNED32 | rw | tr | no | 0x80000000 | +| 0x02 | COB-ID server to client (rx)| UNSIGNED32 | rw | tr | no | 0x80000000 | +| 0x03 | Node-ID of the SDO server| UNSIGNED8 | rw | no | no | 0x01 | + +* Sub-indexes 1 and 2: + * bit 31: If set, SDO does NOT exist / is NOT valid + * bit 30: If set, value is assigned dynamically + * bit 11-29: set to 0 + * bit 0-10: 11-bit CAN-ID +* Node-ID of the SDO server, 0x01 to 0x7F + +### 0x1400 - RPDO communication parameter +| Object Type | Count Label | Storage Group | +| ----------- | -------------- | -------------- | +| RECORD | RPDO | PERSIST_COMM | + +| Sub | Name | Data Type | SDO | PDO | SRDO | Default Value | +| ---- | --------------------- | ---------- | --- | --- | ---- | ------------- | +| 0x00 | Highest sub-index supported| UNSIGNED8 | ro | no | no | 0x05 | +| 0x01 | COB-ID used by RPDO | UNSIGNED32 | rw | no | no | 0x00000200+$NODEID| +| 0x02 | Transmission type | UNSIGNED8 | rw | no | no | 255 | +| 0x05 | Event timer | UNSIGNED16 | rw | no | no | 0 | + +* COB-ID used by RPDO: + * bit 31: If set, PDO does not exist / is not valid + * bit 11-30: set to 0 + * bit 0-10: 11-bit CAN-ID +* Transmission type: + * Value 0-240: synchronous, processed after next reception of SYNC object + * Value 241-253: not used + * Value 254: event-driven (manufacturer-specific) + * Value 255: event-driven (device profile and application profile specific) +* Event timer in ms (0 = disabled) for deadline monitoring. + +### 0x1401 - RPDO communication parameter +| Object Type | Count Label | Storage Group | +| ----------- | -------------- | -------------- | +| RECORD | RPDO | PERSIST_COMM | + +| Sub | Name | Data Type | SDO | PDO | SRDO | Default Value | +| ---- | --------------------- | ---------- | --- | --- | ---- | ------------- | +| 0x00 | Highest sub-index supported| UNSIGNED8 | ro | no | no | 0x05 | +| 0x01 | COB-ID used by RPDO | UNSIGNED32 | rw | no | no | 0x00000300+$NODEID| +| 0x02 | Transmission type | UNSIGNED8 | rw | no | no | 255 | +| 0x05 | Event timer | UNSIGNED16 | rw | no | no | 0 | + +* COB-ID used by RPDO: + * bit 31: If set, PDO does not exist / is not valid + * bit 11-30: set to 0 + * bit 0-10: 11-bit CAN-ID +* Transmission type: + * Value 0-240: synchronous, processed after next reception of SYNC object + * Value 241-253: not used + * Value 254: event-driven (manufacturer-specific) + * Value 255: event-driven (device profile and application profile specific) +* Event timer in ms (0 = disabled) for deadline monitoring. + +### 0x1402 - RPDO communication parameter +| Object Type | Count Label | Storage Group | +| ----------- | -------------- | -------------- | +| RECORD | RPDO | PERSIST_COMM | + +| Sub | Name | Data Type | SDO | PDO | SRDO | Default Value | +| ---- | --------------------- | ---------- | --- | --- | ---- | ------------- | +| 0x00 | Highest sub-index supported| UNSIGNED8 | ro | no | no | 0x05 | +| 0x01 | COB-ID used by RPDO | UNSIGNED32 | rw | no | no | 0x80000400+$NODEID| +| 0x02 | Transmission type | UNSIGNED8 | rw | no | no | 254 | +| 0x05 | Event timer | UNSIGNED16 | rw | no | no | 0 | + +* COB-ID used by RPDO: + * bit 31: If set, PDO does not exist / is not valid + * bit 11-30: set to 0 + * bit 0-10: 11-bit CAN-ID +* Transmission type: + * Value 0-240: synchronous, processed after next reception of SYNC object + * Value 241-253: not used + * Value 254: event-driven (manufacturer-specific) + * Value 255: event-driven (device profile and application profile specific) +* Event timer in ms (0 = disabled) for deadline monitoring. + +### 0x1403 - RPDO communication parameter +| Object Type | Count Label | Storage Group | +| ----------- | -------------- | -------------- | +| RECORD | RPDO | PERSIST_COMM | + +| Sub | Name | Data Type | SDO | PDO | SRDO | Default Value | +| ---- | --------------------- | ---------- | --- | --- | ---- | ------------- | +| 0x00 | Highest sub-index supported| UNSIGNED8 | ro | no | no | 0x05 | +| 0x01 | COB-ID used by RPDO | UNSIGNED32 | rw | no | no | 0x80000500+$NODEID| +| 0x02 | Transmission type | UNSIGNED8 | rw | no | no | 254 | +| 0x05 | Event timer | UNSIGNED16 | rw | no | no | 0 | + +* COB-ID used by RPDO: + * bit 31: If set, PDO does not exist / is not valid + * bit 11-30: set to 0 + * bit 0-10: 11-bit CAN-ID +* Transmission type: + * Value 0-240: synchronous, processed after next reception of SYNC object + * Value 241-253: not used + * Value 254: event-driven (manufacturer-specific) + * Value 255: event-driven (device profile and application profile specific) +* Event timer in ms (0 = disabled) for deadline monitoring. + +### 0x1600 - RPDO mapping parameter +| Object Type | Count Label | Storage Group | +| ----------- | -------------- | -------------- | +| RECORD | | PERSIST_COMM | + +| Sub | Name | Data Type | SDO | PDO | SRDO | Default Value | +| ---- | --------------------- | ---------- | --- | --- | ---- | ------------- | +| 0x00 | Number of mapped application objects in PDO| UNSIGNED8 | rw | no | no | 2 | +| 0x01 | Application object 1 | UNSIGNED32 | rw | no | no | 0x62000108 | +| 0x02 | Application object 2 | UNSIGNED32 | rw | no | no | 0x62000208 | +| 0x03 | Application object 3 | UNSIGNED32 | rw | no | no | 0x62000308 | +| 0x04 | Application object 4 | UNSIGNED32 | rw | no | no | 0x62000408 | +| 0x05 | Application object 5 | UNSIGNED32 | rw | no | no | 0x62000508 | +| 0x06 | Application object 6 | UNSIGNED32 | rw | no | no | 0x62000608 | +| 0x07 | Application object 7 | UNSIGNED32 | rw | no | no | 0x62000708 | +| 0x08 | Application object 8 | UNSIGNED32 | rw | no | no | 0x62000808 | + +* Number of mapped application objects in PDO: + * Value 0: mapping is disabled. + * Value 1: sub-index 0x01 is valid. + * Value 2-8: sub-indexes 0x01 to (0x02 to 0x08) are valid. +* Application object 1-8: + * bit 16-31: index + * bit 8-15: sub-index + * bit 0-7: data length in bits + +### 0x1601 - RPDO mapping parameter +| Object Type | Count Label | Storage Group | +| ----------- | -------------- | -------------- | +| RECORD | | PERSIST_COMM | + +| Sub | Name | Data Type | SDO | PDO | SRDO | Default Value | +| ---- | --------------------- | ---------- | --- | --- | ---- | ------------- | +| 0x00 | Number of mapped application objects in PDO| UNSIGNED8 | rw | no | no | 4 | +| 0x01 | Application object 1 | UNSIGNED32 | rw | no | no | 0x64110110 | +| 0x02 | Application object 2 | UNSIGNED32 | rw | no | no | 0x64110210 | +| 0x03 | Application object 3 | UNSIGNED32 | rw | no | no | 0x64110310 | +| 0x04 | Application object 4 | UNSIGNED32 | rw | no | no | 0x64110410 | +| 0x05 | Application object 5 | UNSIGNED32 | rw | no | no | 0x00000000 | +| 0x06 | Application object 6 | UNSIGNED32 | rw | no | no | 0x00000000 | +| 0x07 | Application object 7 | UNSIGNED32 | rw | no | no | 0x00000000 | +| 0x08 | Application object 8 | UNSIGNED32 | rw | no | no | 0x00000000 | + +* Number of mapped application objects in PDO: + * Value 0: mapping is disabled. + * Value 1: sub-index 0x01 is valid. + * Value 2-8: sub-indexes 0x01 to (0x02 to 0x08) are valid. +* Application object 1-8: + * bit 16-31: index + * bit 8-15: sub-index + * bit 0-7: data length in bits + +### 0x1602 - RPDO mapping parameter +| Object Type | Count Label | Storage Group | +| ----------- | -------------- | -------------- | +| RECORD | | PERSIST_COMM | + +| Sub | Name | Data Type | SDO | PDO | SRDO | Default Value | +| ---- | --------------------- | ---------- | --- | --- | ---- | ------------- | +| 0x00 | Number of mapped application objects in PDO| UNSIGNED8 | rw | no | no | 0 | +| 0x01 | Application object 1 | UNSIGNED32 | rw | no | no | 0x00000000 | +| 0x02 | Application object 2 | UNSIGNED32 | rw | no | no | 0x00000000 | +| 0x03 | Application object 3 | UNSIGNED32 | rw | no | no | 0x00000000 | +| 0x04 | Application object 4 | UNSIGNED32 | rw | no | no | 0x00000000 | +| 0x05 | Application object 5 | UNSIGNED32 | rw | no | no | 0x00000000 | +| 0x06 | Application object 6 | UNSIGNED32 | rw | no | no | 0x00000000 | +| 0x07 | Application object 7 | UNSIGNED32 | rw | no | no | 0x00000000 | +| 0x08 | Application object 8 | UNSIGNED32 | rw | no | no | 0x00000000 | + +* Number of mapped application objects in PDO: + * Value 0: mapping is disabled. + * Value 1: sub-index 0x01 is valid. + * Value 2-8: sub-indexes 0x01 to (0x02 to 0x08) are valid. +* Application object 1-8: + * bit 16-31: index + * bit 8-15: sub-index + * bit 0-7: data length in bits + +### 0x1603 - RPDO mapping parameter +| Object Type | Count Label | Storage Group | +| ----------- | -------------- | -------------- | +| RECORD | | PERSIST_COMM | + +| Sub | Name | Data Type | SDO | PDO | SRDO | Default Value | +| ---- | --------------------- | ---------- | --- | --- | ---- | ------------- | +| 0x00 | Number of mapped application objects in PDO| UNSIGNED8 | rw | no | no | 0 | +| 0x01 | Application object 1 | UNSIGNED32 | rw | no | no | 0x00000000 | +| 0x02 | Application object 2 | UNSIGNED32 | rw | no | no | 0x00000000 | +| 0x03 | Application object 3 | UNSIGNED32 | rw | no | no | 0x00000000 | +| 0x04 | Application object 4 | UNSIGNED32 | rw | no | no | 0x00000000 | +| 0x05 | Application object 5 | UNSIGNED32 | rw | no | no | 0x00000000 | +| 0x06 | Application object 6 | UNSIGNED32 | rw | no | no | 0x00000000 | +| 0x07 | Application object 7 | UNSIGNED32 | rw | no | no | 0x00000000 | +| 0x08 | Application object 8 | UNSIGNED32 | rw | no | no | 0x00000000 | + +* Number of mapped application objects in PDO: + * Value 0: mapping is disabled. + * Value 1: sub-index 0x01 is valid. + * Value 2-8: sub-indexes 0x01 to (0x02 to 0x08) are valid. +* Application object 1-8: + * bit 16-31: index + * bit 8-15: sub-index + * bit 0-7: data length in bits + +### 0x1800 - TPDO communication parameter +| Object Type | Count Label | Storage Group | +| ----------- | -------------- | -------------- | +| RECORD | TPDO | PERSIST_COMM | + +| Sub | Name | Data Type | SDO | PDO | SRDO | Default Value | +| ---- | --------------------- | ---------- | --- | --- | ---- | ------------- | +| 0x00 | Highest sub-index supported| UNSIGNED8 | ro | no | no | 0x06 | +| 0x01 | COB-ID used by TPDO | UNSIGNED32 | rw | no | no | 0x40000180+$NODEID| +| 0x02 | Transmission type | UNSIGNED8 | rw | no | no | 255 | +| 0x03 | Inhibit time | UNSIGNED16 | rw | no | no | 0 | +| 0x05 | Event timer | UNSIGNED16 | rw | no | no | 0 | +| 0x06 | SYNC start value | UNSIGNED8 | rw | no | no | 0 | + +* COB-ID used by RPDO: + * bit 31: If set, PDO does not exist / is not valid + * bit 30: If set, NO RTR is allowed on this PDO + * bit 11-29: set to 0 + * bit 0-10: 11-bit CAN-ID +* Transmission type: + * Value 0: synchronous (acyclic) + * Value 1-240: synchronous (cyclic every (1-240)-th sync) + * Value 241-253: not used + * Value 254: event-driven (manufacturer-specific) + * Value 255: event-driven (device profile and application profile specific) +* Inhibit time in multiple of 100µs, if the transmission type is set to 254 or 255 (0 = disabled). +* Event timer interval in ms, if the transmission type is set to 254 or 255 (0 = disabled). +* SYNC start value + * Value 0: Counter of the SYNC message shall not be processed. + * Value 1-240: The SYNC message with the counter value equal to this value shall be regarded as the first received SYNC message. + +### 0x1801 - TPDO communication parameter +| Object Type | Count Label | Storage Group | +| ----------- | -------------- | -------------- | +| RECORD | TPDO | PERSIST_COMM | + +| Sub | Name | Data Type | SDO | PDO | SRDO | Default Value | +| ---- | --------------------- | ---------- | --- | --- | ---- | ------------- | +| 0x00 | Highest sub-index supported| UNSIGNED8 | ro | no | no | 0x06 | +| 0x01 | COB-ID used by TPDO | UNSIGNED32 | rw | no | no | 0x40000280+$NODEID| +| 0x02 | Transmission type | UNSIGNED8 | rw | no | no | 255 | +| 0x03 | Inhibit time | UNSIGNED16 | rw | no | no | 0 | +| 0x05 | Event timer | UNSIGNED16 | rw | no | no | 0 | +| 0x06 | SYNC start value | UNSIGNED8 | rw | no | no | 0 | + +* COB-ID used by RPDO: + * bit 31: If set, PDO does not exist / is not valid + * bit 30: If set, NO RTR is allowed on this PDO + * bit 11-29: set to 0 + * bit 0-10: 11-bit CAN-ID +* Transmission type: + * Value 0: synchronous (acyclic) + * Value 1-240: synchronous (cyclic every (1-240)-th sync) + * Value 241-253: not used + * Value 254: event-driven (manufacturer-specific) + * Value 255: event-driven (device profile and application profile specific) +* Inhibit time in multiple of 100µs, if the transmission type is set to 254 or 255 (0 = disabled). +* Event timer interval in ms, if the transmission type is set to 254 or 255 (0 = disabled). +* SYNC start value + * Value 0: Counter of the SYNC message shall not be processed. + * Value 1-240: The SYNC message with the counter value equal to this value shall be regarded as the first received SYNC message. + +### 0x1802 - TPDO communication parameter +| Object Type | Count Label | Storage Group | +| ----------- | -------------- | -------------- | +| RECORD | TPDO | PERSIST_COMM | + +| Sub | Name | Data Type | SDO | PDO | SRDO | Default Value | +| ---- | --------------------- | ---------- | --- | --- | ---- | ------------- | +| 0x00 | Highest sub-index supported| UNSIGNED8 | ro | no | no | 0x06 | +| 0x01 | COB-ID used by TPDO | UNSIGNED32 | rw | no | no | 0xC0000380+$NODEID| +| 0x02 | Transmission type | UNSIGNED8 | rw | no | no | 254 | +| 0x03 | Inhibit time | UNSIGNED16 | rw | no | no | 0 | +| 0x05 | Event timer | UNSIGNED16 | rw | no | no | 0 | +| 0x06 | SYNC start value | UNSIGNED8 | rw | no | no | 0 | + +* COB-ID used by RPDO: + * bit 31: If set, PDO does not exist / is not valid + * bit 30: If set, NO RTR is allowed on this PDO + * bit 11-29: set to 0 + * bit 0-10: 11-bit CAN-ID +* Transmission type: + * Value 0: synchronous (acyclic) + * Value 1-240: synchronous (cyclic every (1-240)-th sync) + * Value 241-253: not used + * Value 254: event-driven (manufacturer-specific) + * Value 255: event-driven (device profile and application profile specific) +* Inhibit time in multiple of 100µs, if the transmission type is set to 254 or 255 (0 = disabled). +* Event timer interval in ms, if the transmission type is set to 254 or 255 (0 = disabled). +* SYNC start value + * Value 0: Counter of the SYNC message shall not be processed. + * Value 1-240: The SYNC message with the counter value equal to this value shall be regarded as the first received SYNC message. + +### 0x1803 - TPDO communication parameter +| Object Type | Count Label | Storage Group | +| ----------- | -------------- | -------------- | +| RECORD | TPDO | PERSIST_COMM | + +| Sub | Name | Data Type | SDO | PDO | SRDO | Default Value | +| ---- | --------------------- | ---------- | --- | --- | ---- | ------------- | +| 0x00 | Highest sub-index supported| UNSIGNED8 | ro | no | no | 0x06 | +| 0x01 | COB-ID used by TPDO | UNSIGNED32 | rw | no | no | 0xC0000480+$NODEID| +| 0x02 | Transmission type | UNSIGNED8 | rw | no | no | 254 | +| 0x03 | Inhibit time | UNSIGNED16 | rw | no | no | 0 | +| 0x05 | Event timer | UNSIGNED16 | rw | no | no | 0 | +| 0x06 | SYNC start value | UNSIGNED8 | rw | no | no | 0 | + +* COB-ID used by RPDO: + * bit 31: If set, PDO does not exist / is not valid + * bit 30: If set, NO RTR is allowed on this PDO + * bit 11-29: set to 0 + * bit 0-10: 11-bit CAN-ID +* Transmission type: + * Value 0: synchronous (acyclic) + * Value 1-240: synchronous (cyclic every (1-240)-th sync) + * Value 241-253: not used + * Value 254: event-driven (manufacturer-specific) + * Value 255: event-driven (device profile and application profile specific) +* Inhibit time in multiple of 100µs, if the transmission type is set to 254 or 255 (0 = disabled). +* Event timer interval in ms, if the transmission type is set to 254 or 255 (0 = disabled). +* SYNC start value + * Value 0: Counter of the SYNC message shall not be processed. + * Value 1-240: The SYNC message with the counter value equal to this value shall be regarded as the first received SYNC message. + +### 0x1A00 - TPDO mapping parameter +| Object Type | Count Label | Storage Group | +| ----------- | -------------- | -------------- | +| RECORD | | PERSIST_COMM | + +| Sub | Name | Data Type | SDO | PDO | SRDO | Default Value | +| ---- | --------------------- | ---------- | --- | --- | ---- | ------------- | +| 0x00 | Number of mapped application objects in PDO| UNSIGNED8 | rw | no | no | 2 | +| 0x01 | Application object 1 | UNSIGNED32 | rw | no | no | 0x60000108 | +| 0x02 | Application object 2 | UNSIGNED32 | rw | no | no | 0x60000208 | +| 0x03 | Application object 3 | UNSIGNED32 | rw | no | no | 0x60000308 | +| 0x04 | Application object 4 | UNSIGNED32 | rw | no | no | 0x60000408 | +| 0x05 | Application object 5 | UNSIGNED32 | rw | no | no | 0x60000508 | +| 0x06 | Application object 6 | UNSIGNED32 | rw | no | no | 0x60000608 | +| 0x07 | Application object 7 | UNSIGNED32 | rw | no | no | 0x60000708 | +| 0x08 | Application object 8 | UNSIGNED32 | rw | no | no | 0x60000808 | + +* Number of mapped application objects in PDO: + * Value 0: mapping is disabled. + * Value 1: sub-index 0x01 is valid. + * Value 2-8: sub-indexes 0x01 to (0x02 to 0x08) are valid. +* Application object 1-8: + * bit 16-31: index + * bit 8-15: sub-index + * bit 0-7: data length in bits + +### 0x1A01 - TPDO mapping parameter +| Object Type | Count Label | Storage Group | +| ----------- | -------------- | -------------- | +| RECORD | | PERSIST_COMM | + +| Sub | Name | Data Type | SDO | PDO | SRDO | Default Value | +| ---- | --------------------- | ---------- | --- | --- | ---- | ------------- | +| 0x00 | Number of mapped application objects in PDO| UNSIGNED8 | rw | no | no | 4 | +| 0x01 | Application object 1 | UNSIGNED32 | rw | no | no | 0x64010110 | +| 0x02 | Application object 2 | UNSIGNED32 | rw | no | no | 0x64010210 | +| 0x03 | Application object 3 | UNSIGNED32 | rw | no | no | 0x64010310 | +| 0x04 | Application object 4 | UNSIGNED32 | rw | no | no | 0x64010410 | +| 0x05 | Application object 5 | UNSIGNED32 | rw | no | no | 0x00000000 | +| 0x06 | Application object 6 | UNSIGNED32 | rw | no | no | 0x00000000 | +| 0x07 | Application object 7 | UNSIGNED32 | rw | no | no | 0x00000000 | +| 0x08 | Application object 8 | UNSIGNED32 | rw | no | no | 0x00000000 | + +* Number of mapped application objects in PDO: + * Value 0: mapping is disabled. + * Value 1: sub-index 0x01 is valid. + * Value 2-8: sub-indexes 0x01 to (0x02 to 0x08) are valid. +* Application object 1-8: + * bit 16-31: index + * bit 8-15: sub-index + * bit 0-7: data length in bits + +### 0x1A02 - TPDO mapping parameter +| Object Type | Count Label | Storage Group | +| ----------- | -------------- | -------------- | +| RECORD | | PERSIST_COMM | + +| Sub | Name | Data Type | SDO | PDO | SRDO | Default Value | +| ---- | --------------------- | ---------- | --- | --- | ---- | ------------- | +| 0x00 | Number of mapped application objects in PDO| UNSIGNED8 | rw | no | no | 0 | +| 0x01 | Application object 1 | UNSIGNED32 | rw | no | no | 0x00000000 | +| 0x02 | Application object 2 | UNSIGNED32 | rw | no | no | 0x00000000 | +| 0x03 | Application object 3 | UNSIGNED32 | rw | no | no | 0x00000000 | +| 0x04 | Application object 4 | UNSIGNED32 | rw | no | no | 0x00000000 | +| 0x05 | Application object 5 | UNSIGNED32 | rw | no | no | 0x00000000 | +| 0x06 | Application object 6 | UNSIGNED32 | rw | no | no | 0x00000000 | +| 0x07 | Application object 7 | UNSIGNED32 | rw | no | no | 0x00000000 | +| 0x08 | Application object 8 | UNSIGNED32 | rw | no | no | 0x00000000 | + +* Number of mapped application objects in PDO: + * Value 0: mapping is disabled. + * Value 1: sub-index 0x01 is valid. + * Value 2-8: sub-indexes 0x01 to (0x02 to 0x08) are valid. +* Application object 1-8: + * bit 16-31: index + * bit 8-15: sub-index + * bit 0-7: data length in bits + +### 0x1A03 - TPDO mapping parameter +| Object Type | Count Label | Storage Group | +| ----------- | -------------- | -------------- | +| RECORD | | PERSIST_COMM | + +| Sub | Name | Data Type | SDO | PDO | SRDO | Default Value | +| ---- | --------------------- | ---------- | --- | --- | ---- | ------------- | +| 0x00 | Number of mapped application objects in PDO| UNSIGNED8 | rw | no | no | 0 | +| 0x01 | Application object 1 | UNSIGNED32 | rw | no | no | 0x00000000 | +| 0x02 | Application object 2 | UNSIGNED32 | rw | no | no | 0x00000000 | +| 0x03 | Application object 3 | UNSIGNED32 | rw | no | no | 0x00000000 | +| 0x04 | Application object 4 | UNSIGNED32 | rw | no | no | 0x00000000 | +| 0x05 | Application object 5 | UNSIGNED32 | rw | no | no | 0x00000000 | +| 0x06 | Application object 6 | UNSIGNED32 | rw | no | no | 0x00000000 | +| 0x07 | Application object 7 | UNSIGNED32 | rw | no | no | 0x00000000 | +| 0x08 | Application object 8 | UNSIGNED32 | rw | no | no | 0x00000000 | + +* Number of mapped application objects in PDO: + * Value 0: mapping is disabled. + * Value 1: sub-index 0x01 is valid. + * Value 2-8: sub-indexes 0x01 to (0x02 to 0x08) are valid. +* Application object 1-8: + * bit 16-31: index + * bit 8-15: sub-index + * bit 0-7: data length in bits + +Manufacturer Specific Parameters {#manufacturer-specific-parameters} +-------------------------------------------------------------------- + +### 0x2100 - Error status bits +| Object Type | Count Label | Storage Group | +| ----------- | -------------- | -------------- | +| VAR | | RAM | + +| Data Type | SDO | PDO | SRDO | Default Value | +| ----------------------- | --- | --- | ---- | ------------------------------- | +| OCTET_STRING | ro | t | no | 00 00 00 00 00 00 00 00 00 00 | + +Error Status Bits indicates internal error conditions inside stack or inside application. For details see CO_EM_errorStatusBits_t in CO_Emergency.h file or in https://canopennode.github.io/CANopenSocket/group__CO__Emergency.html + +### 0x2105 - Version +| Object Type | Count Label | Storage Group | +| ----------- | -------------- | -------------- | +| RECORD | | RAM | + +| Sub | Name | Data Type | SDO | PDO | SRDO | Default Value | +| ---- | --------------------- | ---------- | --- | --- | ---- | ------------- | +| 0x00 | Highest sub-index supported| UNSIGNED8 | ro | no | no | 0x02 | +| 0x01 | CANopenNode | VISIBLE_STRING| ro | no | no | | +| 0x02 | Application | VISIBLE_STRING| ro | no | no | | + +### 0x2106 - Power-on counter +| Object Type | Count Label | Storage Group | +| ----------- | -------------- | -------------- | +| VAR | | PERSIST_TEST_AUTO| + +| Data Type | SDO | PDO | SRDO | Default Value | +| ----------------------- | --- | --- | ---- | ------------------------------- | +| UNSIGNED32 | ro | no | no | 0 | + +Power on Counter counts total microcontroller resets in it's lifetime. Variable is an example of EEPROM usage. + +### 0x2110 - Variable Int32 +| Object Type | Count Label | Storage Group | +| ----------- | -------------- | -------------- | +| ARRAY | | RAM | + +| Sub | Name | Data Type | SDO | PDO | SRDO | Default Value | +| ---- | --------------------- | ---------- | --- | --- | ---- | ------------- | +| 0x00 | Highest sub-index supported| UNSIGNED8 | ro | tr | no | 0x10 | +| 0x01 | int32 | INTEGER32 | rw | tr | no | 0 | +| 0x02 | int32 | INTEGER32 | rw | tr | no | 0 | +| 0x03 | int32 | INTEGER32 | rw | tr | no | 0 | +| 0x04 | int32 | INTEGER32 | rw | tr | no | 0 | +| 0x05 | int32 | INTEGER32 | rw | tr | no | 0 | +| 0x06 | int32 | INTEGER32 | rw | tr | no | 0 | +| 0x07 | int32 | INTEGER32 | rw | tr | no | 0 | +| 0x08 | int32 | INTEGER32 | rw | tr | no | 0 | +| 0x09 | int32 | INTEGER32 | rw | tr | no | 0 | +| 0x0A | int32 | INTEGER32 | rw | tr | no | 0 | +| 0x0B | int32 | INTEGER32 | rw | tr | no | 0 | +| 0x0C | int32 | INTEGER32 | rw | tr | no | 0 | +| 0x0D | int32 | INTEGER32 | rw | tr | no | 0 | +| 0x0E | int32 | INTEGER32 | rw | tr | no | 0 | +| 0x0F | int32 | INTEGER32 | rw | tr | no | 0 | +| 0x10 | int32 | INTEGER32 | rw | tr | no | 0 | + +Vartiable is free to use by application. + +### 0x2111 - Variable Int32 save +| Object Type | Count Label | Storage Group | +| ----------- | -------------- | -------------- | +| ARRAY | | PERSIST_TEST | + +| Sub | Name | Data Type | SDO | PDO | SRDO | Default Value | +| ---- | --------------------- | ---------- | --- | --- | ---- | ------------- | +| 0x00 | Highest sub-index supported| UNSIGNED8 | ro | tr | no | 0x10 | +| 0x01 | int32 | INTEGER32 | rw | no | no | 0 | +| 0x02 | int32 | INTEGER32 | rw | no | no | 0 | +| 0x03 | int32 | INTEGER32 | rw | no | no | 0 | +| 0x04 | int32 | INTEGER32 | rw | no | no | 0 | +| 0x05 | int32 | INTEGER32 | rw | no | no | 0 | +| 0x06 | int32 | INTEGER32 | rw | no | no | 0 | +| 0x07 | int32 | INTEGER32 | rw | no | no | 0 | +| 0x08 | int32 | INTEGER32 | rw | no | no | 0 | +| 0x09 | int32 | INTEGER32 | rw | no | no | 0 | +| 0x0A | int32 | INTEGER32 | rw | no | no | 0 | +| 0x0B | int32 | INTEGER32 | rw | no | no | 0 | +| 0x0C | int32 | INTEGER32 | rw | no | no | 0 | +| 0x0D | int32 | INTEGER32 | rw | no | no | 0 | +| 0x0E | int32 | INTEGER32 | rw | no | no | 0 | +| 0x0F | int32 | INTEGER32 | rw | no | no | 0 | +| 0x10 | int32 | INTEGER32 | rw | no | no | 0 | + +Vartiable is free to use by application. Variables can be saved on command. + +### 0x2112 - Variable NV Int32 auto save +| Object Type | Count Label | Storage Group | +| ----------- | -------------- | -------------- | +| ARRAY | | PERSIST_TEST_AUTO| + +| Sub | Name | Data Type | SDO | PDO | SRDO | Default Value | +| ---- | --------------------- | ---------- | --- | --- | ---- | ------------- | +| 0x00 | Highest sub-index supported| UNSIGNED8 | ro | tr | no | 0x10 | +| 0x01 | int32 | INTEGER32 | rw | no | no | 0 | +| 0x02 | int32 | INTEGER32 | rw | no | no | 0 | +| 0x03 | int32 | INTEGER32 | rw | no | no | 0 | +| 0x04 | int32 | INTEGER32 | rw | no | no | 0 | +| 0x05 | int32 | INTEGER32 | rw | no | no | 0 | +| 0x06 | int32 | INTEGER32 | rw | no | no | 0 | +| 0x07 | int32 | INTEGER32 | rw | no | no | 0 | +| 0x08 | int32 | INTEGER32 | rw | no | no | 0 | +| 0x09 | int32 | INTEGER32 | rw | no | no | 0 | +| 0x0A | int32 | INTEGER32 | rw | no | no | 0 | +| 0x0B | int32 | INTEGER32 | rw | no | no | 0 | +| 0x0C | int32 | INTEGER32 | rw | no | no | 0 | +| 0x0D | int32 | INTEGER32 | rw | no | no | 0 | +| 0x0E | int32 | INTEGER32 | rw | no | no | 0 | +| 0x0F | int32 | INTEGER32 | rw | no | no | 0 | +| 0x10 | int32 | INTEGER32 | rw | no | no | 0 | + +Vartiable is free to use by application. Variable is automatically saved. + +### 0x2120 - Testing variables +| Object Type | Count Label | Storage Group | +| ----------- | -------------- | -------------- | +| RECORD | | PERSIST_TEST | + +| Sub | Name | Data Type | SDO | PDO | SRDO | Default Value | +| ---- | --------------------- | ---------- | --- | --- | ---- | ------------- | +| 0x00 | Highest sub-index supported| UNSIGNED8 | ro | no | no | 0x0C | +| 0x01 | I64 | INTEGER64 | rw | tr | no | -1234567890123456789| +| 0x02 | U64 | UNSIGNED64 | rw | tr | no | 0x1234567890ABCDEF| +| 0x03 | R32 | REAL32 | rw | tr | no | 12.345 | +| 0x04 | R64 | REAL64 | rw | tr | no | 456.789 | +| 0x05 | Average of four numbers| REAL64 | ro | t | no | | +| 0x06 | String short | VISIBLE_STRING| rw | no | no | str | +| 0x07 | String long | VISIBLE_STRING (len=1000)| rw | no | no | Example string with 1000 bytes capacity. It may contain UTF-8 characters, like '€', tabs ' ', newlines, etc.| +| 0x08 | Octet string | OCTET_STRING| rw | no | no | C8 3D BB | +| 0x09 | Parameter with default value| UNSIGNED16 | rw | no | no | 0x1234 | +| 0x0A | Domain | DOMAIN | rw | no | no | | +| 0x0B | Domain file name read | VISIBLE_STRING (len=100)| rw | no | no | basicDevice.md| +| 0x0C | Domain file name write| VISIBLE_STRING (len=100)| rw | no | no | fileWrittenByDomain| + +Device Profile Specific Parameters {#device-profile-specific-parameters} +------------------------------------------------------------------------ + +### 0x6000 - Read digital input 8-bit +| Object Type | Count Label | Storage Group | +| ----------- | -------------- | -------------- | +| ARRAY | | RAM | + +| Sub | Name | Data Type | SDO | PDO | SRDO | Default Value | +| ---- | --------------------- | ---------- | --- | --- | ---- | ------------- | +| 0x00 | Highest sub-index supported| UNSIGNED8 | ro | no | no | 0x08 | +| 0x01 | Input | UNSIGNED8 | ro | t | no | 0x00 | +| 0x02 | Input | UNSIGNED8 | ro | t | no | 0x00 | +| 0x03 | Input | UNSIGNED8 | ro | t | no | 0x00 | +| 0x04 | Input | UNSIGNED8 | ro | t | no | 0x00 | +| 0x05 | Input | UNSIGNED8 | ro | t | no | 0x00 | +| 0x06 | Input | UNSIGNED8 | ro | t | no | 0x00 | +| 0x07 | Input | UNSIGNED8 | ro | t | no | 0x00 | +| 0x08 | Input | UNSIGNED8 | ro | t | no | 0x00 | + +Digital inputs from hardware. + +### 0x6200 - Write digital output 8-bit +| Object Type | Count Label | Storage Group | +| ----------- | -------------- | -------------- | +| ARRAY | | RAM | + +| Sub | Name | Data Type | SDO | PDO | SRDO | Default Value | +| ---- | --------------------- | ---------- | --- | --- | ---- | ------------- | +| 0x00 | Highest sub-index supported| UNSIGNED8 | ro | tr | no | 0x08 | +| 0x01 | Output | UNSIGNED8 | rw | tr | no | 0x00 | +| 0x02 | Output | UNSIGNED8 | rw | tr | no | 0x00 | +| 0x03 | Output | UNSIGNED8 | rw | tr | no | 0x00 | +| 0x04 | Output | UNSIGNED8 | rw | tr | no | 0x00 | +| 0x05 | Output | UNSIGNED8 | rw | tr | no | 0x00 | +| 0x06 | Output | UNSIGNED8 | rw | tr | no | 0x00 | +| 0x07 | Output | UNSIGNED8 | rw | tr | no | 0x00 | +| 0x08 | Output | UNSIGNED8 | rw | tr | no | 0x00 | + +Digital outputs on hardware. + +### 0x6401 - Read analog input 16-bit +| Object Type | Count Label | Storage Group | +| ----------- | -------------- | -------------- | +| ARRAY | | RAM | + +| Sub | Name | Data Type | SDO | PDO | SRDO | Default Value | +| ---- | --------------------- | ---------- | --- | --- | ---- | ------------- | +| 0x00 | Highest sub-index supported| UNSIGNED8 | ro | tr | no | 0x10 | +| 0x01 | Input | INTEGER16 | ro | tr | no | 0 | +| 0x02 | Input | INTEGER16 | ro | tr | no | 0 | +| 0x03 | Input | INTEGER16 | ro | tr | no | 0 | +| 0x04 | Input | INTEGER16 | ro | tr | no | 0 | +| 0x05 | Input | INTEGER16 | ro | tr | no | 0 | +| 0x06 | Input | INTEGER16 | ro | tr | no | 0 | +| 0x07 | Input | INTEGER16 | ro | tr | no | 0 | +| 0x08 | Input | INTEGER16 | ro | tr | no | 0 | +| 0x09 | Input | INTEGER16 | ro | tr | no | 0 | +| 0x0A | Input | INTEGER16 | ro | tr | no | 0 | +| 0x0B | Input | INTEGER16 | ro | tr | no | 0 | +| 0x0C | Input | INTEGER16 | ro | tr | no | 0 | +| 0x0D | Input | INTEGER16 | ro | tr | no | 0 | +| 0x0E | Input | INTEGER16 | ro | tr | no | 0 | +| 0x0F | Input | INTEGER16 | ro | tr | no | 0 | +| 0x10 | Input | INTEGER16 | ro | tr | no | 0 | + +Analogue inputs from hardware. The integer value is left adjusted. + +### 0x6411 - Write analog output 16-bit +| Object Type | Count Label | Storage Group | +| ----------- | -------------- | -------------- | +| ARRAY | | RAM | + +| Sub | Name | Data Type | SDO | PDO | SRDO | Default Value | +| ---- | --------------------- | ---------- | --- | --- | ---- | ------------- | +| 0x00 | Highest sub-index supported| UNSIGNED8 | ro | tr | no | 0x08 | +| 0x01 | Output | INTEGER16 | rw | tr | no | 0 | +| 0x02 | Output | INTEGER16 | rw | tr | no | 0 | +| 0x03 | Output | INTEGER16 | rw | tr | no | 0 | +| 0x04 | Output | INTEGER16 | rw | tr | no | 0 | +| 0x05 | Output | INTEGER16 | rw | tr | no | 0 | +| 0x06 | Output | INTEGER16 | rw | tr | no | 0 | +| 0x07 | Output | INTEGER16 | rw | tr | no | 0 | +| 0x08 | Output | INTEGER16 | rw | tr | no | 0 | + +Analogue outputs on hardware. The integer value is left adjusted. diff --git a/Devices/Libraries/Systems/CANopenSocket/examples/basicDevice/basicDevice.xdd b/Devices/Libraries/Systems/CANopenSocket/examples/basicDevice/basicDevice.xdd new file mode 100755 index 0000000..1671509 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/examples/basicDevice/basicDevice.xdd @@ -0,0 +1,2725 @@ + + + + + + + CANopen device profile + 1.1 + + + Device + + 1 + 1 + CANopen + + + + + + + Basic device + + + Basic CANopen device with example usage. + + 0 + 0 + 0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + * bit 16-31: Additional information +* bit 0-15: Device profile number + + + + + + + * bit 7: manufacturer specific +* bit 6: Reserved (always 0) +* bit 5: device profile specific +* bit 4: communication error (overrun, error state) +* bit 3: temperature +* bit 2: voltage +* bit 1: current +* bit 0: generic error + + + + + + * Sub Index 0: Contains number of actual errors. 0 can be written to clear error history. +* sub-index 1 and above: + * bit 16-31: Manufacturer specific additional information + * bit 0-15: Error code as transmited in the Emergency object + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + * bit 31: set to 0 +* bit 30: If set, CANopen device generates SYNC object +* bit 11-29: set to 0 +* bit 0-10: 11-bit CAN-ID + + + + + + + Period of SYNC transmission in µs (0 = transmission disabled). + + + + + + + Synchronous window leghth in µs (0 = not used). All synchronous PDOs must be transmitted within this time window. + + + + + + Sub-indexes 1 and above: +* Reading provides information about its storage functionality: + * bit 0: If set, CANopen device saves parameters on command + * bit 1: If set, CANopen device saves parameters autonomously +* Writing value 0x65766173 ('s','a','v','e' from LSB to MSB) stores corresponding data. + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Sub-indexes 1 and above: +* Reading provides information about its restoring capability: + * bit 0: If set, CANopen device restores parameters +* Writing value 0x64616F6C ('l','o','a','d' from LSB to MSB) restores corresponding data. + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + * bit 31: If set, CANopen device consumes TIME message +* bit 30: If set, CANopen device produces TIME message +* bit 11-29: set to 0 +* bit 0-10: 11-bit CAN-ID + + + + + + + * bit 31: If set, EMCY does NOT exist / is NOT valid +* bit 11-30: set to 0 +* bit 0-10: 11-bit CAN-ID + + + + + + + Inhibit time of emergency message in multiples of 100µs. The value 0 disables the inhibit time. + + + + + + Consumer Heartbeat Time: + * bit 24-31: set to 0 + * bit 16-23: Node ID of the monitored node. If 0 or greater than 127, sub-entry is not used. + * bit 0-15: Heartbeat time in ms (if 0, sub-intry is not used). Value should be higher than the corresponding producer heartbeat time. + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Heartbeat producer time in ms (0 = disable transmission). + + + + + + + * Vendor-ID, assigned by CiA +* Product code, manufacturer specific +* Revision number: + * bit 16-31: Major revision number (CANopen behavior has changed) + * bit 0-15: Minor revision num. (CANopen behavior has not changed) +* Serial number, manufacturer specific + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + * Value 0: SYNC message is transmitted with data length 0. +* Value 1: reserved. +* Value 2-240: SYNC message has one data byte, which contains the counter. +* Value 241-255: reserved. + + + + + + Sub-indexes 1 and 2: +* bit 11-31: set to 0 +* bit 0-10: 11-bit CAN-ID + + + + + + + + + + + + + + + + + + + + * Sub-indexes 1 and 2: + * bit 31: If set, SDO does NOT exist / is NOT valid + * bit 30: If set, value is assigned dynamically + * bit 11-29: set to 0 + * bit 0-10: 11-bit CAN-ID +* Node-ID of the SDO server, 0x01 to 0x7F + + + + + + + + + + + + + + + + + + + + + + + + + + * COB-ID used by RPDO: + * bit 31: If set, PDO does not exist / is not valid + * bit 11-30: set to 0 + * bit 0-10: 11-bit CAN-ID +* Transmission type: + * Value 0-240: synchronous, processed after next reception of SYNC object + * Value 241-253: not used + * Value 254: event-driven (manufacturer-specific) + * Value 255: event-driven (device profile and application profile specific) +* Event timer in ms (0 = disabled) for deadline monitoring. + + + + + + + + + + + + + + + + + + + + + + + + + + * COB-ID used by RPDO: + * bit 31: If set, PDO does not exist / is not valid + * bit 11-30: set to 0 + * bit 0-10: 11-bit CAN-ID +* Transmission type: + * Value 0-240: synchronous, processed after next reception of SYNC object + * Value 241-253: not used + * Value 254: event-driven (manufacturer-specific) + * Value 255: event-driven (device profile and application profile specific) +* Event timer in ms (0 = disabled) for deadline monitoring. + + + + + + + + + + + + + + + + + + + + + + + + + + * COB-ID used by RPDO: + * bit 31: If set, PDO does not exist / is not valid + * bit 11-30: set to 0 + * bit 0-10: 11-bit CAN-ID +* Transmission type: + * Value 0-240: synchronous, processed after next reception of SYNC object + * Value 241-253: not used + * Value 254: event-driven (manufacturer-specific) + * Value 255: event-driven (device profile and application profile specific) +* Event timer in ms (0 = disabled) for deadline monitoring. + + + + + + + + + + + + + + + + + + + + + + + + + + * COB-ID used by RPDO: + * bit 31: If set, PDO does not exist / is not valid + * bit 11-30: set to 0 + * bit 0-10: 11-bit CAN-ID +* Transmission type: + * Value 0-240: synchronous, processed after next reception of SYNC object + * Value 241-253: not used + * Value 254: event-driven (manufacturer-specific) + * Value 255: event-driven (device profile and application profile specific) +* Event timer in ms (0 = disabled) for deadline monitoring. + + + + + + + + + + + + + + + + + + + + + + + + + + * Number of mapped application objects in PDO: + * Value 0: mapping is disabled. + * Value 1: sub-index 0x01 is valid. + * Value 2-8: sub-indexes 0x01 to (0x02 to 0x08) are valid. +* Application object 1-8: + * bit 16-31: index + * bit 8-15: sub-index + * bit 0-7: data length in bits + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + * Number of mapped application objects in PDO: + * Value 0: mapping is disabled. + * Value 1: sub-index 0x01 is valid. + * Value 2-8: sub-indexes 0x01 to (0x02 to 0x08) are valid. +* Application object 1-8: + * bit 16-31: index + * bit 8-15: sub-index + * bit 0-7: data length in bits + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + * Number of mapped application objects in PDO: + * Value 0: mapping is disabled. + * Value 1: sub-index 0x01 is valid. + * Value 2-8: sub-indexes 0x01 to (0x02 to 0x08) are valid. +* Application object 1-8: + * bit 16-31: index + * bit 8-15: sub-index + * bit 0-7: data length in bits + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + * Number of mapped application objects in PDO: + * Value 0: mapping is disabled. + * Value 1: sub-index 0x01 is valid. + * Value 2-8: sub-indexes 0x01 to (0x02 to 0x08) are valid. +* Application object 1-8: + * bit 16-31: index + * bit 8-15: sub-index + * bit 0-7: data length in bits + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + * COB-ID used by RPDO: + * bit 31: If set, PDO does not exist / is not valid + * bit 30: If set, NO RTR is allowed on this PDO + * bit 11-29: set to 0 + * bit 0-10: 11-bit CAN-ID +* Transmission type: + * Value 0: synchronous (acyclic) + * Value 1-240: synchronous (cyclic every (1-240)-th sync) + * Value 241-253: not used + * Value 254: event-driven (manufacturer-specific) + * Value 255: event-driven (device profile and application profile specific) +* Inhibit time in multiple of 100µs, if the transmission type is set to 254 or 255 (0 = disabled). +* Event timer interval in ms, if the transmission type is set to 254 or 255 (0 = disabled). +* SYNC start value + * Value 0: Counter of the SYNC message shall not be processed. + * Value 1-240: The SYNC message with the counter value equal to this value shall be regarded as the first received SYNC message. + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + * COB-ID used by RPDO: + * bit 31: If set, PDO does not exist / is not valid + * bit 30: If set, NO RTR is allowed on this PDO + * bit 11-29: set to 0 + * bit 0-10: 11-bit CAN-ID +* Transmission type: + * Value 0: synchronous (acyclic) + * Value 1-240: synchronous (cyclic every (1-240)-th sync) + * Value 241-253: not used + * Value 254: event-driven (manufacturer-specific) + * Value 255: event-driven (device profile and application profile specific) +* Inhibit time in multiple of 100µs, if the transmission type is set to 254 or 255 (0 = disabled). +* Event timer interval in ms, if the transmission type is set to 254 or 255 (0 = disabled). +* SYNC start value + * Value 0: Counter of the SYNC message shall not be processed. + * Value 1-240: The SYNC message with the counter value equal to this value shall be regarded as the first received SYNC message. + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + * COB-ID used by RPDO: + * bit 31: If set, PDO does not exist / is not valid + * bit 30: If set, NO RTR is allowed on this PDO + * bit 11-29: set to 0 + * bit 0-10: 11-bit CAN-ID +* Transmission type: + * Value 0: synchronous (acyclic) + * Value 1-240: synchronous (cyclic every (1-240)-th sync) + * Value 241-253: not used + * Value 254: event-driven (manufacturer-specific) + * Value 255: event-driven (device profile and application profile specific) +* Inhibit time in multiple of 100µs, if the transmission type is set to 254 or 255 (0 = disabled). +* Event timer interval in ms, if the transmission type is set to 254 or 255 (0 = disabled). +* SYNC start value + * Value 0: Counter of the SYNC message shall not be processed. + * Value 1-240: The SYNC message with the counter value equal to this value shall be regarded as the first received SYNC message. + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + * COB-ID used by RPDO: + * bit 31: If set, PDO does not exist / is not valid + * bit 30: If set, NO RTR is allowed on this PDO + * bit 11-29: set to 0 + * bit 0-10: 11-bit CAN-ID +* Transmission type: + * Value 0: synchronous (acyclic) + * Value 1-240: synchronous (cyclic every (1-240)-th sync) + * Value 241-253: not used + * Value 254: event-driven (manufacturer-specific) + * Value 255: event-driven (device profile and application profile specific) +* Inhibit time in multiple of 100µs, if the transmission type is set to 254 or 255 (0 = disabled). +* Event timer interval in ms, if the transmission type is set to 254 or 255 (0 = disabled). +* SYNC start value + * Value 0: Counter of the SYNC message shall not be processed. + * Value 1-240: The SYNC message with the counter value equal to this value shall be regarded as the first received SYNC message. + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + * Number of mapped application objects in PDO: + * Value 0: mapping is disabled. + * Value 1: sub-index 0x01 is valid. + * Value 2-8: sub-indexes 0x01 to (0x02 to 0x08) are valid. +* Application object 1-8: + * bit 16-31: index + * bit 8-15: sub-index + * bit 0-7: data length in bits + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + * Number of mapped application objects in PDO: + * Value 0: mapping is disabled. + * Value 1: sub-index 0x01 is valid. + * Value 2-8: sub-indexes 0x01 to (0x02 to 0x08) are valid. +* Application object 1-8: + * bit 16-31: index + * bit 8-15: sub-index + * bit 0-7: data length in bits + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + * Number of mapped application objects in PDO: + * Value 0: mapping is disabled. + * Value 1: sub-index 0x01 is valid. + * Value 2-8: sub-indexes 0x01 to (0x02 to 0x08) are valid. +* Application object 1-8: + * bit 16-31: index + * bit 8-15: sub-index + * bit 0-7: data length in bits + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + * Number of mapped application objects in PDO: + * Value 0: mapping is disabled. + * Value 1: sub-index 0x01 is valid. + * Value 2-8: sub-indexes 0x01 to (0x02 to 0x08) are valid. +* Application object 1-8: + * bit 16-31: index + * bit 8-15: sub-index + * bit 0-7: data length in bits + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Error Status Bits indicates internal error conditions inside stack or inside application. For details see CO_EM_errorStatusBits_t in CO_Emergency.h file or in https://canopennode.github.io/CANopenSocket/group__CO__Emergency.html + + + + + + + + + + + + + + + + + + + + + + Power on Counter counts total microcontroller resets in it's lifetime. Variable is an example of EEPROM usage. + + + + + + Vartiable is free to use by application. + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Vartiable is free to use by application. Variables can be saved on command. + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Vartiable is free to use by application. Variable is automatically saved. + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Digital inputs from hardware. + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Digital outputs on hardware. + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Analogue inputs from hardware. The integer value is left adjusted. + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Analogue outputs on hardware. The integer value is left adjusted. + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + CANopen communication network profile + 1.1 + + + CommunicationNetwork + + 1 + 1 + CANopen + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/Devices/Libraries/Systems/CANopenSocket/examples/basicDevice/gateway.Makefile b/Devices/Libraries/Systems/CANopenSocket/examples/basicDevice/gateway.Makefile new file mode 100755 index 0000000..a9689d9 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/examples/basicDevice/gateway.Makefile @@ -0,0 +1,81 @@ +# Makefile for application with CANopenNode and Linux socketCAN +# It includes gateway with SDOclient and CO_LSSmaster + + +DRV_SRC = ../../CANopenNode/socketCAN +CANOPEN_SRC = ../../CANopenNode +APPL_SRC = . + + +LINK_TARGET = basicDeviceWithGateway +VERSION_FILE = CO_version.h + + +INCLUDE_DIRS = \ + -I$(DRV_SRC) \ + -I$(CANOPEN_SRC) \ + -I$(APPL_SRC) + + +SOURCES = \ + $(DRV_SRC)/CO_driver.c \ + $(DRV_SRC)/CO_error.c \ + $(DRV_SRC)/CO_epoll_interface.c \ + $(DRV_SRC)/CO_storageLinux.c \ + $(CANOPEN_SRC)/301/CO_ODinterface.c \ + $(CANOPEN_SRC)/301/CO_NMT_Heartbeat.c \ + $(CANOPEN_SRC)/301/CO_HBconsumer.c \ + $(CANOPEN_SRC)/301/CO_Emergency.c \ + $(CANOPEN_SRC)/301/CO_SDOserver.c \ + $(CANOPEN_SRC)/301/CO_SDOclient.c \ + $(CANOPEN_SRC)/301/CO_TIME.c \ + $(CANOPEN_SRC)/301/CO_SYNC.c \ + $(CANOPEN_SRC)/301/CO_PDO.c \ + $(CANOPEN_SRC)/301/crc16-ccitt.c \ + $(CANOPEN_SRC)/301/CO_fifo.c \ + $(CANOPEN_SRC)/301/CO_storage.c \ + $(CANOPEN_SRC)/303/CO_LEDs.c \ + $(CANOPEN_SRC)/305/CO_LSSslave.c \ + $(CANOPEN_SRC)/305/CO_LSSmaster.c \ + $(CANOPEN_SRC)/309/CO_gateway_ascii.c \ + $(CANOPEN_SRC)/CANopen.c \ + $(APPL_SRC)/CO_application.c \ + $(APPL_SRC)/OD.c \ + $(APPL_SRC)/testingVariables.c \ + $(DRV_SRC)/CO_main_basic.c + + +OBJS = $(SOURCES:%.c=%.o) +CC ?= gcc +OPT = +OPT += -g +#OPT += -O2 +#OPT += -DCO_SINGLE_THREAD +#OPT += -DCO_CONFIG_DEBUG=0xFFFF +#OPT += -Wextra -Wshadow -pedantic -fanalyzer +#OPT += -DCO_USE_GLOBALS +#OPT += -DCO_MULTIPLE_OD +CFLAGS = -Wall -DCO_DRIVER_CUSTOM -DCO_GATEWAY $(OPT) $(INCLUDE_DIRS) +LDFLAGS = +LDFLAGS += -g +LDFLAGS += -pthread + +#Options can be also passed via make: 'make OPT="-g" LDFLAGS="-pthread"' + + +.PHONY: all clean + +all: clean version $(LINK_TARGET) + +clean: + rm -f $(OBJS) $(LINK_TARGET) $(VERSION_FILE) + +%.o: %.c + $(CC) $(CFLAGS) -c $< -o $@ + +$(LINK_TARGET): $(OBJS) + $(CC) $(LDFLAGS) $^ -o $@ + +version: + echo "#define CO_VERSION_CANOPENNODE \"$(shell git -C $(CANOPEN_SRC) describe --tags --long --dirty)\"" > $(VERSION_FILE) + echo "#define CO_VERSION_APPLICATION \"$(shell git describe --tags --long --dirty)\"" >> $(VERSION_FILE) diff --git a/Devices/Libraries/Systems/CANopenSocket/examples/basicDevice/testingVariables.c b/Devices/Libraries/Systems/CANopenSocket/examples/basicDevice/testingVariables.c new file mode 100755 index 0000000..005c6ae --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/examples/basicDevice/testingVariables.c @@ -0,0 +1,303 @@ +/* Application specific access functions for extended OD objects */ + + +#include "testingVariables.h" +#include +#include + + +#define SUBINDEX_I64 0x01 +#define SUBINDEX_U64 0x02 +#define SUBINDEX_R32 0x03 +#define SUBINDEX_R64 0x04 +#define SUBINDEX_AVERAGE 0x05 +#define SUBINDEX_PARAMETER 0x09 +#define SUBINDEX_DOMAIN 0x0A +#define SUBINDEX_DOMAIN_FILE_READ 0x0B +#define SUBINDEX_DOMAIN_FILE_WRITE 0x0C + +#ifndef DOMAIN_LENGTH_INDICATE +#define DOMAIN_LENGTH_INDICATE 1 +#endif + + +/* + * Custom function for reading OD object _Testing variables_ + * + * For more information see file CO_ODinterface.h, OD_IO_t. + */ +static ODR_t OD_read_testVar(OD_stream_t *stream, void *buf, + OD_size_t count, OD_size_t *countRead) +{ + if (stream == NULL || buf == NULL || countRead == NULL) { + return ODR_DEV_INCOMPAT; + } + + /* Object was passed by OD_extensionIO_init, use correct type. */ + testingVariables_t *testVar = (testingVariables_t *)stream->object; + + switch (stream->subIndex) { + case SUBINDEX_AVERAGE: { + OD_size_t varSize = sizeof(float64_t); + + if (count < varSize || stream->dataLength != varSize) { + return ODR_DEV_INCOMPAT; + } + + float64_t average = (float64_t)*testVar->i64; + average += (float64_t)*testVar->u64; + average += (float64_t)*testVar->r32; + average += *testVar->r64; + average /= 4; + + memcpy(buf, &average, varSize); + + *countRead = varSize; + return ODR_OK; + } + + case SUBINDEX_PARAMETER: { + OD_size_t varSize = sizeof(testVar->parameterU16); + + if (count < varSize || stream->dataLength != varSize) { + return ODR_DEV_INCOMPAT; + } + + CO_setUint16(buf, ++testVar->parameterU16); + + *countRead = varSize; + return ODR_OK; + } + + case SUBINDEX_DOMAIN: { + /* Data is read from the file for this example. Data can be much + * longer than count (available size of the buffer). So + * OD_read_testVar() may be called multiple times. */ + if (stream->dataOffset == 0) { + /* Data offset is 0, so this is the first call of this function + * in current SDO communication. Open the file now. But if it + * is already open, then previous SDO communication didn't + * finish correctly. So close the old file first. */ + if (testVar->domainReadFileStream != NULL) + fclose(testVar->domainReadFileStream); + + /* Get filename from auxiliary OD variable of type + * VISIBLE_STRING. This type of variable may have variable + * string length and always have null terminating character. */ + char *fileName = testVar->domainReadFileName; + + /* open the file and verify success */ + testVar->domainReadFileStream = fopen(fileName, "r"); + if (testVar->domainReadFileStream == NULL) { + return ODR_NO_DATA; + } +#if DOMAIN_LENGTH_INDICATE + /* Indicate dataLength */ + fseek(testVar->domainReadFileStream, 0L, SEEK_END); + size_t dataLen = (size_t)ftell(testVar->domainReadFileStream); + stream->dataLength = dataLen <= 0xFFFFFFFF + ? dataLen : 0; + rewind(testVar->domainReadFileStream); +#else + /* It is not required to indicate data length in SDO transfer */ + stream->dataLength = 0; +#endif + } + + /* fill the buffer */ + size_t len = fread(buf, 1, count, testVar->domainReadFileStream); + ODR_t returnCode; + + /* determine, if file read finished or not */ + if (len != count || feof(testVar->domainReadFileStream)) { + fclose(testVar->domainReadFileStream); + testVar->domainReadFileStream = 0; + returnCode = ODR_OK; + stream->dataOffset = 0; + } + else { + returnCode = ODR_PARTIAL; + stream->dataOffset += len; + } + + /* indicate number of bytes read and return ODR_OK or ODR_PARTIAL */ + *countRead = len; + return returnCode; + } + + default: { + return OD_readOriginal(stream, buf, count, countRead); + } + } +} + + +/* + * Custom function for reading OD object _Testing variables_ + * + * For more information see file CO_ODinterface.h, OD_IO_t. + */ +static ODR_t OD_write_testVar(OD_stream_t *stream, const void *buf, + OD_size_t count, OD_size_t *countWritten) +{ + if (stream == NULL || buf == NULL || countWritten == NULL) { + return ODR_DEV_INCOMPAT; + } + + /* Object was passed by OD_extensionIO_init, use correct type. */ + testingVariables_t *testVar = (testingVariables_t *)stream->object; + + switch (stream->subIndex) { + case SUBINDEX_PARAMETER: { + testVar->parameterU16 = CO_getUint16(buf); + /* write value to the original location in the Object Dictionary */ + return OD_writeOriginal(stream, buf, count, countWritten); + } + + case SUBINDEX_DOMAIN: { + /* Data will be written to the file for this example. Data can be + * much longer than count (current size of data in the buffer). So + * OD_write_testVar() may be called multiple times. */ + if (stream->dataOffset == 0) { + /* Data offset is 0, so this is the first call of this function + * in current SDO communication. Open the file now. Write data + * to temporary file first. When transfer will be finished and + * temporary file will be written successfully, the original + * file will be replaced by with newly transferred. But if + * temporary file is already open in this moment, then previous + * SDO communication didn't finish correctly. So close the old + * file first. */ + if (testVar->domainWriteFileStream != NULL) + fclose(testVar->domainWriteFileStream); + + /* Get filename from auxiliary OD variable of type + * VISIBLE_STRING. This type of variable may have variable + * string length and always have null terminating character. */ + char *fileNameOrig = testVar->domainWriteFileName; + + /* create temporary file and verify success */ + char *fileName = malloc(strlen(fileNameOrig) + 6); + strcpy(fileName, fileNameOrig); + strcat(fileName, ".tmp"); + testVar->domainWriteFileStream = fopen(fileName, "w"); + free(fileName); + if (testVar->domainWriteFileStream == NULL) { + return ODR_OUT_OF_MEM; + } + } + + /* write the data and verify */ + size_t len = fwrite(buf, 1, count, testVar->domainWriteFileStream); + if (testVar->domainWriteFileStream == NULL) { + return ODR_GENERAL; + } + + /* indicate total length written */ + stream->dataOffset += len; + + /* determine, if file write finished or not + * (dataLength may not yet be indicated) */ + ODR_t returnCode = ODR_OK; + if (stream->dataLength > 0 + && stream->dataOffset == stream->dataLength + ) { + fclose(testVar->domainWriteFileStream); + testVar->domainWriteFileStream = 0; + stream->dataOffset = 0; + + /* replace original file with just downloaded */ + char *fileNameOrig = testVar->domainWriteFileName; + char *fileName = malloc(strlen(fileNameOrig) + 6); + strcpy(fileName, fileNameOrig); + strcat(fileName, ".tmp"); + int err = rename(fileName, fileNameOrig); + free(fileName); + + if (err != 0) { + return ODR_GENERAL; + } + } + else { + returnCode = ODR_PARTIAL; + } + + /* indicate number of bytes written and return ODR_OK or + * ODR_PARTIAL */ + *countWritten = len; + return returnCode; + } + + default: { + return OD_writeOriginal(stream, buf, count, countWritten); + } + } +} + + +/******************************************************************************/ +CO_ReturnError_t testingVariables_init(testingVariables_t *testVar, + uint32_t *errInfo, + OD_entry_t *OD_testVar) +{ + if (testVar == NULL || errInfo == NULL || OD_testVar == NULL) + return CO_ERROR_ILLEGAL_ARGUMENT; + + CO_ReturnError_t err = CO_ERROR_NO; + ODR_t odRet; + + /* initialize object variables */ + memset(testVar, 0, sizeof(testingVariables_t)); + + /* Initialize custom OD object "Testing variables" */ + testVar->OD_testVar_extension.object = testVar; + testVar->OD_testVar_extension.read = OD_read_testVar; + testVar->OD_testVar_extension.write = OD_write_testVar; + odRet = OD_extension_init(OD_testVar, &testVar->OD_testVar_extension); + + /* This is strict behavior and will exit the program on error. Error + * checking on all OD functions can also be omitted. In that case program + * will run, but specific OD entry may not be accessible. */ + if (odRet != ODR_OK) { + *errInfo = OD_getIndex(OD_testVar); + return CO_ERROR_OD_PARAMETERS; + } + + /* Get variables from Object dictionary, related to "Average" */ + testVar->i64 = OD_getPtr(OD_testVar, SUBINDEX_I64, sizeof(int64_t), NULL); + testVar->u64 = OD_getPtr(OD_testVar, SUBINDEX_U64, sizeof(uint64_t), NULL); + testVar->r32 = OD_getPtr(OD_testVar, SUBINDEX_R32, sizeof(float32_t), NULL); + testVar->r64 = OD_getPtr(OD_testVar, SUBINDEX_R64, sizeof(float64_t), NULL); + + if (testVar->i64 == NULL || testVar->u64 == NULL + || testVar->r32 == NULL || testVar->r64 == NULL + ) { + *errInfo = OD_getIndex(OD_testVar); + return CO_ERROR_OD_PARAMETERS; + } + + /* Get variable 'Parameter with default value' from Object dictionary */ + odRet = OD_get_u16(OD_testVar, SUBINDEX_PARAMETER, + &testVar->parameterU16, true); + + if (odRet != ODR_OK) { + *errInfo = OD_getIndex(OD_testVar); + return CO_ERROR_OD_PARAMETERS; + } + + /* Get variables from Object dictionary, related to "Domain" */ + testVar->domainReadFileName = OD_getPtr(OD_testVar, + SUBINDEX_DOMAIN_FILE_READ, + 0, NULL); + testVar->domainWriteFileName = OD_getPtr(OD_testVar, + SUBINDEX_DOMAIN_FILE_WRITE, + 0, NULL); + + if (testVar->domainReadFileName == NULL + || testVar->domainWriteFileName == NULL + ) { + *errInfo = OD_getIndex(OD_testVar); + return CO_ERROR_OD_PARAMETERS; + } + + return err; +} diff --git a/Devices/Libraries/Systems/CANopenSocket/examples/basicDevice/testingVariables.h b/Devices/Libraries/Systems/CANopenSocket/examples/basicDevice/testingVariables.h new file mode 100755 index 0000000..b44984c --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/examples/basicDevice/testingVariables.h @@ -0,0 +1,48 @@ +/** Application specific access functions for extended OD objects */ + + +#ifndef TESTING_VARIABLES_H +#define TESTING_VARIABLES_H + + +#include "301/CO_ODinterface.h" +#include + + +/** + * testingVariables object. + */ +typedef struct { + OD_extension_t OD_testVar_extension; /**< Extension for OD object */ + int64_t *i64; /**< Pointer to variable in object dictionary */ + uint64_t *u64; /**< Pointer to variable in object dictionary */ + float32_t *r32; /**< Pointer to variable in object dictionary */ + float64_t *r64; /**< Pointer to variable in object dictionary */ + /** Variable initialised from OD sub-entry 'Parameter with default value' */ + uint16_t parameterU16; + /** domain data type test - stream for reading the file */ + FILE *domainReadFileStream; + /** domain data type test - filename from object dictionary */ + char *domainReadFileName; + /** domain data type test - stream for writing the file */ + FILE *domainWriteFileStream; + /** domain data type test - filename from object dictionary */ + char *domainWriteFileName; +} testingVariables_t; + + +/** + * Initialize testingVariables object. + * + * @param testVar This object will be initialized. + * @param [out] errInfo Variable may indicate additional information for some + * types of errors. + * @param OD_testVar Object Dictionary entry for Testing variables. + * + * @return @ref CO_ReturnError_t CO_ERROR_NO in case of success. + */ +CO_ReturnError_t testingVariables_init(testingVariables_t *testVar, + uint32_t *errInfo, + OD_entry_t *OD_testVar); + +#endif /* TESTING_VARIABLES_H */ diff --git a/Devices/Libraries/Systems/CANopenSocket/test/running_canopen/SDO_transfer.bats b/Devices/Libraries/Systems/CANopenSocket/test/running_canopen/SDO_transfer.bats new file mode 100755 index 0000000..157443e --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/test/running_canopen/SDO_transfer.bats @@ -0,0 +1,91 @@ +#!/usr/bin/env ../libs/bats-core/bin/bats +load '../libs/bats-support/load.bash' +load '../libs/bats-assert/load.bash' + +setup_file() { + . ./config_bats.sh + + if [[ $binaryFileSize = "" ]] ; then + export binaryFileSize=100000 + fi + if [[ $binaryFileSegSize = "" ]] ; then + export binaryFileSegSize=1000 + fi + + export sedStr='s/^\([0-9]*\).*/\1/' +} + +teardown_file() { + cat binaryFileReport >&3 + rm binaryFile* + run rm fileWrittenByDomain +} + +@test 'Segmented transfer - set filename for upload' { + assert $cocomm -n0 "set sdo_block 0" + run $cocomm -n8 "[1] 0x$D2 w 0x2120 11 vs fileWrittenByDomain" + assert_success + assert_output "[1] $OK +6$D2#2120210B13000000 +5$B2#6020210B00000000 +6$D2#0066696C65577269 +5$B2#2000000000000000 +6$D2#107474656E427944 +5$B2#3000000000000000 +6$D2#056F6D61696E0000 +5$B2#2000000000000000" +} + +@test "Block transfer, create $binaryFileSize bytes long random binaryFile" { + assert openssl rand $binaryFileSize > binaryFile +} + +@test 'Block download binaryFile' { + assert $cocomm -n0 "set sdo_block 1" + time_begin=$(date +%s.%N) + assert base64 -w0 binaryFile | $cocomm -n0 -i "[1] 0x$D2 w 0x2120 10 d" + time_end=$(date +%s.%N) + time_diff=$(echo "($time_end - $time_begin) * 1000" | bc | sed $sedStr) + echo -n "# $binaryFileSize bytes block down/upload: $time_diff/" > binaryFileReport +} + +@test 'Block upload to binaryFileReceived - 2x' { + assert $cocomm -n0 "set sdo_block 1" + time_begin=$(date +%s.%N) + assert $cocomm -n0 -odata "[1] 0x$D2 r 0x2120 10 d" | base64 -d > binaryFileReceived1 + assert $cocomm -n0 -odata "[1] 0x$D2 r 0x2120 10 d" | base64 -d > binaryFileReceived2 + time_end=$(date +%s.%N) + time_diff=$(echo "($time_end - $time_begin) * 1000 / 2" | bc | sed $sedStr) + echo "$time_diff milliseconds." >> binaryFileReport +} + +@test 'Block transfer, files equal' { + assert cmp binaryFile binaryFileReceived1 + assert cmp binaryFile binaryFileReceived2 +} + +@test "Segmented transfer, create $binaryFileSegSize bytes long random binaryFileSeg" { + assert openssl rand $binaryFileSegSize > binaryFileSeg +} + +@test 'Segmented download binaryFileSeg' { + assert $cocomm -n0 "set sdo_block 0" + time_begin=$(date +%s.%N) + assert base64 -w0 binaryFileSeg | $cocomm -n0 -i "[1] 0x$D2 w 0x2120 10 d" + time_end=$(date +%s.%N) + time_diff=$(echo "($time_end - $time_begin) * 1000" | bc | sed $sedStr) + echo -n "# $binaryFileSegSize bytes segmented down/upload: $time_diff/" >> binaryFileReport +} + +@test 'Segmented upload to binaryFileSegReceived' { + assert $cocomm -n0 "set sdo_block 0" + time_begin=$(date +%s.%N) + assert $cocomm -n0 -odata "[1] 0x$D2 r 0x2120 10 d" | base64 -d > binaryFileSegReceived + time_end=$(date +%s.%N) + time_diff=$(echo "($time_end - $time_begin) * 1000" | bc | sed $sedStr) + echo "$time_diff milliseconds." >> binaryFileReport +} + +@test 'Segmented transfer, files equal' { + assert cmp binaryFileSeg binaryFileSegReceived +} diff --git a/Devices/Libraries/Systems/CANopenSocket/test/running_canopen/basic_NMT_SDO_heartbeat.bats b/Devices/Libraries/Systems/CANopenSocket/test/running_canopen/basic_NMT_SDO_heartbeat.bats new file mode 100755 index 0000000..0a6b456 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/test/running_canopen/basic_NMT_SDO_heartbeat.bats @@ -0,0 +1,189 @@ +#!/usr/bin/env ../libs/bats-core/bin/bats +load '../libs/bats-support/load.bash' +load '../libs/bats-assert/load.bash' + +setup_file() { + . ./config_bats.sh +} + +@test 'NMT: all preoperational' { + # cocomm must exit with success (return value is 0 (and output is "[1] OK")) + # and must receive at least one CAN message (option -n1) in time interval. + assert $cocomm -n1 "0 preoperational" +} + +@test 'SDO: read heartbeat D1' { + # cocomm must exit with success. + # There is no can message, because SDO of master device works on itself. + assert $cocomm -n0 "0x$D1 r 0x1017 0 i16" +} + +@test 'SDO: read heartbeat D2' { + # cocomm must exit with success and must receive two CAN messages. + assert $cocomm -n2 "0x$D2 r 0x1017 0 i16" +} + +@test 'SDO: disable heartbeats' { + # Run cocomm with four commands, it must exit with success (response from + # each command is success, none of responses is "ERROR"). + run $cocomm -n2 "[1] 0x$D1 w 0x1017 0 i16 0" \ + "[2] 0x$D2 w 0x1017 0 i16 0" + # run must exit with success (none of command responses is "ERROR"). + assert_success + # run output must match the string - exact match including newlines and '\r' + assert_output "[1] $OK +[2] $OK +6$D2#2B17100000000000 +5$B2#6017100000000000" +} + +@test 'candump: no communication, 2sec' { + sleep 0.2 + # cocomm must exit with error (timeout - no message within 2000 ms) + refute $cocomm -n1 -T2000 "0x$D1 w 0x1017 0 i16 0" +} + +@test 'Heartbeats: all present' { + assert $cocomm -n2 "[1] 0x$D1 w 0x1017 0 i16 0x$HB" + assert $cocomm -n0 "[2] 0x$D1 w 0x1017 0 i16 0" + assert $cocomm -n4 "[3] 0x$D2 w 0x1017 0 i16 0x$HB" + assert $cocomm -n2 "[4] 0x$D2 w 0x1017 0 i16 0" +} + +@test 'NMT: reset communication - one' { + run $cocomm -n2 "[1] 0x$D1 reset comm" + assert_success + assert_output "[1] $OK +000#82$D1 +7$D1#00" + run $cocomm -n2 "[2] 0x$D2 reset comm" + assert_success + assert_output "[2] $OK +000#82$D2 +7$D2#00" +} + +@test 'NMT: reset communication - two' { + run $cocomm -n2 "[1] 0x$D1 reset comm" + assert_success + assert_output "[1] $OK +000#82$D1 +7$D1#00" + run $cocomm -n2 "[2] 0x$D2 reset comm" + assert_success + assert_output "[2] $OK +000#82$D2 +7$D2#00" +} + +@test 'NMT: reset communication - three' { + run $cocomm -n2 "[1] 0x$D1 reset comm" + assert_success + assert_output "[1] $OK +000#82$D1 +7$D1#00" + run $cocomm -n2 "[2] 0x$D2 reset comm" + assert_success + assert_output "[2] $OK +000#82$D2 +7$D2#00" +} + +@test 'all preoperational' { + assert $cocomm -n1 "0 preoperational" +} + +@test 'SDO: disable all PDOs on D1' { + run $cocomm -n0 "[11] 0x$D1 w 0x1400 1 u32 0x800002$D1" \ + "[12] 0x$D1 w 0x1401 1 u32 0x800003$D1" \ + "[13] 0x$D1 w 0x1402 1 u32 0x800004$D1" \ + "[14] 0x$D1 w 0x1403 1 u32 0x800005$D1" \ + "[15] 0x$D1 w 0x1800 1 u32 0x800001$B1" \ + "[16] 0x$D1 w 0x1801 1 u32 0x800002$B1" \ + "[17] 0x$D1 w 0x1802 1 u32 0x800003$B1" \ + "[18] 0x$D1 w 0x1803 1 u32 0x800004$B1" + assert_success + assert_output "[11] $OK +[12] $OK +[13] $OK +[14] $OK +[15] $OK +[16] $OK +[17] $OK +[18] $OK" +} +@test 'SDO: disable all PDOs on D2' { + run $cocomm -n16 "[21] 0x$D2 w 0x1400 1 u32 0x800002$D2" \ + "[22] 0x$D2 w 0x1401 1 u32 0x800003$D2" \ + "[23] 0x$D2 w 0x1402 1 u32 0x800004$D2" \ + "[24] 0x$D2 w 0x1403 1 u32 0x800005$D2" \ + "[25] 0x$D2 w 0x1800 1 u32 0x800001$B2" \ + "[26] 0x$D2 w 0x1801 1 u32 0x800002$B2" \ + "[27] 0x$D2 w 0x1802 1 u32 0x800003$B2" \ + "[28] 0x$D2 w 0x1803 1 u32 0x800004$B2" + assert_success + assert_output "[21] $OK +[22] $OK +[23] $OK +[24] $OK +[25] $OK +[26] $OK +[27] $OK +[28] $OK +6$D2#23001401"$D2"020080 +5$B2#6000140100000000 +6$D2#23011401"$D2"030080 +5$B2#6001140100000000 +6$D2#23021401"$D2"040080 +5$B2#6002140100000000 +6$D2#23031401"$D2"050080 +5$B2#6003140100000000 +6$D2#23001801"$B2"010080 +5$B2#6000180100000000 +6$D2#23011801"$B2"020080 +5$B2#6001180100000000 +6$D2#23021801"$B2"030080 +5$B2#6002180100000000 +6$D2#23031801"$B2"040080 +5$B2#6003180100000000" +} + +@test 'NMT: pre-operational D1' { + run $cocomm -n2 "[1] 0x$D1 w 0x1017 0 i16 0x$HB" + assert_success + assert_output "[1] $OK +7$D1#7F +7$D1#7F" +} +@test 'NMT: operational D1' { + run $cocomm -n3 "[1] 0x$D1 start" + assert_success + assert_output "[1] $OK +000#01$D1 +7$D1#05 +7$D1#05" + assert $cocomm -n0 "0x$D1 w 0x1017 0 i16 0" +} + +@test 'NMT: pre-operational D2' { + run $cocomm -n4 "[1] 0x$D2 w 0x1017 0 i16 0x$HB" + assert_success + assert_output "[1] $OK +6$D2#2B171000"$HB"000000 +5$B2#6017100000000000 +7$D2#7F +7$D2#7F" +} +@test 'NMT: operational D2' { + run $cocomm -n3 "[1] 0x$D2 start" + assert $cocomm -n2 "0x$D2 w 0x1017 0 i16 0" + assert_success + assert_output "[1] $OK +000#01$D2 +7$D2#05 +7$D2#05" +} + +@test 'finish: NMT all preoperational' { + assert $cocomm -n1 "0 preoperational" +} diff --git a/Devices/Libraries/Systems/CANopenSocket/test/running_canopen/config_bats.sh b/Devices/Libraries/Systems/CANopenSocket/test/running_canopen/config_bats.sh new file mode 100755 index 0000000..3d8df21 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/test/running_canopen/config_bats.sh @@ -0,0 +1,47 @@ +#!/bin/bash + + +# CAN device for candump +if [[ $can_device = "" ]] ; then + export can_device=vcan0 +fi + +# Location of cocomm program +if [[ $cocomm = "" ]] ; then + export cocomm="../../cocomm/cocomm" +fi + +# Heartbeat producer time, 0xA0 = 160ms +if [[ $HB = "" ]] ; then + export HB=A0 +fi + +# Heartbeat consumer timeout, 0xF0 = 240ms +if [[ $HB_timeout = "" ]] ; then + export HB_timeout=F0 +fi + +# NodeId of device 1 (with command interface) +if [[ $D1 = "" ]] ; then + export D1=01 +fi +# NodeId of device 1 + 0x80 +if [[ $B1 = "" ]] ; then + export B1=81 +fi +# NodeId of device 2 (tested device) +if [[ $D2 = "" ]] ; then + export D2=04 +fi +# NodeId of device 2 + 0x80 +if [[ $B2 = "" ]] ; then + export B2=84 +fi + +# Other +export OK=$'OK\r' +export cocomm_flat=1 +export cocomm_candump=$can_device +# export cocomm_socket=/tmp/CO_command_socket #default +# export cocomm_host= +# export cocomm_port= diff --git a/Devices/Libraries/Systems/CANopenSocket/test/running_canopen/heartbeat_consumer.bats b/Devices/Libraries/Systems/CANopenSocket/test/running_canopen/heartbeat_consumer.bats new file mode 100755 index 0000000..c412131 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/test/running_canopen/heartbeat_consumer.bats @@ -0,0 +1,72 @@ +#!/usr/bin/env ../libs/bats-core/bin/bats +load '../libs/bats-support/load.bash' +load '../libs/bats-assert/load.bash' + +setup_file() { + . ./config_bats.sh +} + +@test 'Start heartbeat producer on D1 only' { + assert $cocomm -n8 "[1] 0x$D2 w 0x1017 0 i16 0" \ + "[2] 0x$D2 w 0x1016 2 u32 0" \ + "[3] 0 reset communication" \ + "[4] 0x$D1 w 0x1017 0 i16 0x$HB" + run $cocomm -n2 "0 preoperational" + assert_success + assert_output "[1] $OK +000#8000 +7$D1#7F" +} + +@test 'Start heartbeat consumer on D2' { + run $cocomm -n4 "[1] 0x$D2 w 0x1016 2 u32 0x00"$D1"00$HB_timeout" + assert_success + assert_output "[1] $OK +6$D2#23161002"$HB_timeout"000100 +5$B2#6016100200000000 +7$D1#7F +7$D1#7F" +} + +@test 'Heartbeat producer D1 missing' { + run $cocomm -n1 "[1] 0x$D1 w 0x1017 0 i16 0" + assert_success + assert_output "[1] $OK +0$B2#3081101B01000000" +} + +@test 'Heartbeat producer D1 established' { + run $cocomm -n3 "[1] 0x$D1 w 0x1017 0 i16 0x$HB" + assert_success + assert_output "[1] $OK +7$D1#7F +0$B2#0000001B00000000 +7$D1#7F" +} + +@test 'Bootup message D1' { + run $cocomm -n6 "0x$D1 reset communication" + assert_success + assert_output "[1] $OK +000#8201 +7$D1#00 +0$B2#3081101C01000000 +7$D1#05 +0$B2#0000001C00000000 +7$D1#05" +} + +@test 'Second HB consumer configuration' { + assert $cocomm -n2 "[1] 0x$D2 w 0x1016 3 u32 0x005500$HB_timeout" +} + +@test 'Illegal HB consumer configuration (duplicate nodeId)' { + refute $cocomm -n2 "[1] 0x$D2 w 0x1016 3 u32 0x00"$D1"00$HB_timeout" +} + +@test 'Cleanup' { + assert $cocomm -n7 "[1] 0x$D1 w 0x1017 0 i16 0" \ + "[2] 0x$D2 w 0x1016 2 u32 0" \ + "[2] 0x$D2 w 0x1016 3 u32 0" \ + "[3] 0 reset communication" +} diff --git a/Devices/Libraries/Systems/CANopenSocket/test/running_canopen/start_canopen.sh b/Devices/Libraries/Systems/CANopenSocket/test/running_canopen/start_canopen.sh new file mode 100755 index 0000000..aac5304 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/test/running_canopen/start_canopen.sh @@ -0,0 +1,55 @@ +#!/bin/bash + +if [[ $1 = "" ]] ; then + echo "Usage: $0 " + exit +else + export can_device=$1 +fi + +if [[ $co_device_1 = "" ]] ; then + export co_device_1="../../CANopenNode/canopend" +fi +if [[ $co_device_4 = "" ]] ; then + export co_device_4="../../examples/basicDevice/basicDevice" +fi + +export cleanup=tmp/stop_canopen.sh + +# if "can_device" does not exist, create virtual CAN device +if ! grep -q $can_device /proc/net/can/rcvlist_all ; then + echo "ip link: adding and setting up virtual CAN device named '$can_device'" + sudo modprobe vcan + sudo ip link add dev $can_device type vcan + sudo ip link set up $can_device +fi + +# prepare files for storage +mkdir tmp + +# prepare stop script and run CANopen devices in background +echo "#!/bin/bash" > $cleanup +chmod a+x $cleanup + +echo "Running in background: '$co_device_1 $can_device -i 1 -s \"tmp/dev1_\" -c \"local-/tmp/CO_command_socket\"'" +echo "-" > tmp/dev1_lss.persist +echo "-" > tmp/dev1_od_comm.persist +$co_device_1 $can_device -i 1 -s "tmp/dev1_" -c "local-/tmp/CO_command_socket"& +echo kill $! >> $cleanup +sleep 0.1 +echo + +echo "Running in background: '$co_device_4 $can_device -i 4 -s \"tmp/dev4_\"'" +echo "-" > tmp/dev4_lss.persist +echo "-" > tmp/dev4_od_comm.persist +echo "-" > tmp/dev4_od_test_auto.persist +echo "-" > tmp/dev4_od_test.persist +$co_device_4 $can_device -i 4 -s "tmp/dev4_"& +echo kill $! >> $cleanup +sleep 0.1 +echo + +echo "rm -r tmp" >> $cleanup + +echo "For cleanup run: $cleanup" +echo diff --git a/Devices/Libraries/Systems/CANopenSocket/test/running_canopen/test_all.sh b/Devices/Libraries/Systems/CANopenSocket/test/running_canopen/test_all.sh new file mode 100755 index 0000000..4a22b7d --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/test/running_canopen/test_all.sh @@ -0,0 +1,34 @@ +#!/bin/bash + +if [[ $1 = "" ]] ; then + echo "No CAN device specified. Assuming two CANopen devices already running:" + echo " - 'canopend' with nodeId=1 and gateway interface: 'local-/tmp/CO_command_socket'" + echo " - 'basicDevice' with nodeId=4" + echo "If that is not the case, read test.md or try: ./test_all.sh vcan0" +else + echo "Starting two CANopen devices in background with 'start_canopen.sh':" + ./start_canopen.sh $1 +fi + + +START=$(date +%s.%N) + +echo -e "\nRunning 'basic_NMT_SDO_heartbeat.bats':" +./basic_NMT_SDO_heartbeat.bats +echo -e "\nRunning 'heartbeat_consumer.bats':" +./heartbeat_consumer.bats +echo -e "\nRunning 'SDO_transfer.bats':" +./SDO_transfer.bats + +END=$(date +%s.%N) +DIFF=$(echo "$END - $START" | bc) +echo -e "Test duration: $DIFF sec\n" + + +if [[ $1 != "" ]] ; then + echo "Cleanup two CANopen devices:" + tmp/stop_canopen.sh +fi + +echo -e "\nPress enter to close..." +read diff --git a/Devices/Libraries/Systems/CANopenSocket/test/test.md b/Devices/Libraries/Systems/CANopenSocket/test/test.md new file mode 100755 index 0000000..b4f87be --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/test/test.md @@ -0,0 +1,83 @@ +Test +==== + +Tests in Running CANopen network +-------------------------------- + +Tests on Running CANopen network are implemented with two CANopenNode devices, by default running on Linux virtual CAN interface. One of them has gateway interface and communicates with program `cocomm`. + +Tests are run in Linux command line with [Bash Automated Testing System - BATS](https://github.com/bats-core/bats-core), which is included in CANopenSocket as three git submodules. + +Test results are available in [test_report.md](test_report.md). + + +### Prerequisites + + - Get CANopenSocket and all submodules as described in README.md + - make `cocomm`, `canopend` and `basicDevice` + + +### Run tests automatically + + cd test/running_canopen$ + ./test_canopend.sh vcan0 + +This will: +1. start `vcan0` interface if not already existed. +2. run two CANopenNode Linux devices in background (`start_canopen.sh` script): + - `CANopenNode/canopend` with nodeId=1 and gateway interface: `local-/tmp/CO_command_socket` + - `examples/basicDevice/basicDevice` with nodeId=4 +3. run tests and measure time +4. cleanup (`tmp/stop_canopen.sh` script, generated by `start_canopen.sh`). + +If argument `vcan0` or similar is omitted, it is assumed, two CANopen devices are already running in system. In that case steps 1, 2 and 4 are skipped. + + +### Start two CANopen devices manually + +`start_canopen.sh ` script runs two CANopenNode Linux devices in background. + +Possible configuration options can configure CANopenNode applications. To override default values run the commands from command line before running test scripts. Default configuration is: + + export co_device_1="../../CANopenNode/canopend" + export co_device_4="../../examples/basicDevice/basicDevice" + +It is possible to run two CANopen devices without `start_canopen.sh` script and then run the tests. Devices can be connected with on real CAN bus and second device can run on any target system. By default this script runs following commands and prepares `tmp/stop_canopen.sh` script for cleanup: + + sudo modprobe vcan + sudo ip link add dev vcan0 type vcan + sudo ip link set up vcan0 + + mkdir tmp + echo "-" > tmp/dev1_lss.persist + echo "-" > tmp/dev1_od_comm.persist + ../../CANopenNode/canopend vcan0 -i 1 -s "tmp/dev1_" -c "local-/tmp/CO_command_socket" + + echo "-" > tmp/dev4_lss.persist + echo "-" > tmp/dev4_od_comm.persist + echo "-" > tmp/dev4_od_app_auto.persist + echo "-" > tmp/dev4_od_app.persist + ../../examples/basicDevice/basicDevice vcan0 -i 4 -s "tmp/dev4_" + + # after finish with testing terminate the programs and `rm -r tmp` + +There is also a helper script, which runs only canopend and candump on specified interface in terminal: + + cd tools + ./run_canopend_candump.sh + + +### Run the tests manually + +Before running the tests, specify cocomm location and CAN device, which will be used for monitoring CAN messages (candump). To override other configuration variables see file `config_bats.sh`. Default configuration is: + + export can_device=vcan0 + export cocomm="../../cocomm/cocomm" + +Then simply run the separate tests from the same terminal: + + ./basic_NMT_SDO_heartbeat.bats + ./heartbeat_consumer.bats + ./SDO_transfer.bats + +Open the `*.bats` script file in text editor and examine the contents. Script is quite straightforward, see [BATS usage](https://bats-core.readthedocs.io/en/latest/writing-tests.html) and [BATS assert](https://github.com/bats-core/bats-assert) for information on testing commands. Command line program `cocomm` is used for running each test. For `cocomm` example usage see [basicDevice/README.md](../examples/basicDevice/README.md). Program uses here one additional functionality - it prints dump of actual CAN messages, which occur with each `cocomm` command. So BATS can verify the complete result. diff --git a/Devices/Libraries/Systems/CANopenSocket/test/test_report.md b/Devices/Libraries/Systems/CANopenSocket/test/test_report.md new file mode 100755 index 0000000..2693c04 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/test/test_report.md @@ -0,0 +1,160 @@ +TEST REPORTS +============ + +Complete tests in running CANopen +--------------------------------- +`running_canopen/test_all.sh`, two CAN devices, 1000 kbps: +``` +No CAN device specified. Assuming two CANopen devices already running: + - 'canopend' with nodeId=1 and gateway interface: 'local-/tmp/CO_command_socket' + - 'basicDevice' with nodeId=4 +If that is not the case, read test.md or try: ./test_all.sh vcan0 + +Running 'basic_NMT_SDO_heartbeat.bats': + ✓ NMT: all preoperational + ✓ SDO: read heartbeat D1 + ✓ SDO: read heartbeat D2 + ✓ SDO: disable heartbeats + ✓ candump: no communication, 2sec + ✓ Heartbeats: all present + ✓ NMT: reset communication - one + ✓ NMT: reset communication - two + ✓ NMT: reset communication - three + ✓ all preoperational + ✓ SDO: disable all PDOs on D1 + ✓ SDO: disable all PDOs on D2 + ✓ NMT: pre-operational D1 + ✓ NMT: operational D1 + ✓ NMT: pre-operational D2 + ✓ NMT: operational D2 + ✓ finish: NMT all preoperational + +17 tests, 0 failures + + +Running 'heartbeat_consumer.bats': + ✓ Start heartbeat producer on D1 only + ✓ Start heartbeat consumer on D2 + ✓ Heartbeat producer D1 missing + ✓ Heartbeat producer D1 established + ✓ Bootup message D1 + ✓ Second HB consumer configuration + ✓ Illegal HB consumer configuration (duplicate nodeId) + ✓ Cleanup + +8 tests, 0 failures + + +Running 'SDO_transfer.bats': + ✓ Segmented transfer - set filename for upload + ✓ Block transfer, create 100000 bytes long random binaryFile + ✓ Block download binaryFile + ✓ Block upload to binaryFileReceived - 2x + ✓ Block transfer, files equal + ✓ Segmented transfer, create 1000 bytes long random binaryFileSeg + ✓ Segmented download binaryFileSeg + ✓ Segmented upload to binaryFileSegReceived + ✓ Segmented transfer, files equal + 100000 bytes block down/upload: 1845/2310 milliseconds. + 1000 bytes segmented down/upload: 256/280 milliseconds. + +9 tests, 0 failures + +Test duration: 13.637902731 sec +``` + + +SDO transfer test reports for different target systems +------------------------------------------------------ + +Running tests with `SDO_transfer.bats`, see test.md for preparation of devices. + +### Devices tested 2021-05-02 +- systems: + - Ubuntu 20.10, Linux 5.8.0-41-generic on desktop PC + - Raspberry Pi OS 10 (buster) on RPI +- canopend, testing SDO client and gateway: + - versions CANopenNode: + - "v4.0-25-g8855820" + - "v2.0-6-gacd719e" +- basicDevice, testing SDO server + - version CANopenSocket "v4.0-2-geb7d5f3-dirty" + - version CANopenNode "v4.0-25-g8855820" (v2.0 was not tested) +- CAN interfaces: + - CPC-USB, EMS Dr. Thomas Wuensche + - PCAN-USB: PEAK-System PCAN-USB FD v1 fw v3.2.0 (1 channel, FD not used) + - USBtin: USBtin EB v2.0 (slcand) + - Raspberry Pi 2 (and 4) Model B + - [CAN shield with MCP2515](https://www.sg-electronic-systems.com/can-bus-dual-iso-v2-1-shield-for-raspberry/) + - CPC-USB (PCAN-USB didn't work) + +### Virtual CAN device +Test result: +``` + ✓ Segmented transfer - set filename for upload + ✓ Block transfer, create 100000 bytes long random binaryFile + ✓ Block download binaryFile + ✓ Block upload to binaryFileReceived - 2x + ✓ Block transfer, files equal + ✓ Segmented transfer, create 1000 bytes long random binaryFileSeg + ✓ Segmented download binaryFileSeg + ✓ Segmented upload to binaryFileSegReceived + ✓ Segmented transfer, files equal + 100000 bytes block down/upload: 70/145 milliseconds. + 1000 bytes segmented down/upload: 10/24 milliseconds. +``` + +### CAN bus 1000 Kbps +Test result: +``` + ✓ Segmented transfer - set filename for upload + ✓ Block transfer, create 100000 bytes long random binaryFile + ✓ Block download binaryFile + ✓ Block upload to binaryFileReceived - 2x + ✓ Block transfer, files equal + ✓ Segmented transfer, create 1000 bytes long random binaryFileSeg + ✓ Segmented download binaryFileSeg + ✓ Segmented upload to binaryFileSegReceived + ✓ Segmented transfer, files equal + 100000 bytes block down/upload: 2316/1840 milliseconds. + 1000 bytes segmented down/upload: 254/270 milliseconds. +``` +#### Notes USBtin +USBtin interface doesn't work when sending sequence of data. In block transfer it sends first CAN message in sub-block correctly, then there is a mess on the bus. However data was transferred correctly in all test cases, because of the robustness of the block transfer. But communication time is up to 300 times longer. Reception of the sequence of data as well as segmented transfer always works correctly with USBtin. + +#### Notes Raspberry PI with CAN shield MCP2515 +Raspberry PI shows had problems with CAN overflow with received sequence of data. It couldn't process fast enough and there was socket overflow. RPI sends emergency message and sets error register. Otherwise data transfer was always correct and time to transfer was not significantly longer. With 250 Kbps there were still overflows, but less. Problem was the same, when RPI was basicDevice (multithreaded program) or canopend (single threaded program). Problem was also the same, when using RPI2 or RPI4. But everything works perfectly when using CPC-USB instead of CAN shield with RPI2 or RPI4. +``` +basicDevice: (CO_CANerror_rxMsgError) Socket error msg ID: 0x20000004, Data[0..7]: 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 (can0) +basicDevice: CAN Interface "can0" Rx buffer overflow. Message dropped +canopend: CANopen Emergency message from node 0x04: errorCode=0x0013, errorRegister=0x10, errorBit=0x13, infoCode=0x00000000 +``` + +#### Notes other +No problems were detected on other CAN interfaces. + + +### CAN bus 10 Kbps +Enlarge SDO timeout and reduce data size: +``` +cocomm "set sdo_timeout 2000" +export binaryFileSize=1000 +``` + +Test result: +``` + ✓ Segmented transfer - set filename for upload + ✓ Block transfer, create 1000 bytes long random binaryFile + ✓ Block download binaryFile + ✓ Block upload to binaryFileReceived - 2x + ✓ Block transfer, files equal + ✓ Segmented transfer, create 1000 bytes long random binaryFileSeg + ✓ Segmented download binaryFileSeg + ✓ Segmented upload to binaryFileSegReceived + ✓ Segmented transfer, files equal + 1000 bytes block down/upload: 1741/1731 milliseconds. + 1000 bytes segmented down/upload: 3611/3621 milliseconds. +``` + +#### Notes +USBtin shows the same problems as with 1000 Kbps. All others worked perfectly. diff --git a/Devices/Libraries/Systems/CANopenSocket/tools/get_tools.sh b/Devices/Libraries/Systems/CANopenSocket/tools/get_tools.sh new file mode 100755 index 0000000..6006c69 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/tools/get_tools.sh @@ -0,0 +1,14 @@ +#!/bin/bash + +wget "https://github.com/robincornelius/libedssharp/raw/gh-pages/build/OpenEDSEditor-latest.zip" + +if [ $? -eq 0 ] ; then + rm -r EDSEditor + unzip OpenEDSEditor-latest.zip -d EDSEditor + chmod a+x EDSEditor/*.exe + rm OpenEDSEditor-latest.zip + echo "EDSEditor updated successfully." +fi + +echo "Press enter to continue..." +read diff --git a/Devices/Libraries/Systems/CANopenSocket/tools/run_canopend_candump.sh b/Devices/Libraries/Systems/CANopenSocket/tools/run_canopend_candump.sh new file mode 100755 index 0000000..e5c5488 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/tools/run_canopend_candump.sh @@ -0,0 +1,41 @@ +#!/bin/bash + +if [[ $1 = "" ]] ; then + if [[ $can_device = "" ]] ; then + can_device=vcan0 + fi +else + can_device=$1 +fi +echo "Using CAN device: '$can_device'" + +# if "can_device" does not exist, create it +if ! grep -q $can_device /proc/net/can/rcvlist_all ; then + echo "ip link: adding and setting up virtual CAN device named '$can_device'" + sudo modprobe vcan + sudo ip link add dev $can_device type vcan + sudo ip link set up $can_device +fi + +echo "Running in background: 'candump -td $can_device" +candump -td $can_device& + + +if [[ $2 = "" ]] ; then + if [[ $co_device = "" ]] ; then + co_device="../CANopenNode/canopend" + fi +else + co_device=$2 +fi + +mkdir tmp +echo "Running: '$co_device $can_device -i 1 -s \"tmp/dev1_\" -c \"local-/tmp/CO_command_socket\"'" +echo "-" > tmp/dev1_lss.persist +echo "-" > tmp/dev1_od_comm.persist +$co_device $can_device -i 1 -s "tmp/dev1_" -c "local-/tmp/CO_command_socket" + +rm -r tmp + +echo -e "\nFinished, press enter to close..." +read diff --git a/Devices/Libraries/Systems/CANopenSocket/tools/update_docs.sh b/Devices/Libraries/Systems/CANopenSocket/tools/update_docs.sh new file mode 100755 index 0000000..6057066 --- /dev/null +++ b/Devices/Libraries/Systems/CANopenSocket/tools/update_docs.sh @@ -0,0 +1,11 @@ +#!/bin/bash + +cd ../CANopenNode + +doxygen > /dev/null + +cd .. +ln -s CANopenNode/doc/html/index.html + +echo "Press enter to continue..." +read diff --git a/Devices/Libraries/Systems/can-utils/.clang-format b/Devices/Libraries/Systems/can-utils/.clang-format new file mode 100755 index 0000000..da4867d --- /dev/null +++ b/Devices/Libraries/Systems/can-utils/.clang-format @@ -0,0 +1,118 @@ +# SPDX-License-Identifier: GPL-2.0 +# +# clang-format configuration file. Intended for clang-format >= 11. +# +# For more information, see: +# +# Documentation/process/clang-format.rst +# https://clang.llvm.org/docs/ClangFormat.html +# https://clang.llvm.org/docs/ClangFormatStyleOptions.html +# +--- +AccessModifierOffset: -4 +AlignAfterOpenBracket: Align +AlignConsecutiveAssignments: false +AlignConsecutiveDeclarations: false +AlignEscapedNewlines: Left +AlignOperands: true +AlignTrailingComments: false +AllowAllParametersOfDeclarationOnNextLine: false +AllowShortBlocksOnASingleLine: false +AllowShortCaseLabelsOnASingleLine: false +AllowShortFunctionsOnASingleLine: None +AllowShortIfStatementsOnASingleLine: false +AllowShortLoopsOnASingleLine: false +AlwaysBreakAfterDefinitionReturnType: None +AlwaysBreakAfterReturnType: None +AlwaysBreakBeforeMultilineStrings: false +AlwaysBreakTemplateDeclarations: false +BinPackArguments: true +BinPackParameters: true +BraceWrapping: + AfterClass: false + AfterControlStatement: false + AfterEnum: false + AfterFunction: true + AfterNamespace: true + AfterObjCDeclaration: false + AfterStruct: false + AfterUnion: false + AfterExternBlock: false + BeforeCatch: false + BeforeElse: false + IndentBraces: false + SplitEmptyFunction: true + SplitEmptyRecord: true + SplitEmptyNamespace: true +BreakBeforeBinaryOperators: None +BreakBeforeBraces: Custom +BreakBeforeInheritanceComma: false +BreakBeforeTernaryOperators: false +BreakConstructorInitializersBeforeComma: false +BreakConstructorInitializers: BeforeComma +BreakAfterJavaFieldAnnotations: false +BreakStringLiterals: false +ColumnLimit: 200 +CommentPragmas: "^ IWYU pragma:" +CompactNamespaces: false +ConstructorInitializerAllOnOneLineOrOnePerLine: false +ConstructorInitializerIndentWidth: 8 +ContinuationIndentWidth: 8 +Cpp11BracedListStyle: false +DerivePointerAlignment: false +DisableFormat: false +ExperimentalAutoDetectBinPacking: false +FixNamespaceComments: false + +IncludeBlocks: Preserve +IncludeCategories: + - Regex: ".*" + Priority: 1 +IncludeIsMainRegex: "(Test)?$" +IndentCaseLabels: false +IndentGotoLabels: false +IndentPPDirectives: None +IndentWidth: 8 +IndentWrappedFunctionNames: false +JavaScriptQuotes: Leave +JavaScriptWrapImports: true +KeepEmptyLinesAtTheStartOfBlocks: false +MacroBlockBegin: "" +MacroBlockEnd: "" +MaxEmptyLinesToKeep: 1 +NamespaceIndentation: None +ObjCBinPackProtocolList: Auto +ObjCBlockIndentWidth: 8 +ObjCSpaceAfterProperty: true +ObjCSpaceBeforeProtocolList: true + +# Taken from git's rules +PenaltyBreakAssignment: 10 +PenaltyBreakBeforeFirstCallParameter: 30 +PenaltyBreakComment: 10 +PenaltyBreakFirstLessLess: 0 +PenaltyBreakString: 10 +PenaltyExcessCharacter: 100 +PenaltyReturnTypeOnItsOwnLine: 60 + +PointerAlignment: Right +ReflowComments: false +SortIncludes: false +SortUsingDeclarations: false +SpaceAfterCStyleCast: false +SpaceAfterTemplateKeyword: true +SpaceBeforeAssignmentOperators: true +SpaceBeforeCtorInitializerColon: true +SpaceBeforeInheritanceColon: true +SpaceBeforeParens: ControlStatementsExceptForEachMacros +SpaceBeforeRangeBasedForLoopColon: true +SpaceInEmptyParentheses: false +SpacesBeforeTrailingComments: 1 +SpacesInAngles: false +SpacesInContainerLiterals: false +SpacesInCStyleCastParentheses: false +SpacesInParentheses: false +SpacesInSquareBrackets: false +Standard: Cpp03 +TabWidth: 8 +UseTab: Always diff --git a/Devices/Libraries/Systems/can-utils/.editorconfig b/Devices/Libraries/Systems/can-utils/.editorconfig new file mode 100755 index 0000000..da2e567 --- /dev/null +++ b/Devices/Libraries/Systems/can-utils/.editorconfig @@ -0,0 +1,16 @@ +root = true + +[*] +end_of_line = lf +trim_trailing_whitespace = true +insert_final_newline = true + +[{CMakeLists.txt,*.cmake}] +indent_style = space +indent_size = 2 +tab_width = 2 + +[{*.c,*.h}] +indent_style = tab +indent_size = 8 +tab_width = 8 diff --git a/Devices/Libraries/Systems/can-utils/.github/workflows/codeql-analysis.yml b/Devices/Libraries/Systems/can-utils/.github/workflows/codeql-analysis.yml new file mode 100755 index 0000000..97daee1 --- /dev/null +++ b/Devices/Libraries/Systems/can-utils/.github/workflows/codeql-analysis.yml @@ -0,0 +1,70 @@ +# For most projects, this workflow file will not need changing; you simply need +# to commit it to your repository. +# +# You may wish to alter this file to override the set of languages analyzed, +# or to provide custom queries or build logic. +# +# ******** NOTE ******** +# We have attempted to detect the languages in your repository. Please check +# the `language` matrix defined below to confirm you have the correct set of +# supported CodeQL languages. +# +name: "CodeQL" + +on: + push: + branches: [ master ] + pull_request: + # The branches below must be a subset of the branches above + branches: [ master ] + schedule: + - cron: '29 21 * * 1' + +jobs: + analyze: + name: Analyze + runs-on: ubuntu-latest + permissions: + actions: read + contents: read + security-events: write + + strategy: + fail-fast: false + matrix: + language: [ 'cpp' ] + # CodeQL supports [ 'cpp', 'csharp', 'go', 'java', 'javascript', 'python', 'ruby' ] + # Learn more about CodeQL language support at https://git.io/codeql-language-support + + steps: + - name: Checkout repository + uses: actions/checkout@v2 + + # Initializes the CodeQL tools for scanning. + - name: Initialize CodeQL + uses: github/codeql-action/init@v1 + with: + languages: ${{ matrix.language }} + # If you wish to specify custom queries, you can do so here or in a config file. + # By default, queries listed here will override any specified in a config file. + # Prefix the list here with "+" to use these queries and those in the config file. + # queries: ./path/to/local/query, your-org/your-repo/queries@main + + # Autobuild attempts to build any compiled languages (C/C++, C#, or Java). + # If this step fails, then you should remove it and run the build manually (see below) + - name: Autobuild + uses: github/codeql-action/autobuild@v1 + + # ℹ️ Command-line programs to run using the OS shell. + # 📚 https://git.io/JvXDl + + # ✏️ If the Autobuild fails above, remove it and uncomment the following three lines + # and modify them (or add more) to build your code if your project + # uses a compiled language + + #- run: | + # make bootstrap + # make release + + - name: Perform CodeQL Analysis + uses: github/codeql-action/analyze@v1 diff --git a/Devices/Libraries/Systems/can-utils/.github/workflows/compile.yml b/Devices/Libraries/Systems/can-utils/.github/workflows/compile.yml new file mode 100755 index 0000000..5c045a8 --- /dev/null +++ b/Devices/Libraries/Systems/can-utils/.github/workflows/compile.yml @@ -0,0 +1,104 @@ +name: native and cross + +on: [push, pull_request] + +jobs: + build: + runs-on: ubuntu-24.04 + strategy: + fail-fast: false + matrix: + release: + - "ubuntu:20.04" + - "ubuntu:22.04" + - "ubuntu:24.04" + - "ubuntu:rolling" + - "debian:oldstable-slim" + - "debian:stable-slim" + - "debian:testing-slim" + - "debian:unstable-slim" + - "debian:experimental" + + steps: + - uses: actions/checkout@v4 + + - name: Prepare ${{ matrix.release }} container + env: + release: ${{ matrix.release == 'debian:experimental' && '-t experimental' || '' }} + run: | + podman version + podman run --name stable -di --userns=keep-id:uid=1000,gid=1000 -v "$PWD":/home -w /home ${{ matrix.release }} bash + podman exec -i stable uname -a + podman exec -i stable id + podman exec -i -u root stable apt update + podman exec -e DEBIAN_FRONTEND='noninteractive' -i -u root stable apt install -o APT::Install-Suggests=false -qy ${release} \ + clang \ + cmake \ + gcc \ + gcc-aarch64-linux-gnu \ + gcc-arm-linux-gnueabihf \ + gcc-mips-linux-gnu \ + make + + - name: Configure & Build with gcc + env: + cc: gcc + run: | + podman exec -i stable cmake -DCMAKE_BUILD_TYPE=Debug -DCMAKE_C_COMPILER=${cc} -DENABLE_WERROR=ON -B build-${cc} + podman exec -i stable cmake --build build-${cc} + + - name: Configure & Build with clang + env: + cc: clang + run: | + podman exec -i stable cmake -DCMAKE_BUILD_TYPE=Debug -DCMAKE_C_COMPILER=${cc} -DENABLE_WERROR=ON -B build-${cc} + podman exec -i stable cmake --build build-${cc} + + - name: Configure & Build with arm-linux-gnueabihf-gcc + env: + toolchain: arm-linux-gnueabihf-gcc + run: | + podman exec -i stable cmake -DCMAKE_BUILD_TYPE=Debug -DCMAKE_TOOLCHAIN_FILE=cmake/${toolchain}.cmake -DENABLE_WERROR=ON -B build-${toolchain} + podman exec -i stable cmake --build build-${toolchain} + + - name: Configure & Build with arm-linux-gnueabihf-clang + if: + ${{ matrix.release != 'ubuntu:20.04' && matrix.release != 'debian:oldstable-slim' }} + env: + toolchain: arm-linux-gnueabihf-clang + run: | + podman exec -i stable cmake -DCMAKE_BUILD_TYPE=Debug -DCMAKE_TOOLCHAIN_FILE=cmake/${toolchain}.cmake -DENABLE_WERROR=ON -B build-${toolchain} + podman exec -i stable cmake --build build-${toolchain} + + - name: Configure & Build with aarch64-linux-gnu-gcc + env: + toolchain: aarch64-linux-gnu-gcc + run: | + podman exec -i stable cmake -DCMAKE_BUILD_TYPE=Debug -DCMAKE_TOOLCHAIN_FILE=cmake/${toolchain}.cmake -DENABLE_WERROR=ON -B build-${toolchain} + podman exec -i stable cmake --build build-${toolchain} + + - name: Configure & Build with aarch64-linux-gnu-clang + if: + ${{ matrix.release != 'ubuntu:20.04' && matrix.release != 'debian:oldstable-slim' }} + env: + toolchain: aarch64-linux-gnu-clang + run: | + podman exec -i stable cmake -DCMAKE_BUILD_TYPE=Debug -DCMAKE_TOOLCHAIN_FILE=cmake/${toolchain}.cmake -DENABLE_WERROR=ON -B build-${toolchain} + podman exec -i stable cmake --build build-${toolchain} + + - name: Configure & Build with mips-linux-gnu-gcc + env: + toolchain: mips-linux-gnu-gcc + run: | + podman exec -i stable cmake -DCMAKE_BUILD_TYPE=Debug -DCMAKE_TOOLCHAIN_FILE=cmake/${toolchain}.cmake -DENABLE_WERROR=ON -B build-${toolchain} + podman exec -i stable cmake --build build-${toolchain} + + - name: Show logs + if: ${{ failure() }} + run: | + for log in build-*/CMakeFiles/{CMakeOutput.log,CMakeConfigureLog.yaml}; do \ + if [ -e ${log} ]; then \ + echo "---------------- ${log} ----------------"; \ + cat ${log}; \ + fi; \ + done diff --git a/Devices/Libraries/Systems/can-utils/.github/workflows/ndk.yml b/Devices/Libraries/Systems/can-utils/.github/workflows/ndk.yml new file mode 100755 index 0000000..3020af1 --- /dev/null +++ b/Devices/Libraries/Systems/can-utils/.github/workflows/ndk.yml @@ -0,0 +1,15 @@ +name: NDK + +on: [push, pull_request] + +jobs: + build: + runs-on: ubuntu-latest + + steps: + - uses: actions/checkout@v4 + + - name: CMake and NDK + run: | + cmake -DCMAKE_BUILD_TYPE=Debug -DENABLE_WERROR=ON -DCMAKE_TOOLCHAIN_FILE=$ANDROID_NDK/build/cmake/android.toolchain.cmake -B build + cmake --build build diff --git a/Devices/Libraries/Systems/can-utils/.gitignore b/Devices/Libraries/Systems/can-utils/.gitignore new file mode 100755 index 0000000..24a8311 --- /dev/null +++ b/Devices/Libraries/Systems/can-utils/.gitignore @@ -0,0 +1,40 @@ +*~ +*.o +.ccls-cache +compile_commands.json +tags +/build + +/asc2log +/bcmserver +/can-calc-bit-timing +/canbusload +/candump +/canfdtest +/cangen +/cangw +/canlogserver +/canplayer +/cansend +/cansequence +/cansniffer +/isobusfs-cli +/isobusfs-srv +/isotpdump +/isotpperf +/isotprecv +/isotpsend +/isotpserver +/isotpsniffer +/isotptun +/j1939acd +/j1939cat +/j1939spy +/j1939sr +/log2asc +/log2long +/mcp251xfd-dump +/slcan_attach +/slcand +/slcanpty +/testj1939 diff --git a/Devices/Libraries/Systems/can-utils/CMakeLists.txt b/Devices/Libraries/Systems/can-utils/CMakeLists.txt new file mode 100755 index 0000000..01f90f9 --- /dev/null +++ b/Devices/Libraries/Systems/can-utils/CMakeLists.txt @@ -0,0 +1,225 @@ +cmake_minimum_required(VERSION 3.5) + +project(can-utils LANGUAGES C) + +message(STATUS "CMake version: ${CMAKE_VERSION}") + +include (CheckFunctionExists) +include (CheckSymbolExists) +include (GNUInstallDirs) + +if(NOT CMAKE_BUILD_TYPE) + set(CMAKE_BUILD_TYPE Release) +endif() + +# Add an option to enable treating warnings as errors +option(ENABLE_WERROR "Treat all compiler warnings as errors" OFF) + +if(ENABLE_WERROR) + add_compile_options(-Werror) +endif() + +add_definitions(-D_GNU_SOURCE) + +set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -Wno-parentheses -Wsign-compare") +set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -fno-strict-aliasing") +set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -DSO_RXQ_OVFL=40") +set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -DPF_CAN=29") +set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -DAF_CAN=PF_CAN") +set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -DN_SLCAN=17") +set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -DSCM_TIMESTAMPING_OPT_STATS=54") +set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -DCLOCK_TAI=11") +set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -DSO_TXTIME=61") +set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -DSCM_TXTIME=SO_TXTIME") + +include_directories (.) +include_directories (./include) + +check_function_exists(fork HAVE_FORK) + +set(PROGRAMS_CANLIB + asc2log + canbusload + candump + cangen + canplayer + cansend + cansequence + log2asc + log2long + slcanpty +) + +if(HAVE_FORK) + list(APPEND PROGRAMS_CANLIB canlogserver) +endif() + +set(PROGRAMS_J1939 + j1939acd + j1939cat + j1939spy + j1939sr + testj1939 +) + +set(PROGRAMS_J1939_TIMEDATE + j1939-timedate-srv + j1939-timedate-cli +) + +set(PROGRAMS_ISOBUSFS + isobusfs-srv + isobusfs-cli +) + +set(PROGRAMS + ${PROGRAMS_CANLIB} + canfdtest + cangw + cansniffer + isotpdump + isotpperf + isotprecv + isotpsend + isotpsniffer + isotptun + slcan_attach + slcand +) + +if(HAVE_FORK) + list(APPEND PROGRAMS bcmserver) + list(APPEND PROGRAMS isotpserver) +endif() + +add_executable(can-calc-bit-timing + calc-bit-timing/can-calc-bit-timing.c +) + +add_executable(mcp251xfd-dump + mcp251xfd/mcp251xfd-dev-coredump.c + mcp251xfd/mcp251xfd-dump.c + mcp251xfd/mcp251xfd-main.c + mcp251xfd/mcp251xfd-regmap.c +) + +if(NOT ANDROID) + list(APPEND PROGRAMS ${PROGRAMS_J1939}) + + add_library(j1939 STATIC + libj1939.c + ) + + target_link_libraries(j1939 + PRIVATE can + ) + + add_library(isobusfs SHARED + isobusfs/isobusfs_cmn.c + isobusfs/isobusfs_cmn_dh.c + ) + + set(PUBLIC_HEADER_ISOBUSFS + isobusfs/isobusfs_cmn.h + isobusfs/isobusfs_cmn_cm.h + ) + + set_target_properties(isobusfs PROPERTIES + PUBLIC_HEADER "${PUBLIC_HEADER_ISOBUSFS}" + ) + + install(TARGETS isobusfs + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} + ) + + add_executable(isobusfs-cli + isobusfs/isobusfs_cli.c + isobusfs/isobusfs_cli_cm.c + isobusfs/isobusfs_cli_dh.c + isobusfs/isobusfs_cli_fa.c + isobusfs/isobusfs_cli_selftests.c + isobusfs/isobusfs_cli_int.c + ) + + target_link_libraries(isobusfs-cli + PRIVATE isobusfs can j1939 + ) + + add_executable(isobusfs-srv + isobusfs/isobusfs_srv.c + isobusfs/isobusfs_srv_cm.c + isobusfs/isobusfs_srv_cm_fss.c + isobusfs/isobusfs_srv_dh.c + isobusfs/isobusfs_srv_fa.c + isobusfs/isobusfs_srv_fh.c + isobusfs/isobusfs_srv_vh.c + ) + + target_link_libraries(isobusfs-srv + PRIVATE isobusfs can j1939 + ) + + install(TARGETS + isobusfs-cli + isobusfs-srv + DESTINATION ${CMAKE_INSTALL_BINDIR}) + + set(PUBLIC_HEADER_j1939_TIMEDATE + j1939_timedate/j1939_timedate_cmn.h + ) + + add_executable(j1939-timedate-cli + j1939_timedate/j1939_timedate_cli.c + ) + + target_link_libraries(j1939-timedate-cli + PRIVATE can j1939 + ) + + add_executable(j1939-timedate-srv + j1939_timedate/j1939_timedate_srv.c + ) + + target_link_libraries(j1939-timedate-srv + PRIVATE can j1939 + ) + + install(TARGETS + j1939-timedate-cli + j1939-timedate-srv + DESTINATION ${CMAKE_INSTALL_BINDIR}) + +endif() + +add_library(can STATIC + lib.c + canframelen.c +) + +foreach(name ${PROGRAMS}) + add_executable(${name} ${name}.c) + + if("${name}" IN_LIST PROGRAMS_J1939) + target_link_libraries(${name} + PRIVATE j1939 can + ) + elseif("${name}" IN_LIST PROGRAMS_CANLIB) + target_link_libraries(${name} + PRIVATE can + ) + endif() + + install(TARGETS ${name} DESTINATION ${CMAKE_INSTALL_BINDIR}) +endforeach() + +install(TARGETS + can-calc-bit-timing + mcp251xfd-dump + DESTINATION ${CMAKE_INSTALL_BINDIR} +) + +add_custom_target(uninstall + "${CMAKE_COMMAND}" -P "${CMAKE_SOURCE_DIR}/cmake/make_uninstall.cmake" + COMMENT "Add uninstall target" +) diff --git a/Devices/Libraries/Systems/can-utils/LICENSES/BSD-3-Clause b/Devices/Libraries/Systems/can-utils/LICENSES/BSD-3-Clause new file mode 100755 index 0000000..5858df5 --- /dev/null +++ b/Devices/Libraries/Systems/can-utils/LICENSES/BSD-3-Clause @@ -0,0 +1,27 @@ +Copyright (c) . All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +1. Redistributions of source code must retain the above copyright notice, + this list of conditions and the following disclaimer. + +2. Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + +3. Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from this + software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +POSSIBILITY OF SUCH DAMAGE. diff --git a/Devices/Libraries/Systems/can-utils/LICENSES/GPL-2.0-only.txt b/Devices/Libraries/Systems/can-utils/LICENSES/GPL-2.0-only.txt new file mode 100755 index 0000000..2b7b643 --- /dev/null +++ b/Devices/Libraries/Systems/can-utils/LICENSES/GPL-2.0-only.txt @@ -0,0 +1,340 @@ + GNU GENERAL PUBLIC LICENSE + Version 2, June 1991 + + Copyright (C) 1989, 1991 Free Software Foundation, Inc. + 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + Everyone is permitted to copy and distribute verbatim copies + of this license document, but changing it is not allowed. + + Preamble + + The licenses for most software are designed to take away your +freedom to share and change it. By contrast, the GNU General Public +License is intended to guarantee your freedom to share and change free +software--to make sure the software is free for all its users. This +General Public License applies to most of the Free Software +Foundation's software and to any other program whose authors commit to +using it. (Some other Free Software Foundation software is covered by +the GNU Library General Public License instead.) You can apply it to +your programs, too. + + When we speak of free software, we are referring to freedom, not +price. Our General Public Licenses are designed to make sure that you +have the freedom to distribute copies of free software (and charge for +this service if you wish), that you receive source code or can get it +if you want it, that you can change the software or use pieces of it +in new free programs; and that you know you can do these things. + + To protect your rights, we need to make restrictions that forbid +anyone to deny you these rights or to ask you to surrender the rights. +These restrictions translate to certain responsibilities for you if you +distribute copies of the software, or if you modify it. + + For example, if you distribute copies of such a program, whether +gratis or for a fee, you must give the recipients all the rights that +you have. You must make sure that they, too, receive or can get the +source code. And you must show them these terms so they know their +rights. + + We protect your rights with two steps: (1) copyright the software, and +(2) offer you this license which gives you legal permission to copy, +distribute and/or modify the software. + + Also, for each author's protection and ours, we want to make certain +that everyone understands that there is no warranty for this free +software. If the software is modified by someone else and passed on, we +want its recipients to know that what they have is not the original, so +that any problems introduced by others will not reflect on the original +authors' reputations. + + Finally, any free program is threatened constantly by software +patents. We wish to avoid the danger that redistributors of a free +program will individually obtain patent licenses, in effect making the +program proprietary. To prevent this, we have made it clear that any +patent must be licensed for everyone's free use or not licensed at all. + + The precise terms and conditions for copying, distribution and +modification follow. + + GNU GENERAL PUBLIC LICENSE + TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION + + 0. This License applies to any program or other work which contains +a notice placed by the copyright holder saying it may be distributed +under the terms of this General Public License. The "Program", below, +refers to any such program or work, and a "work based on the Program" +means either the Program or any derivative work under copyright law: +that is to say, a work containing the Program or a portion of it, +either verbatim or with modifications and/or translated into another +language. (Hereinafter, translation is included without limitation in +the term "modification".) Each licensee is addressed as "you". + +Activities other than copying, distribution and modification are not +covered by this License; they are outside its scope. The act of +running the Program is not restricted, and the output from the Program +is covered only if its contents constitute a work based on the +Program (independent of having been made by running the Program). +Whether that is true depends on what the Program does. + + 1. You may copy and distribute verbatim copies of the Program's +source code as you receive it, in any medium, provided that you +conspicuously and appropriately publish on each copy an appropriate +copyright notice and disclaimer of warranty; keep intact all the +notices that refer to this License and to the absence of any warranty; +and give any other recipients of the Program a copy of this License +along with the Program. + +You may charge a fee for the physical act of transferring a copy, and +you may at your option offer warranty protection in exchange for a fee. + + 2. You may modify your copy or copies of the Program or any portion +of it, thus forming a work based on the Program, and copy and +distribute such modifications or work under the terms of Section 1 +above, provided that you also meet all of these conditions: + + a) You must cause the modified files to carry prominent notices + stating that you changed the files and the date of any change. + + b) You must cause any work that you distribute or publish, that in + whole or in part contains or is derived from the Program or any + part thereof, to be licensed as a whole at no charge to all third + parties under the terms of this License. + + c) If the modified program normally reads commands interactively + when run, you must cause it, when started running for such + interactive use in the most ordinary way, to print or display an + announcement including an appropriate copyright notice and a + notice that there is no warranty (or else, saying that you provide + a warranty) and that users may redistribute the program under + these conditions, and telling the user how to view a copy of this + License. (Exception: if the Program itself is interactive but + does not normally print such an announcement, your work based on + the Program is not required to print an announcement.) + +These requirements apply to the modified work as a whole. If +identifiable sections of that work are not derived from the Program, +and can be reasonably considered independent and separate works in +themselves, then this License, and its terms, do not apply to those +sections when you distribute them as separate works. But when you +distribute the same sections as part of a whole which is a work based +on the Program, the distribution of the whole must be on the terms of +this License, whose permissions for other licensees extend to the +entire whole, and thus to each and every part regardless of who wrote it. + +Thus, it is not the intent of this section to claim rights or contest +your rights to work written entirely by you; rather, the intent is to +exercise the right to control the distribution of derivative or +collective works based on the Program. + +In addition, mere aggregation of another work not based on the Program +with the Program (or with a work based on the Program) on a volume of +a storage or distribution medium does not bring the other work under +the scope of this License. + + 3. You may copy and distribute the Program (or a work based on it, +under Section 2) in object code or executable form under the terms of +Sections 1 and 2 above provided that you also do one of the following: + + a) Accompany it with the complete corresponding machine-readable + source code, which must be distributed under the terms of Sections + 1 and 2 above on a medium customarily used for software interchange; or, + + b) Accompany it with a written offer, valid for at least three + years, to give any third party, for a charge no more than your + cost of physically performing source distribution, a complete + machine-readable copy of the corresponding source code, to be + distributed under the terms of Sections 1 and 2 above on a medium + customarily used for software interchange; or, + + c) Accompany it with the information you received as to the offer + to distribute corresponding source code. (This alternative is + allowed only for noncommercial distribution and only if you + received the program in object code or executable form with such + an offer, in accord with Subsection b above.) + +The source code for a work means the preferred form of the work for +making modifications to it. For an executable work, complete source +code means all the source code for all modules it contains, plus any +associated interface definition files, plus the scripts used to +control compilation and installation of the executable. However, as a +special exception, the source code distributed need not include +anything that is normally distributed (in either source or binary +form) with the major components (compiler, kernel, and so on) of the +operating system on which the executable runs, unless that component +itself accompanies the executable. + +If distribution of executable or object code is made by offering +access to copy from a designated place, then offering equivalent +access to copy the source code from the same place counts as +distribution of the source code, even though third parties are not +compelled to copy the source along with the object code. + + 4. You may not copy, modify, sublicense, or distribute the Program +except as expressly provided under this License. Any attempt +otherwise to copy, modify, sublicense or distribute the Program is +void, and will automatically terminate your rights under this License. +However, parties who have received copies, or rights, from you under +this License will not have their licenses terminated so long as such +parties remain in full compliance. + + 5. You are not required to accept this License, since you have not +signed it. However, nothing else grants you permission to modify or +distribute the Program or its derivative works. These actions are +prohibited by law if you do not accept this License. Therefore, by +modifying or distributing the Program (or any work based on the +Program), you indicate your acceptance of this License to do so, and +all its terms and conditions for copying, distributing or modifying +the Program or works based on it. + + 6. Each time you redistribute the Program (or any work based on the +Program), the recipient automatically receives a license from the +original licensor to copy, distribute or modify the Program subject to +these terms and conditions. You may not impose any further +restrictions on the recipients' exercise of the rights granted herein. +You are not responsible for enforcing compliance by third parties to +this License. + + 7. If, as a consequence of a court judgment or allegation of patent +infringement or for any other reason (not limited to patent issues), +conditions are imposed on you (whether by court order, agreement or +otherwise) that contradict the conditions of this License, they do not +excuse you from the conditions of this License. If you cannot +distribute so as to satisfy simultaneously your obligations under this +License and any other pertinent obligations, then as a consequence you +may not distribute the Program at all. For example, if a patent +license would not permit royalty-free redistribution of the Program by +all those who receive copies directly or indirectly through you, then +the only way you could satisfy both it and this License would be to +refrain entirely from distribution of the Program. + +If any portion of this section is held invalid or unenforceable under +any particular circumstance, the balance of the section is intended to +apply and the section as a whole is intended to apply in other +circumstances. + +It is not the purpose of this section to induce you to infringe any +patents or other property right claims or to contest validity of any +such claims; this section has the sole purpose of protecting the +integrity of the free software distribution system, which is +implemented by public license practices. Many people have made +generous contributions to the wide range of software distributed +through that system in reliance on consistent application of that +system; it is up to the author/donor to decide if he or she is willing +to distribute software through any other system and a licensee cannot +impose that choice. + +This section is intended to make thoroughly clear what is believed to +be a consequence of the rest of this License. + + 8. If the distribution and/or use of the Program is restricted in +certain countries either by patents or by copyrighted interfaces, the +original copyright holder who places the Program under this License +may add an explicit geographical distribution limitation excluding +those countries, so that distribution is permitted only in or among +countries not thus excluded. In such case, this License incorporates +the limitation as if written in the body of this License. + + 9. The Free Software Foundation may publish revised and/or new versions +of the General Public License from time to time. Such new versions will +be similar in spirit to the present version, but may differ in detail to +address new problems or concerns. + +Each version is given a distinguishing version number. If the Program +specifies a version number of this License which applies to it and "any +later version", you have the option of following the terms and conditions +either of that version or of any later version published by the Free +Software Foundation. If the Program does not specify a version number of +this License, you may choose any version ever published by the Free Software +Foundation. + + 10. If you wish to incorporate parts of the Program into other free +programs whose distribution conditions are different, write to the author +to ask for permission. For software which is copyrighted by the Free +Software Foundation, write to the Free Software Foundation; we sometimes +make exceptions for this. Our decision will be guided by the two goals +of preserving the free status of all derivatives of our free software and +of promoting the sharing and reuse of software generally. + + NO WARRANTY + + 11. BECAUSE THE PROGRAM IS LICENSED FREE OF CHARGE, THERE IS NO WARRANTY +FOR THE PROGRAM, TO THE EXTENT PERMITTED BY APPLICABLE LAW. EXCEPT WHEN +OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR OTHER PARTIES +PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED +OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF +MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE ENTIRE RISK AS +TO THE QUALITY AND PERFORMANCE OF THE PROGRAM IS WITH YOU. SHOULD THE +PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF ALL NECESSARY SERVICING, +REPAIR OR CORRECTION. + + 12. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING +WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY AND/OR +REDISTRIBUTE THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, +INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING +OUT OF THE USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED +TO LOSS OF DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY +YOU OR THIRD PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER +PROGRAMS), EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE +POSSIBILITY OF SUCH DAMAGES. + + END OF TERMS AND CONDITIONS + + How to Apply These Terms to Your New Programs + + If you develop a new program, and you want it to be of the greatest +possible use to the public, the best way to achieve this is to make it +free software which everyone can redistribute and change under these terms. + + To do so, attach the following notices to the program. It is safest +to attach them to the start of each source file to most effectively +convey the exclusion of warranty; and each file should have at least +the "copyright" line and a pointer to where the full notice is found. + + + Copyright (C) 19yy + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 2 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program; if not, write to the Free Software + Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + + +Also add information on how to contact you by electronic and paper mail. + +If the program is interactive, make it output a short notice like this +when it starts in an interactive mode: + + Gnomovision version 69, Copyright (C) 19yy name of author + Gnomovision comes with ABSOLUTELY NO WARRANTY; for details type `show w'. + This is free software, and you are welcome to redistribute it + under certain conditions; type `show c' for details. + +The hypothetical commands `show w' and `show c' should show the appropriate +parts of the General Public License. Of course, the commands you use may +be called something other than `show w' and `show c'; they could even be +mouse-clicks or menu items--whatever suits your program. + +You should also get your employer (if you work as a programmer) or your +school, if any, to sign a "copyright disclaimer" for the program, if +necessary. Here is a sample; alter the names: + + Yoyodyne, Inc., hereby disclaims all copyright interest in the program + `Gnomovision' (which makes passes at compilers) written by James Hacker. + + , 1 April 1989 + Ty Coon, President of Vice + +This General Public License does not permit incorporating your program into +proprietary programs. If your program is a subroutine library, you may +consider it more useful to permit linking proprietary applications with the +library. If this is what you want to do, use the GNU Library General +Public License instead of this License. diff --git a/Devices/Libraries/Systems/can-utils/LICENSES/Linux-syscall-note.txt b/Devices/Libraries/Systems/can-utils/LICENSES/Linux-syscall-note.txt new file mode 100755 index 0000000..3d7ab24 --- /dev/null +++ b/Devices/Libraries/Systems/can-utils/LICENSES/Linux-syscall-note.txt @@ -0,0 +1,12 @@ + NOTE! This copyright does *not* cover user programs that use kernel + services by normal system calls - this is merely considered normal use + of the kernel, and does *not* fall under the heading of "derived work". + Also note that the GPL below is copyrighted by the Free Software + Foundation, but the instance of code that it refers to (the Linux + kernel) is copyrighted by me and others who actually wrote it. + + Also note that the only valid version of the GPL as far as the kernel + is concerned is _this_ particular version of the license (ie v2, not + v2.2 or v3.x or whatever), unless explicitly otherwise stated. + + Linus Torvalds diff --git a/Devices/Libraries/Systems/can-utils/Makefile b/Devices/Libraries/Systems/can-utils/Makefile new file mode 100755 index 0000000..08366c8 --- /dev/null +++ b/Devices/Libraries/Systems/can-utils/Makefile @@ -0,0 +1,213 @@ +# +# Copyright (c) 2002-2005 Volkswagen Group Electronic Research +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions, the following disclaimer and +# the referenced file 'COPYING'. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# 3. Neither the name of Volkswagen nor the names of its contributors +# may be used to endorse or promote products derived from this software +# without specific prior written permission. +# +# Alternatively, provided that this notice is retained in full, this +# software may be distributed under the terms of the GNU General +# Public License ("GPL") version 2 as distributed in the 'COPYING' +# file from the main directory of the linux kernel source. +# +# The provided data structures and external interfaces from this code +# are not restricted to be used by modules with a GPL compatible license. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR +# A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +# OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +# SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +# LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, +# DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY +# THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +# (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH +# DAMAGE. +# +# Send feedback to + +DESTDIR ?= +PREFIX ?= /usr/local + +MAKEFLAGS := -k + +CFLAGS := -O2 -Wall -Wno-parentheses -Wsign-compare + +HAVE_FORK := $(shell ./check_cc.sh "$(CC)" fork_test.c) + +CPPFLAGS += \ + -I. \ + -Iinclude \ + -DAF_CAN=PF_CAN \ + -DPF_CAN=29 \ + -DSO_RXQ_OVFL=40 \ + -DSCM_TIMESTAMPING_OPT_STATS=54 \ + -DCLOCK_TAI=11 \ + -DSO_TXTIME=61 \ + -DSCM_TXTIME=SO_TXTIME \ + -D_FILE_OFFSET_BITS=64 \ + -D_GNU_SOURCE + +PROGRAMS_CANGW := \ + cangw + +PROGRAMS_J1939_TIMEDATE := \ + j1939-timedate-srv \ + j1939-timedate-cli + +PROGRAMS_ISOBUSFS := \ + isobusfs-srv \ + isobusfs-cli + +PROGRAMS_ISOTP := \ + isotpdump \ + isotpperf \ + isotprecv \ + isotpsend \ + isotpsniffer \ + isotptun + +ifeq ($(HAVE_FORK),1) +PROGRAMS_ISOTP += \ + isotpserver +endif + +PROGRAMS_J1939 := \ + j1939acd \ + j1939cat \ + j1939spy \ + j1939sr \ + testj1939 + +PROGRAMS_SLCAN := \ + slcan_attach \ + slcand + +PROGRAMS := \ + $(PROGRAMS_CANGW) \ + $(PROGRAMS_J1939_TIMEDATE) \ + $(PROGRAMS_ISOBUSFS) \ + $(PROGRAMS_ISOTP) \ + $(PROGRAMS_J1939) \ + $(PROGRAMS_SLCAN) \ + asc2log \ + can-calc-bit-timing \ + canbusload \ + candump \ + canfdtest \ + cangen \ + cansequence \ + canplayer \ + cansend \ + cansniffer \ + log2asc \ + log2long \ + mcp251xfd-dump \ + slcanpty + +ifeq ($(HAVE_FORK),1) +PROGRAMS += \ + canlogserver \ + bcmserver +endif + +all: $(PROGRAMS) + +clean: + rm -f $(PROGRAMS) *.o mcp251xfd/*.o isobusfs/*.o j1939_timedate/*.o + +install: + mkdir -p $(DESTDIR)$(PREFIX)/bin + cp -f $(PROGRAMS) $(DESTDIR)$(PREFIX)/bin + +distclean: clean + rm -f $(PROGRAMS) $(LIBRARIES) *~ + +asc2log.o: lib.h +candump.o: lib.h +cangen.o: lib.h +canlogserver.o: lib.h +canplayer.o: lib.h +cansend.o: lib.h +log2asc.o: lib.h +log2long.o: lib.h +slcanpty.o: lib.h +j1939acd.o: lib.h libj1939.h +j1939cat.o: lib.h libj1939.h +j1939spy.o: lib.h libj1939.h +j1939sr.o: lib.h libj1939.h +testj1939.o: lib.h libj1939.h +isobusfs_srv.o: lib.h libj1939.h +isobusfs_c.o: lib.h libj1939.h +j1939_timedate_srv.o: lib.h libj1939.h +j1939_timedate_cli.o: lib.h libj1939.h +canframelen.o: canframelen.h + +asc2log: asc2log.o lib.o +canbusload: canbusload.o canframelen.o +candump: candump.o lib.o +cangen: cangen.o lib.o +canlogserver: canlogserver.o lib.o +canplayer: canplayer.o lib.o +cansend: cansend.o lib.o +cansequence: cansequence.o lib.o +log2asc: log2asc.o lib.o +log2long: log2long.o lib.o +slcanpty: slcanpty.o lib.o +j1939acd: j1939acd.o lib.o libj1939.o +j1939cat: j1939cat.o lib.o libj1939.o +j1939spy: j1939spy.o lib.o libj1939.o +j1939sr: j1939sr.o lib.o libj1939.o +testj1939: testj1939.o lib.o libj1939.o + +j1939-timedate-srv: lib.o \ + libj1939.o \ + j1939_timedate/j1939_timedate_srv.o + $(CC) $(LDFLAGS) $^ $(LDLIBS) -o $@ + +j1939-timedate-cli: lib.o \ + libj1939.o \ + j1939_timedate/j1939_timedate_cli.o + $(CC) $(LDFLAGS) $^ $(LDLIBS) -o $@ + +isobusfs-srv: lib.o \ + libj1939.o \ + isobusfs/isobusfs_cmn.o \ + isobusfs/isobusfs_srv.o \ + isobusfs/isobusfs_srv_cm.o \ + isobusfs/isobusfs_srv_cm_fss.o \ + isobusfs/isobusfs_srv_dh.o \ + isobusfs/isobusfs_srv_fa.o \ + isobusfs/isobusfs_srv_fh.o \ + isobusfs/isobusfs_srv_vh.o \ + isobusfs/isobusfs_cmn_dh.o + $(CC) $(LDFLAGS) $^ $(LDLIBS) -o $@ + +isobusfs-cli: lib.o \ + libj1939.o \ + isobusfs/isobusfs_cmn.o \ + isobusfs/isobusfs_cli.o \ + isobusfs/isobusfs_cli_cm.o \ + isobusfs/isobusfs_cli_dh.o \ + isobusfs/isobusfs_cli_fa.o \ + isobusfs/isobusfs_cli_selftests.o \ + isobusfs/isobusfs_cli_int.o + $(CC) $(LDFLAGS) $^ $(LDLIBS) -o $@ + +can-calc-bit-timing: calc-bit-timing/can-calc-bit-timing.o + $(CC) $(LDFLAGS) $^ $(LDLIBS) -o $@ + +mcp251xfd-dump: mcp251xfd/mcp251xfd-dev-coredump.o mcp251xfd/mcp251xfd-dump.o mcp251xfd/mcp251xfd-main.o mcp251xfd/mcp251xfd-regmap.o + $(CC) $(LDFLAGS) $^ $(LDLIBS) -o $@ diff --git a/Devices/Libraries/Systems/can-utils/README.md b/Devices/Libraries/Systems/can-utils/README.md new file mode 100755 index 0000000..acb0ae9 --- /dev/null +++ b/Devices/Libraries/Systems/can-utils/README.md @@ -0,0 +1,78 @@ +

+SocketCAN logo +

+ +### SocketCAN userspace utilities and tools + +This repository contains some userspace utilities for Linux CAN +subsystem (aka SocketCAN): + +#### Basic tools to display, record, generate and replay CAN traffic + +* candump : display, filter and log CAN data to files +* canplayer : replay CAN logfiles +* cansend : send a single frame +* cangen : generate (random) CAN traffic +* cansequence : send and check sequence of CAN frames with incrementing payload +* cansniffer : display CAN data content differences + +#### CAN access via IP sockets +* canlogserver : log CAN frames and serves them +* bcmserver : interactive BCM configuration (remote/local) +* [socketcand](https://github.com/linux-can/socketcand) : use RAW/BCM/ISO-TP sockets via TCP/IP sockets +* [cannelloni](https://github.com/mguentner/cannelloni) : UDP/SCTP based SocketCAN tunnel + +#### CAN in-kernel gateway configuration +* cangw : CAN gateway userspace tool for netlink configuration + +#### CAN bus measurement and testing +* canbusload : calculate and display the CAN busload +* can-calc-bit-timing : userspace version of in-kernel bitrate calculation +* canfdtest : Full-duplex test program (DUT and host part) + +#### ISO-TP tools [ISO15765-2:2016 for Linux](https://github.com/hartkopp/can-isotp) +* isotpsend : send a single ISO-TP PDU +* isotprecv : receive ISO-TP PDU(s) +* isotpsniffer : 'wiretap' ISO-TP PDU(s) +* isotpdump : 'wiretap' and interpret CAN messages (CAN_RAW) +* isotpserver : IP server for simple TCP/IP <-> ISO 15765-2 bridging (ASCII HEX) +* isotpperf : ISO15765-2 protocol performance visualisation +* isotptun : create a bi-directional IP tunnel on CAN via ISO-TP + +#### J1939/ISOBus tools +* j1939acd : address claim daemon +* j1939cat : take a file and send and receive it over CAN +* j1939spy : spy on J1939 messages using SOC_J1939 +* j1939sr : send/recv from stdin or to stdout +* testj1939 : send/receive test packet + +Follow the link to see examples on how this tools can be used: +[Kickstart guide to can-j1939 on linux](https://github.com/linux-can/can-utils/blob/master/can-j1939-kickstart.md) + +#### Log file converters +* asc2log : convert ASC logfile to compact CAN frame logfile +* log2asc : convert compact CAN frame logfile to ASC logfile +* log2long : convert compact CAN frame representation into user readable + +#### Serial Line Discipline configuration (for slcan driver) +* slcan_attach : userspace tool for serial line CAN interface configuration +* slcand : daemon for serial line CAN interface configuration +* slcanpty : creates a pty for applications using the slcan ASCII protocol + +#### CMake Project Generator +* Place your build folder anywhere, passing CMake the path. Relative or absolute. +* Some examples using a build folder under the source tree root: +* Android : ``cmake -DCMAKE_TOOLCHAIN_FILE=~/Android/Sdk/ndk-bundle/build/cmake/android.toolchain.cmake -DANDROID_PLATFORM=android-21 -DANDROID_ABI=armeabi-v7a .. && make`` +* Android Studio : Copy repo under your project's ``app`` folder, add ``add_subdirectory(can-utils)`` to your ``CMakeLists.txt`` file after ``cmake_minimum_required()``. Generating project will build Debug/Release for all supported EABI types. ie. arm64-v8a, armeabi-v7a, x86, x86_64. +* Raspberry Pi : ``cmake -DCMAKE_TOOLCHAIN_FILE=~/rpi/tools/build/cmake/rpi.toolchain.cmake .. && make`` +* Linux : ``cmake -GNinja .. && ninja`` +* Linux Eclipse Photon (Debug) : ``CC=clang cmake -G"Eclipse CDT4 - Unix Makefiles" ../can-utils/ -DCMAKE_BUILD_TYPE=Debug -DCMAKE_ECLIPSE_VERSION=4.8.0`` +* To override the base installation directory use: ``CMAKE_INSTALL_PREFIX`` + ie. ``CC=clang cmake -DCMAKE_INSTALL_PREFIX=./out .. && make install`` + +### Additional Information: + +* [SocketCAN Documentation (Linux Kernel)](https://www.kernel.org/doc/Documentation/networking/can.txt) +* [Elinux.org CAN Bus Page](http://elinux.org/CAN_Bus) +* [Debian Package Description](https://packages.debian.org/sid/can-utils) + diff --git a/Devices/Libraries/Systems/can-utils/asc2log.c b/Devices/Libraries/Systems/can-utils/asc2log.c new file mode 100755 index 0000000..2860f9d --- /dev/null +++ b/Devices/Libraries/Systems/can-utils/asc2log.c @@ -0,0 +1,554 @@ +/* SPDX-License-Identifier: (GPL-2.0-only OR BSD-3-Clause) */ +/* + * asc2log.c - convert ASC logfile to compact CAN frame logfile + * + * Copyright (c) 2002-2007 Volkswagen Group Electronic Research + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * 3. Neither the name of Volkswagen nor the names of its contributors + * may be used to endorse or promote products derived from this software + * without specific prior written permission. + * + * Alternatively, provided that this notice is retained in full, this + * software may be distributed under the terms of the GNU General + * Public License ("GPL") version 2, in which case the provisions of the + * GPL apply INSTEAD OF those given above. + * + * The provided data structures and external interfaces from this code + * are not restricted to be used by modules with a GPL compatible license. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT + * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY + * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH + * DAMAGE. + * + * Send feedback to + * + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "lib.h" + +#define BUFLEN 400 /* CAN FD mode lines can be pretty long */ + +extern int optind, opterr, optopt; + +static void print_usage(char *prg) +{ + fprintf(stderr, "%s - convert ASC logfile to compact CAN frame logfile.\n", prg); + fprintf(stderr, "Usage: %s\n", prg); + fprintf(stderr, "Options:\n"); + fprintf(stderr, "\t-I \t(default stdin)\n"); + fprintf(stderr, "\t-O \t(default stdout)\n"); +} + +static void prframe(FILE *file, struct timeval *tv, int dev, + struct canfd_frame *cf, char *extra_info) +{ + static char abuf[BUFLEN]; + + fprintf(file, "(%llu.%06llu) ", (unsigned long long)tv->tv_sec, (unsigned long long)tv->tv_usec); + + if (dev > 0) + fprintf(file, "can%d ", dev-1); + else + fprintf(file, "canX "); + + snprintf_canframe(abuf, sizeof(abuf), (cu_t *)cf, 0); + fprintf(file, "%s%s", abuf, extra_info); +} + +static void get_can_id(struct canfd_frame *cf, char *idstring, int base) +{ + if (idstring[strlen(idstring)-1] == 'x') { + cf->can_id = CAN_EFF_FLAG; + idstring[strlen(idstring)-1] = 0; + } else + cf->can_id = 0; + + cf->can_id |= strtoul(idstring, NULL, base); +} + +static void calc_tv(struct timeval *tv, struct timeval *read_tv, + struct timeval *date_tv, char timestamps, int dplace) +{ + if (dplace == 4) /* shift values having only 4 decimal places */ + read_tv->tv_usec *= 100; /* and need for 6 */ + + if (dplace == 5) /* shift values having only 5 decimal places */ + read_tv->tv_usec *= 10; /* and need for 6 */ + + if (timestamps == 'a') { /* absolute */ + + tv->tv_sec = date_tv->tv_sec + read_tv->tv_sec; + tv->tv_usec = date_tv->tv_usec + read_tv->tv_usec; + + } else { /* relative */ + + if (((!tv->tv_sec) && (!tv->tv_usec)) && + (date_tv->tv_sec || date_tv->tv_usec)) { + tv->tv_sec = date_tv->tv_sec; /* initial date/time */ + tv->tv_usec = date_tv->tv_usec; + } + + tv->tv_sec += read_tv->tv_sec; + tv->tv_usec += read_tv->tv_usec; + } + + if (tv->tv_usec >= 1000000) { + tv->tv_usec -= 1000000; + tv->tv_sec++; + } +} + +static void eval_can(char* buf, struct timeval *date_tvp, char timestamps, + char base, int dplace, FILE *outfile) +{ + int interface; + static struct timeval tv; /* current frame timestamp */ + static struct timeval read_tv; /* frame timestamp from ASC file */ + struct canfd_frame cf = { 0 }; + struct can_frame *ccf = (struct can_frame *)&cf; /* for len8_dlc */ + char rtr; + int dlc = 0; + int len = 0; + int data[8]; + char tmp1[BUFLEN]; + char dir[3]; /* 'Rx' or 'Tx' plus terminating zero */ + char *extra_info; + int i, items; + unsigned long long sec, usec; + + /* check for ErrorFrames */ + if (sscanf(buf, "%llu.%llu %d %s", + &sec, &usec, + &interface, tmp1) == 4) { + read_tv.tv_sec = sec; + read_tv.tv_usec = usec; + + if (!strncmp(tmp1, "ErrorFrame", strlen("ErrorFrame"))) { + + /* do not know more than 'Error' */ + cf.can_id = (CAN_ERR_FLAG | CAN_ERR_BUSERROR); + cf.len = CAN_ERR_DLC; + + calc_tv(&tv, &read_tv, date_tvp, timestamps, dplace); + prframe(outfile, &tv, interface, &cf, "\n"); + fflush(outfile); + return; + } + } + + /* 0.002367 1 390x Rx d 8 17 00 14 00 C0 00 08 00 */ + + /* check for CAN frames with (hexa)decimal values */ + if (base == 'h') + items = sscanf(buf, "%llu.%llu %d %s %2s %c %x %x %x %x %x %x %x %x %x", + &sec, &usec, &interface, + tmp1, dir, &rtr, &dlc, + &data[0], &data[1], &data[2], &data[3], + &data[4], &data[5], &data[6], &data[7]); + else + items = sscanf(buf, "%llu.%llu %d %s %2s %c %x %d %d %d %d %d %d %d %d", + &sec, &usec, &interface, + tmp1, dir, &rtr, &dlc, + &data[0], &data[1], &data[2], &data[3], + &data[4], &data[5], &data[6], &data[7]); + + read_tv.tv_sec = sec; + read_tv.tv_usec = usec; + if (items < 7 ) /* make sure we've read the dlc */ + return; + + /* dlc is one character hex value 0..F */ + if (dlc > CAN_MAX_RAW_DLC) + return; + + /* retrieve real data length */ + if (dlc > CAN_MAX_DLC) + len = CAN_MAX_DLEN; + else + len = dlc; + + if ((items == len + 7 ) || /* data frame */ + ((items == 6) && (rtr == 'r')) || /* RTR without DLC */ + ((items == 7) && (rtr == 'r'))) { /* RTR with DLC */ + + /* check for CAN ID with (hexa)decimal value */ + if (base == 'h') + get_can_id(&cf, tmp1, 16); + else + get_can_id(&cf, tmp1, 10); + + /* dlc > 8 => len == CAN_MAX_DLEN => fill len8_dlc value */ + if (dlc > CAN_MAX_DLC) + ccf->len8_dlc = dlc; + + if (strlen(dir) != 2) /* "Rx" or "Tx" */ + return; + + /* check for signed integer overflow */ + if (dplace == 4 && read_tv.tv_usec >= INT_MAX / 100) + return; + + if (dplace == 5 && read_tv.tv_usec >= INT_MAX / 10) + return; + + if (dir[0] == 'R') + extra_info = " R\n"; + else + extra_info = " T\n"; + + cf.len = len; + if (rtr == 'r') + cf.can_id |= CAN_RTR_FLAG; + else + for (i = 0; i < len; i++) + cf.data[i] = data[i] & 0xFFU; + + calc_tv(&tv, &read_tv, date_tvp, timestamps, dplace); + prframe(outfile, &tv, interface, &cf, extra_info); + fflush(outfile); + } +} + +static void eval_canfd(char* buf, struct timeval *date_tvp, char timestamps, + int dplace, FILE *outfile) +{ + int interface; + static struct timeval tv; /* current frame timestamp */ + static struct timeval read_tv; /* frame timestamp from ASC file */ + struct canfd_frame cf = { 0 }; + unsigned char brs, esi, ctmp; + unsigned int flags; + int dlc, dlen = 0; + char tmp1[BUFLEN]; + char dir[3]; /* 'Rx' or 'Tx' plus terminating zero */ + char *extra_info; + char *ptr; + int i; + unsigned long long sec, usec; + + /* The CANFD format is mainly in hex representation but + and probably some content we skip anyway. Don't trust the docs! */ + + /* 21.671796 CANFD 1 Tx 11 msgCanFdFr1 1 0 a 16 \ + 00 00 00 00 00 00 00 00 00 00 00 00 00 00 59 c0 \ + 100000 214 223040 80000000 46500250 460a0250 20011736 20010205 */ + + /* check for valid line without symbolic name */ + if (sscanf(buf, "%llu.%llu %*s %d %2s %s %hhx %hhx %x %d ", + &sec, &usec, &interface, + dir, tmp1, &brs, &esi, &dlc, &dlen) != 9) { + + /* check for valid line with a symbolic name */ + if (sscanf(buf, "%llu.%llu %*s %d %2s %s %*s %hhx %hhx %x %d ", + &sec, &usec, &interface, + dir, tmp1, &brs, &esi, &dlc, &dlen) != 9) { + + /* no valid CANFD format pattern */ + return; + } + } + read_tv.tv_sec = sec; + read_tv.tv_usec = usec; + + /* check for allowed (unsigned) value ranges */ + if ((dlen > CANFD_MAX_DLEN) || (dlc > CANFD_MAX_DLC) || + (brs > 1) || (esi > 1)) + return; + + if (strlen(dir) != 2) /* "Rx" or "Tx" */ + return; + + /* check for signed integer overflow */ + if (dplace == 4 && read_tv.tv_usec >= INT_MAX / 100) + return; + + /* check for signed integer overflow */ + if (dplace == 5 && read_tv.tv_usec >= INT_MAX / 10) + return; + + if (dir[0] == 'R') + extra_info = " R\n"; + else + extra_info = " T\n"; + + /* don't trust ASCII content - sanitize data length */ + if (dlen != can_fd_dlc2len(can_fd_len2dlc(dlen))) + return; + + get_can_id(&cf, tmp1, 16); + + /* now search for the beginning of the data[] content */ + sprintf(tmp1, " %x %x %x %2d ", brs, esi, dlc, dlen); + + /* search for the pattern generated by real data */ + ptr = strcasestr(buf, tmp1); + if (ptr == NULL) + return; + + ptr += strlen(tmp1); /* start of ASCII hex frame data */ + + cf.len = dlen; + + for (i = 0; i < dlen; i++) { + ctmp = asc2nibble(ptr[0]); + if (ctmp > 0x0F) + return; + + cf.data[i] = (ctmp << 4); + + ctmp = asc2nibble(ptr[1]); + if (ctmp > 0x0F) + return; + + cf.data[i] |= ctmp; + + ptr += 3; /* start of next ASCII hex byte */ + } + + /* skip MessageDuration and MessageLength to get Flags value */ + if (sscanf(ptr, " %*x %*x %x ", &flags) != 1) + return; + + /* relevant flags in Flags field */ +#define ASC_F_RTR 0x00000010 +#define ASC_F_FDF 0x00001000 +#define ASC_F_BRS 0x00002000 +#define ASC_F_ESI 0x00004000 + + if (flags & ASC_F_FDF) { + cf.flags = CANFD_FDF; + if (flags & ASC_F_BRS) + cf.flags |= CANFD_BRS; + if (flags & ASC_F_ESI) + cf.flags |= CANFD_ESI; + } else { + /* yes. The 'CANFD' format supports classic CAN content! */ + if (flags & ASC_F_RTR) { + cf.can_id |= CAN_RTR_FLAG; + /* dlen is always 0 for classic CAN RTR frames + but the DLC value is valid in RTR cases */ + cf.len = dlc; + /* sanitize payload length value */ + if (dlc > CAN_MAX_DLEN) + cf.len = CAN_MAX_DLEN; + } + /* check for extra DLC when having a Classic CAN with 8 bytes payload */ + if ((cf.len == CAN_MAX_DLEN) && (dlc > CAN_MAX_DLEN) && (dlc <= CAN_MAX_RAW_DLC)) { + struct can_frame *ccf = (struct can_frame *)&cf; + + ccf->len8_dlc = dlc; + } + } + + calc_tv(&tv, &read_tv, date_tvp, timestamps, dplace); + prframe(outfile, &tv, interface, &cf, extra_info); + fflush(outfile); + + /* No support for really strange CANFD ErrorFrames format m( */ +} + +static int get_date(struct timeval *tv, char *date) +{ + struct tm tms; + unsigned int msecs = 0; + + if ((strcasestr(date, " am ") != NULL) || (strcasestr(date, " pm ") != NULL)) { + /* assume EN/US date due to existing am/pm field */ + + if (!setlocale(LC_TIME, "en_US")) { + fprintf(stderr, "Setting locale to 'en_US' failed!\n"); + return 1; + } + + if (!strptime(date, "%B %d %I:%M:%S %p %Y", &tms)) { + /* The string might contain a milliseconds value which strptime() + does not support. So we read the ms value into the year variable + before parsing the real year value (hack) */ + if (!strptime(date, "%B %d %I:%M:%S.%Y %p %Y", &tms)) + return 1; + sscanf(date, "%*s %*d %*d:%*d:%*d.%3u ", &msecs); + } + + } else { + /* assume DE date due to non existing am/pm field */ + + if (!setlocale(LC_TIME, "de_DE")) { + fprintf(stderr, "Setting locale to 'de_DE' failed!\n"); + return 1; + } + + if (!strptime(date, "%B %d %H:%M:%S %Y", &tms)) { + /* The string might contain a milliseconds value which strptime() + does not support. So we read the ms value into the year variable + before parsing the real year value (hack) */ + if (!strptime(date, "%B %d %H:%M:%S.%Y %Y", &tms)) + return 1; + sscanf(date, "%*s %*d %*d:%*d:%*d.%3u ", &msecs); + } + } + + //printf("h %d m %d s %d ms %03d d %d m %d y %d\n", + //tms.tm_hour, tms.tm_min, tms.tm_sec, msecs, + //tms.tm_mday, tms.tm_mon+1, tms.tm_year+1900); + + tv->tv_sec = mktime(&tms); + tv->tv_usec = msecs * 1000; + + if (tv->tv_sec < 0) + return 1; + + return 0; +} + +int main(int argc, char **argv) +{ + char buf[BUFLEN], tmp1[BUFLEN], tmp2[BUFLEN]; + + FILE *infile = stdin; + FILE *outfile = stdout; + static int verbose; + static struct timeval date_tv; /* date of the ASC file */ + static int dplace; /* decimal place 4, 5 or 6 or uninitialized */ + static char base; /* 'd'ec or 'h'ex */ + static char timestamps; /* 'a'bsolute or 'r'elative */ + int opt; + unsigned long long sec, usec; + + while ((opt = getopt(argc, argv, "I:O:v?")) != -1) { + switch (opt) { + case 'I': + infile = fopen(optarg, "r"); + if (!infile) { + perror("infile"); + return 1; + } + break; + + case 'O': + outfile = fopen(optarg, "w"); + if (!outfile) { + perror("outfile"); + return 1; + } + break; + + case 'v': + verbose = 1; + break; + + case '?': + print_usage(basename(argv[0])); + return 0; + break; + + default: + fprintf(stderr, "Unknown option %c\n", opt); + print_usage(basename(argv[0])); + return 1; + break; + } + } + + + while (fgets(buf, BUFLEN-1, infile)) { + + if (!dplace) { /* the representation of a valid CAN frame not known */ + + /* check for base and timestamp entries in the header */ + if ((!base) && + (sscanf(buf, "base %s timestamps %s", tmp1, tmp2) == 2)) { + base = tmp1[0]; + timestamps = tmp2[0]; + if (verbose) + printf("base %c timestamps %c\n", base, timestamps); + if ((base != 'h') && (base != 'd')) { + printf("invalid base %s (must be 'hex' or 'dez')!\n", + tmp1); + return 1; + } + if ((timestamps != 'a') && (timestamps != 'r')) { + printf("invalid timestamps %s (must be 'absolute'" + " or 'relative')!\n", tmp2); + return 1; + } + continue; + } + + /* check for the original logging date in the header */ + if ((!date_tv.tv_sec) && + (!strncmp(buf, "date", 4))) { + + if (get_date(&date_tv, &buf[9])) { /* skip 'date day ' */ + fprintf(stderr, "Not able to determine original log " + "file date. Using current time.\n"); + /* use current date as default */ + gettimeofday(&date_tv, NULL); + } + if (verbose) + printf("date %llu => %s", (unsigned long long)date_tv.tv_sec, ctime(&date_tv.tv_sec)); + continue; + } + + /* check for decimal places length in valid CAN frames */ + if (sscanf(buf, "%llu.%s %s ", &sec, tmp2, + tmp1) != 3) + continue; /* dplace remains zero until first found CAN frame */ + + dplace = strlen(tmp2); + if (verbose) + printf("decimal place %d, e.g. '%s'\n", dplace, + tmp2); + if (dplace < 4 || dplace > 6) { + printf("invalid dplace %d (must be 4, 5 or 6)!\n", + dplace); + return 1; + } + } + + /* the representation of a valid CAN frame is known here */ + /* so try to get CAN frames and ErrorFrames and convert them */ + + /* check classic CAN format or the CANFD tag which can take both types */ + if (sscanf(buf, "%llu.%llu %s ", &sec, &usec, tmp1) == 3){ + if (!strncmp(tmp1, "CANFD", 5)) + eval_canfd(buf, &date_tv, timestamps, dplace, outfile); + else + eval_can(buf, &date_tv, timestamps, base, dplace, outfile); + } + } + fclose(outfile); + fclose(infile); + return 0; +} diff --git a/Devices/Libraries/Systems/can-utils/bcmserver.c b/Devices/Libraries/Systems/can-utils/bcmserver.c new file mode 100755 index 0000000..34ba6c0 --- /dev/null +++ b/Devices/Libraries/Systems/can-utils/bcmserver.c @@ -0,0 +1,369 @@ +/* SPDX-License-Identifier: (GPL-2.0-only OR BSD-3-Clause) */ +/* + * tst-bcm-server.c + * + * Test program that implements a socket server which understands ASCII + * messages for simple broadcast manager frame send commands. + * + * < interface command ival_s ival_us can_id can_dlc [data]* > + * + * Only the items 'can_id' and 'data' are given in (ASCII) hexadecimal values. + * + * ## TX path: + * + * The commands are 'A'dd, 'U'pdate, 'D'elete and 'S'end. + * e.g. + * + * Send the CAN frame 123#1122334455667788 every second on vcan1 + * < vcan1 A 1 0 123 8 11 22 33 44 55 66 77 88 > + * + * Send the CAN frame 123#1122334455667788 every 10 usecs on vcan1 + * < vcan1 A 0 10 123 8 11 22 33 44 55 66 77 88 > + * + * Send the CAN frame 123#42424242 every 20 msecs on vcan1 + * < vcan1 A 0 20000 123 4 42 42 42 42 > + * + * Update the CAN frame 123#42424242 with 123#112233 - no change of timers + * < vcan1 U 0 0 123 3 11 22 33 > + * + * Delete the cyclic send job from above + * < vcan1 D 0 0 123 0 > + * + * Send a single CAN frame without cyclic transmission + * < can0 S 0 0 123 0 > + * + * When the socket is closed the cyclic transmissions are terminated. + * + * ## RX path: + * + * The commands are 'R'eceive setup, 'F'ilter ID Setup and 'X' for delete. + * e.g. + * + * Receive CAN ID 0x123 from vcan1 and check for changes in the first byte + * < vcan1 R 0 0 123 1 FF > + * + * Receive CAN ID 0x123 from vcan1 and check for changes in given mask + * < vcan1 R 0 0 123 8 FF 00 F8 00 00 00 00 00 > + * + * As above but throttle receive update rate down to 1.5 seconds + * < vcan1 R 1 500000 123 8 FF 00 F8 00 00 00 00 00 > + * + * Filter for CAN ID 0x123 from vcan1 without content filtering + * < vcan1 F 0 0 123 0 > + * + * Delete receive filter ('R' or 'F') for CAN ID 0x123 + * < vcan1 X 0 0 123 0 > + * + * CAN messages received by the given filters are send in the format: + * < interface can_id can_dlc [data]* > + * + * e.g. when receiving a CAN message from vcan1 with + * can_id 0x123 , data length 4 and data 0x11, 0x22, 0x33 and 0x44 + * + * < vcan1 123 4 11 22 33 44 > + * + * ## + * + * Authors: + * Andre Naujoks (the socket server stuff) + * Oliver Hartkopp (the rest) + * + * Copyright (c) 2002-2009 Volkswagen Group Electronic Research + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * 3. Neither the name of Volkswagen nor the names of its contributors + * may be used to endorse or promote products derived from this software + * without specific prior written permission. + * + * Alternatively, provided that this notice is retained in full, this + * software may be distributed under the terms of the GNU General + * Public License ("GPL") version 2, in which case the provisions of the + * GPL apply INSTEAD OF those given above. + * + * The provided data structures and external interfaces from this code + * are not restricted to be used by modules with a GPL compatible license. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT + * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY + * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH + * DAMAGE. + * + * Send feedback to + * + */ + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#define MAXLEN 100 +#define FORMATSZ 80 +#define PORT 28600 + +void childdied(int i) +{ + wait(NULL); +} + +int main(void) +{ + + int sl, sa, sc; + int i; + int idx = 0; + struct sockaddr_in saddr, clientaddr; + struct sockaddr_can caddr; + socklen_t caddrlen = sizeof(caddr); + struct ifreq ifr; + fd_set readfds; + socklen_t sin_size = sizeof(clientaddr); + struct sigaction signalaction; + sigset_t sigset; + + char buf[MAXLEN]; + char format[FORMATSZ]; + char rxmsg[50]; + +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wpragmas" +#pragma GCC diagnostic ignored "-Wgnu-variable-sized-type-not-at-end" + struct { + struct bcm_msg_head msg_head; + struct can_frame frame; + } msg; +#pragma GCC diagnostic pop + + if (snprintf(format, FORMATSZ, "< %%%ds %%c %%lu %%lu %%x %%hhu " + "%%hhx %%hhx %%hhx %%hhx %%hhx %%hhx " + "%%hhx %%hhx >", IFNAMSIZ-1) >= FORMATSZ-1) + exit(1); + + sigemptyset(&sigset); + signalaction.sa_handler = &childdied; + signalaction.sa_mask = sigset; + signalaction.sa_flags = 0; + sigaction(SIGCHLD, &signalaction, NULL); /* signal for dying child */ + + if((sl = socket(PF_INET, SOCK_STREAM, 0)) < 0) { + perror("inetsocket"); + exit(1); + } + + saddr.sin_family = AF_INET; + saddr.sin_addr.s_addr = htonl(INADDR_ANY); + saddr.sin_port = htons(PORT); + + while(bind(sl,(struct sockaddr*)&saddr, sizeof(saddr)) < 0) { + struct timespec f = { + .tv_nsec = 100 * 1000 * 1000, + }; + + printf(".");fflush(NULL); + nanosleep(&f, NULL); + } + + if (listen(sl,3) != 0) { + perror("listen"); + exit(1); + } + + while (1) { + sa = accept(sl,(struct sockaddr *)&clientaddr, &sin_size); + if (sa > 0 ){ + if (!fork()) + break; + close(sa); + } + else { + if (errno != EINTR) { + /* + * If the cause for the error was NOT the + * signal from a dying child => give an error + */ + perror("accept"); + exit(1); + } + } + } + + /* open BCM socket */ + + if ((sc = socket(PF_CAN, SOCK_DGRAM, CAN_BCM)) < 0) { + perror("bcmsocket"); + return 1; + } + + memset(&caddr, 0, sizeof(caddr)); + caddr.can_family = PF_CAN; + /* can_ifindex is set to 0 (any device) => need for sendto() */ + + if (connect(sc, (struct sockaddr *)&caddr, sizeof(caddr)) < 0) { + perror("connect"); + return 1; + } + + while (1) { + + FD_ZERO(&readfds); + FD_SET(sc, &readfds); + FD_SET(sa, &readfds); + + select((sc > sa)?sc+1:sa+1, &readfds, NULL, NULL, NULL); + + if (FD_ISSET(sc, &readfds)) { + + recvfrom(sc, &msg, sizeof(msg), 0, + (struct sockaddr*)&caddr, &caddrlen); + + ifr.ifr_ifindex = caddr.can_ifindex; + ioctl(sc, SIOCGIFNAME, &ifr); + + sprintf(rxmsg, "< %s %03X %d ", ifr.ifr_name, + msg.msg_head.can_id, msg.frame.can_dlc); + + for ( i = 0; i < msg.frame.can_dlc; i++) + sprintf(rxmsg + strlen(rxmsg), "%02X ", + msg.frame.data[i]); + + /* delimiter '\0' for Adobe(TM) Flash(TM) XML sockets */ + strcat(rxmsg, ">\0"); + + send(sa, rxmsg, strlen(rxmsg) + 1, 0); + } + + + if (FD_ISSET(sa, &readfds)) { + + char cmd; + int items; + + if (read(sa, buf+idx, 1) < 1) + exit(1); + + if (!idx) { + if (buf[0] == '<') + idx = 1; + + continue; + } + + if (idx > MAXLEN-2) { + idx = 0; + continue; + } + + if (buf[idx] != '>') { + idx++; + continue; + } + + buf[idx+1] = 0; + idx = 0; + + //printf("read '%s'\n", buf); + + /* prepare bcm message settings */ + memset(&msg, 0, sizeof(msg)); + msg.msg_head.nframes = 1; + + items = sscanf(buf, format, + ifr.ifr_name, + &cmd, + &msg.msg_head.ival2.tv_sec, + &msg.msg_head.ival2.tv_usec, + &msg.msg_head.can_id, + &msg.frame.can_dlc, + &msg.frame.data[0], + &msg.frame.data[1], + &msg.frame.data[2], + &msg.frame.data[3], + &msg.frame.data[4], + &msg.frame.data[5], + &msg.frame.data[6], + &msg.frame.data[7]); + + if (items < 6) + break; + if (msg.frame.can_dlc > 8) + break; + if (items != 6 + msg.frame.can_dlc) + break; + + msg.frame.can_id = msg.msg_head.can_id; + + switch (cmd) { + case 'S': + msg.msg_head.opcode = TX_SEND; + break; + case 'A': + msg.msg_head.opcode = TX_SETUP; + msg.msg_head.flags |= SETTIMER | STARTTIMER; + break; + case 'U': + msg.msg_head.opcode = TX_SETUP; + msg.msg_head.flags = 0; + break; + case 'D': + msg.msg_head.opcode = TX_DELETE; + break; + + case 'R': + msg.msg_head.opcode = RX_SETUP; + msg.msg_head.flags = SETTIMER; + break; + case 'F': + msg.msg_head.opcode = RX_SETUP; + msg.msg_head.flags = RX_FILTER_ID | SETTIMER; + break; + case 'X': + msg.msg_head.opcode = RX_DELETE; + break; + default: + printf("unknown command '%c'.\n", cmd); + exit(1); + } + + if (!ioctl(sc, SIOCGIFINDEX, &ifr)) { + caddr.can_ifindex = ifr.ifr_ifindex; + sendto(sc, &msg, sizeof(msg), 0, + (struct sockaddr*)&caddr, sizeof(caddr)); + } + } + } + + close(sc); + close(sa); + + return 0; +} diff --git a/Devices/Libraries/Systems/can-utils/calc-bit-timing/can-calc-bit-timing-v2_6_31.c b/Devices/Libraries/Systems/can-utils/calc-bit-timing/can-calc-bit-timing-v2_6_31.c new file mode 100755 index 0000000..24e912c --- /dev/null +++ b/Devices/Libraries/Systems/can-utils/calc-bit-timing/can-calc-bit-timing-v2_6_31.c @@ -0,0 +1,192 @@ +/* SPDX-License-Identifier: GPL-2.0-only */ + +#include "compat.h" + +/* + * imported from v2.6.31-rc1~330^2~376 + * + * 39549eef3587 can: CAN Network device driver and Netlink interface + * + * cherry-picked for easier integration: + * 61463a30f652 can: make function can_get_bittiming static + * aabdfd6adb80 can: replace the dev_dbg/info/err/... with the new netdev_xxx macros + * 08da7da41ea4 can: provide a separate bittiming_const parameter to bittiming functions + * b25a437206ed can: dev: remove unused variable from can_calc_bittiming() function +*/ + +/* + * Copyright (C) 2005 Marc Kleine-Budde, Pengutronix + * Copyright (C) 2006 Andrey Volkov, Varma Electronics + * Copyright (C) 2008-2009 Wolfgang Grandegger + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the version 2 of the GNU General Public License + * as published by the Free Software Foundation + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +/* + * Bit-timing calculation derived from: + * + * Code based on LinCAN sources and H8S2638 project + * Copyright 2004-2006 Pavel Pisa - DCE FELK CVUT cz + * Copyright 2005 Stanislav Marek + * email: pisa@cmp.felk.cvut.cz + * + * Calculates proper bit-timing parameters for a specified bit-rate + * and sample-point, which can then be used to set the bit-timing + * registers of the CAN controller. You can find more information + * in the header file linux/can/netlink.h. + */ +static int can_update_spt(const struct can_bittiming_const *btc, + int sampl_pt, int tseg, int *tseg1, int *tseg2) +{ + *tseg2 = tseg + 1 - (sampl_pt * (tseg + 1)) / 1000; + if (*tseg2 < btc->tseg2_min) + *tseg2 = btc->tseg2_min; + if (*tseg2 > btc->tseg2_max) + *tseg2 = btc->tseg2_max; + *tseg1 = tseg - *tseg2; + if (*tseg1 > btc->tseg1_max) { + *tseg1 = btc->tseg1_max; + *tseg2 = tseg - *tseg1; + } + return 1000 * (tseg + 1 - *tseg2) / (tseg + 1); +} + +static int can_calc_bittiming(struct net_device *dev, struct can_bittiming *bt, + const struct can_bittiming_const *btc) +{ + struct can_priv *priv = netdev_priv(dev); + long best_error = 1000000000, error = 0; + int best_tseg = 0, best_brp = 0, brp = 0; + int tsegall, tseg = 0, tseg1 = 0, tseg2 = 0; + int spt_error = 1000, spt = 0, sampl_pt; + long rate; + u64 v64; + + /* Use CIA recommended sample points */ + if (bt->sample_point) { + sampl_pt = bt->sample_point; + } else { + if (bt->bitrate > 800000) + sampl_pt = 750; + else if (bt->bitrate > 500000) + sampl_pt = 800; + else + sampl_pt = 875; + } + + /* tseg even = round down, odd = round up */ + for (tseg = (btc->tseg1_max + btc->tseg2_max) * 2 + 1; + tseg >= (btc->tseg1_min + btc->tseg2_min) * 2; tseg--) { + tsegall = 1 + tseg / 2; + /* Compute all possible tseg choices (tseg=tseg1+tseg2) */ + brp = priv->clock.freq / (tsegall * bt->bitrate) + tseg % 2; + /* chose brp step which is possible in system */ + brp = (brp / btc->brp_inc) * btc->brp_inc; + if ((brp < btc->brp_min) || (brp > btc->brp_max)) + continue; + rate = priv->clock.freq / (brp * tsegall); + error = bt->bitrate - rate; + /* tseg brp biterror */ + if (error < 0) + error = -error; + if (error > best_error) + continue; + best_error = error; + if (error == 0) { + spt = can_update_spt(btc, sampl_pt, tseg / 2, + &tseg1, &tseg2); + error = sampl_pt - spt; + if (error < 0) + error = -error; + if (error > spt_error) + continue; + spt_error = error; + } + best_tseg = tseg / 2; + best_brp = brp; + if (error == 0) + break; + } + + if (best_error) { + /* Error in one-tenth of a percent */ + error = (best_error * 1000) / bt->bitrate; + if (error > CAN_CALC_MAX_ERROR) { + netdev_err(dev, + "bitrate error %ld.%ld%% too high\n", + error / 10, error % 10); + return -EDOM; + } else { + netdev_warn(dev, "bitrate error %ld.%ld%%\n", + error / 10, error % 10); + } + } + + /* real sample point */ + bt->sample_point = can_update_spt(btc, sampl_pt, best_tseg, + &tseg1, &tseg2); + + v64 = (u64)best_brp * 1000000000UL; + do_div(v64, priv->clock.freq); + bt->tq = (u32)v64; + bt->prop_seg = tseg1 / 2; + bt->phase_seg1 = tseg1 - bt->prop_seg; + bt->phase_seg2 = tseg2; + bt->sjw = 1; + bt->brp = best_brp; + /* real bit-rate */ + bt->bitrate = priv->clock.freq / (bt->brp * (tseg1 + tseg2 + 1)); + + return 0; +} + +/* + * Checks the validity of the specified bit-timing parameters prop_seg, + * phase_seg1, phase_seg2 and sjw and tries to determine the bitrate + * prescaler value brp. You can find more information in the header + * file linux/can/netlink.h. + */ +static int can_fixup_bittiming(struct net_device *dev, struct can_bittiming *bt, + const struct can_bittiming_const *btc) +{ + struct can_priv *priv = netdev_priv(dev); + int tseg1, alltseg; + u64 brp64; + + tseg1 = bt->prop_seg + bt->phase_seg1; + if (!bt->sjw) + bt->sjw = 1; + if (bt->sjw > btc->sjw_max || + tseg1 < btc->tseg1_min || tseg1 > btc->tseg1_max || + bt->phase_seg2 < btc->tseg2_min || bt->phase_seg2 > btc->tseg2_max) + return -ERANGE; + + brp64 = (u64)priv->clock.freq * (u64)bt->tq; + if (btc->brp_inc > 1) + do_div(brp64, btc->brp_inc); + brp64 += 500000000UL - 1; + do_div(brp64, 1000000000UL); /* the practicable BRP */ + if (btc->brp_inc > 1) + brp64 *= btc->brp_inc; + bt->brp = (u32)brp64; + + if (bt->brp < btc->brp_min || bt->brp > btc->brp_max) + return -EINVAL; + + alltseg = bt->prop_seg + bt->phase_seg1 + bt->phase_seg2 + 1; + bt->bitrate = priv->clock.freq / (bt->brp * alltseg); + bt->sample_point = ((tseg1 + 1) * 1000) / alltseg; + + return 0; +} diff --git a/Devices/Libraries/Systems/can-utils/calc-bit-timing/can-calc-bit-timing-v3_18.c b/Devices/Libraries/Systems/can-utils/calc-bit-timing/can-calc-bit-timing-v3_18.c new file mode 100755 index 0000000..be34f39 --- /dev/null +++ b/Devices/Libraries/Systems/can-utils/calc-bit-timing/can-calc-bit-timing-v3_18.c @@ -0,0 +1,196 @@ +/* SPDX-License-Identifier: GPL-2.0-only */ + +#include "compat.h" + +/* + * imported from v3.18-rc1~52^2~248^2~1 + * + */ + +/* + * Copyright (C) 2005 Marc Kleine-Budde, Pengutronix + * Copyright (C) 2006 Andrey Volkov, Varma Electronics + * Copyright (C) 2008-2009 Wolfgang Grandegger + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the version 2 of the GNU General Public License + * as published by the Free Software Foundation + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, see . + */ + +/* + * Bit-timing calculation derived from: + * + * Code based on LinCAN sources and H8S2638 project + * Copyright 2004-2006 Pavel Pisa - DCE FELK CVUT cz + * Copyright 2005 Stanislav Marek + * email: pisa@cmp.felk.cvut.cz + * + * Calculates proper bit-timing parameters for a specified bit-rate + * and sample-point, which can then be used to set the bit-timing + * registers of the CAN controller. You can find more information + * in the header file linux/can/netlink.h. + */ +static int can_update_spt(const struct can_bittiming_const *btc, + int sampl_pt, int tseg, int *tseg1, int *tseg2) +{ + *tseg2 = tseg + 1 - (sampl_pt * (tseg + 1)) / 1000; + if (*tseg2 < btc->tseg2_min) + *tseg2 = btc->tseg2_min; + if (*tseg2 > btc->tseg2_max) + *tseg2 = btc->tseg2_max; + *tseg1 = tseg - *tseg2; + if (*tseg1 > btc->tseg1_max) { + *tseg1 = btc->tseg1_max; + *tseg2 = tseg - *tseg1; + } + return 1000 * (tseg + 1 - *tseg2) / (tseg + 1); +} + +static int can_calc_bittiming(struct net_device *dev, struct can_bittiming *bt, + const struct can_bittiming_const *btc) +{ + struct can_priv *priv = netdev_priv(dev); + long best_error = 1000000000, error = 0; + int best_tseg = 0, best_brp = 0, brp = 0; + int tsegall, tseg = 0, tseg1 = 0, tseg2 = 0; + int spt_error = 1000, spt = 0, sampl_pt; + long rate; + u64 v64; + + /* Use CIA recommended sample points */ + if (bt->sample_point) { + sampl_pt = bt->sample_point; + } else { + if (bt->bitrate > 800000) + sampl_pt = 750; + else if (bt->bitrate > 500000) + sampl_pt = 800; + else + sampl_pt = 875; + } + + /* tseg even = round down, odd = round up */ + for (tseg = (btc->tseg1_max + btc->tseg2_max) * 2 + 1; + tseg >= (btc->tseg1_min + btc->tseg2_min) * 2; tseg--) { + tsegall = 1 + tseg / 2; + /* Compute all possible tseg choices (tseg=tseg1+tseg2) */ + brp = priv->clock.freq / (tsegall * bt->bitrate) + tseg % 2; + /* chose brp step which is possible in system */ + brp = (brp / btc->brp_inc) * btc->brp_inc; + if ((brp < btc->brp_min) || (brp > btc->brp_max)) + continue; + rate = priv->clock.freq / (brp * tsegall); + error = bt->bitrate - rate; + /* tseg brp biterror */ + if (error < 0) + error = -error; + if (error > best_error) + continue; + best_error = error; + if (error == 0) { + spt = can_update_spt(btc, sampl_pt, tseg / 2, + &tseg1, &tseg2); + error = sampl_pt - spt; + if (error < 0) + error = -error; + if (error > spt_error) + continue; + spt_error = error; + } + best_tseg = tseg / 2; + best_brp = brp; + if (error == 0) + break; + } + + if (best_error) { + /* Error in one-tenth of a percent */ + error = (best_error * 1000) / bt->bitrate; + if (error > CAN_CALC_MAX_ERROR) { + netdev_err(dev, + "bitrate error %ld.%ld%% too high\n", + error / 10, error % 10); + return -EDOM; + } else { + netdev_warn(dev, "bitrate error %ld.%ld%%\n", + error / 10, error % 10); + } + } + + /* real sample point */ + bt->sample_point = can_update_spt(btc, sampl_pt, best_tseg, + &tseg1, &tseg2); + + v64 = (u64)best_brp * 1000000000UL; + do_div(v64, priv->clock.freq); + bt->tq = (u32)v64; + bt->prop_seg = tseg1 / 2; + bt->phase_seg1 = tseg1 - bt->prop_seg; + bt->phase_seg2 = tseg2; + + /* check for sjw user settings */ + if (!bt->sjw || !btc->sjw_max) + bt->sjw = 1; + else { + /* bt->sjw is at least 1 -> sanitize upper bound to sjw_max */ + if (bt->sjw > btc->sjw_max) + bt->sjw = btc->sjw_max; + /* bt->sjw must not be higher than tseg2 */ + if (tseg2 < bt->sjw) + bt->sjw = tseg2; + } + + bt->brp = best_brp; + /* real bit-rate */ + bt->bitrate = priv->clock.freq / (bt->brp * (tseg1 + tseg2 + 1)); + + return 0; +} + +/* + * Checks the validity of the specified bit-timing parameters prop_seg, + * phase_seg1, phase_seg2 and sjw and tries to determine the bitrate + * prescaler value brp. You can find more information in the header + * file linux/can/netlink.h. + */ +static int can_fixup_bittiming(struct net_device *dev, struct can_bittiming *bt, + const struct can_bittiming_const *btc) +{ + struct can_priv *priv = netdev_priv(dev); + int tseg1, alltseg; + u64 brp64; + + tseg1 = bt->prop_seg + bt->phase_seg1; + if (!bt->sjw) + bt->sjw = 1; + if (bt->sjw > btc->sjw_max || + tseg1 < btc->tseg1_min || tseg1 > btc->tseg1_max || + bt->phase_seg2 < btc->tseg2_min || bt->phase_seg2 > btc->tseg2_max) + return -ERANGE; + + brp64 = (u64)priv->clock.freq * (u64)bt->tq; + if (btc->brp_inc > 1) + do_div(brp64, btc->brp_inc); + brp64 += 500000000UL - 1; + do_div(brp64, 1000000000UL); /* the practicable BRP */ + if (btc->brp_inc > 1) + brp64 *= btc->brp_inc; + bt->brp = (u32)brp64; + + if (bt->brp < btc->brp_min || bt->brp > btc->brp_max) + return -EINVAL; + + alltseg = bt->prop_seg + bt->phase_seg1 + bt->phase_seg2 + 1; + bt->bitrate = priv->clock.freq / (bt->brp * alltseg); + bt->sample_point = ((tseg1 + 1) * 1000) / alltseg; + + return 0; +} diff --git a/Devices/Libraries/Systems/can-utils/calc-bit-timing/can-calc-bit-timing-v4_8.c b/Devices/Libraries/Systems/can-utils/calc-bit-timing/can-calc-bit-timing-v4_8.c new file mode 100755 index 0000000..05532e7 --- /dev/null +++ b/Devices/Libraries/Systems/can-utils/calc-bit-timing/can-calc-bit-timing-v4_8.c @@ -0,0 +1,225 @@ +/* SPDX-License-Identifier: GPL-2.0-only */ + +#include "compat.h" + +/* + * imported from v4.8-rc1~140^2~304^2~11 + * + */ + +/* + * Copyright (C) 2005 Marc Kleine-Budde, Pengutronix + * Copyright (C) 2006 Andrey Volkov, Varma Electronics + * Copyright (C) 2008-2009 Wolfgang Grandegger + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the version 2 of the GNU General Public License + * as published by the Free Software Foundation + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, see . + */ + +/* + * Bit-timing calculation derived from: + * + * Code based on LinCAN sources and H8S2638 project + * Copyright 2004-2006 Pavel Pisa - DCE FELK CVUT cz + * Copyright 2005 Stanislav Marek + * email: pisa@cmp.felk.cvut.cz + * + * Calculates proper bit-timing parameters for a specified bit-rate + * and sample-point, which can then be used to set the bit-timing + * registers of the CAN controller. You can find more information + * in the header file linux/can/netlink.h. + */ +static int can_update_sample_point(const struct can_bittiming_const *btc, + unsigned int sample_point_nominal, unsigned int tseg, + unsigned int *tseg1_ptr, unsigned int *tseg2_ptr, + unsigned int *sample_point_error_ptr) +{ + unsigned int sample_point_error, best_sample_point_error = UINT_MAX; + unsigned int sample_point, best_sample_point = 0; + unsigned int tseg1, tseg2; + int i; + + for (i = 0; i <= 1; i++) { + tseg2 = tseg + CAN_CALC_SYNC_SEG - (sample_point_nominal * (tseg + CAN_CALC_SYNC_SEG)) / 1000 - i; + tseg2 = clamp(tseg2, btc->tseg2_min, btc->tseg2_max); + tseg1 = tseg - tseg2; + if (tseg1 > btc->tseg1_max) { + tseg1 = btc->tseg1_max; + tseg2 = tseg - tseg1; + } + + sample_point = 1000 * (tseg + CAN_CALC_SYNC_SEG - tseg2) / (tseg + CAN_CALC_SYNC_SEG); + sample_point_error = abs(sample_point_nominal - sample_point); + + if ((sample_point <= sample_point_nominal) && (sample_point_error < best_sample_point_error)) { + best_sample_point = sample_point; + best_sample_point_error = sample_point_error; + *tseg1_ptr = tseg1; + *tseg2_ptr = tseg2; + } + } + + if (sample_point_error_ptr) + *sample_point_error_ptr = best_sample_point_error; + + return best_sample_point; +} + +static int can_calc_bittiming(struct net_device *dev, struct can_bittiming *bt, + const struct can_bittiming_const *btc) +{ + struct can_priv *priv = netdev_priv(dev); + unsigned int bitrate; /* current bitrate */ + unsigned int bitrate_error; /* difference between current and nominal value */ + unsigned int best_bitrate_error = UINT_MAX; + unsigned int sample_point_error; /* difference between current and nominal value */ + unsigned int best_sample_point_error = UINT_MAX; + unsigned int sample_point_nominal; /* nominal sample point */ + unsigned int best_tseg = 0; /* current best value for tseg */ + unsigned int best_brp = 0; /* current best value for brp */ + unsigned int brp, tsegall, tseg, tseg1 = 0, tseg2 = 0; + u64 v64; + + /* Use CiA recommended sample points */ + if (bt->sample_point) { + sample_point_nominal = bt->sample_point; + } else { + if (bt->bitrate > 800000) + sample_point_nominal = 750; + else if (bt->bitrate > 500000) + sample_point_nominal = 800; + else + sample_point_nominal = 875; + } + + /* tseg even = round down, odd = round up */ + for (tseg = (btc->tseg1_max + btc->tseg2_max) * 2 + 1; + tseg >= (btc->tseg1_min + btc->tseg2_min) * 2; tseg--) { + tsegall = CAN_CALC_SYNC_SEG + tseg / 2; + + /* Compute all possible tseg choices (tseg=tseg1+tseg2) */ + brp = priv->clock.freq / (tsegall * bt->bitrate) + tseg % 2; + + /* choose brp step which is possible in system */ + brp = (brp / btc->brp_inc) * btc->brp_inc; + if ((brp < btc->brp_min) || (brp > btc->brp_max)) + continue; + + bitrate = priv->clock.freq / (brp * tsegall); + bitrate_error = abs(bt->bitrate - bitrate); + + /* tseg brp biterror */ + if (bitrate_error > best_bitrate_error) + continue; + + /* reset sample point error if we have a better bitrate */ + if (bitrate_error < best_bitrate_error) + best_sample_point_error = UINT_MAX; + + can_update_sample_point(btc, sample_point_nominal, tseg / 2, &tseg1, &tseg2, &sample_point_error); + if (sample_point_error > best_sample_point_error) + continue; + + best_sample_point_error = sample_point_error; + best_bitrate_error = bitrate_error; + best_tseg = tseg / 2; + best_brp = brp; + + if (bitrate_error == 0 && sample_point_error == 0) + break; + } + + if (best_bitrate_error) { + /* Error in one-tenth of a percent */ + v64 = (u64)best_bitrate_error * 1000; + do_div(v64, bt->bitrate); + bitrate_error = (u32)v64; + if (bitrate_error > CAN_CALC_MAX_ERROR) { + netdev_err(dev, + "bitrate error %d.%d%% too high\n", + bitrate_error / 10, bitrate_error % 10); + return -EDOM; + } + netdev_warn(dev, "bitrate error %d.%d%%\n", + bitrate_error / 10, bitrate_error % 10); + } + + /* real sample point */ + bt->sample_point = can_update_sample_point(btc, sample_point_nominal, best_tseg, + &tseg1, &tseg2, NULL); + + v64 = (u64)best_brp * 1000 * 1000 * 1000; + do_div(v64, priv->clock.freq); + bt->tq = (u32)v64; + bt->prop_seg = tseg1 / 2; + bt->phase_seg1 = tseg1 - bt->prop_seg; + bt->phase_seg2 = tseg2; + + /* check for sjw user settings */ + if (!bt->sjw || !btc->sjw_max) { + bt->sjw = 1; + } else { + /* bt->sjw is at least 1 -> sanitize upper bound to sjw_max */ + if (bt->sjw > btc->sjw_max) + bt->sjw = btc->sjw_max; + /* bt->sjw must not be higher than tseg2 */ + if (tseg2 < bt->sjw) + bt->sjw = tseg2; + } + + bt->brp = best_brp; + + /* real bitrate */ + bt->bitrate = priv->clock.freq / (bt->brp * (CAN_CALC_SYNC_SEG + tseg1 + tseg2)); + + return 0; +} + +/* + * Checks the validity of the specified bit-timing parameters prop_seg, + * phase_seg1, phase_seg2 and sjw and tries to determine the bitrate + * prescaler value brp. You can find more information in the header + * file linux/can/netlink.h. + */ +static int can_fixup_bittiming(struct net_device *dev, struct can_bittiming *bt, + const struct can_bittiming_const *btc) +{ + struct can_priv *priv = netdev_priv(dev); + int tseg1, alltseg; + u64 brp64; + + tseg1 = bt->prop_seg + bt->phase_seg1; + if (!bt->sjw) + bt->sjw = 1; + if (bt->sjw > btc->sjw_max || + tseg1 < btc->tseg1_min || tseg1 > btc->tseg1_max || + bt->phase_seg2 < btc->tseg2_min || bt->phase_seg2 > btc->tseg2_max) + return -ERANGE; + + brp64 = (u64)priv->clock.freq * (u64)bt->tq; + if (btc->brp_inc > 1) + do_div(brp64, btc->brp_inc); + brp64 += 500000000UL - 1; + do_div(brp64, 1000000000UL); /* the practicable BRP */ + if (btc->brp_inc > 1) + brp64 *= btc->brp_inc; + bt->brp = (u32)brp64; + + if (bt->brp < btc->brp_min || bt->brp > btc->brp_max) + return -EINVAL; + + alltseg = bt->prop_seg + bt->phase_seg1 + bt->phase_seg2 + 1; + bt->bitrate = priv->clock.freq / (bt->brp * alltseg); + bt->sample_point = ((tseg1 + 1) * 1000) / alltseg; + + return 0; +} diff --git a/Devices/Libraries/Systems/can-utils/calc-bit-timing/can-calc-bit-timing-v5_16.c b/Devices/Libraries/Systems/can-utils/calc-bit-timing/can-calc-bit-timing-v5_16.c new file mode 100755 index 0000000..2637671 --- /dev/null +++ b/Devices/Libraries/Systems/can-utils/calc-bit-timing/can-calc-bit-timing-v5_16.c @@ -0,0 +1,218 @@ +/* SPDX-License-Identifier: GPL-2.0-only */ + +#include "compat.h" + +/* + * imported from v5.16-rc1~159^2~104^2~13 + * + */ + +/* Copyright (C) 2005 Marc Kleine-Budde, Pengutronix + * Copyright (C) 2006 Andrey Volkov, Varma Electronics + * Copyright (C) 2008-2009 Wolfgang Grandegger + */ + +/* Bit-timing calculation derived from: + * + * Code based on LinCAN sources and H8S2638 project + * Copyright 2004-2006 Pavel Pisa - DCE FELK CVUT cz + * Copyright 2005 Stanislav Marek + * email: pisa@cmp.felk.cvut.cz + * + * Calculates proper bit-timing parameters for a specified bit-rate + * and sample-point, which can then be used to set the bit-timing + * registers of the CAN controller. You can find more information + * in the header file linux/can/netlink.h. + */ +static int +can_update_sample_point(const struct can_bittiming_const *btc, + unsigned int sample_point_nominal, unsigned int tseg, + unsigned int *tseg1_ptr, unsigned int *tseg2_ptr, + unsigned int *sample_point_error_ptr) +{ + unsigned int sample_point_error, best_sample_point_error = UINT_MAX; + unsigned int sample_point, best_sample_point = 0; + unsigned int tseg1, tseg2; + int i; + + for (i = 0; i <= 1; i++) { + tseg2 = tseg + CAN_SYNC_SEG - + (sample_point_nominal * (tseg + CAN_SYNC_SEG)) / + 1000 - i; + tseg2 = clamp(tseg2, btc->tseg2_min, btc->tseg2_max); + tseg1 = tseg - tseg2; + if (tseg1 > btc->tseg1_max) { + tseg1 = btc->tseg1_max; + tseg2 = tseg - tseg1; + } + + sample_point = 1000 * (tseg + CAN_SYNC_SEG - tseg2) / + (tseg + CAN_SYNC_SEG); + sample_point_error = abs(sample_point_nominal - sample_point); + + if (sample_point <= sample_point_nominal && + sample_point_error < best_sample_point_error) { + best_sample_point = sample_point; + best_sample_point_error = sample_point_error; + *tseg1_ptr = tseg1; + *tseg2_ptr = tseg2; + } + } + + if (sample_point_error_ptr) + *sample_point_error_ptr = best_sample_point_error; + + return best_sample_point; +} + +int can_calc_bittiming(struct net_device *dev, struct can_bittiming *bt, + const struct can_bittiming_const *btc) +{ + struct can_priv *priv = netdev_priv(dev); + unsigned int bitrate; /* current bitrate */ + unsigned int bitrate_error; /* difference between current and nominal value */ + unsigned int best_bitrate_error = UINT_MAX; + unsigned int sample_point_error; /* difference between current and nominal value */ + unsigned int best_sample_point_error = UINT_MAX; + unsigned int sample_point_nominal; /* nominal sample point */ + unsigned int best_tseg = 0; /* current best value for tseg */ + unsigned int best_brp = 0; /* current best value for brp */ + unsigned int brp, tsegall, tseg, tseg1 = 0, tseg2 = 0; + u64 v64; + + /* Use CiA recommended sample points */ + if (bt->sample_point) { + sample_point_nominal = bt->sample_point; + } else { + if (bt->bitrate > 800 * CAN_KBPS) + sample_point_nominal = 750; + else if (bt->bitrate > 500 * CAN_KBPS) + sample_point_nominal = 800; + else + sample_point_nominal = 875; + } + + /* tseg even = round down, odd = round up */ + for (tseg = (btc->tseg1_max + btc->tseg2_max) * 2 + 1; + tseg >= (btc->tseg1_min + btc->tseg2_min) * 2; tseg--) { + tsegall = CAN_SYNC_SEG + tseg / 2; + + /* Compute all possible tseg choices (tseg=tseg1+tseg2) */ + brp = priv->clock.freq / (tsegall * bt->bitrate) + tseg % 2; + + /* choose brp step which is possible in system */ + brp = (brp / btc->brp_inc) * btc->brp_inc; + if (brp < btc->brp_min || brp > btc->brp_max) + continue; + + bitrate = priv->clock.freq / (brp * tsegall); + bitrate_error = abs(bt->bitrate - bitrate); + + /* tseg brp biterror */ + if (bitrate_error > best_bitrate_error) + continue; + + /* reset sample point error if we have a better bitrate */ + if (bitrate_error < best_bitrate_error) + best_sample_point_error = UINT_MAX; + + can_update_sample_point(btc, sample_point_nominal, tseg / 2, + &tseg1, &tseg2, &sample_point_error); + if (sample_point_error > best_sample_point_error) + continue; + + best_sample_point_error = sample_point_error; + best_bitrate_error = bitrate_error; + best_tseg = tseg / 2; + best_brp = brp; + + if (bitrate_error == 0 && sample_point_error == 0) + break; + } + + if (best_bitrate_error) { + /* Error in one-tenth of a percent */ + v64 = (u64)best_bitrate_error * 1000; + do_div(v64, bt->bitrate); + bitrate_error = (u32)v64; + if (bitrate_error > CAN_CALC_MAX_ERROR) { + netdev_err(dev, + "bitrate error %d.%d%% too high\n", + bitrate_error / 10, bitrate_error % 10); + return -EDOM; + } + netdev_warn(dev, "bitrate error %d.%d%%\n", + bitrate_error / 10, bitrate_error % 10); + } + + /* real sample point */ + bt->sample_point = can_update_sample_point(btc, sample_point_nominal, + best_tseg, &tseg1, &tseg2, + NULL); + + v64 = (u64)best_brp * 1000 * 1000 * 1000; + do_div(v64, priv->clock.freq); + bt->tq = (u32)v64; + bt->prop_seg = tseg1 / 2; + bt->phase_seg1 = tseg1 - bt->prop_seg; + bt->phase_seg2 = tseg2; + + /* check for sjw user settings */ + if (!bt->sjw || !btc->sjw_max) { + bt->sjw = 1; + } else { + /* bt->sjw is at least 1 -> sanitize upper bound to sjw_max */ + if (bt->sjw > btc->sjw_max) + bt->sjw = btc->sjw_max; + /* bt->sjw must not be higher than tseg2 */ + if (tseg2 < bt->sjw) + bt->sjw = tseg2; + } + + bt->brp = best_brp; + + /* real bitrate */ + bt->bitrate = priv->clock.freq / + (bt->brp * (CAN_SYNC_SEG + tseg1 + tseg2)); + + return 0; +} + +/* Checks the validity of the specified bit-timing parameters prop_seg, + * phase_seg1, phase_seg2 and sjw and tries to determine the bitrate + * prescaler value brp. You can find more information in the header + * file linux/can/netlink.h. + */ +static int can_fixup_bittiming(struct net_device *dev, struct can_bittiming *bt, + const struct can_bittiming_const *btc) +{ + struct can_priv *priv = netdev_priv(dev); + unsigned int tseg1, alltseg; + u64 brp64; + + tseg1 = bt->prop_seg + bt->phase_seg1; + if (!bt->sjw) + bt->sjw = 1; + if (bt->sjw > btc->sjw_max || + tseg1 < btc->tseg1_min || tseg1 > btc->tseg1_max || + bt->phase_seg2 < btc->tseg2_min || bt->phase_seg2 > btc->tseg2_max) + return -ERANGE; + + brp64 = (u64)priv->clock.freq * (u64)bt->tq; + if (btc->brp_inc > 1) + do_div(brp64, btc->brp_inc); + brp64 += 500000000UL - 1; + do_div(brp64, 1000000000UL); /* the practicable BRP */ + if (btc->brp_inc > 1) + brp64 *= btc->brp_inc; + bt->brp = (u32)brp64; + + if (bt->brp < btc->brp_min || bt->brp > btc->brp_max) + return -EINVAL; + + alltseg = bt->prop_seg + bt->phase_seg1 + bt->phase_seg2 + 1; + bt->bitrate = priv->clock.freq / (bt->brp * alltseg); + bt->sample_point = ((tseg1 + 1) * 1000) / alltseg; + + return 0; +} diff --git a/Devices/Libraries/Systems/can-utils/calc-bit-timing/can-calc-bit-timing-v5_19.c b/Devices/Libraries/Systems/can-utils/calc-bit-timing/can-calc-bit-timing-v5_19.c new file mode 100755 index 0000000..9ad05db --- /dev/null +++ b/Devices/Libraries/Systems/can-utils/calc-bit-timing/can-calc-bit-timing-v5_19.c @@ -0,0 +1,218 @@ +/* SPDX-License-Identifier: GPL-2.0-only */ + +#include "compat.h" + +/* + * imported from v5.19-rc1~159^2~286^2~15 + * + */ + +/* Copyright (C) 2005 Marc Kleine-Budde, Pengutronix + * Copyright (C) 2006 Andrey Volkov, Varma Electronics + * Copyright (C) 2008-2009 Wolfgang Grandegger + */ + +/* Bit-timing calculation derived from: + * + * Code based on LinCAN sources and H8S2638 project + * Copyright 2004-2006 Pavel Pisa - DCE FELK CVUT cz + * Copyright 2005 Stanislav Marek + * email: pisa@cmp.felk.cvut.cz + * + * Calculates proper bit-timing parameters for a specified bit-rate + * and sample-point, which can then be used to set the bit-timing + * registers of the CAN controller. You can find more information + * in the header file linux/can/netlink.h. + */ +static int +can_update_sample_point(const struct can_bittiming_const *btc, + const unsigned int sample_point_nominal, const unsigned int tseg, + unsigned int *tseg1_ptr, unsigned int *tseg2_ptr, + unsigned int *sample_point_error_ptr) +{ + unsigned int sample_point_error, best_sample_point_error = UINT_MAX; + unsigned int sample_point, best_sample_point = 0; + unsigned int tseg1, tseg2; + int i; + + for (i = 0; i <= 1; i++) { + tseg2 = tseg + CAN_SYNC_SEG - + (sample_point_nominal * (tseg + CAN_SYNC_SEG)) / + 1000 - i; + tseg2 = clamp(tseg2, btc->tseg2_min, btc->tseg2_max); + tseg1 = tseg - tseg2; + if (tseg1 > btc->tseg1_max) { + tseg1 = btc->tseg1_max; + tseg2 = tseg - tseg1; + } + + sample_point = 1000 * (tseg + CAN_SYNC_SEG - tseg2) / + (tseg + CAN_SYNC_SEG); + sample_point_error = abs(sample_point_nominal - sample_point); + + if (sample_point <= sample_point_nominal && + sample_point_error < best_sample_point_error) { + best_sample_point = sample_point; + best_sample_point_error = sample_point_error; + *tseg1_ptr = tseg1; + *tseg2_ptr = tseg2; + } + } + + if (sample_point_error_ptr) + *sample_point_error_ptr = best_sample_point_error; + + return best_sample_point; +} + +int can_calc_bittiming(const struct net_device *dev, struct can_bittiming *bt, + const struct can_bittiming_const *btc) +{ + struct can_priv *priv = netdev_priv(dev); + unsigned int bitrate; /* current bitrate */ + unsigned int bitrate_error; /* difference between current and nominal value */ + unsigned int best_bitrate_error = UINT_MAX; + unsigned int sample_point_error; /* difference between current and nominal value */ + unsigned int best_sample_point_error = UINT_MAX; + unsigned int sample_point_nominal; /* nominal sample point */ + unsigned int best_tseg = 0; /* current best value for tseg */ + unsigned int best_brp = 0; /* current best value for brp */ + unsigned int brp, tsegall, tseg, tseg1 = 0, tseg2 = 0; + u64 v64; + + /* Use CiA recommended sample points */ + if (bt->sample_point) { + sample_point_nominal = bt->sample_point; + } else { + if (bt->bitrate > 800 * KILO /* BPS */) + sample_point_nominal = 750; + else if (bt->bitrate > 500 * KILO /* BPS */) + sample_point_nominal = 800; + else + sample_point_nominal = 875; + } + + /* tseg even = round down, odd = round up */ + for (tseg = (btc->tseg1_max + btc->tseg2_max) * 2 + 1; + tseg >= (btc->tseg1_min + btc->tseg2_min) * 2; tseg--) { + tsegall = CAN_SYNC_SEG + tseg / 2; + + /* Compute all possible tseg choices (tseg=tseg1+tseg2) */ + brp = priv->clock.freq / (tsegall * bt->bitrate) + tseg % 2; + + /* choose brp step which is possible in system */ + brp = (brp / btc->brp_inc) * btc->brp_inc; + if (brp < btc->brp_min || brp > btc->brp_max) + continue; + + bitrate = priv->clock.freq / (brp * tsegall); + bitrate_error = abs(bt->bitrate - bitrate); + + /* tseg brp biterror */ + if (bitrate_error > best_bitrate_error) + continue; + + /* reset sample point error if we have a better bitrate */ + if (bitrate_error < best_bitrate_error) + best_sample_point_error = UINT_MAX; + + can_update_sample_point(btc, sample_point_nominal, tseg / 2, + &tseg1, &tseg2, &sample_point_error); + if (sample_point_error >= best_sample_point_error) + continue; + + best_sample_point_error = sample_point_error; + best_bitrate_error = bitrate_error; + best_tseg = tseg / 2; + best_brp = brp; + + if (bitrate_error == 0 && sample_point_error == 0) + break; + } + + if (best_bitrate_error) { + /* Error in one-tenth of a percent */ + v64 = (u64)best_bitrate_error * 1000; + do_div(v64, bt->bitrate); + bitrate_error = (u32)v64; + if (bitrate_error > CAN_CALC_MAX_ERROR) { + netdev_err(dev, + "bitrate error %d.%d%% too high\n", + bitrate_error / 10, bitrate_error % 10); + return -EDOM; + } + netdev_warn(dev, "bitrate error %d.%d%%\n", + bitrate_error / 10, bitrate_error % 10); + } + + /* real sample point */ + bt->sample_point = can_update_sample_point(btc, sample_point_nominal, + best_tseg, &tseg1, &tseg2, + NULL); + + v64 = (u64)best_brp * 1000 * 1000 * 1000; + do_div(v64, priv->clock.freq); + bt->tq = (u32)v64; + bt->prop_seg = tseg1 / 2; + bt->phase_seg1 = tseg1 - bt->prop_seg; + bt->phase_seg2 = tseg2; + + /* check for sjw user settings */ + if (!bt->sjw || !btc->sjw_max) { + bt->sjw = 1; + } else { + /* bt->sjw is at least 1 -> sanitize upper bound to sjw_max */ + if (bt->sjw > btc->sjw_max) + bt->sjw = btc->sjw_max; + /* bt->sjw must not be higher than tseg2 */ + if (tseg2 < bt->sjw) + bt->sjw = tseg2; + } + + bt->brp = best_brp; + + /* real bitrate */ + bt->bitrate = priv->clock.freq / + (bt->brp * (CAN_SYNC_SEG + tseg1 + tseg2)); + + return 0; +} + +/* Checks the validity of the specified bit-timing parameters prop_seg, + * phase_seg1, phase_seg2 and sjw and tries to determine the bitrate + * prescaler value brp. You can find more information in the header + * file linux/can/netlink.h. + */ +static int can_fixup_bittiming(const struct net_device *dev, struct can_bittiming *bt, + const struct can_bittiming_const *btc) +{ + const struct can_priv *priv = netdev_priv(dev); + unsigned int tseg1, alltseg; + u64 brp64; + + tseg1 = bt->prop_seg + bt->phase_seg1; + if (!bt->sjw) + bt->sjw = 1; + if (bt->sjw > btc->sjw_max || + tseg1 < btc->tseg1_min || tseg1 > btc->tseg1_max || + bt->phase_seg2 < btc->tseg2_min || bt->phase_seg2 > btc->tseg2_max) + return -ERANGE; + + brp64 = (u64)priv->clock.freq * (u64)bt->tq; + if (btc->brp_inc > 1) + do_div(brp64, btc->brp_inc); + brp64 += 500000000UL - 1; + do_div(brp64, 1000000000UL); /* the practicable BRP */ + if (btc->brp_inc > 1) + brp64 *= btc->brp_inc; + bt->brp = (u32)brp64; + + if (bt->brp < btc->brp_min || bt->brp > btc->brp_max) + return -EINVAL; + + alltseg = bt->prop_seg + bt->phase_seg1 + bt->phase_seg2 + 1; + bt->bitrate = priv->clock.freq / (bt->brp * alltseg); + bt->sample_point = ((tseg1 + 1) * 1000) / alltseg; + + return 0; +} diff --git a/Devices/Libraries/Systems/can-utils/calc-bit-timing/can-calc-bit-timing-v6_3.c b/Devices/Libraries/Systems/can-utils/calc-bit-timing/can-calc-bit-timing-v6_3.c new file mode 100755 index 0000000..857d44a --- /dev/null +++ b/Devices/Libraries/Systems/can-utils/calc-bit-timing/can-calc-bit-timing-v6_3.c @@ -0,0 +1,292 @@ +/* SPDX-License-Identifier: GPL-2.0-only */ + +#include "compat.h" + +/* + * imported from v6.3-rc1~162^2~124^2^2~1 + * + */ + +void can_sjw_set_default(struct can_bittiming *bt) +{ + if (bt->sjw) + return; + + /* If user space provides no sjw, use sane default of phase_seg2 / 2 */ + bt->sjw = max(1U, min(bt->phase_seg1, bt->phase_seg2 / 2)); +} + +int can_sjw_check(const struct net_device *dev, const struct can_bittiming *bt, + const struct can_bittiming_const *btc, struct netlink_ext_ack *extack) +{ + if (bt->sjw > btc->sjw_max) { + NL_SET_ERR_MSG_FMT(extack, "sjw: %u greater than max sjw: %u", + bt->sjw, btc->sjw_max); + return -EINVAL; + } + + if (bt->sjw > bt->phase_seg1) { + NL_SET_ERR_MSG_FMT(extack, + "sjw: %u greater than phase-seg1: %u", + bt->sjw, bt->phase_seg1); + return -EINVAL; + } + + if (bt->sjw > bt->phase_seg2) { + NL_SET_ERR_MSG_FMT(extack, + "sjw: %u greater than phase-seg2: %u", + bt->sjw, bt->phase_seg2); + return -EINVAL; + } + + return 0; +} + +/* + * can_bit_time() - Duration of one bit + * + * Please refer to ISO 11898-1:2015, section 11.3.1.1 "Bit time" for + * additional information. + * + * Return: the number of time quanta in one bit. + */ +static inline unsigned int can_bit_time(const struct can_bittiming *bt) +{ + return CAN_SYNC_SEG + bt->prop_seg + bt->phase_seg1 + bt->phase_seg2; +} + +/* Copyright (C) 2005 Marc Kleine-Budde, Pengutronix + * Copyright (C) 2006 Andrey Volkov, Varma Electronics + * Copyright (C) 2008-2009 Wolfgang Grandegger + */ + +/* Bit-timing calculation derived from: + * + * Code based on LinCAN sources and H8S2638 project + * Copyright 2004-2006 Pavel Pisa - DCE FELK CVUT cz + * Copyright 2005 Stanislav Marek + * email: pisa@cmp.felk.cvut.cz + * + * Calculates proper bit-timing parameters for a specified bit-rate + * and sample-point, which can then be used to set the bit-timing + * registers of the CAN controller. You can find more information + * in the header file linux/can/netlink.h. + */ +static int +can_update_sample_point(const struct can_bittiming_const *btc, + const unsigned int sample_point_nominal, const unsigned int tseg, + unsigned int *tseg1_ptr, unsigned int *tseg2_ptr, + unsigned int *sample_point_error_ptr) +{ + unsigned int sample_point_error, best_sample_point_error = UINT_MAX; + unsigned int sample_point, best_sample_point = 0; + unsigned int tseg1, tseg2; + int i; + + for (i = 0; i <= 1; i++) { + tseg2 = tseg + CAN_SYNC_SEG - + (sample_point_nominal * (tseg + CAN_SYNC_SEG)) / + 1000 - i; + tseg2 = clamp(tseg2, btc->tseg2_min, btc->tseg2_max); + tseg1 = tseg - tseg2; + if (tseg1 > btc->tseg1_max) { + tseg1 = btc->tseg1_max; + tseg2 = tseg - tseg1; + } + + sample_point = 1000 * (tseg + CAN_SYNC_SEG - tseg2) / + (tseg + CAN_SYNC_SEG); + sample_point_error = abs(sample_point_nominal - sample_point); + + if (sample_point <= sample_point_nominal && + sample_point_error < best_sample_point_error) { + best_sample_point = sample_point; + best_sample_point_error = sample_point_error; + *tseg1_ptr = tseg1; + *tseg2_ptr = tseg2; + } + } + + if (sample_point_error_ptr) + *sample_point_error_ptr = best_sample_point_error; + + return best_sample_point; +} + +int can_calc_bittiming(const struct net_device *dev, struct can_bittiming *bt, + const struct can_bittiming_const *btc, struct netlink_ext_ack *extack) +{ + struct can_priv *priv = netdev_priv(dev); + unsigned int bitrate; /* current bitrate */ + unsigned int bitrate_error; /* difference between current and nominal value */ + unsigned int best_bitrate_error = UINT_MAX; + unsigned int sample_point_error; /* difference between current and nominal value */ + unsigned int best_sample_point_error = UINT_MAX; + unsigned int sample_point_nominal; /* nominal sample point */ + unsigned int best_tseg = 0; /* current best value for tseg */ + unsigned int best_brp = 0; /* current best value for brp */ + unsigned int brp, tsegall, tseg, tseg1 = 0, tseg2 = 0; + u64 v64; + int err; + + /* Use CiA recommended sample points */ + if (bt->sample_point) { + sample_point_nominal = bt->sample_point; + } else { + if (bt->bitrate > 800 * KILO /* BPS */) + sample_point_nominal = 750; + else if (bt->bitrate > 500 * KILO /* BPS */) + sample_point_nominal = 800; + else + sample_point_nominal = 875; + } + + /* tseg even = round down, odd = round up */ + for (tseg = (btc->tseg1_max + btc->tseg2_max) * 2 + 1; + tseg >= (btc->tseg1_min + btc->tseg2_min) * 2; tseg--) { + tsegall = CAN_SYNC_SEG + tseg / 2; + + /* Compute all possible tseg choices (tseg=tseg1+tseg2) */ + brp = priv->clock.freq / (tsegall * bt->bitrate) + tseg % 2; + + /* choose brp step which is possible in system */ + brp = (brp / btc->brp_inc) * btc->brp_inc; + if (brp < btc->brp_min || brp > btc->brp_max) + continue; + + bitrate = priv->clock.freq / (brp * tsegall); + bitrate_error = abs(bt->bitrate - bitrate); + + /* tseg brp biterror */ + if (bitrate_error > best_bitrate_error) + continue; + + /* reset sample point error if we have a better bitrate */ + if (bitrate_error < best_bitrate_error) + best_sample_point_error = UINT_MAX; + + can_update_sample_point(btc, sample_point_nominal, tseg / 2, + &tseg1, &tseg2, &sample_point_error); + if (sample_point_error >= best_sample_point_error) + continue; + + best_sample_point_error = sample_point_error; + best_bitrate_error = bitrate_error; + best_tseg = tseg / 2; + best_brp = brp; + + if (bitrate_error == 0 && sample_point_error == 0) + break; + } + + if (best_bitrate_error) { + /* Error in one-tenth of a percent */ + v64 = (u64)best_bitrate_error * 1000; + do_div(v64, bt->bitrate); + bitrate_error = (u32)v64; + if (bitrate_error > CAN_CALC_MAX_ERROR) { + NL_SET_ERR_MSG_FMT(extack, + "bitrate error: %u.%u%% too high", + bitrate_error / 10, bitrate_error % 10); + return -EINVAL; + } + NL_SET_ERR_MSG_FMT(extack, + "bitrate error: %u.%u%%", + bitrate_error / 10, bitrate_error % 10); + } + + /* real sample point */ + bt->sample_point = can_update_sample_point(btc, sample_point_nominal, + best_tseg, &tseg1, &tseg2, + NULL); + + v64 = (u64)best_brp * 1000 * 1000 * 1000; + do_div(v64, priv->clock.freq); + bt->tq = (u32)v64; + bt->prop_seg = tseg1 / 2; + bt->phase_seg1 = tseg1 - bt->prop_seg; + bt->phase_seg2 = tseg2; + + can_sjw_set_default(bt); + + err = can_sjw_check(dev, bt, btc, extack); + if (err) + return err; + + bt->brp = best_brp; + + /* real bitrate */ + bt->bitrate = priv->clock.freq / + (bt->brp * can_bit_time(bt)); + + return 0; +} + +/* Checks the validity of the specified bit-timing parameters prop_seg, + * phase_seg1, phase_seg2 and sjw and tries to determine the bitrate + * prescaler value brp. You can find more information in the header + * file linux/can/netlink.h. + */ +static int can_fixup_bittiming(const struct net_device *dev, struct can_bittiming *bt, + const struct can_bittiming_const *btc, + struct netlink_ext_ack *extack) +{ + const unsigned int tseg1 = bt->prop_seg + bt->phase_seg1; + const struct can_priv *priv = netdev_priv(dev); + u64 brp64; + int err; + + if (tseg1 < btc->tseg1_min) { + NL_SET_ERR_MSG_FMT(extack, "prop-seg + phase-seg1: %u less than tseg1-min: %u", + tseg1, btc->tseg1_min); + return -EINVAL; + } + if (tseg1 > btc->tseg1_max) { + NL_SET_ERR_MSG_FMT(extack, "prop-seg + phase-seg1: %u greater than tseg1-max: %u", + tseg1, btc->tseg1_max); + return -EINVAL; + } + if (bt->phase_seg2 < btc->tseg2_min) { + NL_SET_ERR_MSG_FMT(extack, "phase-seg2: %u less than tseg2-min: %u", + bt->phase_seg2, btc->tseg2_min); + return -EINVAL; + } + if (bt->phase_seg2 > btc->tseg2_max) { + NL_SET_ERR_MSG_FMT(extack, "phase-seg2: %u greater than tseg2-max: %u", + bt->phase_seg2, btc->tseg2_max); + return -EINVAL; + } + + can_sjw_set_default(bt); + + err = can_sjw_check(dev, bt, btc, extack); + if (err) + return err; + + brp64 = (u64)priv->clock.freq * (u64)bt->tq; + if (btc->brp_inc > 1) + do_div(brp64, btc->brp_inc); + brp64 += 500000000UL - 1; + do_div(brp64, 1000000000UL); /* the practicable BRP */ + if (btc->brp_inc > 1) + brp64 *= btc->brp_inc; + bt->brp = (u32)brp64; + + if (bt->brp < btc->brp_min) { + NL_SET_ERR_MSG_FMT(extack, "resulting brp: %u less than brp-min: %u", + bt->brp, btc->brp_min); + return -EINVAL; + } + if (bt->brp > btc->brp_max) { + NL_SET_ERR_MSG_FMT(extack, "resulting brp: %u greater than brp-max: %u", + bt->brp, btc->brp_max); + return -EINVAL; + } + + bt->bitrate = priv->clock.freq / (bt->brp * can_bit_time(bt)); + bt->sample_point = ((CAN_SYNC_SEG + tseg1) * 1000) / can_bit_time(bt); + bt->tq = DIV_U64_ROUND_CLOSEST(mul_u32_u32(bt->brp, NSEC_PER_SEC), + priv->clock.freq); + + return 0; +} diff --git a/Devices/Libraries/Systems/can-utils/calc-bit-timing/can-calc-bit-timing.c b/Devices/Libraries/Systems/can-utils/calc-bit-timing/can-calc-bit-timing.c new file mode 100755 index 0000000..12a7b97 --- /dev/null +++ b/Devices/Libraries/Systems/can-utils/calc-bit-timing/can-calc-bit-timing.c @@ -0,0 +1,1617 @@ +/* SPDX-License-Identifier: GPL-2.0-only */ +/* can-calc-bit-timing.c: Calculate CAN bit timing parameters + * + * Copyright (C) 2008 Wolfgang Grandegger + * Copyright (C) 2016, 2021, 2022 Marc Kleine-Budde + * + * Derived from: + * can_baud.c - CAN baudrate calculation + * Code based on LinCAN sources and H8S2638 project + * Copyright 2004-2006 Pavel Pisa - DCE FELK CVUT cz + * Copyright 2005 Stanislav Marek + * email:pisa@cmp.felk.cvut.cz + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the version 2 of the GNU General Public License + * as published by the Free Software Foundation + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "compat.h" + +enum { + OPT_TQ = UCHAR_MAX + 1, + OPT_PROP_SEG, + OPT_PHASE_SEG1, + OPT_PHASE_SEG2, + OPT_SJW, + OPT_BRP, + OPT_TSEG1, + OPT_TSEG2, + OPT_ALG, +}; + +struct calc_bittiming_const { + const struct can_bittiming_const bittiming_const; + const struct can_bittiming_const data_bittiming_const; + + const struct calc_ref_clk ref_clk[16]; + + void (*printf_btr)(struct can_bittiming *bt, bool hdr); + void (*printf_data_btr)(struct can_bittiming *bt, bool hdr); +}; + +struct alg { + union { + int (*calc_bittiming)(struct net_device *dev, struct can_bittiming *bt, + const struct can_bittiming_const *btc); + int (*calc_bittiming_const)(const struct net_device *dev, struct can_bittiming *bt, + const struct can_bittiming_const *btc); + int (*calc_bittiming_extack)(const struct net_device *dev, struct can_bittiming *bt, + const struct can_bittiming_const *btc, struct netlink_ext_ack *extack); + }; + union { + int (*fixup_bittiming)(struct net_device *dev, struct can_bittiming *bt, + const struct can_bittiming_const *btc); + int (*fixup_bittiming_const)(const struct net_device *dev, struct can_bittiming *bt, + const struct can_bittiming_const *btc); + int (*fixup_bittiming_extack)(const struct net_device *dev, struct can_bittiming *bt, + const struct can_bittiming_const *btc, + struct netlink_ext_ack *extack); + + }; + const char *name; +}; + +struct calc_data { + const struct can_bittiming_const *bittiming_const; + const struct alg *alg; + void (*printf_btr)(struct can_bittiming *bt, bool hdr); + const char *name; + + const struct calc_ref_clk *ref_clks; + const unsigned int *bitrates; + + unsigned int sample_point; + + const struct calc_ref_clk *opt_ref_clk; + const unsigned int *opt_bitrates; + const unsigned int *opt_data_bitrates; + const struct can_bittiming *opt_bt; + + bool quiet; + bool verbose; + bool fd_mode; +}; + +static void print_usage(char *cmd) +{ + printf("%s - calculate CAN bit timing parameters.\n", cmd); + printf("Usage: %s [options] []\n" + "Options:\n" + "\t-q don't print header line\n" + "\t-v verbose output, print bit timing const\n" + "\t-l list all support CAN controller names\n" + "\t-b arbitration bit-rate in bits/sec\n" + "\t-d data bit-rate in bits/sec\n" + "\t-s sample-point in one-tenth of a percent\n" + "\t or 0 for CIA recommended sample points\n" + "\t-c real CAN system clock in Hz\n" + "\t--alg choose specified algorithm for bit-timing calculation\n" + "\n" + "Or supply low level bit timing parameters to decode them:\n" + "\n" + "\t--tq Time quantum in ns\n" + "\t--prop-seg Propagation segment in TQs\n" + "\t--phase-seg1 Phase buffer segment 1 in TQs\n" + "\t--phase-seg2 Phase buffer segment 2 in TQs\n" + "\t--sjw Synchronisation jump width in TQs\n" + "\t--brp Bit-rate prescaler\n" + "\t--tseg1 Time segment 1 = prop-seg + phase-seg1\n" + "\t--tseg2 Time segment 2 = phase_seg2\n", + cmd); +} + +static void printf_btr_nop(struct can_bittiming *bt, bool hdr) +{ +} + +#define RCAR_CAN_BCR_TSEG1(x) (((x) & 0x0f) << 20) +#define RCAR_CAN_BCR_BPR(x) (((x) & 0x3ff) << 8) +#define RCAR_CAN_BCR_SJW(x) (((x) & 0x3) << 4) +#define RCAR_CAN_BCR_TSEG2(x) ((x) & 0x07) + +static void printf_btr_rcar_can(struct can_bittiming *bt, bool hdr) +{ + if (hdr) { + printf("%10s", "CiBCR"); + } else { + uint32_t bcr; + + bcr = RCAR_CAN_BCR_TSEG1(bt->phase_seg1 + bt->prop_seg - 1) | + RCAR_CAN_BCR_BPR(bt->brp - 1) | + RCAR_CAN_BCR_SJW(bt->sjw - 1) | + RCAR_CAN_BCR_TSEG2(bt->phase_seg2 - 1); + + printf("0x%08x", bcr << 8); + } +} + +static void printf_btr_mcp251x(struct can_bittiming *bt, bool hdr) +{ + uint8_t cnf1, cnf2, cnf3; + + if (hdr) { + printf("CNF1 CNF2 CNF3"); + } else { + cnf1 = ((bt->sjw - 1) << 6) | (bt->brp - 1); + cnf2 = 0x80 | ((bt->phase_seg1 - 1) << 3) | (bt->prop_seg - 1); + cnf3 = bt->phase_seg2 - 1; + printf("0x%02x 0x%02x 0x%02x", cnf1, cnf2, cnf3); + } +} + +static void printf_btr_mcp251xfd(struct can_bittiming *bt, bool hdr) +{ + if (hdr) { + printf("%10s", "NBTCFG"); + } else { + uint32_t nbtcfg = ((bt->brp - 1) << 24) | + ((bt->prop_seg + bt->phase_seg1 - 1) << 16) | + ((bt->phase_seg2 - 1) << 8) | + (bt->sjw - 1); + printf("0x%08x", nbtcfg); + } +} + +static void printf_btr_bxcan(struct can_bittiming *bt, bool hdr) +{ + if (hdr) { + printf("%10s", "CAN_BTR"); + } else { + uint32_t btr; + + btr = (((bt->brp -1) & 0x3ff) << 0) | + (((bt->prop_seg + bt->phase_seg1 -1) & 0xf) << 16) | + (((bt->phase_seg2 -1) & 0x7) << 20) | + (((bt->sjw -1) & 0x3) << 24); + + printf("0x%08x", btr); + } +} + +static void printf_btr_at91(struct can_bittiming *bt, bool hdr) +{ + if (hdr) { + printf("%10s", "CAN_BR"); + } else { + uint32_t br = ((bt->phase_seg2 - 1) | + ((bt->phase_seg1 - 1) << 4) | + ((bt->prop_seg - 1) << 8) | + ((bt->sjw - 1) << 12) | + ((bt->brp - 1) << 16)); + printf("0x%08x", br); + } +} + +static void printf_btr_c_can(struct can_bittiming *bt, bool hdr) +{ + if (hdr) { + printf("%13s", "BTR BRPEXT"); + } else { + uint32_t btr; + uint32_t brpext; + + btr = (((bt->brp -1) & 0x3f) << 0) | + (((bt->sjw -1) & 0x3) << 6) | + (((bt->prop_seg + bt->phase_seg1 -1) & 0xf) << 8) | + (((bt->phase_seg2 -1) & 0x7) << 12); + brpext = ((bt->brp -1) >> 6) & 0xf; + + printf("0x%04x 0x%04x", btr, brpext); + } +} + +static void printf_btr_flexcan(struct can_bittiming *bt, bool hdr) +{ + if (hdr) { + printf("%10s", "CAN_CTRL"); + } else { + uint32_t ctrl = (((bt->brp - 1) << 24) | + ((bt->sjw - 1) << 22) | + ((bt->phase_seg1 - 1) << 19) | + ((bt->phase_seg2 - 1) << 16) | + ((bt->prop_seg - 1) << 0)); + + printf("0x%08x", ctrl); + } +} + +static void printf_btr_mcan(struct can_bittiming *bt, bool hdr) +{ + if (hdr) { + printf("%10s", "NBTP"); + } else { + uint32_t nbtp; + + + nbtp = (((bt->brp -1) & 0x1ff) << 16) | + (((bt->sjw -1) & 0x7f) << 25) | + (((bt->prop_seg + bt->phase_seg1 -1) & 0xff) << 8) | + (((bt->phase_seg2 -1) & 0x7f) << 0); + + printf("0x%08x", nbtp); + } +} + +static void printf_btr_sja1000(struct can_bittiming *bt, bool hdr) +{ + uint8_t btr0, btr1; + + if (hdr) { + printf("%9s", "BTR0 BTR1"); + } else { + btr0 = ((bt->brp - 1) & 0x3f) | (((bt->sjw - 1) & 0x3) << 6); + btr1 = ((bt->prop_seg + bt->phase_seg1 - 1) & 0xf) | + (((bt->phase_seg2 - 1) & 0x7) << 4); + printf("0x%02x 0x%02x", btr0, btr1); + } +} + +static void printf_btr_ti_hecc(struct can_bittiming *bt, bool hdr) +{ + if (hdr) { + printf("%10s", "CANBTC"); + } else { + uint32_t can_btc; + + can_btc = (bt->phase_seg2 - 1) & 0x7; + can_btc |= ((bt->phase_seg1 + bt->prop_seg - 1) + & 0xF) << 3; + can_btc |= ((bt->sjw - 1) & 0x3) << 8; + can_btc |= ((bt->brp - 1) & 0xFF) << 16; + + printf("0x%08x", can_btc); + } +} + +static const struct calc_bittiming_const can_calc_consts[] = { + { + .bittiming_const = { + .name = "rcar_can", + .tseg1_min = 4, + .tseg1_max = 16, + .tseg2_min = 2, + .tseg2_max = 8, + .sjw_max = 4, + .brp_min = 1, + .brp_max = 1024, + .brp_inc = 1, + }, + .ref_clk = { + { .clk = 65000000, }, + }, + .printf_btr = printf_btr_rcar_can, + }, { + .bittiming_const = { + .name = "rcar_canfd", + .tseg1_min = 2, + .tseg1_max = 128, + .tseg2_min = 2, + .tseg2_max = 32, + .sjw_max = 32, + .brp_min = 1, + .brp_max = 1024, + .brp_inc = 1, + }, + .data_bittiming_const = { + .name = "rcar_canfd", + .tseg1_min = 2, + .tseg1_max = 16, + .tseg2_min = 2, + .tseg2_max = 8, + .sjw_max = 8, + .brp_min = 1, + .brp_max = 256, + .brp_inc = 1, + }, + .ref_clk = { + { .clk = 20000000, .name = "CIA recommendation" }, + { .clk = 40000000, .name = "CIA recommendation" }, + }, + }, { + .bittiming_const = { + .name = "rcar_canfd (CC)", + .tseg1_min = 4, + .tseg1_max = 16, + .tseg2_min = 2, + .tseg2_max = 8, + .sjw_max = 4, + .brp_min = 1, + .brp_max = 1024, + .brp_inc = 1, + }, + }, { + .bittiming_const = { + .name = "rockchip_canfd", + .tseg1_min = 1, + .tseg1_max = 256, + .tseg2_min = 1, + .tseg2_max = 128, + .sjw_max = 128, + .brp_min = 2, + .brp_max = 256, + .brp_inc = 2, + }, + .data_bittiming_const = { + .name = "rockchip_canfd", + .tseg1_min = 1, + .tseg1_max = 32, + .tseg2_min = 1, + .tseg2_max = 16, + .sjw_max = 16, + .brp_min = 2, + .brp_max = 256, + .brp_inc = 2, + }, + .ref_clk = { + { .clk = 20000000, .name = "CIA recommendation" }, + { .clk = 40000000, .name = "CIA recommendation" }, + { .clk = 300000000, .name = "rock-3a" }, + }, + }, { /* -------- SPI -------- */ + .bittiming_const = { + .name = "hi311x", + .tseg1_min = 2, + .tseg1_max = 16, + .tseg2_min = 2, + .tseg2_max = 8, + .sjw_max = 4, + .brp_min = 1, + .brp_max = 64, + .brp_inc = 1, + }, + .ref_clk = { + { .clk = 24000000, }, + }, + }, { + .bittiming_const = { + .name = "mcp251x", + .tseg1_min = 3, + .tseg1_max = 16, + .tseg2_min = 2, + .tseg2_max = 8, + .sjw_max = 4, + .brp_min = 1, + .brp_max = 64, + .brp_inc = 1, + }, + .ref_clk = { + /* The mcp251x uses half of the external OSC clock as the base clock */ + { .clk = 8000000 / 2, .name = "8 MHz OSC" }, + { .clk = 12000000 / 2, .name = "12 MHz OSC" }, + { .clk = 16000000 / 2, .name = "16 MHz OSC" }, + { .clk = 20000000 / 2, .name = "20 MHz OSC" }, + }, + .printf_btr = printf_btr_mcp251x, + }, { + .bittiming_const = { + .name = "mcp251xfd", + .tseg1_min = 2, + .tseg1_max = 256, + .tseg2_min = 1, + .tseg2_max = 128, + .sjw_max = 128, + .brp_min = 1, + .brp_max = 256, + .brp_inc = 1, + }, + .data_bittiming_const = { + .name = "mcp251xfd", + .tseg1_min = 1, + .tseg1_max = 32, + .tseg2_min = 1, + .tseg2_max = 16, + .sjw_max = 16, + .brp_min = 1, + .brp_max = 256, + .brp_inc = 1, + }, + .ref_clk = { + { .clk = 20000000, .name = "CIA recommendation" }, + { .clk = 40000000, .name = "CIA recommendation" }, + }, + .printf_btr = printf_btr_mcp251xfd, + }, { /* -------- USB -------- */ + .bittiming_const = { + .name = "usb_8dev", + .tseg1_min = 1, + .tseg1_max = 16, + .tseg2_min = 1, + .tseg2_max = 8, + .sjw_max = 4, + .brp_min = 1, + .brp_max = 1024, + .brp_inc = 1, + }, + .ref_clk = { + { .clk = 32000000, }, + } + }, { + .bittiming_const = { + .name = "ems_usb", + .tseg1_min = 1, + .tseg1_max = 16, + .tseg2_min = 1, + .tseg2_max = 8, + .sjw_max = 4, + .brp_min = 1, + .brp_max = 64, + .brp_inc = 1, + }, + .ref_clk = { + { .clk = 8000000, }, + }, + }, { + +#define ESD_USB2_TSEG1_MIN 1 +#define ESD_USB2_TSEG1_MAX 16 +#define ESD_USB2_TSEG2_MIN 1 +#define ESD_USB2_TSEG2_MAX 8 +#define ESD_USB2_SJW_MAX 4 +#define ESD_USB2_BRP_MIN 1 +#define ESD_USB2_BRP_MAX 1024 +#define ESD_USB2_BRP_INC 1 + +#define ESD_USB2_CAN_CLOCK 60000000 +#define ESD_USBM_CAN_CLOCK 36000000 + + .bittiming_const = { + .name = "esd_usb2", + .tseg1_min = ESD_USB2_TSEG1_MIN, + .tseg1_max = ESD_USB2_TSEG1_MAX, + .tseg2_min = ESD_USB2_TSEG2_MIN, + .tseg2_max = ESD_USB2_TSEG2_MAX, + .sjw_max = ESD_USB2_SJW_MAX, + .brp_min = ESD_USB2_BRP_MIN, + .brp_max = ESD_USB2_BRP_MAX, + .brp_inc = ESD_USB2_BRP_INC, + }, + .ref_clk = { + { .clk = ESD_USB2_CAN_CLOCK, .name = "CAN-USB/2", }, + { .clk = ESD_USBM_CAN_CLOCK, .name = "CAN-USB/Micro", }, + }, + }, { /* gs_usb */ + .bittiming_const = { + .name = "bxcan", // stm32f072 + .tseg1_min = 1, + .tseg1_max = 16, + .tseg2_min = 1, + .tseg2_max = 8, + .sjw_max = 4, + .brp_min = 1, + .brp_max = 1024, + .brp_inc = 1, + }, + .ref_clk = { + { .clk = 48000000, }, + }, + .printf_btr = printf_btr_bxcan, + }, { /* gs_usb */ + .bittiming_const = { + .name = "CANtact Pro", // LPC65616 + .tseg1_min = 1, + .tseg1_max = 16, + .tseg2_min = 1, + .tseg2_max = 8, + .sjw_max = 4, + .brp_min = 1, + .brp_max = 1024, + .brp_inc = 1, + }, + .data_bittiming_const = { + .name = "CANtact Pro", // LPC65616 + .tseg1_min = 1, + .tseg1_max = 16, + .tseg2_min = 1, + .tseg2_max = 8, + .sjw_max = 4, + .brp_min = 1, + .brp_max = 1024, + .brp_inc = 1, + }, + .ref_clk = { + { .clk = 24000000, .name = "CANtact Pro (original)", }, + { .clk = 40000000, .name = "CIA recommendation" }, + }, + }, { + +#define KVASER_USB_TSEG1_MIN 1 +#define KVASER_USB_TSEG1_MAX 16 +#define KVASER_USB_TSEG2_MIN 1 +#define KVASER_USB_TSEG2_MAX 8 +#define KVASER_USB_SJW_MAX 4 +#define KVASER_USB_BRP_MIN 1 +#define KVASER_USB_BRP_MAX 64 +#define KVASER_USB_BRP_INC 1 + + .bittiming_const = { + .name = "kvaser_usb", + .tseg1_min = KVASER_USB_TSEG1_MIN, + .tseg1_max = KVASER_USB_TSEG1_MAX, + .tseg2_min = KVASER_USB_TSEG2_MIN, + .tseg2_max = KVASER_USB_TSEG2_MAX, + .sjw_max = KVASER_USB_SJW_MAX, + .brp_min = KVASER_USB_BRP_MIN, + .brp_max = KVASER_USB_BRP_MAX, + .brp_inc = KVASER_USB_BRP_INC, + }, + .ref_clk = { + { .clk = 8000000, }, + }, + }, { + .bittiming_const = { + .name = "kvaser_usb_kcan", + .tseg1_min = 1, + .tseg1_max = 255, + .tseg2_min = 1, + .tseg2_max = 32, + .sjw_max = 16, + .brp_min = 1, + .brp_max = 8192, + .brp_inc = 1, + }, + .data_bittiming_const = { + .name = "kvaser_usb_kcan", + .tseg1_min = 1, + .tseg1_max = 255, + .tseg2_min = 1, + .tseg2_max = 32, + .sjw_max = 16, + .brp_min = 1, + .brp_max = 8192, + .brp_inc = 1, + }, + .ref_clk = { + { .clk = 80000000, }, + }, + }, { + .bittiming_const = { + .name = "kvaser_usb_flex", + .tseg1_min = 4, + .tseg1_max = 16, + .tseg2_min = 2, + .tseg2_max = 8, + .sjw_max = 4, + .brp_min = 1, + .brp_max = 256, + .brp_inc = 1, + }, + .ref_clk = { + { .clk = 24000000, }, + }, + }, { + .bittiming_const = { + .name = "pcan_usb_pro", + .tseg1_min = 1, + .tseg1_max = 16, + .tseg2_min = 1, + .tseg2_max = 8, + .sjw_max = 4, + .brp_min = 1, + .brp_max = 1024, + .brp_inc = 1, + }, + .ref_clk = { + { .clk = 56000000, }, + }, + }, { + +#define PUCAN_TSLOW_BRP_BITS 10 +#define PUCAN_TSLOW_TSGEG1_BITS 8 +#define PUCAN_TSLOW_TSGEG2_BITS 7 +#define PUCAN_TSLOW_SJW_BITS 7 + +#define PUCAN_TFAST_BRP_BITS 10 +#define PUCAN_TFAST_TSGEG1_BITS 5 +#define PUCAN_TFAST_TSGEG2_BITS 4 +#define PUCAN_TFAST_SJW_BITS 4 + + .bittiming_const = { + .name = "pcan_usb_fd", + .tseg1_min = 1, + .tseg1_max = (1 << PUCAN_TSLOW_TSGEG1_BITS), + .tseg2_min = 1, + .tseg2_max = (1 << PUCAN_TSLOW_TSGEG2_BITS), + .sjw_max = (1 << PUCAN_TSLOW_SJW_BITS), + .brp_min = 1, + .brp_max = (1 << PUCAN_TSLOW_BRP_BITS), + .brp_inc = 1, + }, + .data_bittiming_const = { + .name = "pcan_usb_fd", + .tseg1_min = 1, + .tseg1_max = (1 << PUCAN_TFAST_TSGEG1_BITS), + .tseg2_min = 1, + .tseg2_max = (1 << PUCAN_TFAST_TSGEG2_BITS), + .sjw_max = (1 << PUCAN_TFAST_SJW_BITS), + .brp_min = 1, + .brp_max = (1 << PUCAN_TFAST_BRP_BITS), + .brp_inc = 1, + }, + .ref_clk = { + { .clk = 80000000, }, + }, + }, { + .bittiming_const = { + .name = "softing", + .tseg1_min = 1, + .tseg1_max = 16, + .tseg2_min = 1, + .tseg2_max = 8, + .sjw_max = 4, + .brp_min = 1, + .brp_max = 32, + .brp_inc = 1, + }, + .ref_clk = { + { .clk = 8000000, }, + { .clk = 16000000, }, + }, + }, { + .bittiming_const = { + .name = "at91", + .tseg1_min = 4, + .tseg1_max = 16, + .tseg2_min = 2, + .tseg2_max = 8, + .sjw_max = 4, + .brp_min = 2, + .brp_max = 128, + .brp_inc = 1, + }, + .ref_clk = { + { .clk = 66000000, .name = "sama5d3", }, + { .clk = 99532800, .name = "ronetix PM9263", }, + { .clk = 100000000, }, + }, + .printf_btr = printf_btr_at91, + }, { + .bittiming_const = { + .name = "cc770", + .tseg1_min = 1, + .tseg1_max = 16, + .tseg2_min = 1, + .tseg2_max = 8, + .sjw_max = 4, + .brp_min = 1, + .brp_max = 64, + .brp_inc = 1, + }, + .ref_clk = { + { .clk = 8000000 }, + } + }, { + .bittiming_const = { + .name = "c_can", + .tseg1_min = 2, + .tseg1_max = 16, + .tseg2_min = 1, + .tseg2_max = 8, + .sjw_max = 4, + .brp_min = 1, + .brp_max = 1024, + .brp_inc = 1, + }, + .ref_clk = { + { .clk = 24000000, }, + }, + .printf_btr = printf_btr_c_can, + }, { + .bittiming_const = { + .name = "flexcan", + .tseg1_min = 4, + .tseg1_max = 16, + .tseg2_min = 2, + .tseg2_max = 8, + .sjw_max = 4, + .brp_min = 1, + .brp_max = 256, + .brp_inc = 1, + }, + .ref_clk = { + { .clk = 24000000, .name = "mx28" }, + { .clk = 30000000, .name = "mx6" }, + { .clk = 49875000, }, + { .clk = 66000000, }, + { .clk = 66500000, .name = "mx25" }, + { .clk = 66666666, }, + { .clk = 83368421, .name = "vybrid" }, + }, + .printf_btr = printf_btr_flexcan, + }, { + .bittiming_const = { + .name = "flexcan-fd", + .tseg1_min = 2, + .tseg1_max = 96, + .tseg2_min = 2, + .tseg2_max = 32, + .sjw_max = 16, + .brp_min = 1, + .brp_max = 1024, + .brp_inc = 1, + }, + .data_bittiming_const = { + .name = "flexcan-fd", + .tseg1_min = 2, + .tseg1_max = 39, + .tseg2_min = 2, + .tseg2_max = 8, + .sjw_max = 4, + .brp_min = 1, + .brp_max = 1024, + .brp_inc = 1, + }, + .ref_clk = { + { .clk = 20000000, .name = "CIA recommendation" }, + { .clk = 40000000, .name = "CIA recommendation" }, + }, + }, { + +#define GRCAN_CONF_PS1_MIN 1 +#define GRCAN_CONF_PS1_MAX 15 +#define GRCAN_CONF_PS2_MIN 2 +#define GRCAN_CONF_PS2_MAX 8 +#define GRCAN_CONF_RSJ_MAX 4 +#define GRCAN_CONF_SCALER_MIN 0 +#define GRCAN_CONF_SCALER_MAX 255 +#define GRCAN_CONF_SCALER_INC 1 + + .bittiming_const = { + .name = "grcan", + .tseg1_min = GRCAN_CONF_PS1_MIN + 1, + .tseg1_max = GRCAN_CONF_PS1_MAX + 1, + .tseg2_min = GRCAN_CONF_PS2_MIN, + .tseg2_max = GRCAN_CONF_PS2_MAX, + .sjw_max = GRCAN_CONF_RSJ_MAX, + .brp_min = GRCAN_CONF_SCALER_MIN + 1, + .brp_max = GRCAN_CONF_SCALER_MAX + 1, + .brp_inc = GRCAN_CONF_SCALER_INC, + }, + }, { + .bittiming_const = { + .name = "ifi_canfd", + .tseg1_min = 1, + .tseg1_max = 256, + .tseg2_min = 2, + .tseg2_max = 256, + .sjw_max = 128, + .brp_min = 2, + .brp_max = 512, + .brp_inc = 1, + }, + .data_bittiming_const = { + .name = "ifi_canfd", + .tseg1_min = 1, + .tseg1_max = 256, + .tseg2_min = 2, + .tseg2_max = 256, + .sjw_max = 128, + .brp_min = 2, + .brp_max = 512, + .brp_inc = 1, + }, + .ref_clk = { + { .clk = 20000000, .name = "CIA recommendation" }, + { .clk = 40000000, .name = "CIA recommendation" }, + }, + }, { + .bittiming_const = { + .name = "janz-ican3", + .tseg1_min = 1, + .tseg1_max = 16, + .tseg2_min = 1, + .tseg2_max = 8, + .sjw_max = 4, + .brp_min = 1, + .brp_max = 64, + .brp_inc = 1, + }, + .ref_clk = { + { .clk = 8000000, }, + }, + }, { + .bittiming_const = { + .name = "kvaser_pciefd", + .tseg1_min = 1, + .tseg1_max = 512, + .tseg2_min = 1, + .tseg2_max = 32, + .sjw_max = 16, + .brp_min = 1, + .brp_max = 8192, + .brp_inc = 1, + }, + .data_bittiming_const = { + .name = "kvaser_pciefd", + .tseg1_min = 1, + .tseg1_max = 512, + .tseg2_min = 1, + .tseg2_max = 32, + .sjw_max = 16, + .brp_min = 1, + .brp_max = 8192, + .brp_inc = 1, + }, + .ref_clk = { + { .clk = 20000000, .name = "CIA recommendation" }, + { .clk = 40000000, .name = "CIA recommendation" }, + }, + }, { + .bittiming_const = { + .name = "mscan", + .tseg1_min = 4, + .tseg1_max = 16, + .tseg2_min = 2, + .tseg2_max = 8, + .sjw_max = 4, + .brp_min = 1, + .brp_max = 64, + .brp_inc = 1, + }, + .ref_clk = { + { .clk = 32000000, }, + { .clk = 33000000, }, + { .clk = 33300000, }, + { .clk = 33333333, }, + { .clk = 66660000, .name = "mpc5121", }, + { .clk = 66666666, .name = "mpc5121" }, + }, + }, { + .bittiming_const = { + .name = "mcan-v3.0", + .tseg1_min = 2, + .tseg1_max = 64, + .tseg2_min = 1, + .tseg2_max = 16, + .sjw_max = 16, + .brp_min = 1, + .brp_max = 1024, + .brp_inc = 1, + }, + .data_bittiming_const = { + .name = "mcan-v3.0", + .tseg1_min = 2, + .tseg1_max = 16, + .tseg2_min = 1, + .tseg2_max = 8, + .sjw_max = 4, + .brp_min = 1, + .brp_max = 32, + .brp_inc = 1, + }, + .ref_clk = { + { .clk = 20000000, .name = "CIA recommendation" }, + { .clk = 40000000, .name = "CIA recommendation" }, + }, + .printf_btr = printf_btr_mcan, + }, { + .bittiming_const = { + .name = "mcan-v3.1+", + .tseg1_min = 2, + .tseg1_max = 256, + .tseg2_min = 2, + .tseg2_max = 128, + .sjw_max = 128, + .brp_min = 1, + .brp_max = 512, + .brp_inc = 1, + }, + .data_bittiming_const = { + .name = "mcan-v3.1+", + .tseg1_min = 1, + .tseg1_max = 32, + .tseg2_min = 1, + .tseg2_max = 16, + .sjw_max = 16, + .brp_min = 1, + .brp_max = 32, + .brp_inc = 1, + }, + .ref_clk = { + { .clk = 20000000, .name = "CIA recommendation" }, + { .clk = 40000000, .name = "CIA recommendation" }, + { .clk = 24000000, .name = "stm32mp1 - ck_hse" }, + { .clk = 24573875, .name = "stm32mp1 - pll3_q" }, + { .clk = 29700000, .name = "stm32mp1 - pll4_q" }, + { .clk = 48000000, .name = "stm32mp1 lxatac (new)" }, + { .clk = 60000000, .name = "stm32mp1 ecu02.5- pll4_r" }, + { .clk = 62500000, .name = "stm32mp1 lxatac (old) - pll4_r" }, + { .clk = 74250000, .name = "stm32mp1 - pll4_r" }, + }, + .printf_btr = printf_btr_mcan, + }, { + +#define PUCAN_TSLOW_BRP_BITS 10 +#define PUCAN_TSLOW_TSGEG1_BITS 8 +#define PUCAN_TSLOW_TSGEG2_BITS 7 +#define PUCAN_TSLOW_SJW_BITS 7 + +#define PUCAN_TFAST_BRP_BITS 10 +#define PUCAN_TFAST_TSGEG1_BITS 5 +#define PUCAN_TFAST_TSGEG2_BITS 4 +#define PUCAN_TFAST_SJW_BITS 4 + + .bittiming_const = { + .name = "peak_canfd", + .tseg1_min = 1, + .tseg1_max = (1 << PUCAN_TSLOW_TSGEG1_BITS), + .tseg2_min = 1, + .tseg2_max = (1 << PUCAN_TSLOW_TSGEG2_BITS), + .sjw_max = (1 << PUCAN_TSLOW_SJW_BITS), + .brp_min = 1, + .brp_max = (1 << PUCAN_TSLOW_BRP_BITS), + .brp_inc = 1, + }, + .data_bittiming_const = { + .name = "peak_canfd", + .tseg1_min = 1, + .tseg1_max = (1 << PUCAN_TFAST_TSGEG1_BITS), + .tseg2_min = 1, + .tseg2_max = (1 << PUCAN_TFAST_TSGEG2_BITS), + .sjw_max = (1 << PUCAN_TFAST_SJW_BITS), + .brp_min = 1, + .brp_max = (1 << PUCAN_TFAST_BRP_BITS), + .brp_inc = 1, + }, + .ref_clk = { + { .clk = 20000000, }, + { .clk = 24000000, }, + { .clk = 30000000, }, + { .clk = 40000000, }, + { .clk = 60000000, }, + { .clk = 80000000, }, + }, + }, { + .bittiming_const = { + .name = "sja1000", + .tseg1_min = 1, + .tseg1_max = 16, + .tseg2_min = 1, + .tseg2_max = 8, + .sjw_max = 4, + .brp_min = 1, + .brp_max = 64, + .brp_inc = 1, + }, + .ref_clk = { + { .clk = 16000000 / 2, }, + { .clk = 24000000 / 2, .name = "f81601" }, + }, + .printf_btr = printf_btr_sja1000, + }, { + .bittiming_const = { + .name = "sun4i_can", + .tseg1_min = 1, + .tseg1_max = 16, + .tseg2_min = 1, + .tseg2_max = 8, + .sjw_max = 4, + .brp_min = 1, + .brp_max = 64, + .brp_inc = 1, + }, + }, { + .bittiming_const = { + .name = "ti_hecc", + .tseg1_min = 1, + .tseg1_max = 16, + .tseg2_min = 1, + .tseg2_max = 8, + .sjw_max = 4, + .brp_min = 1, + .brp_max = 256, + .brp_inc = 1, + }, + .ref_clk = { + { .clk = 13000000, }, + }, + .printf_btr = printf_btr_ti_hecc, + }, { + .bittiming_const = { + .name = "xilinx_can", + .tseg1_min = 1, + .tseg1_max = 16, + .tseg2_min = 1, + .tseg2_max = 8, + .sjw_max = 4, + .brp_min = 1, + .brp_max = 256, + .brp_inc = 1, + }, + }, { + .bittiming_const = { + .name = "xilinx_can_fd", + .tseg1_min = 1, + .tseg1_max = 64, + .tseg2_min = 1, + .tseg2_max = 16, + .sjw_max = 16, + .brp_min = 1, + .brp_max = 256, + .brp_inc = 1, + }, + .data_bittiming_const = { + .name = "xilinx_can_fd", + .tseg1_min = 1, + .tseg1_max = 16, + .tseg2_min = 1, + .tseg2_max = 8, + .sjw_max = 8, + .brp_min = 1, + .brp_max = 256, + .brp_inc = 1, + }, + .ref_clk = { + { .clk = 20000000, .name = "CIA recommendation" }, + { .clk = 40000000, .name = "CIA recommendation" }, + }, + }, { + .bittiming_const = { + .name = "xilinx_can_fd2", + .tseg1_min = 1, + .tseg1_max = 256, + .tseg2_min = 1, + .tseg2_max = 128, + .sjw_max = 128, + .brp_min = 2, + .brp_max = 256, + .brp_inc = 1, + }, + .data_bittiming_const = { + .name = "xilinx_can_fd2", + .tseg1_min = 1, + .tseg1_max = 32, + .tseg2_min = 1, + .tseg2_max = 16, + .sjw_max = 16, + .brp_min = 2, + .brp_max = 256, + .brp_inc = 1, + }, + .ref_clk = { + { .clk = 20000000, .name = "CIA recommendation" }, + { .clk = 40000000, .name = "CIA recommendation" }, + { .clk = 79999999, .name = "Versal ACAP" }, + { .clk = 80000000, .name = "Versal ACAP" }, + }, + }, +}; + +static const unsigned int common_bitrates[] = { + 1000000, + 800000, + 666666, + 500000, + 250000, + 125000, + 100000, + 83333, + 50000, + 33333, + 20000, + 10000, + 0 +}; + +static const unsigned int common_data_bitrates[] = { + 12000000, + 10000000, + 8000000, + 5000000, + 4000000, + 2000000, + 1000000, + 0 +}; + +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wsign-compare" + +#define can_update_spt can_update_spt_v2_6_31 +#define can_calc_bittiming can_calc_bittiming_v2_6_31 +#define can_fixup_bittiming can_fixup_bittiming_v2_6_31 +#include "can-calc-bit-timing-v2_6_31.c" +#undef can_update_spt +#undef can_calc_bittiming +#undef can_fixup_bittiming + +#define can_update_spt can_update_spt_v3_18 +#define can_calc_bittiming can_calc_bittiming_v3_18 +#define can_fixup_bittiming can_fixup_bittiming_v3_18 +#include "can-calc-bit-timing-v3_18.c" +#undef can_update_spt +#undef can_calc_bittiming +#undef can_fixup_bittiming + +#define can_update_sample_point can_update_sample_point_v4_8 +#define can_calc_bittiming can_calc_bittiming_v4_8 +#define can_fixup_bittiming can_fixup_bittiming_v4_8 +#include "can-calc-bit-timing-v4_8.c" +#undef can_update_sample_point +#undef can_calc_bittiming +#undef can_fixup_bittiming + +#pragma GCC diagnostic pop + +#define can_update_sample_point can_update_sample_point_v5_16 +#define can_calc_bittiming can_calc_bittiming_v5_16 +#define can_fixup_bittiming can_fixup_bittiming_v5_16 +#include "can-calc-bit-timing-v5_16.c" +#undef can_update_sample_point +#undef can_calc_bittiming +#undef can_fixup_bittiming + +#define can_update_sample_point can_update_sample_point_v5_19 +#define can_calc_bittiming can_calc_bittiming_v5_19 +#define can_fixup_bittiming can_fixup_bittiming_v5_19 +#include "can-calc-bit-timing-v5_19.c" +#undef can_update_sample_point +#undef can_calc_bittiming +#undef can_fixup_bittiming + +#define can_update_sample_point can_update_sample_point_v6_3 +#define can_calc_bittiming can_calc_bittiming_v6_3 +#define can_fixup_bittiming can_fixup_bittiming_v6_3 +#include "can-calc-bit-timing-v6_3.c" +#undef can_update_sample_point +#undef can_calc_bittiming +#undef can_fixup_bittiming + +static const struct alg alg_list[] = { + /* 1st will be default */ + { + .calc_bittiming_extack = can_calc_bittiming_v6_3, + .fixup_bittiming_extack = can_fixup_bittiming_v6_3, + .name = "v6.3", + }, { + .calc_bittiming_const = can_calc_bittiming_v5_19, + .fixup_bittiming_const = can_fixup_bittiming_v5_19, + .name = "v5.19", + }, { + .calc_bittiming = can_calc_bittiming_v5_16, + .fixup_bittiming = can_fixup_bittiming_v5_16, + .name = "v5.16", + }, { + .calc_bittiming = can_calc_bittiming_v4_8, + .fixup_bittiming = can_fixup_bittiming_v4_8, + .name = "v4.8", + }, { + .calc_bittiming = can_calc_bittiming_v3_18, + .fixup_bittiming = can_fixup_bittiming_v3_18, + .name = "v3.18", + }, { + .calc_bittiming = can_calc_bittiming_v2_6_31, + .fixup_bittiming = can_fixup_bittiming_v2_6_31, + .name = "v2.6.31", + }, +}; + +static __u32 get_cia_sample_point(__u32 bitrate) +{ + __u32 sampl_pt; + + if (bitrate > 800000) + sampl_pt = 750; + else if (bitrate > 500000) + sampl_pt = 800; + else + sampl_pt = 875; + + return sampl_pt; +} + +static void print_bittiming_one(const struct alg *alg, + const struct can_bittiming_const *bittiming_const, + const struct can_bittiming *ref_bt, + const struct calc_ref_clk *ref_clk, + unsigned int bitrate_nominal, + unsigned int sample_point_nominal, + void (*printf_btr)(struct can_bittiming *bt, bool hdr), + bool quiet, + bool verbose, + bool fd_mode) +{ + struct net_device dev = { + .priv.clock.freq = ref_clk->clk, + }; + struct can_bittiming bt = { + .bitrate = bitrate_nominal, + .sample_point = sample_point_nominal, + }; + unsigned int bitrate_error, sample_point_error; + + if (!quiet) { + printf("%sBit timing parameters for %s with %.6f MHz ref clock %s%s%susing algo '%s'\n", + fd_mode ? "Data " : "", + bittiming_const->name, + ref_clk->clk / 1000000.0, + ref_clk->name ? "(" : "", + ref_clk->name ? ref_clk->name : "", + ref_clk->name ? ") " : "", + alg->name); + if (verbose) { + printf(" _----+--------------=> TSeg1: %u … %4u\n" + " / / _---------=> TSeg2: %u … %4u\n" + " | | / _-----=> SJW: %u … %4u\n" + " | | | / _-=> BRP: %u … %4u (inc: %u)\n" + " | | | | /\n", + bittiming_const->tseg1_min, bittiming_const->tseg1_max, + bittiming_const->tseg2_min, bittiming_const->tseg2_max, + 1, bittiming_const->sjw_max, + bittiming_const->brp_min, bittiming_const->brp_max, bittiming_const->brp_inc); + printf(" nominal | | | | | real Bitrt nom real SampP\n"); + } else { + printf(" nominal real Bitrt nom real SampP\n"); + } + printf(" Bitrate TQ[ns] PrS PhS1 PhS2 SJW BRP Bitrate Error SampP SampP Error "); + + printf_btr(&bt, true); + printf("\n"); + } + + if (ref_bt) { + bt = *ref_bt; + + if (alg->fixup_bittiming(&dev, &bt, bittiming_const)) { + printf("%8d ***parameters exceed controller's range***\n", bitrate_nominal); + return; + } + } else { + if (alg->calc_bittiming(&dev, &bt, bittiming_const)) { + printf("%8d ***bitrate not possible***\n", bitrate_nominal); + return; + } + } + + bitrate_error = abs(bitrate_nominal - bt.bitrate); + sample_point_error = abs(sample_point_nominal - bt.sample_point); + + printf("%8d " /* Bitrate */ + "%6d %3d %4d %4d " /* TQ[ns], PrS, PhS1, PhS2 */ + "%3d %3d " /* SJW, BRP */ + "%8d ", /* real Bitrate */ + bitrate_nominal, + bt.tq, bt.prop_seg, bt.phase_seg1, bt.phase_seg2, + bt.sjw, bt.brp, + bt.bitrate); + + if (100.0 * bitrate_error / bitrate_nominal > 99.9) + printf("≥100%% "); + else + printf("%4.1f%% ", /* Bitrate Error */ + 100.0 * bitrate_error / bitrate_nominal); + + printf("%4.1f%% %4.1f%% ", /* nom Sample Point, real Sample Point */ + sample_point_nominal / 10.0, + bt.sample_point / 10.0); + + if (100.0 * sample_point_error / sample_point_nominal > 99.9) + printf("≥100%% "); + else + printf("%4.1f%% ", /* Sample Point Error */ + 100.0 * sample_point_error / sample_point_nominal); + + printf_btr(&bt, false); + printf("\n"); +} + +static void print_bittiming(const struct calc_data *data) +{ + const struct calc_ref_clk *ref_clks = data->ref_clks; + + if (!ref_clks->clk && !data->quiet) + printf("Skipping bit timing parameter calculation for %s, no ref clock defined\n\n", + data->bittiming_const->name); + + while (ref_clks->clk) { + void (*printf_btr)(struct can_bittiming *bt, bool hdr); + unsigned int const *bitrates = data->bitrates; + bool quiet = data->quiet; + bool verbose = data->verbose; + + if (data->printf_btr) + printf_btr = data->printf_btr; + else + printf_btr = printf_btr_nop; + + while (*bitrates) { + unsigned int sample_point; + + /* get nominal sample point */ + if (data->sample_point) + sample_point = data->sample_point; + else + sample_point = get_cia_sample_point(*bitrates); + + print_bittiming_one(data->alg, + data->bittiming_const, + data->opt_bt, + ref_clks, + *bitrates, + sample_point, + printf_btr, + quiet, + verbose, + data->fd_mode); + bitrates++; + quiet = true; + } + + printf("\n"); + ref_clks++; + } +} + +static void do_list_calc_bittiming_list(void) +{ + unsigned int i; + + for (i = 0; i < ARRAY_SIZE(alg_list); i++) + printf(" %s\n", alg_list[i].name); +} + +static void do_list(void) +{ + unsigned int i; + + for (i = 0; i < ARRAY_SIZE(can_calc_consts); i++) + printf("%s\n", can_calc_consts[i].bittiming_const.name); +} + +static void do_calc(struct calc_data *data) +{ + unsigned int i; + bool found = false; + + for (i = 0; i < ARRAY_SIZE(can_calc_consts); i++) { + const struct calc_bittiming_const *btc; + + btc = &can_calc_consts[i]; + + if (data->name && + strcmp(data->name, btc->bittiming_const.name) && + strcmp(data->name, btc->data_bittiming_const.name)) + continue; + + found = true; + + if (btc->bittiming_const.name[0]) { + data->bittiming_const = &btc->bittiming_const; + data->printf_btr = btc->printf_btr; + + if (data->opt_ref_clk) + data->ref_clks = data->opt_ref_clk; + else + data->ref_clks = btc->ref_clk; + + if (data->opt_bitrates) + data->bitrates = data->opt_bitrates; + else + data->bitrates = common_bitrates; + + data->fd_mode = false; + + print_bittiming(data); + } + + if (btc->data_bittiming_const.name[0]) { + data->bittiming_const = &btc->data_bittiming_const; + + if (btc->printf_data_btr) + data->printf_btr = btc->printf_data_btr; + else + data->printf_btr = btc->printf_btr; + + if (data->opt_ref_clk) + data->ref_clks = data->opt_ref_clk; + else + data->ref_clks = btc->ref_clk; + + if (data->opt_data_bitrates) + data->bitrates = data->opt_data_bitrates; + else if (data->opt_bitrates) + data->bitrates = data->opt_bitrates; + else + data->bitrates = common_data_bitrates; + + data->fd_mode = true; + + print_bittiming(data); + } + } + + if (!found) { + printf("error: unknown CAN controller '%s', try one of these:\n\n", data->name); + do_list(); + exit(EXIT_FAILURE); + } +} + +int main(int argc, char *argv[]) +{ + struct calc_ref_clk opt_ref_clk[] = { + { .name = "cmd-line" }, + { /* sentinel */ } + }; + struct can_bittiming opt_bt[1] = { }; + unsigned int opt_bitrate[] = { + 0, + 0 /* sentinel */ + }; + unsigned int opt_data_bitrate[] = { + 0, + 0 /* sentinel */ + }; + struct calc_data data[] = { + { + .alg = alg_list, + } + }; + const char *opt_alg_name = NULL; + bool list = false; + int opt; + + const struct option long_options[] = { + { "tq", required_argument, 0, OPT_TQ, }, + { "prop-seg", required_argument, 0, OPT_PROP_SEG, }, + { "phase-seg1", required_argument, 0, OPT_PHASE_SEG1, }, + { "phase-seg2", required_argument, 0, OPT_PHASE_SEG2, }, + { "sjw", required_argument, 0, OPT_SJW, }, + { "brp", required_argument, 0, OPT_BRP, }, + { "tseg1", required_argument, 0, OPT_TSEG1, }, + { "tseg2", required_argument, 0, OPT_TSEG2, }, + { "alg", optional_argument, 0, OPT_ALG, }, + { 0, 0, 0, 0 }, + }; + + while ((opt = getopt_long(argc, argv, "b:c:d:lqs:v?", long_options, NULL)) != -1) { + switch (opt) { + case 'b': + opt_bitrate[0] = strtoul(optarg, NULL, 10); + break; + + case 'c': + opt_ref_clk->clk = strtoul(optarg, NULL, 10); + break; + + case 'd': + opt_data_bitrate[0] = strtoul(optarg, NULL, 10); + break; + + case 'l': + list = true; + break; + + case 'q': + data->quiet = true; + break; + + case 's': + data->sample_point = strtoul(optarg, NULL, 10); + break; + + case 'v': + data->verbose = true; + break; + + case OPT_TQ: + opt_bt->tq = strtoul(optarg, NULL, 10); + break; + + case OPT_PROP_SEG: + opt_bt->prop_seg = strtoul(optarg, NULL, 10); + break; + + case OPT_PHASE_SEG1: + opt_bt->phase_seg1 = strtoul(optarg, NULL, 10); + break; + + case OPT_PHASE_SEG2: + opt_bt->phase_seg2 = strtoul(optarg, NULL, 10); + break; + + case OPT_SJW: + opt_bt->sjw = strtoul(optarg, NULL, 10); + break; + + case OPT_BRP: + opt_bt->brp = strtoul(optarg, NULL, 10); + break; + + case OPT_TSEG1: { + __u32 tseg1; + + tseg1 = strtoul(optarg, NULL, 10); + opt_bt->prop_seg = tseg1 / 2; + opt_bt->phase_seg1 = tseg1 - opt_bt->prop_seg; + break; + } + + case OPT_TSEG2: + opt_bt->phase_seg2 = strtoul(optarg, NULL, 10); + break; + + case OPT_ALG: + if (!optarg) { + printf("Supported CAN calc bit timing algorithms:\n\n"); + do_list_calc_bittiming_list(); + printf("\n"); + exit(EXIT_SUCCESS); + } + opt_alg_name = optarg; + break; + + default: + print_usage(basename(argv[0])); + exit(EXIT_FAILURE); + break; + } + } + + if (argc > optind + 1) { + print_usage(argv[0]); + exit(EXIT_FAILURE); + } + + if (argc == optind + 1) + data->name = argv[optind]; + + if (list) { + do_list(); + exit(EXIT_SUCCESS); + } + + if (data->sample_point && (data->sample_point >= 1000 || data->sample_point < 100)) + print_usage(argv[0]); + + if (opt_alg_name) { + bool alg_found = false; + unsigned int i; + + for (i = 0; i < ARRAY_SIZE(alg_list); i++) { + if (!strcmp(opt_alg_name, alg_list[i].name)) { + data->alg = &alg_list[i]; + alg_found = true; + } + } + + if (!alg_found) { + printf("error: unknown CAN calc bit timing algorithm '%s', try one of these:\n\n", opt_alg_name); + do_list_calc_bittiming_list(); + exit(EXIT_FAILURE); + } + } + + if (opt_ref_clk->clk) + data->opt_ref_clk = opt_ref_clk; + if (opt_bitrate[0]) + data->opt_bitrates = opt_bitrate; + if (opt_data_bitrate[0]) + data->opt_data_bitrates = opt_data_bitrate; + if (opt_bt->prop_seg) + data->opt_bt = opt_bt; + + do_calc(data); + + exit(EXIT_SUCCESS); +} diff --git a/Devices/Libraries/Systems/can-utils/calc-bit-timing/compat.h b/Devices/Libraries/Systems/can-utils/calc-bit-timing/compat.h new file mode 100755 index 0000000..2186ac9 --- /dev/null +++ b/Devices/Libraries/Systems/can-utils/calc-bit-timing/compat.h @@ -0,0 +1,163 @@ +/* SPDX-License-Identifier: GPL-2.0 */ + +#ifndef _COMPAT_H +#define _COMPAT_H + +#include +#include +#include +#include + +#include +#include + +/* imported from kernel */ + +/* define in-kernel-types */ +typedef __u64 u64; +typedef __u32 u32; + +#define NSEC_PER_SEC 1000000000L + +#define CAN_CALC_MAX_ERROR 50 /* in one-tenth of a percent */ +#define CAN_CALC_SYNC_SEG 1 +#define CAN_SYNC_SEG 1 +#define CAN_KBPS 1000 +#define KILO 1000UL + +/** + * abs - return absolute value of an argument + * @x: the value. If it is unsigned type, it is converted to signed type first. + * char is treated as if it was signed (regardless of whether it really is) + * but the macro's return type is preserved as char. + * + * Return: an absolute value of x. + */ +#define abs(x) __abs_choose_expr(x, long long, \ + __abs_choose_expr(x, long, \ + __abs_choose_expr(x, int, \ + __abs_choose_expr(x, short, \ + __abs_choose_expr(x, char, \ + __builtin_choose_expr( \ + __builtin_types_compatible_p(typeof(x), char), \ + (char)({ signed char __x = (x); __x < 0 ? -__x:__x; }), \ + ((void)0))))))) + +#define __abs_choose_expr(x, type, other) __builtin_choose_expr( \ + __builtin_types_compatible_p(typeof(x), signed type) || \ + __builtin_types_compatible_p(typeof(x), unsigned type), \ + ({ signed type __x = (x); __x < 0 ? -__x : __x; }), other) + +/* + * min()/max()/clamp() macros that also do + * strict type-checking.. See the + * "unnecessary" pointer comparison. + */ +#define min(x, y) ({ \ + typeof(x) _min1 = (x); \ + typeof(y) _min2 = (y); \ + (void) (&_min1 == &_min2); \ + _min1 < _min2 ? _min1 : _min2; }) + +#define max(x, y) ({ \ + typeof(x) _max1 = (x); \ + typeof(y) _max2 = (y); \ + (void) (&_max1 == &_max2); \ + _max1 > _max2 ? _max1 : _max2; }) + +/** + * clamp - return a value clamped to a given range with strict typechecking + * @val: current value + * @lo: lowest allowable value + * @hi: highest allowable value + * + * This macro does strict typechecking of lo/hi to make sure they are of the + * same type as val. See the unnecessary pointer comparisons. + */ +#define clamp(val, lo, hi) min((typeof(val))max(val, lo), hi) + +#define do_div(n, base) ({ \ + uint32_t __base = (base); \ + uint32_t __rem; \ + __rem = ((uint64_t)(n)) % __base; \ + (n) = ((uint64_t)(n)) / __base; \ + __rem; \ +}) + + +/** + * DIV_U64_ROUND_CLOSEST - unsigned 64bit divide with 32bit divisor rounded to nearest integer + * @dividend: unsigned 64bit dividend + * @divisor: unsigned 32bit divisor + * + * Divide unsigned 64bit dividend by unsigned 32bit divisor + * and round to closest integer. + * + * Return: dividend / divisor rounded to nearest integer + */ +#define DIV_U64_ROUND_CLOSEST(dividend, divisor) \ + ({ u32 _tmp = (divisor); div_u64((u64)(dividend) + _tmp / 2, _tmp); }) + +/** + * div_u64_rem - unsigned 64bit divide with 32bit divisor with remainder + * @dividend: unsigned 64bit dividend + * @divisor: unsigned 32bit divisor + * @remainder: pointer to unsigned 32bit remainder + * + * Return: sets ``*remainder``, then returns dividend / divisor + * + * This is commonly provided by 32bit archs to provide an optimized 64bit + * divide. + */ +static inline u64 div_u64_rem(u64 dividend, u32 divisor, u32 *remainder) +{ + *remainder = dividend % divisor; + return dividend / divisor; +} + +static inline u64 div_u64(u64 dividend, u32 divisor) +{ + u32 remainder; + return div_u64_rem(dividend, divisor, &remainder); +} + +static inline u64 mul_u32_u32(u32 a, u32 b) +{ + return (u64)a * b; +} + +/* */ + +#define ARRAY_SIZE(arr) (sizeof(arr) / sizeof((arr)[0])) + +/* we don't want to see these prints */ +#define netdev_err(dev, format, arg...) do { } while (0) +#define netdev_warn(dev, format, arg...) do { } while (0) +#define NL_SET_ERR_MSG_FMT(dev, format, arg...) do { } while (0) + +struct calc_ref_clk { + __u32 clk; /* CAN system clock frequency in Hz */ + const char *name; +}; + +/* + * minimal structs, just enough to be source level compatible + */ +struct can_priv { + struct can_clock clock; +}; + +struct net_device { + struct can_priv priv; +}; + +struct netlink_ext_ack { + u32 __dummy; +}; + +static inline void *netdev_priv(const struct net_device *dev) +{ + return (void *)&dev->priv; +} + +#endif /*_COMPAT_H */ diff --git a/Devices/Libraries/Systems/can-utils/can-j1939-install-kernel-module.md b/Devices/Libraries/Systems/can-utils/can-j1939-install-kernel-module.md new file mode 100755 index 0000000..7de99a3 --- /dev/null +++ b/Devices/Libraries/Systems/can-utils/can-j1939-install-kernel-module.md @@ -0,0 +1,180 @@ +# can-j1939 kernel module installation # + + + +### Problem + +You already have **can0** or **vcan0** up and working, **can-utils** downloaded and compiled to **~/can/can-utils** and you can send and receive frames without problems. However, when you want to bring up **can-j1939** you get error like this: + +```bash +avra@vm-debian:~/can/can-utils$ sudo modprobe can-j1939 +modprobe: FATAL: Module can-j1939 not found in directory /lib/modules/5.7.0.0.bpo.2-amd64 +``` + +and also this: + +```bash +avra@vm-debian:~/can/can-utils$ testj1939 +testj1939: socket(j1939): Protocol not supported +``` + + + +### Solution + +Above errors mean that **can-j1939** was not enabled in your kernel and you need to compile it manually. There are several ways to do it. Any Linux kernel since 5.4 has **can-j1939** module, but you will probably want to install fresher version, which leads to downloading kernel sources, enabling **can-j1939** module, recompiling kernel and installing it. I will be using Debian 10.5 x64 (buster testing) virtual machine. + + + +#### 1. Download kernel source #### + +We will download Debian patched kernel 5.8. First update your sources + +``` +avra@vm-debian:~$ sudo apt update +``` + +and then look at available Debian patched kernel source packages + +``` +avra@vm-debian:~$ apt-cache search linux-source +linux-source-4.19 - Linux kernel source for version 4.19 with Debian patches +linux-source - Linux kernel source (meta-package) +linux-source-5.4 - Linux kernel source for version 5.4 with Debian patches +linux-source-5.5 - Linux kernel source for version 5.5 with Debian patches +linux-source-5.6 - Linux kernel source for version 5.6 with Debian patches +linux-source-5.7 - Linux kernel source for version 5.7 with Debian patches +linux-source-5.8 - Linux kernel source for version 5.8 with Debian patches +``` + +If kernel 5.8 does not show in your linux-sources list (it shows above in mine since I have already upgraded stock 4.19 kernel to backported 5.7), then you will need to add backports to your sources list. It is best to do it like this + +``` +echo 'deb http://deb.debian.org/debian buster-backports main contrib' | sudo tee -a /etc/apt/sources.list.d/debian-backports.list +``` + +Alternatively, or in case you have problems with installation of some packages, or you just want to have everything in a single list, here is what my **/etc/apt/sources.list** looks like (you will need to append at least last line to yours) + +``` +deb http://security.debian.org/debian-security buster/updates main contrib +deb-src http://security.debian.org/debian-security buster/updates main contrib + +deb http://deb.debian.org/debian/ buster main contrib non-free +deb-src http://deb.debian.org/debian/ buster main contrib non-free + +deb http://deb.debian.org/debian buster-backports main contrib +``` + +After adding backports in one way or another, try **sudo apt update** again, and after that **apt-cache search linux-source** should show kernel 5.8 in the list, so you can install it's source package + +``` +sudo apt install linux-source-5.8 +``` + +and unpack it +``` +avra@vm-debian:~$ cd /usr/src +avra@vm-debian:/usr/src$ sudo tar -xaf linux-source-5.8.tar.xz +avra@vm-debian:/usr/src$ cd linux-source-5.8 +``` + + + +#### 2. Add can-j1939 module to kernel #### + +First we need some packages for **menuconfig** + +``` +sudo apt-get install libncurses5 libncurses5-dev +``` + +copy and use our old configuration to run **menuconfig** + +``` +avra@vm-debian:/usr/src/linux-source-5.8$ sudo cp /boot/config-$(uname -r) .config +avra@vm-debian:/usr/src/linux-source-5.8$ sudo make menuconfig +``` + +where we enable SAE J1939 kernel module as shown + +``` + - Networking Support + - Can bus subsystem support + - SAE J1939 +``` + +Now edit **/usr/src/linux-source-5.8/.config**, find CONFIG_SYSTEM_TRUSTED_KEYS, change it as following +``` +CONFIG_SYSTEM_TRUSTED_KEYS="" +``` + +and save the file. + + + +#### 3. Compile and install kernel and modules + +We will have to download necessary packages + +``` +sudo apt install build-essential libssl-dev libelf-dev bison flex +``` + +compile kernel (using threads to make it faster) + +``` +avra@vm-debian:/usr/src/linux-source-5.8$ sudo make -j $(nproc) +``` + +install + +``` +avra@vm-debian:/usr/src/linux-source-5.8$ sudo make modules_install +avra@vm-debian:/usr/src/linux-source-5.8$ sudo make install +``` + +and update grub + +``` +avra@vm-debian:/usr/src/linux-source-5.8$ sudo update-grub +avra@vm-debian:/usr/src/linux-source-5.8$ sudo reboot +``` + +Check if installation is correct with + +``` +sudo modprobe can-j1939 +``` + +and if you get no error then you can enjoy **can-j1939**. If you get some error then you might check if this alternative command works: + +``` +sudo insmod /lib/modules/5.8.10/kernel/net/can/j1939/can-j1939.ko +``` + +If it does then all you need to do is + +``` +sudo depmod -av +``` + +reboot once, and **modprobe** command from the above should finally work. + + + +#### 4. Install headers if needed + +You might have a problem with headers not being updated. To check that open file **/usr/include/linux/can.h** with + +``` +nano /usr/include/linux/can.h +``` + +If in the struct **sockaddr_can** you don’t see **j1939**, then header files did not upgrade and you need to do it manually + +``` +sudo cp /usr/src/linux-source-5.8/include/uapi/linux/can.h /usr/include/linux/can.h +sudo cp /usr/src/linux-source-5.8/include/uapi/linux/can/j1939.h /usr/include/linux/can/ +``` + +That is the minimum for compiling some **J1939** C code, but you might want to upgrade other header files as well. That's up to you. Enjoy! diff --git a/Devices/Libraries/Systems/can-utils/can-j1939-kickstart.md b/Devices/Libraries/Systems/can-utils/can-j1939-kickstart.md new file mode 100755 index 0000000..721656a --- /dev/null +++ b/Devices/Libraries/Systems/can-utils/can-j1939-kickstart.md @@ -0,0 +1,197 @@ +# Kickstart guide to can-j1939 on linux + +## Prepare using VCAN + +You may skip this step entirely if you have a functional +**can0** bus on your system. + +Load module, when *vcan* is not in-kernel + + modprobe vcan + +Create a virtual can0 device and start the device + + ip link add can0 type vcan + ip link set can0 up + +## First steps with j1939 + +Use [testj1939](testj1939.c) + +When *can-j1939* is compiled as module, opening a socket will load it, +__or__ you can load it manually + + modprobe can-j1939 + +Most of the subsequent examples will use 2 sockets programs (in 2 terminals). +One will use CAN_J1939 sockets using *testj1939*, +and the other will use CAN_RAW sockets using cansend+candump. + +testj1939 can be told to print the used API calls by adding **-v** program argument. + +### receive without source address + +Do in terminal 1 + + testj1939 -B -r can0 + +Send raw CAN in terminal 2 + + cansend can0 1823ff40#0123 + +You should have this output in terminal 1 + + 40 02300: 01 23 + +This means, from NAME 0, SA 40, PGN 02300 was received, +with 2 databytes, *01* & *23*. + +now emit this CAN message: + + cansend can0 18234140#0123 + +In J1939, this means that ECU 0x40 sends directly to ECU 0x41 +Since we did not bind to address 0x41, this traffic +is not meant for us and *testj1939* does not receive it. + +### receive with source address + +Terminal 1: + + testj1939 -r can0:0x80 + +Terminal 2: + + cansend can0 18238040#0123 + +Will emit this output + + 40 02300: 01 23 + +This is because the traffic had destination address __0x80__ . + +### send + +Open in terminal 1: + + candump -L can0 + +And to these test in another terminal + + testj1939 -B -s can0:0x80 can0:,0x3ffff + +This produces **1BFFFF80#0123456789ABCDEF** on CAN. + +Note: To be able to send a broadcast we need to use, we need to use "-B" flag. + +### Multiple source addresses on 1 CAN device + + testj1939 -B -s can0:0x90 can0:,0x3ffff + +produces **1BFFFF90#0123456789ABCDEF** , + +### Use PDU1 PGN + + testj1939 -B -s can0:0x80 can0:,0x12300 + +emits **1923FF80#0123456789ABCDEF** . + +Note that the PGN is **0x12300**, and destination address is **0xff**. + +### Use destination address info + +Since in this example we use unicast source and destination addresses, we do +not need to use "-B" (broadcast) flag. + +The destination field may be set during sendto(). +*testj1939* implements that like this + + testj1939 -s can0:0x80 can0:0x40,0x12300 + +emits **19234080#0123456789ABCDEF** . + +The destination CAN iface __must__ always match the source CAN iface. +Specifying one during bind is therefore sufficient. + + testj1939 -s can0:0x80 :0x40,0x12300 + +emits the very same. + +### Emit different PGNs using the same socket + +The PGN is provided in both __bind( *sockname* )__ and +__sendto( *peername* )__ , and only one is used. +*peername* PGN has highest precedence. + +For broadcasted transmissions + + testj1939 -B -s can0:0x80 :,0x32100 + +emits **1B21FF80#0123456789ABCDEF** + +Destination specific transmissions + + testj1939 -s can0:0x80,0x12300 :0x40,0x32100 + +emits **1B214080#0123456789ABCDEF** . + +It makes sometimes sense to omit the PGN in __bind( *sockname* )__ . + +### Larger packets + +J1939 transparently switches to *Transport Protocol* when packets +do not fit into single CAN packets. + + testj1939 -B -s20 can0:0x80 :,0x12300 + +emits: + + 18ECFF80#20140003FF002301 + 18EBFF80#010123456789ABCD + 18EBFF80#02EF0123456789AB + 18EBFF80#03CDEF01234567FF + +The fragments for broadcasted *Transport Protocol* are separated +__50ms__ from each other. +Destination specific *Transport Protocol* applies flow control +and may emit CAN packets much faster. + +First assign 0x90 to the local system. +This becomes important because the kernel must interact in the +transport protocol sessions before the complete packet is delivered. + + testj1939 can0:0x90 -r & + +Now test: + + testj1939 -s20 can0:0x80 :0x90,0x12300 + +emits: + + 18EC9080#1014000303002301 + 18EC8090#110301FFFF002301 + 18EB9080#010123456789ABCD + 18EB9080#02EF0123456789AB + 18EB9080#03CDEF01234567FF + 18EC8090#13140003FF002301 + +The flow control causes a bit overhead. +This overhead scales very good for larger J1939 packets. + +## Advanced topics with j1939 + +### Change priority of J1939 packets + + testj1939 -B -s can0:0x80 :,0x0100 + testj1939 -B -s -p3 can0:0x80 :,0x0200 + +emits + + 1801FF80#0123456789ABCDEF + 0C02FF80#0123456789ABCDEF + +### using connect + +### advanced filtering + +## dynamic addressing diff --git a/Devices/Libraries/Systems/can-utils/can-j1939.md b/Devices/Libraries/Systems/can-utils/can-j1939.md new file mode 100755 index 0000000..4b97085 --- /dev/null +++ b/Devices/Libraries/Systems/can-utils/can-j1939.md @@ -0,0 +1,98 @@ +# CAN-J1939 on linux + +The [Kickstart guide is here](can-j1939-kickstart.md) + +## CAN on linux + +See [Wikipedia:socketcan](http://en.wikipedia.org/wiki/Socketcan) + +## J1939 networking in short + +* Add addressing on top of CAN (destination address & broadcast) + +* Any (max 1780) length packets. + Packets of 9 or more use **Transport Protocol** (fragmentation) + Such packets use different CANid for the same PGN. + +* only **29**bit, non-**RTR** CAN frames + +* CAN id is composed of + * 0..8: SA (source address) + * 9..26: + * PDU1: PGN+DA (destination address) + * PDU2: PGN + * 27..29: PRIO + +* SA / DA may be dynamically assigned via j1939-81 + Fixed rules of precedence in Specification, no master necessary + +## J1939 on SocketCAN + +J1939 is *just another protocol* that fits +in the Berkely sockets. + + socket(AF_CAN, SOCK_DGRAM, CAN_J1939) + +## differences from CAN_RAW +### addressing + +SA, DA & PGN are used, not CAN id. + +Berkeley socket API is used to communicate these to userspace: + + * SA+PGN is put in sockname ([getsockname](http://man7.org/linux/man-pages/man2/getsockname.2.html)) + * DA+PGN is put in peername ([getpeername](http://man7.org/linux/man-pages/man2/getpeername.2.html)) + PGN is put in both structs + +PRIO is a datalink property, and irrelevant for interpretation +Therefore, PRIO is not in *sockname* or *peername*. + +The *data* that is [recv][recvfrom] or [send][sendto] is the real payload. +Unlike CAN_RAW, where addressing info is data. + +### Packet size + +J1939 handles packets of 8+ bytes with **Transport Protocol** fragmentation transparently. +No fixed data size is necessary. + + send(sock, data, 8, 0); + +will emit a single CAN frame. + + send(sock, data, 9, 0); + +will use fragmentation, emitting 1+ CAN frames. + +# Using J1939 + +## BSD socket implementation +* socket +* bind / connect +* recvfrom / sendto +* getsockname / getpeername + +## Modified *struct sockaddr_can* + + struct sockaddr_can { + sa_family_t can_family; + int can_ifindex; + union { + struct { + __u64 name; + __u32 pgn; + __u8 addr; + } j1939; + } can_addr; + } + +* *can_addr.j1939.pgn* is PGN + +* *can_addr.j1939.addr* & *can_addr.j1939.name* + determine the ECU + + * receiving address information, + *addr* is always set, + *name* is set when available. + + * When providing address information, + *name* != 0 indicates dynamic addressing diff --git a/Devices/Libraries/Systems/can-utils/can-tc-init-etf.sh b/Devices/Libraries/Systems/can-utils/can-tc-init-etf.sh new file mode 100755 index 0000000..0c35983 --- /dev/null +++ b/Devices/Libraries/Systems/can-utils/can-tc-init-etf.sh @@ -0,0 +1,59 @@ +#!/bin/bash +# SPDX-License-Identifier: GPL-2.0-only +# Copyright (C) 2022 Pengutronix, Marc Kleine-Budde +# +# This script requires a kernel compiled with the following options: +# +# CONFIG_NET_SCH_PRIO +# CONFIG_NET_SCH_ETF +# CONFIG_NET_CLS_BASIC +# CONFIG_NET_CLS_FW +# CONFIG_NET_EMATCH +# CONFIG_NET_EMATCH_CANID +# + +set -e + +IFACE=${1:-can0} +MARK=${2:-1} + +clear() { + tc -batch - < + * + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "lib.h" +#include "terminal.h" +#include "canframelen.h" + +#define MAXSOCK 16 /* max. number of CAN interfaces given on the cmdline */ + +#define PERCENTRES 5 /* resolution in percent for bargraph */ +#define NUMBAR (100 / PERCENTRES) /* number of bargraph elements */ + +extern int optind, opterr, optopt; + +static struct { + char devname[IFNAMSIZ + 1]; + unsigned int bitrate; + unsigned int dbitrate; + unsigned int recv_frames; + unsigned int recv_bits_total; + unsigned int recv_bits_payload; + unsigned int recv_bits_dbitrate; +} stat[MAXSOCK + 1]; + +static volatile int running = 1; +static volatile sig_atomic_t signal_num; +static int max_devname_len; /* to prevent frazzled device name output */ +static int max_bitrate_len; +static int currmax; +static unsigned char redraw; +static unsigned char timestamp; +static unsigned char color; +static unsigned char bargraph; +static enum cfl_mode mode = CFL_WORSTCASE; +static char *prg; + +static void print_usage(char *prg) +{ + fprintf(stderr, "%s - monitor CAN bus load.\n", prg); + fprintf(stderr, "\nUsage: %s [options] +\n", prg); + fprintf(stderr, " (use CTRL-C to terminate %s)\n\n", prg); + fprintf(stderr, "Options:\n"); + fprintf(stderr, " -t (show current time on the first line)\n"); + fprintf(stderr, " -c (colorize lines)\n"); + fprintf(stderr, " -b (show bargraph in %d%% resolution)\n", PERCENTRES); + fprintf(stderr, " -r (redraw the terminal - similar to top)\n"); + fprintf(stderr, " -i (ignore bitstuffing in bandwidth calculation)\n"); + fprintf(stderr, " -e (exact calculation of stuffed bits)\n"); + fprintf(stderr, "\n"); + fprintf(stderr, "Up to %d CAN interfaces with mandatory bitrate can be specified on the \n", MAXSOCK); + fprintf(stderr, "commandline in the form: @[,]\n\n"); + fprintf(stderr, "The bitrate is mandatory as it is needed to know the CAN bus bitrate to\n"); + fprintf(stderr, "calculate the bus load percentage based on the received CAN frames.\n"); + fprintf(stderr, "Due to the bitstuffing estimation the calculated busload may exceed 100%%.\n"); + fprintf(stderr, "For each given interface the data is presented in one line which contains:\n\n"); + fprintf(stderr, "(interface) (received CAN frames) (used bits total) (used bits for payload)\n"); + fprintf(stderr, "\nExamples:\n"); + fprintf(stderr, "\nuser$> canbusload can0@100000 can1@500000 can2@500000 can3@500000 -r -t -b -c\n\n"); + fprintf(stderr, "%s 2014-02-01 21:13:16 (worst case bitstuffing)\n", prg); + fprintf(stderr, " can0@100000 805 74491 36656 74%% |XXXXXXXXXXXXXX......|\n"); + fprintf(stderr, " can1@500000 796 75140 37728 15%% |XXX.................|\n"); + fprintf(stderr, " can2@500000 0 0 0 0%% |....................|\n"); + fprintf(stderr, " can3@500000 47 4633 2424 0%% |....................|\n"); + fprintf(stderr, "\n"); +} + +static void sigterm(int signo) +{ + running = 0; + signal_num = signo; +} + +static void printstats(int signo) +{ + int i, j, percent; + + if (redraw) + printf("%s", CSR_HOME); + + if (timestamp) { + time_t currtime; + struct tm now; + + if (time(&currtime) == (time_t)-1) { + perror("time"); + exit(1); + } + + localtime_r(&currtime, &now); + + printf("%s %04d-%02d-%02d %02d:%02d:%02d ", + prg, + now.tm_year + 1900, + now.tm_mon + 1, + now.tm_mday, + now.tm_hour, + now.tm_min, + now.tm_sec); + + switch (mode) { + + case CFL_NO_BITSTUFFING: + /* plain bit calculation without bitstuffing */ + printf("(ignore bitstuffing)\n"); + break; + + case CFL_WORSTCASE: + /* worst case estimation - see above */ + printf("(worst case bitstuffing)\n"); + break; + + case CFL_EXACT: + /* exact calculation of stuffed bits based on frame content and CRC */ + printf("(exact bitstuffing)\n"); + break; + + default: + printf("(unknown bitstuffing)\n"); + break; + } + } + + for (i = 0; i < currmax; i++) { + if (color) { + if (i % 2) + printf("%s", FGRED); + else + printf("%s", FGBLUE); + } + + if (stat[i].bitrate) + percent = ((stat[i].recv_bits_total - stat[i].recv_bits_dbitrate) * 100) / stat[i].bitrate + + (stat[i].recv_bits_dbitrate * 100) / stat[i].dbitrate; + else + percent = 0; + + printf(" %*s@%-*d %5d %7d %6d %6d %3d%%", + max_devname_len, stat[i].devname, + max_bitrate_len, stat[i].bitrate, + stat[i].recv_frames, + stat[i].recv_bits_total, + stat[i].recv_bits_payload, + stat[i].recv_bits_dbitrate, + percent); + + if (bargraph) { + + printf(" |"); + + if (percent > 100) + percent = 100; + + for (j = 0; j < NUMBAR; j++) { + if (j < percent / PERCENTRES) + printf("X"); + else + printf("."); + } + + printf("|"); + } + + if (color) + printf("%s", ATTRESET); + + if (!redraw || (i < currmax - 1)) + printf("\n"); + + stat[i].recv_frames = 0; + stat[i].recv_bits_total = 0; + stat[i].recv_bits_dbitrate = 0; + stat[i].recv_bits_payload = 0; + } + + if (!redraw) + printf("\n"); + + fflush(stdout); + + alarm(1); +} + +int main(int argc, char **argv) +{ + fd_set rdfs; + int s[MAXSOCK]; + + int opt; + char *ptr, *nptr; + struct sockaddr_can addr; + struct canfd_frame frame; + int nbytes, i; + struct ifreq ifr; + + signal(SIGTERM, sigterm); + signal(SIGHUP, sigterm); + signal(SIGINT, sigterm); + + signal(SIGALRM, printstats); + + prg = basename(argv[0]); + + while ((opt = getopt(argc, argv, "rtbcieh?")) != -1) { + switch (opt) { + case 'r': + redraw = 1; + break; + + case 't': + timestamp = 1; + break; + + case 'b': + bargraph = 1; + break; + + case 'c': + color = 1; + break; + + case 'i': + mode = CFL_NO_BITSTUFFING; + break; + + case 'e': + mode = CFL_EXACT; + break; + + default: + print_usage(prg); + exit(1); + break; + } + } + + if (optind == argc) { + print_usage(prg); + exit(0); + } + + currmax = argc - optind; /* find real number of CAN devices */ + + if (currmax > MAXSOCK) { + printf("More than %d CAN devices given on commandline!\n", MAXSOCK); + return 1; + } + + for (i = 0; i < currmax; i++) { + ptr = argv[optind + i]; + + nbytes = strlen(ptr); + if (nbytes >= (int)(IFNAMSIZ + sizeof("@1000000") + 1)) { + printf("name of CAN device '%s' is too long!\n", ptr); + return 1; + } + + pr_debug("open %d '%s'.\n", i, ptr); + + s[i] = socket(PF_CAN, SOCK_RAW, CAN_RAW); + if (s[i] < 0) { + perror("socket"); + return 1; + } + + nptr = strchr(ptr, '@'); + + if (!nptr) { + fprintf(stderr, "Specify CAN interfaces in the form @, e.g. can0@500000\n"); + print_usage(prg); + return 1; + } + + nbytes = nptr - ptr; /* interface name is up the first '@' */ + + if (nbytes >= (int)IFNAMSIZ) { + printf("name of CAN device '%s' is too long!\n", ptr); + return 1; + } + + strncpy(stat[i].devname, ptr, nbytes); + memset(&ifr.ifr_name, 0, sizeof(ifr.ifr_name)); + strncpy(ifr.ifr_name, ptr, nbytes); + + if (nbytes > max_devname_len) + max_devname_len = nbytes; /* for nice printing */ + + char *endp; + stat[i].bitrate = strtol(nptr + 1, &endp, 0); /* bitrate is placed behind the '@' */ + if (*endp == ',') + /* data bitrate is placed behind the ',' */ + stat[i].dbitrate = strtol(endp + 1, &endp, 0); + else + stat[i].dbitrate = stat[i].bitrate; + + if (!stat[i].bitrate || stat[i].bitrate > 1000000) { + printf("invalid bitrate for CAN device '%s'!\n", ptr); + return 1; + } + + nbytes = strlen(nptr + 1); + if (nbytes > max_bitrate_len) + max_bitrate_len = nbytes; /* for nice printing */ + + pr_debug("using interface name '%s'.\n", ifr.ifr_name); + + /* try to switch the socket into CAN FD mode */ + const int canfd_on = 1; + setsockopt(s[i], SOL_CAN_RAW, CAN_RAW_FD_FRAMES, &canfd_on, sizeof(canfd_on)); + + if (ioctl(s[i], SIOCGIFINDEX, &ifr) < 0) { + perror("SIOCGIFINDEX"); + exit(1); + } + + addr.can_family = AF_CAN; + addr.can_ifindex = ifr.ifr_ifindex; + + if (bind(s[i], (struct sockaddr *)&addr, sizeof(addr)) < 0) { + perror("bind"); + return 1; + } + } + + alarm(1); + + if (redraw) + printf("%s", CLR_SCREEN); + + while (running) { + FD_ZERO(&rdfs); + for (i = 0; i < currmax; i++) + FD_SET(s[i], &rdfs); + + if (select(s[currmax - 1] + 1, &rdfs, NULL, NULL, NULL) < 0) { + //perror("pselect"); + continue; + } + + for (i = 0; i < currmax; i++) { /* check all CAN RAW sockets */ + + if (FD_ISSET(s[i], &rdfs)) { + nbytes = read(s[i], &frame, sizeof(frame)); + + if (nbytes < 0) { + perror("read"); + return 1; + } + + if (nbytes < (int)sizeof(struct can_frame)) { + fprintf(stderr, "read: incomplete CAN frame\n"); + return 1; + } + + stat[i].recv_frames++; + stat[i].recv_bits_payload += frame.len * 8; + stat[i].recv_bits_dbitrate += can_frame_dbitrate_length( + &frame, mode, sizeof(frame)); + stat[i].recv_bits_total += can_frame_length(&frame, + mode, nbytes); + } + } + } + + for (i = 0; i < currmax; i++) + close(s[i]); + + if (signal_num) + return 128 + signal_num; + + return 0; +} diff --git a/Devices/Libraries/Systems/can-utils/candump.c b/Devices/Libraries/Systems/can-utils/candump.c new file mode 100755 index 0000000..11594d1 --- /dev/null +++ b/Devices/Libraries/Systems/can-utils/candump.c @@ -0,0 +1,906 @@ +/* SPDX-License-Identifier: (GPL-2.0-only OR BSD-3-Clause) */ +/* + * candump.c + * + * Copyright (c) 2002-2009 Volkswagen Group Electronic Research + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * 3. Neither the name of Volkswagen nor the names of its contributors + * may be used to endorse or promote products derived from this software + * without specific prior written permission. + * + * Alternatively, provided that this notice is retained in full, this + * software may be distributed under the terms of the GNU General + * Public License ("GPL") version 2, in which case the provisions of the + * GPL apply INSTEAD OF those given above. + * + * The provided data structures and external interfaces from this code + * are not restricted to be used by modules with a GPL compatible license. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT + * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY + * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH + * DAMAGE. + * + * Send feedback to + * + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "terminal.h" +#include "lib.h" + +/* for hardware timestamps - since Linux 2.6.30 */ +#ifndef SO_TIMESTAMPING +#define SO_TIMESTAMPING 37 +#endif + +#define TIMESTAMPSZ 50 /* string 'absolute with date' requires max 49 bytes */ + +#define MAXSOCK 16 /* max. number of CAN interfaces given on the cmdline */ +#define MAXIFNAMES 30 /* size of receive name index to omit ioctls */ +#define MAXCOL 6 /* number of different colors for colorized output */ +#define ANYDEV "any" /* name of interface to receive from any CAN interface */ +#define ANL "\r\n" /* newline in ASC mode */ + +#define SILENT_INI 42 /* detect user setting on commandline */ +#define SILENT_OFF 0 /* no silent mode */ +#define SILENT_ANI 1 /* silent mode with animation */ +#define SILENT_ON 2 /* silent mode (completely silent) */ + +#define BOLD ATTBOLD +#define RED (ATTBOLD FGRED) +#define GREEN (ATTBOLD FGGREEN) +#define YELLOW (ATTBOLD FGYELLOW) +#define BLUE (ATTBOLD FGBLUE) +#define MAGENTA (ATTBOLD FGMAGENTA) +#define CYAN (ATTBOLD FGCYAN) + +static const char col_on[MAXCOL][19] = { BLUE, RED, GREEN, BOLD, MAGENTA, CYAN }; +static const char col_off[] = ATTRESET; + +struct if_info { /* bundled information per open socket */ + int s; /* socket */ + char *cmdlinename; + __u32 dropcnt; + __u32 last_dropcnt; +}; +static struct if_info sock_info[MAXSOCK]; + +static char *progname; +static char devname[MAXIFNAMES][IFNAMSIZ + 1]; +static int dindex[MAXIFNAMES]; +static int max_devname_len; /* to prevent frazzled device name output */ +static const int canfx_on = 1; + +#define MAXANI 4 +static const char anichar[MAXANI] = { '|', '/', '-', '\\' }; +static const char extra_m_info[4][4] = { "- -", "B -", "- E", "B E" }; + +extern int optind, opterr, optopt; + +static volatile int running = 1; +static volatile sig_atomic_t signal_num; + +static void print_usage(void) +{ + fprintf(stderr, "%s - dump CAN bus traffic.\n", progname); + fprintf(stderr, "\nUsage: %s [options] +\n", progname); + fprintf(stderr, " (use CTRL-C to terminate %s)\n\n", progname); + fprintf(stderr, "Options:\n"); + fprintf(stderr, " -t (timestamp: (a)bsolute/(d)elta/(z)ero/(A)bsolute w date)\n"); + fprintf(stderr, " -H (read hardware timestamps instead of system timestamps)\n"); + fprintf(stderr, " -c (increment color mode level)\n"); + fprintf(stderr, " -i (binary output - may exceed 80 chars/line)\n"); + fprintf(stderr, " -a (enable additional ASCII output)\n"); + fprintf(stderr, " -S (swap byte order in printed CAN data[] - marked with '%c' )\n", SWAP_DELIMITER); + fprintf(stderr, " -s (silent mode - %d: off (default) %d: animation %d: silent)\n", SILENT_OFF, SILENT_ANI, SILENT_ON); + fprintf(stderr, " -l (log CAN-frames into file. Sets '-s %d' by default)\n", SILENT_ON); + fprintf(stderr, " -f (log CAN-frames into file . Sets '-s %d' by default)\n", SILENT_ON); + fprintf(stderr, " -L (use log file format on stdout)\n"); + fprintf(stderr, " -n (terminate after reception of CAN frames)\n"); + fprintf(stderr, " -r (set socket receive buffer to )\n"); + fprintf(stderr, " -D (Don't exit if a \"detected\" can device goes down)\n"); + fprintf(stderr, " -d (monitor dropped CAN frames)\n"); + fprintf(stderr, " -e (dump CAN error frames in human-readable format)\n"); + fprintf(stderr, " -8 (display raw DLC values in {} for Classical CAN)\n"); + fprintf(stderr, " -x (print extra message infos, rx/tx brs esi)\n"); + fprintf(stderr, " -T (terminate after if no frames were received)\n"); + fprintf(stderr, "\n"); + fprintf(stderr, "Up to %d CAN interfaces with optional filter sets can be specified\n", MAXSOCK); + fprintf(stderr, "on the commandline in the form: [,filter]*\n"); + fprintf(stderr, "\nFilters:\n"); + fprintf(stderr, " Comma separated filters can be specified for each given CAN interface:\n"); + fprintf(stderr, " :\n (matches when & mask == can_id & mask)\n"); + fprintf(stderr, " ~\n (matches when & mask != can_id & mask)\n"); + fprintf(stderr, " #\n (set error frame filter, see include/linux/can/error.h)\n"); + fprintf(stderr, " [j|J]\n (join the given CAN filters - logical AND semantic)\n"); + fprintf(stderr, "\nCAN IDs, masks and data content are given and expected in hexadecimal values.\n"); + fprintf(stderr, "When the can_id is 8 digits long the CAN_EFF_FLAG is set for 29 bit EFF format.\n"); + fprintf(stderr, "Without any given filter all data frames are received ('0:0' default filter).\n"); + fprintf(stderr, "\nUse interface name '%s' to receive from all CAN interfaces.\n", ANYDEV); + fprintf(stderr, "\nExamples:\n"); + fprintf(stderr, "%s -c -c -ta can0,123:7FF,400:700,#000000FF can2,400~7F0 can3 can8\n\n", progname); + fprintf(stderr, "%s -l any,0~0,#FFFFFFFF\n (log only error frames but no(!) data frames)\n", progname); + fprintf(stderr, "%s -l any,0:0,#FFFFFFFF\n (log error frames and also all data frames)\n", progname); + fprintf(stderr, "%s vcan2,12345678:DFFFFFFF\n (match only for extended CAN ID 12345678)\n", progname); + fprintf(stderr, "%s vcan2,123:7FF\n (matches CAN ID 123 - including EFF and RTR frames)\n", progname); + fprintf(stderr, "%s vcan2,123:C00007FF\n (matches CAN ID 123 - only SFF and non-RTR frames)\n", progname); + fprintf(stderr, "\n"); +} + +static void sigterm(int signo) +{ + running = 0; + signal_num = signo; +} + +static int idx2dindex(int ifidx, int socket) +{ + int i; + struct ifreq ifr; + + for (i = 0; i < MAXIFNAMES; i++) { + if (dindex[i] == ifidx) + return i; + } + + /* create new interface index cache entry */ + + /* remove index cache zombies first */ + for (i = 0; i < MAXIFNAMES; i++) { + if (dindex[i]) { + ifr.ifr_ifindex = dindex[i]; + if (ioctl(socket, SIOCGIFNAME, &ifr) < 0) + dindex[i] = 0; + } + } + + for (i = 0; i < MAXIFNAMES; i++) + if (!dindex[i]) /* free entry */ + break; + + if (i == MAXIFNAMES) { + fprintf(stderr, "Interface index cache only supports %d interfaces.\n", + MAXIFNAMES); + exit(1); + } + + dindex[i] = ifidx; + + ifr.ifr_ifindex = ifidx; + if (ioctl(socket, SIOCGIFNAME, &ifr) < 0) + perror("SIOCGIFNAME"); + + if (max_devname_len < (int)strlen(ifr.ifr_name)) + max_devname_len = strlen(ifr.ifr_name); + + strcpy(devname[i], ifr.ifr_name); + + pr_debug("new index %d (%s)\n", i, devname[i]); + + return i; +} + +static int sprint_timestamp(char *ts_buffer, const char timestamp, + const struct timeval *tv, struct timeval *const last_tv) +{ + int numchars = 0; + + switch (timestamp) { + case 'a': /* absolute with timestamp */ + numchars = sprintf(ts_buffer, "(%010llu.%06llu) ", + (unsigned long long)tv->tv_sec, + (unsigned long long)tv->tv_usec); + break; + + case 'A': /* absolute with date */ + { + struct tm tm; + char timestring[25]; + + tm = *localtime(&tv->tv_sec); + strftime(timestring, 24, "%Y-%m-%d %H:%M:%S", &tm); + numchars = sprintf(ts_buffer, "(%s.%06llu) ", timestring, + (unsigned long long)tv->tv_usec); + } + break; + + case 'd': /* delta */ + case 'z': /* starting with zero */ + { + struct timeval diff; + + if (last_tv->tv_sec == 0) /* first init */ + *last_tv = *tv; + diff.tv_sec = tv->tv_sec - last_tv->tv_sec; + diff.tv_usec = tv->tv_usec - last_tv->tv_usec; + if (diff.tv_usec < 0) + diff.tv_sec--, diff.tv_usec += 1000000; + if (diff.tv_sec < 0) + diff.tv_sec = diff.tv_usec = 0; + numchars = sprintf(ts_buffer, "(%03llu.%06llu) ", + (unsigned long long)diff.tv_sec, + (unsigned long long)diff.tv_usec); + + if (timestamp == 'd') + *last_tv = *tv; /* update for delta calculation */ + } + break; + + default: /* no timestamp output */ + break; + } + + if (numchars <= 0) { + ts_buffer[0] = 0; /* empty terminated string */ + numchars = 0; + } + + return numchars; +} + +int main(int argc, char **argv) +{ + int fd_epoll; + struct epoll_event events_pending[MAXSOCK]; + struct epoll_event event_setup = { + .events = EPOLLIN, /* prepare the common part */ + }; + unsigned char timestamp = 0; + unsigned char logtimestamp = 'a'; + unsigned char hwtimestamp = 0; + unsigned char down_causes_exit = 1; + unsigned char dropmonitor = 0; + unsigned char extra_msg_info = 0; + unsigned char silent = SILENT_INI; + unsigned char silentani = 0; + unsigned char color = 0; + unsigned char view = 0; + unsigned char log = 0; + unsigned char logfrmt = 0; + int count = 0; + int rcvbuf_size = 0; + int opt, num_events; + int currmax, numfilter; + int join_filter; + char *ptr, *nptr; + struct sockaddr_can addr = { + .can_family = AF_CAN, + }; + struct can_raw_vcid_options vcid_opts = { + .flags = CAN_RAW_XL_VCID_RX_FILTER, + .rx_vcid = 0, + .rx_vcid_mask = 0, + }; + char ctrlmsg[CMSG_SPACE(sizeof(struct timeval)) + + CMSG_SPACE(3 * sizeof(struct timespec)) + + CMSG_SPACE(sizeof(__u32))]; + struct iovec iov; + struct msghdr msg; + struct cmsghdr *cmsg; + struct can_filter *rfilter; + can_err_mask_t err_mask; + static cu_t cu; /* union for CAN CC/FD/XL frames */ + int nbytes, i; + struct ifreq ifr; + struct timeval tv, last_tv; + int timeout_ms = -1; /* default to no timeout */ + FILE *logfile = NULL; + char fname[83]; /* suggested by -Wformat-overflow= */ + const char *logname = NULL; + static char afrbuf[AFRSZ]; /* ASCII CAN frame buffer size */ + static int alen; + + signal(SIGTERM, sigterm); + signal(SIGHUP, sigterm); + signal(SIGINT, sigterm); + + last_tv.tv_sec = 0; + last_tv.tv_usec = 0; + + progname = basename(argv[0]); + + while ((opt = getopt(argc, argv, "t:HciaSs:lf:Ln:r:Dde8xT:h?")) != -1) { + switch (opt) { + case 't': + timestamp = optarg[0]; + logtimestamp = optarg[0]; + if ((timestamp != 'a') && (timestamp != 'A') && + (timestamp != 'd') && (timestamp != 'z')) { + fprintf(stderr, "%s: unknown timestamp mode '%c' - ignored\n", + progname, optarg[0]); + timestamp = 0; + } + if ((logtimestamp != 'a') && (logtimestamp != 'z')) { + logtimestamp = 'a'; + } + break; + + case 'H': + hwtimestamp = 1; + break; + + case 'c': + color++; + break; + + case 'i': + view |= CANLIB_VIEW_BINARY; + break; + + case 'a': + view |= CANLIB_VIEW_ASCII; + break; + + case 'S': + view |= CANLIB_VIEW_SWAP; + break; + + case 'e': + view |= CANLIB_VIEW_ERROR; + break; + + case '8': + view |= CANLIB_VIEW_LEN8_DLC; + break; + + case 's': + silent = atoi(optarg); + if (silent > SILENT_ON) { + print_usage(); + exit(1); + } + break; + + case 'l': + log = 1; + break; + + case 'D': + down_causes_exit = 0; + break; + + case 'd': + dropmonitor = 1; + break; + + case 'x': + extra_msg_info = 1; + break; + + case 'L': + logfrmt = 1; + break; + + case 'f': + logname = optarg; + log = 1; + break; + + case 'n': + count = atoi(optarg); + if (count < 1) { + print_usage(); + exit(1); + } + break; + + case 'r': + rcvbuf_size = atoi(optarg); + if (rcvbuf_size < 1) { + print_usage(); + exit(1); + } + break; + + case 'T': + errno = 0; + timeout_ms = strtol(optarg, NULL, 0); + if (errno != 0) { + print_usage(); + exit(1); + } + break; + default: + print_usage(); + exit(1); + break; + } + } + + if (optind == argc) { + print_usage(); + exit(0); + } + + if (logfrmt && view) { + fprintf(stderr, "Log file format selected: Please disable ASCII/BINARY/SWAP/RAWDLC options!\n"); + exit(0); + } + + /* "-f -" is equal to "-L" (print logfile format on stdout) */ + if (log && logname && strcmp("-", logname) == 0) { + log = 0; /* no logging into a file */ + logfrmt = 1; /* print logformat output to stdout */ + } + + if (silent == SILENT_INI) { + if (log) { + fprintf(stderr, "Disabled standard output while logging.\n"); + silent = SILENT_ON; /* disable output on stdout */ + } else + silent = SILENT_OFF; /* default output */ + } + + currmax = argc - optind; /* find real number of CAN devices */ + + if (currmax > MAXSOCK) { + fprintf(stderr, "More than %d CAN devices given on commandline!\n", MAXSOCK); + return 1; + } + + fd_epoll = epoll_create(1); + if (fd_epoll < 0) { + perror("epoll_create"); + return 1; + } + + for (i = 0; i < currmax; i++) { + struct if_info *obj = &sock_info[i]; + ptr = argv[optind + i]; + nptr = strchr(ptr, ','); + + pr_debug("open %d '%s'.\n", i, ptr); + + obj->s = socket(PF_CAN, SOCK_RAW, CAN_RAW); + if (obj->s < 0) { + perror("socket"); + return 1; + } + + event_setup.data.ptr = obj; /* remember the instance as private data */ + if (epoll_ctl(fd_epoll, EPOLL_CTL_ADD, obj->s, &event_setup)) { + perror("failed to add socket to epoll"); + return 1; + } + + obj->cmdlinename = ptr; /* save pointer to cmdline name of this socket */ + + if (nptr) + nbytes = nptr - ptr; /* interface name is up the first ',' */ + else + nbytes = strlen(ptr); /* no ',' found => no filter definitions */ + + if (nbytes >= IFNAMSIZ) { + fprintf(stderr, "name of CAN device '%s' is too long!\n", ptr); + return 1; + } + + if (nbytes > max_devname_len) + max_devname_len = nbytes; /* for nice printing */ + + memset(&ifr.ifr_name, 0, sizeof(ifr.ifr_name)); + strncpy(ifr.ifr_name, ptr, nbytes); + + pr_debug("using interface name '%s'.\n", ifr.ifr_name); + + if (strcmp(ANYDEV, ifr.ifr_name) != 0) { + if (ioctl(obj->s, SIOCGIFINDEX, &ifr) < 0) { + perror("SIOCGIFINDEX"); + exit(1); + } + addr.can_ifindex = ifr.ifr_ifindex; + } else + addr.can_ifindex = 0; /* any can interface */ + + if (nptr) { + /* found a ',' after the interface name => check for filters */ + + /* determine number of filters to alloc the filter space */ + numfilter = 0; + ptr = nptr; + while (ptr) { + numfilter++; + ptr++; /* hop behind the ',' */ + ptr = strchr(ptr, ','); /* exit condition */ + } + + rfilter = malloc(sizeof(struct can_filter) * numfilter); + if (!rfilter) { + fprintf(stderr, "Failed to create filter space!\n"); + return 1; + } + + numfilter = 0; + err_mask = 0; + join_filter = 0; + + while (nptr) { + + ptr = nptr + 1; /* hop behind the ',' */ + nptr = strchr(ptr, ','); /* update exit condition */ + + if (sscanf(ptr, "%x:%x", + &rfilter[numfilter].can_id, + &rfilter[numfilter].can_mask) == 2) { + rfilter[numfilter].can_mask &= ~CAN_ERR_FLAG; + if (*(ptr + 8) == ':') + rfilter[numfilter].can_id |= CAN_EFF_FLAG; + numfilter++; + } else if (sscanf(ptr, "%x~%x", + &rfilter[numfilter].can_id, + &rfilter[numfilter].can_mask) == 2) { + rfilter[numfilter].can_id |= CAN_INV_FILTER; + rfilter[numfilter].can_mask &= ~CAN_ERR_FLAG; + if (*(ptr + 8) == '~') + rfilter[numfilter].can_id |= CAN_EFF_FLAG; + numfilter++; + } else if (*ptr == 'j' || *ptr == 'J') { + join_filter = 1; + } else if (sscanf(ptr, "#%x", &err_mask) != 1) { + fprintf(stderr, "Error in filter option parsing: '%s'\n", ptr); + return 1; + } + } + + if (err_mask) + setsockopt(obj->s, SOL_CAN_RAW, CAN_RAW_ERR_FILTER, + &err_mask, sizeof(err_mask)); + + if (join_filter && setsockopt(obj->s, SOL_CAN_RAW, CAN_RAW_JOIN_FILTERS, + &join_filter, sizeof(join_filter)) < 0) { + perror("setsockopt CAN_RAW_JOIN_FILTERS not supported by your Linux Kernel"); + return 1; + } + + if (numfilter) + setsockopt(obj->s, SOL_CAN_RAW, CAN_RAW_FILTER, + rfilter, numfilter * sizeof(struct can_filter)); + + free(rfilter); + + } /* if (nptr) */ + + /* try to switch the socket into CAN FD mode */ + setsockopt(obj->s, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, &canfx_on, sizeof(canfx_on)); + + /* try to switch the socket into CAN XL mode */ + setsockopt(obj->s, SOL_CAN_RAW, CAN_RAW_XL_FRAMES, &canfx_on, sizeof(canfx_on)); + + /* try to enable the CAN XL VCID pass through mode */ + setsockopt(obj->s, SOL_CAN_RAW, CAN_RAW_XL_VCID_OPTS, &vcid_opts, sizeof(vcid_opts)); + + if (rcvbuf_size) { + int curr_rcvbuf_size; + socklen_t curr_rcvbuf_size_len = sizeof(curr_rcvbuf_size); + + /* try SO_RCVBUFFORCE first, if we run with CAP_NET_ADMIN */ + if (setsockopt(obj->s, SOL_SOCKET, SO_RCVBUFFORCE, + &rcvbuf_size, sizeof(rcvbuf_size)) < 0) { + pr_debug("SO_RCVBUFFORCE failed so try SO_RCVBUF ...\n"); + if (setsockopt(obj->s, SOL_SOCKET, SO_RCVBUF, + &rcvbuf_size, sizeof(rcvbuf_size)) < 0) { + perror("setsockopt SO_RCVBUF"); + return 1; + } + + if (getsockopt(obj->s, SOL_SOCKET, SO_RCVBUF, + &curr_rcvbuf_size, &curr_rcvbuf_size_len) < 0) { + perror("getsockopt SO_RCVBUF"); + return 1; + } + + /* Only print a warning the first time we detect the adjustment */ + /* n.b.: The wanted size is doubled in Linux in net/sore/sock.c */ + if (!i && curr_rcvbuf_size < rcvbuf_size * 2) + fprintf(stderr, "The socket receive buffer size was " + "adjusted due to /proc/sys/net/core/rmem_max.\n"); + } + } + + if (timestamp || log || logfrmt) { + if (hwtimestamp) { + const int timestamping_flags = (SOF_TIMESTAMPING_SOFTWARE | + SOF_TIMESTAMPING_RX_SOFTWARE | + SOF_TIMESTAMPING_RAW_HARDWARE); + + if (setsockopt(obj->s, SOL_SOCKET, SO_TIMESTAMPING, + ×tamping_flags, sizeof(timestamping_flags)) < 0) { + perror("setsockopt SO_TIMESTAMPING is not supported by your Linux kernel"); + return 1; + } + } else { + const int timestamp_on = 1; + + if (setsockopt(obj->s, SOL_SOCKET, SO_TIMESTAMP, + ×tamp_on, sizeof(timestamp_on)) < 0) { + perror("setsockopt SO_TIMESTAMP"); + return 1; + } + } + } + + if (dropmonitor) { + const int dropmonitor_on = 1; + + if (setsockopt(obj->s, SOL_SOCKET, SO_RXQ_OVFL, + &dropmonitor_on, sizeof(dropmonitor_on)) < 0) { + perror("setsockopt SO_RXQ_OVFL not supported by your Linux Kernel"); + return 1; + } + } + + if (bind(obj->s, (struct sockaddr *)&addr, sizeof(addr)) < 0) { + perror("bind"); + return 1; + } + } + + if (log) { + if (!logname) { + time_t currtime; + struct tm now; + + if (time(&currtime) == (time_t)-1) { + perror("time"); + return 1; + } + + localtime_r(&currtime, &now); + + snprintf(fname, sizeof(fname), "candump-%04d-%02d-%02d_%02d%02d%02d.log", + now.tm_year + 1900, + now.tm_mon + 1, + now.tm_mday, + now.tm_hour, + now.tm_min, + now.tm_sec); + + logname = fname; + } + + if (silent != SILENT_ON) + fprintf(stderr, "Warning: Console output active while logging!\n"); + + fprintf(stderr, "Enabling Logfile '%s'\n", logname); + + logfile = fopen(logname, "w"); + if (!logfile) { + perror("logfile"); + return 1; + } + } + + /* these settings are static and can be held out of the hot path */ + iov.iov_base = &cu; + msg.msg_name = &addr; + msg.msg_iov = &iov; + msg.msg_iovlen = 1; + msg.msg_control = &ctrlmsg; + + while (running) { + num_events = epoll_wait(fd_epoll, events_pending, currmax, timeout_ms); + if (num_events == -1) { + if (errno != EINTR) + running = 0; + continue; + } + + /* handle timeout */ + if (!num_events && timeout_ms >= 0) { + running = 0; + continue; + } + + for (i = 0; i < num_events; i++) { /* check waiting CAN RAW sockets */ + struct if_info *obj = events_pending[i].data.ptr; + int idx; + char *extra_info = ""; + + /* these settings may be modified by recvmsg() */ + iov.iov_len = sizeof(cu); + msg.msg_namelen = sizeof(addr); + msg.msg_controllen = sizeof(ctrlmsg); + msg.msg_flags = 0; + + nbytes = recvmsg(obj->s, &msg, 0); + idx = idx2dindex(addr.can_ifindex, obj->s); + + if (nbytes < 0) { + if ((errno == ENETDOWN) && !down_causes_exit) { + fprintf(stderr, "%s: interface down\n", devname[idx]); + continue; + } + perror("read"); + return 1; + } + + /* mark dual-use struct canfd_frame */ + if (nbytes < (int)CANXL_HDR_SIZE + CANXL_MIN_DLEN) { + fprintf(stderr, "read: no CAN frame\n"); + return 1; + } + + if (cu.xl.flags & CANXL_XLF) { + if (nbytes != (int)CANXL_HDR_SIZE + cu.xl.len) { + printf("nbytes = %d\n", nbytes); + fprintf(stderr, "read: no CAN XL frame\n"); + return 1; + } + } else { + if (nbytes == CAN_MTU) + cu.fd.flags = 0; + else if (nbytes == CANFD_MTU) + cu.fd.flags |= CANFD_FDF; + else { + fprintf(stderr, "read: incomplete CAN CC/FD frame\n"); + return 1; + } + } + + if (count && (--count == 0)) + running = 0; + + for (cmsg = CMSG_FIRSTHDR(&msg); + cmsg && (cmsg->cmsg_level == SOL_SOCKET); + cmsg = CMSG_NXTHDR(&msg,cmsg)) { + if (cmsg->cmsg_type == SO_TIMESTAMP) { + memcpy(&tv, CMSG_DATA(cmsg), sizeof(tv)); + } else if (cmsg->cmsg_type == SO_TIMESTAMPING) { + struct timespec *stamp = (struct timespec *)CMSG_DATA(cmsg); + + /* + * stamp[0] is the software timestamp + * stamp[1] is deprecated + * stamp[2] is the raw hardware timestamp + * See chapter 2.1.2 Receive timestamps in + * linux/Documentation/networking/timestamping.txt + */ + tv.tv_sec = stamp[2].tv_sec; + tv.tv_usec = stamp[2].tv_nsec / 1000; + } else if (cmsg->cmsg_type == SO_RXQ_OVFL) { + memcpy(&obj->dropcnt, CMSG_DATA(cmsg), sizeof(__u32)); + } + } + + /* check for (unlikely) dropped frames on this specific socket */ + if (obj->dropcnt != obj->last_dropcnt) { + __u32 frames = obj->dropcnt - obj->last_dropcnt; + + if (silent != SILENT_ON) + printf("DROPCOUNT: dropped %u CAN frame%s on '%s' socket (total drops %u)\n", + frames, (frames > 1)?"s":"", devname[idx], obj->dropcnt); + + if (log) + fprintf(logfile, "DROPCOUNT: dropped %u CAN frame%s on '%s' socket (total drops %u)\n", + frames, (frames > 1)?"s":"", devname[idx], obj->dropcnt); + + obj->last_dropcnt = obj->dropcnt; + } + + /* once we detected a EFF frame indent SFF frames accordingly */ + if (cu.fd.can_id & CAN_EFF_FLAG) + view |= CANLIB_VIEW_INDENT_SFF; + + if (extra_msg_info) { + if (msg.msg_flags & MSG_DONTROUTE) + extra_info = " T"; + else + extra_info = " R"; + } + + /* build common log format output */ + if ((log) || ((logfrmt) && (silent == SILENT_OFF))) { + + alen = sprint_timestamp(afrbuf, logtimestamp, + &tv, &last_tv); + + alen += sprintf(afrbuf + alen, "%*s ", + max_devname_len, devname[idx]); + + alen += snprintf_canframe(afrbuf + alen, sizeof(afrbuf) - alen, &cu, 0); + } + + /* write CAN frame in log file style to logfile */ + if (log) + fprintf(logfile, "%s%s\n", afrbuf, extra_info); + + /* print CAN frame in log file style to stdout */ + if ((logfrmt) && (silent == SILENT_OFF)) { + printf("%s%s\n", afrbuf, extra_info); + goto out_fflush; /* no other output to stdout */ + } + + /* print only animation */ + if (silent != SILENT_OFF) { + if (silent == SILENT_ANI) { + printf("%c\b", anichar[silentani %= MAXANI]); + silentani++; + } + goto out_fflush; /* no other output to stdout */ + } + + /* print (colored) long CAN frame style to stdout */ + alen = sprintf(afrbuf, " %s", (color > 2) ? col_on[idx % MAXCOL] : ""); + alen += sprint_timestamp(afrbuf + alen, timestamp, &tv, &last_tv); + alen += sprintf(afrbuf + alen, " %s%*s", + (color && (color < 3)) ? col_on[idx % MAXCOL] : "", + max_devname_len, devname[idx]); + + if (extra_msg_info) { + if (msg.msg_flags & MSG_DONTROUTE) + alen += sprintf(afrbuf + alen, " TX %s", + extra_m_info[cu.fd.flags & 3]); + else + alen += sprintf(afrbuf + alen, " RX %s", + extra_m_info[cu.fd.flags & 3]); + } + + alen += sprintf(afrbuf + alen, "%s ", (color == 1) ? col_off : ""); + alen += snprintf_long_canframe(afrbuf + alen, sizeof(afrbuf) - alen, &cu, view); + + if ((view & CANLIB_VIEW_ERROR) && (cu.fd.can_id & CAN_ERR_FLAG)) { + alen += sprintf(afrbuf + alen, "\n\t"); + alen += snprintf_can_error_frame(afrbuf + alen, + sizeof(afrbuf) - alen, + &cu.fd, "\n\t"); + } + + printf("%s%s\n", afrbuf, (color > 1) ? col_off : ""); +out_fflush: + fflush(stdout); + } + } + + for (i = 0; i < currmax; i++) + close(sock_info[i].s); + + close(fd_epoll); + + if (log) + fclose(logfile); + + if (signal_num) + return 128 + signal_num; + + return 0; +} diff --git a/Devices/Libraries/Systems/can-utils/canfdtest.c b/Devices/Libraries/Systems/can-utils/canfdtest.c new file mode 100755 index 0000000..5f0a642 --- /dev/null +++ b/Devices/Libraries/Systems/can-utils/canfdtest.c @@ -0,0 +1,626 @@ +/* SPDX-License-Identifier: GPL-2.0-only */ +/* + * canfdtest.c - Full-duplex test program (DUT and host part) + * + * (C) 2009 by Vladislav Gribov, IXXAT Automation GmbH, + * (C) 2009 Wolfgang Grandegger + * (C) 2021 Jean Gressmann, IAV GmbH, + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the version 2 of the GNU General Public License + * as published by the Free Software Foundation. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * Send feedback to + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include + +#define CAN_MSG_ID_PING 0x77 +#define CAN_MSG_ID_PONG 0x78 +#define CAN_MSG_LEN 8 +#define CAN_MSG_COUNT 50 +#define CAN_MSG_WAIT 27 + +static int running = 1; +static int verbose; +static int sockfd; +static int test_loops; +static int exit_sig; +static int inflight_count = CAN_MSG_COUNT; +static int filter; +static canid_t can_id_ping = CAN_MSG_ID_PING; +static canid_t can_id_pong = CAN_MSG_ID_PONG; +static bool has_pong_id; +static bool is_can_fd; +static bool bit_rate_switch; +static int msg_len = CAN_MSG_LEN; +static bool is_extended_frame_format; + +static void print_usage(char *prg) +{ + fprintf(stderr, + "%s - Full-duplex test program (DUT and host part).\n" + "Usage: %s [options] []\n" + "\n" + "Options:\n" + " -b (enable CAN FD Bit Rate Switch)\n" + " -d (use CAN FD frames instead of classic CAN)\n" + " -e (use 29-bit extended frame format instead of classic 11-bit one)\n" + " -f COUNT (number of frames in flight, default: %d)\n" + " -g (generate messages)\n" + " -i ID (CAN ID to use for frames to DUT (ping), default %x)\n" + " -l COUNT (test loop count)\n" + " -o ID (CAN ID to use for frames to host (pong), default %x)\n" + " -s SIZE (frame payload size in bytes)\n" + " -v (low verbosity)\n" + " -vv (high verbosity)\n" + " -x (ignore other frames on bus)\n" + " -xx (ignore locally generated and other frames on bus -- use for loopback testing)\n" + "\n" + "With the option '-g' CAN messages are generated and checked\n" + "on , otherwise all messages received on the\n" + " are sent back incrementing the CAN id and\n" + "all data bytes. The program can be aborted with ^C.\n" + "\n" + "Using 'can0' as default CAN-interface.\n" + "\n" + "Examples:\n" + "\ton DUT:\n" + "%s -v can0\n" + "\ton Host:\n" + "%s -g -v can2\n", + prg, prg, CAN_MSG_COUNT, CAN_MSG_ID_PING, CAN_MSG_ID_PONG, prg, prg); + + exit(1); +} + +static void print_frame(canid_t id, const uint8_t *data, int dlc, int inc_data) +{ + int i; + + printf("%04x: ", id); + if (id & CAN_RTR_FLAG) { + printf("remote request"); + } else { + printf("[%d]", dlc); + for (i = 0; i < dlc; i++) + printf(" %02x", (uint8_t)(data[i] + inc_data)); + } + printf("\n"); +} + +static void print_compare(canid_t exp_id, const uint8_t *exp_data, uint8_t exp_dlc, + canid_t rec_id, const uint8_t *rec_data, uint8_t rec_dlc, + int inc) +{ + printf("expected: "); + print_frame(exp_id, exp_data, exp_dlc, inc); + printf("received: "); + print_frame(rec_id, rec_data, rec_dlc, 0); +} + +static canid_t normalize_canid(canid_t id) +{ + if (is_extended_frame_format) { + id &= CAN_EFF_MASK; + id |= CAN_EFF_FLAG; + } else { + id &= CAN_SFF_MASK; + } + + return id; +} + +static int compare_frame(const struct canfd_frame *exp, const struct canfd_frame *rec, int inc) +{ + int i, err = 0; + const canid_t expected_can_id = inc ? can_id_pong : can_id_ping; + + if (rec->can_id != expected_can_id) { + printf("Message ID mismatch!\n"); + print_compare(expected_can_id, exp->data, exp->len, + rec->can_id, rec->data, rec->len, inc); + running = 0; + err = -1; + } else if (rec->len != exp->len) { + printf("Message length mismatch!\n"); + print_compare(expected_can_id, exp->data, exp->len, + rec->can_id, rec->data, rec->len, inc); + running = 0; + err = -1; + } else { + for (i = 0; i < rec->len; i++) { + if (rec->data[i] != (uint8_t)(exp->data[i] + inc)) { + printf("Databyte %x mismatch!\n", i); + print_compare(expected_can_id, exp->data, exp->len, + rec->can_id, rec->data, rec->len, inc); + running = 0; + err = -1; + } + } + } + return err; +} + +static void millisleep(int msecs) +{ + struct timespec rqtp, rmtp; + int err; + + /* sleep in ms */ + rqtp.tv_sec = msecs / 1000; + rqtp.tv_nsec = msecs % 1000 * 1000000; + + do { + err = clock_nanosleep(CLOCK_MONOTONIC, 0, &rqtp, &rmtp); + if (err != 0 && err != EINTR) { + printf("t\n"); + break; + } + rqtp = rmtp; + } while (err != 0); +} + +static void echo_progress(unsigned char data) +{ + if (data == 0xff) { + printf("."); + fflush(stdout); + } +} + +static void signal_handler(int signo) +{ + close(sockfd); + running = 0; + exit_sig = signo; +} + +static int recv_frame(struct canfd_frame *frame, int *flags) +{ + struct iovec iov = { + .iov_base = frame, + .iov_len = is_can_fd ? sizeof(struct canfd_frame) : sizeof(struct can_frame), + }; + struct msghdr msg = { + .msg_iov = &iov, + .msg_iovlen = 1, + }; + ssize_t ret; + + ret = recvmsg(sockfd, &msg, 0); + if (ret < 0) { + perror("recvmsg() failed"); + return -1; + } + if ((size_t)ret != iov.iov_len) { + fprintf(stderr, "recvmsg() returned %zd", ret); + return -1; + } + + if (flags) + *flags = msg.msg_flags; + + return 0; +} + +static int send_frame(struct canfd_frame *frame) +{ + ssize_t ret, len; + + if (is_can_fd) + len = sizeof(struct canfd_frame); + else + len = sizeof(struct can_frame); + + if (bit_rate_switch) + frame->flags |= CANFD_BRS; + + while ((ret = send(sockfd, frame, len, 0)) != len) { + if (ret >= 0) { + fprintf(stderr, "send returned %zd", ret); + return -1; + } + if (errno != ENOBUFS) { + perror("send failed"); + return -1; + } + if (verbose) { + printf("N"); + fflush(stdout); + } + } + return 0; +} + +static int check_frame(const struct canfd_frame *frame) +{ + int err = 0; + int i; + + if (frame->can_id != can_id_ping) { + printf("Unexpected Message ID 0x%04x!\n", frame->can_id); + err = -1; + } + + if (frame->len != msg_len) { + printf("Unexpected Message length %d!\n", frame->len); + err = -1; + } + + for (i = 1; i < frame->len; i++) { + if (frame->data[i] != (uint8_t)(frame->data[i - 1] + 1)) { + printf("Frame inconsistent!\n"); + print_frame(frame->can_id, frame->data, frame->len, 0); + err = -1; + goto out; + } + } + +out: + return err; +} + +static void inc_frame(struct canfd_frame *frame) +{ + int i; + + if (has_pong_id) + frame->can_id = can_id_pong; + else + frame->can_id = normalize_canid(frame->can_id + 1); + + for (i = 0; i < frame->len; i++) + frame->data[i]++; +} + +static int can_echo_dut(void) +{ + unsigned int frame_count = 0; + struct canfd_frame frame; + int err = 0; + + while (running) { + int flags; + + if (recv_frame(&frame, &flags)) + return -1; + + if (filter > 1 && flags & MSG_DONTROUTE) + continue; + + frame_count++; + if (verbose == 1) { + echo_progress(frame.data[0]); + } else if (verbose > 1) { + if (verbose > 2) + printf("%s %s: ", + flags & MSG_DONTROUTE ? "DR" : " ", + flags & MSG_CONFIRM ? "CF" : " "); + print_frame(frame.can_id, frame.data, frame.len, 0); + } + + err = check_frame(&frame); + inc_frame(&frame); + if (send_frame(&frame)) + return -1; + + /* + * to force a interlacing of the frames send by DUT and PC + * test tool a waiting time is injected + */ + if (frame_count == CAN_MSG_WAIT) { + frame_count = 0; + millisleep(3); + } + } + + return err; +} + +static int can_echo_gen(void) +{ + struct canfd_frame *tx_frames; + bool *recv_tx; + unsigned char counter = 0; + int send_pos = 0, recv_rx_pos = 0, recv_tx_pos = 0, unprocessed = 0, loops = 0; + int err = 0; + int i; + + tx_frames = calloc(inflight_count, sizeof(*tx_frames)); + if (!tx_frames) + return -1; + + recv_tx = calloc(inflight_count, sizeof(*recv_tx)); + if (!recv_tx) { + err = -1; + goto out_free_tx_frames; + } + + while (running) { + if (unprocessed < inflight_count) { + /* still send messages */ + struct canfd_frame *tx_frame = &tx_frames[send_pos]; + + tx_frame->len = msg_len; + tx_frame->can_id = can_id_ping; + recv_tx[send_pos] = false; + + for (i = 0; i < msg_len; i++) + tx_frame->data[i] = counter + i; + if (send_frame(tx_frame)) { + err = -1; + goto out_free; + } + + send_pos++; + send_pos %= inflight_count; + + unprocessed++; + if (verbose == 1) + echo_progress(counter); + counter++; + + if ((counter % 33) == 0) + millisleep(3); + else + millisleep(1); + } else { + struct canfd_frame rx_frame; + int flags; + + if (recv_frame(&rx_frame, &flags)) { + err = -1; + goto out_free; + } + + if (filter > 1 && + ((rx_frame.can_id == can_id_ping && !(flags & MSG_CONFIRM)) || + (rx_frame.can_id == can_id_pong && (flags & MSG_DONTROUTE)))) + continue; + + if (verbose > 1) { + if (verbose > 2) + printf("%s %s: ", + flags & MSG_DONTROUTE ? "DR" : " ", + flags & MSG_CONFIRM ? "CF" : " "); + print_frame(rx_frame.can_id, rx_frame.data, rx_frame.len, 0); + } + + /* own frame */ + if (flags & MSG_CONFIRM) { + err = compare_frame(&tx_frames[recv_tx_pos], &rx_frame, 0); + recv_tx[recv_tx_pos] = true; + recv_tx_pos++; + recv_tx_pos %= inflight_count; + continue; + } + + if (!recv_tx[recv_rx_pos]) { + printf("RX before TX!\n"); + print_frame(rx_frame.can_id, rx_frame.data, rx_frame.len, 0); + running = 0; + } + /* compare with expected */ + err = compare_frame(&tx_frames[recv_rx_pos], &rx_frame, 1); + recv_rx_pos++; + recv_rx_pos %= inflight_count; + + loops++; + if (test_loops && loops >= test_loops) + break; + + unprocessed--; + } + } + + printf("\nTest messages sent and received: %d\n", loops); + +out_free: + free(recv_tx); +out_free_tx_frames: + free(tx_frames); + + return err; +} + +int main(int argc, char *argv[]) +{ + struct sockaddr_can addr; + char *intf_name = "can0"; + int family = PF_CAN, type = SOCK_RAW, proto = CAN_RAW; + int echo_gen = 0; + int opt, err; + int enable_socket_option = 1; + + signal(SIGTERM, signal_handler); + signal(SIGHUP, signal_handler); + signal(SIGINT, signal_handler); + + while ((opt = getopt(argc, argv, "bdef:gi:l:o:s:vx?")) != -1) { + switch (opt) { + case 'b': + bit_rate_switch = true; + break; + + case 'd': + is_can_fd = true; + break; + + case 'e': + is_extended_frame_format = true; + break; + + case 'f': + inflight_count = atoi(optarg); + break; + + case 'g': + echo_gen = 1; + break; + + case 'i': + can_id_ping = strtoul(optarg, NULL, 16); + break; + + case 'l': + test_loops = atoi(optarg); + break; + + case 'o': + can_id_pong = strtoul(optarg, NULL, 16); + has_pong_id = true; + break; + + case 's': + msg_len = atoi(optarg); + break; + + case 'v': + verbose++; + break; + + case 'x': + filter++; + break; + + case '?': + default: + print_usage(basename(argv[0])); + break; + } + } + + /* BRS can be enabled only if CAN FD is enabled */ + if (bit_rate_switch && !is_can_fd) { + printf("Bit rate switch (-b) needs CAN FD (-d) to be enabled\n"); + return 1; + } + + /* Make sure the message length is valid */ + if (msg_len <= 0) { + printf("Message length must > 0\n"); + return 1; + } + if (is_can_fd) { + if (msg_len > CANFD_MAX_DLEN) { + printf("Message length must be <= %d bytes for CAN FD\n", CANFD_MAX_DLEN); + return 1; + } + } else { + if (msg_len > CAN_MAX_DLEN) { + printf("Message length must be <= %d bytes for CAN 2.0B\n", CAN_MAX_DLEN); + return 1; + } + } + + can_id_ping = normalize_canid(can_id_ping); + can_id_pong = normalize_canid(can_id_pong); + + if ((argc - optind) == 1) + intf_name = argv[optind]; + else if ((argc - optind)) + print_usage(basename(argv[0])); + + printf("interface = %s, family = %d, type = %d, proto = %d\n", + intf_name, family, type, proto); + + if ((sockfd = socket(family, type, proto)) < 0) { + perror("socket"); + return 1; + } + + if (echo_gen) { + if (setsockopt(sockfd, SOL_CAN_RAW, CAN_RAW_RECV_OWN_MSGS, + &enable_socket_option, sizeof(enable_socket_option)) == -1) { + perror("setsockopt CAN_RAW_RECV_OWN_MSGS"); + return 1; + } + } + + if (is_can_fd) { + if (setsockopt(sockfd, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, + &enable_socket_option, sizeof(enable_socket_option)) == -1) { + perror("setsockopt CAN_RAW_FD_FRAMES"); + return 1; + } + } + + addr.can_family = family; + addr.can_ifindex = if_nametoindex(intf_name); + if (!addr.can_ifindex) { + perror("if_nametoindex"); + close(sockfd); + return 1; + } + + if (bind(sockfd, (struct sockaddr *)&addr, sizeof(addr)) < 0) { + perror("bind"); + close(sockfd); + return 1; + } + + if (!has_pong_id) + can_id_pong = can_id_ping + 1; + + if (filter) { + const struct can_filter filters[] = { + { + .can_id = can_id_ping, + .can_mask = CAN_EFF_FLAG | CAN_RTR_FLAG | CAN_EFF_MASK, + }, + { + .can_id = can_id_pong, + .can_mask = CAN_EFF_FLAG | CAN_RTR_FLAG | CAN_EFF_MASK, + }, + }; + + if (setsockopt(sockfd, SOL_CAN_RAW, CAN_RAW_FILTER, filters, + sizeof(struct can_filter) * (1 + echo_gen))) { + perror("setsockopt()"); + close(sockfd); + return 1; + } + } + + if (echo_gen) + err = can_echo_gen(); + else + err = can_echo_dut(); + + if (verbose) + printf("Exiting...\n"); + + close(sockfd); + + if (exit_sig) + return 128 + exit_sig; + + return err; +} diff --git a/Devices/Libraries/Systems/can-utils/canframelen.c b/Devices/Libraries/Systems/can-utils/canframelen.c new file mode 100755 index 0000000..e0ca1f5 --- /dev/null +++ b/Devices/Libraries/Systems/can-utils/canframelen.c @@ -0,0 +1,283 @@ +/* SPDX-License-Identifier: (GPL-2.0-only OR BSD-3-Clause) */ +/* + * canframelen.c + * + * Copyright (c) 2013, 2014 Czech Technical University in Prague + * + * Author: Michal Sojka + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * 3. Neither the name of Czech Technical University in Prague nor the + * names of its contributors may be used to endorse or promote + * products derived from this software without specific prior + * written permission. + * + * Alternatively, provided that this notice is retained in full, this + * software may be distributed under the terms of the GNU General + * Public License ("GPL") version 2, in which case the provisions of the + * GPL apply INSTEAD OF those given above. + * + * The provided data structures and external interfaces from this code + * are not restricted to be used by modules with a GPL compatible license. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT + * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY + * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH + * DAMAGE. + * + * Send feedback to + * + */ + +#include +#include +#include +#include +#include +#include + +#include "canframelen.h" + +/** + * Functions and types for CRC checks. + * + * Generated on Wed Jan 8 15:14:20 2014, + * by pycrc v0.8.1, http://www.tty1.net/pycrc/ + * using the configuration: + * Width = 15 + * Poly = 0x4599 + * XorIn = 0x0000 + * ReflectIn = False + * XorOut = 0x0000 + * ReflectOut = False + * Algorithm = table-driven + *****************************************************************************/ + +typedef uint16_t crc_t; + +/** + * Static table used for the table_driven implementation. + *****************************************************************************/ +static const crc_t crc_table[256] = { + 0x0000, 0x4599, 0x4eab, 0x0b32, 0x58cf, 0x1d56, 0x1664, 0x53fd, 0x7407, 0x319e, 0x3aac, 0x7f35, 0x2cc8, 0x6951, 0x6263, 0x27fa, + 0x2d97, 0x680e, 0x633c, 0x26a5, 0x7558, 0x30c1, 0x3bf3, 0x7e6a, 0x5990, 0x1c09, 0x173b, 0x52a2, 0x015f, 0x44c6, 0x4ff4, 0x0a6d, + 0x5b2e, 0x1eb7, 0x1585, 0x501c, 0x03e1, 0x4678, 0x4d4a, 0x08d3, 0x2f29, 0x6ab0, 0x6182, 0x241b, 0x77e6, 0x327f, 0x394d, 0x7cd4, + 0x76b9, 0x3320, 0x3812, 0x7d8b, 0x2e76, 0x6bef, 0x60dd, 0x2544, 0x02be, 0x4727, 0x4c15, 0x098c, 0x5a71, 0x1fe8, 0x14da, 0x5143, + 0x73c5, 0x365c, 0x3d6e, 0x78f7, 0x2b0a, 0x6e93, 0x65a1, 0x2038, 0x07c2, 0x425b, 0x4969, 0x0cf0, 0x5f0d, 0x1a94, 0x11a6, 0x543f, + 0x5e52, 0x1bcb, 0x10f9, 0x5560, 0x069d, 0x4304, 0x4836, 0x0daf, 0x2a55, 0x6fcc, 0x64fe, 0x2167, 0x729a, 0x3703, 0x3c31, 0x79a8, + 0x28eb, 0x6d72, 0x6640, 0x23d9, 0x7024, 0x35bd, 0x3e8f, 0x7b16, 0x5cec, 0x1975, 0x1247, 0x57de, 0x0423, 0x41ba, 0x4a88, 0x0f11, + 0x057c, 0x40e5, 0x4bd7, 0x0e4e, 0x5db3, 0x182a, 0x1318, 0x5681, 0x717b, 0x34e2, 0x3fd0, 0x7a49, 0x29b4, 0x6c2d, 0x671f, 0x2286, + 0x2213, 0x678a, 0x6cb8, 0x2921, 0x7adc, 0x3f45, 0x3477, 0x71ee, 0x5614, 0x138d, 0x18bf, 0x5d26, 0x0edb, 0x4b42, 0x4070, 0x05e9, + 0x0f84, 0x4a1d, 0x412f, 0x04b6, 0x574b, 0x12d2, 0x19e0, 0x5c79, 0x7b83, 0x3e1a, 0x3528, 0x70b1, 0x234c, 0x66d5, 0x6de7, 0x287e, + 0x793d, 0x3ca4, 0x3796, 0x720f, 0x21f2, 0x646b, 0x6f59, 0x2ac0, 0x0d3a, 0x48a3, 0x4391, 0x0608, 0x55f5, 0x106c, 0x1b5e, 0x5ec7, + 0x54aa, 0x1133, 0x1a01, 0x5f98, 0x0c65, 0x49fc, 0x42ce, 0x0757, 0x20ad, 0x6534, 0x6e06, 0x2b9f, 0x7862, 0x3dfb, 0x36c9, 0x7350, + 0x51d6, 0x144f, 0x1f7d, 0x5ae4, 0x0919, 0x4c80, 0x47b2, 0x022b, 0x25d1, 0x6048, 0x6b7a, 0x2ee3, 0x7d1e, 0x3887, 0x33b5, 0x762c, + 0x7c41, 0x39d8, 0x32ea, 0x7773, 0x248e, 0x6117, 0x6a25, 0x2fbc, 0x0846, 0x4ddf, 0x46ed, 0x0374, 0x5089, 0x1510, 0x1e22, 0x5bbb, + 0x0af8, 0x4f61, 0x4453, 0x01ca, 0x5237, 0x17ae, 0x1c9c, 0x5905, 0x7eff, 0x3b66, 0x3054, 0x75cd, 0x2630, 0x63a9, 0x689b, 0x2d02, + 0x276f, 0x62f6, 0x69c4, 0x2c5d, 0x7fa0, 0x3a39, 0x310b, 0x7492, 0x5368, 0x16f1, 0x1dc3, 0x585a, 0x0ba7, 0x4e3e, 0x450c, 0x0095 +}; + +/** + * Update the crc value with new data. + * + * \param crc The current crc value. + * \param data Pointer to a buffer of \a data_len bytes. + * \param data_len Number of bytes in the \a data buffer. + * \return The updated crc value. + *****************************************************************************/ +static crc_t crc_update_bytewise(crc_t crc, const unsigned char *data, size_t data_len) +{ + unsigned int tbl_idx; + + while (data_len--) { + tbl_idx = ((crc >> 7) ^ *data) & 0xff; + crc = (crc_table[tbl_idx] ^ (crc << 8)) & 0x7fff; + + data++; + } + return crc & 0x7fff; +} + +/** + * Update the crc value with new data. + * + * \param crc The current crc value. + * \param data Data value + * \param bits The number of most significant bits in data used for CRC calculation + * \return The updated crc value. + *****************************************************************************/ +static crc_t crc_update_bitwise(crc_t crc, uint8_t data, size_t bits) +{ + uint8_t i; + bool bit; + + for (i = 0x80; bits--; i >>= 1) { + bit = crc & 0x4000; + if (data & i) { + bit = !bit; + } + crc <<= 1; + if (bit) { + crc ^= 0x4599; + } + } + return crc & 0x7fff; +} + +static crc_t calc_bitmap_crc(uint8_t *bitmap, unsigned start, unsigned end) +{ + crc_t crc = 0; + + if (start % 8) { + crc = crc_update_bitwise(crc, bitmap[start / 8] << (start % 8), 8 - start % 8); + start += 8 - start % 8; + } + crc = crc_update_bytewise(crc, &bitmap[start / 8], (end - start) / 8); + crc = crc_update_bitwise(crc, bitmap[end / 8], end % 8); + return crc; +} + +static unsigned cfl_exact(struct can_frame *frame) +{ + uint8_t bitmap[16]; + unsigned start = 0, end; + crc_t crc; + uint16_t crc_be; + uint8_t mask, lookfor; + unsigned i, stuffed; + const int8_t clz[32] = /* count of leading zeros in 5 bit numbers */ + { 5, 4, 3, 3, 2, 2, 2, 2, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; + + /* Prepare bitmap */ + memset(bitmap, 0, sizeof(bitmap)); + if (frame->can_id & CAN_EFF_FLAG) { + /* bit 7 0 7 0 7 0 7 0 + * bitmap[0-3] |.sBBBBBB BBBBBSIE EEEEEEEE EEEEEEEE| s = SOF, B = Base ID (11 bits), S = SRR, I = IDE, E = Extended ID (18 bits) + * bitmap[4-7] |ER10DLC4 00000000 11111111 22222222| R = RTR, 0 = r0, 1 = r1, DLC4 = DLC, Data bytes + * bitmap[8-11] |33333333 44444444 55555555 66666666| Data bytes + * bitmap[12-15] |77777777 ........ ........ ........| Data bytes + */ + bitmap[0] = (frame->can_id & CAN_EFF_MASK) >> 23; + bitmap[1] = ((frame->can_id >> 18) & 0x3f) << 3 | + 3 << 1 | /* SRR, IDE */ + ((frame->can_id >> 17) & 0x01); + bitmap[2] = (frame->can_id >> 9) & 0xff; + bitmap[3] = (frame->can_id >> 1) & 0xff; + bitmap[4] = (frame->can_id & 0x1) << 7 | + (!!(frame->can_id & CAN_RTR_FLAG)) << 6 | + 0 << 4 | /* r1, r0 */ + (frame->can_dlc & 0xf); + memcpy(&bitmap[5], &frame->data, frame->can_dlc); + start = 1; + end = 40 + 8*frame->can_dlc; + } else { + /* bit 7 0 7 0 7 0 7 0 + * bitmap[0-3] |.....sII IIIIIIII IRE0DLC4 00000000| s = SOF, I = ID (11 bits), R = RTR, E = IDE, DLC4 = DLC + * bitmap[4-7] |11111111 22222222 33333333 44444444| Data bytes + * bitmap[8-11] |55555555 66666666 77777777 ........| Data bytes + */ + bitmap[0] = (frame->can_id & CAN_SFF_MASK) >> 9; + bitmap[1] = (frame->can_id >> 1) & 0xff; + bitmap[2] = ((frame->can_id << 7) & 0xff) | + (!!(frame->can_id & CAN_RTR_FLAG)) << 6 | + 0 << 4 | /* IDE, r0 */ + (frame->can_dlc & 0xf); + memcpy(&bitmap[3], &frame->data, frame->can_dlc); + start = 5; + end = 24 + 8 * frame->can_dlc; + } + + /* Calc and append CRC */ + crc = calc_bitmap_crc(bitmap, start, end); + crc_be = htons(crc << 1); + assert(end % 8 == 0); + memcpy(bitmap + end / 8, &crc_be, 2); + end += 15; + + /* Count stuffed bits */ + mask = 0x1f; + lookfor = 0; + i = start; + stuffed = 0; + while (i < end) { + unsigned change; + unsigned bits = (bitmap[i / 8] << 8 | bitmap[i / 8 + 1]) >> (16 - 5 - i % 8); + lookfor = lookfor ? 0 : mask; /* We alternate between looking for a series of zeros or ones */ + change = (bits & mask) ^ lookfor; /* 1 indicates a change */ + if (change) { /* No bit was stuffed here */ + i += clz[change]; + mask = 0x1f; /* Next look for 5 same bits */ + } else { + i += (mask == 0x1f) ? 5 : 4; + if (i <= end) { + stuffed++; + mask = 0x1e; /* Next look for 4 bits (5th bit is the stuffed one) */ + } + } + } + return end - start + stuffed + + 3 + /* CRC del, ACK, ACK del */ + 7 + /* EOF */ + 3; /* IFS */ +} + +unsigned can_frame_dbitrate_length(struct canfd_frame *frame, enum cfl_mode mode, int mtu) +{ + if (mtu != CANFD_MTU || !(frame->flags & CANFD_BRS)) + return 0; + switch (mode) { + case CFL_NO_BITSTUFFING: + return 1 /* brs/crcdel */ + 1 /* esi */ + 4 /* dlc */ + + ((frame->len >= 16) ? 21 : 17) + + frame->len * 8; + case CFL_WORSTCASE: + return can_frame_dbitrate_length(frame, CFL_NO_BITSTUFFING, mtu) * 5 / 4; + default: + return 0; + } +} + +unsigned can_frame_length(struct canfd_frame *frame, enum cfl_mode mode, int mtu) +{ + int eff = (frame->can_id & CAN_EFF_FLAG); + + if (mtu == CANFD_MTU) + /* not correct, but close ? */ + switch (mode) { + case CFL_NO_BITSTUFFING: + return 1 + (eff ? 29 : 11) + ((frame->len >= 16) ? 21 : 17) + + 5 /* r1, ide, edl, r0, brs/crcdel, */ + 12 /* trail */ + + frame->len * 8; + case CFL_WORSTCASE: + return can_frame_length(frame, CFL_NO_BITSTUFFING, mtu) * 5 / 4; + case CFL_EXACT: + return 0; /* exact bittiming for CANFD not supported yet */ + } + else if (mtu != CAN_MTU) + return 0; /* Only CAN2.0 and CANFD supported now */ + + switch (mode) { + case CFL_NO_BITSTUFFING: + return (eff ? 67 : 47) + frame->len * 8; + case CFL_WORSTCASE: + return (eff ? 80 : 55) + frame->len * 10; + case CFL_EXACT: + return cfl_exact((struct can_frame*)frame); + } + return 0; /* Unknown mode */ +} diff --git a/Devices/Libraries/Systems/can-utils/canframelen.h b/Devices/Libraries/Systems/can-utils/canframelen.h new file mode 100755 index 0000000..47739b2 --- /dev/null +++ b/Devices/Libraries/Systems/can-utils/canframelen.h @@ -0,0 +1,85 @@ +/* SPDX-License-Identifier: (GPL-2.0-only OR BSD-3-Clause) */ +/* + * canframelen.h + * + * Copyright (c) 2013, 2014 Czech Technical University in Prague + * + * Author: Michal Sojka + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * 3. Neither the name of Czech Technical University in Prague nor the + * names of its contributors may be used to endorse or promote + * products derived from this software without specific prior + * written permission. + * + * Alternatively, provided that this notice is retained in full, this + * software may be distributed under the terms of the GNU General + * Public License ("GPL") version 2, in which case the provisions of the + * GPL apply INSTEAD OF those given above. + * + * The provided data structures and external interfaces from this code + * are not restricted to be used by modules with a GPL compatible license. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT + * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY + * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH + * DAMAGE. + * + * Send feedback to + * + */ + +#ifndef CANFRAMELEN_H +#define CANFRAMELEN_H + +#include + +/** + * Frame length calculation modes. + * + * CFL_WORSTCASE corresponds to *worst* case calculation for + * stuff-bits - see (1)-(3) in [1]. The worst case number of bits on + * the wire can be calculated as: + * + * (34 + 8n - 1)/4 + 34 + 8n + 13 for SFF frames (11 bit CAN-ID) => 55 + 10n + * (54 + 8n - 1)/4 + 54 + 8n + 13 for EFF frames (29 bit CAN-ID) => 80 + 10n + * + * while 'n' is the data length code (number of payload bytes) + * + * [1] "Controller Area Network (CAN) schedulability analysis: + * Refuted, revisited and revised", Real-Time Syst (2007) + * 35:239-272. + * + */ +enum cfl_mode { + CFL_NO_BITSTUFFING, /* plain bit calculation without bitstuffing */ + CFL_WORSTCASE, /* worst case estimation - see above */ + CFL_EXACT, /* exact calculation of stuffed bits based on frame + * content and CRC */ +}; + +/** + * Calculates the number of bits a frame needs on the wire (including + * inter frame space). + * + * Mode determines how to deal with stuffed bits. + */ +unsigned can_frame_length(struct canfd_frame *frame, enum cfl_mode mode, int mtu); +unsigned can_frame_dbitrate_length(struct canfd_frame *frame, enum cfl_mode mode, int mtu); + +#endif diff --git a/Devices/Libraries/Systems/can-utils/cangen.c b/Devices/Libraries/Systems/can-utils/cangen.c new file mode 100755 index 0000000..d9a0448 --- /dev/null +++ b/Devices/Libraries/Systems/can-utils/cangen.c @@ -0,0 +1,1090 @@ +/* SPDX-License-Identifier: (GPL-2.0-only OR BSD-3-Clause) */ +/* + * cangen.c - CAN frames generator + * + * Copyright (c) 2022 Pengutronix, + * Marc Kleine-Budde + * Copyright (c) 2002-2007 Volkswagen Group Electronic Research + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * 3. Neither the name of Volkswagen nor the names of its contributors + * may be used to endorse or promote products derived from this software + * without specific prior written permission. + * + * Alternatively, provided that this notice is retained in full, this + * software may be distributed under the terms of the GNU General + * Public License ("GPL") version 2, in which case the provisions of the + * GPL apply INSTEAD OF those given above. + * + * The provided data structures and external interfaces from this code + * are not restricted to be used by modules with a GPL compatible license. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT + * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY + * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH + * DAMAGE. + * + * Send feedback to + * + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "lib.h" + +#define DEFAULT_GAP 200 /* ms */ +#define DEFAULT_BURST_COUNT 1 +#define DEFAULT_SO_MARK_VAL 1 + +#define MODE_RANDOM 0 +#define MODE_INCREMENT 1 +#define MODE_FIX 2 +#define MODE_RANDOM_EVEN 3 +#define MODE_RANDOM_ODD 4 +#define MODE_RANDOM_FIX 5 + +#define NIBBLE_H 1 +#define NIBBLE_L 2 + +#define CHAR_RANDOM 'x' + +extern int optind, opterr, optopt; + +static volatile int running = 1; +static volatile sig_atomic_t signal_num; +static unsigned long long enobufs_count; +static bool ignore_enobufs; +static bool use_so_txtime; + +static int clockid = CLOCK_TAI; +static int clock_nanosleep_flags; +static struct timespec ts, ts_gap; +static int so_mark_val = DEFAULT_SO_MARK_VAL; + +#define NSEC_PER_SEC 1000000000LL + +static struct timespec timespec_normalise(struct timespec ts) +{ + while (ts.tv_nsec >= NSEC_PER_SEC) { + ++(ts.tv_sec); + ts.tv_nsec -= NSEC_PER_SEC; + } + + while (ts.tv_nsec <= -NSEC_PER_SEC) { + --(ts.tv_sec); + ts.tv_nsec += NSEC_PER_SEC; + } + + if (ts.tv_nsec < 0) { + /* + * Negative nanoseconds isn't valid according to + * POSIX. Decrement tv_sec and roll tv_nsec over. + */ + + --(ts.tv_sec); + ts.tv_nsec = (NSEC_PER_SEC + ts.tv_nsec); + } + + return ts; +} + +static struct timespec timespec_add(struct timespec ts1, struct timespec ts2) +{ + /* + * Normalize inputs to prevent tv_nsec rollover if + * whole-second values are packed in it. + */ + ts1 = timespec_normalise(ts1); + ts2 = timespec_normalise(ts2); + + ts1.tv_sec += ts2.tv_sec; + ts1.tv_nsec += ts2.tv_nsec; + + return timespec_normalise(ts1); +} + +struct timespec double_to_timespec(double s) +{ + struct timespec ts = { + .tv_sec = s, + .tv_nsec = (s - (long)(s)) * NSEC_PER_SEC, + }; + + return timespec_normalise(ts); +} + +static struct timespec ns_to_timespec(int64_t ns) +{ + struct timespec ts = { + .tv_sec = ns / NSEC_PER_SEC, + .tv_nsec = ns % NSEC_PER_SEC, + }; + + return timespec_normalise(ts); +} + +static void print_usage(char *prg) +{ + fprintf(stderr, "%s - CAN frames generator.\n\n", prg); + fprintf(stderr, "Usage: %s [options] \n", prg); + fprintf(stderr, "Options:\n"); + fprintf(stderr, " -g (gap in milli seconds - default: %d ms)\n", DEFAULT_GAP); + fprintf(stderr, " -a (use absolute time for gap)\n"); + fprintf(stderr, " -t (use SO_TXTIME)\n"); + fprintf(stderr, " --start (start time (UTC nanoseconds))\n"); + fprintf(stderr, " --mark (set SO_MARK to , default %u)\n", DEFAULT_SO_MARK_VAL); + fprintf(stderr, " -e (generate extended frame mode (EFF) CAN frames)\n"); + fprintf(stderr, " -f (generate CAN FD CAN frames)\n"); + fprintf(stderr, " -b (generate CAN FD CAN frames with bitrate switch (BRS))\n"); + fprintf(stderr, " -E (generate CAN FD CAN frames with error state (ESI))\n"); + fprintf(stderr, " -X (generate CAN XL CAN frames)\n"); + fprintf(stderr, " -R (generate RTR frames)\n"); + fprintf(stderr, " -8 (allow DLC values greater then 8 for Classic CAN frames)\n"); + fprintf(stderr, " -m (mix -e -f -b -E -R -X frames)\n"); + fprintf(stderr, " -I (CAN ID generation mode - see below)\n"); + fprintf(stderr, " -L (CAN data length code (dlc) generation mode - see below)\n"); + fprintf(stderr, " -D (CAN data (payload) generation mode - see below)\n"); + fprintf(stderr, " -F (CAN XL Flags generation mode - see below, no e/o mode)\n"); + fprintf(stderr, " -S (CAN XL SDT generation mode - see below, no e/o mode)\n"); + fprintf(stderr, " -A (CAN XL AF generation mode - see below, no e/o mode)\n"); + fprintf(stderr, " -V (CAN XL VCID generation mode - see below, no e/o mode)\n"); + fprintf(stderr, " -p (poll on -ENOBUFS to write frames with ms)\n"); + fprintf(stderr, " -n (terminate after CAN frames - default infinite)\n"); + fprintf(stderr, " -i (ignore -ENOBUFS return values on write() syscalls)\n"); + fprintf(stderr, " -x (disable local loopback of generated CAN frames)\n"); + fprintf(stderr, " -c (number of messages to send in burst, default %u)\n", DEFAULT_BURST_COUNT); + fprintf(stderr, " -v (increment verbose level for printing sent CAN frames)\n\n"); + fprintf(stderr, "Generation modes:\n"); + fprintf(stderr, " 'r' => random values (default)\n"); + fprintf(stderr, " 'e' => random values, even ID\n"); + fprintf(stderr, " 'o' => random values, odd ID\n"); + fprintf(stderr, " 'i' => increment values\n"); + fprintf(stderr, " => fixed value (in hexadecimal for -I and -D)\n"); + fprintf(stderr, " => nibbles written as '%c' are randomized (only -D)\n\n", CHAR_RANDOM); + fprintf(stderr, "The gap value (in milliseconds) may have decimal places, e.g. '-g 4.73'\n"); + fprintf(stderr, "When incrementing the CAN data the data length code minimum is set to 1.\n"); + fprintf(stderr, "CAN IDs and data content are given and expected in hexadecimal values.\n\n"); + fprintf(stderr, "Examples:\n"); + fprintf(stderr, "%s vcan0 -g 4 -I 42A -L 1 -D i -v -v\n", prg); + fprintf(stderr, "\t(fixed CAN ID and length, inc. data)\n"); + fprintf(stderr, "%s vcan0 -e -L i -v -v -v\n", prg); + fprintf(stderr, "\t(generate EFF frames, incr. length)\n"); + fprintf(stderr, "%s vcan0 -D 11223344DEADBEEF -L 8\n", prg); + fprintf(stderr, "\t(fixed CAN data payload and length)\n"); + fprintf(stderr, "%s vcan0 -D 11%c%c3344DEADBEEF -L 8\n", prg, CHAR_RANDOM, CHAR_RANDOM); + fprintf(stderr, "\t(fixed CAN data payload where 2. byte is randomized, fixed length)\n"); + fprintf(stderr, "%s vcan0 -I 555 -D CCCCCCCCCCCCCCCC -L 8 -g 3.75\n", prg); + fprintf(stderr, "\t(generate a fix busload without bit-stuffing effects)\n"); + fprintf(stderr, "%s vcan0 -g 0 -i -x\n", prg); + fprintf(stderr, "\t(full load test ignoring -ENOBUFS)\n"); + fprintf(stderr, "%s vcan0 -g 0 -p 10 -x\n", prg); + fprintf(stderr, "\t(full load test with polling, 10ms timeout)\n"); + fprintf(stderr, "%s vcan0\n", prg); + fprintf(stderr, "\t(my favourite default :)\n\n"); +} + +static void sigterm(int signo) +{ + running = 0; + signal_num = signo; +} + +static int setsockopt_txtime(int fd) +{ + const struct sock_txtime so_txtime_val = { + .clockid = clockid, + .flags = SOF_TXTIME_REPORT_ERRORS, + }; + struct sock_txtime so_txtime_val_read; + int so_mark_val_read; + socklen_t vallen; + int ret; + + /* SO_TXTIME */ + + ret = setsockopt(fd, SOL_SOCKET, SO_TXTIME, + &so_txtime_val, sizeof(so_txtime_val)); + if (ret) { + int err = errno; + + perror("setsockopt() SO_TXTIME"); + if (err == EPERM) + fprintf(stderr, "Run with CAP_NET_ADMIN or as root.\n"); + + return -err; + }; + + vallen = sizeof(so_txtime_val_read); + ret = getsockopt(fd, SOL_SOCKET, SO_TXTIME, + &so_txtime_val_read, &vallen); + if (ret) { + perror("getsockopt() SO_TXTIME"); + return -errno; + }; + + if (vallen != sizeof(so_txtime_val) || + memcmp(&so_txtime_val, &so_txtime_val_read, vallen)) { + perror("getsockopt() SO_TXTIME: mismatch"); + return -EINVAL; + } + + /* SO_MARK */ + + ret = setsockopt(fd, SOL_SOCKET, SO_MARK, &so_mark_val, sizeof(so_mark_val)); + if (ret) { + int err = errno; + + perror("setsockopt() SO_MARK"); + if (err == EPERM) + fprintf(stderr, "Run with CAP_NET_ADMIN or as root.\n"); + + return -err; + }; + + vallen = sizeof(so_mark_val_read); + ret = getsockopt(fd, SOL_SOCKET, SO_MARK, + &so_mark_val_read, &vallen); + if (ret) { + perror("getsockopt() SO_MARK"); + return -errno; + }; + + if (vallen != sizeof(so_mark_val) || + memcmp(&so_mark_val, &so_mark_val_read, vallen)) { + perror("getsockopt() SO_MARK: mismatch"); + return -EINVAL; + } + + return 0; +} + +static int do_send_one(int fd, cu_t *cu, size_t len, int timeout) +{ + uint8_t control[CMSG_SPACE(sizeof(uint64_t))] = { 0 }; + struct iovec iov = { + .iov_base = cu, + }; + struct msghdr msg = { + .msg_iov = &iov, + .msg_iovlen = 1, + }; + ssize_t nbytes; + int ret; + + /* CAN XL frames need real frame length for sending */ + if (len == CANXL_MTU) + len = CANXL_HDR_SIZE + cu->xl.len; + + iov.iov_len = len; + + if (use_so_txtime) { + struct cmsghdr *cm; + uint64_t tdeliver; + + msg.msg_control = control; + msg.msg_controllen = sizeof(control); + + tdeliver = ts.tv_sec * NSEC_PER_SEC + ts.tv_nsec; + ts = timespec_add(ts, ts_gap); + + cm = CMSG_FIRSTHDR(&msg); + cm->cmsg_level = SOL_SOCKET; + cm->cmsg_type = SCM_TXTIME; + cm->cmsg_len = CMSG_LEN(sizeof(tdeliver)); + memcpy(CMSG_DATA(cm), &tdeliver, sizeof(tdeliver)); + } + +resend: + nbytes = sendmsg(fd, &msg, 0); + if (nbytes < 0) { + ret = -errno; + if (ret != -ENOBUFS) { + perror("write"); + return ret; + } + if (!ignore_enobufs && !timeout) { + perror("write"); + return ret; + } + if (timeout) { + struct pollfd fds = { + .fd = fd, + .events = POLLOUT, + }; + + /* wait for the write socket (with timeout) */ + ret = poll(&fds, 1, timeout); + if (ret == 0 || (ret == -1 && errno != EINTR)) { + ret = -errno; + perror("poll"); + return ret; + } + goto resend; + } else { + enobufs_count++; + } + + } else if (nbytes < (ssize_t)len) { + fprintf(stderr, "write: incomplete CAN frame\n"); + return -EINVAL; + } + + return 0; +} + +static int setup_time(void) +{ + int ret; + + if (use_so_txtime) { + /* start time is defined */ + if (ts.tv_sec || ts.tv_nsec) + return 0; + + /* start time is now .... */ + ret = clock_gettime(clockid, &ts); + if (ret) { + perror("clock_gettime"); + return ret; + } + + /* ... + gap */ + ts = timespec_add(ts, ts_gap); + + return 0; + } + + if (ts.tv_sec || ts.tv_nsec) { + ret = clock_nanosleep(clockid, TIMER_ABSTIME, &ts, NULL); + if (ret != 0 && ret != EINTR) { + perror("clock_nanosleep"); + return ret; + } + } else if (clock_nanosleep_flags == TIMER_ABSTIME) { + ret = clock_gettime(clockid, &ts); + if (ret) + perror("clock_gettime"); + + return ret; + } + + if (clock_nanosleep_flags != TIMER_ABSTIME) + ts = ts_gap; + + return 0; +} + +enum { + OPT_MARK = UCHAR_MAX + 1, + OPT_START = UCHAR_MAX + 2, +}; + +/* + * Search for CHAR_RANDOM in dataoptarg, save its position, replace it with 0. + * Return 1 if at least one CHAR_RANDOM found. + */ +static int parse_dataoptarg(char *dataoptarg, unsigned char *rand_position) +{ + int mode_format_selected = MODE_FIX; + int arglen = strlen(dataoptarg); + int i; + + /* Mark nibbles with * as fuzzable */ + for (i = 0; i < CANFD_MAX_DLEN && i < arglen / 2; i++) { + if (optarg[2 * i] == CHAR_RANDOM) { + optarg[2 * i] = '0'; + rand_position[i] += NIBBLE_H; + mode_format_selected = MODE_RANDOM_FIX; + } + if (optarg[2 * i + 1] == CHAR_RANDOM) { + optarg[2 * i + 1] = '0'; + rand_position[i] += NIBBLE_L; + mode_format_selected = MODE_RANDOM_FIX; + } + } + + return mode_format_selected; +} + +int main(int argc, char **argv) +{ + double gap = DEFAULT_GAP; + unsigned long burst_count = DEFAULT_BURST_COUNT; + unsigned long polltimeout = 0; + unsigned char extended = 0; + unsigned char canfd = 0; + unsigned char canxl = 0; + unsigned char brs = 0; + unsigned char esi = 0; + unsigned char mix = 0; + unsigned char id_mode = MODE_RANDOM; + unsigned char data_mode = MODE_RANDOM; + unsigned char dlc_mode = MODE_RANDOM; + __u8 xl_flags = 0; + __u8 xl_sdt = 0; + __u32 xl_af = 0; + __u8 xl_vcid = 0; + unsigned char xl_flags_mode = MODE_RANDOM; + unsigned char xl_sdt_mode = MODE_RANDOM; + unsigned char xl_af_mode = MODE_RANDOM; + unsigned char xl_vcid_mode = MODE_RANDOM; + unsigned char loopback_disable = 0; + unsigned char verbose = 0; + unsigned char rtr_frame = 0; + unsigned char len8_dlc = 0; + unsigned char view = 0; + int count = 0; + unsigned long burst_sent_count = 0; + int mtu, maxdlen; + uint64_t incdata = 0; + __u8 *data; /* base pointer for CC/FD or XL data */ + int incdlc = 0; + unsigned long rnd; + unsigned char fixdata[CANFD_MAX_DLEN]; + unsigned char rand_position[CANFD_MAX_DLEN] = { 0 }; + + int opt; + int s; /* socket */ + + struct sockaddr_can addr = { 0 }; + struct can_raw_vcid_options vcid_opts = { + .flags = CAN_RAW_XL_VCID_TX_PASS, + }; + static cu_t cu; + int i; + struct ifreq ifr = { 0 }; + const int enable_canfx = 1; + + struct timeval now; + int ret; + + /* set seed value for pseudo random numbers */ + gettimeofday(&now, NULL); + srandom(now.tv_usec); + + signal(SIGTERM, sigterm); + signal(SIGHUP, sigterm); + signal(SIGINT, sigterm); + + const struct option long_options[] = { + { "mark", required_argument, 0, OPT_MARK, }, + { "start", required_argument, 0, OPT_START, }, + { 0, 0, 0, 0 }, + }; + + while ((opt = getopt_long(argc, argv, "g:atefbEXR8mI:L:D:F:S:A:V:p:n:ixc:vh?", long_options, NULL)) != -1) { + switch (opt) { + case 'g': + gap = strtod(optarg, NULL); + break; + + case 'a': + clock_nanosleep_flags = TIMER_ABSTIME; + break; + + case 't': + clock_nanosleep_flags = TIMER_ABSTIME; + use_so_txtime = true; + break; + + case OPT_START: { + int64_t start_time_ns; + + start_time_ns = strtoll(optarg, NULL, 0); + ts = ns_to_timespec(start_time_ns); + + break; + } + case OPT_MARK: + so_mark_val = strtoul(optarg, NULL, 0); + break; + case 'e': + extended = 1; + view |= CANLIB_VIEW_INDENT_SFF; + break; + + case 'f': + canfd = 1; + break; + + case 'b': + brs = 1; /* bitrate switch implies CAN FD */ + canfd = 1; + break; + + case 'E': + esi = 1; /* error state indicator implies CAN FD */ + canfd = 1; + break; + + case 'X': + canxl = 1; + break; + + case 'R': + rtr_frame = 1; + break; + + case '8': + len8_dlc = 1; + view |= CANLIB_VIEW_LEN8_DLC; + break; + + case 'm': + mix = 1; + canfd = 1; /* to switch the socket into CAN FD mode */ + view |= CANLIB_VIEW_INDENT_SFF; + break; + + case 'I': + if (optarg[0] == 'r') { + id_mode = MODE_RANDOM; + } else if (optarg[0] == 'i') { + id_mode = MODE_INCREMENT; + } else if (optarg[0] == 'e') { + id_mode = MODE_RANDOM_EVEN; + } else if (optarg[0] == 'o') { + id_mode = MODE_RANDOM_ODD; + } else { + id_mode = MODE_FIX; + cu.fd.can_id = strtoul(optarg, NULL, 16); + } + break; + + case 'L': + if (optarg[0] == 'r') { + dlc_mode = MODE_RANDOM; + } else if (optarg[0] == 'i') { + dlc_mode = MODE_INCREMENT; + } else { + dlc_mode = MODE_FIX; + cu.fd.len = atoi(optarg) & 0xFF; /* is cut to 8 / 64 later */ + } + break; + + case 'D': + if (optarg[0] == 'r') { + data_mode = MODE_RANDOM; + } else if (optarg[0] == 'i') { + data_mode = MODE_INCREMENT; + } else { + data_mode = parse_dataoptarg(optarg, rand_position); + + if (hexstring2data(optarg, fixdata, CANFD_MAX_DLEN)) { + printf("wrong fix data definition\n"); + return 1; + } + } + break; + + case 'F': + if (optarg[0] == 'r') { + xl_flags_mode = MODE_RANDOM; + } else if (optarg[0] == 'i') { + xl_flags_mode = MODE_INCREMENT; + } else { + xl_flags_mode = MODE_FIX; + if (sscanf(optarg, "%hhx", &xl_flags) != 1) { + printf("Bad xl_flags definition '%s'.\n", optarg); + exit(1); + } + } + break; + + case 'S': + if (optarg[0] == 'r') { + xl_sdt_mode = MODE_RANDOM; + } else if (optarg[0] == 'i') { + xl_sdt_mode = MODE_INCREMENT; + } else { + xl_sdt_mode = MODE_FIX; + if (sscanf(optarg, "%hhx", &xl_sdt) != 1) { + printf("Bad xl_sdt definition '%s'.\n", optarg); + exit(1); + } + } + break; + + case 'A': + if (optarg[0] == 'r') { + xl_af_mode = MODE_RANDOM; + } else if (optarg[0] == 'i') { + xl_af_mode = MODE_INCREMENT; + } else { + xl_af_mode = MODE_FIX; + xl_af = strtoul(optarg, NULL, 16); + } + break; + + case 'V': + if (optarg[0] == 'r') { + xl_vcid_mode = MODE_RANDOM; + } else if (optarg[0] == 'i') { + xl_vcid_mode = MODE_INCREMENT; + } else { + xl_vcid_mode = MODE_FIX; + if (sscanf(optarg, "%hhx", &xl_vcid) != 1) { + printf("Bad xl_vcid definition '%s'.\n", optarg); + exit(1); + } + } + break; + + case 'p': + polltimeout = strtoul(optarg, NULL, 10); + break; + + case 'n': + count = atoi(optarg); + if (count < 1) { + print_usage(basename(argv[0])); + return 1; + } + break; + + case 'i': + ignore_enobufs = true; + break; + + case 'x': + loopback_disable = 1; + break; + + case 'c': + burst_count = strtoul(optarg, NULL, 10); + break; + + case 'v': + verbose++; + break; + + case '?': + case 'h': + default: + print_usage(basename(argv[0])); + return 1; + } + } + + if (optind == argc) { + print_usage(basename(argv[0])); + return 1; + } + + if (verbose > 2) + view |= CANLIB_VIEW_ASCII; + + ts_gap = double_to_timespec(gap / 1000); + + /* recognize obviously missing commandline option */ + if (id_mode == MODE_FIX && cu.fd.can_id > 0x7FF && !extended) { + printf("The given CAN-ID is greater than 0x7FF and the '-e' option is not set.\n"); + return 1; + } + + if (strlen(argv[optind]) >= IFNAMSIZ) { + printf("Name of CAN device '%s' is too long!\n\n", argv[optind]); + return 1; + } + + s = socket(PF_CAN, SOCK_RAW, CAN_RAW); + if (s < 0) { + perror("socket"); + return 1; + } + + addr.can_family = AF_CAN; + + strcpy(ifr.ifr_name, argv[optind]); + if (ioctl(s, SIOCGIFINDEX, &ifr) < 0) { + perror("SIOCGIFINDEX"); + return 1; + } + addr.can_ifindex = ifr.ifr_ifindex; + + /* + * disable default receive filter on this RAW socket + * This is obsolete as we do not read from the socket at all, but for + * this reason we can remove the receive list in the Kernel to save a + * little (really a very little!) CPU usage. + */ + setsockopt(s, SOL_CAN_RAW, CAN_RAW_FILTER, NULL, 0); + + if (loopback_disable) { + const int loopback = 0; + + setsockopt(s, SOL_CAN_RAW, CAN_RAW_LOOPBACK, + &loopback, sizeof(loopback)); + } + + if (canfd || canxl) { + + /* check if the frame fits into the CAN netdevice */ + if (ioctl(s, SIOCGIFMTU, &ifr) < 0) { + perror("SIOCGIFMTU"); + return 1; + } + + if (canfd) { + /* ensure discrete CAN FD length values 0..8, 12, 16, 20, 24, 32, 64 */ + cu.fd.len = can_fd_dlc2len(can_fd_len2dlc(cu.fd.len)); + } else { + /* limit fixed CAN XL data length to 64 */ + if (cu.fd.len > CANFD_MAX_DLEN) + cu.fd.len = CANFD_MAX_DLEN; + } + + if (canxl && (ifr.ifr_mtu < (int)CANXL_MIN_MTU)) { + printf("CAN interface not CAN XL capable - sorry.\n"); + return 1; + } + + if (canfd && (ifr.ifr_mtu < (int)CANFD_MTU)) { + printf("CAN interface not CAN FD capable - sorry.\n"); + return 1; + } + + if (ifr.ifr_mtu == (int)CANFD_MTU) { + /* interface is ok - try to switch the socket into CAN FD mode */ + if (setsockopt(s, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, + &enable_canfx, sizeof(enable_canfx))){ + printf("error when enabling CAN FD support\n"); + return 1; + } + } + + if (ifr.ifr_mtu >= (int)CANXL_MIN_MTU) { + /* interface is ok - try to switch the socket into CAN XL mode */ + if (setsockopt(s, SOL_CAN_RAW, CAN_RAW_XL_FRAMES, + &enable_canfx, sizeof(enable_canfx))){ + printf("error when enabling CAN XL support\n"); + return 1; + } + /* try to enable the CAN XL VCID pass through mode */ + if (setsockopt(s, SOL_CAN_RAW, CAN_RAW_XL_VCID_OPTS, + &vcid_opts, sizeof(vcid_opts))) { + printf("error when enabling CAN XL VCID pass through\n"); + return 1; + } + } + + } else { + /* sanitize Classical CAN 2.0 frame length */ + if (len8_dlc) { + if (cu.cc.len > CAN_MAX_RAW_DLC) + cu.cc.len = CAN_MAX_RAW_DLC; + + if (cu.cc.len > CAN_MAX_DLEN) + cu.cc.len8_dlc = cu.cc.len; + } + + if (cu.cc.len > CAN_MAX_DLEN) + cu.cc.len = CAN_MAX_DLEN; + } + + if (bind(s, (struct sockaddr *)&addr, sizeof(addr)) < 0) { + perror("bind"); + return 1; + } + + if (use_so_txtime) { + ret = setsockopt_txtime(s); + if (ret) + return 1; + } + + ret = setup_time(); + if (ret) + return 1; + + while (running) { + /* clear values but preserve cu.fd.len */ + cu.fd.flags = 0; + cu.fd.__res0 = 0; + cu.fd.__res1 = 0; + + if (count && (--count == 0)) + running = 0; + + if (canxl) { + mtu = CANXL_MTU; + maxdlen = CANFD_MAX_DLEN; /* generate up to 64 byte */ + extended = 0; /* prio has only 11 bit ID content */ + data = cu.xl.data; /* fill CAN XL data */ + } else if (canfd) { + mtu = CANFD_MTU; + maxdlen = CANFD_MAX_DLEN; + data = cu.fd.data; /* fill CAN CC/FD data */ + cu.fd.flags = CANFD_FDF; + if (brs) + cu.fd.flags |= CANFD_BRS; + if (esi) + cu.fd.flags |= CANFD_ESI; + } else { + mtu = CAN_MTU; + maxdlen = CAN_MAX_DLEN; + data = cu.cc.data; /* fill CAN CC/FD data */ + } + + if (id_mode == MODE_RANDOM) + cu.fd.can_id = random(); + else if (id_mode == MODE_RANDOM_EVEN) + cu.fd.can_id = random() & ~0x1; + else if (id_mode == MODE_RANDOM_ODD) + cu.fd.can_id = random() | 0x1; + + if (extended) { + cu.fd.can_id &= CAN_EFF_MASK; + cu.fd.can_id |= CAN_EFF_FLAG; + } else { + cu.fd.can_id &= CAN_SFF_MASK; + } + + if (rtr_frame && !canfd && !canxl) + cu.fd.can_id |= CAN_RTR_FLAG; + + if (dlc_mode == MODE_RANDOM) { + if (canxl) + cu.fd.len = CANXL_MIN_DLEN + (random() & 0x3F); + else if (canfd) + cu.fd.len = can_fd_dlc2len(random() & 0xF); + else { + cu.cc.len = random() & 0xF; + + if (cu.cc.len > CAN_MAX_DLEN) { + /* generate Classic CAN len8 DLCs? */ + if (len8_dlc) + cu.cc.len8_dlc = cu.cc.len; + + cu.cc.len = 8; /* for about 50% of the frames */ + } else { + cu.cc.len8_dlc = 0; + } + } + } + + if (data_mode == MODE_INCREMENT && !cu.cc.len) + cu.cc.len = 1; /* min dlc value for incr. data */ + + if (data_mode == MODE_RANDOM) { + rnd = random(); + memcpy(&data[0], &rnd, 4); + rnd = random(); + memcpy(&data[4], &rnd, 4); + + /* omit extra random number generation for CAN FD */ + if ((canfd || canxl) && cu.fd.len > 8) { + memcpy(&data[8], &data[0], 8); + memcpy(&data[16], &data[0], 16); + memcpy(&data[32], &data[0], 32); + } + } + + if (data_mode == MODE_RANDOM_FIX) { + int i; + + memcpy(data, fixdata, CANFD_MAX_DLEN); + + for (i = 0; i < cu.fd.len; i++) { + if (rand_position[i] == (NIBBLE_H | NIBBLE_L)) { + data[i] = random(); + } else if (rand_position[i] == NIBBLE_H) { + data[i] = (data[i] & 0x0f) | (random() & 0xf0); + } else if (rand_position[i] == NIBBLE_L) { + data[i] = (data[i] & 0xf0) | (random() & 0x0f); + } + } + } + + if (data_mode == MODE_FIX) + memcpy(data, fixdata, CANFD_MAX_DLEN); + + /* set unused payload data to zero like the CAN driver does it on rx */ + if (cu.fd.len < maxdlen) + memset(&data[cu.fd.len], 0, maxdlen - cu.fd.len); + + if (!use_so_txtime && + (ts.tv_sec || ts.tv_nsec) && + burst_sent_count >= burst_count) { + if (clock_nanosleep_flags == TIMER_ABSTIME) + ts = timespec_add(ts, ts_gap); + + ret = clock_nanosleep(clockid, clock_nanosleep_flags, &ts, NULL); + if (ret != 0 && ret != EINTR) { + perror("clock_nanosleep"); + return 1; + } + } + + if (canxl) { + /* convert some CAN FD frame content into a CAN XL frame */ + if (cu.fd.len < CANXL_MIN_DLEN) { + cu.fd.len = CANXL_MIN_DLEN; + data[0] = 0xCC; /* default filler */ + } + cu.xl.len = cu.fd.len; + + rnd = random(); + + if (xl_flags_mode == MODE_RANDOM) { + cu.xl.flags = rnd & CANXL_SEC; + } else if (xl_flags_mode == MODE_FIX) { + cu.xl.flags = xl_flags; + } else if (xl_flags_mode == MODE_INCREMENT) { + xl_flags ^= CANXL_SEC; + cu.xl.flags = (xl_flags & CANXL_SEC); + } + + /* mark CAN XL frame */ + cu.xl.flags |= CANXL_XLF; + + if (xl_sdt_mode == MODE_RANDOM) { + cu.xl.sdt = rnd & 0xFF; + } else if (xl_sdt_mode == MODE_FIX) { + cu.xl.sdt = xl_sdt; + } else if (xl_sdt_mode == MODE_INCREMENT) { + xl_sdt++; + cu.xl.sdt = xl_sdt; + } + + if (xl_af_mode == MODE_RANDOM) { + cu.xl.af = rnd; + } else if (xl_af_mode == MODE_FIX) { + cu.xl.af = xl_af; + } else if (xl_af_mode == MODE_INCREMENT) { + xl_af++; + cu.xl.af = xl_af; + } + + if (xl_vcid_mode == MODE_RANDOM) { + cu.xl.prio |= rnd & CANXL_VCID_MASK; + } else if (xl_vcid_mode == MODE_FIX) { + cu.xl.prio |= xl_vcid << CANXL_VCID_OFFSET; + } else if (xl_vcid_mode == MODE_INCREMENT) { + xl_vcid++; + cu.xl.prio |= xl_vcid << CANXL_VCID_OFFSET; + } + } + + if (verbose) { + static char afrbuf[AFRSZ]; /* ASCII CAN frame buffer size */ + + printf(" %s ", argv[optind]); + + if (verbose > 1) + snprintf_long_canframe(afrbuf, sizeof(afrbuf), &cu, view); + else + snprintf_canframe(afrbuf, sizeof(afrbuf), &cu, 1); + + printf("%s\n", afrbuf); + } + + ret = do_send_one(s, &cu, mtu, polltimeout); + if (ret) + return 1; + + if (burst_sent_count >= burst_count) + burst_sent_count = 0; + burst_sent_count++; + + /* restore some CAN FD frame content from CAN XL frame */ + if (canxl) + cu.fd.len = cu.xl.len; + + if (id_mode == MODE_INCREMENT) + cu.cc.can_id++; + + if (dlc_mode == MODE_INCREMENT) { + incdlc++; + incdlc %= CAN_MAX_RAW_DLC + 1; + + if ((canfd || canxl) && !mix) + cu.fd.len = can_fd_dlc2len(incdlc); + else if (len8_dlc) { + if (incdlc > CAN_MAX_DLEN) { + cu.cc.len = CAN_MAX_DLEN; + cu.cc.len8_dlc = incdlc; + } else { + cu.cc.len = incdlc; + cu.cc.len8_dlc = 0; + } + } else { + incdlc %= CAN_MAX_DLEN + 1; + cu.fd.len = incdlc; + } + } + + if (data_mode == MODE_INCREMENT) { + incdata++; + + for (i = 0; i < 8; i++) + data[i] = incdata >> i * 8; + } + + if (mix) { + i = random(); + extended = i & 1; + canfd = i & 2; + if (canfd) { + brs = i & 4; + esi = i & 8; + } + /* generate CAN XL traffic if the interface is capable */ + if (ifr.ifr_mtu >= (int)CANXL_MIN_MTU) + canxl = ((i & 96) == 96); + + rtr_frame = ((i & 24) == 24); /* reduce RTR frames to 1/4 */ + } + } + + if (enobufs_count) + printf("\nCounted %llu ENOBUFS return values on write().\n\n", + enobufs_count); + + close(s); + + if (signal_num) + return 128 + signal_num; + + return 0; +} diff --git a/Devices/Libraries/Systems/can-utils/cangw.c b/Devices/Libraries/Systems/can-utils/cangw.c new file mode 100755 index 0000000..f99a956 --- /dev/null +++ b/Devices/Libraries/Systems/can-utils/cangw.c @@ -0,0 +1,1030 @@ +/* SPDX-License-Identifier: (GPL-2.0-only OR BSD-3-Clause) */ +/* + * cangw.c - manage PF_CAN netlink gateway + * + * Copyright (c) 2010 Volkswagen Group Electronic Research + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * 3. Neither the name of Volkswagen nor the names of its contributors + * may be used to endorse or promote products derived from this software + * without specific prior written permission. + * + * Alternatively, provided that this notice is retained in full, this + * software may be distributed under the terms of the GNU General + * Public License ("GPL") version 2, in which case the provisions of the + * GPL apply INSTEAD OF those given above. + * + * The provided data structures and external interfaces from this code + * are not restricted to be used by modules with a GPL compatible license. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT + * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY + * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH + * DAMAGE. + * + * Send feedback to + * + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +enum { + UNSPEC, + ADD, + DEL, + FLUSH, + LIST +}; + +struct modattr { + struct can_frame cf; + __u8 modtype; + __u8 instruction; +} __attribute__((packed)); + +struct fdmodattr { + struct canfd_frame cf; + __u8 modtype; + __u8 instruction; +} __attribute__((packed)); + + +#define RTCAN_RTA(r) ((struct rtattr*)(((char*)(r)) + NLMSG_ALIGN(sizeof(struct rtcanmsg)))) +#define RTCAN_PAYLOAD(n) NLMSG_PAYLOAD(n,sizeof(struct rtcanmsg)) + +/* some netlink helpers stolen from iproute2 package */ +#define NLMSG_TAIL(nmsg) \ + ((struct rtattr *)(((char *) (nmsg)) + NLMSG_ALIGN((nmsg)->nlmsg_len))) + +static int addattr_l(struct nlmsghdr *n, int maxlen, int type, + const void *data, int alen) +{ + int len = RTA_LENGTH(alen); + struct rtattr *rta; + + if ((int)(NLMSG_ALIGN(n->nlmsg_len) + RTA_ALIGN(len)) > maxlen) { + fprintf(stderr, "addattr_l: message exceeded bound of %d\n", + maxlen); + return -1; + } + rta = NLMSG_TAIL(n); + rta->rta_type = type; + rta->rta_len = len; + memcpy(RTA_DATA(rta), data, alen); + n->nlmsg_len = NLMSG_ALIGN(n->nlmsg_len) + RTA_ALIGN(len); + return 0; +} + +static void printfilter(const void *data) +{ + struct can_filter *filter = (struct can_filter *)data; + + if (filter->can_id & CAN_INV_FILTER) + printf("-f %03X~%X ", (filter->can_id & ~CAN_INV_FILTER), filter->can_mask); + else + printf("-f %03X:%X ", filter->can_id, filter->can_mask); +} + +static void printmod(const char *type, const void *data) +{ + struct modattr mod; + int i; + + memcpy (&mod, data, CGW_MODATTR_LEN); + + printf("-m %s:", type); + + if (mod.modtype & CGW_MOD_ID) + printf("I"); + + if (mod.modtype & CGW_MOD_DLC) + printf("L"); + + if (mod.modtype & CGW_MOD_DATA) + printf("D"); + + printf(":%03X.%X.", mod.cf.can_id, mod.cf.can_dlc); + + for (i = 0; i < CAN_MAX_DLEN; i++) + printf("%02X", mod.cf.data[i]); + + printf(" "); +} + +static void printfdmod(const char *type, const void *data) +{ + struct fdmodattr mod; + int i; + + memcpy (&mod, data, CGW_FDMODATTR_LEN); + + printf("-M %s:", type); + + if (mod.modtype & CGW_MOD_ID) + printf("I"); + + if (mod.modtype & CGW_MOD_FLAGS) + printf("F"); + + if (mod.modtype & CGW_MOD_LEN) + printf("L"); + + if (mod.modtype & CGW_MOD_DATA) + printf("D"); + + printf(":%03X.%X.%X.", mod.cf.can_id, mod.cf.flags, mod.cf.len); + + for (i = 0; i < CANFD_MAX_DLEN; i++) + printf("%02X", mod.cf.data[i]); + + printf(" "); +} + +static void print_cs_xor(struct cgw_csum_xor *cs_xor) +{ + printf("-x %d:%d:%d:%02X ", + cs_xor->from_idx, cs_xor->to_idx, + cs_xor->result_idx, cs_xor->init_xor_val); +} + +static void print_cs_crc8_profile(struct cgw_csum_crc8 *cs_crc8) +{ + int i; + + printf("-p %d:", cs_crc8->profile); + + switch (cs_crc8->profile) { + + case CGW_CRC8PRF_1U8: + + printf("%02X", cs_crc8->profile_data[0]); + break; + + case CGW_CRC8PRF_16U8: + + for (i = 0; i < 16; i++) + printf("%02X", cs_crc8->profile_data[i]); + break; + + case CGW_CRC8PRF_SFFID_XOR: + break; + + default: + printf("", cs_crc8->profile); + } + + printf(" "); +} + +static void print_cs_crc8(struct cgw_csum_crc8 *cs_crc8) +{ + int i; + + printf("-c %d:%d:%d:%02X:%02X:", + cs_crc8->from_idx, cs_crc8->to_idx, + cs_crc8->result_idx, cs_crc8->init_crc_val, + cs_crc8->final_xor_val); + + for (i = 0; i < 256; i++) + printf("%02X", cs_crc8->crctab[i]); + + printf(" "); + + if (cs_crc8->profile != CGW_CRC8PRF_UNSPEC) + print_cs_crc8_profile(cs_crc8); +} + +static void print_usage(char *prg) +{ + fprintf(stderr, "%s - manage PF_CAN netlink gateway.\n", prg); + fprintf(stderr, "\nUsage: %s [options]\n\n", prg); + fprintf(stderr, "Commands:\n"); + fprintf(stderr, " -A (add a new rule)\n"); + fprintf(stderr, " -D (delete a rule)\n"); + fprintf(stderr, " -F (flush / delete all rules)\n"); + fprintf(stderr, " -L (list all rules)\n"); + fprintf(stderr, "Mandatory:\n"); + fprintf(stderr, " -s (source netdevice)\n"); + fprintf(stderr, " -d (destination netdevice)\n"); + fprintf(stderr, "Options:\n"); + fprintf(stderr, " -X (this is a CAN FD rule)\n"); + fprintf(stderr, " -t (preserve src_dev rx timestamp)\n"); + fprintf(stderr, " -e (echo sent frames - recommended on vcanx)\n"); + fprintf(stderr, " -i (allow to route to incoming interface)\n"); + fprintf(stderr, " -u (user defined modification identifier)\n"); + fprintf(stderr, " -l (limit the number of frame hops / routings)\n"); + fprintf(stderr, " -f (set CAN filter)\n"); + fprintf(stderr, " -m (set Classical CAN frame modifications)\n"); + fprintf(stderr, " -M (set CAN FD frame modifications)\n"); + fprintf(stderr, " -x ::: (XOR checksum)\n"); + fprintf(stderr, " -c ::::: (CRC8 cs)\n"); + fprintf(stderr, " -p :[] (CRC8 checksum profile & parameters)\n"); + fprintf(stderr, "\nValues are given and expected in hexadecimal values. Leading 0s can be omitted.\n"); + fprintf(stderr, "\n"); + fprintf(stderr, " is a CAN identifier filter:\n"); + fprintf(stderr, " : (matches when & mask == can_id & mask)\n"); + fprintf(stderr, " ~ (matches when & mask != can_id & mask)\n"); + fprintf(stderr, "\n"); + fprintf(stderr, " is a Classical CAN frame modification instruction consisting of\n"); + fprintf(stderr, "::..\n"); + fprintf(stderr, " is one of 'AND' 'OR' 'XOR' 'SET'\n"); + fprintf(stderr, " is _one_ or _more_ of 'I'dentifier 'L'ength 'D'ata\n"); + fprintf(stderr, " is an u32 value containing the CAN Identifier\n"); + fprintf(stderr, " is an u8 value containing the data length code in hex (0 .. F)\n"); + fprintf(stderr, " is always eight(!) u8 values containing the CAN frames data\n"); + fprintf(stderr, "\n"); + fprintf(stderr, " is a CAN FD frame modification instruction consisting of\n"); + fprintf(stderr, "::...\n"); + fprintf(stderr, " is one of 'AND' 'OR' 'XOR' 'SET'\n"); + fprintf(stderr, " is _one_ or _more_ of 'I'd 'F'lags 'L'ength 'D'ata\n"); + fprintf(stderr, " is an u32 value containing the CAN FD Identifier\n"); + fprintf(stderr, " is an u8 value containing CAN FD flags (CANFD_BRS, CANFD_ESI)\n"); + fprintf(stderr, " is an u8 value containing the data length in hex (0 .. 40)\n"); + fprintf(stderr, " is always 64(!) u8 values containing the CAN FD frames data\n"); + fprintf(stderr, "The max. four modifications are performed in the order AND -> OR -> XOR -> SET\n"); + fprintf(stderr, "\n"); + fprintf(stderr, "Supported CRC 8 profiles:\n"); + fprintf(stderr, " Profile '%d' (1U8) add one additional u8 value\n", CGW_CRC8PRF_1U8); + fprintf(stderr, " Profile '%d' (16U8) add u8 value from table[16] indexed by (data[1] & 0xF)\n", CGW_CRC8PRF_16U8); + fprintf(stderr, " Profile '%d' (SFFID_XOR) add u8 value (can_id & 0xFF) ^ (can_id >> 8 & 0xFF)\n", CGW_CRC8PRF_SFFID_XOR); + fprintf(stderr, "\n"); + fprintf(stderr, "Examples:\n"); + fprintf(stderr, "%s -A -s can0 -d vcan3 -e -f 123:C00007FF -m SET:IL:333.4.1122334455667788\n", prg); + fprintf(stderr, "\n"); +} + +static int b64hex(char *asc, unsigned char *bin, int len) +{ + int i; + + for (i = 0; i < len; i++) { + if (!sscanf(asc+(i*2), "%2hhx", bin+i)) + return 1; + } + return 0; +} + +static int parse_crc8_profile(char *optarg, struct cgw_csum_crc8 *crc8) +{ + int ret = 1; + char *ptr; + + if (sscanf(optarg, "%hhu:", &crc8->profile) != 1) + return ret; + + switch (crc8->profile) { + + case CGW_CRC8PRF_1U8: + + if (sscanf(optarg, "%hhu:%2hhx", &crc8->profile, &crc8->profile_data[0]) == 2) + ret = 0; + + break; + + case CGW_CRC8PRF_16U8: + + ptr = strchr(optarg, ':'); + + /* check if length contains 16 base64 hex values */ + if (ptr != NULL && + strlen(ptr) == strlen(":00112233445566778899AABBCCDDEEFF") && + b64hex(ptr+1, (unsigned char *)&crc8->profile_data[0], 16) == 0) + ret = 0; + + break; + + case CGW_CRC8PRF_SFFID_XOR: + + /* no additional parameters needed */ + ret = 0; + break; + } + + return ret; +} + +static int parse_mod(char *optarg, struct modattr *modmsg) +{ + char *ptr, *nptr; + char hexdata[(CAN_MAX_DLEN * 2) + 1] = {0}; + canid_t can_id; + + ptr = optarg; + nptr = strchr(ptr, ':'); + + if ((nptr - ptr > 3) || (nptr - ptr == 0)) + return 1; + + if (!strncmp(ptr, "AND", 3)) + modmsg->instruction = CGW_MOD_AND; + else if (!strncmp(ptr, "OR", 2)) + modmsg->instruction = CGW_MOD_OR; + else if (!strncmp(ptr, "XOR", 3)) + modmsg->instruction = CGW_MOD_XOR; + else if (!strncmp(ptr, "SET", 3)) + modmsg->instruction = CGW_MOD_SET; + else + return 2; + + ptr = nptr+1; + nptr = strchr(ptr, ':'); + + if ((nptr - ptr > 3) || (nptr - ptr == 0)) + return 3; + + modmsg->modtype = 0; + + while (*ptr != ':') { + + switch (*ptr) { + + case 'I': + modmsg->modtype |= CGW_MOD_ID; + break; + + case 'L': + modmsg->modtype |= CGW_MOD_DLC; + break; + + case 'D': + modmsg->modtype |= CGW_MOD_DATA; + break; + + default: + return 4; + } + ptr++; + } + + if (sscanf(++ptr, "%x.%hhx.%16s", &can_id, + (unsigned char *)&modmsg->cf.can_dlc, hexdata) != 3) + return 5; + + modmsg->cf.can_id = can_id; + + if (strlen(hexdata) != (CAN_MAX_DLEN * 2)) + return 6; + + if (b64hex(hexdata, &modmsg->cf.data[0], CAN_MAX_DLEN)) + return 7; + + return 0; /* ok */ +} + +static int parse_fdmod(char *optarg, struct fdmodattr *modmsg) +{ + char *ptr, *nptr; + char hexdata[(CANFD_MAX_DLEN * 2) + 1] = {0}; + canid_t can_id; + + ptr = optarg; + nptr = strchr(ptr, ':'); + + if ((nptr - ptr > 3) || (nptr - ptr == 0)) + return 1; + + if (!strncmp(ptr, "AND", 3)) + modmsg->instruction = CGW_FDMOD_AND; + else if (!strncmp(ptr, "OR", 2)) + modmsg->instruction = CGW_FDMOD_OR; + else if (!strncmp(ptr, "XOR", 3)) + modmsg->instruction = CGW_FDMOD_XOR; + else if (!strncmp(ptr, "SET", 3)) + modmsg->instruction = CGW_FDMOD_SET; + else + return 2; + + ptr = nptr+1; + nptr = strchr(ptr, ':'); + + if ((nptr - ptr > 4) || (nptr - ptr == 0)) + return 3; + + modmsg->modtype = 0; + + while (*ptr != ':') { + + switch (*ptr) { + + case 'I': + modmsg->modtype |= CGW_MOD_ID; + break; + + case 'F': + modmsg->modtype |= CGW_MOD_FLAGS; + break; + + case 'L': + modmsg->modtype |= CGW_MOD_LEN; + break; + + case 'D': + modmsg->modtype |= CGW_MOD_DATA; + break; + + default: + return 4; + } + ptr++; + } + + if (sscanf(++ptr, "%x.%hhx.%hhx.%128s", &can_id, + (unsigned char *)&modmsg->cf.flags, + (unsigned char *)&modmsg->cf.len, hexdata) != 4) + return 5; + + modmsg->cf.can_id = can_id; + + if (strlen(hexdata) != (CANFD_MAX_DLEN * 2)) + return 6; + + if (b64hex(hexdata, &modmsg->cf.data[0], CANFD_MAX_DLEN)) + return 7; + + return 0; /* ok */ +} + +static int parse_rtlist(char *prgname, unsigned char *rxbuf, int len) +{ + char ifname[IF_NAMESIZE]; /* interface name for if_indextoname() */ + struct rtcanmsg *rtc; + struct rtattr *rta; + struct nlmsghdr *nlh; + unsigned int src_ifindex = 0; + unsigned int dst_ifindex = 0; + __u32 handled, dropped, deleted; + int rtlen; + + + nlh = (struct nlmsghdr *)rxbuf; + + while (1) { + if (!NLMSG_OK(nlh, len)) + return 0; + + if (nlh->nlmsg_type == NLMSG_ERROR) { + printf("NLMSG_ERROR\n"); + return 1; + } + + if (nlh->nlmsg_type == NLMSG_DONE) { + //printf("NLMSG_DONE\n"); + return 1; + } + + rtc = (struct rtcanmsg *)NLMSG_DATA(nlh); + if (rtc->can_family != AF_CAN) { + printf("received msg from unknown family %d\n", rtc->can_family); + return -EINVAL; + } + + if (rtc->gwtype != CGW_TYPE_CAN_CAN) { + printf("received msg with unknown gwtype %d\n", rtc->gwtype); + return -EINVAL; + } + + /* + * print list in a representation that + * can be used directly for start scripts. + * + * To order the mandatory and optional parameters in the + * output string, the NLMSG is parsed twice. + */ + + handled = 0; + dropped = 0; + deleted = 0; + src_ifindex = 0; + dst_ifindex = 0; + + printf("%s -A ", basename(prgname)); + + /* first parse for mandatory options */ + rta = (struct rtattr *) RTCAN_RTA(rtc); + rtlen = RTCAN_PAYLOAD(nlh); + for(;RTA_OK(rta, rtlen);rta=RTA_NEXT(rta,rtlen)) + { + //printf("(A-%d)", rta->rta_type); + switch(rta->rta_type) { + + case CGW_FILTER: + case CGW_MOD_AND: + case CGW_MOD_OR: + case CGW_MOD_XOR: + case CGW_MOD_SET: + case CGW_FDMOD_AND: + case CGW_FDMOD_OR: + case CGW_FDMOD_XOR: + case CGW_FDMOD_SET: + case CGW_MOD_UID: + case CGW_LIM_HOPS: + case CGW_CS_XOR: + case CGW_CS_CRC8: + break; + + case CGW_SRC_IF: + src_ifindex = *(__u32 *)RTA_DATA(rta); + break; + + case CGW_DST_IF: + dst_ifindex = *(__u32 *)RTA_DATA(rta); + break; + + case CGW_HANDLED: + handled = *(__u32 *)RTA_DATA(rta); + break; + + case CGW_DROPPED: + dropped = *(__u32 *)RTA_DATA(rta); + break; + + case CGW_DELETED: + deleted = *(__u32 *)RTA_DATA(rta); + break; + + default: + printf("Unknown attribute %d!", rta->rta_type); + return -EINVAL; + break; + } + } + + + printf("-s %s ", if_indextoname(src_ifindex, ifname)); + printf("-d %s ", if_indextoname(dst_ifindex, ifname)); + + if (rtc->flags & CGW_FLAGS_CAN_FD) + printf("-X "); + + if (rtc->flags & CGW_FLAGS_CAN_ECHO) + printf("-e "); + + if (rtc->flags & CGW_FLAGS_CAN_SRC_TSTAMP) + printf("-t "); + + if (rtc->flags & CGW_FLAGS_CAN_IIF_TX_OK) + printf("-i "); + + /* second parse for mod attributes */ + rta = (struct rtattr *) RTCAN_RTA(rtc); + rtlen = RTCAN_PAYLOAD(nlh); + for(;RTA_OK(rta, rtlen);rta=RTA_NEXT(rta,rtlen)) + { + //printf("(B-%d)", rta->rta_type); + switch(rta->rta_type) { + + case CGW_FILTER: + printfilter(RTA_DATA(rta)); + break; + + case CGW_MOD_AND: + printmod("AND", RTA_DATA(rta)); + break; + + case CGW_MOD_OR: + printmod("OR", RTA_DATA(rta)); + break; + + case CGW_MOD_XOR: + printmod("XOR", RTA_DATA(rta)); + break; + + case CGW_MOD_SET: + printmod("SET", RTA_DATA(rta)); + break; + + case CGW_FDMOD_AND: + printfdmod("AND", RTA_DATA(rta)); + break; + + case CGW_FDMOD_OR: + printfdmod("OR", RTA_DATA(rta)); + break; + + case CGW_FDMOD_XOR: + printfdmod("XOR", RTA_DATA(rta)); + break; + + case CGW_FDMOD_SET: + printfdmod("SET", RTA_DATA(rta)); + break; + + case CGW_MOD_UID: + printf("-u %X ", *(__u32 *)RTA_DATA(rta)); + break; + + case CGW_LIM_HOPS: + printf("-l %d ", *(__u8 *)RTA_DATA(rta)); + break; + + case CGW_CS_XOR: + print_cs_xor((struct cgw_csum_xor *)RTA_DATA(rta)); + break; + + case CGW_CS_CRC8: + print_cs_crc8((struct cgw_csum_crc8 *)RTA_DATA(rta)); + break; + + case CGW_SRC_IF: + case CGW_DST_IF: + case CGW_HANDLED: + case CGW_DROPPED: + case CGW_DELETED: + break; + + default: + printf("Unknown attribute %d!", rta->rta_type); + return -EINVAL; + break; + } + } + + /* end of entry */ + printf("# %d handled %d dropped %d deleted\n", + handled, dropped, deleted); + + /* jump to next NLMSG in the given buffer */ + nlh = NLMSG_NEXT(nlh, len); + } +} + +int main(int argc, char **argv) +{ + int s; + int err = 0; + + int opt; + extern int optind, opterr, optopt; + + int cmd = UNSPEC; + int have_filter = 0; + int have_cs_xor = 0; + int have_cs_crc8 = 0; + + struct { + struct nlmsghdr nh; + struct rtcanmsg rtcan; + char buf[1500]; + } req; + + unsigned char rxbuf[8192]; /* netlink receive buffer */ + struct nlmsghdr *nlh; + struct nlmsgerr *rte; + unsigned int src_ifindex = 0; + unsigned int dst_ifindex = 0; + __u32 uid = 0; + __u8 limit_hops = 0; + __u16 flags = 0; + int len; + + struct can_filter filter; + struct sockaddr_nl nladdr; + + struct cgw_csum_xor cs_xor = { 0 }; + struct cgw_csum_crc8 cs_crc8 = { 0 }; + char crc8tab[513] = {0}; + + struct modattr modmsg[CGW_MOD_FUNCS]; + struct fdmodattr fdmodmsg[CGW_MOD_FUNCS]; + int modidx = 0; + int fdmodidx = 0; + int i; + + memset(&req, 0, sizeof(req)); + + while ((opt = getopt(argc, argv, "ADFLs:d:Xteiu:l:f:c:p:x:m:M:?")) != -1) { + switch (opt) { + + case 'A': + if (cmd == UNSPEC) + cmd = ADD; + break; + + case 'D': + if (cmd == UNSPEC) + cmd = DEL; + break; + + case 'F': + if (cmd == UNSPEC) + cmd = FLUSH; + break; + + case 'L': + if (cmd == UNSPEC) + cmd = LIST; + break; + + case 's': + src_ifindex = if_nametoindex(optarg); + if (!src_ifindex) { + perror("src if_nametoindex"); + exit(1); + } + break; + + case 'd': + dst_ifindex = if_nametoindex(optarg); + if (!dst_ifindex) { + perror("dst if_nametoindex"); + exit(1); + } + break; + + case 'X': + flags |= CGW_FLAGS_CAN_FD; + break; + + case 't': + flags |= CGW_FLAGS_CAN_SRC_TSTAMP; + break; + + case 'e': + flags |= CGW_FLAGS_CAN_ECHO; + break; + + case 'i': + flags |= CGW_FLAGS_CAN_IIF_TX_OK; + break; + + case 'u': + uid = strtoul(optarg, NULL, 16); + break; + + case 'l': + if (sscanf(optarg, "%hhu", &limit_hops) != 1 || !(limit_hops)) { + printf("Bad hop limit definition '%s'.\n", optarg); + exit(1); + } + break; + + case 'f': + if (sscanf(optarg, "%x:%x", &filter.can_id, + &filter.can_mask) == 2) { + have_filter = 1; + } else if (sscanf(optarg, "%x~%x", &filter.can_id, + &filter.can_mask) == 2) { + filter.can_id |= CAN_INV_FILTER; + have_filter = 1; + } else { + printf("Bad filter definition '%s'.\n", optarg); + exit(1); + } + break; + + case 'x': + if (sscanf(optarg, "%hhd:%hhd:%hhd:%hhx", + &cs_xor.from_idx, &cs_xor.to_idx, + &cs_xor.result_idx, &cs_xor.init_xor_val) == 4) { + have_cs_xor = 1; + } else { + printf("Bad XOR checksum definition '%s'.\n", optarg); + exit(1); + } + break; + + case 'c': + if ((sscanf(optarg, "%hhd:%hhd:%hhd:%hhx:%hhx:%512s", + &cs_crc8.from_idx, &cs_crc8.to_idx, + &cs_crc8.result_idx, &cs_crc8.init_crc_val, + &cs_crc8.final_xor_val, crc8tab) == 6) && + (strlen(crc8tab) == 512) && + (b64hex(crc8tab, (unsigned char *)&cs_crc8.crctab, 256) == 0)) { + have_cs_crc8 = 1; + } else { + printf("Bad CRC8 checksum definition '%s'.\n", optarg); + exit(1); + } + break; + + case 'p': + if (parse_crc8_profile(optarg, &cs_crc8)) { + printf("Bad CRC8 profile definition '%s'.\n", optarg); + exit(1); + } + break; + + case 'm': + /* may be triggered by each of the CGW_MOD_FUNCS functions */ + if ((modidx < CGW_MOD_FUNCS) && (err = parse_mod(optarg, &modmsg[modidx++]))) { + printf("Problem %d with modification definition '%s'.\n", err, optarg); + exit(1); + } + break; + + case 'M': + /* may be triggered by each of the CGW_FDMOD_FUNCS functions */ + if ((fdmodidx < CGW_MOD_FUNCS) && (err = parse_fdmod(optarg, &fdmodmsg[fdmodidx++]))) { + printf("Problem %d with modification definition '%s'.\n", err, optarg); + exit(1); + } + break; + + case '?': + print_usage(basename(argv[0])); + exit(0); + break; + + default: + fprintf(stderr, "Unknown option %c\n", opt); + print_usage(basename(argv[0])); + exit(1); + break; + } + } + + if ((argc - optind != 0) || (cmd == UNSPEC)) { + print_usage(basename(argv[0])); + exit(1); + } + + if ((cmd == ADD || cmd == DEL) && + ((!src_ifindex) || (!dst_ifindex))) { + print_usage(basename(argv[0])); + exit(1); + } + + if (flags & CGW_FLAGS_CAN_FD) { + if (modidx) { + printf("No -m modifications allowed in CAN FD mode!\n"); + exit(1); + } + } else { + if (fdmodidx) { + printf("No -M modifications allowed in Classic CAN mode!\n"); + exit(1); + } + } + + if ((!modidx && !fdmodidx) && (have_cs_crc8 || have_cs_xor)) { + printf("-c or -x can only be used in conjunction with -m/-M\n"); + exit(1); + } + + s = socket(PF_NETLINK, SOCK_RAW, NETLINK_ROUTE); + + switch (cmd) { + + case ADD: + req.nh.nlmsg_flags = NLM_F_REQUEST | NLM_F_ACK; + req.nh.nlmsg_type = RTM_NEWROUTE; + break; + + case DEL: + req.nh.nlmsg_flags = NLM_F_REQUEST | NLM_F_ACK; + req.nh.nlmsg_type = RTM_DELROUTE; + break; + + case FLUSH: + req.nh.nlmsg_flags = NLM_F_REQUEST | NLM_F_ACK; + req.nh.nlmsg_type = RTM_DELROUTE; + /* if_index set to 0 => remove all entries */ + src_ifindex = 0; + dst_ifindex = 0; + break; + + case LIST: + req.nh.nlmsg_flags = NLM_F_REQUEST | NLM_F_DUMP; + req.nh.nlmsg_type = RTM_GETROUTE; + break; + + default: + printf("This function is not yet implemented.\n"); + exit(1); + break; + } + + req.nh.nlmsg_len = NLMSG_LENGTH(sizeof(struct rtcanmsg)); + req.nh.nlmsg_seq = 0; + + req.rtcan.can_family = AF_CAN; + req.rtcan.gwtype = CGW_TYPE_CAN_CAN; + req.rtcan.flags = flags; + + addattr_l(&req.nh, sizeof(req), CGW_SRC_IF, &src_ifindex, sizeof(src_ifindex)); + addattr_l(&req.nh, sizeof(req), CGW_DST_IF, &dst_ifindex, sizeof(dst_ifindex)); + + /* add new attributes here */ + + if (have_filter) + addattr_l(&req.nh, sizeof(req), CGW_FILTER, &filter, sizeof(filter)); + + if (have_cs_crc8) + addattr_l(&req.nh, sizeof(req), CGW_CS_CRC8, &cs_crc8, sizeof(cs_crc8)); + + if (have_cs_xor) + addattr_l(&req.nh, sizeof(req), CGW_CS_XOR, &cs_xor, sizeof(cs_xor)); + + if (uid) + addattr_l(&req.nh, sizeof(req), CGW_MOD_UID, &uid, sizeof(__u32)); + + if (limit_hops) + addattr_l(&req.nh, sizeof(req), CGW_LIM_HOPS, &limit_hops, sizeof(__u8)); + + /* + * a better example code + * modmsg.modtype = CGW_MOD_ID; + * addattr_l(&req.n, sizeof(req), CGW_MOD_SET, &modmsg, CGW_MODATTR_LEN); + */ + + /* add up to CGW_MOD_FUNCS modification definitions */ + for (i = 0; i < modidx; i++) + addattr_l(&req.nh, sizeof(req), modmsg[i].instruction, &modmsg[i], CGW_MODATTR_LEN); + + /* add up to CGW_FDMOD_FUNCS modification definitions */ + for (i = 0; i < fdmodidx; i++) + addattr_l(&req.nh, sizeof(req), fdmodmsg[i].instruction, &fdmodmsg[i], CGW_FDMODATTR_LEN); + + memset(&nladdr, 0, sizeof(nladdr)); + nladdr.nl_family = AF_NETLINK; + nladdr.nl_pid = 0; + nladdr.nl_groups = 0; + + err = sendto(s, &req, req.nh.nlmsg_len, 0, + (struct sockaddr*)&nladdr, sizeof(nladdr)); + if (err < 0) { + perror("netlink sendto"); + return err; + } + + /* clean netlink receive buffer */ + memset(rxbuf, 0x0, sizeof(rxbuf)); + + if (cmd != LIST) { + + /* + * cmd == ADD || cmd == DEL || cmd == FLUSH + * + * Parse the requested netlink acknowledge return values. + */ + + err = recv(s, &rxbuf, sizeof(rxbuf), 0); + if (err < 0) { + perror("netlink recv"); + return err; + } + nlh = (struct nlmsghdr *)rxbuf; + if (nlh->nlmsg_type != NLMSG_ERROR) { + fprintf(stderr, "unexpected netlink answer of type %d\n", nlh->nlmsg_type); + return -EINVAL; + } + rte = (struct nlmsgerr *)NLMSG_DATA(nlh); + err = rte->error; + if (err < 0) + fprintf(stderr, "netlink error %d (%s)\n", err, strerror(abs(err))); + + } else { + + /* cmd == LIST */ + + while (1) { + len = recv(s, &rxbuf, sizeof(rxbuf), 0); + if (len < 0) { + perror("netlink recv"); + return len; + } +#if 0 + printf("received msg len %d\n", len); + + for (i = 0; i < len; i++) + printf("%02X ", rxbuf[i]); + + printf("\n"); +#endif + /* leave on errors or NLMSG_DONE */ + if (parse_rtlist(argv[0], rxbuf, len)) + break; + } + } + + close(s); + + return err; +} + diff --git a/Devices/Libraries/Systems/can-utils/canlogserver.c b/Devices/Libraries/Systems/can-utils/canlogserver.c new file mode 100755 index 0000000..2e8d288 --- /dev/null +++ b/Devices/Libraries/Systems/can-utils/canlogserver.c @@ -0,0 +1,467 @@ +/* SPDX-License-Identifier: (GPL-2.0-only OR BSD-3-Clause) */ +/* + * canlogserver.c + * + * Copyright (c) 2002-2007 Volkswagen Group Electronic Research + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * 3. Neither the name of Volkswagen nor the names of its contributors + * may be used to endorse or promote products derived from this software + * without specific prior written permission. + * + * Alternatively, provided that this notice is retained in full, this + * software may be distributed under the terms of the GNU General + * Public License ("GPL") version 2, in which case the provisions of the + * GPL apply INSTEAD OF those given above. + * + * The provided data structures and external interfaces from this code + * are not restricted to be used by modules with a GPL compatible license. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT + * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY + * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH + * DAMAGE. + * + * Send feedback to + * + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "lib.h" + +#define MAXDEV 6 /* change sscanf()'s manually if changed here */ +#define ANYDEV "any" +#define ANL "\r\n" /* newline in ASC mode */ + +#define DEFPORT 28700 + +static char devname[MAXDEV][IFNAMSIZ+1]; +static int dindex[MAXDEV]; +static int max_devname_len; + +extern int optind, opterr, optopt; + +static volatile int running = 1; +static volatile sig_atomic_t signal_num; + +static void print_usage(char *prg) +{ + fprintf(stderr, "%s - log CAN frames and serves them.\n", prg); + fprintf(stderr, "\nUsage: %s [options] +\n", prg); + fprintf(stderr, " (use CTRL-C to terminate %s)\n\n", prg); + fprintf(stderr, "Options:\n"); + fprintf(stderr, " -m (ID filter mask. Default 0x00000000) *\n"); + fprintf(stderr, " -v (ID filter value. Default 0x00000000) *\n"); + fprintf(stderr, " -i <0|1> (invert the specified ID filter) *\n"); + fprintf(stderr, " -e (mask for error frames)\n"); + fprintf(stderr, " -p (listen on port . Default: %d)\n", DEFPORT); + fprintf(stderr, "\n"); + fprintf(stderr, "* The CAN ID filter matches, when ...\n"); + fprintf(stderr, " & mask == value & mask\n"); + fprintf(stderr, "\n"); + fprintf(stderr, "When using more than one CAN interface the options\n"); + fprintf(stderr, "m/v/i/e have comma separated values e.g. '-m 0,7FF,0'\n"); + fprintf(stderr, "\nUse interface name '%s' to receive from all CAN interfaces.\n", ANYDEV); + fprintf(stderr, "\n"); + fprintf(stderr, "After running canlogserver, connect to it via TCP to get logged data.\n"); + fprintf(stderr, "e.g. with 'nc localhost %d'\n", DEFPORT); + fprintf(stderr, "\n"); +} + +static int idx2dindex(int ifidx, int socket) +{ + int i; + struct ifreq ifr; + + for (i=0; i currmax) + currmax = i; + break; + + case 'v': + i = sscanf(optarg, "%x,%x,%x,%x,%x,%x", + &value[0], &value[1], &value[2], + &value[3], &value[4], &value[5]); + if (i > currmax) + currmax = i; + break; + + case 'i': + i = sscanf(optarg, "%d,%d,%d,%d,%d,%d", + &inv_filter[0], &inv_filter[1], &inv_filter[2], + &inv_filter[3], &inv_filter[4], &inv_filter[5]); + if (i > currmax) + currmax = i; + break; + + case 'e': + i = sscanf(optarg, "%x,%x,%x,%x,%x,%x", + &err_mask[0], &err_mask[1], &err_mask[2], + &err_mask[3], &err_mask[4], &err_mask[5]); + if (i > currmax) + currmax = i; + break; + case 'p': + port = atoi(optarg); + break; + default: + print_usage(basename(argv[0])); + exit(1); + break; + } + } + + if (optind == argc) { + print_usage(basename(argv[0])); + exit(0); + } + + /* count in options higher than device count ? */ + if (optind + currmax > argc) { + printf("low count of CAN devices!\n"); + return 1; + } + + currmax = argc - optind; /* find real number of CAN devices */ + + if (currmax > MAXDEV) { + printf("More than %d CAN devices!\n", MAXDEV); + return 1; + } + + + socki = socket(PF_INET, SOCK_STREAM, 0); + if (socki < 0) { + perror("socket"); + exit(1); + } + + inaddr.sin_family = AF_INET; + inaddr.sin_addr.s_addr = htonl(INADDR_ANY); + inaddr.sin_port = htons(port); + + while(bind(socki, (struct sockaddr*)&inaddr, sizeof(inaddr)) < 0) { + struct timespec f = { + .tv_nsec = 100 * 1000 * 1000, + }; + + printf(".");fflush(NULL); + nanosleep(&f, NULL); + } + + if (listen(socki, 3) != 0) { + perror("listen"); + exit(1); + } + + while(1) { + accsocket = accept(socki, (struct sockaddr*)&clientaddr, &sin_size); + if (accsocket > 0) { + //printf("accepted\n"); + if (!fork()) + break; + close(accsocket); + } + else if (errno != EINTR) { + perror("accept"); + exit(1); + } + } + + for (i=0; i max_devname_len) + max_devname_len = j; /* for nice printing */ + + addr.can_family = AF_CAN; + + if (strcmp(ANYDEV, argv[optind + i]) != 0) { + strcpy(ifr.ifr_name, argv[optind+i]); + if (ioctl(s[i], SIOCGIFINDEX, &ifr) < 0) { + perror("SIOCGIFINDEX"); + exit(1); + } + addr.can_ifindex = ifr.ifr_ifindex; + } else + addr.can_ifindex = 0; /* any can interface */ + + if (bind(s[i], (struct sockaddr *)&addr, sizeof(addr)) < 0) { + perror("bindcan"); + return 1; + } + } + + while (running) { + + FD_ZERO(&rdfs); + for (i=0; i + * + */ + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include "lib.h" + +#define DEFAULT_GAP 1 /* ms */ +#define DEFAULT_LOOPS 1 /* only one replay */ +#define CHANNELS 20 /* anyone using more than 20 CAN interfaces at a time? */ +#define STDOUTIDX 65536 /* interface index for printing on stdout - bigger than max uint16 */ + +#if (IFNAMSIZ != 16) +#error "IFNAMSIZ value does not to DEVSZ calculation!" +#endif + +#define DEVSZ 22 /* IFNAMSZ + 6 */ +#define TIMESZ sizeof("(1345212884.318850) ") +#define BUFSZ (TIMESZ + DEVSZ + AFRSZ) + +/* adapt sscanf() functions below on error */ +#if (AFRSZ != 6300) +#error "AFRSZ value does not fit sscanf restrictions!" +#endif +#if (DEVSZ != 22) +#error "DEVSZ value does not fit sscanf restrictions!" +#endif + +struct assignment { + char txif[IFNAMSIZ]; + int txifidx; + char rxif[IFNAMSIZ]; +}; +static struct assignment asgn[CHANNELS]; +const int canfx_on = 1; + +extern int optind, opterr, optopt; + +static void print_usage(char *prg) +{ + fprintf(stderr, "%s - replay a compact CAN frame logfile to CAN devices.\n", prg); + fprintf(stderr, "\nUsage: %s [interface assignment]*\n\n", prg); + fprintf(stderr, "Options:\n"); + fprintf(stderr, " -I (default stdin)\n"); + fprintf(stderr, + " -l " + "(process input file times)\n" + " " + "(Use 'i' for infinite loop - default: %d)\n", + DEFAULT_LOOPS); + fprintf(stderr, " -t (ignore timestamps: " + "send frames immediately)\n"); + fprintf(stderr, " -i (interactive - wait " + "for ENTER key to process next frame)\n"); + fprintf(stderr, " -n (terminate after " + "processing CAN frames)\n"); + fprintf(stderr, + " -g (gap in milli " + "seconds - default: %d ms)\n", + DEFAULT_GAP); + fprintf(stderr, " -s (skip gaps in " + "timestamps > 's' seconds)\n"); + fprintf(stderr, " -x (disable local " + "loopback of sent CAN frames)\n"); + fprintf(stderr, " -v (verbose: print " + "sent CAN frames)\n"); + fprintf(stderr, " -h (show " + "this help message)\n\n"); + fprintf(stderr, "Interface assignment:\n"); + fprintf(stderr, " 0..n assignments like =\n\n"); + fprintf(stderr, " e.g. vcan2=can0 (send frames received from can0 on " + "vcan2)\n"); + fprintf(stderr, " extra hook: stdout=can0 (print logfile line marked with can0 on " + "stdout)\n"); + fprintf(stderr, " No assignments => send frames to the interface(s) they " + "had been received from\n\n"); + fprintf(stderr, "Lines in the logfile not beginning with '(' (start of " + "timestamp) are ignored.\n\n"); +} + +/* copied from /usr/src/linux/include/linux/time.h ... + * lhs < rhs: return <0 + * lhs == rhs: return 0 + * lhs > rhs: return >0 + */ +static inline int timeval_compare(struct timeval *lhs, struct timeval *rhs) +{ + if (lhs->tv_sec < rhs->tv_sec) + return -1; + if (lhs->tv_sec > rhs->tv_sec) + return 1; + return lhs->tv_usec - rhs->tv_usec; +} + +static inline void create_diff_tv(struct timeval *today, struct timeval *diff, struct timeval *log) +{ + /* create diff_tv so that log_tv + diff_tv = today_tv */ + diff->tv_sec = today->tv_sec - log->tv_sec; + diff->tv_usec = today->tv_usec - log->tv_usec; +} + +static inline int frames_to_send(struct timeval *today, struct timeval *diff, struct timeval *log) +{ + /* return value <0 when log + diff < today */ + + struct timeval cmp; + + cmp.tv_sec = log->tv_sec + diff->tv_sec; + cmp.tv_usec = log->tv_usec + diff->tv_usec; + + if (cmp.tv_usec >= 1000000) { + cmp.tv_usec -= 1000000; + cmp.tv_sec++; + } + + if (cmp.tv_usec < 0) { + cmp.tv_usec += 1000000; + cmp.tv_sec--; + } + + return timeval_compare(&cmp, today); +} + +static int get_txidx(char *logif_name) +{ + int i; + + for (i = 0; i < CHANNELS; i++) { + if (asgn[i].rxif[0] == 0) /* end of table content */ + break; + if (strcmp(asgn[i].rxif, logif_name) == 0) /* found device name */ + break; + } + + if ((i == CHANNELS) || (asgn[i].rxif[0] == 0)) + return 0; /* not found */ + + return asgn[i].txifidx; /* return interface index */ +} + +static char *get_txname(char *logif_name) +{ + int i; + + for (i = 0; i < CHANNELS; i++) { + if (asgn[i].rxif[0] == 0) /* end of table content */ + break; + if (strcmp(asgn[i].rxif, logif_name) == 0) /* found device name */ + break; + } + + if ((i == CHANNELS) || (asgn[i].rxif[0] == 0)) + return 0; /* not found */ + + return asgn[i].txif; /* return interface name */ +} + +static int add_assignment(char *mode, int socket, char *txname, + char *rxname, int verbose) +{ + struct ifreq ifr; + int i; + + /* find free entry */ + for (i = 0; i < CHANNELS; i++) { + if (asgn[i].txif[0] == 0) + break; + } + + if (i == CHANNELS) { + fprintf(stderr, "Assignment table exceeded!\n"); + return 1; + } + + if (strlen(txname) >= IFNAMSIZ) { + fprintf(stderr, "write-if interface name '%s' too long!", txname); + return 1; + } + strcpy(asgn[i].txif, txname); + + if (strlen(rxname) >= IFNAMSIZ) { + fprintf(stderr, "log-if interface name '%s' too long!", rxname); + return 1; + } + strcpy(asgn[i].rxif, rxname); + + if (strcmp(txname, "stdout") != 0) { + strcpy(ifr.ifr_name, txname); + if (ioctl(socket, SIOCGIFINDEX, &ifr) < 0) { + perror("SIOCGIFINDEX"); + fprintf(stderr, "write-if interface name '%s' is wrong!\n", txname); + return 1; + } + asgn[i].txifidx = ifr.ifr_ifindex; + } else + asgn[i].txifidx = STDOUTIDX; + + if (verbose > 1) /* use -v -v to see this */ + printf("added %s assignment: log-if=%s write-if=%s write-if-idx=%d\n", mode, asgn[i].rxif, asgn[i].txif, asgn[i].txifidx); + + return 0; +} + +int main(int argc, char **argv) +{ + static char buf[BUFSZ], device[DEVSZ], afrbuf[AFRSZ]; + struct sockaddr_can addr; + struct can_raw_vcid_options vcid_opts = { + .flags = CAN_RAW_XL_VCID_TX_PASS, + }; + static cu_t cu; + static struct timeval today_tv, log_tv, last_log_tv, diff_tv; + struct timespec sleep_ts; + int s; /* CAN_RAW socket */ + FILE *infile = stdin; + unsigned long gap = DEFAULT_GAP; + int use_timestamps = 1; + int interactive = 0; /* wait for ENTER keypress to process next frame */ + int count = 0; /* end replay after sending count frames. 0 = disabled */ + static int verbose, opt, delay_loops; + static unsigned long skipgap; + static int loopback_disable = 0; + static int infinite_loops = 0; + static int loops = DEFAULT_LOOPS; + int assignments; /* assignments defined on the commandline */ + int txidx; /* sendto() interface index */ + int eof, txmtu, i, j; + char *fret; + unsigned long long sec, usec; + + while ((opt = getopt(argc, argv, "I:l:tin:g:s:xvh")) != -1) { + switch (opt) { + case 'I': + infile = fopen(optarg, "r"); + if (!infile) { + perror("infile"); + return 1; + } + break; + + case 'l': + if (optarg[0] == 'i') + infinite_loops = 1; + else if (!(loops = atoi(optarg))) { + fprintf(stderr, "Invalid argument for option -l !\n"); + return 1; + } + break; + + case 't': + use_timestamps = 0; + break; + + case 'i': + interactive = 1; + break; + + case 'n': + count = atoi(optarg); + if (count < 1) { + print_usage(basename(argv[0])); + exit(1); + } + break; + + case 'g': + gap = strtoul(optarg, NULL, 10); + break; + + case 's': + skipgap = strtoul(optarg, NULL, 10); + if (skipgap < 1) { + fprintf(stderr, "Invalid argument for option -s !\n"); + return 1; + } + break; + + case 'x': + loopback_disable = 1; + break; + + case 'v': + verbose++; + break; + + case 'h': + print_usage(basename(argv[0])); + exit(EXIT_SUCCESS); + break; + + default: + print_usage(basename(argv[0])); + exit(EXIT_FAILURE); + break; + } + } + + assignments = argc - optind; /* find real number of user assignments */ + + if (infile == stdin) { /* no jokes with stdin */ + infinite_loops = 0; + loops = 1; + } + + if (verbose > 1) { /* use -v -v to see this */ + if (infinite_loops) + printf("infinite_loops\n"); + else + printf("%d loops\n", loops); + } + + /* ignore timestamps from logfile when in single step keypress mode */ + if (interactive) { + use_timestamps = 0; + printf("interactive mode: press ENTER to process next CAN frame ...\n"); + } + + sleep_ts.tv_sec = gap / 1000; + sleep_ts.tv_nsec = (gap % 1000) * 1000000; + + /* open socket */ + if ((s = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) { + perror("socket"); + return 1; + } + + addr.can_family = AF_CAN; + addr.can_ifindex = 0; + + /* disable unneeded default receive filter on this RAW socket */ + setsockopt(s, SOL_CAN_RAW, CAN_RAW_FILTER, NULL, 0); + + /* try to switch the socket into CAN FD mode */ + setsockopt(s, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, &canfx_on, sizeof(canfx_on)); + + /* try to switch the socket into CAN XL mode */ + setsockopt(s, SOL_CAN_RAW, CAN_RAW_XL_FRAMES, &canfx_on, sizeof(canfx_on)); + + /* try to enable the CAN XL VCID pass through mode */ + setsockopt(s, SOL_CAN_RAW, CAN_RAW_XL_VCID_OPTS, &vcid_opts, sizeof(vcid_opts)); + + if (loopback_disable) { + int loopback = 0; + + setsockopt(s, SOL_CAN_RAW, CAN_RAW_LOOPBACK, &loopback, sizeof(loopback)); + } + + if (bind(s, (struct sockaddr *)&addr, sizeof(addr)) < 0) { + perror("bind"); + return 1; + } + + if (assignments) { + /* add & check user assignments from commandline */ + for (i = 0; i < assignments; i++) { + if (strlen(argv[optind + i]) >= BUFSZ) { + fprintf(stderr, "Assignment too long!\n"); + print_usage(basename(argv[0])); + return 1; + } + strcpy(buf, argv[optind + i]); + for (j = 0; j < (int)BUFSZ; j++) { /* find '=' in assignment */ + if (buf[j] == '=') + break; + } + if ((j == BUFSZ) || (buf[j] != '=')) { + fprintf(stderr, "'=' missing in assignment!\n"); + print_usage(basename(argv[0])); + return 1; + } + buf[j] = 0; /* cut string in two pieces */ + if (add_assignment("user", s, &buf[0], &buf[j + 1], verbose)) + return 1; + } + } + + while (infinite_loops || loops--) { + if (infile != stdin) + rewind(infile); /* for each loop */ + + if (verbose > 1) /* use -v -v to see this */ + printf(">>>>>>>>> start reading file. remaining loops = %d\n", loops); + + /* read first non-comment frame from logfile */ + while ((fret = fgets(buf, BUFSZ - 1, infile)) != NULL && buf[0] != '(') { + if (strlen(buf) >= BUFSZ - 2) { + fprintf(stderr, "comment line too long for input buffer\n"); + return 1; + } + } + + if (!fret) + goto out; /* nothing to read */ + + eof = 0; + + if (sscanf(buf, "(%llu.%llu) %21s %6299s", &sec, &usec, device, afrbuf) != 4) { + fprintf(stderr, "incorrect line format in logfile\n"); + return 1; + } + log_tv.tv_sec = sec; + log_tv.tv_usec = usec; + + /* + * ensure the fractions of seconds are 6 decimal places long to catch + * 3rd party or handcrafted logfiles that treat the timestamp as float + */ + if (strchr(buf, ')') - strchr(buf, '.') != 7) { + fprintf(stderr, "timestamp format in logfile requires 6 decimal places\n"); + return 1; + } + + if (use_timestamps) { /* throttle sending due to logfile timestamps */ + + gettimeofday(&today_tv, NULL); + create_diff_tv(&today_tv, &diff_tv, &log_tv); + last_log_tv = log_tv; + } + + while (!eof) { + while ((!use_timestamps) || (frames_to_send(&today_tv, &diff_tv, &log_tv) < 0)) { + /* wait for keypress to process next frame */ + if (interactive) + getchar(); + + /* log_tv/device/afrbuf are valid here */ + + if (strlen(device) >= IFNAMSIZ) { + fprintf(stderr, "log interface name '%s' too long!", device); + return 1; + } + + txidx = get_txidx(device); /* get ifindex for sending the frame */ + + if ((!txidx) && (!assignments)) { + /* ifindex not found and no user assignments */ + /* => assign this device automatically */ + if (add_assignment("auto", s, device, device, verbose)) + return 1; + txidx = get_txidx(device); + } + + if (txidx == STDOUTIDX) { /* hook to print logfile lines on stdout */ + + printf("%s", buf); /* print the line AS-IS without extra \n */ + fflush(stdout); + + } else if (txidx > 0) { /* only send to valid CAN devices */ + + txmtu = parse_canframe(afrbuf, &cu); /* dual-use frame */ + if (!txmtu) { + fprintf(stderr, "wrong CAN frame format: '%s'!", afrbuf); + return 1; + } + + /* CAN XL frames need real frame length for sending */ + if (txmtu == CANXL_MTU) + txmtu = CANXL_HDR_SIZE + cu.xl.len; + + addr.can_family = AF_CAN; + addr.can_ifindex = txidx; /* send via this interface */ + + if (sendto(s, &cu, txmtu, 0, (struct sockaddr *)&addr, sizeof(addr)) != txmtu) { + perror("sendto"); + return 1; + } + + if (verbose) { + printf("%s (%s) ", get_txname(device), device); + snprintf_long_canframe(afrbuf, sizeof(afrbuf), &cu, CANLIB_VIEW_INDENT_SFF); + printf("%s\n", afrbuf); + } + + if (count && (--count == 0)) + goto out; + } + + /* read next non-comment frame from logfile */ + while ((fret = fgets(buf, BUFSZ - 1, infile)) != NULL && buf[0] != '(') { + if (strlen(buf) >= BUFSZ - 2) { + fprintf(stderr, "comment line too long for input buffer\n"); + return 1; + } + } + + if (!fret) { + eof = 1; /* this file is completely processed */ + break; + } + + if (sscanf(buf, "(%llu.%llu) %s %s", &sec, &usec, device, afrbuf) != 4) { + fprintf(stderr, "incorrect line format in logfile\n"); + return 1; + } + log_tv.tv_sec = sec; + log_tv.tv_usec = usec; + + /* + * ensure the fractions of seconds are 6 decimal places long to catch + * 3rd party or handcrafted logfiles that treat the timestamp as float + */ + if (strchr(buf, ')') - strchr(buf, '.') != 7) { + fprintf(stderr, "timestamp format in logfile requires 6 decimal places\n"); + return 1; + } + + if (use_timestamps) { + gettimeofday(&today_tv, NULL); + + /* test for logfile timestamps jumping backwards OR */ + /* if the user likes to skip long gaps in the timestamps */ + if ((last_log_tv.tv_sec > log_tv.tv_sec) || (skipgap && labs(last_log_tv.tv_sec - log_tv.tv_sec) > (long)skipgap)) + create_diff_tv(&today_tv, &diff_tv, &log_tv); + + last_log_tv = log_tv; + } + + } /* while frames_to_send ... */ + + if (nanosleep(&sleep_ts, NULL)) + return 1; + + delay_loops++; /* private statistics */ + gettimeofday(&today_tv, NULL); + + } /* while (!eof) */ + + } /* while (infinite_loops || loops--) */ + +out: + + close(s); + fclose(infile); + + if (verbose > 1) /* use -v -v to see this */ + printf("%d delay_loops\n", delay_loops); + + return 0; +} diff --git a/Devices/Libraries/Systems/can-utils/cansend.c b/Devices/Libraries/Systems/can-utils/cansend.c new file mode 100755 index 0000000..7e9c627 --- /dev/null +++ b/Devices/Libraries/Systems/can-utils/cansend.c @@ -0,0 +1,214 @@ +/* SPDX-License-Identifier: (GPL-2.0-only OR BSD-3-Clause) */ +/* + * cansend.c - send CAN-frames via CAN_RAW sockets + * + * Copyright (c) 2002-2007 Volkswagen Group Electronic Research + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * 3. Neither the name of Volkswagen nor the names of its contributors + * may be used to endorse or promote products derived from this software + * without specific prior written permission. + * + * Alternatively, provided that this notice is retained in full, this + * software may be distributed under the terms of the GNU General + * Public License ("GPL") version 2, in which case the provisions of the + * GPL apply INSTEAD OF those given above. + * + * The provided data structures and external interfaces from this code + * are not restricted to be used by modules with a GPL compatible license. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT + * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY + * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH + * DAMAGE. + * + * Send feedback to + * + */ + +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include "lib.h" + +static void print_usage(char *prg) +{ + fprintf(stderr, + "%s - send CAN-frames via CAN_RAW sockets.\n" + "\n" + "Usage: %s .\n" + "\n" + ":\n" + " #{data} for CAN CC (Classical CAN 2.0B) data frames\n" + " #R{len} for CAN CC (Classical CAN 2.0B) data frames\n" + " #{data}_{dlc} for CAN CC (Classical CAN 2.0B) data frames\n" + " #R{len}_{dlc} for CAN CC (Classical CAN 2.0B) data frames\n" + " ##{data} for CAN FD frames\n" + " #::# for CAN XL frames\n" + "\n" + ":\n" + " 3 (SFF) or 8 (EFF) hex chars\n" + "{data}:\n" + " 0..8 (0..64 CAN FD) ASCII hex-values (optionally separated by '.')\n" + "{len}:\n" + " an optional 0..8 value as RTR frames can contain a valid dlc field\n" + "_{dlc}:\n" + " an optional 9..F data length code value when payload length is 8\n" + ":\n" + " a single ASCII Hex value (0 .. F) which defines canfd_frame.flags\n" + "\n" + ":\n" + " 2 hex chars - virtual CAN network identifier (00 .. FF)\n" + ":\n" + " 3 hex chars - 11 bit priority value (000 .. 7FF)\n" + ":\n" + " 2 hex chars values (00 .. FF) which defines canxl_frame.flags\n" + ":\n" + " 2 hex chars values (00 .. FF) which defines canxl_frame.sdt\n" + ":\n" + " 8 hex chars - 32 bit acceptance field (canxl_frame.af)\n" + ":\n" + " 1..2048 ASCII hex-values (optionally separated by '.')\n" + "\n" + "Examples:\n" + " 5A1#11.2233.44556677.88 / 123#DEADBEEF / 5AA# / 123##1 / 213##311223344 /\n" + " 1F334455#1122334455667788_B / 123#R / 00000123#R3 / 333#R8_E /\n" + " 45123#81:00:12345678#11223344.556677 / 00242#81:07:40000123#112233\n" + "\n", + prg, prg); +} + +int main(int argc, char **argv) +{ + int s; /* can raw socket */ + int required_mtu; + int mtu; + int enable_canfx = 1; + struct sockaddr_can addr; + struct can_raw_vcid_options vcid_opts = { + .flags = CAN_RAW_XL_VCID_TX_PASS, + }; + static cu_t cu; + struct ifreq ifr; + + /* check command line options */ + if (argc != 3) { + print_usage(argv[0]); + return 1; + } + + /* parse CAN frame */ + required_mtu = parse_canframe(argv[2], &cu); + if (!required_mtu) { + fprintf(stderr, "\nWrong CAN-frame format!\n\n"); + print_usage(argv[0]); + return 1; + } + + /* open socket */ + if ((s = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) { + perror("socket"); + return 1; + } + + strncpy(ifr.ifr_name, argv[1], IFNAMSIZ - 1); + ifr.ifr_name[IFNAMSIZ - 1] = '\0'; + ifr.ifr_ifindex = if_nametoindex(ifr.ifr_name); + if (!ifr.ifr_ifindex) { + perror("if_nametoindex"); + return 1; + } + + memset(&addr, 0, sizeof(addr)); + addr.can_family = AF_CAN; + addr.can_ifindex = ifr.ifr_ifindex; + + if (required_mtu > (int)CAN_MTU) { + /* check if the frame fits into the CAN netdevice */ + if (ioctl(s, SIOCGIFMTU, &ifr) < 0) { + perror("SIOCGIFMTU"); + return 1; + } + mtu = ifr.ifr_mtu; + + if (mtu == (int)CANFD_MTU) { + /* interface is ok - try to switch the socket into CAN FD mode */ + if (setsockopt(s, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, + &enable_canfx, sizeof(enable_canfx))){ + printf("error when enabling CAN FD support\n"); + return 1; + } + } + + if (mtu >= (int)CANXL_MIN_MTU) { + /* interface is ok - try to switch the socket into CAN XL mode */ + if (setsockopt(s, SOL_CAN_RAW, CAN_RAW_XL_FRAMES, + &enable_canfx, sizeof(enable_canfx))){ + printf("error when enabling CAN XL support\n"); + return 1; + } + /* try to enable the CAN XL VCID pass through mode */ + if (setsockopt(s, SOL_CAN_RAW, CAN_RAW_XL_VCID_OPTS, + &vcid_opts, sizeof(vcid_opts))) { + printf("error when enabling CAN XL VCID pass through\n"); + return 1; + } + } + } + + /* ensure discrete CAN FD length values 0..8, 12, 16, 20, 24, 32, 64 */ + if (required_mtu == CANFD_MTU) + cu.fd.len = can_fd_dlc2len(can_fd_len2dlc(cu.fd.len)); + + /* CAN XL frames need real frame length for sending */ + if (required_mtu == CANXL_MTU) + required_mtu = CANXL_HDR_SIZE + cu.xl.len; + + /* + * disable default receive filter on this RAW socket This is + * obsolete as we do not read from the socket at all, but for + * this reason we can remove the receive list in the Kernel to + * save a little (really a very little!) CPU usage. + */ + setsockopt(s, SOL_CAN_RAW, CAN_RAW_FILTER, NULL, 0); + + if (bind(s, (struct sockaddr *)&addr, sizeof(addr)) < 0) { + perror("bind"); + return 1; + } + + /* send frame */ + if (write(s, &cu, required_mtu) != required_mtu) { + perror("write"); + return 1; + } + + close(s); + + return 0; +} diff --git a/Devices/Libraries/Systems/can-utils/cansequence.c b/Devices/Libraries/Systems/can-utils/cansequence.c new file mode 100755 index 0000000..5134083 --- /dev/null +++ b/Devices/Libraries/Systems/can-utils/cansequence.c @@ -0,0 +1,446 @@ +/* SPDX-License-Identifier: GPL-2.0-only */ +// Copyright (c) 2007, 2008, 2009, 2010, 2014, 2015, 2019, 2023 Pengutronix, +// Marc Kleine-Budde +// Copyright (c) 2005 Pengutronix, +// Sascha Hauer + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include + +#include +#include + +#define CAN_ID_DEFAULT (2) +#define ANYDEV "any" /* name of interface to receive from any CAN interface */ + +extern int optind, opterr, optopt; + +static int s = -1; +static bool running = true; +static volatile sig_atomic_t signal_num; +static bool infinite = true; +static bool canfd = false; +static bool canfd_strict = false; +static unsigned int drop_until_quit; +static unsigned int drop_count; +static bool use_poll = false; + +static unsigned int loopcount = 1; +static int verbose; + +static struct canfd_frame frame = { + .len = 1, +}; +static struct can_filter filter[] = { + { + .can_id = CAN_ID_DEFAULT, + }, +}; + +static void print_usage(char *prg) +{ + fprintf(stderr, + "Usage: %s [] [Options]\n" + "\n" + "cansequence sends CAN messages with a rising sequence number as payload.\n" + "When the -r option is given, cansequence expects to receive these messages\n" + "and prints an error message if a wrong sequence number is encountered.\n" + "The main purpose of this program is to test the reliability of CAN links.\n" + "\n" + "Options:\n" + " -e, --extended send/receive extended frames\n" + " -f, --canfd send/receive CAN-FD CAN frames\n" + " -s, --strict refuse classical CAN frames in CAN-FD mode\n" + " -b, --brs send CAN-FD CAN frames with bitrate switch (BRS)\n" + " -i, --identifier=ID CAN Identifier (default = %u)\n" + " --loop=COUNT send message COUNT times\n" + " -p, --poll use poll(2) to wait for buffer space while sending\n" + " -q, --quit quit if wrong sequences are encountered\n" + " -r, --receive work as receiver\n" + " -v, --verbose be verbose (twice to be even more verbose\n" + " -h, --help this help\n", + prg, CAN_ID_DEFAULT); +} + +static void sig_handler(int signo) +{ + running = false; + signal_num = signo; +} + +static void do_receive() +{ + uint8_t ctrlmsg[CMSG_SPACE(sizeof(struct timeval)) + CMSG_SPACE(sizeof(__u32))]; + struct iovec iov = { + .iov_base = &frame, + }; + struct msghdr msg = { + .msg_iov = &iov, + .msg_iovlen = 1, + .msg_control = &ctrlmsg, + }; + const int dropmonitor_on = 1; + bool sequence_init = true; + unsigned int sequence_wrap = 0; + uint32_t sequence_mask = 0xff; + uint32_t sequence_rx = 0; + uint8_t sequence_delta = 0; + uint32_t sequence = 0; + unsigned int overflow_old = 0; + can_err_mask_t err_mask = CAN_ERR_MASK; + size_t mtu; + + if (canfd) + mtu = CANFD_MTU; + else + mtu = CAN_MTU; + + if (setsockopt(s, SOL_SOCKET, SO_RXQ_OVFL, + &dropmonitor_on, sizeof(dropmonitor_on)) < 0) { + perror("setsockopt() SO_RXQ_OVFL not supported by your Linux Kernel"); + } + + /* enable recv. of error messages */ + if (setsockopt(s, SOL_CAN_RAW, CAN_RAW_ERR_FILTER, &err_mask, sizeof(err_mask))) { + perror("setsockopt()"); + exit(EXIT_FAILURE); + } + + /* enable recv. now */ + if (setsockopt(s, SOL_CAN_RAW, CAN_RAW_FILTER, filter, sizeof(filter))) { + perror("setsockopt()"); + exit(EXIT_FAILURE); + } + + while ((infinite || loopcount--) && running) { + ssize_t nbytes; + + msg.msg_iov[0].iov_len = mtu; + msg.msg_controllen = sizeof(ctrlmsg); + msg.msg_flags = 0; + + nbytes = recvmsg(s, &msg, 0); + if (nbytes < 0) { + perror("recvmsg()"); + exit(EXIT_FAILURE); + } + + if (frame.can_id & CAN_ERR_FLAG) { + fprintf(stderr, + "sequence CNT: %6u, ERRORFRAME %7x %02x %02x %02x %02x %02x %02x %02x %02x\n", + sequence, frame.can_id, + frame.data[0], frame.data[1], frame.data[2], frame.data[3], + frame.data[4], frame.data[5], frame.data[6], frame.data[7]); + continue; + } + + sequence_rx = frame.data[0]; + + if (canfd_strict && nbytes == CAN_MTU) { + if (verbose > 1) + printf("sequence CNT: 0x%07x RX: 0x%02x (ignoring classical CAN frame)\n", sequence, sequence_rx); + continue; + } + + if (sequence_init) { + sequence_init = false; + sequence = sequence_rx; + } + + sequence_delta = sequence_rx - (uint8_t)sequence; + if (sequence_delta) { + struct cmsghdr *cmsg; + uint32_t overflow = 0; + uint32_t overflow_delta; + + drop_count++; + + for (cmsg = CMSG_FIRSTHDR(&msg); + cmsg && (cmsg->cmsg_level == SOL_SOCKET); + cmsg = CMSG_NXTHDR(&msg,cmsg)) { + if (cmsg->cmsg_type == SO_RXQ_OVFL) { + memcpy(&overflow, CMSG_DATA(cmsg), sizeof(overflow)); + break; + } + } + + overflow_delta = overflow - overflow_old; + + fprintf(stderr, + "sequence CNT: 0x%07x RX: 0x%02x expected: 0x%02x missing: %4u skt overflow delta: %4u absolute: %4u hw - skt: %5d incident: %4u %s%s\n", + sequence, sequence_rx, /* CNT, RX */ + sequence & sequence_mask, sequence_delta, /* expected, missing */ + overflow_delta, overflow, /* skb overflow delta, absolute */ + sequence_delta - overflow_delta, /* hw - skb */ + drop_count, /* incident */ + overflow_delta ? "[SOCKET]" : "", + overflow_delta != sequence_delta ? + ((overflow_delta - sequence_delta > 0 && (overflow_delta - sequence_delta) & 0xff) ? + "[HARDWARE]" : "[HARDWARE?]") : ""); + + if (drop_count == drop_until_quit) + exit(EXIT_FAILURE); + + sequence = sequence_rx; + overflow_old = overflow; + } else if (verbose > 1) { + printf("sequence CNT: 0x%07x RX: 0x%02x\n", sequence, sequence_rx); + } + + sequence++; + if (verbose && !(sequence & sequence_mask)) + printf("sequence wrap around (%d)\n", sequence_wrap++); + } +} + +static void do_send() +{ + unsigned int seq_wrap = 0; + uint8_t sequence = 0; + size_t mtu; + + if (canfd) + mtu = CANFD_MTU; + else + mtu = CAN_MTU; + + while ((infinite || loopcount--) && running) { + ssize_t len; + + if (verbose > 1) + printf("sending frame. sequence number: %d\n", sequence); + +again: + len = write(s, &frame, mtu); + if (len == -1) { + switch (errno) { + case ENOBUFS: { + int err; + struct pollfd fds[] = { + { + .fd = s, + .events = POLLOUT, + }, + }; + + if (!use_poll) { + perror("write"); + exit(EXIT_FAILURE); + } + + err = poll(fds, 1, 1000); + if (err == 0 || (err == -1 && errno != EINTR)) { + perror("poll()"); + exit(EXIT_FAILURE); + } + } + case EINTR: /* fallthrough */ + goto again; + default: + perror("write"); + exit(EXIT_FAILURE); + } + } + + frame.data[0]++; + sequence++; + + if (verbose && !sequence) + printf("sequence wrap around (%d)\n", seq_wrap++); + } +} + +int main(int argc, char **argv) +{ + struct sigaction act = { + .sa_handler = sig_handler, + }; + struct sockaddr_can addr = { + .can_family = AF_CAN, + }; + char *interface = "can0"; + bool extended = false; + bool brs = false; + bool receive = false; + int opt; + + sigaction(SIGINT, &act, NULL); + sigaction(SIGTERM, &act, NULL); + sigaction(SIGHUP, &act, NULL); + + struct option long_options[] = { + { "extended", no_argument, 0, 'e' }, + { "canfd", no_argument, 0, 'f' }, + { "strict", no_argument, 0, 's' }, + { "brs", no_argument, 0, 'b' }, + { "identifier", required_argument, 0, 'i' }, + { "loop", required_argument, 0, 'l' }, + { "poll", no_argument, 0, 'p' }, + { "quit", optional_argument, 0, 'q' }, + { "receive", no_argument, 0, 'r' }, + { "verbose", no_argument, 0, 'v' }, + { "help", no_argument, 0, 'h' }, + { 0, 0, 0, 0 }, + }; + + while ((opt = getopt_long(argc, argv, "efsbi:pq::rvh?", long_options, NULL)) != -1) { + switch (opt) { + case 'e': + extended = true; + break; + + case 'f': + canfd = true; + break; + + case 's': + canfd_strict = true; + break; + + case 'b': + brs = true; /* bitrate switch implies CAN-FD */ + canfd = true; + break; + + case 'i': + filter->can_id = strtoul(optarg, NULL, 0); + break; + + case 'r': + receive = true; + break; + + case 'l': + if (optarg) { + loopcount = strtoul(optarg, NULL, 0); + infinite = false; + } else { + infinite = true; + } + break; + + case 'p': + use_poll = true; + break; + + case 'q': + if (optarg) + drop_until_quit = strtoul(optarg, NULL, 0); + else + drop_until_quit = 1; + break; + + case 'v': + verbose++; + break; + + case 'h': + case '?': + print_usage(basename(argv[0])); + exit(EXIT_SUCCESS); + break; + + default: + print_usage(basename(argv[0])); + exit(EXIT_FAILURE); + break; + } + } + + if (argv[optind] != NULL) + interface = argv[optind]; + + if (extended) { + filter->can_mask = CAN_EFF_MASK; + filter->can_id &= CAN_EFF_MASK; + filter->can_id |= CAN_EFF_FLAG; + } else { + filter->can_mask = CAN_SFF_MASK; + filter->can_id &= CAN_SFF_MASK; + } + frame.can_id = filter->can_id; + filter->can_mask |= CAN_EFF_FLAG; + + printf("interface = %s\n", interface); + + s = socket(AF_CAN, SOCK_RAW, CAN_RAW); + if (s < 0) { + perror("socket()"); + exit(EXIT_FAILURE); + } + + if (strcmp(ANYDEV, interface)) { + addr.can_ifindex = if_nametoindex(interface); + if (!addr.can_ifindex) { + perror("if_nametoindex()"); + exit(EXIT_FAILURE); + } + } + + /* first don't recv. any msgs */ + if (setsockopt(s, SOL_CAN_RAW, CAN_RAW_FILTER, NULL, 0)) { + perror("setsockopt()"); + exit(EXIT_FAILURE); + } + + if (canfd) { + const int enable_canfd = 1; + struct ifreq ifr; + + strncpy(ifr.ifr_name, interface, sizeof(ifr.ifr_name)); + + /* check if the frame fits into the CAN netdevice */ + if (ioctl(s, SIOCGIFMTU, &ifr) < 0) { + perror("SIOCGIFMTU"); + exit(EXIT_FAILURE); + } + + if (ifr.ifr_mtu != CANFD_MTU && ifr.ifr_mtu != CANXL_MTU) { + printf("CAN interface is only Classical CAN capable - sorry.\n"); + exit(EXIT_FAILURE); + } + + /* interface is ok - try to switch the socket into CAN FD mode */ + if (setsockopt(s, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, &enable_canfd, sizeof(enable_canfd))) { + printf("error when enabling CAN FD support\n"); + exit(EXIT_FAILURE); + } + } else { + canfd_strict = false; + } + + if (brs) + frame.flags |= CANFD_BRS; + + if (bind(s, (struct sockaddr *)&addr, sizeof(addr)) < 0) { + perror("bind()"); + exit(EXIT_FAILURE); + } + + if (receive) + do_receive(); + else + do_send(); + + if (signal_num) + return 128 + signal_num; + + exit(EXIT_SUCCESS); +} diff --git a/Devices/Libraries/Systems/can-utils/cansniffer.c b/Devices/Libraries/Systems/can-utils/cansniffer.c new file mode 100755 index 0000000..975166d --- /dev/null +++ b/Devices/Libraries/Systems/can-utils/cansniffer.c @@ -0,0 +1,972 @@ +/* SPDX-License-Identifier: (GPL-2.0-only OR BSD-3-Clause) */ +/* + * cansniffer.c - volatile CAN content visualizer + * + * Copyright (c) 2002-2007 Volkswagen Group Electronic Research + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * 3. Neither the name of Volkswagen nor the names of its contributors + * may be used to endorse or promote products derived from this software + * without specific prior written permission. + * + * Alternatively, provided that this notice is retained in full, this + * software may be distributed under the terms of the GNU General + * Public License ("GPL") version 2, in which case the provisions of the + * GPL apply INSTEAD OF those given above. + * + * The provided data structures and external interfaces from this code + * are not restricted to be used by modules with a GPL compatible license. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT + * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY + * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH + * DAMAGE. + * + * Send feedback to + * + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "terminal.h" + +#define SETFNAME "sniffset." +#define SETFDFNAME "sniffset_fd." +#define FNAME_MAX_LEN 40 + +#define ANYDEV "any" +#define MAX_SLOTS 2048 + +#define CANFD_OFF 0 /* set to OFF */ +#define CANFD_ON 1 /* set to ON */ +#define CANFD_AUTO 2 /* unspecified => check for first received frame */ + +/* flags */ + +#define ENABLE 1 /* by filter or user */ +#define DISPLAY 2 /* is on the screen */ +#define UPDATE 4 /* needs to be printed on the screen */ +#define CLRSCR 8 /* clear screen in next loop */ + +/* flags testing & setting */ + +#define is_set(id, flag) (sniftab[id].flags & flag) +#define is_clr(id, flag) (!(sniftab[id].flags & flag)) + +#define do_set(id, flag) (sniftab[id].flags |= flag) +#define do_clr(id, flag) (sniftab[id].flags &= ~flag) + +/* time defaults */ + +#define TIMEOUT 500 /* in 10ms */ +#define HOLD 100 /* in 10ms */ +#define LOOP 20 /* in 10ms */ + +#define ATTCOLOR ATTBOLD FGRED + +#define LDL " | " /* long delimiter */ +#define SDL "|" /* short delimiter for binary on 80 chars terminal */ + +#define CC_SEP '#' /* interface name separator for Classical CAN */ +#define FD_SEP '*' /* interface name separator for CAN FD */ + +static struct snif { + int flags; + long hold; + long timeout; + struct timeval laststamp; + struct timeval currstamp; + struct canfd_frame last; + struct canfd_frame current; + struct canfd_frame marker; + struct canfd_frame notch; +} sniftab[MAX_SLOTS]; + +extern int optind, opterr, optopt; + +static int idx; +static int running = 1; +static volatile sig_atomic_t signal_num; +static int clearscreen = 1; +static int print_eff; +static int print_ascii = 1; +static int notch; +static int max_dlen = CAN_MAX_DLEN; +static long timeout = TIMEOUT; +static long hold = HOLD; +static long loop = LOOP; +static long canfd_mode = CANFD_AUTO; +static unsigned char binary; +static unsigned char binary8; +static unsigned char binary_gap; +static unsigned char color; +static unsigned char name_sep = CC_SEP; +static char *interface; +static char *vdl = LDL; /* variable delimiter */ +static char *ldl = LDL; /* long delimiter */ + +void print_snifline(int slot); +int handle_keyb(void); +int handle_frame(int fd, long currcms); +int handle_timeo(long currcms); +int writesettings(char* name); +int readsettings(char* name); +int sniftab_index(canid_t id); + +void switchvdl(char *delim) +{ + /* reduce delimiter size for EFF IDs in binary display of up + to 8 data bytes payload to fit into 80 chars per line */ + if (binary8) + vdl = delim; +} + +int comp(const void *elem1, const void *elem2) +{ + unsigned long f = ((struct snif*)elem1)->current.can_id; + unsigned long s = ((struct snif*)elem2)->current.can_id; + + if (f > s) + return 1; + if (f < s) + return -1; + + return 0; +} + +void print_usage(char *prg) +{ + const char manual [] = { + "commands that can be entered at runtime:\n" + " q - quit\n" + " b - toggle binary / HEX-ASCII output\n" + " 8 - toggle binary / HEX-ASCII output (small for EFF on 80 chars)\n" + " B - toggle binary with gap / HEX-ASCII output (exceeds 80 chars!)\n" + " c - toggle color mode\n" + " @ - toggle ASCII output (disabled for CAN FD by default)\n" + " - force a clear screen\n" + " # - notch currently marked/changed bits (can be used repeatedly)\n" + " * - clear notched marked\n" + " rMYNAME - read settings file (filter/notch)\n" + " wMYNAME - write settings file (filter/notch)\n" + " a - enable 'a'll SFF CAN-IDs to sniff\n" + " n - enable 'n'one SFF CAN-IDs to sniff\n" + " A - enable 'A'll EFF CAN-IDs to sniff\n" + " N - enable 'N'one EFF CAN-IDs to sniff\n" + " +FILTER - add CAN-IDs to sniff\n" + " -FILTER - remove CAN-IDs to sniff\n" + "\n" + "FILTER can be a single CAN-ID or a CAN-ID/Bitmask:\n" + "\n" + " single SFF 11 bit IDs:\n" + " +1F5 - add SFF CAN-ID 0x1F5\n" + " -42E - remove SFF CAN-ID 0x42E\n" + "\n" + " single EFF 29 bit IDs:\n" + " +18FEDF55 - add EFF CAN-ID 0x18FEDF55\n" + " -00000090 - remove EFF CAN-ID 0x00000090\n" + "\n" + " CAN-ID/Bitmask SFF:\n" + " -42E7FF - remove SFF CAN-ID 0x42E (using Bitmask)\n" + " -500700 - remove SFF CAN-IDs 0x500 - 0x5FF\n" + " +400600 - add SFF CAN-IDs 0x400 - 0x5FF\n" + " +000000 - add all SFF CAN-IDs\n" + " -000000 - remove all SFF CAN-IDs\n" + "\n" + " CAN-ID/Bitmask EFF:\n" + " -0000000000000000 - remove all EFF CAN-IDs\n" + " +12345678000000FF - add EFF CAN IDs xxxxxx78\n" + " +0000000000000000 - add all EFF CAN-IDs\n" + "\n" + "if (id & filter) == (sniff-id & filter) the action (+/-) is performed,\n" + "which is quite easy when the filter is 000 resp. 00000000 for EFF.\n" + "\n" + }; + + fprintf(stderr, "%s - volatile CAN content visualizer.\n", prg); + fprintf(stderr, "\nUsage: %s [can-interface]\n", prg); + fprintf(stderr, "Options:\n"); + fprintf(stderr, " -q (quiet - all IDs deactivated)\n"); + fprintf(stderr, " -r (read %sname from file)\n", SETFNAME); + fprintf(stderr, " -e (fix extended frame format output - no auto detect)\n"); + fprintf(stderr, " -b (start with binary mode)\n"); + fprintf(stderr, " -8 (start with binary mode - for EFF on 80 chars)\n"); + fprintf(stderr, " -B (start with binary mode with gap - exceeds 80 chars!)\n"); + fprintf(stderr, " -c (color changes)\n"); + fprintf(stderr, " -f (CAN FD mode: 0 = OFF, 1 = ON, 2 = auto detect, default: %d)\n", CANFD_AUTO); + fprintf(stderr, " -t
+ + + diff --git a/Devices/Libraries/Systems/can-utils/slcan_attach.c b/Devices/Libraries/Systems/can-utils/slcan_attach.c new file mode 100755 index 0000000..e72610a --- /dev/null +++ b/Devices/Libraries/Systems/can-utils/slcan_attach.c @@ -0,0 +1,272 @@ +/* SPDX-License-Identifier: (GPL-2.0-only OR BSD-3-Clause) */ +/* + * slcan_attach.c - userspace tool for serial line CAN interface driver SLCAN + * + * Copyright (c) 2002-2007 Volkswagen Group Electronic Research + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * 3. Neither the name of Volkswagen nor the names of its contributors + * may be used to endorse or promote products derived from this software + * without specific prior written permission. + * + * Alternatively, provided that this notice is retained in full, this + * software may be distributed under the terms of the GNU General + * Public License ("GPL") version 2, in which case the provisions of the + * GPL apply INSTEAD OF those given above. + * + * The provided data structures and external interfaces from this code + * are not restricted to be used by modules with a GPL compatible license. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT + * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY + * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH + * DAMAGE. + * + * Send feedback to + * + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +void print_usage(char *prg) +{ + fprintf(stderr, "%s - userspace tool for serial line CAN interface driver SLCAN.\n", prg); + fprintf(stderr, "\nUsage: %s [options] tty\n\n", prg); + fprintf(stderr, "Options:\n"); + fprintf(stderr, " -o (send open command 'O\\r')\n"); + fprintf(stderr, " -l (send listen only command 'L\\r', overrides -o)\n"); + fprintf(stderr, " -c (send close command 'C\\r')\n"); + fprintf(stderr, " -f (read status flags with 'F\\r' to reset error states)\n"); + fprintf(stderr, " -s (set CAN speed 0..8)\n"); + fprintf(stderr, " -b (set bit time register value)\n"); + fprintf(stderr, " -d (only detach line discipline)\n"); + fprintf(stderr, " -w (attach - wait for keypress - detach)\n"); + fprintf(stderr, " -n (assign created netdevice name)\n"); + fprintf(stderr, "\n" + " Bitrate\n" + " 0 10 Kbit/s\n" + " 1 20 Kbit/s\n" + " 2 50 Kbit/s\n" + " 3 100 Kbit/s\n" + " 4 125 Kbit/s\n" + " 5 250 Kbit/s\n" + " 6 500 Kbit/s\n" + " 7 800 Kbit/s\n" + " 8 1000 Kbit/s\n" + "\n"); + fprintf(stderr, "\nExamples:\n"); + fprintf(stderr, "slcan_attach -w -o -f -s6 -c /dev/ttyS1\n\n"); + fprintf(stderr, "slcan_attach /dev/ttyS1\n\n"); + fprintf(stderr, "slcan_attach -d /dev/ttyS1\n\n"); + fprintf(stderr, "slcan_attach -w -n can15 /dev/ttyS1\n\n"); + exit(1); +} + +int main(int argc, char **argv) +{ + int fd; + int ldisc = N_SLCAN; + int detach = 0; + int waitkey = 0; + int send_open = 0; + int send_listen = 0; + int send_close = 0; + int send_read_status_flags = 0; + char *speed = NULL; + char *btr = NULL; + char buf[20]; + static struct ifreq ifr; + char *tty; + char *name = NULL; + int opt; + + while ((opt = getopt(argc, argv, "ldwocfs:b:n:?")) != -1) { + switch (opt) { + case 'd': + detach = 1; + break; + + case 'w': + waitkey = 1; + break; + + case 'o': + send_open = 1; + break; + + case 'l': + send_listen = 1; + break; + + case 'c': + send_close = 1; + break; + + case 'f': + send_read_status_flags = 1; + break; + + case 's': + speed = optarg; + if (strlen(speed) > 1) + print_usage(argv[0]); + break; + + case 'b': + btr = optarg; + if (strlen(btr) > 8) + print_usage(argv[0]); + break; + + case 'n': + name = optarg; + if (strlen(name) > sizeof(ifr.ifr_newname) - 1) + print_usage(argv[0]); + break; + + case '?': + default: + print_usage(argv[0]); + break; + } + } + + if (argc - optind != 1) + print_usage(argv[0]); + + tty = argv[optind]; + + if ((fd = open (tty, O_WRONLY | O_NOCTTY)) < 0) { + perror(tty); + exit(1); + } + + if (waitkey || !detach) { + + if (speed) { + sprintf(buf, "C\rS%s\r", speed); + if (write(fd, buf, strlen(buf)) <= 0) { + perror("write"); + exit(EXIT_FAILURE); + } + } + + if (btr) { + sprintf(buf, "C\rs%s\r", btr); + if (write(fd, buf, strlen(buf)) <= 0) { + perror("write"); + exit(EXIT_FAILURE); + } + } + + if (send_read_status_flags) { + sprintf(buf, "F\r"); + if (write(fd, buf, strlen(buf)) <= 0) { + perror("write"); + exit(EXIT_FAILURE); + } + } + + if (send_listen) { + sprintf(buf, "L\r"); + if (write(fd, buf, strlen(buf)) <= 0) { + perror("write"); + exit(EXIT_FAILURE); + } + } else if (send_open) { + sprintf(buf, "O\r"); + if (write(fd, buf, strlen(buf)) <= 0) { + perror("write"); + exit(EXIT_FAILURE); + } + } + + /* set slcan line discipline on given tty */ + if (ioctl (fd, TIOCSETD, &ldisc) < 0) { + perror("ioctl TIOCSETD"); + exit(1); + } + + /* retrieve the name of the created CAN netdevice */ + if (ioctl (fd, SIOCGIFNAME, ifr.ifr_name) < 0) { + perror("ioctl SIOCGIFNAME"); + exit(1); + } + + printf("attached tty %s to netdevice %s\n", tty, ifr.ifr_name); + + /* try to rename the created device if requested */ + if (name) { + int s = socket(PF_INET, SOCK_DGRAM, 0); + + printf("rename netdevice %s to %s ... ", buf, name); + + if (s < 0) + perror("socket for interface rename"); + else { + /* current slcan%d name is still in ifr.ifr_name */ + memset (ifr.ifr_newname, 0, sizeof(ifr.ifr_newname)); + strncpy (ifr.ifr_newname, name, sizeof(ifr.ifr_newname) - 1); + + if (ioctl(s, SIOCSIFNAME, &ifr) < 0) + printf("failed!\n"); + else + printf("ok.\n"); + + close(s); + } + } + } + + if (waitkey) { + printf("Press any key to detach %s ...\n", tty); + getchar(); + } + + if (waitkey || detach) { + ldisc = N_TTY; + if (ioctl (fd, TIOCSETD, &ldisc) < 0) { + perror("ioctl"); + exit(1); + } + + if (send_close) { + sprintf(buf, "C\r"); + if (write(fd, buf, strlen(buf)) <= 0) { + perror("write"); + exit(EXIT_FAILURE); + } + } + } + + close(fd); + + return 0; +} diff --git a/Devices/Libraries/Systems/can-utils/slcand.c b/Devices/Libraries/Systems/can-utils/slcand.c new file mode 100755 index 0000000..4eb4450 --- /dev/null +++ b/Devices/Libraries/Systems/can-utils/slcand.c @@ -0,0 +1,448 @@ +/* SPDX-License-Identifier: GPL-2.0-only */ +/* + * slcand.c - userspace daemon for serial line CAN interface driver SLCAN + * + * Copyright (c) 2009 Robert Haddon + * Copyright (c) 2009 Verari Systems Inc. + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the version 2 of the GNU General Public License + * as published by the Free Software Foundation. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + * Send feedback to + * + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/* Change this to whatever your daemon is called */ +#define DAEMON_NAME "slcand" + +/* Change this to the user under which to run */ +#define RUN_AS_USER "root" + +/* The length of ttypath buffer */ +#define TTYPATH_LENGTH 256 + +/* UART flow control types */ +#define FLOW_NONE 0 +#define FLOW_HW 1 +#define FLOW_SW 2 + +static void fake_syslog(int priority, const char *format, ...) +{ + va_list ap; + + printf("[%d] ", priority); + va_start(ap, format); + vprintf(format, ap); + va_end(ap); + printf("\n"); +} + +typedef void (*syslog_t)(int priority, const char *format, ...); +static syslog_t syslogger = syslog; + +void print_usage(char *prg) +{ + fprintf(stderr, "%s - userspace daemon for serial line CAN interface driver SLCAN.\n", prg); + fprintf(stderr, "\nUsage: %s [options] [canif-name]\n\n", prg); + fprintf(stderr, "Options:\n"); + fprintf(stderr, " -o (send open command 'O\\r')\n"); + fprintf(stderr, " -c (send close command 'C\\r')\n"); + fprintf(stderr, " -f (read status flags with 'F\\r' to reset error states)\n"); + fprintf(stderr, " -l (send listen only command 'L\\r', overrides -o)\n"); + fprintf(stderr, " -s (set CAN speed 0..8)\n"); + fprintf(stderr, " -S (set UART speed in baud)\n"); + fprintf(stderr, " -t (set UART flow control type 'hw' or 'sw')\n"); + fprintf(stderr, " -b (set bit time register value)\n"); + fprintf(stderr, " -F (stay in foreground; no daemonize)\n"); + fprintf(stderr, " -h (show this help page)\n"); + fprintf(stderr, "\nExamples:\n"); + fprintf(stderr, "slcand -o -c -f -s6 ttyUSB0\n\n"); + fprintf(stderr, "slcand -o -c -f -s6 ttyUSB0 can0\n\n"); + fprintf(stderr, "slcand -o -c -f -s6 /dev/ttyUSB0\n\n"); + exit(EXIT_FAILURE); +} + +static int slcand_running; +static volatile sig_atomic_t exit_code; +static char ttypath[TTYPATH_LENGTH]; + +static void child_handler(int signum) +{ + switch (signum) { + + case SIGUSR1: + /* exit parent */ + exit(EXIT_SUCCESS); + break; + case SIGINT: + case SIGTERM: + case SIGALRM: + case SIGCHLD: + syslogger(LOG_NOTICE, "received signal %i on %s", signum, ttypath); + exit_code = 128 + signum; + slcand_running = 0; + break; + } +} + +static int look_up_uart_speed(long int s) +{ + switch (s) { + + case 9600: + return B9600; + case 19200: + return B19200; + case 38400: + return B38400; + case 57600: + return B57600; + case 115200: + return B115200; + case 230400: + return B230400; + case 460800: + return B460800; + case 500000: + return B500000; + case 576000: + return B576000; + case 921600: + return B921600; + case 1000000: + return B1000000; + case 1152000: + return B1152000; + case 1500000: + return B1500000; + case 2000000: + return B2000000; +#ifdef B2500000 + case 2500000: + return B2500000; +#endif +#ifdef B3000000 + case 3000000: + return B3000000; +#endif +#ifdef B3500000 + case 3500000: + return B3500000; +#endif +#ifdef B3710000 + case 3710000 + return B3710000; +#endif +#ifdef B4000000 + case 4000000: + return B4000000; +#endif + default: + return -1; + } +} + +int main(int argc, char *argv[]) +{ + char *tty = NULL; + char const *devprefix = "/dev/"; + char *name = NULL; + char buf[20]; + static struct ifreq ifr; + struct termios tios; + speed_t old_ispeed; + speed_t old_ospeed; + + int opt; + int send_open = 0; + int send_close = 0; + int send_listen = 0; + int send_read_status_flags = 0; + char *speed = NULL; + char *uart_speed_str = NULL; + long int uart_speed = 0; + int flow_type = FLOW_NONE; + char *btr = NULL; + int run_as_daemon = 1; + char *pch; + int ldisc = N_SLCAN; + int fd; + + ttypath[0] = '\0'; + + while ((opt = getopt(argc, argv, "ocfls:S:t:b:?hF")) != -1) { + switch (opt) { + case 'o': + send_open = 1; + break; + case 'c': + send_close = 1; + break; + case 'f': + send_read_status_flags = 1; + break; + case 'l': + send_listen = 1; + break; + case 's': + speed = optarg; + if (strlen(speed) > 1) + print_usage(argv[0]); + break; + case 'S': + uart_speed_str = optarg; + errno = 0; + uart_speed = strtol(uart_speed_str, NULL, 10); + if (errno) + print_usage(argv[0]); + if (look_up_uart_speed(uart_speed) == -1) { + fprintf(stderr, "Unsupported UART speed (%lu)\n", uart_speed); + exit(EXIT_FAILURE); + } + break; + case 't': + if (!strcmp(optarg, "hw")) { + flow_type = FLOW_HW; + } else if (!strcmp(optarg, "sw")) { + flow_type = FLOW_SW; + } else { + fprintf(stderr, "Unsupported flow type (%s)\n", optarg); + exit(EXIT_FAILURE); + } + break; + case 'b': + btr = optarg; + if (strlen(btr) > 8) + print_usage(argv[0]); + break; + case 'F': + run_as_daemon = 0; + break; + case 'h': + case '?': + default: + print_usage(argv[0]); + break; + } + } + + if (!run_as_daemon) + syslogger = fake_syslog; + + /* Initialize the logging interface */ + openlog(DAEMON_NAME, LOG_PID, LOG_LOCAL5); + + /* Parse serial device name and optional can interface name */ + tty = argv[optind]; + if (NULL == tty) + print_usage(argv[0]); + + name = argv[optind + 1]; + if (name && (strlen(name) > sizeof(ifr.ifr_newname) - 1)) + print_usage(argv[0]); + + /* Prepare the tty device name string */ + pch = strstr(tty, devprefix); + if (pch != tty) + snprintf(ttypath, TTYPATH_LENGTH, "%s%s", devprefix, tty); + else + snprintf(ttypath, TTYPATH_LENGTH, "%s", tty); + + syslogger(LOG_INFO, "starting on TTY device %s", ttypath); + + fd = open(ttypath, O_RDWR | O_NONBLOCK | O_NOCTTY); + if (fd < 0) { + syslogger(LOG_NOTICE, "failed to open TTY device %s\n", ttypath); + perror(ttypath); + exit(EXIT_FAILURE); + } + + /* Configure baud rate */ + memset(&tios, 0, sizeof(tios)); + if (tcgetattr(fd, &tios) < 0) { + syslogger(LOG_NOTICE, "failed to get attributes for TTY device %s: %s\n", ttypath, strerror(errno)); + exit(EXIT_FAILURE); + } + + // Because of a recent change in linux - https://patchwork.kernel.org/patch/9589541/ + // we need to set low latency flag to get proper receive latency + struct serial_struct snew; + ioctl (fd, TIOCGSERIAL, &snew); + snew.flags |= ASYNC_LOW_LATENCY; + ioctl (fd, TIOCSSERIAL, &snew); + + /* Get old values for later restore */ + old_ispeed = cfgetispeed(&tios); + old_ospeed = cfgetospeed(&tios); + + /* Reset UART settings */ + cfmakeraw(&tios); + tios.c_iflag &= ~IXOFF; + tios.c_cflag &= ~CRTSCTS; + + /* Baud Rate */ + cfsetispeed(&tios, look_up_uart_speed(uart_speed)); + cfsetospeed(&tios, look_up_uart_speed(uart_speed)); + + /* Flow control */ + if (flow_type == FLOW_HW) + tios.c_cflag |= CRTSCTS; + else if (flow_type == FLOW_SW) + tios.c_iflag |= (IXON | IXOFF); + + /* apply changes */ + if (tcsetattr(fd, TCSADRAIN, &tios) < 0) + syslogger(LOG_NOTICE, "Cannot set attributes for device \"%s\": %s!\n", ttypath, strerror(errno)); + + if (speed) { + sprintf(buf, "C\rS%s\r", speed); + if (write(fd, buf, strlen(buf)) <= 0) { + perror("write"); + exit(EXIT_FAILURE); + } + } + + if (btr) { + sprintf(buf, "C\rs%s\r", btr); + if (write(fd, buf, strlen(buf)) <= 0) { + perror("write"); + exit(EXIT_FAILURE); + } + } + + if (send_read_status_flags) { + sprintf(buf, "F\r"); + if (write(fd, buf, strlen(buf)) <= 0) { + perror("write"); + exit(EXIT_FAILURE); + } + } + + if (send_listen) { + sprintf(buf, "L\r"); + if (write(fd, buf, strlen(buf)) <= 0) { + perror("write"); + exit(EXIT_FAILURE); + } + } else if (send_open) { + sprintf(buf, "O\r"); + if (write(fd, buf, strlen(buf)) <= 0) { + perror("write"); + exit(EXIT_FAILURE); + } + } + + /* set slcan like discipline on given tty */ + if (ioctl(fd, TIOCSETD, &ldisc) < 0) { + perror("ioctl TIOCSETD"); + exit(EXIT_FAILURE); + } + + /* retrieve the name of the created CAN netdevice */ + if (ioctl(fd, SIOCGIFNAME, ifr.ifr_name) < 0) { + perror("ioctl SIOCGIFNAME"); + exit(EXIT_FAILURE); + } + + syslogger(LOG_NOTICE, "attached TTY %s to netdevice %s\n", ttypath, ifr.ifr_name); + + /* try to rename the created netdevice */ + if (name) { + int s = socket(PF_INET, SOCK_DGRAM, 0); + + if (s < 0) + perror("socket for interface rename"); + else { + /* current slcan%d name is still in ifr.ifr_name */ + memset (ifr.ifr_newname, 0, sizeof(ifr.ifr_newname)); + strncpy (ifr.ifr_newname, name, sizeof(ifr.ifr_newname) - 1); + + if (ioctl(s, SIOCSIFNAME, &ifr) < 0) { + syslogger(LOG_NOTICE, "netdevice %s rename to %s failed\n", buf, name); + perror("ioctl SIOCSIFNAME rename"); + exit(EXIT_FAILURE); + } else + syslogger(LOG_NOTICE, "netdevice %s renamed to %s\n", buf, name); + + close(s); + } + } + + /* Daemonize */ + if (run_as_daemon) { + if (daemon(0, 0)) { + syslogger(LOG_ERR, "failed to daemonize"); + exit(EXIT_FAILURE); + } + } + else { + /* Trap signals that we expect to receive */ + signal(SIGINT, child_handler); + signal(SIGTERM, child_handler); + } + + slcand_running = 1; + + /* The Big Loop */ + while (slcand_running) + sleep(1); /* wait 1 second */ + + /* Reset line discipline */ + syslogger(LOG_INFO, "stopping on TTY device %s", ttypath); + ldisc = N_TTY; + if (ioctl(fd, TIOCSETD, &ldisc) < 0) { + perror("ioctl TIOCSETD"); + exit(EXIT_FAILURE); + } + + if (send_close) { + sprintf(buf, "C\r"); + if (write(fd, buf, strlen(buf)) <= 0) { + perror("write"); + exit(EXIT_FAILURE); + } + } + + /* Reset old rates */ + cfsetispeed(&tios, old_ispeed); + cfsetospeed(&tios, old_ospeed); + + /* apply changes */ + if (tcsetattr(fd, TCSADRAIN, &tios) < 0) + syslogger(LOG_NOTICE, "Cannot set attributes for device \"%s\": %s!\n", ttypath, strerror(errno)); + + /* Finish up */ + syslogger(LOG_NOTICE, "terminated on %s", ttypath); + closelog(); + return exit_code; +} diff --git a/Devices/Libraries/Systems/can-utils/slcanpty.c b/Devices/Libraries/Systems/can-utils/slcanpty.c new file mode 100755 index 0000000..bb26610 --- /dev/null +++ b/Devices/Libraries/Systems/can-utils/slcanpty.c @@ -0,0 +1,532 @@ +/* SPDX-License-Identifier: GPL-2.0-only */ +/* + * slcanpty: adapter for applications using the slcan ASCII protocol + * + * slcanpty.c - creates a pty for applications using the slcan ASCII protocol + * and converts the ASCII data to a CAN network interface (and vice versa) + * + * Copyright (c)2009 Oliver Hartkopp + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the version 2 of the GNU General Public License + * as published by the Free Software Foundation + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + * Send feedback to + * + */ + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "lib.h" + +/* maximum rx buffer len: extended CAN frame with timestamp */ +#define SLC_MTU (sizeof("T1111222281122334455667788EA5F\r") + 1) +#define DEVICE_NAME_PTMX "/dev/ptmx" + +/* read data from pty, send CAN frames to CAN socket and answer commands */ +static int pty2can(int pty, int socket, struct can_filter *fi, + int *is_open, int *tstamp) +{ + unsigned int nbytes, tmp; + char cmd; + static char buf[200]; + char replybuf[10]; /* for answers to received commands */ + unsigned int ptr; + struct can_frame frame; + int ret, i; + static unsigned int rxoffset = 0; /* points to the end of an received incomplete SLCAN message */ + + ret = read(pty, &buf[rxoffset], sizeof(buf) - rxoffset - 1); + if (ret <= 0) { + /* ret == 0 : no error but pty descriptor has been closed */ + if (ret < 0) + perror("read pty"); + + return 1; + } + + nbytes = ret; + /* reset incomplete message offset */ + nbytes += rxoffset; + rxoffset = 0; + +rx_restart: + /* remove trailing '\r' characters to be robust against some apps */ + while (buf[0] == '\r' && nbytes > 0) { + for (tmp = 0; tmp < nbytes; tmp++) + buf[tmp] = buf[tmp + 1]; + nbytes--; + } + + if (!nbytes) + return 0; + + /* check if we can detect a complete SLCAN message including '\r' */ + for (tmp = 0; tmp < nbytes; tmp++) { + if (buf[tmp] == '\r') + break; + } + + /* no '\r' found in the message buffer? */ + if (tmp == nbytes) { + /* save incomplete message */ + rxoffset = nbytes; + + /* leave here and read from pty again */ + return 0; + } + + cmd = buf[0]; + buf[nbytes] = 0; + + for (tmp = 0; tmp < nbytes; tmp++) + if (buf[tmp] == '\r') + pr_debug("@"); + else + pr_debug("%c", buf[tmp]); + pr_debug("\n"); + + /* check for filter configuration commands */ + if (cmd == 'm' || cmd == 'M') { + buf[9] = 0; /* terminate filter string */ + ptr = 9; +#if 0 + /* the filter is no SocketCAN filter :-( */ + + /* TODO: behave like a SJA1000 controller specific filter */ + + if (cmd == 'm') { + fi->can_id = strtoul(buf+1,NULL,16); + fi->can_id &= CAN_EFF_MASK; + } else { + fi->can_mask = strtoul(buf+1,NULL,16); + fi->can_mask &= CAN_EFF_MASK; + } + + if (*is_open) + setsockopt(socket, SOL_CAN_RAW, + CAN_RAW_FILTER, fi, + sizeof(struct can_filter)); +#endif + goto rx_out_ack; + } + + /* check for timestamp on/off command */ + if (cmd == 'Z') { + *tstamp = buf[1] & 0x01; + ptr = 2; + goto rx_out_ack; + } + + /* check for 'O'pen command */ + if (cmd == 'O') { + setsockopt(socket, SOL_CAN_RAW, CAN_RAW_FILTER, fi, sizeof(struct can_filter)); + ptr = 1; + *is_open = 1; + goto rx_out_ack; + } + + /* check for 'C'lose command */ + if (cmd == 'C') { + setsockopt(socket, SOL_CAN_RAW, CAN_RAW_FILTER, NULL, 0); + ptr = 1; + *is_open = 0; + goto rx_out_ack; + } + + /* check for 'V'ersion command */ + if (cmd == 'V') { + sprintf(replybuf, "V1013\r"); + tmp = strlen(replybuf); + ptr = 1; + goto rx_out; + } + /* check for 'v'ersion command */ + if (cmd == 'v') { + sprintf(replybuf, "v1014\r"); + tmp = strlen(replybuf); + ptr = 1; + goto rx_out; + } + + /* check for serial 'N'umber command */ + if (cmd == 'N') { + sprintf(replybuf, "N4242\r"); + tmp = strlen(replybuf); + ptr = 1; + goto rx_out; + } + + /* check for read status 'F'lags */ + if (cmd == 'F') { + sprintf(replybuf, "F00\r"); + tmp = strlen(replybuf); + ptr = 1; + goto rx_out; + } + + /* correctly answer unsupported commands */ + if (cmd == 'U') { + ptr = 2; + goto rx_out_ack; + } + if (cmd == 'S') { + ptr = 2; + goto rx_out_ack; + } + if (cmd == 's') { + ptr = 5; + goto rx_out_ack; + } + if (cmd == 'P' || cmd == 'A') { + ptr = 1; + goto rx_out_nack; + } + if (cmd == 'X') { + ptr = 2; + if (buf[1] & 0x01) + goto rx_out_ack; + else + goto rx_out_nack; + } + + /* catch unknown commands */ + if ((cmd != 't') && (cmd != 'T') && (cmd != 'r') && (cmd != 'R')) { + ptr = nbytes - 1; + goto rx_out_nack; + } + + if (cmd & 0x20) /* tiny chars 'r' 't' => SFF */ + ptr = 4; /* dlc position tiiid */ + else + ptr = 9; /* dlc position Tiiiiiiiid */ + + memset(&frame.data, 0, 8); /* clear data[] */ + + if ((cmd | 0x20) == 'r' && buf[ptr] != '0') { + /* + * RTR frame without dlc information! + * This is against the SLCAN spec but sent + * by a commercial CAN tool ... so we are + * robust against this protocol violation. + */ + + frame.can_dlc = buf[ptr]; /* save following byte */ + + buf[ptr] = 0; /* terminate can_id string */ + + frame.can_id = strtoul(buf + 1, NULL, 16); + frame.can_id |= CAN_RTR_FLAG; + + if (!(cmd & 0x20)) /* NO tiny chars => EFF */ + frame.can_id |= CAN_EFF_FLAG; + + buf[ptr] = frame.can_dlc; /* restore following byte */ + frame.can_dlc = 0; + ptr--; /* we have no dlc component in the violation case */ + + } else { + if (!(buf[ptr] >= '0' && buf[ptr] < '9')) + goto rx_out_nack; + + frame.can_dlc = buf[ptr] - '0'; /* get dlc from ASCII val */ + + buf[ptr] = 0; /* terminate can_id string */ + + frame.can_id = strtoul(buf + 1, NULL, 16); + + if (!(cmd & 0x20)) /* NO tiny chars => EFF */ + frame.can_id |= CAN_EFF_FLAG; + + if ((cmd | 0x20) == 'r') /* RTR frame */ + frame.can_id |= CAN_RTR_FLAG; + + for (i = 0, ptr++; i < frame.can_dlc; i++) { + tmp = asc2nibble(buf[ptr++]); + if (tmp > 0x0F) + goto rx_out_nack; + frame.data[i] = (tmp << 4); + tmp = asc2nibble(buf[ptr++]); + if (tmp > 0x0F) + goto rx_out_nack; + frame.data[i] |= tmp; + } + /* point to last real data */ + if (frame.can_dlc) + ptr--; + } + + ret = write(socket, &frame, sizeof(frame)); + if (ret != sizeof(frame)) { + perror("write socket"); + return 1; + } + +rx_out_ack: + replybuf[0] = '\r'; + tmp = 1; + goto rx_out; +rx_out_nack: + replybuf[0] = '\a'; + tmp = 1; +rx_out: + ret = write(pty, replybuf, tmp); + if (ret < 0) { + perror("write pty replybuf"); + return 1; + } + + /* check if there is another command in this buffer */ + if (nbytes > ptr + 1) { + for (tmp = 0, ptr++; ptr + tmp < nbytes; tmp++) + buf[tmp] = buf[ptr + tmp]; + nbytes = tmp; + goto rx_restart; + } + + return 0; +} + +/* read CAN frames from CAN interface and write it to the pty */ +static int can2pty(int pty, int socket, int *tstamp) +{ + int nbytes; + char cmd; + char buf[SLC_MTU]; + int ptr; + struct can_frame frame; + int i; + + nbytes = read(socket, &frame, sizeof(frame)); + if (nbytes != sizeof(frame)) { + perror("read socket"); + return 1; + } + + /* convert to slcan ASCII frame */ + if (frame.can_id & CAN_RTR_FLAG) + cmd = 'R'; /* becomes 'r' in SFF format */ + else + cmd = 'T'; /* becomes 't' in SFF format */ + + if (frame.can_id & CAN_EFF_FLAG) + sprintf(buf, "%c%08X%d", cmd, frame.can_id & CAN_EFF_MASK, frame.can_dlc); + else + sprintf(buf, "%c%03X%d", cmd | 0x20, frame.can_id & CAN_SFF_MASK, frame.can_dlc); + + ptr = strlen(buf); + + for (i = 0; i < frame.can_dlc; i++) + sprintf(&buf[ptr + 2 * i], "%02X", frame.data[i]); + + if (*tstamp) { + struct timeval tv; + + if (ioctl(socket, SIOCGSTAMP, &tv) < 0) + perror("SIOCGSTAMP"); + + sprintf(&buf[ptr + 2*frame.can_dlc], "%04llX", + (unsigned long long)(tv.tv_sec%60)*1000 + tv.tv_usec/1000); + } + + strcat(buf, "\r"); /* add terminating character */ + nbytes = write(pty, buf, strlen(buf)); + if (nbytes < 0) { + perror("write pty"); + return 1; + } + fflush(NULL); + + return 0; +} + +static int check_select_stdin(void) +{ + fd_set rdfs; + struct timeval timeout; + int ret; + + FD_ZERO(&rdfs); + FD_SET(0, &rdfs); + timeout.tv_sec = 0; + timeout.tv_usec = 0; + + ret = select(1, &rdfs, NULL, NULL, &timeout); + + if (ret < 0) + return 0; /* not selectable */ + + if (ret > 0 && getchar() == EOF) + return 0; /* EOF, eg. /dev/null */ + + return 1; +} + +static void print_usage(const char *prg) +{ + fprintf(stderr, + "%s: adapter for applications using the slcan ASCII protocol.\n" + "\n" + "%s creates a pty for applications using the slcan ASCII protocol and\n" + "converts the ASCII data to a CAN network interface (and vice versa)\n" + "\n" + "Usage: %s \n" + "\n" + "Examples:\n" + "%s /dev/ptyc0 can0 - creates /dev/ttyc0 for the slcan application\n" + "\n" + "e.g. for pseudo-terminal '%s %s can0' creates /dev/pts/N\n" + "\n", + prg, prg, prg, prg, prg, DEVICE_NAME_PTMX); +} + +int main(int argc, char **argv) +{ + fd_set rdfs; + int p; /* pty master file */ + int s; /* can raw socket */ + struct sockaddr_can addr; + struct termios topts; + int select_stdin = 0; + int running = 1; + int tstamp = 0; + int is_open = 0; + struct can_filter fi; + + /* check command line options */ + if (argc != 3) { + print_usage(basename(argv[0])); + return 1; + } + + select_stdin = check_select_stdin(); + + /* open pty */ + p = open(argv[1], O_RDWR); + if (p < 0) { + perror("open pty"); + return 1; + } + + if (tcgetattr(p, &topts)) { + perror("tcgetattr"); + return 1; + } + + /* disable local echo which would cause double frames */ + topts.c_lflag &= ~(ICANON | ECHO | ECHOE | ECHOK | ECHONL | ECHOPRT | ECHOKE); + topts.c_iflag &= ~(ICRNL); + topts.c_iflag |= INLCR; + tcsetattr(p, TCSANOW, &topts); + + /* Support for the Unix 98 pseudo-terminal interface /dev/ptmx /dev/pts/N */ + if (strcmp(argv[1], DEVICE_NAME_PTMX) == 0) { + char *name_pts = NULL; /* slave pseudo-terminal device name */ + + if (grantpt(p) < 0) { + perror("grantpt"); + return 1; + } + + if (unlockpt(p) < 0) { + perror("unlockpt"); + return 1; + } + + name_pts = ptsname(p); + if (name_pts == NULL) { + perror("ptsname"); + return 1; + } + printf("open: %s: slave pseudo-terminal is %s\n", argv[1], name_pts); + } + + /* open socket */ + s = socket(PF_CAN, SOCK_RAW, CAN_RAW); + if (s < 0) { + perror("socket"); + return 1; + } + + addr.can_family = AF_CAN; + addr.can_ifindex = if_nametoindex(argv[2]); + if (!addr.can_ifindex) { + perror("if_nametoindex"); + return 1; + } + + /* disable reception of CAN frames until we are opened by 'O' */ + setsockopt(s, SOL_CAN_RAW, CAN_RAW_FILTER, NULL, 0); + + if (bind(s, (struct sockaddr *)&addr, sizeof(addr)) < 0) { + perror("bind"); + return 1; + } + + /* open filter by default */ + fi.can_id = 0; + fi.can_mask = 0; + + while (running) { + FD_ZERO(&rdfs); + + if (select_stdin) + FD_SET(0, &rdfs); + + FD_SET(p, &rdfs); + FD_SET(s, &rdfs); + + if (select(s + 1, &rdfs, NULL, NULL, NULL) < 0) { + perror("select"); + return 1; + } + + if (FD_ISSET(0, &rdfs)) { + running = 0; + continue; + } + + if (FD_ISSET(p, &rdfs)) + if (pty2can(p, s, &fi, &is_open, &tstamp)) { + running = 0; + continue; + } + + if (FD_ISSET(s, &rdfs)) + if (can2pty(p, s, &tstamp)) { + running = 0; + continue; + } + } + + close(p); + close(s); + + return 0; +} diff --git a/Devices/Libraries/Systems/can-utils/style.css b/Devices/Libraries/Systems/can-utils/style.css new file mode 100755 index 0000000..6be67d2 --- /dev/null +++ b/Devices/Libraries/Systems/can-utils/style.css @@ -0,0 +1,68 @@ +* { + font-family: Helvetica Neue, Helvetica, Arial, sans-serif; +} +body { + max-width: 60em; + margin: 0 auto; + color: #111; +} + +pre, code { + font-family: Monaco, Courier New, monospace; + font-size: 11px; +} + +h1 { + color: rgb(43,105,145); + font-weight: bold; + font-size: 40px; + letter-spacing: -1px; + margin-bottom: -5px; + margin: 0; +} +h1 code { + font-size: 32px; +} + +h2 { + color: rgb(43,105,145); + font-weight: bold; + margin-bottom: -5px; +} +h2 code { + font-size: 22px; +} + +h3 { + margin-bottom: -5px; +} +h3 code { + font-size: 16px; +} + +a { + color: blue; + text-decoration: none; +} +a:visited { + color: navy; +} +a:hover { + text-decoration: underline; +} + +pre { + border-width: 1px; + border-color: #777; + border-style: solid; + padding: 0.5em; + background-color: #ccc; + overflow: auto; + color: #000; + font-weight: bold; +} + +p, li { + font-size: 13px; + line-height: 18px; +} diff --git a/Devices/Libraries/Systems/can-utils/terminal.h b/Devices/Libraries/Systems/can-utils/terminal.h new file mode 100755 index 0000000..198b48e --- /dev/null +++ b/Devices/Libraries/Systems/can-utils/terminal.h @@ -0,0 +1,95 @@ +/* SPDX-License-Identifier: (GPL-2.0-only OR BSD-3-Clause) */ +/* + * Copyright (c) 2002-2007 Volkswagen Group Electronic Research + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * 3. Neither the name of Volkswagen nor the names of its contributors + * may be used to endorse or promote products derived from this software + * without specific prior written permission. + * + * Alternatively, provided that this notice is retained in full, this + * software may be distributed under the terms of the GNU General + * Public License ("GPL") version 2, in which case the provisions of the + * GPL apply INSTEAD OF those given above. + * + * The provided data structures and external interfaces from this code + * are not restricted to be used by modules with a GPL compatible license. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT + * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY + * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH + * DAMAGE. + * + * Send feedback to + * + */ + +#ifndef TERMINAL_H +#define TERMINAL_H + +/* reset to default */ + +#define ATTRESET "\33[0m" + +/* attributes */ + +#define ATTBOLD "\33[1m" +#define ATTUNDERLINE "\33[4m" +#define ATTBLINK "\33[5m" +#define ATTINVERSE "\33[7m" +#define ATTINVISIBLE "\33[8m" + +/* foreground colors */ + +#define FGBLACK "\33[30m" +#define FGRED "\33[31m" +#define FGGREEN "\33[32m" +#define FGYELLOW "\33[33m" +#define FGBLUE "\33[34m" +#define FGMAGENTA "\33[35m" +#define FGCYAN "\33[36m" +#define FGWHITE "\33[37m" + +/* background colors */ + +#define BGBLACK "\33[40m" +#define BGRED "\33[41m" +#define BGGREEN "\33[42m" +#define BGYELLOW "\33[43m" +#define BGBLUE "\33[44m" +#define BGMAGENTA "\33[45m" +#define BGCYAN "\33[46m" +#define BGWHITE "\33[47m" + +/* cursor */ + +#define CSR_HOME "\33[H" +#define CSR_UP "\33[A" +#define CSR_DOWN "\33[B" +#define CSR_RIGHT "\33[C" +#define CSR_LEFT "\33[D" + +#define CSR_HIDE "\33[?25l" +#define CSR_SHOW "\33[?25h" + +/* clear screen */ + +#define CLR_SCREEN "\33[2J" + +#endif /* TERMINAL_H */ diff --git a/Devices/Libraries/Systems/can-utils/testj1939.c b/Devices/Libraries/Systems/can-utils/testj1939.c new file mode 100755 index 0000000..51008d6 --- /dev/null +++ b/Devices/Libraries/Systems/can-utils/testj1939.c @@ -0,0 +1,316 @@ +/* SPDX-License-Identifier: GPL-2.0-only */ +/* + * Copyright (c) 2013 EIA Electronics + * + * Authors: + * Kurt Van Dijck + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the version 2 of the GNU General Public License + * as published by the Free Software Foundation + */ + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include "libj1939.h" + +static const char help_msg[] = + "testj1939: demonstrate j1939 use\n" + "Usage: testj1939 [OPTIONS] FROM TO\n" + " FROM / TO - or [IFACE][:[SA][,[PGN][,NAME]]]\n" + "Options:\n" + " -v Print relevant API calls\n" + " -s[=LEN] Initial send of LEN bytes dummy data\n" + " -r Receive (and print) data\n" + " -e Echo incoming packets back\n" + " This actually receives packets\n" + " -c Issue connect()\n" + " -p=PRIO Set priority to PRIO\n" + " -P Promiscuous mode. Allow to receive all packets\n" + " -b Do normal bind with SA+1 and rebind with actual SA\n" + " -B Allow to send and receive broadcast packets.\n" + " -o Omit bind\n" + " -n Emit 64bit NAMEs in output\n" + " -w[TIME] Return after TIME (default 1) seconds\n" + "\n" + "Examples:\n" + "testj1939 can1 20\n" + "\n" + ; + +static const char optstring[] = "?vbBPos::rep:cnw::"; + +static void onsigalrm(int sig) +{ + err(0, "exit as requested"); + exit(0); +} + +static void schedule_oneshot_itimer(double delay) +{ + struct itimerval it = { 0 }; + + it.it_value.tv_sec = delay; + it.it_value.tv_usec = (long)(delay * 1e6) % 1000000; + if (setitimer(ITIMER_REAL, &it, NULL) < 0) + err(1, "schedule itimer %.3lfs", delay); +} + +/* main */ +int main(int argc, char *argv[]) +{ + int ret, sock, opt; + unsigned int j; + int verbose = 0; + socklen_t peernamelen; + struct sockaddr_can sockname = { + .can_family = AF_CAN, + .can_addr.j1939 = { + .addr = J1939_NO_ADDR, + .name = J1939_NO_NAME, + .pgn = J1939_NO_PGN, + }, + }, peername = { + .can_family = AF_CAN, + .can_addr.j1939 = { + .addr = J1939_NO_ADDR, + .name = J1939_NO_NAME, + .pgn = J1939_NO_PGN, + }, + }; + uint8_t dat[128]; + int valid_peername = 0; + unsigned int todo_send = 0; + int todo_recv = 0, todo_echo = 0, todo_prio = -1; + int todo_connect = 0, todo_names = 0, todo_wait = 0, todo_rebind = 0; + int todo_broadcast = 0, todo_promisc = 0; + int no_bind = 0; + + /* argument parsing */ + while ((opt = getopt(argc, argv, optstring)) != -1) + switch (opt) { + case 'v': + verbose = 1; + break; + case 's': + todo_send = strtoul(optarg ?: "8", NULL, 0); + if (todo_send > sizeof(dat)) + err(1, "Unsupported size. max: %zu", + sizeof(dat)); + break; + case 'r': + todo_recv = 1; + break; + case 'e': + todo_echo = 1; + break; + case 'p': + todo_prio = strtoul(optarg, NULL, 0); + break; + case 'P': + todo_promisc = 1; + break; + case 'c': + todo_connect = 1; + break; + case 'n': + todo_names = 1; + break; + case 'b': + todo_rebind = 1; + break; + case 'B': + todo_broadcast = 1; + break; + case 'o': + no_bind = 1; + break; + case 'w': + schedule_oneshot_itimer(strtod(optarg ?: "1", NULL)); + signal(SIGALRM, onsigalrm); + todo_wait = 1; + break; + default: + fputs(help_msg, stderr); + exit(1); + break; + } + + if (argv[optind]) { + if (strcmp("-", argv[optind]) != 0) + libj1939_parse_canaddr(argv[optind], &sockname); + ++optind; + } + + if (todo_rebind) + sockname.can_addr.j1939.addr++; + + if (argv[optind]) { + if (strcmp("-", argv[optind]) != 0) { + libj1939_parse_canaddr(argv[optind], &peername); + valid_peername = 1; + } + ++optind; + } + + /* open socket */ + if (verbose) + fprintf(stderr, "- socket(PF_CAN, SOCK_DGRAM, CAN_J1939);\n"); + sock = ret = socket(PF_CAN, SOCK_DGRAM, CAN_J1939); + if (ret < 0) + err(1, "socket(j1939)"); + + if (todo_promisc) { + if (verbose) + fprintf(stderr, "- setsockopt(, SOL_CAN_J1939, SO_J1939_PROMISC, %d, %zd);\n", + todo_promisc, sizeof(todo_promisc)); + ret = setsockopt(sock, SOL_CAN_J1939, SO_J1939_PROMISC, + &todo_promisc, sizeof(todo_promisc)); + if (ret < 0) + err(1, "setsockopt: filed to set promiscuous mode"); + } + + if (todo_broadcast) { + if (verbose) + fprintf(stderr, "- setsockopt(, SOL_SOCKET, SO_BROADCAST, %d, %zd);\n", + todo_broadcast, sizeof(todo_broadcast)); + ret = setsockopt(sock, SOL_SOCKET, SO_BROADCAST, + &todo_broadcast, sizeof(todo_broadcast)); + if (ret < 0) + err(1, "setsockopt: filed to set broadcast"); + } + + if (todo_prio >= 0) { + if (verbose) + fprintf(stderr, "- setsockopt(, SOL_CAN_J1939, SO_J1939_SEND_PRIO, &%i);\n", todo_prio); + ret = setsockopt(sock, SOL_CAN_J1939, SO_J1939_SEND_PRIO, + &todo_prio, sizeof(todo_prio)); + if (ret < 0) + err(1, "set priority %i", todo_prio); + } + + if (!no_bind) { + + if (verbose) + fprintf(stderr, "- bind(, %s, %zi);\n", libj1939_addr2str(&sockname), sizeof(sockname)); + ret = bind(sock, (void *)&sockname, sizeof(sockname)); + if (ret < 0) + err(1, "bind()"); + + if (todo_rebind) { + /* rebind with actual SA */ + sockname.can_addr.j1939.addr--; + + if (verbose) + fprintf(stderr, "- bind(, %s, %zi);\n", libj1939_addr2str(&sockname), sizeof(sockname)); + ret = bind(sock, (void *)&sockname, sizeof(sockname)); + if (ret < 0) + err(1, "re-bind()"); + } + } + + if (todo_connect) { + if (!valid_peername) + err(1, "no peername supplied"); + if (verbose) + fprintf(stderr, "- connect(, %s, %zi);\n", libj1939_addr2str(&peername), sizeof(peername)); + ret = connect(sock, (void *)&peername, sizeof(peername)); + if (ret < 0) + err(1, "connect()"); + } + + if (todo_send) { + /* initialize test vector */ + for (j = 0; j < sizeof(dat); ++j) + dat[j] = ((2*j) << 4) + ((2*j+1) & 0xf); + + /* send data */ + /* + * when using connect, do not provide additional + * destination information and use send() + */ + if (valid_peername && !todo_connect) { + if (verbose) + fprintf(stderr, "- sendto(, , %i, 0, %s, %zi);\n", todo_send, libj1939_addr2str(&peername), sizeof(peername)); + ret = sendto(sock, dat, todo_send, 0, + (void *)&peername, sizeof(peername)); + } else { + /* + * we may do sendto(sock, dat, todo_send, 0, NULL, 0) + * as well, but using send() demonstrates the API better + */ + if (verbose) + fprintf(stderr, "- send(, , %i, 0);\n", todo_send); + ret = send(sock, dat, todo_send, 0); + } + + if (ret < 0) + err(1, "sendto"); + } + + /* main loop */ + if ((todo_echo || todo_recv) && verbose) + fprintf(stderr, "- while (1)\n"); + while (todo_echo || todo_recv) { + /* + * re-use peername for storing the sender's peername of + * received packets + */ + if (verbose) + fprintf(stderr, "- recvfrom(, , %zi, 0, &, %zi);\n", sizeof(peername), sizeof(peername)); + peernamelen = sizeof(peername); + ret = recvfrom(sock, dat, sizeof(dat), 0, + (void *)&peername, &peernamelen); + if (ret < 0) { + if (EINTR == errno) { + if (verbose) + fprintf(stderr, "-\t\n"); + continue; + } + err(1, "recvfrom()"); + } + + if (todo_echo) { + if (verbose) + fprintf(stderr, "- sendto(, , %i, 0, %s, %i);\n", ret, libj1939_addr2str(&peername), peernamelen); + ret = sendto(sock, dat, ret, 0, + (void *)&peername, peernamelen); + if (ret < 0) + err(1, "sendto"); + } + if (todo_recv) { + int i; + + if (todo_names && peername.can_addr.j1939.name) + printf("%016llx ", peername.can_addr.j1939.name); + printf("%02x %05x:", peername.can_addr.j1939.addr, + peername.can_addr.j1939.pgn); + for (i = 0, j = 0; i < ret; ++i, j++) { + if (j == 8) { + printf("\n%05x ", i); + j = 0; + } + printf(" %02x", dat[i]); + } + printf("\n"); + } + } + if (todo_wait) + for (;;) + sleep(1); + return 0; +} + diff --git a/Devices/Packages/diff_wheel_plugin/CMakeLists.txt b/Devices/Packages/diff_wheel_plugin/CMakeLists.txt new file mode 100755 index 0000000..6462a7d --- /dev/null +++ b/Devices/Packages/diff_wheel_plugin/CMakeLists.txt @@ -0,0 +1,233 @@ +cmake_minimum_required(VERSION 3.0.2) +project(diff_wheel_plugin) + +## Compile as C++17, supported in ROS Kinetic and newer +add_compile_options(-std=c++17) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + roscpp + std_msgs + models + pluginlib + realtime_tools + geometry_msgs + delta_modbus + md_controller + nav_2d_utils +) + +## System dependencies are found with CMake's conventions +find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a exec_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# std_msgs # Or other packages containing msgs +# ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( + INCLUDE_DIRS include + LIBRARIES diff_wheel_plugin + CATKIN_DEPENDS roscpp geometry_msgs std_msgs realtime_tools + DEPENDS Boost +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} +) + +## Declare a C++ library +add_library(${PROJECT_NAME} + src/wheel_plugin.cpp + # src/encoder_plugin.cpp +) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +## With catkin_make all packages are built within a single CMake context +## The recommended prefix ensures that target names across packages don't collide +add_executable(diff_wheel_feedback src/diff_wheel_feedback.cpp) +add_executable(diff_wheel_controller src/diff_wheel_controller.cpp) + +## Rename C++ executable without prefix +## The above recommended prefix causes long target names, the following renames the +## target back to the shorter version for ease of user use +## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" +# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") + +## Add cmake target dependencies of the executable +## same as for the library above +add_dependencies(diff_wheel_feedback ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +add_dependencies(diff_wheel_controller ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +target_link_libraries(${PROJECT_NAME} + ${Boost_LIBRARIES} + ${catkin_LIBRARIES} +) + +target_link_libraries(diff_wheel_feedback + ${PROJECT_NAME} + ${Boost_LIBRARIES} + ${catkin_LIBRARIES} +) + +target_link_libraries(diff_wheel_controller + ${PROJECT_NAME} + ${Boost_LIBRARIES} + ${catkin_LIBRARIES} +) +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# catkin_install_python(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html +install(TARGETS diff_wheel_feedback diff_wheel_controller + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +## Mark libraries for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) + +## Mark cpp header files for installation +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE +) + +install(DIRECTORY + launch + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_diff_wheel_plugin.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/Devices/Packages/diff_wheel_plugin/include/diff_wheel_plugin/command.h b/Devices/Packages/diff_wheel_plugin/include/diff_wheel_plugin/command.h new file mode 100755 index 0000000..942309a --- /dev/null +++ b/Devices/Packages/diff_wheel_plugin/include/diff_wheel_plugin/command.h @@ -0,0 +1,17 @@ +#ifndef __DIFF_WHEEL_PLUGIN_COMMAND_H_INCLUDED_ +#define __DIFF_WHEEL_PLUGIN_COMMAND_H_INCLUDED_ + +#include + +namespace diff_wheel_plugin +{ + class WheelCommand + { + public: + double vel_wheel; + ros::Time stamp; + + WheelCommand() : vel_wheel(0.0), stamp(0.0) {} + }; +} // namespace diff_wheel_plugin +#endif \ No newline at end of file diff --git a/Devices/Packages/diff_wheel_plugin/include/diff_wheel_plugin/wheel_plugin.h b/Devices/Packages/diff_wheel_plugin/include/diff_wheel_plugin/wheel_plugin.h new file mode 100755 index 0000000..e92ab25 --- /dev/null +++ b/Devices/Packages/diff_wheel_plugin/include/diff_wheel_plugin/wheel_plugin.h @@ -0,0 +1,159 @@ +#ifndef __ROS_DIFF_WHEEL_PLUGIN_H_INCLUDED_ +#define __ROS_DIFF_WHEEL_PLUGIN_H_INCLUDED_ + +#include +#include +#include "diff_wheel_plugin/command.h" +#include +#include +// Boost +#include +#include +//plugin +#include + + +namespace diff_wheel_plugin +{ +/** + * @class WheelPlugin + */ +class WheelPlugin : public models::BaseSteerDrive +{ +public: + + /** + * @brief Contructor + */ + WheelPlugin(); + + /** + * @brief Contructor + * @param[in] nh NodeHandle + * @param[in] name The name to give this instance of the model base + */ + WheelPlugin(ros::NodeHandle & nh, const std::string &name); + + + /** + * @brief Decontructor + */ + virtual ~WheelPlugin(); + + /** + * @brief Init + * @param[in] nh NodeHandle + * @param name The name to give this instance of the model base + */ + virtual void initialize(ros::NodeHandle & nh, const std::string &name) override; + + /** + * @brief Constructs + * @param name Check model is deady + * @return true, if model is ready, false otherwise + */ + virtual bool Ready() override; + + /** + * @brief Get the velocity limit on axis(index). + * @return Velocity limit specified in SDF + */ + virtual double GetVelocityLimit(unsigned int _index) override; + + /** + * @brief Set the velocity limit on a joint axis. + * @param[in] _velocity Velocity limit for the axis. + */ + virtual void SetVelocityLimit(double _velocity) override; + + /** + * @brief Get the rotation rate of an axis(index) + * @return The rotaional velocity of the joint axis. (rad/s) + */ + virtual double GetVelocity() override; + + /** + * @brief Set the velocity of an axis(index). + * In ODE and Bullet, the SetVelocityMaximal function is used to + * set the velocity of the child link relative to the parent. + * In Simbody and DART, this function updates the velocity state, + * which has a recursive effect on the rest of the chain. + * @param[in] _vel Velocity. + */ + virtual void SetVelocity(double _vel) override; + + /** + * @brief Get the position of an axis according to its index. + * + * For rotational axes, the value is in radians. For prismatic axes, + * it is in meters. + * + * For static models, it returns the static joint position. + * + * It returns ignition::math::NAN_D in case the position can't be + * obtained. For instance, if the index is invalid, if the joint is + * fixed, etc. + * + * Subclasses can't override this method. See PositionImpl instead. + * + * @return Current position of the axis. + * @sa PositionImpl + */ + virtual double Position() override; + + /** + * @brief The child links of this joint are updated based on desired + * position. And all the links connected to the child link of this joint + * except through the parent link of this joint moves with the child + * link. + * @param[in] _position Position to set the joint to. + * unspecified, pure kinematic teleportation. + * link with respect to the world frame will remain the same after + * setting the position. By default this is false, which means there are + * no guarantees about what the child link's world velocity will be after + * the position is changed (the behavior is determined by the underlying + * physics engine). + * + * @note{Only ODE and Bullet support _preserveWorldVelocity being true.} + */ + virtual void SetPosition(const double _position) override; + +private: + + /** + * @brief Brake by away SetVelocity(0.0) + */ + void brake() { this->SetVelocity(0.0); } + + /** + * @brief call back from a subscriber + */ + void DLS_Callback(const std_msgs::Float32::ConstPtr & msg); + + /** + * @brief Do stuff when create boost::Thread + */ + void DoStuff(void); + + ros::NodeHandle nh_; + ros::NodeHandle nh_priv_; + std::string name_; + bool initialized_; + bool is_ready_; + int m_subscribe_queue_size_; + double update_rate_; + boost::thread * m_publish_thread_; + boost::mutex callback_mutex_; + std_msgs::Float32::ConstPtr m_measurement_; + + bool allow_multiple_command_publishers_; + realtime_tools::RealtimeBuffer command_; + diff_wheel_plugin::WheelCommand command_struct_; + double command_timeout_; + ros::Subscriber mesurement_sub_; + boost::shared_ptr> wheel_pub_; +}; + +} // namespace diff_wheel_plugin + +#endif // __ROS_DIFF_WHEEL_PLUGIN_H_INCLUDED_ \ No newline at end of file diff --git a/Devices/Packages/diff_wheel_plugin/launch/run.launch b/Devices/Packages/diff_wheel_plugin/launch/run.launch new file mode 100755 index 0000000..87c508d --- /dev/null +++ b/Devices/Packages/diff_wheel_plugin/launch/run.launch @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/Devices/Packages/diff_wheel_plugin/package.xml b/Devices/Packages/diff_wheel_plugin/package.xml new file mode 100755 index 0000000..4f4a770 --- /dev/null +++ b/Devices/Packages/diff_wheel_plugin/package.xml @@ -0,0 +1,89 @@ + + + diff_wheel_plugin + 0.0.0 + The diff_wheel_plugin package + + + + + robotics + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + roscpp + roscpp + roscpp + roscpp + + std_msgs + geometry_msgs + delta_modbus_tcp + md_controller + models + pluginlib + realtime_tools + nav_2d_utils + + std_msgs + geometry_msgs + delta_modbus_tcp + md_controller + models + pluginlib + realtime_tools + nav_2d_utils + + std_msgs + geometry_msgs + delta_modbus_tcp + md_controller + models + pluginlib + realtime_tools + nav_2d_utils + + + + + + + diff --git a/Devices/Packages/diff_wheel_plugin/plugins.xml b/Devices/Packages/diff_wheel_plugin/plugins.xml new file mode 100755 index 0000000..dfc9096 --- /dev/null +++ b/Devices/Packages/diff_wheel_plugin/plugins.xml @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/Devices/Packages/diff_wheel_plugin/src/diff_wheel_controller.cpp b/Devices/Packages/diff_wheel_plugin/src/diff_wheel_controller.cpp new file mode 100755 index 0000000..d956e58 --- /dev/null +++ b/Devices/Packages/diff_wheel_plugin/src/diff_wheel_controller.cpp @@ -0,0 +1,201 @@ +#include +#include +#include +#include "md_controller/md_controller.h" +#include "md_controller/diagnostic.h" +#include "md_controller/md_controller_subscriber.h" +#include "libserial/udevadm_info.h" +#include +#include + +/* define struct */ +typedef struct device{ + std::string _port; + int _baud; +}device_t; + + +bool findDevice(ros::NodeHandlePtr nh, std::string node_name, device_t *dv) { + ROS_INFO("loadParam() - node_name: %s", node_name.c_str()); + if(!nh->param(node_name + "/baudrate", dv->_baud, dv->_baud)) + { + if(!nh->getParam("baudrate", dv->_baud)) + return false; + } + + ROS_INFO("product"); + std::string product; //port name + if(!nh->param(node_name + "/product", product, product)) {} + else if(!nh->getParam("product", product)) {} + else + { + udevadmInfo *ludev = new udevadmInfo(product.c_str()); + if(ludev->init() == EXIT_FAILURE) {} + if(ludev->scanDevices()) + { + strcpy(dv->_port.data(), ludev->port); + return true; + } + } + + ROS_INFO("port_name"); + if(!nh->param(node_name + "/port_name", dv->_port, dv->_port)) + { + if(!nh->getParam("port_name", dv->_port)) + return false; + } + + ROS_WARN("Can not scan device from /product. So try to use /port_name is %s", dv->_port.c_str()); + return true; + +} + +/** + * @brief shortcut to read a member from a XmlRpcValue, or to return a defaultvalue, it the member does not exist + */ +template static T readMember(XmlRpc::XmlRpcValue & value, const std::string & member, const T & defaultvalue) +{ + try + { + if(value.hasMember(member)) + return value[member]; + return defaultvalue; + } + catch(const XmlRpc::XmlRpcException& e) + { + ROS_WARN_STREAM( "'"<(); + std::string node_name = ros::this_node::getName(); + ROS_INFO("%s.cpp-node_name: %s", node_name.c_str(), node_name.c_str()); + + /* connect with the server */ + std::string diagnostic_topic = "MD_diagnostic"; + MD::Diagnostic::init(*nh, diagnostic_topic, "MD md_controller"); + + + XmlRpc::XmlRpcValue drivers; + if(!nh->getParam(node_name + "/drivers", drivers) || drivers.size() < 1) + { + ROS_ERROR("%s: no driver found in yaml-file, please check configuration. Aborting...", node_name.c_str()); + MD::Diagnostic::update(MD::DIAGNOSTIC_STATUS::CONFIGURATION_ERROR, " No driver found in yaml-file"); + return 2; + } + + std::vector> p_md_controller_subscriber; + for(XmlRpc::XmlRpcValue::iterator object_iter = drivers.begin(); object_iter != drivers.end(); ++object_iter) + { + std::string topic_name = readMember(object_iter->second, "toppic", ""); + int subscribe_queue_size = readMember(object_iter->second, "subscribe_queue_size", 1); + ROS_INFO("%s: initializing toppic \"%s\" queue_size \"%d\"...", node_name.c_str(), topic_name.c_str(), subscribe_queue_size); + p_md_controller_subscriber.push_back(boost::make_shared(*nh, object_iter)); + } + + device_t dv; + if(findDevice(nh, node_name, &dv)) + { + ROS_INFO("%s: found device successfull", node_name.c_str()); + } + else + { + ROS_ERROR("%s: Error when find device", node_name.c_str()); + return 2; + } + + uint8_t rate = 50; // Hz + ros::Rate loop_rate(rate); + + if (p_md_controller_subscriber.empty()) { + ROS_WARN("No subscribers available!"); + return 1; + } + + while (ros::ok()) + { + ROS_INFO("%s: Conecting port name %s with baudrate %d ...", node_name.c_str(), dv._port.c_str(), dv._baud); + boost::shared_ptr md_controller = boost::make_shared(nh, dv._port.c_str(), dv._baud, PARITY_NONE, DATABIT_8, STOPBIT_1); + ros::Duration(1.0).sleep(); + const std::string password = "robotics"; // Replace with the correct password + // First, set to RS232 + std::string command = "sudo -S -k echo rs232 > /sys/class/sp339_mode_ctl/uartMode"; + std::string full_command = "echo '" +password + "' | " + command; + system(full_command.c_str()); + + // Sleep for 0.5 seconds + ros::Duration(1.0).sleep(); + + // Now, set to RS485 + command = "sudo -S -k echo rs485 > /sys/class/sp339_mode_ctl/uartMode"; + full_command = "echo '" + password + "' | " + command; + system(full_command.c_str()); + + for(auto &p_sub : p_md_controller_subscriber) + { + md_controller->pid_break(p_sub->slave_id); + md_controller->pid_break(p_sub->slave_id); + } + int v_min = 60; + while(ros::ok() && md_controller->_connected) + { + for(auto it = p_md_controller_subscriber.begin(); it != p_md_controller_subscriber.end(); ++it) + { + auto &p_sub = *it; + if(p_sub->isWheelTriggered()) + { + int16_t rotate_speed_wheel; + { + boost::lock_guard publish_lockguard(p_sub->m_measurement_mutex); + rotate_speed_wheel = p_sub->speed; + } + + if(std::abs(rotate_speed_wheel) > 0) + { + int sign = (rotate_speed_wheel / abs(rotate_speed_wheel)) > 0 ? 1 : -1; + int16_t speed = abs(rotate_speed_wheel) < v_min? (int)(sign * v_min) : rotate_speed_wheel; + md_controller->pid_vel_cmd(p_sub->slave_id, speed); + // ROS_INFO("%x %d", p_sub->slave_id, speed); + } + else + { + md_controller->pid_break(p_sub->slave_id); + if(abs(rotate_speed_wheel) > 0) ROS_INFO("%x Brake %d", p_sub->slave_id, rotate_speed_wheel); + } + p_sub->scheduleWheelController(false); + } + + if(p_sub->isWheelLastestTriggered()) + { + int16_t rotate_speed_wheel; + { + boost::lock_guard publish_lockguard(p_sub->m_measurement_mutex); + rotate_speed_wheel = p_sub->speed; + } + md_controller->pid_stop(p_sub->slave_id); + if(std::abs(rotate_speed_wheel) > 0) ROS_WARN("%x stop %d", p_sub->slave_id, rotate_speed_wheel); + p_sub->scheduleWheelController(false); + } + } + loop_rate.sleep(); + ros::spinOnce(); + } + ros::spinOnce(); + for(auto &p_sub : p_md_controller_subscriber) + { + md_controller->pid_break(p_sub->slave_id); + md_controller->pid_break(p_sub->slave_id); + } + md_controller->close_port(); + } + ros::spin(); + return 0; +} + diff --git a/Devices/Packages/diff_wheel_plugin/src/diff_wheel_feedback.cpp b/Devices/Packages/diff_wheel_plugin/src/diff_wheel_feedback.cpp new file mode 100755 index 0000000..abe52fd --- /dev/null +++ b/Devices/Packages/diff_wheel_plugin/src/diff_wheel_feedback.cpp @@ -0,0 +1,81 @@ +#include +#include +#include "delta_modbus/delta_modbus_tcp.h" +#include + +double combine_uint16_to_double(uint16_t high, uint16_t low) { + // Chuyển đổi high thành double và dịch nó sang trái 16 bit + double result = (double)high * 65536.0; // 65536 = 2^16 + + // Thêm low vào kết quả + result += (double)low; + + return result; +} + +/********************************************************************** + * MAIN +***********************************************************************/ +int main(int argc, char** argv) { + ros::init(argc, argv, "diff_encoder_node"); + ros::NodeHandle nh; + ros::NodeHandle nh_node("~"); + std::string node_name = ros::this_node::getName(); + ROS_INFO("node_name: %s", node_name.c_str()); + + std::string ip_address; + int port; + + if(!nh_node.getParam("ip_address", ip_address)) + { + ROS_ERROR("Could not find 'ip_address' in node handle"); + return 1; + } + if(!nh_node.getParam("port", port)) + { + ROS_ERROR("Could not find 'port' in node handle"); + return 1; + } + + int id; + nh_node.param("id", id, 1); + double rate; + nh_node.param("publish_rate", rate, 20.0); + + int start, end; + nh_node.param("start_bit", start, 101); + nh_node.param("end_bit", end, 103); + + ROS_INFO("%s: start_bit: %d end_bit: %d ", node_name.c_str(), start, end); + + ros::Publisher left_encoder_pub = nh.advertise("left_encoder", 1); + ros::Publisher right_encoder_pub = nh.advertise("right_encoder", 1); + DELTA_NAMESPACE::PLC *plc = NULL; + plc = new DELTA_NAMESPACE::PLC(ip_address, port, id); + ros::Rate r(rate); + uint16_t dataD[4]; + float left_encoder, righ_encoder; + while (ros::ok()) + { + plc->connect(); + while (ros::ok() && plc->checkConnected()) + { + plc->mulGetD(start, end, dataD); + std::memcpy(reinterpret_cast(&righ_encoder), reinterpret_cast(&dataD[0]), sizeof left_encoder); + std::memcpy(reinterpret_cast(&left_encoder), reinterpret_cast(&dataD[2]), sizeof righ_encoder); + std_msgs::Float32 left_encoder_msg, righ_encoder_msg; + left_encoder_msg.data = left_encoder * 2 * M_PI; + righ_encoder_msg.data = righ_encoder * 2 * M_PI; + left_encoder_pub.publish(left_encoder_msg); + right_encoder_pub.publish(righ_encoder_msg); + r.sleep(); + ros::spinOnce(); + } + ros::spinOnce(); + } + + ros::spin(); + plc = NULL; + delete(plc); + return 0; +} \ No newline at end of file diff --git a/Devices/Packages/diff_wheel_plugin/src/wheel_plugin.cpp b/Devices/Packages/diff_wheel_plugin/src/wheel_plugin.cpp new file mode 100755 index 0000000..ae403cf --- /dev/null +++ b/Devices/Packages/diff_wheel_plugin/src/wheel_plugin.cpp @@ -0,0 +1,192 @@ +#include "diff_wheel_plugin/wheel_plugin.h" +#include +#include +#include +#include +#include + + +namespace diff_wheel_plugin +{ + +WheelPlugin::WheelPlugin() + : command_timeout_(0.5) +{ + +} + +WheelPlugin::WheelPlugin(ros::NodeHandle & nh, const std::string &name) + : command_timeout_(0.5) +{ + initialize(nh, name); +} + + +WheelPlugin::~WheelPlugin() +{ + if (m_publish_thread_) + { + m_publish_thread_->join(); + delete (m_publish_thread_); + m_publish_thread_ = 0; + } +} + +void WheelPlugin::initialize(ros::NodeHandle & nh, const std::string &name) +{ + if(!initialized_) + { + nh_ = nh; + name_ = name; + nh_priv_ = ros::NodeHandle(nh, name); + nh_priv_.param("command_timeout", command_timeout_, 0.5); + nh_priv_.param("subscribe_queue_size", m_subscribe_queue_size_, 1); + nh_priv_.param("max_publish_rate", update_rate_, 100.0); + + ROS_INFO_NAMED("WheelPlugin","Initializing on %s with command_timeout %f subscribe_queue_size %d max_publish_rate %f", + name_.c_str(), + command_timeout_, + m_subscribe_queue_size_, + update_rate_ + ); + std::string mesurement_topic; + nh_priv_.param("mesurement_topic", mesurement_topic, std::string("")); + std::string wheel_topic; + nh_priv_.param("wheel_topic", wheel_topic, name_); + + mesurement_sub_ = nh_.subscribe(mesurement_topic, m_subscribe_queue_size_, &diff_wheel_plugin::WheelPlugin::DLS_Callback, this); + + wheel_pub_.reset(new realtime_tools::RealtimePublisher(nh_, wheel_topic , 100)); + + m_publish_thread_ = new boost::thread(&diff_wheel_plugin::WheelPlugin::DoStuff,this); + command_struct_.stamp = ros::Time::now(); + ROS_INFO_NAMED("WheelPlugin","Initializing on %s is successed", name_.c_str()); + + initialized_ = true; + } +} + +bool WheelPlugin::Ready() +{ + return is_ready_; +} + +double WheelPlugin::GetVelocityLimit(unsigned int _index) +{ + throw std::bad_function_call(); +} + +void WheelPlugin::SetVelocityLimit(double _velocity) +{ + throw std::bad_function_call(); +} + +double WheelPlugin::GetVelocity() +{ + boost::lock_guard publish_lockguard(callback_mutex_); + double result = 0.0; + if(m_measurement_) + { + result = m_measurement_->data; + } + return result; +} + +void WheelPlugin::SetVelocity(double _vel) +{ + if (!std::isfinite(_vel)) + { + ROS_WARN_THROTTLE(1.0, "Received NaN in velocity %s command. Ignoring.", name_.c_str()); + return; + } + command_struct_.vel_wheel = _vel; + command_struct_.stamp = ros::Time::now(); + command_.writeFromNonRT(command_struct_); + ROS_DEBUG_STREAM_NAMED(name_, + "Added values to command." + << "vel_wheel: " << command_struct_.vel_wheel << ", " + << "Stamp: " << command_struct_.stamp); + + // ROS_INFO_STREAM_NAMED(name_, + // "Added values to command." + // << "vel_wheel: " << command_struct_.vel_wheel << ", " + // << "Stamp: " << command_struct_.stamp); +} + +double WheelPlugin::Position() +{ + throw std::bad_function_call(); +} + +void WheelPlugin::SetPosition(const double _position) +{ + throw std::bad_function_call(); +} + +void WheelPlugin::DLS_Callback(const std_msgs::Float32::ConstPtr & msg) +{ + boost::lock_guard publish_lockguard(callback_mutex_); + // check that we don't have multiple publishers on the command topic + if (!allow_multiple_command_publishers_ && mesurement_sub_.getNumPublishers() > 1) + { + ROS_ERROR_STREAM_THROTTLE_NAMED(1.0, name_, "Detected " << mesurement_sub_.getNumPublishers() + << " publishers. Only 1 publisher is allowed. Going to brake."); + this->brake(); + return; + } + + if (!std::isfinite(msg->data)) + { + ROS_WARN_THROTTLE(1.0, "Received NaN in velocity %s encoder. Ignoring.", name_.c_str()); + return; + } + + m_measurement_ = msg; +} + +void WheelPlugin::DoStuff() +{ + ros::Rate rate(update_rate_); + is_ready_ = true; + ROS_INFO_NAMED("WheelPlugin","...Threading on %s is ready", name_.c_str()); + bool stoped; + while (ros::ok()) + { + WheelCommand curr_cmd = *(command_.readFromRT()); + ros::Time current_time = ros::Time::now(); + double dt = ros::Duration(current_time - command_struct_.stamp).toSec(); + + if(dt <= command_timeout_ && fabs(curr_cmd.vel_wheel) > 0.001) + { + if (wheel_pub_ && wheel_pub_->trylock()) + { + wheel_pub_->msg_.data = curr_cmd.vel_wheel; + wheel_pub_->unlockAndPublish(); + } + stoped = false; + } + else + { + if(!stoped) + { + for(int i = 0; i < 3; i++) + { + if (wheel_pub_ && wheel_pub_->trylock()) + { + wheel_pub_->msg_.data = 0; + wheel_pub_->unlockAndPublish(); + } + ros::Duration(0.1).sleep(); + } + stoped = true; + } + } + + rate.sleep(); + ros::spinOnce(); + } +} + +}; //namespace diff_wheel_plugin + +PLUGINLIB_EXPORT_CLASS(diff_wheel_plugin::WheelPlugin, models::BaseSteerDrive) \ No newline at end of file diff --git a/Devices/Packages/msle_tf_base_link/CMakeLists.txt b/Devices/Packages/msle_tf_base_link/CMakeLists.txt new file mode 100755 index 0000000..807f314 --- /dev/null +++ b/Devices/Packages/msle_tf_base_link/CMakeLists.txt @@ -0,0 +1,209 @@ +cmake_minimum_required(VERSION 3.0.2) +project(msle_tf_base_link) + +## Compile as C++11, supported in ROS Kinetic and newer +# add_compile_options(-std=c++11) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + roscpp + std_msgs + sick_line_guidance + tf2 + tf2_ros +) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a exec_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# std_msgs +# ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES msle_tf_base_link +# CATKIN_DEPENDS roscpp std_msgs +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( +# include + ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +#add_library(${PROJECT_NAME} +# src/mlse_tf_base_link_subscriber.cpp +#) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +## With catkin_make all packages are built within a single CMake context +## The recommended prefix ensures that target names across packages don't collide +add_executable(mlse_base_link_broadcaster src/mlse_tf_base_link_broadcaster.cpp) + +## Rename C++ executable without prefix +## The above recommended prefix causes long target names, the following renames the +## target back to the shorter version for ease of user use +## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" +# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") + +## Add cmake target dependencies of the executable +## same as for the library above +add_dependencies(mlse_base_link_broadcaster ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +target_link_libraries(mlse_base_link_broadcaster + ${catkin_LIBRARIES} +) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# catkin_install_python(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html +install(TARGETS mlse_base_link_broadcaster + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +## Mark libraries for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html +# install(TARGETS ${PROJECT_NAME} +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +install(DIRECTORY + launch + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE +) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_msle_tf_base_link.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/Devices/Packages/msle_tf_base_link/launch/msle_tf_base_link.launch b/Devices/Packages/msle_tf_base_link/launch/msle_tf_base_link.launch new file mode 100755 index 0000000..922d965 --- /dev/null +++ b/Devices/Packages/msle_tf_base_link/launch/msle_tf_base_link.launch @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/Devices/Packages/msle_tf_base_link/package.xml b/Devices/Packages/msle_tf_base_link/package.xml new file mode 100755 index 0000000..4ae638f --- /dev/null +++ b/Devices/Packages/msle_tf_base_link/package.xml @@ -0,0 +1,75 @@ + + + msle_tf_base_link + 0.0.0 + The msle_tf_base_link package + + + + + robotics + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + message_runtime + + + + + catkin + roscpp + std_msgs + sick_line_guidance + tf2 + tf2_ros + + roscpp + std_msgs + sick_line_guidance + tf2 + tf2_ros + + roscpp + std_msgs + sick_line_guidance + tf2 + tf2_ros + + + + + + + diff --git a/Devices/Packages/msle_tf_base_link/src/mlse_tf_base_link_broadcaster.cpp b/Devices/Packages/msle_tf_base_link/src/mlse_tf_base_link_broadcaster.cpp new file mode 100755 index 0000000..368f996 --- /dev/null +++ b/Devices/Packages/msle_tf_base_link/src/mlse_tf_base_link_broadcaster.cpp @@ -0,0 +1,66 @@ +#include +#include +#include +#include +#include +#include + +template static T readMember(XmlRpc::XmlRpcValue & value, const std::string & member, const T & defaultvalue) +{ + if(value.hasMember(member)) + return value[member]; + return defaultvalue; +} + + +std_msgs::UInt8MultiArray measurement_array; + +void msleCallback(const sick_line_guidance::MLS_MeasurementConstPtr& msg) +{ + if(((msg->lcp & 0x02) >> 1) == 0x01) + { + static tf2_ros::TransformBroadcaster br; + geometry_msgs::TransformStamped transformStamped; + int sign = msg->header.frame_id[0] == 'f' ? 1 : -1; + + transformStamped.header.stamp = ros::Time::now(); + transformStamped.header.frame_id = "base_link"; + transformStamped.child_frame_id = msg->header.frame_id; + transformStamped.transform.translation.x = sign * 0.476; + transformStamped.transform.translation.y = msg->position[1]; + transformStamped.transform.translation.z = 0.0; + tf2::Quaternion q; + q.setRPY(0, 0, 0); + transformStamped.transform.rotation.x = q.x(); + transformStamped.transform.rotation.y = q.y(); + transformStamped.transform.rotation.z = q.z(); + transformStamped.transform.rotation.w = q.w(); + br.sendTransform(transformStamped); + } +} + +int main(int argc, char **argv) +{ + /* Khoi tao Node */ + ros::init(argc, argv, "mlse_base_link_broadcaster"); + ros::NodeHandlePtr nh = boost::make_shared(); + std::string node_name = ros::this_node::getName(); + ROS_INFO("%s.cpp-node_name: %s", node_name.c_str(), node_name.c_str()); + XmlRpc::XmlRpcValue nodes; + if(!nh->getParam(node_name + "/nodes", nodes) || nodes.size() < 1) + { + ROS_ERROR("%s: no driver found in yaml-file, please check configuration. Aborting...", node_name.c_str()); + return 1; + } + std::vector mlse_subscribers; + for(XmlRpc::XmlRpcValue::iterator object_iter = nodes.begin(); object_iter != nodes.end(); ++object_iter) + { + std::string topic_name = readMember(object_iter->second, "sick_topic", ""); + int subscribe_queue_size = readMember(object_iter->second, "subscribe_queue_size", 1); + ROS_INFO("%s: initializing toppic \"%s\" queue_size \"%d\"...", node_name.c_str(), topic_name.c_str(), subscribe_queue_size); + ros::Subscriber sub = nh->subscribe(topic_name, subscribe_queue_size, &msleCallback); + mlse_subscribers.push_back(sub); + } + ros::spin(); + return 0; +} diff --git a/Devices/Packages/olei_lidar_driver_ros/olei_msgs/CMakeLists.txt b/Devices/Packages/olei_lidar_driver_ros/olei_msgs/CMakeLists.txt new file mode 100755 index 0000000..d59f10a --- /dev/null +++ b/Devices/Packages/olei_lidar_driver_ros/olei_msgs/CMakeLists.txt @@ -0,0 +1,16 @@ +cmake_minimum_required(VERSION 2.8.3) +project(olei_msgs) + +find_package(catkin REQUIRED COMPONENTS message_generation std_msgs) + +add_message_files( + DIRECTORY msg + FILES + oleiPacket.msg + oleiScan.msg +) +generate_messages(DEPENDENCIES std_msgs) + +catkin_package( + CATKIN_DEPENDS message_runtime std_msgs +) diff --git a/Devices/Packages/olei_lidar_driver_ros/olei_msgs/msg/oleiPacket.msg b/Devices/Packages/olei_lidar_driver_ros/olei_msgs/msg/oleiPacket.msg new file mode 100755 index 0000000..fb2db1e --- /dev/null +++ b/Devices/Packages/olei_lidar_driver_ros/olei_msgs/msg/oleiPacket.msg @@ -0,0 +1,5 @@ +# Raw olei LIDAR packet. + +time stamp # packet timestamp +uint8[1240] data # packet contents + diff --git a/Devices/Packages/olei_lidar_driver_ros/olei_msgs/msg/oleiScan.msg b/Devices/Packages/olei_lidar_driver_ros/olei_msgs/msg/oleiScan.msg new file mode 100755 index 0000000..934a3f0 --- /dev/null +++ b/Devices/Packages/olei_lidar_driver_ros/olei_msgs/msg/oleiScan.msg @@ -0,0 +1,4 @@ +# olei LIDAR scan packets. + +Header header # standard ROS message header +oleiPacket[] packets # vector of raw packets diff --git a/Devices/Packages/olei_lidar_driver_ros/olei_msgs/package.xml b/Devices/Packages/olei_lidar_driver_ros/olei_msgs/package.xml new file mode 100755 index 0000000..92379ea --- /dev/null +++ b/Devices/Packages/olei_lidar_driver_ros/olei_msgs/package.xml @@ -0,0 +1,23 @@ + + + olei_msgs + 1.5.2 + + ROS message definitions for olei 2D LIDARs. + + Emanuele + Emanuele + BSD + + TODO + TODO + TODO + + catkin + + message_generation + + std_msgs + + message_runtime + diff --git a/Devices/Packages/olei_lidar_driver_ros/olelidar/CMakeLists.txt b/Devices/Packages/olei_lidar_driver_ros/olelidar/CMakeLists.txt new file mode 100755 index 0000000..01699e1 --- /dev/null +++ b/Devices/Packages/olei_lidar_driver_ros/olelidar/CMakeLists.txt @@ -0,0 +1,54 @@ +cmake_minimum_required(VERSION 3.5) +project(olelidar) + +set(CMAKE_CXX_STANDARD 11) +set(CMAKE_CXX_FLAGS "-std=c++11 -g ${CMAKE_CXX_FLAGS}") + +find_package(catkin REQUIRED + COMPONENTS + roscpp + diagnostic_updater + dynamic_reconfigure + sensor_msgs + olei_msgs) + +generate_dynamic_reconfigure_options(cfg/oleiPuck.cfg) + +catkin_package() + +#add_executable(${PROJECT_NAME}_driver src/driver.cpp) +add_library(${PROJECT_NAME}_driver src/driver.cpp) +target_include_directories(${PROJECT_NAME}_driver PUBLIC ${catkin_INCLUDE_DIRS} src) +target_link_libraries(${PROJECT_NAME}_driver PUBLIC ${catkin_LIBRARIES}) + +add_executable(${PROJECT_NAME}_decoder src/decoder.cpp) +target_include_directories(${PROJECT_NAME}_decoder PUBLIC ${catkin_INCLUDE_DIRS} src) +target_link_libraries(${PROJECT_NAME}_decoder PUBLIC ${catkin_LIBRARIES} ${PROJECT_NAME}_driver) + +add_dependencies(${PROJECT_NAME}_driver ${PROJECT_NAME}_gencfg) +add_dependencies(${PROJECT_NAME}_decoder ${PROJECT_NAME}_gencfg) + +## Mark executables and/or libraries for installation +install(TARGETS ${PROJECT_NAME}_driver ${PROJECT_NAME}_decoder + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +## Mark cpp header files for installation +install(DIRECTORY include/${PROJECT_NAME}/include + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".git" EXCLUDE +) + +install(FILES + cfg/oleiPuck.cfg + launch/decoder.launch + launch/driver.launch + launch/scan.launch + # launch/olelidar.launch + launch/debug.conf + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch +) + diff --git a/Devices/Packages/olei_lidar_driver_ros/olelidar/cfg/oleiPuck.cfg b/Devices/Packages/olei_lidar_driver_ros/olelidar/cfg/oleiPuck.cfg new file mode 100755 index 0000000..7861cc1 --- /dev/null +++ b/Devices/Packages/olei_lidar_driver_ros/olelidar/cfg/oleiPuck.cfg @@ -0,0 +1,39 @@ +#!/usr/bin/env python +PACKAGE = "olelidar" + +from dynamic_reconfigure.parameter_generator_catkin import * + +gen = ParameterGenerator() + + +# device +gen.add("device_ip", str_t, 0, "IP addr to receive packet from.", "192.168.2.122") +gen.add("device_port", int_t, 0, "UDP port to receive packet from.", 2368) +gen.add("local_ip", str_t, 0, "loacl IP addr.", "192.168.2.198") +gen.add("multiaddr", str_t, 0, "multiaddr.", "239.255.0.100") +gen.add("freq", double_t, 0, "rotate degree per second", 25.0) +gen.add("route", int_t, 0, "rotate degree per second", 2000) +gen.add("step", double_t, 0, "rotate degree per second", 0.225) +# laserscan +''' +std_msgs/Header header + uint32 seq + time stamp + string frame_id +float32 angle_min +float32 angle_max +float32 angle_increment +float32 time_increment +float32 scan_time +float32 range_min +float32 range_max +float32[] ranges +float32[] intensities +''' +# gen.add(name, type, level, description, default, min, max) +gen.add("frame_id", str_t, 0, "olelidar") +gen.add("angle_min", double_t, 0, "angle_min",0,0,360) +gen.add("angle_max", double_t, 360, "angle_max",360,0,360) +gen.add("range_min", double_t, 0.1, "min range", 0.1, 0.1, 20) +gen.add("range_max", double_t, 0.1, "max range", 50.0, 0.1, 50) +exit(gen.generate(PACKAGE, PACKAGE, "oleiPuck")) diff --git a/Devices/Packages/olei_lidar_driver_ros/olelidar/launch/debug.conf b/Devices/Packages/olei_lidar_driver_ros/olelidar/launch/debug.conf new file mode 100755 index 0000000..b54168f --- /dev/null +++ b/Devices/Packages/olei_lidar_driver_ros/olelidar/launch/debug.conf @@ -0,0 +1 @@ +log4j.logger.ros.olelidar=DEBUG diff --git a/Devices/Packages/olei_lidar_driver_ros/olelidar/launch/decoder.launch b/Devices/Packages/olei_lidar_driver_ros/olelidar/launch/decoder.launch new file mode 100755 index 0000000..7f5edad --- /dev/null +++ b/Devices/Packages/olei_lidar_driver_ros/olelidar/launch/decoder.launch @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/Devices/Packages/olei_lidar_driver_ros/olelidar/launch/driver.launch b/Devices/Packages/olei_lidar_driver_ros/olelidar/launch/driver.launch new file mode 100755 index 0000000..3c931a4 --- /dev/null +++ b/Devices/Packages/olei_lidar_driver_ros/olelidar/launch/driver.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/Devices/Packages/olei_lidar_driver_ros/olelidar/launch/scan.launch b/Devices/Packages/olei_lidar_driver_ros/olelidar/launch/scan.launch new file mode 100755 index 0000000..b8d76a4 --- /dev/null +++ b/Devices/Packages/olei_lidar_driver_ros/olelidar/launch/scan.launch @@ -0,0 +1,57 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Devices/Packages/olei_lidar_driver_ros/olelidar/olei_msgs/CMakeLists.txt b/Devices/Packages/olei_lidar_driver_ros/olelidar/olei_msgs/CMakeLists.txt new file mode 100755 index 0000000..d59f10a --- /dev/null +++ b/Devices/Packages/olei_lidar_driver_ros/olelidar/olei_msgs/CMakeLists.txt @@ -0,0 +1,16 @@ +cmake_minimum_required(VERSION 2.8.3) +project(olei_msgs) + +find_package(catkin REQUIRED COMPONENTS message_generation std_msgs) + +add_message_files( + DIRECTORY msg + FILES + oleiPacket.msg + oleiScan.msg +) +generate_messages(DEPENDENCIES std_msgs) + +catkin_package( + CATKIN_DEPENDS message_runtime std_msgs +) diff --git a/Devices/Packages/olei_lidar_driver_ros/olelidar/olei_msgs/msg/oleiPacket.msg b/Devices/Packages/olei_lidar_driver_ros/olelidar/olei_msgs/msg/oleiPacket.msg new file mode 100755 index 0000000..fb2db1e --- /dev/null +++ b/Devices/Packages/olei_lidar_driver_ros/olelidar/olei_msgs/msg/oleiPacket.msg @@ -0,0 +1,5 @@ +# Raw olei LIDAR packet. + +time stamp # packet timestamp +uint8[1240] data # packet contents + diff --git a/Devices/Packages/olei_lidar_driver_ros/olelidar/olei_msgs/msg/oleiScan.msg b/Devices/Packages/olei_lidar_driver_ros/olelidar/olei_msgs/msg/oleiScan.msg new file mode 100755 index 0000000..934a3f0 --- /dev/null +++ b/Devices/Packages/olei_lidar_driver_ros/olelidar/olei_msgs/msg/oleiScan.msg @@ -0,0 +1,4 @@ +# olei LIDAR scan packets. + +Header header # standard ROS message header +oleiPacket[] packets # vector of raw packets diff --git a/Devices/Packages/olei_lidar_driver_ros/olelidar/olei_msgs/package.xml b/Devices/Packages/olei_lidar_driver_ros/olelidar/olei_msgs/package.xml new file mode 100755 index 0000000..92379ea --- /dev/null +++ b/Devices/Packages/olei_lidar_driver_ros/olelidar/olei_msgs/package.xml @@ -0,0 +1,23 @@ + + + olei_msgs + 1.5.2 + + ROS message definitions for olei 2D LIDARs. + + Emanuele + Emanuele + BSD + + TODO + TODO + TODO + + catkin + + message_generation + + std_msgs + + message_runtime + diff --git a/Devices/Packages/olei_lidar_driver_ros/olelidar/package.xml b/Devices/Packages/olei_lidar_driver_ros/olelidar/package.xml new file mode 100755 index 0000000..118f559 --- /dev/null +++ b/Devices/Packages/olei_lidar_driver_ros/olelidar/package.xml @@ -0,0 +1,26 @@ + + + olelidar + 3.0.0 + + Basic ROS support for the olei 2D LIDARs. + + Emanuele + Emanuele + GNU General Public License V3.0 + + catkin + + sensor_msgs + olei_msgs + + roscpp + diagnostic_updater + dynamic_reconfigure + + + + + + + diff --git a/Devices/Packages/olei_lidar_driver_ros/olelidar/src/constants.h b/Devices/Packages/olei_lidar_driver_ros/olelidar/src/constants.h new file mode 100755 index 0000000..114684f --- /dev/null +++ b/Devices/Packages/olei_lidar_driver_ros/olelidar/src/constants.h @@ -0,0 +1,75 @@ +#pragma once + +#include +#include +#include + +namespace olelidar { + +static constexpr auto kNaNF = std::numeric_limits::quiet_NaN(); +static constexpr auto kNaND = std::numeric_limits::quiet_NaN(); +static constexpr float kTau = M_PI * 2; +static constexpr float deg2rad(float deg) { return deg * M_PI / 180.0; } +static constexpr float rad2deg(float rad) { return rad * 180.0 / M_PI; } + +// Raw olei packet constants and structures. +// 1 packet = 150 blocks +// 1 block = 1 sequence +// 1 sequence = 1 firing = 1 point +// 1 point = 8 bytes +static constexpr int kPointBytes = 8; +static constexpr int kPointsPerBlock = 1; + +// According to Bruce Hall DISTANCE_MAX is 65.0, but we noticed +// valid packets with readings up to 130.0. +static constexpr float kDistanceMax = 20.0; // [m] +static constexpr float kDistanceResolution = 0.001; // [m] 2d lidar, + +/** @todo make this work for both big and little-endian machines */ +static const uint16_t UPPER_BANK = 0xeeff; //rsv +static const uint16_t LOWER_BANK = 0xddff; //rsv + +/** Special Defines for olei support **/ +static constexpr double kSingleFiringNs = 2304; // [ns] rsv,1s/10hz/1600points = 62500 +static constexpr double kFiringCycleNs = 55296; // [ns] rsv, = 62500 +static constexpr double kSingleFiringRatio = kSingleFiringNs / kFiringCycleNs; // 1 + +// The information from two firing sequences of 16 lasers is contained in each +// data block. Each packet contains the data from 24 firing sequences in 12 data +// blocks. +static constexpr int kFiringsPerSequence = 1; +static constexpr int kSequencesPerBlock = 1; +static constexpr int kBlocksPerPacket = 150; +static constexpr int kSequencesPerPacket = + kSequencesPerBlock * kBlocksPerPacket; // 150 + +inline int LaserId2Row(int id) { + const auto index = (id % 2 == 0) ? id / 2 : id / 2 + kFiringsPerSequence / 2; + return kFiringsPerSequence - index - 1; +} + +static constexpr uint16_t kMaxRawAzimuth = 35999; +static constexpr float kAzimuthResolution = 0.01f; + +// static constexpr float kMinElevation = deg2rad(-15.0f); +// static constexpr float kMaxElevation = deg2rad(15.0f); +// static constexpr float kDeltaElevation = +// (kMaxElevation - kMinElevation) / (kFiringsPerSequence - 1); + +inline constexpr float Raw2Azimuth(uint16_t raw) { + // According to the user manual, + return deg2rad(static_cast(raw) * kAzimuthResolution); +} + +/// p55 9.3.1.2 +inline constexpr float Raw2Distance(uint16_t raw) { + return static_cast(raw) * kDistanceResolution; +} + +/// p51 8.3.1 +inline constexpr float AzimuthResolutionDegree(int rpm) { + // rpm % 60 == 0 + return rpm / 60.0 * 360.0 * kFiringCycleNs / 1e9; +} + +} // namespace olelidar diff --git a/Devices/Packages/olei_lidar_driver_ros/olelidar/src/decoder.cpp b/Devices/Packages/olei_lidar_driver_ros/olelidar/src/decoder.cpp new file mode 100755 index 0000000..a6c37da --- /dev/null +++ b/Devices/Packages/olei_lidar_driver_ros/olelidar/src/decoder.cpp @@ -0,0 +1,446 @@ +#include "constants.h" + +#include + +#include + +#include +#include +#include + +#include "driver.cpp" +// here maskoff waring of macro 'ROS_LOG' +#pragma clang diagnostic ignored "-Wzero-as-null-pointer-constant" + +namespace olelidar +{ + + using namespace sensor_msgs; + using namespace olei_msgs; + + class Decoder + { + public: + explicit Decoder(const ros::NodeHandle &pnh); + + Decoder(const Decoder &) = delete; + Decoder operator=(const Decoder &) = delete; + + void PacketCb(const oleiPacketConstPtr &packet_msg); + void ConfigCb(oleiPuckConfig &config, int level); + void PublishMsg(ros::Publisher *pub, std::vector &packet_r, std::vector &packet_i, ros::Time t); + + private: + /// 9.3.1.3 Data Point + /// A data point is a measurement by one laser channel of a relection of a + /// laser pulse + struct DataPoint + { + uint16_t azimuth; //unit: 0.01 degree + uint16_t distance; //unit: 1mm + uint16_t reflectivity; //unit: 1 percent,100 is the reflet of white target + uint16_t distance2; //rsv for dual return + } __attribute__((packed)); + static_assert(sizeof(DataPoint) == 8, "sizeof(DataPoint) != 8"); + + struct FiringSequence + { + DataPoint points[kFiringsPerSequence]; // 8 + } __attribute__((packed)); + static_assert(sizeof(FiringSequence) == 8, "sizeof(FiringSequence) != 8"); + + struct DataHead + { + uint8_t magic[4]; + uint8_t version[2]; + uint8_t scale; + uint8_t oem[3]; + uint8_t model[12]; + uint8_t code[2]; + uint8_t hw[2]; + uint8_t sw[2]; + uint32_t timestamp; + uint16_t rpm; + uint8_t flag[2]; + uint32_t rsv; + } __attribute__((packed)); + static_assert(sizeof(DataHead) == 40, "sizeof(DataBlock) != 40"); + + struct DataBlock + { + FiringSequence sequences[kSequencesPerBlock]; // 8 * 1 + } __attribute__((packed)); + static_assert(sizeof(DataBlock) == 8, "sizeof(DataBlock) != 8"); + + /// 9.3.1.5 Packet format for 2d + struct Packet + { + DataHead head; + DataBlock blocks[kBlocksPerPacket]; // 8 * 150 + + } __attribute__((packed)); + static_assert(sizeof(Packet) == 1240, "sizeof(DataBlock) != 1240"); + static_assert(sizeof(Packet) == sizeof(olei_msgs::oleiPacket().data), "sizeof(Packet) != 1240"); + + void DecodeAndFill(const Packet *const packet_buf, uint64_t); + + private: + bool CheckFactoryBytes(const Packet *const packet); + + // ROS related parameters + uint32_t laststamp; + uint32_t scantime; + ros::Time start; + bool is_time_base_{false}; + ros::Time local_timestamp_base_; + uint32_t inner_timestamp_base_; + + std::string frame_id_; + int range_min_=200; + int range_max_; + int ange_start_; + int ange_end_; + bool inverted_; + int poly_=1; + ros::NodeHandle pnh_; + // sub driver topic(msg) + ros::Subscriber packet_sub_; + // pub laserscan message + ros::Publisher scan_pub_; + + // dynamic param server + dynamic_reconfigure::Server cfg_server_; + oleiPuckConfig config_; + + // add vector for laserscan + std::vector scanAngleVec_; + std::vector scanRangeVec_; + std::vector scanIntensityVec_; + + std::vector scanAngleInVec_; + std::vector scanRangeInVec_; + std::vector scanIntensityInVec_; + + std::vector scanRangeBuffer; + std::vector scanintensitiesBuffer; + uint16_t azimuthLast_; + uint16_t azimuthNow_; + uint16_t azimuthFirst_; + + // laserscan msg + uint32_t scanMsgSeq_; + //电机频率 + float frequency; + //雷达型号类型 + unsigned char lidarType=0x01; + //电机方向定义 + int direction; + + ros::Time log_time_ = ros::Time::now(); + + std::shared_ptr drv_; + }; + + Decoder::Decoder(const ros::NodeHandle &pnh) + : pnh_(pnh), cfg_server_(pnh) + { + // get param from cfg file at node start + pnh_.param("frame_id", frame_id_, "olelidar"); + pnh_.param("r_max", range_max_, 30); + ROS_INFO("==========================="); + ROS_INFO("Frame_id: %s", frame_id_.c_str()); + ROS_INFO("Topic: /%s/scan", frame_id_.c_str()); + ROS_INFO("Range: [%d ~ %d] mm",range_min_, range_max_*1000); + ROS_INFO("==========================="); + start = ros::Time::now(); + // dynamic callback + cfg_server_.setCallback(boost::bind(&Decoder::ConfigCb, this, _1, _2)); + // packet receive + azimuthLast_ = 0; + azimuthNow_ = 0; + azimuthFirst_ = 0xFFFF; + // laserscan msg init + scanMsgSeq_ = 0; + direction = 0; + + drv_ = std::make_shared(pnh); + + scan_pub_ = pnh_.advertise("scan", 100); +#ifdef DRIVER_MODULE + packet_sub_ = pnh_.subscribe("packet", 100, &Decoder::PacketCb, this, ros::TransportHints().tcpNoDelay(true)); +#endif + drv_->setCallback(std::bind(&Decoder::PacketCb, this, std::placeholders::_1)); + + ROS_INFO("Drive Ver:2.0.11"); + ROS_INFO("Decoder initialized"); + + } + + void Decoder::PublishMsg(ros::Publisher *pub, std::vector &packet_r, std::vector &packet_i, ros::Time lidar_time) + { + sensor_msgs::LaserScan scanMsg; + /* + std_msgs/Header header + uint32 seq + time stamp + string frame_id + float32 angle_min + float32 angle_max + float32 angle_increment + float32 time_increment + float32 scan_time + float32 range_min + float32 range_max + float32[] ranges + float32[] intensities + */ + + int min = config_.angle_min * 100 + 18000; + int max = config_.angle_max * 100 + 18000; + + scanRangeBuffer.clear(); + scanintensitiesBuffer.clear(); + + for (uint16_t i = 0; i < scanAngleInVec_.size(); i++) + { + // 过滤出指定角度范围内点云 + int angle = scanAngleInVec_[i]; + if (angle >= min && angle <= max && i % poly_ == 0) + { + float range = scanRangeInVec_[i] * 0.001f; + float intensities = scanIntensityInVec_[i] * 1.0f; + + scanRangeBuffer.push_back(range); + scanintensitiesBuffer.push_back(intensities); + } + } + + float bufferlen = scanRangeBuffer.size(); + scanMsg.ranges.resize(bufferlen); + scanMsg.intensities.resize(bufferlen); + + if(direction ==0) //电机旋转顺时针时，转换右手坐标系法则 + { + reverse(scanRangeBuffer.begin(),scanRangeBuffer.end()); + reverse(scanintensitiesBuffer.begin(),scanintensitiesBuffer.end()); + } + + for (uint16_t i = 0; i < bufferlen; i++) + { + scanMsg.ranges[i] = scanRangeBuffer[i]; + scanMsg.intensities[i] = scanintensitiesBuffer[i]; + } + + float len = scanMsg.ranges.size(); + //扫描顺序自增ID序列 + scanMsg.header.seq = scanMsgSeq_; + scanMsgSeq_++; + //激光数据时间戳 + //scanMsg.header.stamp = lidar_time; + scanMsg.header.stamp = ros::Time::now(); + //FrameId + scanMsg.header.frame_id = frame_id_.c_str(); + //定义开始角度和结束角度 + scanMsg.angle_min = deg2rad(config_.angle_min); + scanMsg.angle_max = deg2rad(config_.angle_max); + //定义测距的最小值和最大值单位:m + scanMsg.range_min = config_.range_min; + scanMsg.range_max = config_.range_max; + + float step=(config_.angle_max - config_.angle_min)/len; + //ROS_INFO("len:%f step:%f",len,step); + //角度分辨率 + scanMsg.angle_increment = deg2rad(step); + //扫描的时间间隔 + scanMsg.scan_time = 1/frequency; + //时间分辨率（相邻两个角度之间耗费时间） + scanMsg.time_increment = scanMsg.scan_time/float(len); + //修正为雷达内部时钟间隔 + //scanMsg.scan_time = scantime*0.001f; //雷达时间戳为毫秒计数，故而转为秒单位应乘以0.001 + //scanMsg.time_increment = scanMsg.scan_time/float(len); + + //ROS_INFO("scan_time:%f time_increment:%f",scanMsg.scan_time,scanMsg.time_increment); + + + uint16_t size=scanAngleInVec_.size(); + uint16_t fb=360/(config_.step); + if (fb==size){ + pub->publish(scanMsg); //校验当符合点数完整的一帧数据才向外发布话题 + } + else{ + if(scanMsgSeq_==1) return; + ROS_INFO("pointCloud frame:%d size:%d scanMsgSeq_:%d",fb,size,scanMsgSeq_); + } + } + + void Decoder::DecodeAndFill(const Packet *const packet_buf, uint64_t time) + { + + // For each data block, 150 total + uint16_t azimuth; + uint16_t range; + uint16_t intensity; + for (int iblk = 0; iblk < kBlocksPerPacket; ++iblk) + { + const auto &block = packet_buf->blocks[iblk]; + + // simple loop + azimuth = block.sequences[0].points[0].azimuth; + range = block.sequences[0].points[0].distance; + intensity = block.sequences[0].points[0].reflectivity; + //排除异常点云数据 + if (range > range_max_*1000 || range < range_min_) + { + range = 0; + intensity = 0; + } + // azimuth ge 36000 is not valid + if (azimuth < 0xFF00) + { + scanAngleVec_.push_back(azimuth); + scanRangeVec_.push_back(range); + scanIntensityVec_.push_back(intensity); + } + } + } + + void Decoder::PacketCb(const oleiPacketConstPtr &packet_msg) + { + ros::Time now = ros::Time::now(); + ros::Duration delta_t = now - log_time_; + log_time_ = now; + + const auto *packet_buf = reinterpret_cast(&(packet_msg->data[0])); + azimuthNow_ = packet_buf->blocks[0].sequences[0].points[0].azimuth; + //取得第一个数据包 + if (azimuthFirst_ == 0xFFFF) + { + //雷达型号类型 + lidarType = packet_buf->head.code[1]; + azimuthFirst_ = azimuthNow_; + //取得转速 + int rpm = (packet_buf->head.rpm) & 0x7FFF; + //取得电机旋转方向 + direction = (packet_buf->head.rpm) >> 15; + ROS_INFO("rpm:%d direction:%d lidarType:%d", rpm, direction,lidarType); + //是否启用倒装设定 + if (inverted_) direction = !direction; + } + + if (azimuthLast_ < azimuthNow_) + { + DecodeAndFill(packet_buf, packet_msg->stamp.toNSec()); + azimuthLast_ = azimuthNow_; + return; + } + else + { + azimuthLast_ = azimuthNow_; + } + // scan first half route + if (azimuthFirst_ >= 200) + { + azimuthFirst_ = azimuthNow_; + return; + } + //雷达时间戳 + uint32_t nowstamp = packet_buf->head.timestamp; + ros::Time lidar_time = packet_msg->stamp; + +#ifdef TIMESTAMP_DEBUG + if(!is_time_base_) { + local_timestamp_base_ = ros::Time::now(); + inner_timestamp_base_ = nowstamp; + lidar_time = local_timestamp_base_; + is_time_base_ = true; + } else { + uint32_t delta_time = nowstamp - inner_timestamp_base_; + ros::Duration dur_time; + ros::Duration delta_t = dur_time.fromNSec(delta_time*1000000); + lidar_time = local_timestamp_base_ + delta_t; + //ROS_INFO_STREAM("ros timestamp:" << delta_time << "," << delta_t << "," << lidar_time); + } +#endif + scantime = nowstamp - laststamp; + laststamp = nowstamp; + //ROS_INFO("inner timestamp: %u, %d", nowstamp, scantime); + + //雷达工作频率 + if(frequency<0.001){ + lidarType = packet_buf->head.code[1]; //雷达型号类型 + if(lidarType==0x01){ + int rpm = (packet_buf->head.rpm) & 0x7FFF; + frequency = rpm / 60.0; + config_.step=0.225; //当雷达型号为0x01类型时，角分辨率为固定值 + } + else{ + if(scanAngleVec_.size()>2) { + //角度分辨率 + config_.step = (scanAngleVec_[1]-scanAngleVec_[0])/100.0; + frequency = config_.step * 10000.0/60.0; + } + else{ + return; + } + } + ROS_INFO("frequency: %.0f hz config_.step:%f",frequency,config_.step); + } + + //当启用NTP服务时，时间戳重定向为NTP服务时间 + if(packet_buf->head.rsv>0){ + ros::Time ntp(packet_buf->head.rsv, packet_buf->head.timestamp); + lidar_time=ntp; + } + scanAngleInVec_.clear(); + scanRangeInVec_.clear(); + scanIntensityInVec_.clear(); + + scanAngleInVec_.assign(scanAngleVec_.begin(), scanAngleVec_.end()); + scanRangeInVec_.assign(scanRangeVec_.begin(), scanRangeVec_.end()); + scanIntensityInVec_.assign(scanIntensityVec_.begin(), scanIntensityVec_.end()); + + scanAngleVec_.clear(); + scanRangeVec_.clear(); + scanIntensityVec_.clear(); + + //抛出点云数据 + PublishMsg(&scan_pub_, scanRangeInVec_, scanIntensityInVec_, lidar_time); + + //解码原始数据包 + DecodeAndFill(packet_buf, packet_msg->stamp.toNSec()); + + //ROS_INFO("Time: %f", (ros::Time::now() - start).toSec()); + } + + void Decoder::ConfigCb(oleiPuckConfig &config, int level) + { + // config.min_range = std::min(config.min_range, config.max_range); + //config.route =4000; + pnh_.param("ang_start", ange_start_, 0); + pnh_.param("ang_end", ange_end_, 360); + pnh_.param("r_max", range_max_, 50); + pnh_.param("inverted", inverted_, false); + config.angle_min = ange_start_; + config.angle_max = ange_end_; + config.range_min = range_min_*0.001; + config.range_max = range_max_; + config_ = config; + + + if (level < 0){ + // topic = scan, msg = LaserScan, queuesize = 1000 + scan_pub_ = pnh_.advertise("scan", 100); +#ifdef DRIVER_MODULE + packet_sub_ = pnh_.subscribe("packet", 100, &Decoder::PacketCb, this); +#endif + } + } +} // namespace olelidar + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "olelidar_decoder"); + ros::NodeHandle pnh("~"); + olelidar::Decoder node(pnh); + ros::spin(); +} diff --git a/Devices/Packages/olei_lidar_driver_ros/olelidar/src/driver.cpp b/Devices/Packages/olei_lidar_driver_ros/olelidar/src/driver.cpp new file mode 100755 index 0000000..5096ca4 --- /dev/null +++ b/Devices/Packages/olei_lidar_driver_ros/olelidar/src/driver.cpp @@ -0,0 +1,285 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "constants.h" + +#include +#include +#include +#include + +// here maskoff waring of macro 'ROS_LOG' +#pragma clang diagnostic ignored "-Wzero-as-null-pointer-constant" + +namespace olelidar +{ + + using namespace olei_msgs; + using namespace diagnostic_updater; + + /// Constants + //static constexpr uint16_t kUdpPort = 2368; + static constexpr size_t kPacketSize = sizeof(oleiPacket().data); + static constexpr int kError = -1; + + class Driver + { + using data_cb_t = std::function; + public: + explicit Driver(const ros::NodeHandle &pnh); + ~Driver(); + + bool Poll(); + + void setCallback(const data_cb_t &cb) { data_cb_ = cb; } + + private: + bool OpenUdpPort(); + int ReadPacket(oleiPacket &packet); + + // Ethernet relate variables + std::string device_ip_str_; + std::string local_ip_str_; + std::string multiaddr_ip_str_; + int device_port_; + in_addr device_ip_; + int socket_id_{-1}; + ros::Time last_time_; + + // ROS related variables + ros::NodeHandle pnh_; + ros::Publisher pub_packet_; + + // Diagnostics updater + diagnostic_updater::Updater updater_; + boost::shared_ptr topic_diag_; + std::vector buffer_; + // double freq_; + + // raw data callback + data_cb_t data_cb_{nullptr}; + std::thread data_thr_; + bool is_loop_{false}; + }; + + Driver::Driver(const ros::NodeHandle &pnh) + : pnh_(pnh) + , last_time_(ros::Time::now()) + { + ROS_INFO("packet size: %zu", kPacketSize); + pnh_.param("device_ip", device_ip_str_, std::string("192.168.1.122")); + pnh_.param("device_port", device_port_, 2369); + pnh_.param("local_ip", local_ip_str_, std::string("192.168.1.99")); + pnh_.param("multiaddr", multiaddr_ip_str_, std::string("")); + ROS_INFO("device_ip: %s:%d", device_ip_str_.c_str(), device_port_); + + if (inet_aton(device_ip_str_.c_str(), &device_ip_) == 0) + { + // inet_aton() returns nonzero if the address is valid, zero if not. + ROS_FATAL("Invalid device ip: %s:%d", device_ip_str_.c_str(), device_port_); + ros::shutdown(); + } + + // Output + pub_packet_ = pnh_.advertise("packet", 10); + + if (!OpenUdpPort()) ROS_ERROR("Failed to open UDP Port"); + + data_thr_ = std::move(std::thread( [&] { + is_loop_ = true; + while (is_loop_) { + if(!is_loop_) break; + Poll(); + } + })); + ROS_INFO("Successfully init olelidar driver"); + } + + Driver::~Driver() + { + is_loop_ = false; + if(data_thr_.joinable()) { + data_thr_.join(); + } + if (close(socket_id_)) { + ROS_INFO("Close socket %d at %s", socket_id_, device_ip_str_.c_str()); + } + else { + ROS_ERROR("Failed to close socket %d at %s", socket_id_,device_ip_str_.c_str()); + } + } + + bool Driver::OpenUdpPort() + { + socket_id_ = socket(AF_INET, SOCK_DGRAM, 0); + if (socket_id_ == -1) { + perror("socket"); + ROS_ERROR("Failed to create socket"); + return false; + } + + sockaddr_in my_addr; // my address information + memset(&my_addr, 0, sizeof(my_addr)); // initialize to zeros + my_addr.sin_family = AF_INET; // host byte order + my_addr.sin_port = htons(uint16_t(device_port_)); // short, in network byte order + my_addr.sin_addr.s_addr = INADDR_ANY; // automatically fill in my IP + + + if (bind(socket_id_, (sockaddr *)&my_addr, sizeof(sockaddr)) == -1) + { + perror("bind"); // TODO: ROS_ERROR errno + ROS_ERROR("Failed to bind to socket %d", socket_id_); + return false; + } + if(multiaddr_ip_str_!="") + { + //加入组播才能接受到组播信息 + struct ip_mreq mreq; + mreq.imr_multiaddr.s_addr = inet_addr(multiaddr_ip_str_.c_str()); + mreq.imr_interface.s_addr = inet_addr(local_ip_str_.c_str()); + int err=setsockopt(socket_id_,IPPROTO_IP,IP_ADD_MEMBERSHIP,&mreq,sizeof(mreq)); + ROS_INFO("AddMultiaddr:%s local:%s =>%d ",multiaddr_ip_str_.c_str(),local_ip_str_.c_str(),err); + } + if (fcntl(socket_id_, F_SETFL, O_NONBLOCK | FASYNC) < 0) + { + perror("non-block"); + ROS_ERROR("Failed to set socket to non-blocking"); + return false; + } + return true; + } + + + + int Driver::ReadPacket(oleiPacket &packet) + { + ros::Time time_before = ros::Time::now(); + + struct pollfd fds[1]; + fds[0].fd = socket_id_; + fds[0].events = POLLIN; + const int timeout_ms = 500; // one second (in msec) + + sockaddr_in sender_address; + socklen_t sender_address_len = sizeof(sender_address); + + while (true) + { + do + { + const int retval = poll(fds, 1, timeout_ms); + if (retval < 0) { + if (errno != EINTR) ROS_ERROR("poll() error: %s", strerror(errno)); + return kError; + } + else if (retval == 0) + { + ROS_WARN("olamlidar poll() timeout"); + return kError; + } + + if ((fds[0].revents & POLLERR) || (fds[0].revents & POLLHUP) || (fds[0].revents & POLLNVAL)) + { + ROS_ERROR("poll() reports olamlidar error"); + return kError; + } + } while ((fds[0].revents & POLLIN) == 0); + + // Receive packets that should now be available from the + // socket using a blocking read. + const ssize_t nbytes = recvfrom(socket_id_, &packet.data[0], kPacketSize, 0, (sockaddr *)&sender_address, &sender_address_len); + + if (nbytes < 0) + { + if (errno != EWOULDBLOCK) + { + perror("recvfail"); + ROS_ERROR("Failed to read from socket"); + return kError; + } + } + else if (static_cast(nbytes) == kPacketSize) + { + //else if ((size_t)nbytes == kPacketSize) { + // read successful, + // if packet is not from the lidar scanner we selected by IP, + // continue otherwise we are done + //ROS_ERROR("Failed to read from socket"); + + //ROS_INFO("sender:%d device:%d",sender_address.sin_addr.s_addr,device_ip_.s_addr); + if (device_ip_str_ != "" && sender_address.sin_addr.s_addr != device_ip_.s_addr) + continue; + else + break; // done + } + + ROS_DEBUG_STREAM("incomplete olei packet read: " << nbytes << " bytes"); + } + + packet.stamp = time_before; + +#ifdef TIMESTAMP_DEBUG + ros::Duration delta = time_before - last_time_; + ROS_INFO_STREAM("raw data delta time: " << time_before << "," << delta.toSec()*1000); + last_time_ = time_before; +#endif + + return 0; + } + + bool Driver::Poll() + { + oleiPacket::Ptr packet(new oleiPacket); + + while (true) + { + // keep reading until full packet received + const int rc = ReadPacket(*packet); + if (rc == 0) + break; // got a full packet? + if (rc < 0) + return false; // end of file reached? + } + + // publish message using time of last packet read +#ifdef DRIVER_MODULE + pub_packet_.publish(packet); +#else + data_cb_(packet); +#endif + + return true; + } + +} // namespace olelidar + +#ifdef DRIVER_MODULE +int main(int argc, char **argv) +{ + ros::init(argc, argv, "olelidar_driver"); + ros::NodeHandle pnh("~"); + + olelidar::Driver node(pnh); + + while (ros::ok()) + { + // poll device until end of file + if (!node.Poll()) + { + ROS_WARN("Failed to poll device"); + //break; + } + ros::spinOnce(); + } + return 0; +} +#endif diff --git a/Devices/Packages/ros_kinematics/.gitignore b/Devices/Packages/ros_kinematics/.gitignore new file mode 100755 index 0000000..d5dca0c --- /dev/null +++ b/Devices/Packages/ros_kinematics/.gitignore @@ -0,0 +1,414 @@ +# ---> VisualStudio +## Ignore Visual Studio temporary files, build results, and +## files generated by popular Visual Studio add-ons. +## +## Get latest from https://github.com/github/gitignore/blob/main/VisualStudio.gitignore + +# User-specific files +*.rsuser +*.suo +*.user +*.userosscache +*.sln.docstates + +# User-specific files (MonoDevelop/Xamarin Studio) +*.userprefs + +# Mono auto generated files +mono_crash.* + +# Build results +[Dd]ebug/ +[Dd]ebugPublic/ +[Rr]elease/ +[Rr]eleases/ +x64/ +x86/ +[Ww][Ii][Nn]32/ +[Aa][Rr][Mm]/ +[Aa][Rr][Mm]64/ +bld/ +[Bb]in/ +[Oo]bj/ +[Ll]og/ +[Ll]ogs/ + +# Visual Studio 2015/2017 cache/options directory +.vs/ +# Uncomment if you have tasks that create the project's static files in wwwroot +#wwwroot/ + +# Visual Studio 2017 auto generated files +Generated\ Files/ + +# MSTest test Results +[Tt]est[Rr]esult*/ +[Bb]uild[Ll]og.* + +# NUnit +*.VisualState.xml +TestResult.xml +nunit-*.xml + +# Build Results of an ATL Project +[Dd]ebugPS/ +[Rr]eleasePS/ +dlldata.c + +# Benchmark Results +BenchmarkDotNet.Artifacts/ + +# .NET Core +project.lock.json +project.fragment.lock.json +artifacts/ + +# ASP.NET Scaffolding +ScaffoldingReadMe.txt + +# StyleCop +StyleCopReport.xml + +# Files built by Visual Studio +*_i.c +*_p.c +*_h.h +*.ilk +*.meta +*.obj +*.iobj +*.pch +*.pdb +*.ipdb +*.pgc +*.pgd +*.rsp +*.sbr +*.tlb +*.tli +*.tlh +*.tmp +*.tmp_proj +*_wpftmp.csproj +*.log +*.tlog +*.vspscc +*.vssscc +.builds +*.pidb +*.svclog +*.scc + +# Chutzpah Test files +_Chutzpah* + +# Visual C++ cache files +ipch/ +*.aps +*.ncb +*.opendb +*.opensdf +*.sdf +*.cachefile +*.VC.db +*.VC.VC.opendb + +# Visual Studio profiler +*.psess +*.vsp +*.vspx +*.sap + +# Visual Studio Trace Files +*.e2e + +# TFS 2012 Local Workspace +$tf/ + +# Guidance Automation Toolkit +*.gpState + +# ReSharper is a .NET coding add-in +_ReSharper*/ +*.[Rr]e[Ss]harper +*.DotSettings.user + +# TeamCity is a build add-in +_TeamCity* + +# DotCover is a Code Coverage Tool +*.dotCover + +# AxoCover is a Code Coverage Tool +.axoCover/* +!.axoCover/settings.json + +# Coverlet is a free, cross platform Code Coverage Tool +coverage*.json +coverage*.xml +coverage*.info + +# Visual Studio code coverage results +*.coverage +*.coveragexml + +# NCrunch +_NCrunch_* +.*crunch*.local.xml +nCrunchTemp_* + +# MightyMoose +*.mm.* +AutoTest.Net/ + +# Web workbench (sass) +.sass-cache/ + +# Installshield output folder +[Ee]xpress/ + +# DocProject is a documentation generator add-in +DocProject/buildhelp/ +DocProject/Help/*.HxT +DocProject/Help/*.HxC +DocProject/Help/*.hhc +DocProject/Help/*.hhk +DocProject/Help/*.hhp +DocProject/Help/Html2 +DocProject/Help/html + +# Click-Once directory +publish/ + +# Publish Web Output +*.[Pp]ublish.xml +*.azurePubxml +# Note: Comment the next line if you want to checkin your web deploy settings, +# but database connection strings (with potential passwords) will be unencrypted +*.pubxml +*.publishproj + +# Microsoft Azure Web App publish settings. Comment the next line if you want to +# checkin your Azure Web App publish settings, but sensitive information contained +# in these scripts will be unencrypted +PublishScripts/ + +# NuGet Packages +*.nupkg +# NuGet Symbol Packages +*.snupkg +# The packages folder can be ignored because of Package Restore +**/[Pp]ackages/* +# except build/, which is used as an MSBuild target. +!**/[Pp]ackages/build/ +# Uncomment if necessary however generally it will be regenerated when needed +#!**/[Pp]ackages/repositories.config +# NuGet v3's project.json files produces more ignorable files +*.nuget.props +*.nuget.targets + +# Microsoft Azure Build Output +csx/ +*.build.csdef + +# Microsoft Azure Emulator +ecf/ +rcf/ + +# Windows Store app package directories and files +AppPackages/ +BundleArtifacts/ +Package.StoreAssociation.xml +_pkginfo.txt +*.appx +*.appxbundle +*.appxupload + +# Visual Studio cache files +# files ending in .cache can be ignored +*.[Cc]ache +# but keep track of directories ending in .cache +!?*.[Cc]ache/ + +# Others +ClientBin/ +~$* +*~ +*.dbmdl +*.dbproj.schemaview +*.jfm +*.pfx +*.publishsettings +orleans.codegen.cs + +# Including strong name files can present a security risk +# (https://github.com/github/gitignore/pull/2483#issue-259490424) +#*.snk + +# Since there are multiple workflows, uncomment next line to ignore bower_components +# (https://github.com/github/gitignore/pull/1529#issuecomment-104372622) +#bower_components/ + +# RIA/Silverlight projects +Generated_Code/ + +# Backup & report files from converting an old project file +# to a newer Visual Studio version. Backup files are not needed, +# because we have git ;-) +_UpgradeReport_Files/ +Backup*/ +UpgradeLog*.XML +UpgradeLog*.htm +ServiceFabricBackup/ +*.rptproj.bak + +# SQL Server files +*.mdf +*.ldf +*.ndf + +# Business Intelligence projects +*.rdl.data +*.bim.layout +*.bim_*.settings +*.rptproj.rsuser +*- [Bb]ackup.rdl +*- [Bb]ackup ([0-9]).rdl +*- [Bb]ackup ([0-9][0-9]).rdl + +# Microsoft Fakes +FakesAssemblies/ + +# GhostDoc plugin setting file +*.GhostDoc.xml + +# Node.js Tools for Visual Studio +.ntvs_analysis.dat +node_modules/ + +# Visual Studio 6 build log +*.plg + +# Visual Studio 6 workspace options file +*.opt + +# Visual Studio 6 auto-generated workspace file (contains which files were open etc.) +*.vbw + +# Visual Studio 6 auto-generated project file (contains which files were open etc.) +*.vbp + +# Visual Studio 6 workspace and project file (working project files containing files to include in project) +*.dsw +*.dsp + +# Visual Studio 6 technical files +*.ncb +*.aps + +# Visual Studio LightSwitch build output +**/*.HTMLClient/GeneratedArtifacts +**/*.DesktopClient/GeneratedArtifacts +**/*.DesktopClient/ModelManifest.xml +**/*.Server/GeneratedArtifacts +**/*.Server/ModelManifest.xml +_Pvt_Extensions + +# Paket dependency manager +.paket/paket.exe +paket-files/ + +# FAKE - F# Make +.fake/ + +# CodeRush personal settings +.cr/personal + +# Python Tools for Visual Studio (PTVS) +__pycache__/ +*.pyc + +# Cake - Uncomment if you are using it +# tools/** +# !tools/packages.config + +# Tabs Studio +*.tss + +# Telerik's JustMock configuration file +*.jmconfig + +# BizTalk build output +*.btp.cs +*.btm.cs +*.odx.cs +*.xsd.cs + +# OpenCover UI analysis results +OpenCover/ + +# Azure Stream Analytics local run output +ASALocalRun/ + +# MSBuild Binary and Structured Log +*.binlog + +# NVidia Nsight GPU debugger configuration file +*.nvuser + +# MFractors (Xamarin productivity tool) working folder +.mfractor/ + +# Local History for Visual Studio +.localhistory/ + +# Visual Studio History (VSHistory) files +.vshistory/ + +# BeatPulse healthcheck temp database +healthchecksdb + +# Backup folder for Package Reference Convert tool in Visual Studio 2017 +MigrationBackup/ + +# Ionide (cross platform F# VS Code tools) working folder +.ionide/ + +# Fody - auto-generated XML schema +FodyWeavers.xsd + +# VS Code files for those working on multiple tools +.vscode/* +!.vscode/settings.json +!.vscode/tasks.json +!.vscode/launch.json +!.vscode/extensions.json +*.code-workspace + +# Local History for Visual Studio Code +.history/ + +# Windows Installer files from build outputs +*.cab +*.msi +*.msix +*.msm +*.msp + +# JetBrains Rider +*.sln.iml + +# ---> VisualStudioCode +.vscode/* +!.vscode/settings.json +!.vscode/tasks.json +!.vscode/launch.json +!.vscode/extensions.json +!.vscode/*.code-snippets + +# Local History for Visual Studio Code +.history/ + +# Built Visual Studio Code Extensions +*.vsix + diff --git a/Devices/Packages/ros_kinematics/CMakeLists.txt b/Devices/Packages/ros_kinematics/CMakeLists.txt new file mode 100755 index 0000000..9f08e87 --- /dev/null +++ b/Devices/Packages/ros_kinematics/CMakeLists.txt @@ -0,0 +1,239 @@ +cmake_minimum_required(VERSION 3.0.2) +project(ros_kinematics) + +## Compile as C++11, supported in ROS Kinetic and newer +add_compile_options(-std=c++17) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + roscpp + rospy + pluginlib + models + geometry_msgs + std_msgs + dynamic_reconfigure + control_msgs + realtime_tools + urdf + tf + tf2 + tf2_ros +) + +## System dependencies are found with CMake's conventions +find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a exec_depend tag for "message_runtime" +## * In this file (CMakeLists.txt):ros_plugins +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# geometry_msgs +# ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +generate_dynamic_reconfigure_options( + cfg/SteerDriveParameters.cfg + cfg/DiffDriveParameters.cfg +) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( + INCLUDE_DIRS include + LIBRARIES ros_kinematics + CATKIN_DEPENDS + geometry_msgs + roscpp + rospy + models + pluginlib + tf + tf2 + tf2_ros + dynamic_reconfigure + control_msgs + realtime_tools + DEPENDS Boost +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} +) + +## Declare a C++ library +add_library(${PROJECT_NAME} + src/ros_steer_drive.cpp + src/ros_steer_drive_parameters.cpp + src/ros_diff_drive_parameters.cpp + src/odometry.cpp + src/speed_limiter.cpp + src/ros_diff_drive.cpp + ##src/ros_diff_drive_controller.cpp +) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +## With catkin_make all packages are built within a single CMake context +## The recommended prefix ensures that target names across packages don't collide +add_executable(${PROJECT_NAME}_node src/ros_kinematics_node.cpp) + +## Rename C++ executable without prefix +## The above recommended prefix causes long target names, the following renames the +## target back to the shorter version for ease of user use +## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" +# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") + +## Add cmake target dependencies of the executable +## same as for the library above +add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against + +target_link_libraries(${PROJECT_NAME} + ${Boost_LIBRARIES} + ${catkin_LIBRARIES} +) +target_link_libraries(${PROJECT_NAME}_node + ${PROJECT_NAME} + ${Boost_LIBRARIES} + ${catkin_LIBRARIES} +) + + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# catkin_install_python(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html +install(TARGETS ${PROJECT_NAME}_node + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +## Mark libraries for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) + +## Mark cpp header files for installation +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE +) + +## Mark other files for installation (e.g. launch and bag files, etc.) + +install(DIRECTORY + launch + config + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_ros_kinematics.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/Devices/Packages/ros_kinematics/README.md b/Devices/Packages/ros_kinematics/README.md new file mode 100755 index 0000000..7b6da1b --- /dev/null +++ b/Devices/Packages/ros_kinematics/README.md @@ -0,0 +1,2 @@ +# ros_kinematics + diff --git a/Devices/Packages/ros_kinematics/cfg/DiffDriveParameters.cfg b/Devices/Packages/ros_kinematics/cfg/DiffDriveParameters.cfg new file mode 100755 index 0000000..22adc57 --- /dev/null +++ b/Devices/Packages/ros_kinematics/cfg/DiffDriveParameters.cfg @@ -0,0 +1,18 @@ +#!/usr/bin/env python + +PACKAGE = 'ros_kinematics' + +from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, bool_t, double_t + +gen = ParameterGenerator() + +# Kinematic parameters related +gen.add("left_wheel_radius_multiplier", double_t, 0, "Left wheel radius multiplier.", 1.0, 0.5, 1.5) +gen.add("right_wheel_radius_multiplier", double_t, 0, "Right wheel radius multiplier.", 1.0, 0.5, 1.5) +gen.add("wheel_separation_multiplier", double_t, 0, "Wheel separation multiplier.", 1.0, 0.5, 1.5) + +# Publication related +gen.add("publish_rate", double_t, 0, "Publish rate of odom.", 50.0, 0.0, 2000.0) +gen.add("enable_odom_tf", bool_t, 0, "Publish odom frame to tf.", False) + +exit(gen.generate(PACKAGE, "ros_kinematics", "DiffDriveParameters")) diff --git a/Devices/Packages/ros_kinematics/cfg/SteerDriveParameters.cfg b/Devices/Packages/ros_kinematics/cfg/SteerDriveParameters.cfg new file mode 100755 index 0000000..6e07a35 --- /dev/null +++ b/Devices/Packages/ros_kinematics/cfg/SteerDriveParameters.cfg @@ -0,0 +1,11 @@ +#!/usr/bin/env python +from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, double_t + +gen = ParameterGenerator() + +gen.add('odomEncSteeringAngleOffset', double_t, 0, 'Góc bẻ lái động cơ lái được offset so với gốc origin ', 1.757546557, 0.0, 6.283185) +gen.add('steering_fix_wheel_distance_x', double_t, 0, 'Khoảng cách tâm O quay quanh trục Z động cơ Steer theo trục X', 1.3268, 0.0, 5.0) +gen.add('steering_fix_wheel_distance_y', double_t, 0, 'Khoảng cách tâm O quay quanh trục Z động cơ Steer theo trục X', 0.0, 0.0, 5.0) +gen.add('wheelAcceleration', double_t, 0, 'Gia tốc bánh kéo', 0.0, 0.0, 3.0) + +exit(gen.generate('ros_kinematics', 'ros_kinematics', 'SteerDriveParameters')) \ No newline at end of file diff --git a/Devices/Packages/ros_kinematics/config/ros_diff_drive_controller.yaml b/Devices/Packages/ros_kinematics/config/ros_diff_drive_controller.yaml new file mode 100755 index 0000000..dcd4685 --- /dev/null +++ b/Devices/Packages/ros_kinematics/config/ros_diff_drive_controller.yaml @@ -0,0 +1,64 @@ +# ----------------------------------- +left_wheel : 'left_wheel_joint' +right_wheel : 'right_wheel_joint' +publish_rate: 60 # this is what the real MiR platform publishes (default: 50) +# These covariances are exactly what the real MiR platform publishes +pose_covariance_diagonal : [0.00001, 0.00001, 1000000.0, 1000000.0, 1000000.0, 1000.0] +twist_covariance_diagonal: [0.1, 0.1, 1000000.0, 1000000.0, 1000000.0, 1000.0] +enable_odom_tf: false +# open_loop: false +# Wheel separation and diameter. These are both optional. +# diff_drive_controller will attempt to read either one or both from the +# URDF if not specified as a parameter. +# We don't set the value here because it's different for each MiR type (100, 250, ...), and +# the plugin figures out the correct values. +wheel_separation : 0.445208 +wheel_radius : 0.0625 + +# Wheel separation and radius multipliers +wheel_separation_multiplier: 1.0 # default: 1.0 +wheel_radius_multiplier : 1.0 # default: 1.0 + +# Velocity commands timeout [s], default 0.5 +cmd_vel_timeout: 0.5 + +# frame_ids (same as real MiR platform) +base_frame_id: base_footprint # default: base_link base_footprint +odom_frame_id: odom # default: odom + +# Velocity and acceleration limits +# Whenever a min_* is unspecified, default to -max_* +linear: + x: + has_velocity_limits : true + max_velocity : 1.5 # m/s; move_base max_vel_x: 0.8 + min_velocity : -1.5 # m/s + has_acceleration_limits: true + max_acceleration : 2.0 # m/s^2; move_base acc_lim_x: 1.5 + min_acceleration : -2.0 # m/s^2 + has_jerk_limits : true + max_jerk : 3.0 # m/s^3 +angular: + z: + has_velocity_limits : true + max_velocity : 2.0 # rad/s; move_base max_rot_vel: 1.0 + has_acceleration_limits: true + max_acceleration : 1.5 # rad/s^2; move_base acc_lim_th: 2.0 + has_jerk_limits : true + max_jerk : 2.5 # rad/s^3 + +left_wheel_joint: + lookup_name: WheelPlugin + max_publish_rate: 50 + mesurement_topic: left_encoder + wheel_topic: left_wheel + subscribe_queue_size: 1 + command_timeout: 5.0 + +right_wheel_joint: + lookup_name: WheelPlugin + max_publish_rate: 50 + mesurement_topic: right_encoder + wheel_topic: right_wheel + subscribe_queue_size: 1 + command_timeout: 5.0 diff --git a/Devices/Packages/ros_kinematics/config/ros_steer_drive.yaml b/Devices/Packages/ros_kinematics/config/ros_steer_drive.yaml new file mode 100755 index 0000000..90ba75d --- /dev/null +++ b/Devices/Packages/ros_kinematics/config/ros_steer_drive.yaml @@ -0,0 +1,29 @@ +commandTopic: $(arg prefix)cmd_vel +odometryTopic: odom +odometryFrame: $(arg prefix)odom +odometryEncTopic: odom_enc +odometryEncChildFrame: $(arg prefix)odom_enc +robotBaseFrame: $(arg prefix)base_link +steerChildFrame: $(arg prefix)steer_link +subscribe_queue_size: 1 +max_update_rate: 60 +odomEncSteeringAngleOffset: 0.0 +steering_fix_wheel_distance_x: 0.0 +steering_fix_wheel_distance_y: 0.0 +wheelAcceleration: 0.0 +use_abs_encoder: false + +steer_drive: tzbot_motor_wheel::SteerMotor +SteerMotor: + ratio: 171 + node_id: node1 + max_publish_rate: 100 + subscribe_queue_size: 1 + +traction_drive: tzbot_motor_wheel::TractionMotor +TractionMotor: + wheel_diameter: 0.210 + ratio: 24.68 + node_id: node2 + max_publish_rate: 100 + subscribe_queue_size: 1 \ No newline at end of file diff --git a/Devices/Packages/ros_kinematics/include/ros_kinematics/base_drive.h b/Devices/Packages/ros_kinematics/include/ros_kinematics/base_drive.h new file mode 100755 index 0000000..6570f2e --- /dev/null +++ b/Devices/Packages/ros_kinematics/include/ros_kinematics/base_drive.h @@ -0,0 +1,41 @@ +#ifndef _ROS_KINEMATICS_BASE_DRIVE_H_INCLUDED_ +#define _ROS_KINEMATICS_BASE_DRIVE_H_INCLUDED_ + +#include + +namespace ros_kinematics +{ + class BaseDrive + { + protected: + class DriveCmd + { + struct Linear_st + { + double x; + double y; + double z; + Linear_st() : x(0.0), y(0.0), z(0.0) {} + }; + + struct Angular_st + { + double x; + double y; + double z; + Angular_st() : x(0.0), y(0.0), z(0.0) {} + }; + + public: + Linear_st linear; + Angular_st angular; + ros::Time stamp; + DriveCmd() : stamp(0.0) {} + }; + + realtime_tools::RealtimeBuffer command_; + DriveCmd command_struct_; + }; +} + +#endif \ No newline at end of file diff --git a/Devices/Packages/ros_kinematics/include/ros_kinematics/odometry.h b/Devices/Packages/ros_kinematics/include/ros_kinematics/odometry.h new file mode 100755 index 0000000..7f25a6c --- /dev/null +++ b/Devices/Packages/ros_kinematics/include/ros_kinematics/odometry.h @@ -0,0 +1,212 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, PAL Robotics, S.L. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the PAL Robotics nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* + * Author: Luca Marchionni + * Author: Bence Magyar + * Author: Enrique Fernández + * Author: Paul Mathieu + */ + +#ifndef __ROS_KINEMATICS_ODOMETRY_H_INCLUDED_ +#define __ROS_KINEMATICS_ODOMETRY_H_INCLUDED_ + +#include +#include +#include +#include +#include + +namespace ros_kinematics +{ + namespace bacc = boost::accumulators; + + /** + * \brief The Odometry class handles odometry readings + * (2D pose and velocity with related timestamp) + */ + class Odometry + { + public: + + /// Integration function, used to integrate the odometry: + typedef boost::function IntegrationFunction; + + /** + * \brief Constructor + * Timestamp will get the current time value + * Value will be set to zero + * \param velocity_rolling_window_size Rolling window size used to compute the velocity mean + */ + Odometry(size_t velocity_rolling_window_size = 10); + + /** + * \brief Initialize the odometry + * \param time Current time + */ + void init(const ros::Time &time); + + /** + * \brief Updates the odometry class with latest wheels position + * \param left_pos Left wheel position [rad] + * \param right_pos Right wheel position [rad] + * \param time Current time + * \return true if the odometry is actually updated + */ + bool update(double left_pos, double right_pos, const ros::Time &time); + + /** + * \brief Updates the odometry class with latest velocity command + * \param linear Linear velocity [m/s] + * \param angular Angular velocity [rad/s] + * \param time Current time + */ + void updateOpenLoop(double linear, double angular, const ros::Time &time); + + /** + * \brief heading getter + * \return heading [rad] + */ + double getHeading() const + { + return heading_; + } + + /** + * \brief x position getter + * \return x position [m] + */ + double getX() const + { + return x_; + } + + /** + * \brief y position getter + * \return y position [m] + */ + double getY() const + { + return y_; + } + + /** + * \brief linear velocity getter + * \return linear velocity [m/s] + */ + double getLinear() const + { + return fabs(linear_) > 1e-9? linear_ : 0.0; + } + + /** + * \brief angular velocity getter + * \return angular velocity [rad/s] + */ + double getAngular() const + { + return fabs(angular_) > 1e-9? angular_ : 0.0;; + } + + /** + * \brief Sets the wheel parameters: radius and separation + * \param wheel_separation Separation between left and right wheels [m] + * \param left_wheel_radius Left wheel radius [m] + * \param right_wheel_radius Right wheel radius [m] + */ + void setWheelParams(double wheel_separation, double left_wheel_radius, double right_wheel_radius); + + /** + * \brief Velocity rolling window size setter + * \param velocity_rolling_window_size Velocity rolling window size + */ + void setVelocityRollingWindowSize(size_t velocity_rolling_window_size); + + private: + + /// Rolling mean accumulator and window: + typedef bacc::accumulator_set > RollingMeanAcc; + typedef bacc::tag::rolling_window RollingWindow; + + /** + * \brief Integrates the velocities (linear and angular) using 2nd order Runge-Kutta + * \param linear Linear velocity [m] (linear displacement, i.e. m/s * dt) computed by encoders + * \param angular Angular velocity [rad] (angular displacement, i.e. m/s * dt) computed by encoders + */ + void integrateRungeKutta2(double linear, double angular); + + /** + * \brief Integrates the velocities (linear and angular) using exact method + * \param linear Linear velocity [m] (linear displacement, i.e. m/s * dt) computed by encoders + * \param angular Angular velocity [rad] (angular displacement, i.e. m/s * dt) computed by encoders + */ + void integrateExact(double linear, double angular); + + /** + * \brief Reset linear and angular accumulators + */ + void resetAccumulators(); + + /// Current timestamp: + ros::Time timestamp_; + + /// Current pose: + double x_; // [m] + double y_; // [m] + double heading_; // [rad] + + /// Current velocity: + double linear_; // [m/s] + double angular_; // [rad/s] + + /// Wheel kinematic parameters [m]: + double wheel_separation_; + double left_wheel_radius_; + double right_wheel_radius_; + + /// Previou wheel position/state [rad]: + double left_wheel_old_pos_; + double right_wheel_old_pos_; + + /// Rolling mean accumulators for the linar and angular velocities: + size_t velocity_rolling_window_size_; + RollingMeanAcc linear_acc_; + RollingMeanAcc angular_acc_; + + /// Integration funcion, used to integrate the odometry: + IntegrationFunction integrate_fun_; + }; +} + +#endif \ No newline at end of file diff --git a/Devices/Packages/ros_kinematics/include/ros_kinematics/ros_diff_drive.h b/Devices/Packages/ros_kinematics/include/ros_kinematics/ros_diff_drive.h new file mode 100755 index 0000000..dd049b4 --- /dev/null +++ b/Devices/Packages/ros_kinematics/include/ros_kinematics/ros_diff_drive.h @@ -0,0 +1,194 @@ +#ifndef _ROS_KINEMATICS_DIFF_DRIVE_H_INCLUDED_ +#define _ROS_KINEMATICS_DIFF_DRIVE_H_INCLUDED_ + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +// Boost +#include +// plugin +#include +#include +#include + +namespace ros_kinematics +{ + class RosDiffDrive : public BaseDrive + { + public: + /** + * @brief Constructor + */ + RosDiffDrive(ros::NodeHandle& nh, const std::string& name); + + /** + * @brief Deconstructor + */ + virtual ~RosDiffDrive(); + + private: + + void CmdVelCallback(const geometry_msgs::Twist::ConstPtr& cmd_msg); + /** + * @brief returns true, if publishing of a cmd_vel topic is publishing + * + */ + bool isCmdVelTriggered(void); + + /** + * @brief returns true, if not subcribe cmd_vel + */ + bool isCmdVelLastestTriggered(void); + + /** + * @brief schedules MotorController function + * @param[in] schedule if true, publishing is scheduled, otherwise a possibly pending schedule is removed. + */ + void scheduleMotorController(bool schedule); + + void updateDynamicParams(); + void updateChild(void); + void updateOdometryEncoder(const ros::Time& current_time); + void publishOdometry(const ros::Time& current_time); + void brake(); + bool allReady(); + + /** + * @brief Sets the odometry publishing fields + * @param root_nh Root node handle + * @param controller_nh Node handle inside the controller namespace + */ + void setOdomPubFields(ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh); + + /** + * @brief Get the wheel names from a wheel param + * @param [in] controller_nh Controller node handler + * @param [in] wheel_param Param name + * @param [out] wheel_names Vector with the whel names + * @return true if the wheel_param is available and the wheel_names are + * retrieved successfully from the param server; false otherwise + */ + bool getWheelNames(ros::NodeHandle& controller_nh, const std::string& wheel_param, std::vector& wheel_names); + + bool getWheelParams(ros::NodeHandle& controller_nh, const std::vector& wheel_names, + std::map& drive_param_map); + + // properties + bool initialized_; + std::string name_; + ros::NodeHandle nh_; + ros::NodeHandle nh_priv_; + + pluginlib::ClassLoader motor_loader_; + std::vector > left_drive_; + std::vector > right_drive_; + + std::map left_drive_param_map_; + std::map right_drive_param_map_; + boost::shared_ptr > left_wheel_tf_pub_; + boost::shared_ptr > right_wheel_tf_pub_; + + bool use_abs_encoder; + pluginlib::ClassLoader encoder_loader_; + std::vector > left_encoder_; + std::vector > right_encoder_; + double left_wheel_curr_pos_; + double right_wheel_curr_pos_; + + + + ros::Subscriber cmd_vel_subscriber_; + int m_subscribe_queue_size_; + boost::shared_ptr callback_thread_; + ros::Duration schedule_delay_; + ros::Duration schedule_lastest_delay_; + ros::Time publish_time_; + ros::Time publish_lastest_time_; + boost::mutex publish_mutex_; + /// Publish executed commands + boost::shared_ptr > cmd_vel_pub_; + + ros_kinematics::Odometry odometry_; + ros::Duration publish_period_; + ros::Time last_odom_state_publish_time_; + bool open_loop_; + ros::Time last_odom_update_; + std::shared_ptr > odom_pub_; + std::shared_ptr > tf_odom_pub_; + + /// Wheel separation, wrt the midpoint of the wheel width: + double wheel_separation_; + + /// Wheel radius (assuming it's the same for the left and right wheels): + double wheel_radius_; + + /// Wheel separation and radius calibration multipliers: + double wheel_separation_multiplier_; + double left_wheel_radius_multiplier_; + double right_wheel_radius_multiplier_; + + /// Timeout to consider cmd_vel commands old: + double cmd_vel_timeout_; + + /// Whether to allow multiple publishers on cmd_vel topic or not: + bool allow_multiple_cmd_vel_publishers_; + + /// Frame to use for the robot base: + std::string base_frame_id_; + + /// Frame to use for odometry and odom tf: + std::string odom_frame_id_; + + /// Whether to publish odometry to tf or not: + bool enable_odom_tf_; + + /// Whether to publish wheel link to tf or not: + bool enable_wheel_tf_; + + /// Number of wheel joints: + size_t wheel_joints_size_; + boost::shared_ptr transform_broadcaster_; + + /// Speed limiters: + ros_kinematics::BaseDrive::DriveCmd last1_cmd_; + ros_kinematics::BaseDrive::DriveCmd last0_cmd_; + ros_kinematics::SpeedLimiter limiter_lin_; + ros_kinematics::SpeedLimiter limiter_ang_; + + /// Publish limited velocity: + bool publish_cmd_; + // Update Rate + double update_rate_; + + + /// Dynamic Reconfigure server + boost::shared_ptr kinematics_param_ptr_; + + std::string odometry_topic_; + std::string odometry_frame_; + std::string odometry_enc_topic_; + std::string odometry_enc_child_frame_; + std::string command_topic_; + + // Flags + bool publishOdomTF_; + + // bool publishWheelJointState_; + bool odom_enc_initialized_; + }; +} +#endif // _ROS_KINEMATICS_DIFF_DRIVE_H_INCLUDED_ \ No newline at end of file diff --git a/Devices/Packages/ros_kinematics/include/ros_kinematics/ros_diff_drive_parameters.h b/Devices/Packages/ros_kinematics/include/ros_kinematics/ros_diff_drive_parameters.h new file mode 100755 index 0000000..0c2b532 --- /dev/null +++ b/Devices/Packages/ros_kinematics/include/ros_kinematics/ros_diff_drive_parameters.h @@ -0,0 +1,51 @@ +#ifndef __ROS_KINEMATICS_DIFF_DRIVE_PARAMETERS_H_INCLUDED_ +#define __ROS_KINEMATICS_DIFF_DRIVE_PARAMETERS_H_INCLUDED_ + +#include +#include +#include +#include +#include +#include + +namespace ros_kinematics +{ +/** + * @class KinematicDiffDriveParameters + * @brief A dynamically reconfigurable class containing one representation of the robot's kinematics + */ +class KinematicDiffDriveParameters +{ +public: + KinematicDiffDriveParameters(); + KinematicDiffDriveParameters(const ros::NodeHandle& nh); + void initialize(const ros::NodeHandle& nh); + + inline double getLeftWheelRadiusMultiplier() { return left_wheel_radius_multiplier_; } + inline double getRightWheelRadiusMultiplier() { return right_wheel_radius_multiplier_; } + inline double getWheelSeparationMultiplier() { return wheel_separation_multiplier_; } + inline double getPublishRate() { return publish_rate_; } + inline bool getEnableOdomTf() { return enable_odom_tf_; } + inline double isSetup() {return setup_;} + using Ptr = std::shared_ptr; +protected: + void reconfigureCB(DiffDriveParametersConfig &config, uint32_t level); + void kinematicsCallback(const dynamic_reconfigure::Config::ConstPtr& param); + + bool initialized_; + bool setup_; + // For parameter descriptions, see cfg/KinematicParams.cfg + double left_wheel_radius_multiplier_; + double right_wheel_radius_multiplier_; + double wheel_separation_multiplier_; + double publish_rate_; + bool enable_odom_tf_; + + boost::mutex reconfig_mutex; + ros::NodeHandle nh_; + ros::Subscriber kinematics_sub_; + std::shared_ptr > dsrv_; +}; + +} // namespace ros_kinematics +#endif \ No newline at end of file diff --git a/Devices/Packages/ros_kinematics/include/ros_kinematics/ros_steer_drive.h b/Devices/Packages/ros_kinematics/include/ros_kinematics/ros_steer_drive.h new file mode 100755 index 0000000..ac630b8 --- /dev/null +++ b/Devices/Packages/ros_kinematics/include/ros_kinematics/ros_steer_drive.h @@ -0,0 +1,118 @@ +#ifndef _ROS_KINEMATICS_STEER_DRIVE_H_INCLUDED_ +#define _ROS_KINEMATICS_STEER_DRIVE_H_INCLUDED_ + +#include +#include +#include + +#include +#include +#include +#include +#include + +// Boost +#include +// plugin +#include +#include +#include + +namespace ros_kinematics +{ + class RosSteerDrive : public BaseDrive + { + public: + RosSteerDrive(ros::NodeHandle & nh, const std::string &name); + virtual ~RosSteerDrive(); + private: + + void CmdVelCallback(const geometry_msgs::Twist::ConstPtr& cmd_msg); + + /** + * @brief returns true, if publishing of a cmd_vel topic is publishing + * + */ + bool isCmdVelTriggered(void); + + /** + * @brief returns true, if not subcribe cmd_vel + */ + bool isCmdVelLastestTriggered(void); + + /** + * @brief schedules MotorController function + * @param[in] schedule if true, publishing is scheduled, otherwise a possibly pending schedule is removed. + */ + void scheduleMotorController(bool schedule); + + void MotorController(double target_speed, double target_steering_speed, double dt); + + void UpdateOdometryEncoder(); + void UpdateChild(void); + void PublishOdometry(double step_time); + void publishSteerJoint(double odom_alpha); + + pluginlib::ClassLoader motor_loader_; + std::string steer_motor_mode_, traction_motor_mode_; + boost::shared_ptr steer_motor_; + boost::shared_ptr traction_motor_; + + bool use_abs_encoder; + pluginlib::ClassLoader encoder_loader_; + boost::shared_ptr absolute_encoder_; + + ros::NodeHandle nh_; + ros::NodeHandle nh_priv_; + + ros::Publisher odometry_publisher_; + ros::Publisher odometry_enc_publisher_; + ros::Publisher cmd_vel_feedback_; + ros::Publisher steer_angle_pub_; + ros::Publisher cmd_vel_rb; + ros::Subscriber cmd_vel_subscriber_; + boost::shared_ptr transform_broadcaster_; + int m_subscribe_queue_size_; + boost::mutex callback_mutex_; + boost::thread *callback_thread_; + + nav_msgs::Odometry odom_; + nav_msgs::Odometry odom_enc_; + geometry_msgs::Pose2D pose_encoder_; + ros::Time last_odom_update_; + + // Update Rate + double update_rate_; + double update_period_; + ros::Time last_actuator_update_; + ros::Time publish_time_; + ros::Time publish_lastest_time_; + ros::Duration schedule_delay_; + ros::Duration schedule_lastest_delay_; + boost::mutex publish_mutex_; + + boost::shared_ptr kinematics_param_ptr; + double steering_fix_wheel_distance_x_; + double steering_fix_wheel_distance_y_; + double odom_enc_steering_angle_offset_; + + double wheel_diameter_; + double wheel_acceleration_; + double steering_ratio_; + double traction_ratio_; + + std::string odometry_topic_; + std::string odometry_frame_; + std::string odometry_enc_topic_; + std::string odometry_enc_child_frame_; + std::string robot_base_frame_; + std::string command_topic_; + std::string steer_id_; + + // Flags + bool publishOdomTF_; + // bool publishWheelJointState_; + bool odom_enc_initialized_; + }; +} +#endif \ No newline at end of file diff --git a/Devices/Packages/ros_kinematics/include/ros_kinematics/ros_steer_drive_parameters.h b/Devices/Packages/ros_kinematics/include/ros_kinematics/ros_steer_drive_parameters.h new file mode 100755 index 0000000..6f49d00 --- /dev/null +++ b/Devices/Packages/ros_kinematics/include/ros_kinematics/ros_steer_drive_parameters.h @@ -0,0 +1,48 @@ +#ifndef __ROS_KINEMATICS_STEER_DRIVE_PARAMETERS_H_INCLUDED_ +#define __ROS_KINEMATICS_STEER_DRIVE_PARAMETERS_H_INCLUDED_ + +#include +#include +#include +#include +#include +#include + +namespace ros_kinematics +{ +/** + * @class KinematicSteerDriveParameters + * @brief A dynamically reconfigurable class containing one representation of the robot's kinematics + */ +class KinematicSteerDriveParameters +{ +public: + KinematicSteerDriveParameters(); + KinematicSteerDriveParameters(const ros::NodeHandle& nh); + void initialize(const ros::NodeHandle& nh); + + inline double getOdomEncSteeringAngleOffset() { return odom_enc_steering_angle_offset_; } + inline double getSteeringFixWheelDistanceX() { return steering_fix_wheel_distance_x_; } + inline double getSteeringFixWheelDistanceY() { return steering_fix_wheel_distance_y_; } + inline double getWheelAcceleration() { return wheel_acceleration_; } + inline double isSetup() {return setup_;} + using Ptr = std::shared_ptr; +protected: + void reconfigureCB(SteerDriveParametersConfig &config, uint32_t level); + void kinematicsCallback(const dynamic_reconfigure::Config::ConstPtr& param); + + bool initialized_; + bool setup_; + // For parameter descriptions, see cfg/KinematicParams.cfg + double odom_enc_steering_angle_offset_; + double steering_fix_wheel_distance_x_; + double steering_fix_wheel_distance_y_; + double wheel_acceleration_; + boost::mutex reconfig_mutex; + ros::NodeHandle nh_; + ros::Subscriber kinematics_sub_; + std::shared_ptr > dsrv_; +}; + +} // namespace ros_kinematics +#endif \ No newline at end of file diff --git a/Devices/Packages/ros_kinematics/include/ros_kinematics/speed_limiter.h b/Devices/Packages/ros_kinematics/include/ros_kinematics/speed_limiter.h new file mode 100755 index 0000000..1849903 --- /dev/null +++ b/Devices/Packages/ros_kinematics/include/ros_kinematics/speed_limiter.h @@ -0,0 +1,131 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, PAL Robotics, S.L. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the PAL Robotics nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* + * Author: Enrique Fernández + */ + +#ifndef __ROS_KINEMATICS_SPEED_LIMITER_H_INCLUDED_ +#define __ROS_KINEMATICS_SPEED_LIMITER_H_INCLUDED_ + +namespace ros_kinematics +{ + + class SpeedLimiter + { + public: + + /** + * \brief Constructor + * \param [in] has_velocity_limits if true, applies velocity limits + * \param [in] has_acceleration_limits if true, applies acceleration limits + * \param [in] has_jerk_limits if true, applies jerk limits + * \param [in] min_velocity Minimum velocity [m/s], usually <= 0 + * \param [in] max_velocity Maximum velocity [m/s], usually >= 0 + * \param [in] min_acceleration Minimum acceleration [m/s^2], usually <= 0 + * \param [in] max_acceleration Maximum acceleration [m/s^2], usually >= 0 + * \param [in] min_jerk Minimum jerk [m/s^3], usually <= 0 + * \param [in] max_jerk Maximum jerk [m/s^3], usually >= 0 + */ + SpeedLimiter( + bool has_velocity_limits = false, + bool has_acceleration_limits = false, + bool has_jerk_limits = false, + double min_velocity = 0.0, + double max_velocity = 0.0, + double min_acceleration = 0.0, + double max_acceleration = 0.0, + double min_jerk = 0.0, + double max_jerk = 0.0 + ); + + /** + * \brief Limit the velocity and acceleration + * \param [in, out] v Velocity [m/s] + * \param [in] v0 Previous velocity to v [m/s] + * \param [in] v1 Previous velocity to v0 [m/s] + * \param [in] dt Time step [s] + * \return Limiting factor (1.0 if none) + */ + double limit(double& v, double v0, double v1, double dt); + + /** + * \brief Limit the velocity + * \param [in, out] v Velocity [m/s] + * \return Limiting factor (1.0 if none) + */ + double limit_velocity(double& v); + + /** + * \brief Limit the acceleration + * \param [in, out] v Velocity [m/s] + * \param [in] v0 Previous velocity [m/s] + * \param [in] dt Time step [s] + * \return Limiting factor (1.0 if none) + */ + double limit_acceleration(double& v, double v0, double dt); + + /** + * \brief Limit the jerk + * \param [in, out] v Velocity [m/s] + * \param [in] v0 Previous velocity to v [m/s] + * \param [in] v1 Previous velocity to v0 [m/s] + * \param [in] dt Time step [s] + * \return Limiting factor (1.0 if none) + * \see http://en.wikipedia.org/wiki/Jerk_%28physics%29#Motion_control + */ + double limit_jerk(double& v, double v0, double v1, double dt); + + public: + // Enable/Disable velocity/acceleration/jerk limits: + bool has_velocity_limits; + bool has_acceleration_limits; + bool has_jerk_limits; + + // Velocity limits: + double min_velocity; + double max_velocity; + + // Acceleration limits: + double min_acceleration; + double max_acceleration; + + // Jerk limits: + double min_jerk; + double max_jerk; + }; + +} // namespace ros_kinematics + +#endif \ No newline at end of file diff --git a/Devices/Packages/ros_kinematics/launch/diff_drive.launch b/Devices/Packages/ros_kinematics/launch/diff_drive.launch new file mode 100755 index 0000000..fa08d9b --- /dev/null +++ b/Devices/Packages/ros_kinematics/launch/diff_drive.launch @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + \ No newline at end of file diff --git a/Devices/Packages/ros_kinematics/launch/steer_drive.launch b/Devices/Packages/ros_kinematics/launch/steer_drive.launch new file mode 100755 index 0000000..1220784 --- /dev/null +++ b/Devices/Packages/ros_kinematics/launch/steer_drive.launch @@ -0,0 +1,13 @@ + + + + + + + + + + + + + \ No newline at end of file diff --git a/Devices/Packages/ros_kinematics/package.xml b/Devices/Packages/ros_kinematics/package.xml new file mode 100755 index 0000000..5fec74f --- /dev/null +++ b/Devices/Packages/ros_kinematics/package.xml @@ -0,0 +1,96 @@ + + + ros_kinematics + 0.0.0 + The ros_kinematics package + + + + + robotics + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + message_runtime + + + + + catkin + geometry_msgs + std_msgs + roscpp + rospy + models + pluginlib + tf + dynamic_reconfigure + tf2 + tf2_ros + control_msgs + realtime_tools + + geometry_msgs + std_msgs + roscpp + rospy + models + pluginlib + tf + dynamic_reconfigure + tf2 + tf2_ros + control_msgs + realtime_tools + + geometry_msgs + std_msgs + roscpp + rospy + models + pluginlib + tf + dynamic_reconfigure + tf2 + tf2_ros + control_msgs + realtime_tools + + + + + + + diff --git a/Devices/Packages/ros_kinematics/src/odometry.cpp b/Devices/Packages/ros_kinematics/src/odometry.cpp new file mode 100755 index 0000000..fbf2191 --- /dev/null +++ b/Devices/Packages/ros_kinematics/src/odometry.cpp @@ -0,0 +1,173 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, PAL Robotics, S.L. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the PAL Robotics nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* + * Author: Luca Marchionni + * Author: Bence Magyar + * Author: Enrique Fernández + * Author: Paul Mathieu + */ +#include +#include + +namespace ros_kinematics +{ + namespace bacc = boost::accumulators; + + Odometry::Odometry(size_t velocity_rolling_window_size) + : timestamp_(0.0) + , x_(0.0) + , y_(0.0) + , heading_(0.0) + , linear_(0.0) + , angular_(0.0) + , wheel_separation_(0.0) + , left_wheel_radius_(0.0) + , right_wheel_radius_(0.0) + , left_wheel_old_pos_(0.0) + , right_wheel_old_pos_(0.0) + , velocity_rolling_window_size_(velocity_rolling_window_size) + , linear_acc_(RollingWindow::window_size = velocity_rolling_window_size) + , angular_acc_(RollingWindow::window_size = velocity_rolling_window_size) + , integrate_fun_(std::bind(&Odometry::integrateExact, this, std::placeholders::_1, std::placeholders::_2)) + { + } + + void Odometry::init(const ros::Time& time) + { + // Reset accumulators and timestamp: + resetAccumulators(); + timestamp_ = time; + } + + bool Odometry::update(double left_pos, double right_pos, const ros::Time &time) + { + /// Get current wheel joint positions: + const double left_wheel_cur_pos = left_pos * left_wheel_radius_; + const double right_wheel_cur_pos = right_pos * right_wheel_radius_; + + /// Estimate velocity of wheels using old and current position: + const double left_wheel_est_vel = left_wheel_cur_pos - left_wheel_old_pos_; + const double right_wheel_est_vel = right_wheel_cur_pos - right_wheel_old_pos_; + + /// Update old position with current: + left_wheel_old_pos_ = left_wheel_cur_pos; + right_wheel_old_pos_ = right_wheel_cur_pos; + + /// Compute linear and angular diff: + const double linear = (right_wheel_est_vel + left_wheel_est_vel) * 0.5 ; + const double angular = (right_wheel_est_vel - left_wheel_est_vel) / wheel_separation_; + + /// Integrate odometry: + integrate_fun_(linear, angular); + + /// We cannot estimate the speed with very small time intervals: + const double dt = (time - timestamp_).toSec(); + if (dt < 0.0001) + return false; // Interval too small to integrate with + + timestamp_ = time; + + /// Estimate speeds using a rolling mean to filter them out: + linear_acc_(linear/dt); + angular_acc_(angular/dt); + + linear_ = bacc::rolling_mean(linear_acc_); + angular_ = bacc::rolling_mean(angular_acc_); + return true; + } + + void Odometry::updateOpenLoop(double linear, double angular, const ros::Time &time) + { + /// Save last linear and angular velocity: + linear_ = linear; + angular_ = angular; + + /// Integrate odometry: + const double dt = (time - timestamp_).toSec(); + timestamp_ = time; + integrate_fun_(linear * dt, angular * dt); + } + + void Odometry::setWheelParams(double wheel_separation, double left_wheel_radius, double right_wheel_radius) + { + wheel_separation_ = wheel_separation; + left_wheel_radius_ = left_wheel_radius; + right_wheel_radius_ = right_wheel_radius; + } + + void Odometry::setVelocityRollingWindowSize(size_t velocity_rolling_window_size) + { + velocity_rolling_window_size_ = velocity_rolling_window_size; + + resetAccumulators(); + } + + void Odometry::integrateRungeKutta2(double linear, double angular) + { + const double direction = heading_ + angular * 0.5; + + /// Runge-Kutta 2nd order integration: + x_ += linear * cos(direction); + y_ += linear * sin(direction); + heading_ += angular; + } + + /** + * \brief Other possible integration method provided by the class + * \param linear + * \param angular + */ + void Odometry::integrateExact(double linear, double angular) + { + if (fabs(angular) < 1e-6) + integrateRungeKutta2(linear, angular); + else + { + /// Exact integration (should solve problems when angular is zero): + const double heading_old = heading_; + const double r = linear/angular; + heading_ += angular; + x_ += r * (sin(heading_) - sin(heading_old)); + y_ += -r * (cos(heading_) - cos(heading_old)); + } + } + + void Odometry::resetAccumulators() + { + linear_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_); + angular_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_); + } + +} // namespace ros_kinematics diff --git a/Devices/Packages/ros_kinematics/src/ros_diff_drive.cpp b/Devices/Packages/ros_kinematics/src/ros_diff_drive.cpp new file mode 100755 index 0000000..b3fd9b7 --- /dev/null +++ b/Devices/Packages/ros_kinematics/src/ros_diff_drive.cpp @@ -0,0 +1,824 @@ +#include "ros_kinematics/ros_diff_drive.h" +#include +#include +#include + + +ros_kinematics::RosDiffDrive::RosDiffDrive(ros::NodeHandle& nh, const std::string& name) + : nh_(nh), + name_(name), + motor_loader_("models", "models::BaseSteerDrive"), + encoder_loader_("models", "models::BaseAbsoluteEncoder"), + last_odom_state_publish_time_(ros::Time(0)), + left_wheel_curr_pos_(0), + right_wheel_curr_pos_(0), + publish_time_(ros::Time(0)), + initialized_(false) +{ + if(!initialized_) + { + nh_priv_ = ros::NodeHandle(nh_, name_); + ROS_INFO("RosDiffDrive get node name is %s", name_.c_str()); + + bool init_odom_enc; + nh_priv_.param("initialize_odom_enc", init_odom_enc, true); + odom_enc_initialized_ = !init_odom_enc; + + nh_priv_.param("wheel_separation_multiplier", wheel_separation_multiplier_, wheel_separation_multiplier_); + ROS_INFO_STREAM_NAMED(name_, "Wheel separation will be multiplied by " << wheel_separation_multiplier_ << "."); + + if (nh_priv_.hasParam("wheel_radius_multiplier")) + { + double wheel_radius_multiplier; + nh_priv_.getParam("wheel_radius_multiplier", wheel_radius_multiplier); + + left_wheel_radius_multiplier_ = wheel_radius_multiplier; + right_wheel_radius_multiplier_ = wheel_radius_multiplier; + } + else + { + nh_priv_.param("left_wheel_radius_multiplier", left_wheel_radius_multiplier_, left_wheel_radius_multiplier_); + nh_priv_.param("right_wheel_radius_multiplier", right_wheel_radius_multiplier_, right_wheel_radius_multiplier_); + } + + ROS_INFO_STREAM_NAMED(name_, "Left wheel radius will be multiplied by " << left_wheel_radius_multiplier_ << "."); + ROS_INFO_STREAM_NAMED(name_, "Right wheel radius will be multiplied by " << right_wheel_radius_multiplier_ << "."); + + int velocity_rolling_window_size = 10; + nh_priv_.param("velocity_rolling_window_size", velocity_rolling_window_size, velocity_rolling_window_size); + ROS_INFO_STREAM_NAMED(name_, "Velocity rolling window size of " << velocity_rolling_window_size << "."); + + odometry_.setVelocityRollingWindowSize(velocity_rolling_window_size); + + // Twist command related: + nh_priv_.param("cmd_vel_timeout", cmd_vel_timeout_, cmd_vel_timeout_); + ROS_INFO_STREAM_NAMED(name_, "Velocity commands will be considered old if they are older than " << cmd_vel_timeout_ << "s."); + + nh_priv_.param("allow_multiple_cmd_vel_publishers", allow_multiple_cmd_vel_publishers_, allow_multiple_cmd_vel_publishers_); + ROS_INFO_STREAM_NAMED(name_, "Allow mutiple cmd_vel publishers is " << (allow_multiple_cmd_vel_publishers_ ? "enabled" : "disabled")); + + nh_priv_.param("base_frame_id", base_frame_id_, base_frame_id_); + ROS_INFO_STREAM_NAMED(name_, "Base frame_id set to " << base_frame_id_); + + nh_priv_.param("odom_frame_id", odom_frame_id_, odom_frame_id_); + ROS_INFO_STREAM_NAMED(name_, "Odometry frame_id set to " << odom_frame_id_); + + nh_priv_.param("enable_odom_tf", enable_odom_tf_, enable_odom_tf_); + ROS_INFO_STREAM_NAMED(name_, "Publishing base frame to tf is " << (enable_odom_tf_ ? "enabled" : "disabled")); + + nh_priv_.param("enable_wheel_tf", enable_wheel_tf_, enable_wheel_tf_); + ROS_INFO_STREAM_NAMED(name_, "Publishing wheel frame to tf is " << (enable_wheel_tf_ ? "enabled" : "disabled")); + + nh_priv_.param("commandTopic", command_topic_, std::string("cmd_vel")); + // nh_priv_.param("publishOdomTF", publishOdomTF_, false); + nh_priv_.param("odometryTopic", odometry_topic_, std::string("odom")); + // nh_priv_.param("odometryFrame", odometry_frame_, std::string("odom")); + nh_priv_.param("odometryEncTopic", odometry_enc_topic_, std::string("odom_enc")); + nh_priv_.param("odometryEncChildFrame", odometry_enc_child_frame_, std::string("odom_enc")); + // nh_priv_.param("robotBaseFrame", robot_base_frame_, std::string("base_link")); + nh_priv_.param("subscribe_queue_size", m_subscribe_queue_size_, 1); + nh_priv_.param("publish_rate", update_rate_, 100.0); + publish_period_ = ros::Duration(1.0 / update_rate_); + nh_priv_.param("open_loop", open_loop_, false); + + // Velocity and acceleration limits: + nh_priv_.param("linear/x/has_velocity_limits", limiter_lin_.has_velocity_limits, limiter_lin_.has_velocity_limits); + nh_priv_.param("linear/x/has_acceleration_limits", limiter_lin_.has_acceleration_limits, limiter_lin_.has_acceleration_limits); + nh_priv_.param("linear/x/has_jerk_limits", limiter_lin_.has_jerk_limits, limiter_lin_.has_jerk_limits); + nh_priv_.param("linear/x/max_velocity", limiter_lin_.max_velocity, limiter_lin_.max_velocity); + nh_priv_.param("linear/x/min_velocity", limiter_lin_.min_velocity, -limiter_lin_.max_velocity); + nh_priv_.param("linear/x/max_acceleration", limiter_lin_.max_acceleration, limiter_lin_.max_acceleration); + nh_priv_.param("linear/x/min_acceleration", limiter_lin_.min_acceleration, -limiter_lin_.max_acceleration); + nh_priv_.param("linear/x/max_jerk", limiter_lin_.max_jerk, limiter_lin_.max_jerk); + nh_priv_.param("linear/x/min_jerk", limiter_lin_.min_jerk, -limiter_lin_.max_jerk); + + nh_priv_.param("angular/z/has_velocity_limits", limiter_ang_.has_velocity_limits, limiter_ang_.has_velocity_limits); + nh_priv_.param("angular/z/has_acceleration_limits", limiter_ang_.has_acceleration_limits, limiter_ang_.has_acceleration_limits); + nh_priv_.param("angular/z/has_jerk_limits", limiter_ang_.has_jerk_limits, limiter_ang_.has_jerk_limits); + nh_priv_.param("angular/z/max_velocity", limiter_ang_.max_velocity, limiter_ang_.max_velocity); + nh_priv_.param("angular/z/min_velocity", limiter_ang_.min_velocity, -limiter_ang_.max_velocity); + nh_priv_.param("angular/z/max_acceleration", limiter_ang_.max_acceleration, limiter_ang_.max_acceleration); + nh_priv_.param("angular/z/min_acceleration", limiter_ang_.min_acceleration, -limiter_ang_.max_acceleration); + nh_priv_.param("angular/z/max_jerk", limiter_ang_.max_jerk, limiter_ang_.max_jerk); + nh_priv_.param("angular/z/min_jerk", limiter_ang_.min_jerk, -limiter_ang_.max_jerk); + // Publish limited velocity: + nh_priv_.param("publish_cmd", publish_cmd_, publish_cmd_); + // Use encoder + nh_.param(name_ + "/use_encoder", use_abs_encoder, false); + // If either parameter is not available, we need to look up the value in the URDF + bool lookup_wheel_separation = !nh_priv_.getParam("wheel_separation", wheel_separation_); + bool lookup_wheel_radius = !nh_priv_.getParam("wheel_radius", wheel_radius_); + + if(lookup_wheel_separation || lookup_wheel_radius) + ROS_INFO("URDF if not specified as a parameter. We don't set the value here \n wheel_separation: %f, wheel_radius %f", + wheel_separation_, wheel_radius_); + + // Regardless of how we got the separation and radius, use them + // to set the odometry parameters + const double ws = wheel_separation_multiplier_ * wheel_separation_; + const double lwr = left_wheel_radius_multiplier_ * wheel_radius_; + const double rwr = right_wheel_radius_multiplier_ * wheel_radius_; + + odometry_.setWheelParams(ws, lwr, rwr); + ROS_INFO_STREAM_NAMED(name_, + "Odometry params : wheel separation " << ws + << ", left wheel radius " << lwr + << ", right wheel radius " << rwr); + + if (publish_cmd_) + { + cmd_vel_pub_.reset(new realtime_tools::RealtimePublisher(nh_priv_, "cmd_vel_out", 100)); + } + + // Get joint names from the parameter server + std::vector left_wheel_names, right_wheel_names; + if (!getWheelNames(nh_priv_, "left_wheel", left_wheel_names) || + !getWheelNames(nh_priv_, "right_wheel", right_wheel_names)) + { + exit(1); + } + + if (!getWheelParams(nh_priv_, left_wheel_names, left_drive_param_map_) || + !getWheelParams(nh_priv_, right_wheel_names, right_drive_param_map_)) + { + exit(1); + } + + if (left_wheel_names.size() != right_wheel_names.size() || + left_drive_param_map_.size() != left_wheel_names.size() || + right_drive_param_map_.size() != right_wheel_names.size() + ) + { + ROS_ERROR_STREAM_NAMED(name_, + "#left wheels (" << left_wheel_names.size() << ") != " << + "#right wheels (" << right_wheel_names.size() << ")."); + exit(1); + } + else + { + wheel_joints_size_ = left_wheel_names.size(); + + left_drive_.resize(wheel_joints_size_); + right_drive_.resize(wheel_joints_size_); + if (use_abs_encoder) + { + left_encoder_.resize(wheel_joints_size_); + right_encoder_.resize(wheel_joints_size_); + } + } + + // plugin models + ROS_INFO("RosDiffDrive is plugining models..."); + for (size_t i = 0; i < wheel_joints_size_; ++i) + { + std::string left_drive; + nh_priv_.param(left_wheel_names[i] + "/lookup_name", left_drive, std::string("motor_wheel::LeftMotor")); + try + { + left_drive_[i] = motor_loader_.createInstance(left_drive); + // left_drive_[i]->initialize(nh_, name_ + "/" + motor_loader_.getName(left_drive)); + left_drive_[i]->initialize(nh_, name_ + "/" + left_wheel_names[i]); + if (use_abs_encoder) + { + std::string right_encoder; + nh_priv_.param(right_wheel_names[i] + "/encoder/lookup_name", right_encoder, std::string("absolute_encoder::Measurements")); + try + { + right_encoder_[i] = encoder_loader_.createInstance(right_encoder); + right_encoder_[i]->initialize(nh_, name_ + "/" + right_wheel_names[i] + "/encoder"); + } + catch (const pluginlib::PluginlibException& ex) + { + ROS_FATAL("Failed to create the %s model, are you sure it is properly registered and that the containing library is built? Exception: %s", + right_encoder.c_str(), ex.what()); + exit(1); + } + } + + } + catch (const pluginlib::PluginlibException& ex) + { + ROS_FATAL("Failed to create the %s model, are you sure it is properly registered and that the containing library is built? Exception: %s", + left_drive.c_str(), ex.what()); + exit(1); + } + + + std::string right_drive; + nh_priv_.param(right_wheel_names[i] + "/lookup_name", right_drive, std::string("motor_wheel::RightMotor")); + try + { + right_drive_[i] = motor_loader_.createInstance(right_drive); + // right_drive_[i]->initialize(nh_, name_ + "/" + motor_loader_.getName(right_drive)); + right_drive_[i]->initialize(nh_, name_ + "/" + right_wheel_names[i]); + + if (use_abs_encoder) + { + std::string left_encoder; + nh_priv_.param(left_wheel_names[i] + "/encoder/lookup_name", left_encoder, std::string("absolute_encoder::Measurements")); + try + { + left_encoder_[i] = encoder_loader_.createInstance(left_encoder); + left_encoder_[i]->initialize(nh_, name_ + "/" + left_wheel_names[i] + "/encoder"); + } + catch (const pluginlib::PluginlibException& ex) + { + ROS_FATAL("Failed to create the %s model, are you sure it is properly registered and that the containing library is built? Exception: %s", + left_encoder.c_str(), ex.what()); + exit(1); + } + } + } + catch (const pluginlib::PluginlibException& ex) + { + ROS_FATAL("Failed to create the %s model, are you sure it is properly registered and that the containing library is built? Exception: %s", + right_drive.c_str(), ex.what()); + exit(1); + } + } + + if(enable_wheel_tf_) + { + left_wheel_tf_pub_.reset(new realtime_tools::RealtimePublisher(nh_, "/tf", 100)); + for (std::map::iterator itr = left_drive_param_map_.begin(); itr != left_drive_param_map_.end(); ++itr) + { + geometry_msgs::TransformStamped tfs; + tfs.child_frame_id = static_cast(itr->second["frame_id"]); + tfs.header.frame_id = "base_link"; + tfs.transform.translation.x = static_cast(itr->second["origin"][0]); + tfs.transform.translation.y = static_cast(itr->second["origin"][1]); + tfs.transform.translation.z = static_cast(itr->second["origin"][2]); + double roll = static_cast(itr->second["origin"][3]); + double pitch = static_cast(itr->second["origin"][4]); + double yaw = static_cast(itr->second["origin"][5]); + geometry_msgs::Quaternion q = tf::createQuaternionMsgFromRollPitchYaw(roll, pitch, yaw); + tfs.transform.rotation = q; + left_wheel_tf_pub_->msg_.transforms.push_back(tfs); + } + + right_wheel_tf_pub_.reset(new realtime_tools::RealtimePublisher(nh_, "/tf", 100)); + for (std::map::iterator itr = right_drive_param_map_.begin(); itr != right_drive_param_map_.end(); ++itr) + { + geometry_msgs::TransformStamped tfs; + tfs.child_frame_id = static_cast(itr->second["frame_id"]); + tfs.header.frame_id = "base_link"; + tfs.transform.translation.x = static_cast(itr->second["origin"][0]); + tfs.transform.translation.y = static_cast(itr->second["origin"][1]); + tfs.transform.translation.z = static_cast(itr->second["origin"][2]); + double roll = static_cast(itr->second["origin"][3]); + double pitch = static_cast(itr->second["origin"][4]); + double yaw = static_cast(itr->second["origin"][5]); + geometry_msgs::Quaternion q = tf::createQuaternionMsgFromRollPitchYaw(roll, pitch, yaw); + tfs.transform.rotation = q; + right_wheel_tf_pub_->msg_.transforms.push_back(tfs); + } + } + + schedule_delay_ = ros::Duration(1/update_rate_); + schedule_lastest_delay_ = ros::Duration(0.5); + + this->setOdomPubFields(nh_, nh_priv_); + transform_broadcaster_ = boost::shared_ptr(new tf2_ros::TransformBroadcaster()); + cmd_vel_subscriber_ = nh_.subscribe(command_topic_, m_subscribe_queue_size_, &ros_kinematics::RosDiffDrive::CmdVelCallback, this); + + // Kinematics + kinematics_param_ptr_ = boost::make_shared(); + kinematics_param_ptr_->initialize(nh_); + + // Thread + callback_thread_ = boost::make_shared(&ros_kinematics::RosDiffDrive::updateChild, this); + + ROS_INFO_NAMED("RosDiffDrive","Initializing on %s is successed", name_.c_str()); + initialized_ = true; + } +} + +ros_kinematics::RosDiffDrive::~RosDiffDrive() +{ + callback_thread_ = nullptr; + kinematics_param_ptr_ = nullptr; +} + +void ros_kinematics::RosDiffDrive::CmdVelCallback(const geometry_msgs::Twist::ConstPtr& cmd_msg) +{ + // check that we don't have multiple publishers on the command topic + if (!allow_multiple_cmd_vel_publishers_ && cmd_vel_subscriber_.getNumPublishers() > 1) + { + ROS_ERROR_STREAM_THROTTLE_NAMED(1.0, name_, "Detected " << cmd_vel_subscriber_.getNumPublishers() + << " publishers " << command_topic_ << ". Only 1 publisher is allowed. Going to brake."); + brake(); + return; + } + + if (!std::isfinite(cmd_msg->angular.z) || !std::isfinite(cmd_msg->linear.x)) + { + ROS_WARN_THROTTLE(1.0, "Received NaN in velocity command. Ignoring."); + return; + } + + command_struct_.angular.z = cmd_msg->angular.z; + command_struct_.linear.x = cmd_msg->linear.x; + command_struct_.stamp = ros::Time::now(); + command_.writeFromNonRT(command_struct_); + this->scheduleMotorController(true); + ROS_DEBUG_STREAM_NAMED(name_, + "Added values to command. " + << "Ang: " << command_struct_.angular.z << ", " + << "Lin: " << command_struct_.linear.x << ", " + << "Stamp: " << command_struct_.stamp); +} + +bool ros_kinematics::RosDiffDrive::isCmdVelTriggered(void) +{ + boost::lock_guard schedule_lockguard(publish_mutex_); + return !publish_time_.isZero() && ros::Time::now() > publish_time_; +} + + +bool ros_kinematics::RosDiffDrive::isCmdVelLastestTriggered(void) +{ + boost::lock_guard schedule_lockguard(publish_mutex_); + return !publish_lastest_time_.isZero() && ros::Time::now() > publish_lastest_time_; +} + + +void ros_kinematics::RosDiffDrive::scheduleMotorController(bool schedule) +{ + boost::lock_guard schedule_lockguard(publish_mutex_); + if(schedule && publish_time_.isZero()) + { + publish_time_ = ros::Time::now() + schedule_delay_; + publish_lastest_time_ = ros::Time::now() + schedule_lastest_delay_; + } + if(!schedule && !publish_time_.isZero()) + { + publish_time_ = ros::Time(0); + publish_lastest_time_ = ros::Time(0); + } +} + +void ros_kinematics::RosDiffDrive::updateOdometryEncoder(const ros::Time& current_time) +{ + // ros::Time current_time = ros::Time::now(); + double step_time = ros::Duration(current_time - last_odom_update_).toSec(); + last_odom_update_ = current_time; + + // Apply (possibly new) multipliers: + const double ws = wheel_separation_multiplier_ * wheel_separation_; + const double lwr = left_wheel_radius_multiplier_ * wheel_radius_; + const double rwr = right_wheel_radius_multiplier_ * wheel_radius_; + odometry_.setWheelParams(ws, lwr, rwr); + // COMPUTE AND PUBLISH ODOMETRY + if (open_loop_) + { + odometry_.updateOpenLoop(last0_cmd_.linear.x, last0_cmd_.angular.z, current_time); + } + else + { + double left_pos = 0.0; + double right_pos = 0.0; + for (size_t i = 0; i < wheel_joints_size_; ++i) + { + double left_speed, right_speed; + if (use_abs_encoder) + { + left_speed = left_encoder_[i]->Velocity(); + right_speed = right_encoder_[i]->Velocity(); + } + else + { + left_speed = left_drive_[i]->GetVelocity(); + right_speed = right_drive_[i]->GetVelocity(); + } + + const double lp = left_speed * step_time; // rad/s -> rad + const double rp = right_speed * step_time; + + if (std::isnan(lp) || std::isnan(rp)) + return; + + left_pos += lp; + right_pos += rp; + } + left_pos /= wheel_joints_size_; + right_pos /= wheel_joints_size_; + left_wheel_curr_pos_ += left_pos; + right_wheel_curr_pos_ += right_pos; + // Estimate linear and angular velocity using joint information + odometry_.update(left_wheel_curr_pos_, right_wheel_curr_pos_, current_time); + + if(enable_wheel_tf_) + { + if(left_wheel_tf_pub_->trylock()) + { + geometry_msgs::Quaternion q = tf::createQuaternionMsgFromRollPitchYaw(0, left_wheel_curr_pos_, 0); + for(auto &msg_tf : left_wheel_tf_pub_->msg_.transforms) + { + msg_tf.header.stamp = current_time; + msg_tf.transform.rotation = q; + } + left_wheel_tf_pub_->unlockAndPublish(); + } + + if(right_wheel_tf_pub_->trylock()) + { + geometry_msgs::Quaternion q = tf::createQuaternionMsgFromRollPitchYaw(0, right_wheel_curr_pos_, 0); + for(auto &msg_tf : right_wheel_tf_pub_->msg_.transforms) + { + msg_tf.header.stamp = current_time; + msg_tf.transform.rotation = q; + } + right_wheel_tf_pub_->unlockAndPublish(); + } + } + + } +} + +void ros_kinematics::RosDiffDrive::publishOdometry(const ros::Time& current_time) +{ + // Publish odometry message + if (last_odom_state_publish_time_ + publish_period_ < current_time) + { + last_odom_state_publish_time_ += publish_period_; + // Compute and store orientation info + const geometry_msgs::Quaternion orientation( + tf::createQuaternionMsgFromYaw(odometry_.getHeading())); + + // Populate odom message and publish + if (odom_pub_ && odom_pub_->trylock()) + { + odom_pub_->msg_.header.stamp = current_time; + odom_pub_->msg_.pose.pose.position.x = odometry_.getX(); + odom_pub_->msg_.pose.pose.position.y = odometry_.getY(); + odom_pub_->msg_.pose.pose.orientation = orientation; + odom_pub_->msg_.twist.twist.linear.x = odometry_.getLinear(); + odom_pub_->msg_.twist.twist.angular.z = odometry_.getAngular(); + odom_pub_->unlockAndPublish(); + } + + // Publish tf /odom frame + if (enable_odom_tf_ && tf_odom_pub_->trylock()) + { + geometry_msgs::TransformStamped& odom_frame = tf_odom_pub_->msg_.transforms[0]; + odom_frame.header.stamp = current_time; + odom_frame.transform.translation.x = odometry_.getX(); + odom_frame.transform.translation.y = odometry_.getY(); + odom_frame.transform.rotation = orientation; + tf_odom_pub_->unlockAndPublish(); + } + } +} + +void ros_kinematics::RosDiffDrive::updateChild(void) +{ + ros::Rate rate(update_rate_); + last_odom_state_publish_time_ = ros::Time::now(); + last_odom_update_ = ros::Time::now(); + odometry_.init(ros::Time::now()); + + while (ros::ok()) + { + ros::Time current_time = ros::Time::now(); + // update parameter from dynamic reconf + this->updateDynamicParams(); + + const double ws = wheel_separation_multiplier_ * wheel_separation_; + const double lwr = left_wheel_radius_multiplier_ * wheel_radius_; + const double rwr = right_wheel_radius_multiplier_ * wheel_radius_; + + if (this->allReady()) + { + this->updateOdometryEncoder(current_time); + this->publishOdometry(current_time); + } + + // MOVE ROBOT + DriveCmd curr_cmd = *(command_.readFromRT()); + const double dt = (current_time - curr_cmd.stamp).toSec(); + + // Brake if cmd_vel has timeout: + if (dt > cmd_vel_timeout_) + { + curr_cmd.linear.x = 0.0; + curr_cmd.angular.z = 0.0; + } + + // Limit velocities and accelerations: + const double cmd_dt(dt); + + limiter_lin_.limit(curr_cmd.linear.x, last0_cmd_.linear.x, last1_cmd_.linear.x, cmd_dt); + limiter_ang_.limit(curr_cmd.angular.z, last0_cmd_.angular.z, last1_cmd_.angular.z, cmd_dt); + + last1_cmd_ = last0_cmd_; + last0_cmd_ = curr_cmd; + + // Compute wheels velocities: + const double vel_left = (curr_cmd.linear.x - curr_cmd.angular.z * ws / 2.0) / lwr; + const double vel_right = (curr_cmd.linear.x + curr_cmd.angular.z * ws / 2.0) / rwr; + + if(this->isCmdVelTriggered()) + { + // Publish limited velocity: + if (publish_cmd_ && cmd_vel_pub_ && cmd_vel_pub_->trylock()) + { + cmd_vel_pub_->msg_.header.stamp = current_time; + cmd_vel_pub_->msg_.twist.linear.x = curr_cmd.linear.x; + cmd_vel_pub_->msg_.twist.angular.z = curr_cmd.angular.z; + cmd_vel_pub_->unlockAndPublish(); + } + + for (size_t i = 0; i < wheel_joints_size_; ++i) + { + left_drive_[i]->SetVelocity(vel_left); + right_drive_[i]->SetVelocity(vel_right); + } + scheduleMotorController(false); + } + + if(this->isCmdVelLastestTriggered()) + { + int counter = 3; + do + { + for (size_t i = 0; i < wheel_joints_size_; ++i) + { + left_drive_[i]->SetVelocity(0.0); + right_drive_[i]->SetVelocity(0.0); + } + counter--; + rate.sleep(); + ros::spinOnce(); + }while(ros::ok() && counter > 0); + scheduleMotorController(false); + } + + rate.sleep(); + ros::spinOnce(); + } + +} + +void ros_kinematics::RosDiffDrive::brake() +{ + const double vel = 0.0; + for (size_t i = 0; i < wheel_joints_size_; ++i) + { + left_drive_[i]->SetVelocity(vel); + right_drive_[i]->SetVelocity(vel); + } +} + +void ros_kinematics::RosDiffDrive::updateDynamicParams() +{ + // Retreive dynamic params: + if (kinematics_param_ptr_ != nullptr) + { + left_wheel_radius_multiplier_ = kinematics_param_ptr_->getLeftWheelRadiusMultiplier(); + right_wheel_radius_multiplier_ = kinematics_param_ptr_->getRightWheelRadiusMultiplier(); + wheel_separation_multiplier_ = kinematics_param_ptr_->getWheelSeparationMultiplier(); + publish_period_ = ros::Duration(1.0 / kinematics_param_ptr_->getPublishRate()); + // enable_odom_tf_ = kinematics_param_ptr_->getEnableOdomTf(); + } +} + +void ros_kinematics::RosDiffDrive::setOdomPubFields(ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh) +{ + // Get and check params for covariances + XmlRpc::XmlRpcValue pose_cov_list; + controller_nh.getParam("pose_covariance_diagonal", pose_cov_list); + ROS_ASSERT(pose_cov_list.getType() == XmlRpc::XmlRpcValue::TypeArray); + ROS_ASSERT(pose_cov_list.size() == 6); + for (int i = 0; i < pose_cov_list.size(); ++i) + ROS_ASSERT(pose_cov_list[i].getType() == XmlRpc::XmlRpcValue::TypeDouble); + + XmlRpc::XmlRpcValue twist_cov_list; + controller_nh.getParam("twist_covariance_diagonal", twist_cov_list); + ROS_ASSERT(twist_cov_list.getType() == XmlRpc::XmlRpcValue::TypeArray); + ROS_ASSERT(twist_cov_list.size() == 6); + for (int i = 0; i < twist_cov_list.size(); ++i) + ROS_ASSERT(twist_cov_list[i].getType() == XmlRpc::XmlRpcValue::TypeDouble); + + // Setup odometry realtime publisher + odom message constant fields + odom_pub_.reset(new realtime_tools::RealtimePublisher(controller_nh, "odom", 100)); + odom_pub_->msg_.header.frame_id = odom_frame_id_; + odom_pub_->msg_.child_frame_id = base_frame_id_; + odom_pub_->msg_.pose.pose.position.z = 0; + odom_pub_->msg_.pose.covariance = { + static_cast(pose_cov_list[0]), 0., 0., 0., 0., 0., + 0., static_cast(pose_cov_list[1]), 0., 0., 0., 0., + 0., 0., static_cast(pose_cov_list[2]), 0., 0., 0., + 0., 0., 0., static_cast(pose_cov_list[3]), 0., 0., + 0., 0., 0., 0., static_cast(pose_cov_list[4]), 0., + 0., 0., 0., 0., 0., static_cast(pose_cov_list[5]) }; + odom_pub_->msg_.twist.twist.linear.y = 0; + odom_pub_->msg_.twist.twist.linear.z = 0; + odom_pub_->msg_.twist.twist.angular.x = 0; + odom_pub_->msg_.twist.twist.angular.y = 0; + odom_pub_->msg_.twist.covariance = { + static_cast(twist_cov_list[0]), 0., 0., 0., 0., 0., + 0., static_cast(twist_cov_list[1]), 0., 0., 0., 0., + 0., 0., static_cast(twist_cov_list[2]), 0., 0., 0., + 0., 0., 0., static_cast(twist_cov_list[3]), 0., 0., + 0., 0., 0., 0., static_cast(twist_cov_list[4]), 0., + 0., 0., 0., 0., 0., static_cast(twist_cov_list[5]) }; + tf_odom_pub_.reset(new realtime_tools::RealtimePublisher(root_nh, "/tf", 100)); + tf_odom_pub_->msg_.transforms.resize(1); + tf_odom_pub_->msg_.transforms[0].transform.translation.z = 0.0; + tf_odom_pub_->msg_.transforms[0].child_frame_id = base_frame_id_; + tf_odom_pub_->msg_.transforms[0].header.frame_id = odom_frame_id_; +} + + +bool ros_kinematics::RosDiffDrive::getWheelNames(ros::NodeHandle& _nh, + const std::string& wheel_param, + std::vector& wheel_names) +{ + XmlRpc::XmlRpcValue wheel_list; + if (!nh_priv_.getParam(wheel_param, wheel_list)) + { + ROS_ERROR_STREAM_NAMED(name_, + "Couldn't retrieve wheel param '" << wheel_param << "'."); + return false; + } + + if (wheel_list.getType() == XmlRpc::XmlRpcValue::TypeArray) + { + if (wheel_list.size() == 0) + { + ROS_ERROR_STREAM_NAMED(name_, + "Wheel param '" << wheel_param << "' is an empty list"); + return false; + } + + for (int i = 0; i < wheel_list.size(); ++i) + { + if (wheel_list[i].getType() != XmlRpc::XmlRpcValue::TypeString) + { + ROS_ERROR_STREAM_NAMED(name_, + "Wheel param '" << wheel_param << "' #" << i << + " isn't a string."); + return false; + } + } + + wheel_names.resize(wheel_list.size()); + for (int i = 0; i < wheel_list.size(); ++i) + { + wheel_names[i] = static_cast(wheel_list[i]); + } + } + else if (wheel_list.getType() == XmlRpc::XmlRpcValue::TypeString) + { + wheel_names.push_back(wheel_list); + } + else + { + ROS_ERROR_STREAM_NAMED(name_, + "Wheel param '" << wheel_param << + "' is neither a list of strings nor a string."); + return false; + } + + return true; +} + + +bool ros_kinematics::RosDiffDrive::getWheelParams(ros::NodeHandle& controller_nh, + const std::vector& wheel_names, + std::map& drive_param_map) +{ + for (int i = 0; i < wheel_names.size(); ++i) + { + XmlRpc::XmlRpcValue element_param; + if (!nh_priv_.getParam(wheel_names[i], element_param)) + { + ROS_ERROR_STREAM_NAMED(name_, + "Couldn't retrieve wheel param '" << wheel_names[i] << "'."); + return false; + } + // check type of member + if(element_param.hasMember("lookup_name")) + { + if(element_param["lookup_name"].getType() != XmlRpc::XmlRpcValue::TypeString) + { + ROS_ERROR_STREAM_NAMED(name_, "'" << wheel_names[i] << "'->lookup_name" << " isn't a string."); + return false; + } + } + + if(element_param.hasMember("mesurement_topic")) + { + if(element_param["mesurement_topic"].getType() != XmlRpc::XmlRpcValue::TypeString) + { + ROS_ERROR_STREAM_NAMED(name_, "'" << wheel_names[i] << "'->mesurement_topic" << " isn't a string."); + return false; + } + } + + if(element_param.hasMember("frame_id")) + { + if(element_param["frame_id"].getType() != XmlRpc::XmlRpcValue::TypeString) + { + ROS_ERROR_STREAM_NAMED(name_, "'" << wheel_names[i] << "'->frame_id" << " isn't a string."); + return false; + } + } + + if(element_param.hasMember("wheel_topic")) + { + if(element_param["wheel_topic"].getType() != XmlRpc::XmlRpcValue::TypeString) + { + ROS_ERROR_STREAM_NAMED(name_, "'" << wheel_names[i] << "'->wheel_topic" << " isn't a string."); + return false; + } + } + + if(element_param.hasMember("max_publish_rate")) + { + if( element_param["max_publish_rate"].getType() != XmlRpc::XmlRpcValue::TypeDouble && + element_param["max_publish_rate"].getType() != XmlRpc::XmlRpcValue::TypeInt + ) + { + ROS_ERROR_STREAM_NAMED(name_, "'" << wheel_names[i] << "'->max_publish_rate" << " isn't a double/int."); + return false; + } + } + + if(element_param.hasMember("subscribe_queue_size")) + { + if( element_param["subscribe_queue_size"].getType() != XmlRpc::XmlRpcValue::TypeDouble && + element_param["subscribe_queue_size"].getType() != XmlRpc::XmlRpcValue::TypeInt + ) + { + ROS_ERROR_STREAM_NAMED(name_, "'" << wheel_names[i] << "'->subscribe_queue_size" << " isn't a double/int."); + return false; + } + } + + if(element_param.hasMember("command_timeout")) + { + if( element_param["command_timeout"].getType() != XmlRpc::XmlRpcValue::TypeDouble && + element_param["command_timeout"].getType() != XmlRpc::XmlRpcValue::TypeInt + ) + { + ROS_ERROR_STREAM_NAMED(name_, "'" << wheel_names[i] << "'->command_timeout" << " isn't a double/int."); + return false; + } + } + + if(!element_param.hasMember("origin")) + { + ROS_ERROR_STREAM_NAMED(name_, + "Couldn't retrieve wheel param '" << wheel_names[i] << "'->origin."); + return false; + } + else + { + if(element_param["origin"].getType() != XmlRpc::XmlRpcValue::TypeArray) + { + ROS_ERROR_STREAM_NAMED(name_, "'" << wheel_names[i] << "'->origin" << " isn't a arrays."); + return false; + } + else + { + if(element_param["origin"].size() < 6) // x y z roll pich yaw + { + ROS_ERROR_STREAM_NAMED(name_, + "Wheel param '" << wheel_names[i] << "'->origin is an error array"); + return false; + } + else + { + for (int j = 0; j < element_param["origin"].size(); ++j) + { + if( element_param["origin"][j].getType() != XmlRpc::XmlRpcValue::TypeDouble && + element_param["origin"][j].getType() != XmlRpc::XmlRpcValue::TypeInt + ) + { + ROS_ERROR_STREAM_NAMED(name_, + "Wheel param '" << wheel_names[i] << "'->origin #" << j << + " isn't a double/int."); + return false; + } + } + } + } + } + // Done + drive_param_map[wheel_names[i]] = element_param; + } + return true; +} + + +bool ros_kinematics::RosDiffDrive::allReady() +{ + bool result = true; + for (size_t i = 0; i < wheel_joints_size_; ++i) + { + result = result && left_drive_[i]->Ready() && right_drive_[i]->Ready(); + } + return result; +} + diff --git a/Devices/Packages/ros_kinematics/src/ros_diff_drive_parameters.cpp b/Devices/Packages/ros_kinematics/src/ros_diff_drive_parameters.cpp new file mode 100755 index 0000000..cee9130 --- /dev/null +++ b/Devices/Packages/ros_kinematics/src/ros_diff_drive_parameters.cpp @@ -0,0 +1,117 @@ +#include +#include +#include +#include + +namespace ros_kinematics +{ + + KinematicDiffDriveParameters::KinematicDiffDriveParameters() + : left_wheel_radius_multiplier_(0.0), + right_wheel_radius_multiplier_(0.0), + wheel_separation_multiplier_(0.0), + publish_rate_(0.0), + enable_odom_tf_(false), + setup_(false), + dsrv_(nullptr) + { + } + + KinematicDiffDriveParameters::KinematicDiffDriveParameters(const ros::NodeHandle& nh) + : left_wheel_radius_multiplier_(0.0), + right_wheel_radius_multiplier_(0.0), + wheel_separation_multiplier_(0.0), + publish_rate_(0.0), + enable_odom_tf_(false), + setup_(false), + dsrv_(nullptr) + { + initialize(nh); + } + + void KinematicDiffDriveParameters::initialize(const ros::NodeHandle& nh) + { + if (!initialized_) + { + nh_ = ros::NodeHandle(nh); + // the rest of the initial values are loaded through the dynamic reconfigure mechanisms + dsrv_ = std::make_shared >(nh_); + dynamic_reconfigure::Server::CallbackType cb = + boost::bind(&KinematicDiffDriveParameters::reconfigureCB, this, _1, _2); + dsrv_->setCallback(cb); + int m_subscribe_queue_size = 1000; + kinematics_sub_ = nh_.subscribe("/agv_dynparam/parameter_updates", m_subscribe_queue_size, &KinematicDiffDriveParameters::kinematicsCallback, this); + initialized_ = true; + } + } + + void KinematicDiffDriveParameters::reconfigureCB(DiffDriveParametersConfig& config, uint32_t level) + { + boost::lock_guard schedule_lockguard(reconfig_mutex); + // ROS_WARN("reconfigureCB"); + left_wheel_radius_multiplier_ = config.left_wheel_radius_multiplier; + right_wheel_radius_multiplier_ = config.right_wheel_radius_multiplier; + wheel_separation_multiplier_ = config.wheel_separation_multiplier; + publish_rate_ = config.publish_rate; + enable_odom_tf_ = config.enable_odom_tf; + // reconfig_mutex.unlock(); + } + + void KinematicDiffDriveParameters::kinematicsCallback(const dynamic_reconfigure::Config::ConstPtr& param) + { + boost::lock_guard schedule_lockguard(reconfig_mutex); + // ROS_WARN("dynamic_reconfigure"); + if (param != nullptr) + { + std::map param_m; + for (auto db : param->doubles) param_m[db.name] = db.value; + + auto it = param_m.find("left_wheel_radius_multiplier"); + if (it != param_m.end()) + { + left_wheel_radius_multiplier_ = param_m["left_wheel_radius_multiplier"]; + ROS_INFO("[ros_kinematics]-left_wheel_radius_multiplier %f", left_wheel_radius_multiplier_); + } + else ROS_WARN("[ros_kinematics]-left_wheel_radius_multiplier %f", left_wheel_radius_multiplier_); + + it = param_m.find("right_wheel_radius_multiplier"); + if (it != param_m.end()) + { + right_wheel_radius_multiplier_ = param_m["right_wheel_radius_multiplier"]; + ROS_INFO("[ros_kinematics]-right_wheel_radius_multiplier %f", right_wheel_radius_multiplier_); + } + else ROS_WARN("[ros_kinematics]-right_wheel_radius_multiplier %f", right_wheel_radius_multiplier_); + + it = param_m.find("wheel_separation_multiplier"); + if (it != param_m.end()) + { + wheel_separation_multiplier_ = param_m["wheel_separation_multiplier"]; + ROS_INFO("[ros_kinematics]-wheel_separation_multiplier %f", wheel_separation_multiplier_); + } + else ROS_WARN("[ros_kinematics]-wheel_separation_multiplier %f", wheel_separation_multiplier_); + + it = param_m.find("publish_rate"); + if (it != param_m.end()) + { + publish_rate_ = param_m["publish_rate"]; + ROS_INFO("[ros_kinematics]-publish_rate %f", publish_rate_); + } + else ROS_WARN("[ros_kinematics]-publish_rate %f", publish_rate_); + + // it = param_m.find("enable_odom_tf"); + // if (it != param_m.end()) + // { + // enable_odom_tf_ = param_m["enable_odom_tf"]; + // ROS_INFO("[ros_kinematics]-enable_odom_tf %x", enable_odom_tf_); + // } + // else ROS_WARN("[ros_kinematics]-enable_odom_tf %x", enable_odom_tf_); + + param_m.clear(); + setup_ = true; + } + else ROS_WARN("dynamic_reconfigure param is null"); + // reconfig_mutex.unlock(); + } + + +} // namespace ros_kinematics diff --git a/Devices/Packages/ros_kinematics/src/ros_kinematics_node.cpp b/Devices/Packages/ros_kinematics/src/ros_kinematics_node.cpp new file mode 100755 index 0000000..1ef4245 --- /dev/null +++ b/Devices/Packages/ros_kinematics/src/ros_kinematics_node.cpp @@ -0,0 +1,33 @@ +#include +#include +#include "ros_kinematics/base_drive.h" +#include "ros_kinematics/ros_steer_drive.h" +#include "ros_kinematics/ros_diff_drive.h" + +int main(int argc, char **argv) +{ + /* Khoi tao Node */ + ros::init(argc, argv, "ros_kinematics"); + ros::NodeHandle nh; + int type; + nh.param(ros::this_node::getName()+"/type", type, 1); + /* Constructors */ + boost::shared_ptr ros_drive = nullptr; + switch (type) + { + case 1: + ros_drive = boost::make_shared(nh, ros::this_node::getName()); + break; + + case 2: + ros_drive = boost::make_shared(nh, ros::this_node::getName()); + break; + + default: + ros_drive = boost::make_shared(nh, ros::this_node::getName()); + break; + } + ros::spin(); + ros_drive = nullptr; + return 0; +} diff --git a/Devices/Packages/ros_kinematics/src/ros_steer_drive.cpp b/Devices/Packages/ros_kinematics/src/ros_steer_drive.cpp new file mode 100755 index 0000000..7ac2bf6 --- /dev/null +++ b/Devices/Packages/ros_kinematics/src/ros_steer_drive.cpp @@ -0,0 +1,447 @@ +#include "ros_kinematics/ros_steer_drive.h" +#include +#include +#include + +ros_kinematics::RosSteerDrive::RosSteerDrive(ros::NodeHandle & nh, const std::string &name) + : nh_(nh), + motor_loader_("models", "models::BaseSteerDrive"), + encoder_loader_("models", "models::BaseAbsoluteEncoder"), + last_odom_update_(ros::Time(0)), + last_actuator_update_(ros::Time(0)), + publish_time_(ros::Time(0)), + publish_lastest_time_(ros::Time(0)) +{ + + nh_priv_ = ros::NodeHandle(nh_, name); + ROS_INFO("RosSteerDrive get node name is %s", name.c_str()); + + bool init_odom_enc; + nh_priv_.param("initialize_odom_enc", init_odom_enc, true); + odom_enc_initialized_ = !init_odom_enc; + + double schedule_delay, schedule_lastest_delay; + nh_priv_.param("commandTopic", command_topic_, std::string("cmd_vel")); + nh_priv_.param("publishOdomTF", publishOdomTF_, false); + nh_priv_.param("odometryTopic", odometry_topic_, std::string("odom")); + nh_priv_.param("odometryFrame", odometry_frame_, std::string("odom")); + nh_priv_.param("odometryEncTopic", odometry_enc_topic_, std::string("odom_enc")); + nh_priv_.param("odometryEncChildFrame", odometry_enc_child_frame_, std::string("odom_enc")); + nh_priv_.param("robotBaseFrame", robot_base_frame_, std::string("base_link")); + nh_priv_.param("steerChildFrame", steer_id_, std::string("steer_link")); + + nh_priv_.param("subscribe_queue_size", m_subscribe_queue_size_, 1); + nh_priv_.param("max_update_rate", update_rate_, 100.0); + nh_priv_.param("schedule_delay", schedule_delay, 0.005); + nh_priv_.param("schedule_lastest_delay", schedule_lastest_delay, 0.5); + nh_priv_.param("odomEncSteeringAngleOffset",odom_enc_steering_angle_offset_, 0.); + nh_priv_.param("wheelAcceleration", wheel_acceleration_, 0.); + nh_priv_.param("steering_fix_wheel_distance_x", steering_fix_wheel_distance_x_, 0.68); + nh_priv_.param("steering_fix_wheel_distance_y", steering_fix_wheel_distance_y_, 0.); + + + std::string steer_drive; + nh_priv_.param("steer_drive", steer_drive, std::string("motor_wheel::SteerMotor")); + try + { + steer_motor_ = motor_loader_.createInstance(steer_drive); + steer_motor_->initialize(nh_, name + "/" + motor_loader_.getName(steer_drive)); + ros::NodeHandle nh(nh_priv_, motor_loader_.getName(steer_drive)); + nh.param("method", steer_motor_mode_, std::string("")); + } + catch(const pluginlib::PluginlibException &ex) + { + ROS_FATAL("Failed to create the %s model, are you sure it is properly registered and that the containing library is built? Exception: %s", steer_drive.c_str(), ex.what()); + exit(1); + } + + std::string traction_drive; + nh_priv_.param("traction_drive", traction_drive, std::string("motor_wheel::TractionMotor")); + nh_priv_.param("wheel_diameter", wheel_diameter_, 0.210); + try + { + traction_motor_ = motor_loader_.createInstance(traction_drive); + traction_motor_->initialize(nh_, name + "/" + motor_loader_.getName(traction_drive)); + ros::NodeHandle nh(nh_priv_, motor_loader_.getName(traction_drive)); + nh.param("method", traction_motor_mode_, std::string("")); + } + catch(const pluginlib::PluginlibException &ex) + { + ROS_FATAL("Failed to create the %s model, are you sure it is properly registered and that the containing library is built? Exception: %s", traction_drive.c_str(), ex.what()); + exit(1); + } + + nh_.param(name + "/use_encoder", use_abs_encoder, false); + if(use_abs_encoder) + { + std::string abs_encoder; + nh_priv_.param("absolute_encoder", abs_encoder, std::string("absolute_encoder::Measurements")); + try + { + absolute_encoder_ = encoder_loader_.createInstance(abs_encoder); + absolute_encoder_->initialize(nh, "AbsoluteEncoder"); + } + catch(const pluginlib::PluginlibException &ex) + { + ROS_FATAL("Failed to create the %s model, are you sure it is properly registered and that the containing library is built? Exception: %s", abs_encoder.c_str(), ex.what()); + exit(1); + } + } + + transform_broadcaster_ = boost::shared_ptr(new tf::TransformBroadcaster()); + odometry_publisher_ = nh_.advertise(odometry_topic_, 1); + odometry_enc_publisher_ = nh_.advertise(odometry_enc_topic_, 1); + cmd_vel_subscriber_ = nh_.subscribe(command_topic_, m_subscribe_queue_size_, &ros_kinematics::RosSteerDrive::CmdVelCallback, this); + cmd_vel_feedback_ = nh_.advertise("cmd_vel_feedback", 1); + cmd_vel_rb = nh_.advertise("cmd_vel_rb", 1); + steer_angle_pub_ = nh_.advertise("steer_angle", 1); + + // Initialize update rate stuff + if (this->update_rate_ > 0.0) + this->update_period_ = 1.0 / this->update_rate_; + else + this->update_period_ = 0.0; + + schedule_delay_ = ros::Duration(schedule_delay); + schedule_lastest_delay_ = ros::Duration(schedule_lastest_delay); + + // Kinematics + kinematics_param_ptr = boost::make_shared(); + kinematics_param_ptr->initialize(nh_); + + // Thread + callback_thread_ = new boost::thread(&ros_kinematics::RosSteerDrive::UpdateChild,this); +} + +ros_kinematics::RosSteerDrive::~RosSteerDrive() +{ + if (callback_thread_) + { + callback_thread_->join(); + delete (callback_thread_); + callback_thread_ = 0; + } +} + +void ros_kinematics::RosSteerDrive::CmdVelCallback(const geometry_msgs::Twist::ConstPtr &cmd_msg) +{ + boost::lock_guard schedule_lockguard(callback_mutex_); + command_struct_.linear.x = cmd_msg->linear.x; + command_struct_.angular.z = cmd_msg->angular.z; + scheduleMotorController(true); +} + +/** + * @brief returns true, if publishing of a cmd_vel topic is publishing +*/ +bool ros_kinematics::RosSteerDrive::isCmdVelTriggered(void) +{ + boost::lock_guard schedule_lockguard(publish_mutex_); + return !publish_time_.isZero() && ros::Time::now() > publish_time_; +} + +/** + * @brief returns true, if publishing of a cmd_vel topic is publishing +*/ +bool ros_kinematics::RosSteerDrive::isCmdVelLastestTriggered(void) +{ + boost::lock_guard schedule_lockguard(publish_mutex_); + return !publish_lastest_time_.isZero() && ros::Time::now() > publish_lastest_time_; +} + +/** + * @brief schedules MotorController function + * @param[in] schedule if true, publishing is scheduled, otherwise a possibly pending schedule is removed. +*/ +void ros_kinematics::RosSteerDrive::scheduleMotorController(bool schedule) +{ + boost::lock_guard schedule_lockguard(publish_mutex_); + if(schedule && publish_time_.isZero()) + { + publish_time_ = ros::Time::now() + schedule_delay_; + publish_lastest_time_ = ros::Time::now() + schedule_lastest_delay_; + } + if(!schedule && !publish_time_.isZero()) + { + publish_time_ = ros::Time(0); + // publish_lastest_time_ = ros::Time(0); + } +} + +void ros_kinematics::RosSteerDrive::MotorController(double target_speed, double target_steering_speed, double dt) +{ + // TODO add the accelerations etc. properly + double applied_speed = target_speed; + double applied_steering_speed = target_steering_speed; + + if(traction_motor_->Ready() && steer_motor_->Ready()) + { + double current_speed = traction_motor_->GetVelocity(); + double current_steering_speed = steer_motor_->GetVelocity(); + + if(kinematics_param_ptr->isSetup()) + { + wheel_acceleration_ = kinematics_param_ptr->getWheelAcceleration(); + } + + if (wheel_acceleration_ > 0) + { + // TODO + // applied_speed = ...; + // applied_steering_speed = ...; + } + if(traction_motor_mode_ == std::string("speed_mode")) + traction_motor_->SetVelocity(applied_speed); + + if(steer_motor_mode_ == std::string("position_mode")) + steer_motor_->SetPosition(applied_steering_speed); + else if(steer_motor_mode_ == std::string("speed_mode")) + steer_motor_->SetVelocity(applied_steering_speed); + } + if(!traction_motor_->Ready()) + ROS_WARN_THROTTLE_NAMED(2.0, "ros_kinematics", "traction_motor_ is not Ready."); + if(!steer_motor_->Ready()) + ROS_WARN_THROTTLE_NAMED(2.0, "ros_kinematics", "steer_motor_ is not Ready."); +} + +void ros_kinematics::RosSteerDrive::publishSteerJoint(double odom_alpha) +{ + ros::Time current_time = ros::Time::now(); + tf::Quaternion qt; + tf::Vector3 vt; + qt.setRPY(0, 0, odom_alpha); + if(kinematics_param_ptr->isSetup()) + { + steering_fix_wheel_distance_x_ = kinematics_param_ptr->getSteeringFixWheelDistanceX(); + steering_fix_wheel_distance_y_ = kinematics_param_ptr->getSteeringFixWheelDistanceY(); + } + vt = tf::Vector3(steering_fix_wheel_distance_x_, steering_fix_wheel_distance_y_, 0); + tf::Transform base_to_steer_link(qt, vt); + transform_broadcaster_->sendTransform( tf::StampedTransform(base_to_steer_link, current_time, + robot_base_frame_, steer_id_)); +} + +void ros_kinematics::RosSteerDrive::UpdateOdometryEncoder() +{ + ros::Time current_time = ros::Time::now(); + double step_time = ros::Duration(current_time - last_odom_update_).toSec(); + last_odom_update_ = current_time; + + double odom_alpha; + if(use_abs_encoder) + odom_alpha = absolute_encoder_->Position(); + else + odom_alpha = steer_motor_->Position(); + + if(kinematics_param_ptr->isSetup()) + { + odom_enc_steering_angle_offset_= kinematics_param_ptr->getOdomEncSteeringAngleOffset(); + // ROS_WARN("odom_enc_steering_angle_offset_ %f", odom_enc_steering_angle_offset_); + } + odom_alpha += odom_enc_steering_angle_offset_; + + std_msgs::Float32 msg; + msg.data = odom_alpha/M_PI*180; + steer_angle_pub_.publish(msg); + + publishSteerJoint(odom_alpha); + // ROS_WARN_STREAM("Odom alpha " << odom_alpha << " "<< odom_alpha / M_PI *180); + // Distance travelled drive wheel + double drive_dist = traction_motor_->GetVelocity() * (wheel_diameter_ / 2) * step_time; + + double dd = 0.; + double da = 0.; + + if (fabs(odom_alpha) < 0.000001) // Avoid dividing with a very small number... + { + dd = drive_dist; + da = 0.; + } + else + { + if(kinematics_param_ptr->isSetup()) + { + steering_fix_wheel_distance_x_ = kinematics_param_ptr->getSteeringFixWheelDistanceX(); + steering_fix_wheel_distance_y_ = kinematics_param_ptr->getSteeringFixWheelDistanceY(); + } + double r_stear = steering_fix_wheel_distance_x_ / sin(odom_alpha); + double r_fix = r_stear * cos(odom_alpha) - steering_fix_wheel_distance_y_; + + dd = r_fix / r_stear * drive_dist; // Adjust the actual forward movement (located between the fixed front wheels) based on the radius of the drive wheel). + da = drive_dist / r_stear; + } + + // Update the current estimate + double dx = dd * cos(pose_encoder_.theta + da / 2.); + double dy = dd * sin(pose_encoder_.theta + da / 2.); + + // Compute odometric pose + pose_encoder_.x += dx; + pose_encoder_.y += dy; + pose_encoder_.theta += da; + + double w = da / step_time; + + double v = dd / step_time; + geometry_msgs::Twist fb; + fb.linear.x = v; + fb.angular.z = w; + cmd_vel_feedback_.publish(fb); + + tf::Quaternion qt; + tf::Vector3 vt; + qt.setRPY(0, 0, pose_encoder_.theta); + vt = tf::Vector3(pose_encoder_.x, pose_encoder_.y, 0); + + odom_enc_.pose.pose.position.x = vt.x(); + odom_enc_.pose.pose.position.y = vt.y(); + odom_enc_.pose.pose.position.z = vt.z(); + + odom_enc_.pose.pose.orientation.x = qt.x(); + odom_enc_.pose.pose.orientation.y = qt.y(); + odom_enc_.pose.pose.orientation.z = qt.z(); + odom_enc_.pose.pose.orientation.w = qt.w(); + + odom_enc_.twist.twist.angular.z = w; + odom_enc_.twist.twist.linear.x = v; + odom_enc_.twist.twist.linear.y = 0.0; + +} + +void ros_kinematics::RosSteerDrive::PublishOdometry(double step_time) +{ + // getting data form encoder integration + odom_ = odom_enc_; + ros::Time current_time = ros::Time::now(); + std::string odom_frame = odometry_frame_; + std::string base_footprint_frame = robot_base_frame_; + tf::Quaternion qt; + tf::Vector3 vt; + if(publishOdomTF_) + { + qt = tf::Quaternion(odom_.pose.pose.orientation.x, odom_.pose.pose.orientation.y, odom_.pose.pose.orientation.z, odom_.pose.pose.orientation.w); + vt = tf::Vector3(odom_.pose.pose.position.x, odom_.pose.pose.position.y, odom_.pose.pose.position.z); + tf::Transform base_footprint_to_odom(qt, vt); + transform_broadcaster_->sendTransform( + tf::StampedTransform(base_footprint_to_odom, current_time, + odom_frame, base_footprint_frame)); + } + // set covariance - TODO, fix this(!) + odom_.pose.covariance[0] = 0.00001; + odom_.pose.covariance[7] = 0.00001; + odom_.pose.covariance[14] = 1000000000000.0; + odom_.pose.covariance[21] = 1000000000000.0; + odom_.pose.covariance[28] = 1000000000000.0; + odom_.pose.covariance[35] = 0.001; + + // set header + odom_.header.stamp = current_time; + odom_.header.frame_id = odom_frame; + odom_.child_frame_id = base_footprint_frame; + + odometry_publisher_.publish(odom_); + + // publish the encoder based odometry + { + if (!odom_enc_initialized_) + { + pose_encoder_.x = odom_.pose.pose.position.x; + pose_encoder_.y = odom_.pose.pose.position.y; + pose_encoder_.theta = tf::getYaw(odom_.pose.pose.orientation); + odom_enc_initialized_ = true; + + odom_enc_ = odom_; + } + std::string odom_enc_child_frame = odometry_enc_child_frame_; + if(publishOdomTF_) + { + qt = tf::Quaternion(odom_enc_.pose.pose.orientation.x, odom_enc_.pose.pose.orientation.y, odom_enc_.pose.pose.orientation.z, odom_enc_.pose.pose.orientation.w); + vt = tf::Vector3(odom_enc_.pose.pose.position.x, odom_enc_.pose.pose.position.y, odom_enc_.pose.pose.position.z); + + tf::Transform odom_enc_child_to_odom(qt, vt); + transform_broadcaster_->sendTransform( + tf::StampedTransform(odom_enc_child_to_odom, current_time, + odom_frame, odom_enc_child_frame)); + } + odom_enc_.pose.covariance = odom_.pose.covariance; // TODO... + odom_enc_.header.stamp = current_time; + odom_enc_.header.frame_id = odom_frame; // Note - this is typically /world + odom_enc_.child_frame_id = odom_enc_child_frame; + odometry_enc_publisher_.publish(odom_enc_); + } +} + +void ros_kinematics::RosSteerDrive::UpdateChild(void) +{ + ros::Rate rate(update_rate_); + bool stopped; + while (ros::ok()) + { + if(traction_motor_->Ready() && steer_motor_->Ready()) + { + UpdateOdometryEncoder(); + } + ros::Time current_time = ros::Time::now(); + double seconds_since_last_update = ros::Duration(current_time - last_actuator_update_).toSec(); + + if (seconds_since_last_update > update_period_) + { + if(traction_motor_->Ready() && steer_motor_->Ready()) + { + PublishOdometry(seconds_since_last_update); + } + // if (publishWheelTF_) + // publishWheelTF(); + // if (publishWheelJointState_) + // publishWheelJointState(); + double target_steering_angle_speed_saved_; + if(isCmdVelTriggered()) + { + double target_wheel_rotation_speed = command_struct_.linear.x / (wheel_diameter_ / 2.0); + double target_steering_angle_speed = command_struct_.angular.z; + double odom_alpha; + if(use_abs_encoder) + odom_alpha = absolute_encoder_->Position(); + else + odom_alpha = steer_motor_->Position(); + + if(kinematics_param_ptr->isSetup()) + { + odom_enc_steering_angle_offset_= kinematics_param_ptr->getOdomEncSteeringAngleOffset(); + } + odom_alpha += odom_enc_steering_angle_offset_; + + geometry_msgs::Twist fb; + + fb.linear.x = command_struct_.linear.x * fabs(cos(odom_alpha)); + + cmd_vel_rb.publish(fb); + + MotorController(target_wheel_rotation_speed, target_steering_angle_speed, seconds_since_last_update); + target_steering_angle_speed_saved_ = target_steering_angle_speed; + scheduleMotorController(false); + stopped = false; + } + if(isCmdVelLastestTriggered() && !stopped) + { + if(steer_motor_mode_ == std::string("position_mode")) + { + MotorController(0, target_steering_angle_speed_saved_, seconds_since_last_update); + MotorController(0, target_steering_angle_speed_saved_, seconds_since_last_update); + } + else + { + MotorController(0, 0, seconds_since_last_update); + MotorController(0, 0, seconds_since_last_update); + } + stopped = true; + } + last_actuator_update_ = current_time; + } + rate.sleep(); + ros::spinOnce(); + } +} + + + diff --git a/Devices/Packages/ros_kinematics/src/ros_steer_drive_parameters.cpp b/Devices/Packages/ros_kinematics/src/ros_steer_drive_parameters.cpp new file mode 100755 index 0000000..89dd322 --- /dev/null +++ b/Devices/Packages/ros_kinematics/src/ros_steer_drive_parameters.cpp @@ -0,0 +1,106 @@ +#include +#include +#include +#include + +namespace ros_kinematics +{ + + KinematicSteerDriveParameters::KinematicSteerDriveParameters() + : odom_enc_steering_angle_offset_(0.0), + steering_fix_wheel_distance_x_(0.0), + steering_fix_wheel_distance_y_(0.0), + wheel_acceleration_(0.0), + setup_(false), + dsrv_(nullptr) + { + } + + KinematicSteerDriveParameters::KinematicSteerDriveParameters(const ros::NodeHandle& nh) + : odom_enc_steering_angle_offset_(0.0), + steering_fix_wheel_distance_x_(0.0), + steering_fix_wheel_distance_y_(0.0), + wheel_acceleration_(0.0), + setup_(false), + dsrv_(nullptr) + { + initialize(nh); + } + + void KinematicSteerDriveParameters::initialize(const ros::NodeHandle& nh) + { + if (!initialized_) + { + nh_ = ros::NodeHandle(nh); + // the rest of the initial values are loaded through the dynamic reconfigure mechanisms + dsrv_ = std::make_shared >(nh_); + dynamic_reconfigure::Server::CallbackType cb = + boost::bind(&KinematicSteerDriveParameters::reconfigureCB, this, _1, _2); + dsrv_->setCallback(cb); + int m_subscribe_queue_size = 1000; + kinematics_sub_ = nh_.subscribe("/agv_dynparam/parameter_updates", m_subscribe_queue_size, &KinematicSteerDriveParameters::kinematicsCallback, this); + initialized_ = true; + } + } + + void KinematicSteerDriveParameters::reconfigureCB(SteerDriveParametersConfig& config, uint32_t level) + { + boost::lock_guard schedule_lockguard(reconfig_mutex); + // ROS_WARN("reconfigureCB"); + odom_enc_steering_angle_offset_ = config.odomEncSteeringAngleOffset; + steering_fix_wheel_distance_x_ = config.steering_fix_wheel_distance_x; + steering_fix_wheel_distance_y_ = config.steering_fix_wheel_distance_y; + wheel_acceleration_ = config.wheelAcceleration; + // reconfig_mutex.unlock(); + } + + void KinematicSteerDriveParameters::kinematicsCallback(const dynamic_reconfigure::Config::ConstPtr& param) + { + boost::lock_guard schedule_lockguard(reconfig_mutex); + // ROS_WARN("dynamic_reconfigure"); + if (param != nullptr) + { + std::map param_m; + for (auto db : param->doubles) param_m[db.name] = db.value; + + auto it = param_m.find("odomEncSteeringAngleOffset"); + if (it != param_m.end()) + { + odom_enc_steering_angle_offset_ = param_m["odomEncSteeringAngleOffset"]; + ROS_INFO("[ros_kinematics]-odomEncSteeringAngleOffset %f", odom_enc_steering_angle_offset_); + } + else ROS_WARN("[ros_kinematics]-odomEncSteeringAngleOffset %f", odom_enc_steering_angle_offset_); + + it = param_m.find("steering_fix_wheel_distance_x"); + if (it != param_m.end()) + { + steering_fix_wheel_distance_x_ = param_m["steering_fix_wheel_distance_x"]; + ROS_INFO("[ros_kinematics]-steering_fix_wheel_distance_x %f", steering_fix_wheel_distance_x_); + } + else ROS_WARN("[ros_kinematics]-steering_fix_wheel_distance_x %f", steering_fix_wheel_distance_x_); + + it = param_m.find("steering_fix_wheel_distance_y"); + if (it != param_m.end()) + { + steering_fix_wheel_distance_y_ = param_m["steering_fix_wheel_distance_y"]; + ROS_INFO("[ros_kinematics]-steering_fix_wheel_distance_y %f", steering_fix_wheel_distance_y_); + } + else ROS_WARN("[ros_kinematics]-steering_fix_wheel_distance_y %f", steering_fix_wheel_distance_y_); + + it = param_m.find("wheelAcceleration"); + if (it != param_m.end()) + { + wheel_acceleration_ = param_m["wheelAcceleration"]; + ROS_INFO("[ros_kinematics]-wheelAcceleration %f", wheel_acceleration_); + } + else ROS_WARN("[ros_kinematics]-wheelAcceleration %f", wheel_acceleration_); + + param_m.clear(); + setup_ = true; + } + else ROS_WARN("dynamic_reconfigure param is null"); + // reconfig_mutex.unlock(); + } + + +} // namespace ros_kinematics diff --git a/Devices/Packages/ros_kinematics/src/speed_limiter.cpp b/Devices/Packages/ros_kinematics/src/speed_limiter.cpp new file mode 100755 index 0000000..63be132 --- /dev/null +++ b/Devices/Packages/ros_kinematics/src/speed_limiter.cpp @@ -0,0 +1,137 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, PAL Robotics, S.L. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the PAL Robotics nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* + * Author: Enrique Fernández + */ + +#include + +#include + +template +T clamp(T x, T min, T max) +{ + return std::min(std::max(min, x), max); +} + +namespace ros_kinematics +{ + + SpeedLimiter::SpeedLimiter( + bool has_velocity_limits, + bool has_acceleration_limits, + bool has_jerk_limits, + double min_velocity, + double max_velocity, + double min_acceleration, + double max_acceleration, + double min_jerk, + double max_jerk + ) + : has_velocity_limits(has_velocity_limits) + , has_acceleration_limits(has_acceleration_limits) + , has_jerk_limits(has_jerk_limits) + , min_velocity(min_velocity) + , max_velocity(max_velocity) + , min_acceleration(min_acceleration) + , max_acceleration(max_acceleration) + , min_jerk(min_jerk) + , max_jerk(max_jerk) + { + } + + double SpeedLimiter::limit(double& v, double v0, double v1, double dt) + { + const double tmp = v; + + limit_jerk(v, v0, v1, dt); + limit_acceleration(v, v0, dt); + limit_velocity(v); + + return tmp != 0.0 ? v / tmp : 1.0; + } + + double SpeedLimiter::limit_velocity(double& v) + { + const double tmp = v; + + if (has_velocity_limits) + { + v = clamp(v, min_velocity, max_velocity); + } + + return tmp != 0.0 ? v / tmp : 1.0; + } + + double SpeedLimiter::limit_acceleration(double& v, double v0, double dt) + { + const double tmp = v; + + if (has_acceleration_limits) + { + const double dv_min = min_acceleration * dt; + const double dv_max = max_acceleration * dt; + + const double dv = clamp(v - v0, dv_min, dv_max); + + v = v0 + dv; + } + + return tmp != 0.0 ? v / tmp : 1.0; + } + + double SpeedLimiter::limit_jerk(double& v, double v0, double v1, double dt) + { + const double tmp = v; + + if (has_jerk_limits) + { + const double dv = v - v0; + const double dv0 = v0 - v1; + + const double dt2 = 2. * dt * dt; + + const double da_min = min_jerk * dt2; + const double da_max = max_jerk * dt2; + + const double da = clamp(dv - dv0, da_min, da_max); + + v = v0 + dv0 + da; + } + + return tmp != 0.0 ? v / tmp : 1.0; + } + +} // namespace ros_kinematics diff --git a/Devices/Packages/sick_line_guidance/.gitignore b/Devices/Packages/sick_line_guidance/.gitignore new file mode 100755 index 0000000..7e2383c --- /dev/null +++ b/Devices/Packages/sick_line_guidance/.gitignore @@ -0,0 +1,10 @@ + +turtlebotDemo/test/scripts/sick_line_guidance_demo\.avi + +turtlebotDemo/iam/cmake/decision_making_parsing\.cmake + +turtlebotDemo/test/scripts/log/ + +\.idea/ + +cmake-build-debug/ diff --git a/Devices/Packages/sick_line_guidance/CMakeLists.txt b/Devices/Packages/sick_line_guidance/CMakeLists.txt new file mode 100755 index 0000000..de0774d --- /dev/null +++ b/Devices/Packages/sick_line_guidance/CMakeLists.txt @@ -0,0 +1,332 @@ +cmake_minimum_required(VERSION 2.8.3) +project(sick_line_guidance) + +## +## Compile as C++11, supported in ROS Kinetic and newer +add_compile_options(-std=c++11 -g -Wall -Wno-reorder -Wno-sign-compare -Wno-unused-local-typedefs) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + cmake_modules + can_msgs + canopen_chain_node + message_generation + random_numbers + roscpp + rospy + sensor_msgs + std_msgs +) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) +find_package(TinyXML REQUIRED) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a exec_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +add_message_files( + FILES + MLS_Measurement.msg + OLS_Measurement.msg +) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +generate_messages( + DEPENDENCIES + sensor_msgs + std_msgs + can_msgs +) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( + INCLUDE_DIRS include + LIBRARIES sick_line_guidance_lib + CATKIN_DEPENDS can_msgs canopen_chain_node message_runtime random_numbers roscpp rospy sensor_msgs std_msgs + DEPENDS TinyXML # system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${TinyXML_INCLUDE_DIRS} +) + +## Declare a C++ library +add_library(${PROJECT_NAME}_lib + src/sick_line_guidance_can_subscriber.cpp + src/sick_line_guidance_can_cia401_subscriber.cpp + src/sick_line_guidance_can_mls_subscriber.cpp + src/sick_line_guidance_can_ols_subscriber.cpp + src/sick_line_guidance_canopen_chain.cpp + src/sick_line_guidance_cloud_converter.cpp + src/sick_line_guidance_diagnostic.cpp + src/sick_line_guidance_eds_util.cpp + src/sick_line_guidance_msg_util.cpp + src/sick_canopen_simu_canstate.cpp + src/sick_canopen_simu_device.cpp + src/sick_canopen_simu_verify.cpp +) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +## With catkin_make all packages are built within a single CMake context +## The recommended prefix ensures that target names across packages don't collide +add_executable(${PROJECT_NAME}_node src/sick_line_guidance_node.cpp) +add_executable(${PROJECT_NAME}_can_chain_node src/sick_line_guidance_can_chain_node.cpp) +add_executable(${PROJECT_NAME}_cloud_publisher src/sick_line_guidance_cloud_publisher.cpp) +add_executable(${PROJECT_NAME}_can2ros_node src/sick_line_guidance_can2ros_node.cpp) +add_executable(${PROJECT_NAME}_ros2can_node src/sick_line_guidance_ros2can_node.cpp) +add_executable(sick_canopen_simu_node src/sick_canopen_simu_node.cpp) + +## Rename C++ executable without prefix +## The above recommended prefix causes long target names, the following renames the +## target back to the shorter version for ease of user use +## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" +# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") + +## Add cmake target dependencies of the executable +## same as for the library above +add_dependencies(${PROJECT_NAME}_node + random_numbers + canopen_ros_chain + ${PROJECT_NAME}_lib + ${${PROJECT_NAME}_EXPORTED_TARGETS} + ${catkin_EXPORTED_TARGETS} +) +add_dependencies(${PROJECT_NAME}_can_chain_node + canopen_ros_chain + ${PROJECT_NAME}_lib + ${${PROJECT_NAME}_EXPORTED_TARGETS} + ${catkin_EXPORTED_TARGETS} +) +add_dependencies(${PROJECT_NAME}_cloud_publisher + ${PROJECT_NAME}_lib + ${${PROJECT_NAME}_EXPORTED_TARGETS} + ${catkin_EXPORTED_TARGETS} +) +add_dependencies(${PROJECT_NAME}_can2ros_node + canopen_ros_chain + ${PROJECT_NAME}_lib + ${${PROJECT_NAME}_EXPORTED_TARGETS} + ${catkin_EXPORTED_TARGETS} +) +add_dependencies(${PROJECT_NAME}_ros2can_node + canopen_ros_chain + ${PROJECT_NAME}_lib + ${${PROJECT_NAME}_EXPORTED_TARGETS} + ${catkin_EXPORTED_TARGETS} +) +add_dependencies(${PROJECT_NAME}_lib + ${${PROJECT_NAME}_EXPORTED_TARGETS} + ${catkin_EXPORTED_TARGETS} +) +add_dependencies(sick_canopen_simu_node + random_numbers + canopen_ros_chain + ${PROJECT_NAME}_lib + ${${PROJECT_NAME}_EXPORTED_TARGETS} + ${catkin_EXPORTED_TARGETS} +) + +## Specify libraries to link a library or executable target against +target_link_libraries(${PROJECT_NAME}_node + canopen_ros_chain + random_numbers + ${PROJECT_NAME}_lib + ${Boost_LIBRARIES} + ${catkin_LIBRARIES} + ${TinyXML_LIBRARIES} +) +target_link_libraries(${PROJECT_NAME}_can_chain_node + canopen_ros_chain + ${PROJECT_NAME}_lib + ${Boost_LIBRARIES} + ${catkin_LIBRARIES} + ${TinyXML_LIBRARIES} +) +target_link_libraries(${PROJECT_NAME}_cloud_publisher + ${PROJECT_NAME}_lib + ${Boost_LIBRARIES} + ${catkin_LIBRARIES} + ${TinyXML_LIBRARIES} +) +target_link_libraries(${PROJECT_NAME}_can2ros_node + canopen_ros_chain + ${PROJECT_NAME}_lib + ${Boost_LIBRARIES} + ${catkin_LIBRARIES} + ${TinyXML_LIBRARIES} +) +target_link_libraries(${PROJECT_NAME}_ros2can_node + canopen_ros_chain + ${PROJECT_NAME}_lib + ${Boost_LIBRARIES} + ${catkin_LIBRARIES} + ${TinyXML_LIBRARIES} +) +target_link_libraries(${PROJECT_NAME}_lib + ${Boost_LIBRARIES} + ${catkin_LIBRARIES} + ${TinyXML_LIBRARIES} +) +target_link_libraries(sick_canopen_simu_node + canopen_ros_chain + random_numbers + ${PROJECT_NAME}_lib + ${Boost_LIBRARIES} + ${catkin_LIBRARIES} + ${TinyXML_LIBRARIES} +) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# install(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables and/or libraries for installation +install(TARGETS ${PROJECT_NAME}_node ${PROJECT_NAME}_can_chain_node ${PROJECT_NAME}_cloud_publisher ${PROJECT_NAME}_can2ros_node ${PROJECT_NAME}_ros2can_node sick_canopen_simu_node ${PROJECT_NAME}_lib + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +## Mark cpp header files for installation +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE +) + +## Mark other files for installation (e.g. launch and bag files, etc.) +install(FILES + launch/sick_line_guidance.launch + launch/sick_line_guidance_can2ros_node.launch + launch/sick_line_guidance_ols20_twin.launch + launch/sick_line_guidance_ros2can_node.launch + launch/sick_canopen_simu.launch + mls/SICK-MLS.eds + mls/sick_line_guidance_mls.yaml + ols/SICK_OLS10_CO.eds + ols/SICK_OLS20.eds + ols/SICK_OLS20_CO.eds + ols/sick_line_guidance_ols10.yaml + ols/sick_line_guidance_ols20.yaml + ols/sick_line_guidance_ols20_twin.yaml + test/cfg/sick_canopen_simu_cfg.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_sick_line_guidance.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) + + +if( TURTLEBOT_DEMO ) + add_subdirectory( turtlebotDemo/agc_radar ) + add_subdirectory( turtlebotDemo/custom_messages ) + add_subdirectory( turtlebotDemo/gpio_handling ) + add_subdirectory( turtlebotDemo/lidar_obstacle_detection ) + add_subdirectory( turtlebotDemo/sick_line_guidance_demo ) +endif( TURTLEBOT_DEMO ) diff --git a/Devices/Packages/sick_line_guidance/LICENSE b/Devices/Packages/sick_line_guidance/LICENSE new file mode 100755 index 0000000..261eeb9 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/LICENSE @@ -0,0 +1,201 @@ + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/Devices/Packages/sick_line_guidance/README.md b/Devices/Packages/sick_line_guidance/README.md new file mode 100755 index 0000000..f38ae36 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/README.md @@ -0,0 +1,314 @@ +# sick_line_guidance + +SICK Line Guidance ROS Support + +# Introduction + +The aim of this project is to provide an ROS connection for lane guidance sensors of the OLS10, OLS20 and MLS family. + +Users should regularly inform themselves about updates to this driver (best subscribe under "Watch"). + +# Supported Hardware + +SICK optical and magnetical line sensors OLS10, OLS20 and MLS. + +| Product Family | Product Information and Manuals | +| --- | --- | +| OLS10 | https://www.sick.com/ols10 | +| OLS20 | https://www.sick.com/ols20 | +| MLS | https://www.sick.com/mls | + +# Installation and setup + +## Setup CAN hardware and driver + +  1. Install can-utils and socketcan: + +```bash +sudo apt-get install can-utils +sudo modprobe can +sudo modprobe vcan +sudo modprobe slcan +``` +Details can be found in the following links: https://wiki.linklayer.com/index.php/SocketCAN , https://gribot.org/installing-socketcan/ , +https://www.kernel.org/doc/Documentation/networking/can.txt + +  2. Install linux driver for your CAN hardware: + +ROS support for sick line guidance sensors has been developed and tested using PEAK CAN adapter for the CAN communication. +Unless you're using other CAN hardware or driver, we recommend installation of pcan usb adapter by following the installation +instructions on [doc/pcan-linux-installation.md](doc/pcan-linux-installation.md) + +## Installation from Source + +Run the following script to install sick_line_guidance including all dependancies and packages required: + +```bash +source /opt/ros/noetic/setup.bash # currently ros distro melodic and noetic are supported +cd ~ # or change to your project path +mkdir -p catkin_ws/src/ +cd catkin_ws/src/ +git clone https://github.com/SICKAG/sick_line_guidance.git +git clone https://github.com/ros-industrial/ros_canopen.git +git clone https://github.com/CANopenNode/CANopenSocket.git +git clone https://github.com/linux-can/can-utils.git +git clone https://github.com/ros-planning/random_numbers.git +cd .. +sudo apt-get install libtinyxml-dev +sudo apt-get install libmuparser-dev +catkin_make install +source ./install/setup.bash +``` + +# Configuration + +If not done before, you have to set the CAN bitrate, f.e. to 125 kbit/s (default bitrate for OLS and MLS): +```bash +sudo ip link set can0 up type can bitrate 125000 +ip -details link show can0 +``` + +The can node id of the OLS or MLS device is configured in a yaml-file: +- catkin_ws/src/sick_line_guidance/ols/sick_line_guidance_ols10.yaml for OLS10 devices +- catkin_ws/src/sick_line_guidance/ols/sick_line_guidance_ols20.yaml for OLS20 devices +- catkin_ws/src/sick_line_guidance/mls/sick_line_guidance_mls.yaml for MLS devices + +The default can node id is 0x0A. To use a different can node id, set entry "id: 0x0A" to the appropriate value in the yaml-file: +```bash + node1: + id: 0x0A # CAN-Node-ID of can device, default: Node-ID 10=0x0A for OLS and MLS +``` +Install the new configuration with +```bash +cd catkin_ws +catkin_make install +source ./install/setup.bash +``` +after modifications. + +See [sick_line_guidance configuration](doc/sick_line_guidance_configuration.md) for details about the +sick_line_guidance driver and sensor configuration. See [Configuration for multiple devices](doc/sick_line_guidance_configuration.md#configuration-for-multiple-devices) for the configuration of multiple devices, f.e. of two OLS20 devices. + +# Starting + +To start the driver for MLS or OLS, the ros package sick_line_guidance and its launch-file has to be started by roslaunch: + +```bash +cd ~/catkin_ws # change working directory to the project path +source ./install/setup.bash # set environment +roslaunch -v --screen sick_line_guidance sick_line_guidance.launch yaml:=sick_line_guidance_ols10.yaml # start OLS10 driver +roslaunch -v --screen sick_line_guidance sick_line_guidance.launch yaml:=sick_line_guidance_ols20.yaml # start OLS20 driver +roslaunch -v --screen sick_line_guidance sick_line_guidance.launch yaml:=sick_line_guidance_mls.yaml # start MLS driver +``` + +After successful start, you can observe the sensor measurement data in a shell by subscribing to ros topics "/ols", "/mls" and "/cloud": + +```bash +source ./install/setup.bash +rostopic list +rostopic echo /mls +rostopic echo /ols +rostopic echo /cloud +``` + +or you can plot the sensor positions by + +```bash +source ./install/setup.bash +rqt_plot /mls/position[0] /mls/position[1] /mls/position[2] # plot mls positions +rqt_plot /ols/position[0] /ols/position[1] /ols/position[2] # plot ols positions +``` + +You can visualize the data by starting rviz, subscribe to topic "/cloud" (PointCloud2) and select "ols_frame" or "mls_frame": + +```bash +source ./install/setup.bash +rosrun rviz rviz +``` + +# Troubleshooting and diagnostics + +All measurements are published continously on ros topic "/mls" resp. "/ols". In addition, the current status (ok or error) +and (in case of errors) an error code is published on topic "/diagnostics". Messages on these topics can be views by +```bash +rostopic echo /mls +rostopic echo /ols +rostopic echo /diagnostics +``` +The following error codes are defined in header file sick_line_guidance_diagnostic.h: +```bash + /* + * enum DIAGNOSTIC_STATUS enumerates the possible status values of diagnostic messages. + * Higher values mean more severe failures. + */ + typedef enum DIAGNOSTIC_STATUS_ENUM + { + OK, // status okay, no errors + EXIT, // sick_line_guidance exiting + NO_LINE_DETECTED, // device signaled "no line detected" + ERROR_STATUS, // device signaled an error (error flag set in TPDO) + SDO_COMMUNICATION_ERROR, // error in SDO query, timeout on receiving SDO response + CAN_COMMUNICATION_ERROR, // can communication error, shutdown and reset communication + CONFIGURATION_ERROR, // invalid configuration, check configuration files + INITIALIZATION_ERROR, // initialization of CAN driver failed + INTERNAL_ERROR // internal error, should never happen + } DIAGNOSTIC_STATUS; +``` + +All data transmitted on the CAN bus can be displayed by candump: + +```bash +candump -ta can0 +``` + +In case of a successful installation, heartbeats and PDO messages should be visible. +Example output (can master requests object 1018sub4 from can node 8, device responds with its serial number 0x13015015): +```bash +$ candump -ta can0 +(1549455524.265601) can0 77F [1] 05 # heartbeat +(1549455524.294836) can0 608 [8] 40 18 10 04 00 00 00 00 # SDO request (0x600+nodeid), 8 byte (0x40) data, object 0x101804 +(1549455524.301181) can0 588 [8] 43 18 10 04 15 50 01 13 # SDO response (0x580+nodeid), 4 byte (0x43) value, object 0x101804, value 0x13015015 +``` + +Values in the object dictionary of the CAN device can be viewed by + +```bash +# rosservice call /driver/get_object node1 +rosservice call /driver/get_object node1 1018sub4 false # query serial number +``` +Example output: +```bash +$ rosservice call /driver/get_object node1 1018sub4 false # query serial number +success: True +message: '' +value: "318853141" +``` + +The error register of a can device (object index 0x1001) and its pdo mapped objects are published on ros topic "node1_" +and can be printed by rostopic: + +```bash +# rostopic echo -n 1 /node1_ # print object of can device +rostopic echo -n 1 /node1_1001 # print error register 0x1001 of can device +``` +Example output: +```bash +$ rostopic echo -n 1 /node1_1001 +data: 0 +``` + +For test purposes or in case of hardware problems, cansend can be used to send CAN messages. Example: + +```bash +cansend can0 000#820A # NMT message to can device 0x0A: 0x82, reset communication +cansend can0 60A#4F01100000000000 # PDO request: read error register 1001 +``` + +## Simulation and testing + +A software simulation is available for test purposes. This simulation generates synthetical can frames from an input xml-file, and verifies the measurement messages published by +the sick_line_guidance driver. By sending specified can frames, the complete processing chain of the ros driver can be verified by comparing the actual measurement messages +with the expected results. See https://github.com/SICKAG/sick_line_guidance/tree/master/doc/sick_canopen_simu.md for further details. + +## TurtleBot demonstration + +A demonstration of SICK line guidance with a TurtleBot robot is included in folder sick_line_guidance/turtlebotDemo. +Please see [turtlebotDemo/README.md](turtlebotDemo/README.md) for further details. + +## FAQ + +### "Network is down" + +:question: Question: +```bash +candump -ta can0 +``` +gives the result +```bash +read: Network is down +``` + +:white_check_mark: Answer: +(Re-)start can interface by the following commands: +```bash +sudo ip link set can0 type can +sudo ip link set can0 up type can bitrate 125000 # configure the CAN bitrate, f.e. 125000 bit/s +``` + +### "candump gives no answer" + +:question: Question: +```bash +candump -ta can0 +``` +gives no results. + +:white_check_mark: Answer: +Check the baud rate of your device. For a brand new OLS10 this could be 250000 Baud. +For OLS10 please check the baud rate setting by using the device panel (read operation manual of your device). + +### "device or resource busy" + +:question: Question: +```bash +sudo ip link set can0 up type can bitrate 125000 +``` +gives the result: +```bash +RTNETLINK answers: Device or resource busy +``` + +:white_check_mark: Answer: +Check the baud rate of your device. For a brand new OLS10 this could be 250000 Baud. +For OLS10 please check the baud rate setting by using the device panel (read operation manual of your device). +After checking (and changing) the baud rate unplug and replug the usb connector. + + +### "Device 'can0' does not exist" + +:question: Question: After start, the message +```bash +Device "can0" does not exist. +``` +is displayed. + +:white_check_mark: Answer: +- Check power supply +- Unplug, replug and restart PEAK-USB-Adapter +- If you use a PEAK-USB-Adapter and the error message still displays, re-install the PCAN-driver. +PCAN driver can be overwritten by a default can driver due to system updates ("mainline drivers removed and blacklisted in /etc/modprobe.d/blacklist-peak.conf"). +In this case, the PCAN driver must be re-installed. See the quick installation guide https://github.com/SICKAG/sick_line_guidance/tree/master/doc/pcan-linux-installation.md + +### "Configuration changes do not take effect" + +:question: Question: After editing configuration files, the configuration changes do not take effect. + +:white_check_mark: Answer: Modified configuration files have to be installed by +```bash +cd catkin_ws +catkin_make install +source ./install/setup.bash +``` +Restart the driver for MLS or OLS. To avoid potential errors due to previous ros nodes or processes still running, +you might kill all ros nodes and the ros core by +```bash +rosnode kill -a # kill all ros nodes +killall rosmaster # kill ros core +``` + +Please note, that this kills all ros processes, not just those required for sick_line_guidance. + +### "Debugging" + +:question: Question: How can I run sick_line_guidance or ros_canopen in a debugger, e.g. gdb + +:white_check_mark: Answer: A ros node can be started in gdb with prefix gdb, e.g. + +```bash +rosrun --prefix 'gdb -ex run --args' sick_line_guidance ... +``` + +or with argument `launch-prefix="gdb -ex run - -args"` in the launchfile, e.g. +``` + +``` diff --git a/Devices/Packages/sick_line_guidance/doc/copyright.txt b/Devices/Packages/sick_line_guidance/doc/copyright.txt new file mode 100755 index 0000000..38ec35f --- /dev/null +++ b/Devices/Packages/sick_line_guidance/doc/copyright.txt @@ -0,0 +1,54 @@ +/* + * + * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim + * Copyright (C) 2019 SICK AG, Waldkirch + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of SICK AG nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Authors: + * Michael Lehning + * + * Copyright 2019 SICK AG + * Copyright 2019 Ing.-Buero Dr. Michael Lehning + * + */ + diff --git a/Devices/Packages/sick_line_guidance/doc/pcan-config/screenshot01.png b/Devices/Packages/sick_line_guidance/doc/pcan-config/screenshot01.png new file mode 100755 index 0000000..327dd02 Binary files /dev/null and b/Devices/Packages/sick_line_guidance/doc/pcan-config/screenshot01.png differ diff --git a/Devices/Packages/sick_line_guidance/doc/pcan-config/screenshot02.png b/Devices/Packages/sick_line_guidance/doc/pcan-config/screenshot02.png new file mode 100755 index 0000000..c5263d5 Binary files /dev/null and b/Devices/Packages/sick_line_guidance/doc/pcan-config/screenshot02.png differ diff --git a/Devices/Packages/sick_line_guidance/doc/pcan-config/screenshot03.png b/Devices/Packages/sick_line_guidance/doc/pcan-config/screenshot03.png new file mode 100755 index 0000000..d2898c2 Binary files /dev/null and b/Devices/Packages/sick_line_guidance/doc/pcan-config/screenshot03.png differ diff --git a/Devices/Packages/sick_line_guidance/doc/pcan-linux-installation.md b/Devices/Packages/sick_line_guidance/doc/pcan-linux-installation.md new file mode 100755 index 0000000..5f41735 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/doc/pcan-linux-installation.md @@ -0,0 +1,70 @@ +# pcan-linux-installation + +ROS support for sick line guidance sensors has been developed and tested using PEAK CAN adapter for the CAN communication. +This is a quick howto install linux driver for the pcan usb adapter. + +# Installation + +The following installation is recommended: + +  1. Download PCAN-View for Linux from https://www.peak-system.com/linux/ or https://www.peak-system.com/quick/ViewLinux_amd64/pcanview-ncurses_0.8.7-0_amd64.deb and install: +```bash +sudo apt-get install libncurses5 +sudo dpkg --install pcanview-ncurses_0.8.7-0_amd64.deb +``` + +  2. If you're running ROS in a virtual machine, make sure the usb-port for the PCAN-USB-adapter is connected to your VM. + +  3. Download and unzip peak-linux-driver-8.17.0.tar.gz from https://www.peak-system.com/quick/PCAN-Linux-Driver + + +  4. Install the linux driver and required packages: +```bash +cd peak-linux-driver-8.17.0 +# install required packages +sudo apt-get install can-utils +sudo apt-get install gcc-multilib +sudo apt-get install libelf-dev +sudo apt-get install libpopt-dev +sudo apt-get install tree +# build and install pcan driver +make clean +make NET=NETDEV_SUPPORT +sudo make install +# install the modules +sudo modprobe pcan +sudo modprobe can +sudo modprobe vcan +sudo modprobe slcan +# setup and configure "can0" net device +sudo ip link set can0 type can +sudo ip link set can0 up type can bitrate 125000 # configure the CAN bitrate, f.e. 125000 bit/s +# check installation +./driver/lspcan --all # should print "pcanusb32" and pcan version +tree /dev/pcan-usb # should show a pcan-usb device +ip -a link # should print some "can0: ..." messages +ip -details link show can0 # should print some details about "can0" net device +``` +Example output after successfull installation of a pcan usb adapter: +```bash +user@ubuntu-ros:~/peak-linux-driver-8.17.0$ ./driver/lspcan --all +pcanusb32 CAN1 - 8MHz 125k ACTIVE - 1969 0 0 +user@ubuntu-ros:~/peak-linux-driver-8.17.0$ tree /dev/pcan-usb +/dev/pcan-usb +├── 0 +│   └── can0 -> ../../pcanusb32 +└── devid=5 -> ../pcanusb32 +user@ubuntu-ros:~/peak-linux-driver-8.17.0$ ip -a link +3: can0: mtu 16 qdisc noop state DOWN mode DEFAULT group default qlen 10 + link/can +user@ubuntu-ros:~/peak-linux-driver-8.17.0$ ip -details link show can0 +3: can0: mtu 16 qdisc noop state DOWN mode DEFAULT group default qlen 10 + link/can promiscuity 0 + can state STOPPED restart-ms 0 + bitrate 500000 sample-point 0.875 + tq 125 prop-seg 6 phase-seg1 7 phase-seg2 2 sjw 1 + pcan: tseg1 1..16 tseg2 1..8 sjw 1..4 brp 1..64 brp-inc 1 + clock 8000000numtxqueues 1 numrxqueues 1 gso_max_size 65536 gso_max_segs 65535 +``` + +See https://www.peak-system.com/fileadmin/media/linux/files/PCAN-Driver-Linux_UserMan_eng.pdf for further details. diff --git a/Devices/Packages/sick_line_guidance/doc/sick_canopen_simu.md b/Devices/Packages/sick_line_guidance/doc/sick_canopen_simu.md new file mode 100755 index 0000000..80c9821 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/doc/sick_canopen_simu.md @@ -0,0 +1,128 @@ +# Simulation and testing + +sick_canopen_simu implements a software simulation of MLS and OLS devices for test purposes. This simulation generates synthetical can frames from an input xml-file, +and verifies the measurement messages published by the sick_line_guidance driver. By sending specified can frames, the complete processing chain of the ros driver can be verified +by comparing the actual measurement messages with the expected results. In addition, error messages and error handling can be tested, which otherwise can be challenging. + +Please note, that sick_canopen_simu does not implement or simulate the behavior of the MLS or OLS hardware. It just sends specified can frames and checks the measurement messages +from the sick_line_guidance driver after entering the operational state. The purpose of sick_canopen_simu is driver verification and testing, not a fully functional simulation of +can devices. + +## Usage + +To test the sick_line_guidance driver with simulated can frames, four ros nodes have to be started: +- sick_line_guidance_ros2can_node to send ros messages of type can::Frame to the can driver, which sends the frames to the can bus, +- sick_line_guidance_can2ros_node to receive can frames and to publish them as ros messages of type can::Frame, +- sick_canopen_simu to read the simulation data from file, to convert them to can::Frame messages and to verify the resulting sick_line_guidance messages from the driver, +- sick_line_guidance_node, i.e. the sick_line_guidance driver. + +Example for simulation and test of the OLS20 driver: +```bash +# Start can2ros converter +roslaunch -v sick_line_guidance sick_line_guidance_can2ros_node.launch & +sleep 5 +# Start ros2can converter +roslaunch -v sick_line_guidance sick_line_guidance_ros2can_node.launch & +sleep 5 +# Start OLS20 simulation +roslaunch -v --screen sick_line_guidance sick_canopen_simu.launch device:=OLS20 & +sleep 5 +# Start OLS driver incl. canopen_chain_node, sick_line_guidance_node and sick_line_guidance_cloud_publisher +roslaunch -v --screen sick_line_guidance sick_line_guidance.launch yaml:=sick_line_guidance_ols20.yaml +``` + +The same example works for MLS, too, with slightly different arguments (`device:=MLS` and `yaml:=sick_line_guidance_mls.yaml`): +```bash +# Start can2ros converter +roslaunch -v sick_line_guidance sick_line_guidance_can2ros_node.launch & +sleep 5 +# Start ros2can converter +roslaunch -v sick_line_guidance sick_line_guidance_ros2can_node.launch & +sleep 5 +# Start MLS simulation +roslaunch -v --screen sick_line_guidance sick_canopen_simu.launch device:=MLS & +sleep 5 +# Start MLS driver incl. canopen_chain_node, sick_line_guidance_node and sick_line_guidance_cloud_publisher +roslaunch -v --screen sick_line_guidance sick_line_guidance.launch yaml:=sick_line_guidance_mls.yaml +``` + +To simulate and test the OLS10 driver, just use the parameter `device:=OLS10` and `yaml:=sick_line_guidance_ols10.yaml`. + +If the verification of measurement messages failed, an error message is displayed. + +To run an automated unittest of both the MLS, OLS10 and OLS20 driver, script `runsimu.bash` in folder `sick_line_guidance/test/scripts` is provided: +```bash +cd catkin_ws/src/sick_line_guidance/test/scripts +./runsimu.bash +``` + +## Configuration + +Settings and testcases for the simulation are specified in file `sick_line_guidance/test/cfg/sick_canopen_simu_cfg.xml`. To modify the current unittest +or to append further testcases, open this file in editor, save it and install the new configuration by +```bash +cd catkin_ws +catkin_make install +source ./install/setup.bash +``` + +Configuration file sick_canopen_simu_cfg.xml has three sections: + +1. **sick_canopen_simu/SDO_config** : A lookup table for SDO response from SDO get request. + Whenever the ros driver sends a SDO request, the simulation responds with a SDO request from this table. + + Example: + ```bash + + + + + ``` + The simulation will send a SDO response 4318100411AE2201 when receiving SDO request 4018100400000000 (serial number, object 1018sub4). + SDO response 4F18200000000000 will be sent after receiving a SDO request 4018200000000000 (OLS device status, object 2018). + Note: SDO responses of PDO mapped objects are modified, whenever a PDO is generated by the simulation. + +2. **sick_canopen_simu/OLS20/PDO_config** : A list of sensor states transmitted by PDOs to simulate an OLS20 device. + This list specifies the measurement data transmitted by PDOs (lcp1, lcp2, lcp3, width1, width2, width3, status, barcode, barcode center, line quality and line intensities). + Each measurement is transmitted 25 times with a rate of 50 hertz (PDOs in 20 milliseconds intervall, configurable in the launch file sick_canopen_simu.launch) + before switching to the next sensor state. All measurements are repeated in an endless loop while the simulation is running. + + Example: + ```bash + + + + + + + ``` + The simulation will convert the specified sensor states and transmit PDO1 and PDO2 every 20 milliseconds. + +3. **sick_canopen_simu/OLS10/PDO_config** : A list of sensor states transmitted by PDOs to simulate an OLS10 device. + This list is identical to OLS20, except that barcode center, line quality and line intensities are not supported by OLS10 devices and therefor have to be set to 0. + + Example: + ```bash + + + + + + + ``` + +4. **sick_canopen_simu/MLS/PDO_config** : A list of sensor states transmitted by PDOs to simulate a MLS device. + This list specifies the measurement data transmitted by PDOs (lcp1, lcp2, lcp3, status and #lcp). + Each measurement is transmitted 5 times with a rate of 50 hertz (PDOs in 20 milliseconds intervall, configurable in the launch file sick_canopen_simu.launch) + before switching to the next sensor state. All measurements are repeated in an endless loop while the simulation is running. + + Example: + ```bash + + + + + + + ``` + The simulation will convert the specified sensor states and transmit PDO1 every 20 milliseconds. diff --git a/Devices/Packages/sick_line_guidance/doc/sick_line_guidance_configuration.md b/Devices/Packages/sick_line_guidance/doc/sick_line_guidance_configuration.md new file mode 100755 index 0000000..66f3486 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/doc/sick_line_guidance_configuration.md @@ -0,0 +1,242 @@ +# sick_line_guidance configuration + +The ROS drivers for MLS and OLS devices are configured by launch and yaml files. You can modify the default configuration by editing these files. +Please note, that an invalid or improper configuration may cause errors and unexpected results. + +The basic configuration uses the default values for OLS and MLS devices. All changes should be tested carefully. + +To start the MLS resp. OLS driver, just launch sick_line_guidance with a yaml-configuration file: +```bash +roslaunch sick_line_guidance sick_line_guidance.launch yaml:=sick_line_guidance_mls.yaml # start MLS driver +roslaunch sick_line_guidance sick_line_guidance.launch yaml:=sick_line_guidance_ols10.yaml # start OLS10 driver +roslaunch sick_line_guidance sick_line_guidance.launch yaml:=sick_line_guidance_ols20.yaml # start OLS20 driver +``` + +The launch file "sick_line_guidance.launch" configures the common driver settings, f.e. the ros nodes to start and the ros topics to be used by +sick_line_guidance. Unless special purposes, these settings do not require customization. + +The 3rd argument to roslaunch provides the driver with a yaml-file: sick_line_guidance_mls.yaml for MLS resp. sick_line_guidance_ols10.yaml for OLS10 +or sick_line_guidance_ols20.yaml for OLS20. +These two files contain the settings of the can devices and may require customization. + +## yaml configuration for MLS + +This is the default configuration for a MLS device in file mls/sick_line_guidance_mls.yaml: +```yaml +bus: + device: can0 + driver_plugin: can::SocketCANInterface + master_allocator: canopen::SimpleMaster::Allocator +sync: + interval_ms: 10 + overflow: 0 +nodes: + node1: + id: 0x0A # CAN-Node-ID of can device, default: Node-ID 10=0x0A for OLS and MLS + eds_pkg: sick_line_guidance # package name for relative path + eds_file: SICK-MLS.eds # path to EDS/DCF file + publish: ["1001","1018sub1","1018sub4","2021sub1!","2021sub2!","2021sub3!","2021sub4!","2022!"] + # MLS: 1001 = error register, 1018sub1 = VendorID, 1018sub4 = SerialNumber, TPDO1 = 0x2021sub1 to 0x2021sub4 and 0x2022 + + # sick_line_guidance configuration of this node: + sick_device_family: "MLS" # can devices of OLS10, OLS20 or MLS family currently supported + sick_topic: "mls" # MLS_Measurement messages are published in topic "/mls" + sick_frame_id: "mls_measurement_frame" # MLS_Measurement messages are published frame_id "mls_measurement_frame" + + # device configuration of writable parameter by dcf_overlay: "objectindex": "parametervalue" + # example: "2027": "0x01" # Object 2027 (sensorFlipped, defaultvalue 0x00) will be configured with value 0x01 + # dcf_overlay: + # "2028sub1": "0x01" # UseMarkers (0 = disable marker detection, 1 = enable marker detection), UINT8, DataType=0x0005, defaultvalue=0 + # "2028sub2": "0x02" # MarkerStyle (0 = disable marker detection, 1 = standard mode, 2 = extended mode), UINT8, DataType=0x0005, defaultvalue=0 + # "2028sub3": "0x01" # FailSafeMode (0 = disabled, 1 = enabled), UINT8, DataType=0x0005, defaultvalue=0 + # "2025": "1000" # Min.Level, UINT16, DataType=0x0006, defaultvalue=600 + # "2026": "1" # Offset [mm] Nullpunkt, INT16, DataType=0x0003, defaultvalue=0 + # "2027": "0x01" # sensorFlipped, UINT8, DataType=0x0005, defaultvalue=0 + # "2029": "0x01" # LockTeach, UINT8, DataType=0x0005, defaultvalue=0 +``` + +The following table describes these settings in detail: + +| parameter name | default value | details | +| --- | --- | --- | +| bus / device | can0 | System name of can interface: net device driver for can hardware (f.e. pcan adapter) are installed to a net device named "can0". Check by system command "ifconfig -a" | +| bus / driver_plugin | can:: SocketCANInterface | Implementation of interface SocketCAN, currently only can::SocketCANInterface is supported. | +| bus / master_allocator | canopen:: SimpleMaster:: Allocator | Implementation of CAN master (network manager), currently only canopen::SimpleMaster::Allocator is supported. | +| sync / interval_ms | 10 | Timeinterval for can sync messages in milliseconds (CAN master will send a nmt sync message each 10 ms) | +| sync / overflow | 0 | Length of can sync message (0: sync message does not contain any data byte) | +| nodes | node1 | Name of the first can node configured. If you're running more than 1 can device (multiple MLS or OLS devices), you can configure each device by providing a named section for each node. | +| **nodes / node1 / id** | **0x0A** | **CAN-Node-ID of can device (default: Node-ID 10=0x0A for OLS and MLS). Change this id, if your MLS uses a different CAN-ID. :exclamation:** | +| nodes / node1 / eds_pkg | sick_line_guidance | Name of sick_line_guidance ros package. Required to resolve filenames and should not be modified. | +| nodes / node1 / eds_file | SICK-MLS.eds | Electronic datasheet for your MLS device | +| nodes / node1 / publish | ["1001", "1018sub1", "1018sub4", "2021sub1!", "2021sub2!", "2021sub3!", "2021sub4!", "2022!"] | List of published objects in the object dictionary of a can device. These objects are both published on ros topic _ and required by driver internal callbacks to handle PDO messages. Do not remove any PDO mapped objects from this list! | +| nodes / node1 / sick_device_family | "MLS" | Informs the ros driver that this can device is a MLS. Currently supported values are "OLS10", "OLS20" or "MLS". | +| nodes / node1 / sick_topic | "mls" | Name of the ros topic for publishing MLS measurement messages. If modified, the same topic must be set in file sick_line_guidance.launch for the sick_line_guidance_cloud_publisher node (param name="mls_topic_publish" type="str" value="mls"). Otherwise, measurement messges won't be transformed to PointCloud2 messsages. | +| nodes / node1 / sick_frame_id | "mls_measurement_frame" | Ros frame id of the MLS measurement messages. | +| **# nodes / node1 / dcf_overlay** | | **Configuration of device parameter. To set an object in the object dictionary of a can device to a specific value at startup, append dcf_overlay entries with "objectindex": "parametervalue". This section is intensionally left empty to use default values. See operation manual for possible settings. :exclamation:** | + +Device specific settings can be configured in section "nodes/node1/dcf_overlay". Example to activate marker detection in extended mode (failsafe configuration): +```yaml + dcf_overlay: + "2028sub1": "0x01" # UseMarkers (0 = disable marker detection, 1 = enable marker detection), UINT8, DataType=0x0005, defaultvalue=0 + "2028sub2": "0x02" # MarkerStyle (0 = disable marker detection, 1 = standard mode, 2 = extended mode), UINT8, DataType=0x0005, defaultvalue=0 + "2028sub3": "0x01" # FailSafeMode (0 = disabled, 1 = enabled), UINT8, DataType=0x0005, defaultvalue=0 +``` + +## yaml configuration for OLS + +The configuration of an OLS device is pretty much the same to MLS, just with a different PDO mapping and a different eds-file. Therefore, the object list specified by `publish` +and by `dcf_overlay` vary and the eds-file "SICK_OLS20.eds" is used. Otherwise, the same configuration apply. + +This is the default configuration for a OLS10 device in file ols/sick_line_guidance_ols10.yaml: +```yaml +bus: + device: can0 + driver_plugin: can::SocketCANInterface + master_allocator: canopen::SimpleMaster::Allocator +sync: + interval_ms: 10 + overflow: 0 +nodes: + node1: + id: 0x0A # CAN-Node-ID of can device, default: Node-ID 10=0x0A for OLS and MLS + eds_pkg: sick_line_guidance # package name for relative path + eds_file: SICK_OLS10_CO.eds # path to EDS/DCF file + publish: ["1001","1018sub1","1018sub4","2021sub1!","2021sub2!","2021sub3!","2021sub4!","2021sub5!","2021sub6!","2021sub7!","2021sub8!"] + # OLS10: 1001 = error register, 1018sub1 = VendorID, 1018sub4 = SerialNumber, TPDOs 0x2021sub1 to 0x2021sub8 + + # sick_line_guidance configuration of this node: + sick_device_family: "OLS10" # can devices currently supported: "OLS10", "OLS20" or "MLS" + sick_topic: "ols" # OLS_Measurement messages are published in topic "/ols" + sick_frame_id: "ols_measurement_frame" # OLS_Measurement messages are published with frame_id "ols_measurement_frame" + + # device configuration of writable parameter by dcf_overlay: "objectindex": "parametervalue" + # example: "2001sub5": "0x01" # Object 2001sub5 (sensorFlipped, defaultvalue 0x00) will be configured with value 0x01 + # dcf_overlay: + # "2001sub5": "0x01" # sensorFlipped, UINT8, DataType=0x0005, defaultvalue=0 + # "2002sub1": "0.015" # Typ. Width, FLOAT32, DataType=0x0008 + # "2002sub2": "0.001" # Min. Width, FLOAT32, DataType=0x0008 + # "2002sub3": "0.050" # Max. Width, FLOAT32, DataType=0x0008 +``` + +This is the default configuration for a OLS20 device in file ols/sick_line_guidance_ols20.yaml: +```yaml +bus: + device: can0 + driver_plugin: can::SocketCANInterface + master_allocator: canopen::SimpleMaster::Allocator +sync: + interval_ms: 10 + overflow: 0 +nodes: + node1: + id: 0x0A # CAN-Node-ID of can device, default: Node-ID 10=0x0A for OLS and MLS + eds_pkg: sick_line_guidance # package name for relative path + eds_file: SICK_OLS20_CO.eds # path to EDS/DCF file + publish: ["1001","1018sub1","1018sub4","2021sub1!","2021sub2!","2021sub3!","2021sub4!","2021sub5!","2021sub6!","2021sub7!","2021sub8!"] + # OLS20: 1001 = error register, 1018sub1 = VendorID, 1018sub4 = SerialNumber, TPDOs 0x2021sub1 to 0x2021sub8 + + # sick_line_guidance configuration of this node: + sick_device_family: "OLS20" # can devices currently supported: "OLS10", "OLS20" or "MLS" + sick_topic: "ols" # OLS_Measurement messages are published in topic "/ols" + sick_frame_id: "ols_measurement_frame" # OLS_Measurement messages are published with frame_id "ols_measurement_frame" + + # device configuration of writable parameter by dcf_overlay: "objectindex": "parametervalue" + # example: "2001sub5": "0x01" # Object 2001sub5 (sensorFlipped, defaultvalue 0x00) will be configured with value 0x01 + # dcf_overlay: + # "2001sub5": "0x01" # sensorFlipped, UINT8, DataType=0x0005, defaultvalue=0 + # "2002sub1": "0.015" # Typ. Width, FLOAT32, DataType=0x0008 + # "2002sub2": "0.001" # Min. Width, FLOAT32, DataType=0x0008 + # "2002sub3": "0.050" # Max. Width, FLOAT32, DataType=0x0008 +``` + +The following table describes these settings in detail: + +| parameter name | default value | details | +| --- | --- | --- | +| bus / device | can0 | System name of can interface: net device driver for can hardware (f.e. pcan adapter) are installed to a net device named "can0". Check by system command "ifconfig -a" | +| bus / driver_plugin | can:: SocketCANInterface | Implementation of interface SocketCAN, currently only can::SocketCANInterface is supported. | +| bus / master_allocator | canopen:: SimpleMaster:: Allocator | Implementation of CAN master (network manager), currently only canopen::SimpleMaster::Allocator is supported. | +| sync / interval_ms | 10 | Timeinterval for can sync messages in milliseconds (CAN master will send a nmt sync message each 10 ms) | +| sync / overflow | 0 | Length of can sync message (0: sync message does not contain any data byte) | +| nodes | node1 | Name of the first can node configured. If you're running more than 1 can device (multiple MLS or OLS devices), you can configure each device by providing a named section for each node. | +| **nodes / node1 / id** | **0x0A** | **CAN-Node-ID of can device (default: Node-ID 10=0x0A for OLS and MLS). Change this id, if your OLS uses a different CAN-ID. :exclamation:** | +| nodes / node1 / eds_pkg | sick_line_guidance | Name of sick_line_guidance ros package. Required to resolve filenames and should not be modified. | +| nodes / node1 / eds_file | SICK_OLS20.eds | Electronic datasheet for your OLS device | +| nodes / node1 / publish | ["1001", "1018sub1", "1018sub4", "2021sub1!", "2021sub2!", "2021sub3!", "2021sub4!", "2021sub5!", "2021sub6!", "2021sub7!", "2021sub8!"] | List of published objects in the object dictionary of a can device. These objects are both published on ros topic _ and required by driver internal callbacks to handle PDO messages. Do not remove any PDO mapped objects from this list! | +| nodes / node1 / sick_device_family | "OLS20" | Informs the ros driver that this can device is an OLS20. Currently supported values are "OLS10", "OLS20" or "MLS". | +| nodes / node1 / sick_topic | "ols" | Name of the ros topic for publishing OLS measurement messages. If modified, the same topic must be set in file sick_line_guidance.launch for the sick_line_guidance_cloud_publisher node (param name="ols_topic_publish" type="str" value="ols"). Otherwise, measurement messges won't be transformed to PointCloud2 messsages. | +| nodes / node1 / sick_frame_id | "ols_measurement_frame" | Ros frame id of the OLS measurement messages. | +| **# nodes / node1 / dcf_overlay** | | **Configuration of device parameter. To set an object in the object dictionary of a can device to a specific value at startup, append dcf_overlay entries with "objectindex": "parametervalue". This section is intensionally left empty to use default values. See operation manual for possible settings. :exclamation:** | + +Device specific settings can be configured in section "nodes/node1/dcf_overlay". Example to set object 2001sub5 (sensorFlipped): +```yaml + dcf_overlay: + "2001sub5": "0x01" # sensorFlipped, UINT8, DataType=0x0005, defaultvalue=0 +``` + +## Summary of the yaml configuration for MLS and OLS devices + +Set the CAN-ID of your device with `id: 0x0A # CAN-Node-ID of MLS (default: 0x0A)` and +configure device specific settings in section `dcf_overlay` by appending a line `"objectindex": "parametervalue"` for each parameter. + +## Configuration for multiple devices + +If you want to use multiple can devices (MLS or OLS) on one system, you have to append multiple nodes in your yaml-file. Each node must contain the configuration of one can device as shown above. Make sure to use different node names for each device: +```yaml +nodes: + node1: + id: ... # CAN-ID for the 1. device + node2: + id: ... # CAN-ID for the 2. device +``` + +If you configure different measurement topics for each can device (f.e. `sick_topic: "ols20A"` for the first OLS20 and `sick_topic: "ols20B"` for the second OLS20 device), you have to start multiple sick_line_guidance_cloud_publisher nodes, too. Otherwise, measurement messges from this device can't be transformed to PointCloud2 messsages. + +Append multiple sick_line_guidance_cloud_publisher nodes in file sick_line_guidance.launch, each sick_line_guidance_cloud_publisher node configured with the corresponding +ros topic in parameter `"mls_topic_publish"` resp. `"ols_topic_publish"`. + +Launchfile [sick_line_guidance_ols20_twin.launch](../launch/sick_line_guidance_ols20_twin.launch) and yaml-configuration [sick_line_guidance_ols20_twin.yaml](../ols/sick_line_guidance_ols20_twin.yaml) show an example how to run two OLS20 devices with can node ids 0x0A and 0x0B. Run the example with +``` +rosrun rviz rviz & +roslaunch -v --screen sick_line_guidance sick_line_guidance_ols20_twin.launch +``` +The point clouds of both OLS20 devices will be published on topic "cloudA" and "cloudB" with frame ids "olsA_frame" and "olsB_frame". Use a static_transform_publisher to setup a transform for each OLS frame to a reference coordinate system, e.g. +``` +rosrun tf static_transform_publisher 0 0 0 0 0 0 olsA_frame cloud 10 +rosrun tf static_transform_publisher 0 0 0 0 0 0 olsB_frame cloud 10 +``` + +## Read and write parameter during runtime + +You can read and write to the object dictionary of a can device on runtime, too, by using the ros services implemented by the canopen driver. + +To read a value from the object dictionary, you can run +```bash +rosservice call /driver/get_object "{node: '', object: '', cached: }" +``` +in your terminal. Example to read object 2001sub5 (sensorFlipped) from node1: +```bash +rosservice call /driver/get_object "{node: 'node1', object: '2001sub5', cached: false}" +``` +This command results in a can upload command using a SDO with object 2001sub5. The SDO response will be converted and displayed if the command succeded. + +To write a value to the object dictionary, you can run +```bash +rosservice call /driver/get_object "{node: '', object: '', value: '', cached: }" +``` +in the terminal. Example to write value 0x01 to object 2001sub5 (sensorFlipped): +```bash +rosservice call /driver/set_object "{node: 'node1', object: '2001sub5', value: '0x01', cached: false}" +``` +This command results in a can upload command using a SDO with object 2001sub5. + +## Configuration of CAN node id + +The node id of a CAN device can be configured e.b. with [PCAN-View](https://www.peak-system.com/PCAN-View.242.0.html):
+![screenshot01.png](pcan-config/screenshot01.png)
+![screenshot02.png](pcan-config/screenshot02.png)
+ +## Installation and bus termination + +The following screenshot shows the installation and bus termination for two OLS devices:
+![screenshot03.png](pcan-config/screenshot03.png) diff --git a/Devices/Packages/sick_line_guidance/include/sick_line_guidance/sick_canopen_simu_canstate.h b/Devices/Packages/sick_line_guidance/include/sick_line_guidance/sick_canopen_simu_canstate.h new file mode 100755 index 0000000..e965970 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/include/sick_line_guidance/sick_canopen_simu_canstate.h @@ -0,0 +1,130 @@ +/* + * sick_canopen_simu_canstate implements a state engine for can. + * + * Depending on input messages of type can_msgs::Frame, + * the current state is switched between INITIALIZATION, + * PRE_OPERATIONAL, OPERATIONAL and STOPPED. + * + * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim + * Copyright (C) 2019 SICK AG, Waldkirch + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of SICK AG nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Authors: + * Michael Lehning + * + * Copyright 2019 SICK AG + * Copyright 2019 Ing.-Buero Dr. Michael Lehning + * + */ +#ifndef __SICK_CANOPEN_SIMU_CANSTATE_H_INCLUDED +#define __SICK_CANOPEN_SIMU_CANSTATE_H_INCLUDED + +#include +#include + +namespace sick_canopen_simu +{ + typedef enum CAN_STATE_ENUM + { + INITIALIZATION, + PRE_OPERATIONAL, + OPERATIONAL, + STOPPED + + } CAN_STATE; + + + /* + * class CanState: handles can nmt messages and implements the state engine for can. + */ + class CanState + { + public: + + /* + * Constructor. + * + * @param[in] can_node_id node id of OLS or MLS, default: 0x0A + */ + CanState(int can_node_id); + + /* + * Destructor. + */ + virtual ~CanState(); + + /* + * Constructor. + * + * @param[in] can_node_id node id of OLS or MLS, default: 0x0A + */ + virtual CAN_STATE & state(void) + { + return m_can_state; + } + + /* + * @brief Callbacks for ros messages. Switches the state, and returns true, if msg_in is a can nmt message. + * Otherwise, false is returned. + * + * param[in] msg_in input message of type can_msgs::Frame + * param[out] msg_out output message of type can_msgs::Frame (if input message requires a response to send) + * param[out] send_msg true, if input message msg_in requires the response message msg_out to be send + * + * @return true, if input message handled, otherwise false. + */ + virtual bool messageHandler(const can_msgs::Frame &msg_in, can_msgs::Frame & msg_out, bool & send_msg); + + protected: + + /* + * member data. + */ + + int m_can_node_id; + CAN_STATE m_can_state; + + }; // class CanState + +} // namespace sick_canopen_simu +#endif // __SICK_CANOPEN_SIMU_CANSTATE_H_INCLUDED diff --git a/Devices/Packages/sick_line_guidance/include/sick_line_guidance/sick_canopen_simu_compare.h b/Devices/Packages/sick_line_guidance/include/sick_line_guidance/sick_canopen_simu_compare.h new file mode 100755 index 0000000..f8d63c0 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/include/sick_line_guidance/sick_canopen_simu_compare.h @@ -0,0 +1,177 @@ +/* + * sick_canopen_simu_compare implements utility functions to compare measurements + * for verification of the sick_line_guidance ros driver. + * + * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim + * Copyright (C) 2019 SICK AG, Waldkirch + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of SICK AG nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Authors: + * Michael Lehning + * + * Copyright 2019 SICK AG + * Copyright 2019 Ing.-Buero Dr. Michael Lehning + * + */ +#ifndef __SICK_CANOPEN_SIMU_COMPARE_H_INCLUDED +#define __SICK_CANOPEN_SIMU_COMPARE_H_INCLUDED + +#include +#include "sick_line_guidance/MLS_Measurement.h" +#include "sick_line_guidance/OLS_Measurement.h" + +namespace sick_canopen_simu +{ + /* + * class MeasurementComparator implements utility functions to compare measurements + * for verification of the sick_line_guidance ros driver. + * + */ + template + class MeasurementComparator + { + public: + + /* + * @brief Compares the positions of two measurements. + * @return true, if the positions of two measurements are identical (difference below 1 millimeter), or false otherwise. + */ + static bool cmpPosition(const MsgType &A, const MsgType &B) + { + return (std::fabs(A.position[0] - B.position[0]) < 0.001) && (std::fabs(A.position[1] - B.position[1]) < 0.001) && (std::fabs(A.position[2] - B.position[2]) < 0.001); + } + + /* + * @brief Compares the width of two measurements. + * @return true, if the width of two measurements are identical (difference below 1 millimeter), or false otherwise. + */ + static bool cmpLinewidth(const MsgType &A, const MsgType &B) + { + return (std::fabs(A.width[0] - B.width[0]) < 0.001) && (std::fabs(A.width[1] - B.width[1]) < 0.001) && (std::fabs(A.width[2] - B.width[2]) < 0.001); + } + + /* + * @brief Compares the status of two measurements. + * @return true, if the status of two measurements are identical, or false otherwise. + */ + static bool cmpStatus(const MsgType &A, const MsgType &B) + { + return (A.status == B.status); + } + + /* + * @brief Compares the lcp status of two measurements. + * @return true, if the lcp status of two measurements are identical, or false otherwise. + */ + static bool cmpLcp(const MsgType &A, const MsgType &B) + { + return (A.lcp == B.lcp); + } + + /* + * @brief Compares the barcode of two measurements. + * @return true, if the barcodes of two measurements are identical, or false otherwise. + */ + static bool cmpBarcode(const MsgType &A, const MsgType &B) + { + return (A.barcode == B.barcode); + } + + /* + * @brief Compares the device status of two measurements. + * @return true, if the device status of two measurements are identical, or false otherwise. + */ + static bool cmpDevStatus(const MsgType &A, const MsgType &B) + { + return (A.dev_status == B.dev_status); + } + + /* + * @brief Compares the extended code of two measurements. + * @return true, if the extended code of two measurements are identical, or false otherwise. + */ + static bool cmpExtendedCode(const MsgType &A, const MsgType &B) + { + return (A.extended_code == B.extended_code); + } + + /* + * @brief Compares the error status of two measurements. + * @return true, if the error status of two measurements are identical, or false otherwise. + */ + static bool cmpError(const MsgType &A, const MsgType &B) + { + return (A.error == B.error); + } + + /* + * @brief Compares the barcode center points of two measurements. + * @return true, if the barcode center points of two measurements are identical, or false otherwise. + */ + static bool cmpBarcodeCenter(const MsgType &A, const MsgType &B) + { + return (std::fabs(A.barcode_center_point - B.barcode_center_point) < 0.001); + } + + /* + * @brief Compares the line quality of two measurements. + * @return true, if the line quality of two measurements are identical, or false otherwise. + */ + static bool cmpLineQuality(const MsgType &A, const MsgType &B) + { + return (A.quality_of_lines == B.quality_of_lines); + } + + /* + * @brief Compares the line intensities of two measurements. + * @return true, if the line intensities of two measurements are identical, or false otherwise. + */ + static bool cmpLineIntensity(const MsgType &A, const MsgType &B) + { + return (A.intensity_of_lines[0] == B.intensity_of_lines[0] && A.intensity_of_lines[1] == B.intensity_of_lines[1] && A.intensity_of_lines[2] == B.intensity_of_lines[2]); + } + + }; +} +#endif // __SICK_CANOPEN_SIMU_COMPARE_H_INCLUDED + diff --git a/Devices/Packages/sick_line_guidance/include/sick_line_guidance/sick_canopen_simu_device.h b/Devices/Packages/sick_line_guidance/include/sick_line_guidance/sick_canopen_simu_device.h new file mode 100755 index 0000000..e54e265 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/include/sick_line_guidance/sick_canopen_simu_device.h @@ -0,0 +1,417 @@ +/* + * sick_canopen_simu_device implements simulation of SICK can devices (OLS20 and MLS) + * for tests of sick_line_guidance ros driver. + * + * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim + * Copyright (C) 2019 SICK AG, Waldkirch + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of SICK AG nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Authors: + * Michael Lehning + * + * Copyright 2019 SICK AG + * Copyright 2019 Ing.-Buero Dr. Michael Lehning + * + */ +#ifndef __SICK_CANOPEN_SIMU_DEVICE_H_INCLUDED +#define __SICK_CANOPEN_SIMU_DEVICE_H_INCLUDED + +#include +#include +#include +#include +#include "sick_line_guidance/sick_canopen_simu_canstate.h" +#include "sick_line_guidance/MLS_Measurement.h" +#include "sick_line_guidance/OLS_Measurement.h" + +namespace sick_canopen_simu +{ + /* + * Class SimulatorBase implements the base class for simulation of SICK can devices (OLS20 and MLS). + * + * ROS messages of type can_msgs::Frame are read from ros topic "can0", + * handled to simulate an OLS20 or MLS device, and resulting can_msgs::Frame + * messages are published on ros topic "ros2can0". + * + * MsgType can be sick_line_guidance::OLS_Measurement (for simulation of OLS devices), or + * or sick_line_guidance::MLS_Measurement (for simulation of MLS devices). + * + * Assumption: ros nodes sick_line_guidance_ros2can_node and sick_line_guidance_can2ros_node + * have to be running to convert ros messages from and to canbus messages. + * sick_line_guidance_ros2can_node converts ros messages of type can_msgs::Frame to can frames, + * sick_line_guidance_can2ros_node converts can frames to ros messages of type can_msgs::Frame. + * + * Subclass MLSSimulator extends class SimulatorBase to simulate an MLS device. + * + * Subclass OLS20Simulator extends class SimulatorBase to simulate an OLS20 device. + */ + template class SimulatorBase + { + public: + + /* + * Constructor + * + * @param[in] nh ros node handle + * @param[in] config_file configuration file with testcases for OLS and MLS simulation + * @param[in] can_node_id node id of OLS or MLS, default: 0x0A + * @param[in] subscribe_topic ros topic to receive input messages of type can_msgs::Frame, default: "can0" + * @param[in] publish_topic ros topic to publish output messages of type can_msgs::Frame, default: "ros2can0" + * @param[in] pdo_rate rate of PDO (default: pdo_rate = 50, i.e. 20 ms between two PDOs or 50 PDOs per second) + * @param[in] pdo_repeat_cnt each sensor state spefied in sick_canopen_simu_cfg.xml is repeated N times before switching to the next state (default: 5 times) + * @param[in] subscribe_queue_size buffer size for ros messages + */ + SimulatorBase(ros::NodeHandle & nh, const std::string & config_file, int can_node_id, const std::string & subscribe_topic, const std::string & publish_topic, const ros::Rate & pdo_rate, int pdo_repeat_cnt, int subscribe_queue_size); + + /* + * Destructor + * + */ + virtual ~SimulatorBase(); + + /* + * @brief Callbacks for ros messages. Converts incoming messages of type can_msgs::Frame to simulate a can device + * and publishes simulation results to the configured ros topic. + * + * param[in] msg ros message of type can_msgs::Frame + */ + virtual void messageHandler(const can_msgs::Frame & msg_in); + + /* + * @brief Interface to listen to the simulated sensor state. A listener implementing this interface can be notified on + * PDOs sent by a simulation by registring using function registerSimulationListener. + * After receiving both the sensor state from the simulation and the following OLS/MLS-Measurement ros message from the driver, + * the listener can check the correctness by comparing both messages. The sensor state from simulation and from OLS/MLS-Measurement + * messages from the driver must be identical, otherwise some failure occured. + */ + class SimulationListener + { + public: + /* + * @brief Notification callback of a listener. Whenever the simulation sends a PDO, registered listener are notified about the current sensor state + * by calling function pdoSent. + */ + virtual void pdoSent(const MsgType & sensor_state) = 0; + }; + + /* + * @brief Registers a listener to a simulation. Whenever the simulation sends a PDO, the listener is notified about the current sensor state. + * After receiving both the sensor state and the following OLS/MLS-Measurement ros message from the driver, the listener can check + * the correctness by comparing the sensor state from simulation and the OLS/MLS-Measurement from the driver. Both must be identical, + * otherwise some failure occured. + * + * param[in] pListener listener to current sensor states sent by PDO. + */ + virtual void registerSimulationListener(SimulationListener* pListener); + + /* + * @brief Unregisters a listener from a simulation. The listener will not be notified about simulated sensor states. + * This function is the opposite to registerSimulationListener. + * + * param[in] pListener listener to current sensor states sent by PDO. + */ + virtual void unregisterSimulationListener(SimulationListener* pListener); + + protected: + + /* + * @brief Parses an pdo element from config file and appends it to m_vec_pdo_measurements. + * + * param[in] xml_pdo pdo element from config file + */ + virtual bool parseXmlPDO(TiXmlElement* xml_pdo) = 0; + + /* + * reads SDO configuration from xml-file + * + * @param[in] config_file configuration file with testcases for OLS and MLS simulation + */ + virtual bool readSDOconfig(const std::string & config_file); + + /* + * reads the PDO configuration from xml-file + * + * @param[in] config_file configuration file with testcases for simulation + * @param[in] sick_device_family "OLS10", "OLS20" or "MLS" + */ + virtual bool readPDOconfig(const std::string & config_file, const std::string & sick_device_family); + + /* + * @brief handles SDOs, i.e. sends a SDO response and returns true, if can frame is a SDO request. + * Otherwise, the can frame is not handled and false is returned. + * Note: The SDO response is simply taken from a lookup-table (input: SDO request frame data, output: SDO response frame data). + * If the SDO request frame data (frame.data) is not found in the lookup-table, the can frame is not handled and false is returned, too. + * + * @param[in] msg_in received can frame + * + * @return true if can frame was handled and a SDO request is sent, false otherwise. + */ + virtual bool handleSDO(const can_msgs::Frame & msg_in); + + /* + * @brief Publishes PDOs to simulate a MLS or OLS20 device while can state is operational + */ + virtual void runPDOthread(void); + + /* + * @brief Convertes an MLS_Measurement into a can_msgs::Frame TPDO1. + * @param[in] measurement MLS_Measurement to be converted + * @param[out] tpdo1_msg output can frame TPDO1, + * @param[out] tpdo2_msg dummy frame TPDO2, unused + * @return Returns the number of TPDOs, i.e. 1 for MLS devices. + */ + virtual int convertToCanFrame(const sick_line_guidance::MLS_Measurement & measurement, can_msgs::Frame & tpdo1_msg, can_msgs::Frame & tpdo2_msg); + + /* + * @brief Convertes an OLS_Measurement into two can_msgs::Frame TPDO1 and TPDO2. + * @param[in] measurement OLS_Measurement to be converted + * @param[out] tpdo1_msg output can frame TPDO1, Byte 1-8 := LCP1(LSB,MSB,0x2021sub1), LCP2(LSB,MSB,0x2021sub2), LCP3(LSB,MSB,0x2021sub3), status(UINT8,0x2021sub4), barcode(UINT8,0x2021sub8) + * @param[out] tpdo2_msg output can frame TPDO2, Byte 1-6 := Width1(LSB,MSB,0x2021sub5), Width2(LSB,MSB,0x2021sub6), Width3(LSB,MSB,0x2021sub7) + * @return Returns the number of TPDOs, i.e. 2 for OLS devices. + */ + virtual int convertToCanFrame(const sick_line_guidance::OLS_Measurement & measurement, can_msgs::Frame & tpdo1_msg, can_msgs::Frame & tpdo2_msg); + + /* + * @brief Converts a position or width (float value in meter) to lcp (INT16 value in millimeter), + * shortcut for std::round(lcp * 1000) with clipping to range INT16_MIN ... INT16_MAX. + * @param[in] lcp position or width (float value in meter) + * @return INT16 value in millimeter + */ + virtual int convertLCP(float lcp); + + /* + * @brief Converts frame.data to uint64_t + * @param[in] frame can frame + * @return frame.data converted to uint64_t + */ + virtual uint64_t frameDataToUInt64(const can_msgs::Frame & frame); + + /* + * @brief Converts uint64_t data to frame.data + * @param[in] u64data frame data (uint64_t) + * @param[in+out] frame can frame + */ + virtual void uint64ToFrameData(uint64_t u64data, can_msgs::Frame & frame); + + /* + * @brief returns an unsigned integer in reverse byte order, + * f.e. revertByteorder(0x12345678) returns 0x78563412. + * @param[in] data input data (unsigned integer) + * @return data in reverse byte order + */ + template static T revertByteorder(T data) + { + T reverted = 0; + for(size_t n = 0; n < sizeof(T); n++) + { + reverted = (reverted << 8); + reverted = (reverted | (data & 0xFF)); + data = data >> 8; + } + return reverted; + } + + /* + * @brief prints and returns a can_msgs::Frame in short format like candump (f.a. "18A#B4FFCCFF00000300") + */ + virtual std::string tostring(const can_msgs::Frame & canframe); + + /* + * member variables + */ + + sick_canopen_simu::CanState m_can_state; // the current can state: INITIALIZATION, PRE_OPERATIONAL, OPERATIONAL or STOPPED + uint8_t m_can_node_id; // node id of OLS or MLS device (default: 0x0A) + ros::Subscriber m_ros_subscriber; // subscriber to handle can messages from master (NMT messages and SDOs) + ros::Publisher m_ros_publisher; // publishes can frames (PDO, SDO response, etc.) + boost::mutex m_ros_publisher_mutex; // lock guard for publishing messages with m_ros_publisher + boost::thread* m_pdo_publisher_thread; // thread to publish PDO messages in can state OPERATIONAL + bool m_pdo_publisher_thread_running; // true while m_pdo_publisher_thread is running + ros::Rate m_pdo_rate; // rate of PDOs (default: pdo_rate = 50, i.e. 20 ms between two PDOs or 50 PDOs per second) + int m_pdo_repeat_cnt; // each sensor state spefied in sick_canopen_simu_cfg.xml is repeated N times before switching to the next state (default: 5 times) + std::map m_sdo_request_response_map; // lookup table: sdo response data := m_sdo_request_response_map[] + std::vector m_vec_pdo_measurements; // list of PDOs to simulate OLS or MLS device + std::vector m_vec_simu_listener; // list of registered listeners to the current sensor state simulated + int m_subscribe_queue_size; // buffer size for ros messages + uint64_t m_sdo_response_dev_state; // response to sdo request for dev_status (object 0x2018): MLS and OLS20: 0x4F18200000000000 (sdo response with UINT8 data), OLS10: 0x4B18200000000000 (sdo response with UINT16 data) + bool m_send_tpdo_immediately; // true (OLS10, MLS): send TPDOs immediately in all states (pre-operational and operational), false (default, OLS20): send TPDOs only in state operational + + }; // SimulatorBase + + /* + * Subclass MLSSimulator extends class SimulatorBase to simulate a MLS device. + * + */ + class MLSSimulator : public SimulatorBase + { + public: + + /* + * Constructor + * + * @param[in] nh ros node handle + * @param[in] config_file configuration file with testcases for OLS and MLS simulation + * @param[in] sick_device_family "OLS10", "OLS20" or "MLS" + * @param[in] can_node_id node id of OLS or MLS, default: 0x0A + * @param[in] subscribe_topic ros topic to receive input messages of type can_msgs::Frame, default: "can0" + * @param[in] publish_topic ros topic to publish output messages of type can_msgs::Frame, default: "ros2can0" + * @param[in] pdo_rate rate of PDO (default: pdo_rate = 50, i.e. 20 ms between two PDOs or 50 PDOs per second) + * @param[in] pdo_repeat_cnt each sensor state spefied in sick_canopen_simu_cfg.xml is repeated N times before switching to the next state (default: 5 times) + * @param[in] subscribe_queue_size buffer size for ros messages + */ + MLSSimulator(ros::NodeHandle & nh, const std::string & config_file, const std::string & sick_device_family, int can_node_id, const std::string & subscribe_topic, const std::string & publish_topic, const ros::Rate & pdo_rate, int pdo_repeat_cnt, int subscribe_queue_size); + + /* + * @brief Callbacks for ros messages. Converts incoming messages of type can_msgs::Frame to simulate a MLS device + * and publishes simulation results to the configured ros topic. + * + * param[in] msg ros message of type can_msgs::Frame + */ + virtual void messageHandler(const can_msgs::Frame & msg_in); + + protected: + + /* + * @brief Parses an pdo element from config file and appends it to m_vec_pdo_measurements. + * + * param[in] xml_pdo pdo element from config file + */ + virtual bool parseXmlPDO(TiXmlElement* xml_pdo); + + }; // MLSSimulator + + /* + * Subclass OLSSimulator extends class SimulatorBase to simulate an OLS devices. + * + */ + class OLSSimulator : public SimulatorBase + { + public: + + /* + * Constructor + * + * @param[in] nh ros node handle + * @param[in] config_file configuration file with testcases for OLS and MLS simulation + * @param[in] sick_device_family "OLS10", "OLS20" or "MLS" + * @param[in] can_node_id node id of OLS or MLS, default: 0x0A + * @param[in] subscribe_topic ros topic to receive input messages of type can_msgs::Frame, default: "can0" + * @param[in] publish_topic ros topic to publish output messages of type can_msgs::Frame, default: "ros2can0" + * @param[in] pdo_rate rate of PDO (default: pdo_rate = 50, i.e. 20 ms between two PDOs or 50 PDOs per second) + * @param[in] pdo_repeat_cnt each sensor state spefied in sick_canopen_simu_cfg.xml is repeated N times before switching to the next state (default: 5 times) + * @param[in] subscribe_queue_size buffer size for ros messages + */ + OLSSimulator(ros::NodeHandle & nh, const std::string & config_file, const std::string & sick_device_family, int can_node_id, const std::string & subscribe_topic, const std::string & publish_topic, const ros::Rate & pdo_rate, int pdo_repeat_cnt, int subscribe_queue_size); + + /* + * @brief Callbacks for ros messages. Converts incoming messages of type can_msgs::Frame to simulate an OLS device + * and publishes simulation results to the configured ros topic. + * + * param[in] msg ros message of type can_msgs::Frame + */ + virtual void messageHandler(const can_msgs::Frame & msg_in); + + protected: + + /* + * @brief Parses an pdo element from config file and appends it to m_vec_pdo_measurements. + * + * param[in] xml_pdo pdo element from config file + */ + virtual bool parseXmlPDO(TiXmlElement* xml_pdo); + + }; // OLSSimulator + + /* + * Subclass OLS10Simulator extends class OLSSimulator to simulate an OLS10 device. + * + */ + class OLS10Simulator : public OLSSimulator + { + public: + + /* + * Constructor + * + * @param[in] nh ros node handle + * @param[in] config_file configuration file with testcases for OLS and MLS simulation + * @param[in] sick_device_family "OLS10", "OLS20" or "MLS" + * @param[in] can_node_id node id of OLS or MLS, default: 0x0A + * @param[in] subscribe_topic ros topic to receive input messages of type can_msgs::Frame, default: "can0" + * @param[in] publish_topic ros topic to publish output messages of type can_msgs::Frame, default: "ros2can0" + * @param[in] pdo_rate rate of PDO (default: pdo_rate = 50, i.e. 20 ms between two PDOs or 50 PDOs per second) + * @param[in] pdo_repeat_cnt each sensor state spefied in sick_canopen_simu_cfg.xml is repeated N times before switching to the next state (default: 5 times) + * @param[in] subscribe_queue_size buffer size for ros messages + */ + OLS10Simulator(ros::NodeHandle & nh, const std::string & config_file, const std::string & sick_device_family, int can_node_id, const std::string & subscribe_topic, const std::string & publish_topic, const ros::Rate & pdo_rate, int pdo_repeat_cnt, int subscribe_queue_size); + + }; // OLS10Simulator + + /* + * Subclass OLS20Simulator extends class OLSSimulator to simulate an OLS20 device. + * + */ + class OLS20Simulator : public OLSSimulator + { + public: + + /* + * Constructor + * + * @param[in] nh ros node handle + * @param[in] config_file configuration file with testcases for OLS and MLS simulation + * @param[in] sick_device_family "OLS10", "OLS20" or "MLS" + * @param[in] can_node_id node id of OLS or MLS, default: 0x0A + * @param[in] subscribe_topic ros topic to receive input messages of type can_msgs::Frame, default: "can0" + * @param[in] publish_topic ros topic to publish output messages of type can_msgs::Frame, default: "ros2can0" + * @param[in] pdo_rate rate of PDO (default: pdo_rate = 50, i.e. 20 ms between two PDOs or 50 PDOs per second) + * @param[in] pdo_repeat_cnt each sensor state spefied in sick_canopen_simu_cfg.xml is repeated N times before switching to the next state (default: 5 times) + * @param[in] subscribe_queue_size buffer size for ros messages + */ + OLS20Simulator(ros::NodeHandle & nh, const std::string & config_file, const std::string & sick_device_family, int can_node_id, const std::string & subscribe_topic, const std::string & publish_topic, const ros::Rate & pdo_rate, int pdo_repeat_cnt, int subscribe_queue_size); + + }; // OLS20Simulator + +} // sick_canopen_simu + +#endif // __SICK_CANOPEN_SIMU_DEVICE_H_INCLUDED diff --git a/Devices/Packages/sick_line_guidance/include/sick_line_guidance/sick_canopen_simu_verify.h b/Devices/Packages/sick_line_guidance/include/sick_line_guidance/sick_canopen_simu_verify.h new file mode 100755 index 0000000..734b3fe --- /dev/null +++ b/Devices/Packages/sick_line_guidance/include/sick_line_guidance/sick_canopen_simu_verify.h @@ -0,0 +1,228 @@ +/* + * sick_canopen_simu_verify tests and verifies the sick_line_guidance ros driver. + * + * sick_canopen_simu_verify listenes to both the simulation (interface sick_canopen_simu::SimulatorBase::SimulationListener) + * and to the ros messages of the sick_line_guidance driver. Whenever a MLS_Measurement or OLS_Measurement message + * is received, the measurement is compared to the current sensor state of the simulation. + * Measurement messages from the driver and sensor states from the simulation should be identical, otherwise an error occured. + * The sick_line_guidance driver test is passed, if no error occured (i.e. all measurement messages from the driver have been + * handled correctly, no mismatches between simulated sensor states and published measurement messages). + * + * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim + * Copyright (C) 2019 SICK AG, Waldkirch + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of SICK AG nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Authors: + * Michael Lehning + * + * Copyright 2019 SICK AG + * Copyright 2019 Ing.-Buero Dr. Michael Lehning + * + */ +#ifndef __SICK_CANOPEN_SIMU_VERIFY_H_INCLUDED +#define __SICK_CANOPEN_SIMU_VERIFY_H_INCLUDED + +#include +#include "sick_line_guidance/sick_canopen_simu_device.h" + +namespace sick_canopen_simu +{ + /* + * Class MeasurementVerification tests and verifies measurement messages from the sick_line_guidance ros driver. + * + * MeasurementVerification listenes to both the simulation (interface sick_canopen_simu::SimulatorBase::SimulationListener) + * and to the ros messages of the sick_line_guidance driver. Whenever a MLS_Measurement or OLS_Measurement message + * is received, the measurement is compared to the current sensor state of the simulation. + * Measurement messages from the driver and sensor states from the simulation should be identical, otherwise an error occured. + * The sick_line_guidance driver test is passed, if no error occured (i.e. all measurement messages from the driver have been + * handled correctly, no mismatches between simulated sensor states and published measurement messages). + */ + template class MeasurementVerification : public sick_canopen_simu::SimulatorBase::SimulationListener + { + public: + + /* + * Constructor + * + * @param[in] nh ros node handle + * @param[in] measurement_subscribe_topic ros topic for measurement messages, default: "mls" resp. "ols" + * @param[in] sensor_state_queue_size buffer size for simulated sensor states, default: 2 + * @param[in] devicename descriptional device name, f.e. "OLS20" or "MLS" + */ + MeasurementVerification(ros::NodeHandle & nh, const std::string & measurement_subscribe_topic, int sensor_state_queue_size, const std::string & devicename); + + /* + * Destructor + * + */ + virtual ~MeasurementVerification(); + + /* + * @brief Implements the notification callback for SimulationListener. Whenever the simulation sends a PDO, + * this function is called by the simulation to notify SimulationListeners about the current sensor state. + */ + virtual void pdoSent(const MsgType & sensor_state); + + /* + * @brief Prints the number of verified measuremente and the number of verification failures. + * @return true in case of no verification failures (all measurements verified successfully), false otherwise. + */ + virtual bool printStatistic(void); + + protected: + + /* + * @brief Callback for measurement messages, called whenever the sick_line_guidance ros driver publishes a + * new measurement message. Compares the measurement message with the sensor states from simulation, + * and reports an error, if an equivalent sensor state hasn't been sent by the simulation. + * + * @param[in] measurement measurement message from sick_line_guidance ros driver + * + * @return true, if the measurement message is equivalent to sensor states (verification passed), or false otherwise (verification failed). + */ + virtual bool verifyMeasurement(const MsgType & measurement); + + /* + * @brief Compares and verifies measurement messages from ros driver against sensor states from simulation. + * + * @param[in] measurement_messages list of measurement messages from ros driver (to be compared to sensor_states) + * @param[in] sensor_states list of sensor states from simulation (to be compared to measurement_messages) + * + * @return true, if the measurement messages are equivalent to sensor states (verification passed), or false otherwise (verification failed). + */ + // virtual bool verifyMeasurements(std::list & measurement_messages, std::list & sensor_states); + virtual bool verifyMeasurements(std::list & measurement_messages, std::list & sensor_states); + virtual bool verifyMeasurements(std::list & measurement_messages, std::list & sensor_states); + + /* + * @brief Compares measurement messages from ros driver against sensor states from simulation, using a specified compare function + * (f.e. floating point comparsion for positions with fabs(x-y) < 1 mm and integer comparsion with x == y for barcodes). + * + * @param[in] measurement_messages list of measurement messages from ros driver (to be compared to sensor_states) + * @param[in] sensor_states list of sensor states from simulation (to be compared to measurement_messages) + * @param[in] cmpfunction compare function, called to compare measurement message A to sensor state B + * + * @return true, if the measurement messages are equivalent to sensor states (verification passed), or false otherwise (verification failed). + */ + template + bool verifyMeasurementData(std::list & measurement_messages, std::list & sensor_states, bool(*cmpfunction)(const T & A, const T & B)); + // virtual bool verifyMeasurementData(std::list & measurement_messages, std::list & sensor_states, bool(*cmpfunction)(const MsgType & A, const MsgType & B)); + + /* + * member variables + */ + std::string m_devicename; // descriptional device name, f.e. "OLS20" or "MLS" + ros::Subscriber m_measurement_subscriber; // subscriber ros measurement messages from sick_line_guidance driver + std::list m_sensor_states; // list of sensor states from simulation + std::list m_measurement_messages; // list of measurement messages from sick_line_guidance driver + boost::mutex m_sensor_states_mutex; // lock guard for access to m_vec_sensor_states + int m_measurement_messages_cnt; // reporting and statistics: number of verified measurement messages + int m_measurement_verification_error_cnt; // reporting and statistics: number of measurement messages with verification errors + int m_measurement_verification_ignored_cnt; // reporting and statistics: number of measurement messages with verification ignored (f.e. SDO response was still pending) + int m_measurement_verification_failed; // reporting and statistics: messages with verification errors + int m_measurement_verification_jitter; // reporting and statistics: verification jitter (max. 1 error tolerated, since measurement messages can be sent while a SDO response is still pending) + + }; // MeasurementVerification + + /* + * Subclass MLSMeasurementVerification extends class MeasurementVerification to verify MLS_Measurement messages. + * + */ + class MLSMeasurementVerification : public MeasurementVerification + { + public: + + /* + * Constructor + * + * @param[in] nh ros node handle + * @param[in] measurement_subscribe_topic ros topic for measurement messages, default: "mls" + * @param[in] sensor_state_queue_size buffer size for simulated sensor states, default: 2 + * @param[in] devicename descriptional device name, f.e. "MLS" + */ + MLSMeasurementVerification(ros::NodeHandle & nh, const std::string & measurement_subscribe_topic, int sensor_state_queue_size, const std::string & devicename); + + /* + * @brief Callback for measurement messages, called whenever the sick_line_guidance ros driver publishes a + * new measurement message. Compares the measurement message with the sensor states from simulation, + * and reports an error, if an equivalent sensor state hasn't been sent by the simulation. + * + * @param[in] measurement measurement message from sick_line_guidance ros driver + */ + virtual void measurementCb(const sick_line_guidance::MLS_Measurement & measurement); + + }; // MLSMeasurementVerification + + /* + * Subclass OLSMeasurementVerification extends class MeasurementVerification to verify OLS_Measurement messages. + * + */ + class OLSMeasurementVerification : public MeasurementVerification + { + public: + + /* + * Constructor + * + * @param[in] nh ros node handle + * @param[in] measurement_subscribe_topic ros topic for measurement messages, default: "ols" + * @param[in] sensor_state_queue_size buffer size for simulated sensor states, default: 2 + * @param[in] devicename descriptional device name, f.e. "OLS20" + */ + OLSMeasurementVerification(ros::NodeHandle & nh, const std::string & measurement_subscribe_topic, int sensor_state_queue_size, const std::string & devicename); + + /* + * @brief Callback for measurement messages, called whenever the sick_line_guidance ros driver publishes a + * new measurement message. Compares the measurement message with the sensor states from simulation, + * and reports an error, if an equivalent sensor state hasn't been sent by the simulation. + * + * @param[in] measurement measurement message from sick_line_guidance ros driver + */ + virtual void measurementCb(const sick_line_guidance::OLS_Measurement & measurement); + + }; // OLSMeasurementVerification + +} // sick_canopen_simu + +#endif // __SICK_CANOPEN_SIMU_VERIFY_H_INCLUDED + diff --git a/Devices/Packages/sick_line_guidance/include/sick_line_guidance/sick_line_guidance_can_cia401_subscriber.h b/Devices/Packages/sick_line_guidance/include/sick_line_guidance/sick_line_guidance_can_cia401_subscriber.h new file mode 100755 index 0000000..f7d9045 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/include/sick_line_guidance/sick_line_guidance_can_cia401_subscriber.h @@ -0,0 +1,163 @@ +/* + * sick_line_guidance_can_cia401_subscriber implements a ros subscriber to canopen ols messages (example CiA401 device for testing). + * + * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim + * Copyright (C) 2019 SICK AG, Waldkirch + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of SICK AG nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Authors: + * Michael Lehning + * + * Copyright 2019 SICK AG + * Copyright 2019 Ing.-Buero Dr. Michael Lehning + * + */ +#ifndef __SICK_LINE_GUIDANCE_CAN_CIA401_SUBSCRIBER_H_INCLUDED +#define __SICK_LINE_GUIDANCE_CAN_CIA401_SUBSCRIBER_H_INCLUDED + +#include "sick_line_guidance/sick_line_guidance_can_ols_subscriber.h" + +namespace sick_line_guidance +{ + + /* + * class CanCiA401Subscriber implements the ros subscriber to canopen ols messages (example CiA401 device for testing). + */ + class CanCiA401Subscriber : public CanOlsSubscriber + { + public: + + /* + * Constructor. + * @param[in] nh ros::NodeHandle + * @param[in] can_nodeid can id for canopen_chain_node, f.e. "node1" + * @param[in] max_num_retries_after_sdo_error After SDO error, the query is repeated max. N times (default: N=2). If the SDO error persists, the can driver is shutdown and restarted. + * @param[in] max_sdo_rate max. sdo query and publish rate + * @param[in] ros_topic topic for ros messages, f.e. "mls" or "ols" + * @param[in] ros_frameid frameid for ros messages, f.e. "mls_measurement_frame" or "ols_measurement_frame" + * @param[in] initial_sensor_state initial sensor state (f.e. 0x07 for 3 detected lines, or (1 << 4) to indicate sensor error) + * @param[in] subscribe_queue_size buffer size for ros messages + */ + CanCiA401Subscriber(ros::NodeHandle & nh, const std::string & can_nodeid, int max_num_retries_after_sdo_error, double max_sdo_rate, const std::string & ros_topic, + const std::string & ros_frameid, int initial_sensor_state = 0, int subscribe_queue_size = 1); + + /* + * Destructor. + */ + virtual ~CanCiA401Subscriber(); + + /* + * @brief subsribes to canopen topics for ols messages and sets the callbacks to handle messages from canopen_chain_node + * after PDO messages (LCP = line center point): + * + * Mapping for OLS: + * + * canopenchain-OLS -> CanSubscriber-callback -> OLS sensor state + * ----------------------------------------------------------------------- + * +"_1001" -> cancallbackError(UINT8) -> Error register + * +"_2018" -> cancallbackDevState(UINT8) -> Device state + * +"_2021sub1" -> cancallbackLCP1(INT16) -> LCP1 + * +"_2021sub2" -> cancallbackLCP2(INT16) -> LCP2 + * +"_2021sub3" -> cancallbackLCP3(INT16) -> LCP3 + * +"_2021sub4" -> cancallbackState(UINT8) -> State + * +"_2021sub5" -> cancallbackWidthLCP1(INT16) -> Width LCP1 + * +"_2021sub6" -> cancallbackWidthLCP2(INT16) -> Width LCP2 + * +"_2021sub7" -> cancallbackWidthLCP3(INT16) -> Width LCP3 + * +"_2021sub8" -> cancallbackCode(UINT8) -> Code + * +"_2021sub9" -> cancallbackExtCode(UINT32) -> Extended Code + * + * Mapping for MLS: + * + * canopenchain-MLS -> CanSubscriber-callback -> MLS sensor state + * ----------------------------------------------------------------------- + * +"_1001" -> cancallbackError(UINT8) -> Error register + * +"_2021sub1" -> cancallbackLCP1(INT16) -> LCP1 + * +"_2021sub2" -> cancallbackLCP2(INT16) -> LCP2 + * +"_2021sub3" -> cancallbackLCP3(INT16) -> LCP3 + * +"_2021sub4" -> cancallbackMarker(UINT8) -> #LCP and marker + * +"_2022" -> cancallbackState(UINT8) -> State + * + * Mapping for CiA402 (example, testing only): + * + * canopenchain-CiA402 -> CanSubscriber-callback -> CiA402 state + * --------------------------------------------------------------------- + * +"_6000sub1" -> cancallbackError(UINT8) -> Error register + * +"_6000sub2" -> cancallbackState(UINT8) -> State + * +"_6000sub3" -> cancallbackCode(UINT8) -> Code + * +"_6000sub4" -> cancallbackExtCode(UINT8) -> LSB Extended Code + * +"_6401sub1" -> cancallbackLCP1(INT16) -> LCP1 + * +"_6401sub2" -> cancallbackLCP2(INT16) -> LCP2 + * +"_6401sub3" -> cancallbackLCP3(INT16) -> LCP3 + * +"_6401sub4" -> cancallbackWidthLCP1(INT16) -> Width LCP1 + * +"_6401sub5" -> cancallbackWidthLCP2(INT16) -> Width LCP2 + * +"_6401sub6" -> cancallbackWidthLCP3(INT16) -> Width LCP3 + * + * See operation manuals for details (file SICK-OLS-Operating_instructions_OLS10_de_IM0076955.PDF for OLS + * and file SICK-MLS-Operating_instructions_MLS_de_IM0076567.PDF for MLS) + * + * @return true on success, otherwise false. + */ + virtual bool subscribeCanTopics(void); + + protected: + + /* + * Callbacks for ros messages from canopen_chain_node. There's one callback for each ros topic + * published by canopen_chain_node. Each callback updates the sensor state and publishes an + * OLS-Measurement message. + */ + + virtual void cancallbackError(const boost::shared_ptr& msg); + virtual void cancallbackLCP1(const boost::shared_ptr& msg); + virtual void cancallbackLCP2(const boost::shared_ptr& msg); + virtual void cancallbackLCP3(const boost::shared_ptr& msg); + virtual void cancallbackState(const boost::shared_ptr& msg); + virtual void cancallbackWidthLCP1(const boost::shared_ptr& msg); + virtual void cancallbackWidthLCP2(const boost::shared_ptr& msg); + virtual void cancallbackWidthLCP3(const boost::shared_ptr& msg); + virtual void cancallbackCode(const boost::shared_ptr& msg); + + }; // class CanCiA401Subscriber + +} // namespace sick_line_guidance +#endif // __SICK_LINE_GUIDANCE_CAN_CIA401_SUBSCRIBER_H_INCLUDED diff --git a/Devices/Packages/sick_line_guidance/include/sick_line_guidance/sick_line_guidance_can_mls_subscriber.h b/Devices/Packages/sick_line_guidance/include/sick_line_guidance/sick_line_guidance_can_mls_subscriber.h new file mode 100755 index 0000000..ff6312b --- /dev/null +++ b/Devices/Packages/sick_line_guidance/include/sick_line_guidance/sick_line_guidance_can_mls_subscriber.h @@ -0,0 +1,160 @@ +/* + * sick_line_guidance_can_mls_subscriber implements a ros subscriber to canopen mls messages. + * + * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim + * Copyright (C) 2019 SICK AG, Waldkirch + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of SICK AG nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Authors: + * Michael Lehning + * + * Copyright 2019 SICK AG + * Copyright 2019 Ing.-Buero Dr. Michael Lehning + * + */ +#ifndef __SICK_LINE_GUIDANCE_CAN_MLS_SUBSCRIBER_H_INCLUDED +#define __SICK_LINE_GUIDANCE_CAN_MLS_SUBSCRIBER_H_INCLUDED + +#include "sick_line_guidance/sick_line_guidance_can_subscriber.h" + +namespace sick_line_guidance +{ + + /* + * class CanMlsSubscriber implements the ros subscriber to canopen mls messages. + */ + class CanMlsSubscriber : public CanSubscriber + { + public: + + /* + * Constructor. + * @param[in] nh ros::NodeHandle + * @param[in] can_nodeid can id for canopen_chain_node, f.e. "node1" + * @param[in] max_num_retries_after_sdo_error After SDO error, the query is repeated max. N times (default: N=2). If the SDO error persists, the can driver is shutdown and restarted. + * @param[in] max_sdo_rate max. sdo query and publish rate + * @param[in] ros_topic topic for ros messages, f.e. "mls" or "ols" + * @param[in] ros_frameid frameid for ros messages, f.e. "mls_measurement_frame" or "ols_measurement_frame" + * @param[in] initial_sensor_state initial sensor state (f.e. 0x07 for 3 detected lines, or (1 << 4) to indicate sensor error) + * @param[in] subscribe_queue_size buffer size for ros messages + */ + CanMlsSubscriber(ros::NodeHandle & nh, const std::string & can_nodeid, int max_num_retries_after_sdo_error, double max_sdo_rate, const std::string & ros_topic, + const std::string & ros_frameid, int initial_sensor_state = 0, int subscribe_queue_size = 1); + + /* + * Destructor. + */ + virtual ~CanMlsSubscriber(); + + /* + * @brief subsribes to canopen topics for mls messages and sets the callbacks to handle messages from canopen_chain_node + * after PDO messages (LCP = line center point): + * + * Mapping for OLS: + * + * canopenchain-OLS -> CanSubscriber-callback -> OLS sensor state + * ----------------------------------------------------------------------- + * +"_1001" -> cancallbackError(UINT8) -> Error register + * +"_2018" -> cancallbackDevState(UINT8) -> Device state + * +"_2021sub1" -> cancallbackLCP1(INT16) -> LCP1 + * +"_2021sub2" -> cancallbackLCP2(INT16) -> LCP2 + * +"_2021sub3" -> cancallbackLCP3(INT16) -> LCP3 + * +"_2021sub4" -> cancallbackState(UINT8) -> State + * +"_2021sub5" -> cancallbackWidthLCP1(INT16) -> Width LCP1 + * +"_2021sub6" -> cancallbackWidthLCP2(INT16) -> Width LCP2 + * +"_2021sub7" -> cancallbackWidthLCP3(INT16) -> Width LCP3 + * +"_2021sub8" -> cancallbackCode(UINT8) -> Code + * +"_2021sub9" -> cancallbackExtCode(UINT32) -> Extended Code + * + * Mapping for MLS: + * + * canopenchain-MLS -> CanSubscriber-callback -> MLS sensor state + * ----------------------------------------------------------------------- + * +"_1001" -> cancallbackError(UINT8) -> Error register + * +"_2021sub1" -> cancallbackLCP1(INT16) -> LCP1 + * +"_2021sub2" -> cancallbackLCP2(INT16) -> LCP2 + * +"_2021sub3" -> cancallbackLCP3(INT16) -> LCP3 + * +"_2021sub4" -> cancallbackMarker(UINT8) -> #LCP and marker + * +"_2022" -> cancallbackState(UINT8) -> State + * + * Mapping for CiA402 (example, testing only): + * + * canopenchain-CiA402 -> CanSubscriber-callback -> CiA402 state + * --------------------------------------------------------------------- + * +"_6000sub1" -> cancallbackError(UINT8) -> Error register + * +"_6000sub2" -> cancallbackState(UINT8) -> State + * +"_6000sub3" -> cancallbackCode(UINT8) -> Code + * +"_6000sub4" -> cancallbackExtCode(UINT8) -> LSB Extended Code + * +"_6401sub1" -> cancallbackLCP1(INT16) -> LCP1 + * +"_6401sub2" -> cancallbackLCP2(INT16) -> LCP2 + * +"_6401sub3" -> cancallbackLCP3(INT16) -> LCP3 + * +"_6401sub4" -> cancallbackWidthLCP1(INT16) -> Width LCP1 + * +"_6401sub5" -> cancallbackWidthLCP2(INT16) -> Width LCP2 + * +"_6401sub6" -> cancallbackWidthLCP3(INT16) -> Width LCP3 + * + * See operation manuals for details (file SICK-OLS-Operating_instructions_OLS10_de_IM0076955.PDF for OLS + * and file SICK-MLS-Operating_instructions_MLS_de_IM0076567.PDF for MLS) + * + * @return true on success, otherwise false. + */ + virtual bool subscribeCanTopics(void); + + protected: + + /* + * Callbacks for ros messages from canopen_chain_node. There's one callback for each ros topic + * published by canopen_chain_node. Each callback updates the sensor state and publishes an + * OLS-Measurement message. + */ + + virtual void cancallbackLCP1(const boost::shared_ptr& msg); + virtual void cancallbackLCP2(const boost::shared_ptr& msg); + virtual void cancallbackLCP3(const boost::shared_ptr& msg); + virtual void cancallbackMarker(const boost::shared_ptr& msg); + virtual void cancallbackState(const boost::shared_ptr& msg); + // virtual void cancallbackError(const boost::shared_ptr& msg); + + }; // class CanMlsSubscriber + +} // namespace sick_line_guidance +#endif // __SICK_LINE_GUIDANCE_CAN_MLS_SUBSCRIBER_H_INCLUDED diff --git a/Devices/Packages/sick_line_guidance/include/sick_line_guidance/sick_line_guidance_can_ols_subscriber.h b/Devices/Packages/sick_line_guidance/include/sick_line_guidance/sick_line_guidance_can_ols_subscriber.h new file mode 100755 index 0000000..6dbb7b3 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/include/sick_line_guidance/sick_line_guidance_can_ols_subscriber.h @@ -0,0 +1,213 @@ +/* + * sick_line_guidance_can_ols_subscriber implements a ros subscriber to canopen ols messages. + * + * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim + * Copyright (C) 2019 SICK AG, Waldkirch + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of SICK AG nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Authors: + * Michael Lehning + * + * Copyright 2019 SICK AG + * Copyright 2019 Ing.-Buero Dr. Michael Lehning + * + */ +#ifndef __SICK_LINE_GUIDANCE_CAN_OLS_SUBSCRIBER_H_INCLUDED +#define __SICK_LINE_GUIDANCE_CAN_OLS_SUBSCRIBER_H_INCLUDED + +#include "sick_line_guidance/sick_line_guidance_can_subscriber.h" + +namespace sick_line_guidance +{ + + /* + * class CanOlsSubscriber implements the base ros subscriber to canopen ols messages. + */ + class CanOlsSubscriber : public CanSubscriber + { + public: + + /* + * Constructor. + * @param[in] nh ros::NodeHandle + * @param[in] can_nodeid can id for canopen_chain_node, f.e. "node1" + * @param[in] max_num_retries_after_sdo_error After SDO error, the query is repeated max. N times (default: N=2). If the SDO error persists, the can driver is shutdown and restarted. + * @param[in] max_sdo_rate max. sdo query and publish rate + * @param[in] ros_topic topic for ros messages, f.e. "mls" or "ols" + * @param[in] ros_frameid frameid for ros messages, f.e. "mls_measurement_frame" or "ols_measurement_frame" + * @param[in] initial_sensor_state initial sensor state (f.e. 0x07 for 3 detected lines, or (1 << 4) to indicate sensor error) + * @param[in] subscribe_queue_size buffer size for ros messages + */ + CanOlsSubscriber(ros::NodeHandle & nh, const std::string & can_nodeid, int max_num_retries_after_sdo_error, double max_sdo_rate, const std::string & ros_topic, + const std::string & ros_frameid, int initial_sensor_state = 0, int subscribe_queue_size = 1); + + /* + * Destructor. + */ + virtual ~CanOlsSubscriber(); + + /* + * @brief subsribes to canopen topics for ols messages and sets the callbacks to handle messages from canopen_chain_node + * after PDO messages (LCP = line center point): + * + * Mapping for OLS: + * + * canopenchain-OLS -> CanSubscriber-callback -> OLS sensor state + * ----------------------------------------------------------------------- + * +"_1001" -> cancallbackError(UINT8) -> Error register + * +"_2018" -> cancallbackDevState(UINT8) -> Device state + * +"_2021sub1" -> cancallbackLCP1(INT16) -> LCP1 + * +"_2021sub2" -> cancallbackLCP2(INT16) -> LCP2 + * +"_2021sub3" -> cancallbackLCP3(INT16) -> LCP3 + * +"_2021sub4" -> cancallbackState(UINT8) -> State + * +"_2021sub5" -> cancallbackWidthLCP1(INT16) -> Width LCP1 + * +"_2021sub6" -> cancallbackWidthLCP2(INT16) -> Width LCP2 + * +"_2021sub7" -> cancallbackWidthLCP3(INT16) -> Width LCP3 + * +"_2021sub8" -> cancallbackCode(UINT8) -> Code + * +"_2021sub9" -> cancallbackExtCode(UINT32) -> Extended Code + * + * Mapping for MLS: + * + * canopenchain-MLS -> CanSubscriber-callback -> MLS sensor state + * ----------------------------------------------------------------------- + * +"_1001" -> cancallbackError(UINT8) -> Error register + * +"_2021sub1" -> cancallbackLCP1(INT16) -> LCP1 + * +"_2021sub2" -> cancallbackLCP2(INT16) -> LCP2 + * +"_2021sub3" -> cancallbackLCP3(INT16) -> LCP3 + * +"_2021sub4" -> cancallbackMarker(UINT8) -> #LCP and marker + * +"_2022" -> cancallbackState(UINT8) -> State + * + * Mapping for CiA402 (example, testing only): + * + * canopenchain-CiA402 -> CanSubscriber-callback -> CiA402 state + * --------------------------------------------------------------------- + * +"_6000sub1" -> cancallbackError(UINT8) -> Error register + * +"_6000sub2" -> cancallbackState(UINT8) -> State + * +"_6000sub3" -> cancallbackCode(UINT8) -> Code + * +"_6000sub4" -> cancallbackExtCode(UINT8) -> LSB Extended Code + * +"_6401sub1" -> cancallbackLCP1(INT16) -> LCP1 + * +"_6401sub2" -> cancallbackLCP2(INT16) -> LCP2 + * +"_6401sub3" -> cancallbackLCP3(INT16) -> LCP3 + * +"_6401sub4" -> cancallbackWidthLCP1(INT16) -> Width LCP1 + * +"_6401sub5" -> cancallbackWidthLCP2(INT16) -> Width LCP2 + * +"_6401sub6" -> cancallbackWidthLCP3(INT16) -> Width LCP3 + * + * See operation manuals for details (file SICK-OLS-Operating_instructions_OLS10_de_IM0076955.PDF for OLS + * and file SICK-MLS-Operating_instructions_MLS_de_IM0076567.PDF for MLS) + * + * @return true on success, otherwise false. + */ + virtual bool subscribeCanTopics(void); + + protected: + + /* + * Callbacks for ros messages from canopen_chain_node. There's one callback for each ros topic + * published by canopen_chain_node. Each callback updates the sensor state and publishes an + * OLS-Measurement message. + */ + + virtual void cancallbackLCP1(const boost::shared_ptr& msg); + virtual void cancallbackLCP2(const boost::shared_ptr& msg); + virtual void cancallbackLCP3(const boost::shared_ptr& msg); + virtual void cancallbackState(const boost::shared_ptr& msg); + virtual void cancallbackWidthLCP1(const boost::shared_ptr& msg); + virtual void cancallbackWidthLCP2(const boost::shared_ptr& msg); + virtual void cancallbackWidthLCP3(const boost::shared_ptr& msg); + virtual void cancallbackCode(const boost::shared_ptr& msg); + // virtual void cancallbackError(const boost::shared_ptr& msg); + // virtual void cancallbackDevState(const boost::shared_ptr& msg); + // virtual void cancallbackExtCode(const boost::shared_ptr& msg); + + bool m_bOLS20queries; // OLS20 only: query objects 2021subA (barcode center point, INT16), 2021subB (quality of lines, UINT8) and 2023sub1 to 2023sub3 (intensity line 1 - 3, UINT8) + + }; // class CanOlsSubscriber + + /* + * class CanOls10Subscriber derives from CanOlsSubscriber to implements OLS10 specific handling of the ros canopen messages. + */ + class CanOls10Subscriber : public CanOlsSubscriber + { + public: + + /* + * Constructor. + * @param[in] nh ros::NodeHandle + * @param[in] can_nodeid can id for canopen_chain_node, f.e. "node1" + * @param[in] max_num_retries_after_sdo_error After SDO error, the query is repeated max. N times (default: N=2). If the SDO error persists, the can driver is shutdown and restarted. + * @param[in] max_sdo_rate max. sdo query and publish rate + * @param[in] ros_topic topic for ros messages, f.e. "mls" or "ols" + * @param[in] ros_frameid frameid for ros messages, f.e. "mls_measurement_frame" or "ols_measurement_frame" + * @param[in] initial_sensor_state initial sensor state (f.e. 0x07 for 3 detected lines, or (1 << 4) to indicate sensor error) + * @param[in] subscribe_queue_size buffer size for ros messages + */ + CanOls10Subscriber(ros::NodeHandle & nh, const std::string & can_nodeid, int max_num_retries_after_sdo_error, double max_sdo_rate, const std::string & ros_topic, + const std::string & ros_frameid, int initial_sensor_state = 0, int subscribe_queue_size = 1); + + }; // class CanOls10Subscriber + + /* + * class CanOls20Subscriber derives from CanOlsSubscriber to implements OLS20 specific handling of the ros canopen messages. + */ + class CanOls20Subscriber : public CanOlsSubscriber + { + public: + + /* + * Constructor. + * @param[in] nh ros::NodeHandle + * @param[in] can_nodeid can id for canopen_chain_node, f.e. "node1" + * @param[in] max_num_retries_after_sdo_error After SDO error, the query is repeated max. N times (default: N=2). If the SDO error persists, the can driver is shutdown and restarted. + * @param[in] max_sdo_rate max. sdo query and publish rate + * @param[in] ros_topic topic for ros messages, f.e. "mls" or "ols" + * @param[in] ros_frameid frameid for ros messages, f.e. "mls_measurement_frame" or "ols_measurement_frame" + * @param[in] initial_sensor_state initial sensor state (f.e. 0x07 for 3 detected lines, or (1 << 4) to indicate sensor error) + * @param[in] subscribe_queue_size buffer size for ros messages + */ + CanOls20Subscriber(ros::NodeHandle & nh, const std::string & can_nodeid, int max_num_retries_after_sdo_error, double max_sdo_rate, const std::string & ros_topic, + const std::string & ros_frameid, int initial_sensor_state = 0, int subscribe_queue_size = 1); + + }; // class CanOls20Subscriber + +} // namespace sick_line_guidance +#endif // __SICK_LINE_GUIDANCE_CAN_OLS_SUBSCRIBER_H_INCLUDED diff --git a/Devices/Packages/sick_line_guidance/include/sick_line_guidance/sick_line_guidance_can_subscriber.h b/Devices/Packages/sick_line_guidance/include/sick_line_guidance/sick_line_guidance_can_subscriber.h new file mode 100755 index 0000000..3e4e1ac --- /dev/null +++ b/Devices/Packages/sick_line_guidance/include/sick_line_guidance/sick_line_guidance_can_subscriber.h @@ -0,0 +1,415 @@ +/* + * sick_line_guidance_can_subscriber implements the base class for ros subscriber to canopen messages for OLS and MLS. + * + * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim + * Copyright (C) 2019 SICK AG, Waldkirch + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of SICK AG nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Authors: + * Michael Lehning + * + * Copyright 2019 SICK AG + * Copyright 2019 Ing.-Buero Dr. Michael Lehning + * + */ +#ifndef __SICK_LINE_GUIDANCE_CAN_SUBSCRIBER_H_INCLUDED +#define __SICK_LINE_GUIDANCE_CAN_SUBSCRIBER_H_INCLUDED + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "sick_line_guidance/MLS_Measurement.h" +#include "sick_line_guidance/OLS_Measurement.h" +#include "sick_line_guidance/sick_line_guidance_msg_util.h" + +namespace sick_line_guidance +{ + + /* + * class CanSubscriber: base class for CanOlsSubscriber and CanMlsSubscriber, + * implements the base class for ros subscriber to canopen messages for OLS and MLS. + * Converts canopen messages to MLS/OLS measurement messages and publishes + * MLS/OLS measurement messages on the configured ros topic ("/mls" or "/ols"). + * + * class CanSubscriber::MeasurementHandler: queries SDOs (if required) and publishes MLS/OLS measurement messages in a background thread + */ + class CanSubscriber + { + public: + + /* + * Constructor. + * @param[in] nh ros::NodeHandle + * @param[in] can_nodeid can id for canopen_chain_node, f.e. "node1" + * @param[in] max_num_retries_after_sdo_error After SDO error, the query is repeated max. N times (default: N=2). If the SDO error persists, the can driver is shutdown and restarted. + * @param[in] max_sdo_rate max. sdo query and publish rate + * @param[in] initial_sensor_state initial sensor state (f.e. 0x07 for 3 detected lines, or (1 << 4) to indicate sensor error) + * @param[in] subscribe_queue_size buffer size for ros messages + */ + CanSubscriber(ros::NodeHandle & nh, const std::string & can_nodeid, int max_num_retries_after_sdo_error = 2, double max_sdo_rate = 1000, int initial_sensor_state = 0, int subscribe_queue_size = 1); + + /* + * Destructor. + */ + virtual ~CanSubscriber(); + + /* + * @brief subsribes to canopen topics for ols or mls messages and sets the callbacks to handle messages from canopen_chain_node + * after PDO messages (LCP = line center point). Implemented by subclasses CanOlsSubscriber and CanMlsSubscriber + * @return true on success, otherwise false. + */ + virtual bool subscribeCanTopics(void) = 0; + + protected: + + /* + * @brief Container class for sdo query support. Just collects the pending status of a query (query pending or not pending) + * and the time of the last successfull query. + */ + class QuerySupport + { + public: + + /* + * Constructor + * @param[in] query_jitter jitter in seconds (default: 10 ms), i.e. a sdo is requested, if the query is pending and the last successful query is out of the time jitter. + * By default, a sdo request is send, if the query is pending and not done within the last 10 ms. + */ + QuerySupport(double query_jitter = 0.01) : m_bQueryPending(false), m_tLastQueryTime(0), m_tQueryJitter(query_jitter) {} + + /* + * returns the pending status, i.e. returns true if the query is pending, returns false if the query is not pending. + */ + virtual bool & pending(void){ return m_bQueryPending; } + + /* + * returns the time of the last successful query + */ + virtual ros::Time & time(void){ return m_tLastQueryTime; } + + /* + * returns true, if a query is required, i.e. the query is pending and the last successful query is over time, otherwise false + */ + virtual bool required(void) { return m_bQueryPending && (ros::Time::now() - m_tLastQueryTime) > m_tQueryJitter; } + + protected: + + bool m_bQueryPending; // true if the query is pending, otherwise false + ros::Time m_tLastQueryTime; // time of the last successful query + ros::Duration m_tQueryJitter; // jitter in seconds, i.e. a sdo is requested, if the query is pending and the last successful query is out of a time jitter. + }; + + /* + * class MeasurementHandler: queries SDOs (if required) and publishes MLS/OLS measurement messages in a background thread + */ + class MeasurementHandler + { + public: + + /* + * Constructor. + * @param[in] nh ros::NodeHandle + * @param[in] can_nodeid can id for canopen_chain_node, f.e. "node1" + * @param[in] max_num_retries_after_sdo_error After SDO error, the query is repeated max. N times (default: N=2). If the SDO error persists, the can driver is shutdown and restarted. + * @param[in] initial_sensor_state initial sensor state (f.e. 0x07 for 3 detected lines, or (1 << 4) to indicate sensor error) + * @param[in] max_publish_rate max rate to publish OLS/MLS measurement messages (default: min. 1 ms between two measurement messages) + * @param[in] max_query_rate max rate to query SDOs if required (default: min. 1 ms between sdo queries) + * @param[in] schedule_publish_delay MLS and OLS measurement message are scheduled to be published 5 milliseconds after first PDO is received + * @param[in] max_publish_delay MLS and OLS measurement message are scheduled to be published max. 2*20 milliseconds after first PDO is received, even if a sdo request is pending (max. 2 * tpdo rate) + * @param[in] query_jitter jitter in seconds (default: 10 ms), i.e. a sdo is requested, if the query is pending and the last successful query is out of the time jitter. + * By default, a sdo request is send, if the query is pending and not done within the last 10 ms. + */ + MeasurementHandler(ros::NodeHandle & nh, const std::string & can_nodeid, int max_num_retries_after_sdo_error = 2, int initial_sensor_state = 0, double max_publish_rate = 1000, double max_query_rate = 1000, double schedule_publish_delay = 0.005, double max_publish_delay = 0.04, double query_jitter = 0.01); + + /* + * Destructor. + */ + virtual ~MeasurementHandler(); + + /* + * @brief Runs the background thread to publish MLS/OLS measurement messages + */ + virtual void runMeasurementPublishThread(void); + + /* + * @brief Runs the background thread to query SDOs, if required + */ + virtual void runMeasurementSDOqueryThread(void); + + /* + * @brief schedules the publishing of the current MLS measurement message. + * @param[in] schedule if true, publishing is scheduled, otherwise a possibly pending schedule is removed. + */ + virtual void schedulePublishMLSMeasurement(bool schedule); + + /* + * @brief schedules the publishing of the current OLS measurement message. + * @param[in] schedule if true, publishing is scheduled, otherwise a possibly pending schedule is removed. + */ + virtual void schedulePublishOLSMeasurement(bool schedule); + + /* + * MeasurementHandler member data. + */ + + ros::NodeHandle m_nh; // ros nodehandel + std::string m_can_nodeid; // can id for canopen_chain_node, f.e. "node1" + ros::Publisher m_ros_publisher; // ros publisher for ols or mls measurement messages + boost::mutex m_measurement_mutex; // lock guard for publishing and setting mls/ols sensor state + sick_line_guidance::MLS_Measurement m_mls_state; // current state of an mls sensor + sick_line_guidance::OLS_Measurement m_ols_state; // current state of an ols sensor + ros::Rate m_max_publish_rate; // max. rate to publish OLS/MLS measurement message + ros::Rate m_max_sdo_query_rate; // max. rate to query SDOs if required + ros::Time m_publish_mls_measurement; // time to publish next MLS measurement message + ros::Time m_publish_ols_measurement; // time to publish next OLS measurement message + ros::Time m_publish_measurement_latest; // latest time to publish a measurement message (even if a sdo request is pending) + boost::mutex m_publish_measurement_mutex; // lock guard to schedule publishing measurements using m_publish_mls_measurement and m_publish_ols_measurement + ros::Duration m_schedule_publish_delay; // MLS and OLS measurement message are scheduled to be published 5 milliseconds after first PDO is received + ros::Duration m_max_publish_delay; // MLS and OLS measurement message are scheduled to be published max. 2*20 milliseconds after first PDO is received, even if a sdo request is pending (max. 2 * tpdo rate) + QuerySupport m_ols_query_extended_code; // query object 0x2021sub9 (extended code, UINT32) in object dictionary by SDO, if pending + QuerySupport m_ols_query_device_status_u8; // query object 0x2018 (device status register, OLS20: UINT8) in object dictionary by SDO (query runs in m_measurement in a background thread), if pending + QuerySupport m_ols_query_device_status_u16; // query object 0x2018 (device status register, OLS10: UINT16) in object dictionary by SDO (query runs in m_measurement in a background thread), if pending + QuerySupport m_ols_query_error_register; // query object 0x1001 (error register, UINT8) in object dictionary by SDO (query runs in m_measurement in a background thread), if pending + QuerySupport m_ols_query_barcode_center_point; // OLS20 only: query object 2021subA (barcode center point, INT16), if pending + QuerySupport m_ols_query_quality_of_lines; // OLS20 only: query object 2021subB (quality of lines, UINT8), if pending + QuerySupport m_ols_query_intensity_of_lines[3]; // OLS20 only: query object 2023sub1 to 2023sub3 (intensity lines 1 - 3, UINT8), if pending + boost::thread * m_measurement_publish_thread; // background thread to publishes MLS/OLS measurement messages + boost::thread * m_measurement_sdo_query_thread; // background thread to query SDOs if required + int m_max_num_retries_after_sdo_error; // After SDO error, the query is repeated max. N times (default: N=2). If the SDO error persists, the can driver is shutdown and restarted. + + protected: + + /* + * @brief returns true, if publishing of a MLS measurement is scheduled and time has been reached for publishing the current MLS measurement. + */ + virtual bool isMLSMeasurementTriggered(void); + + /* + * @brief returns true, if publishing of a OLS measurement is scheduled and time has been reached for publishing the current OLS measurement. + */ + virtual bool isOLSMeasurementTriggered(void); + + /* + * @brief returns true, if publishing of a measurement is scheduled and latest time for publishing has been reached. + */ + virtual bool isLatestTimeForMeasurementPublishing(void); + + /* + * @brief returns true, if sdo query is pending, i.e. measurement is not yet completed (sdo request or sdo response still pending) + */ + virtual bool isSDOQueryPending(void); + + /* + * @brief queries an object in the object dictionary by SDO and returns its value. + * @param[in] can_object_idx object index in object dictionary, f.e. "2018" (OLS device status) or "2021sub9" (OLS extended code) + * @param[in] max_num_retries_after_sdo_error After SDO error, the query is repeated max. N times (default: N=2). If the SDO error persists, the can driver is shutdown and restarted. + * @param[out] can_object_value object value from SDO response + * @return true on success (can_object_value set to objects value), false otherwise (can_object_value not set) + */ + virtual bool querySDO(const std::string & can_object_idx, int max_num_retries_after_sdo_error, uint8_t & can_object_value); + + /* + * @brief queries an object in the object dictionary by SDO and returns its value. + * @param[in] can_object_idx object index in object dictionary, f.e. "2018" (OLS device status) or "2021sub9" (OLS extended code) + * @param[in] max_num_retries_after_sdo_error After SDO error, the query is repeated max. N times (default: N=2). If the SDO error persists, the can driver is shutdown and restarted. + * @param[out] can_object_value object value from SDO response + * @return true on success (can_object_value set to objects value), false otherwise (can_object_value not set) + */ + virtual bool querySDO(const std::string & can_object_idx, int max_num_retries_after_sdo_error, uint16_t & can_object_value); + + /* + * @brief queries an object in the object dictionary by SDO and returns its value. + * @param[in] can_object_idx object index in object dictionary, f.e. "2018" (OLS device status) or "2021sub9" (OLS extended code) + * @param[in] max_num_retries_after_sdo_error After SDO error, the query is repeated max. N times (default: N=2). If the SDO error persists, the can driver is shutdown and restarted. + * @param[out] can_object_value object value from SDO response + * @return true on success (can_object_value set to objects value), false otherwise (can_object_value not set) + */ + virtual bool querySDO(const std::string & can_object_idx, int max_num_retries_after_sdo_error, int16_t & can_object_value); + + /* + * @brief queries an object in the object dictionary by SDO and returns its value. + * @param[in] can_object_idx object index in object dictionary, f.e. "2018" (OLS device status) or "2021sub9" (OLS extended code) + * @param[in] max_num_retries_after_sdo_error After SDO error, the query is repeated max. N times (default: N=2). If the SDO error persists, the can driver is shutdown and restarted. + * @param[out] can_object_value object value from SDO response + * @return true on success (can_object_value set to objects value), false otherwise (can_object_value not set) + */ + virtual bool querySDO(const std::string & can_object_idx, int max_num_retries_after_sdo_error, uint32_t & can_object_value); + + /* + * @brief queries an object in the object dictionary by SDO and returns its value. + * @param[in] can_object_idx object index in object dictionary, f.e. "2018" (OLS device status) or "2021sub9" (OLS extended code) + * @param[in] max_num_retries_after_sdo_error After SDO error, the query is repeated max. N times (default: N=2). If the SDO error persists, the can driver is shutdown and restarted. + * @param[out] can_object_value object value from SDO response + * @return true on success (can_object_value set to objects value), false otherwise (can_object_value not set) + */ + virtual bool querySDO(const std::string & can_object_idx, int max_num_retries_after_sdo_error, std::string & can_object_value); + + /* + * @brief queries an object value by SDO, if bQueryPending==true. After SDO query returned, output_value is set and bQueryPending cleared + * (assume bQueryPending==false after this function returned). + * + * @param[in+out] query if query.required() is true, object value is queried by SDO and query is updated (not pending any more). Otherwise, nothing is done. + * @param[in] object_index index in object dictionary, f.e. "2021sub9" for object 0x2021 subindex 9 + * @param[in] max_num_retries_after_sdo_error After SDO error, the query is repeated max. N times (default: N=2). If the SDO error persists, the can driver is shutdown and restarted. + * @param[out] output_value object value queried by SDO + * @param[in] norm_factor factor to convert object to output value, f.e. 0.001 to convert millimeter (object value) to meter (output value). Default: 1 + * + * @return uint8_t value + */ + template void querySDOifPending(QuerySupport & query, const std::string & object_index, int max_num_retries_after_sdo_error, T & output_value, T norm_factor) + { + if(query.pending()) + { + S sdo_value; + if(query.required() && querySDO(object_index, max_num_retries_after_sdo_error, sdo_value)) + { + query.time() = ros::Time::now(); + if(query.pending()) + { + ROS_INFO_STREAM("sick_line_guidance::CanSubscriber::MeasurementHandler: [" << object_index << "]=" << sick_line_guidance::MsgUtil::toHexString(sdo_value)); + boost::lock_guard publish_lockguard(m_measurement_mutex); + output_value = (norm_factor * sdo_value); + } + } + query.pending() = false; + } + } + + /* + * @brief converts a sdo response to uint8. + * Note: nh.serviceClient returns SDO responses as strings. + * In case of 1 Byte (UINT8) values, the returned "string" contains this byte (uint8 value streamed to std::stringstream). + * Example: UINT8 with value 0 are returned as "\0", not "0". Parsing the SDO response has to handle the UINT8 case, + * which is done by this function + * Note: std::exception are caught (error message and return false in this case) + * @param[in] response sdo response as string + * @param[out] value uint8 value converted from SDO response + * @return true on success, false otherwise + */ + virtual bool convertSDOresponse(const std::string & response, uint8_t & value); + + /* + * @brief converts a sdo response to uint32. + * Note: std::exception are caught (error message and return false in this case) + * @param[in] response sdo response as string + * @param[out] value uint32 value converted from SDO response + * @return true on success, false otherwise + */ + virtual bool convertSDOresponse(const std::string & response, uint32_t & value); + + /* + * @brief converts a sdo response to int32. + * Note: std::exception are caught (error message and return false in this case) + * @param[in] response sdo response as string + * @param[out] value uint32 value converted from SDO response + * @return true on success, false otherwise + */ + virtual bool convertSDOresponse(const std::string & response, int32_t & value); + + }; // class MeasurementHandler + + /* + * @brief converts an std_msgs::UInt8/UInt16/UInt32 message to a uint8_t/uint16_t/uint32_t value. + * + * @param[in] msg UINT8 message + * @param[in] defaultvalue default, is returned in case of an invalid message + * @param[in] maxvalue 0xFF, 0xFFFF or 0xFFFFFFFF + * @param[in] dbg_info informal debug message (prints to ROS_DEBUG or ROS_ERROR) + * + * @return uint8_t value + */ + template T convertIntegerMessage(const boost::shared_ptr& msg, const T & defaultvalue, unsigned maxvalue, const std::string & dbg_info) + { + T value = defaultvalue; + if(msg) + { + value = ((msg->data)&maxvalue); + ROS_DEBUG("sick_line_guidance::CanSubscriber(%s): data: 0x%02x", dbg_info.c_str(), (unsigned)value); + } + else + { + ROS_ERROR("## ERROR sick_line_guidance::CanSubscriber(%s): invalid message (%s:%d)", dbg_info.c_str(), __FILE__, __LINE__); + } + return value; + } + + /* + * @brief converts an INT16 message (line center point lcp in millimeter) to a float lcp in meter. + * + * @param[in] msg INT16 message (line center point lcp in millimeter) + * @param[in] defaultvalue default, is returned in case of an invalid message + * @param[in] dbg_info informal debug message (ROS_INFO/ROS_ERROR) + * + * @return float lcp in meter + */ + virtual float convertToLCP(const boost::shared_ptr& msg, const float & defaultvalue, const std::string & dbg_info); + + /* + * @brief schedules the current MLS measurement message for publishing. + */ + virtual void publishMLSMeasurement(void); + + /* + * @brief schedules the current OLS measurement message for publishing. + */ + virtual void publishOLSMeasurement(void); + + /* + * member data. + */ + + std::vector m_can_subscriber; // list of subscriber to canopen_chain_node messages + int m_subscribe_queue_size; // buffer size for ros messages (default: 1) + MeasurementHandler m_measurement; // handles MLS/OLS measurement messages, queries SDO if requiered and publishes ros measurement messages. + + }; // class CanSubscriber + +} // namespace sick_line_guidance +#endif // __SICK_LINE_GUIDANCE_CAN_SUBSCRIBER_H_INCLUDED diff --git a/Devices/Packages/sick_line_guidance/include/sick_line_guidance/sick_line_guidance_canopen_chain.h b/Devices/Packages/sick_line_guidance/include/sick_line_guidance/sick_line_guidance_canopen_chain.h new file mode 100755 index 0000000..ce772eb --- /dev/null +++ b/Devices/Packages/sick_line_guidance/include/sick_line_guidance/sick_line_guidance_canopen_chain.h @@ -0,0 +1,229 @@ +/* + * sick_line_guidance_canopen_chain wraps and adapts RosChain of package canopen_chain_node for sick_line_guidance. + * + * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim + * Copyright (C) 2019 SICK AG, Waldkirch + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of SICK AG nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Authors: + * Michael Lehning + * + * Copyright 2019 SICK AG + * Copyright 2019 Ing.-Buero Dr. Michael Lehning + * + */ +#ifndef __SICK_LINE_GUIDANCE_CANOPEN_CHAIN_H_INCLUDED +#define __SICK_LINE_GUIDANCE_CANOPEN_CHAIN_H_INCLUDED + +#include +#include +#include + +namespace sick_line_guidance +{ + + /* + * class CanopenChain implements canopen support for sick_line_guidance + * based on RosChain of package canopen_chain_node (package ros_canopen). + */ + class CanopenChain : public canopen::RosChain + { + public: + + /* + * Constructor + * + * @param[in] nh ros::NodeHandle + * @param[in] nh_priv ros::NodeHandle + */ + CanopenChain(const ros::NodeHandle &nh, const ros::NodeHandle &nh_priv); + + /* + * Destructor + */ + virtual ~CanopenChain(); + + /* + * Connects to CAN bus by calling "init" of canopen_chain_node ros-service + * + * @param[in] nh ros::NodeHandle + * + * @return true, if initialization successful, otherwise false. + */ + static bool connectInitService(ros::NodeHandle &nh); + + /* + * Sets all dcf overlays in the can devices. All dcf values are reread from the can device and checked to be identical + * to the configured object value. If all dcf overlays have been set successfully (i.e. successfull write operation, + * successfull re-read operation and object value identical to the configured value), true is returned. + * Otherwise, false is returned (i.e. some dcf overlays could not be set). + * + * dcf overlays are a list of key value pairs with "object_index" as key and "object_value" as value. + * Examples for dcf overlays can be found in the yaml configuration file. Example from sick_line_guidance_ols20.yaml: + * node1: + * id: 0x0A # CAN-Node-ID of can device, default: Node-ID 10=0x0A for OLS and MLS + * eds_pkg: sick_line_guidance # package name for relative path + * eds_file: SICK_OLS20.eds # path to EDS/DCF file + * ... + * dcf_overlay: + * "2001sub5": "0x01" # sensorFlipped, UINT8, DataType=0x0005, defaultvalue 0x00 + * "2002sub1": "0.015" # Typ. Width, FLOAT32, DataType=0x0008 + * "2002sub2": "0.001" # Min. Width, FLOAT32, DataType=0x0008 + * "2002sub3": "0.050" # Max. Width, FLOAT32, DataType=0x0008 + * + * @param[in] nh ros::NodeHandle + * @param[in] node_id can node id of the can device + * @param[in] dcf_overlays list of dcf overlays, format "object_index":"object_value" + * + * @return true, if dcf overlays set successfully, otherwise false (write error, read error + * or queried value differs from configured object value). + */ + static bool writeDCFoverlays(ros::NodeHandle &nh, const std::string & node_id, XmlRpc::XmlRpcValue & dcf_overlays); + + /* + * Queries an object of a can node from canopen service by its object index. + * + * @param[in] nh ros::NodeHandle + * @param[in] node_id can node id of the can device + * @param[in] object index in the object dictonary of the can device, f.e. "1001sub", "1018sub4" + * @param[in] max_num_retries_after_sdo_error After SDO error, the query is repeated max. N times (default: N=2). If the SDO error persists, the can driver is shutdown and restarted. + * @param[out] output_message informational message in case of errors (responce from canopen service) + * @param[out] output_value value of the object (responce from canopen service) + * + * @return true, if query successful, otherwise false. + */ + static bool queryCanObject(ros::NodeHandle &nh, const std::string & node_id, const std::string & objectidx, int max_num_retries_after_sdo_error, + std::string & output_message, std::string & output_value); + + /* + * Sets the value of an object in the object dictionary of a can device. + * + * @param[in] nh ros::NodeHandle + * @param[in] node_id can node id of the can device + * @param[in] object index in the object dictonary of the can device, f.e. "1001sub", "1018sub4" + * @param[in] object_value value of the object + * @param[out] response value of the object (responce from canopen service) + * + * @return true, if SDO successful, otherwise false. + */ + static bool setCanObjectValue(ros::NodeHandle &nh, const std::string & node_id, const std::string & objectidx, + const std::string & object_value, std::string & response); + + /* + * Recovers can driver after emergency or other errors. Calls "/driver/recover" from canopen_chain_node + * + * @param[in] nh ros::NodeHandle + * + * @return true, if recover command returned with success, otherwise false. + */ + static bool recoverCanDriver(ros::NodeHandle &nh); + + /* + * Shutdown can driver. Calls "/driver/shutdown" from canopen_chain_node + * + * @param[in] nh ros::NodeHandle + * + * @return true, if shutdown command returned with success, otherwise false. + */ + static bool shutdownCanDriver(ros::NodeHandle & nh); + + protected: + + /* + * Compares a destination value configured by dcf_overlay with the sdo response. + * Comparison by string, integer or float comparison. + * Returns true, if both values are identical, or otherwise false. + * Note: dcf_overlay_value and sdo_response may represent identical numbers, but different strings (different number formatting) + * Therefor, both string and numbers are compared in this function. + * + * @param[in] dcf_overlay_value configured value + * @param[in] sdo_response received value + * + * @return true, if dcf_overlay_value is identical to sdo_response or has identical values. + */ + static bool cmpDCFoverlay(const std::string & dcf_overlay_value, const std::string & sdo_response); + + /* + * Converts a string to UINT32. Catches conversion exceptions and returns false in case of number parsing errors. + * + * @param[in] str input string to be parsed + * @param[out] value output value (== input converted to number in case of success) + * + * @return true in case of success, false otherwise + */ + static uint32_t tryParseUINT32(const std::string & str, uint32_t & value); + + /* + * Converts a string to INT32. Catches conversion exceptions and returns false in case of number parsing errors. + * + * @param[in] str input string to be parsed + * @param[out] value output value (== input converted to number in case of success) + * + * @return true in case of success, false otherwise + */ + static int32_t tryParseINT32(const std::string & str, int32_t & value); + + /* + * Converts a string to FLOAT32. Catches conversion exceptions and returns false in case of number parsing errors. + * + * @param[in] str input string to be parsed + * @param[out] value output value (== input converted to number in case of success) + * + * @return true in case of success, false otherwise + */ + static int32_t tryParseFLOAT(const std::string & str, float & value); + + /* + * Prints and converts a sdo response. 1 byte datatypes (UINT8) are converted to decimal (f.e. "\0" is returned as "0"). + * + * @param[in] response sdo response + * + * @return printed response. + */ + static std::string sdoResponseToString(const std::string & response); + + + }; // class CanopenChain + +} // namespace sick_line_guidance +#endif // __SICK_LINE_GUIDANCE_CANOPEN_CHAIN_H_INCLUDED diff --git a/Devices/Packages/sick_line_guidance/include/sick_line_guidance/sick_line_guidance_cloud_converter.h b/Devices/Packages/sick_line_guidance/include/sick_line_guidance/sick_line_guidance_cloud_converter.h new file mode 100755 index 0000000..5dd42ec --- /dev/null +++ b/Devices/Packages/sick_line_guidance/include/sick_line_guidance/sick_line_guidance_cloud_converter.h @@ -0,0 +1,154 @@ +/* + * sick_line_guidance_cloud_converter converts sensor measurement data to PointCloud2. + * + * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim + * Copyright (C) 2019 SICK AG, Waldkirch + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of SICK AG nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Authors: + * Michael Lehning + * + * Copyright 2019 SICK AG + * Copyright 2019 Ing.-Buero Dr. Michael Lehning + * + */ +#ifndef __SICK_LINE_GUIDANCE_CLOUD_CONVERTER_H_INCLUDED +#define __SICK_LINE_GUIDANCE_CLOUD_CONVERTER_H_INCLUDED + +#include +#include "sick_line_guidance/MLS_Measurement.h" +#include "sick_line_guidance/OLS_Measurement.h" + +namespace sick_line_guidance +{ + + /* + * class CloudConverter implements utility functions to convert measurement data to PointCloud2. + */ + class CloudConverter + { + public: + + /* + * @brief converts OLS measurement data to PointCloud2. + * + * @param[in] measurement OLS measurement data + * @param[in]frame_id frame_id of PointCloud2 message + * + * @return sensor_msgs::PointCloud2 data converted from measurement + */ + static sensor_msgs::PointCloud2 convert(const sick_line_guidance::OLS_Measurement & measurement, const std::string & frame_id); + + /* + * @brief converts OLS measurement data to PointCloud2. + * + * @param[in] measurement OLS measurement data + * @param[in]frame_id frame_id of PointCloud2 message + * + * @return sensor_msgs::PointCloud2 data converted from measurement + */ + static sensor_msgs::PointCloud2 convert(const sick_line_guidance::MLS_Measurement & measurement, const std::string & frame_id); + + /* + * @brief prints a PointCloud2 data field to string. + * + * @param[in] cloud PointCloud2 data + * + * @return PointCloud2 data converted to string + */ + static std::string cloudDataToString(const sensor_msgs::PointCloud2 & cloud); + + protected: + + /* + * @brief returns the status for each line (detected or not detected). + * + * @param[in] status status byte of measurement data + * @param[in] numlines number of lined (normally 3 lines) + * + * @return detection status for each line + */ + static std::vector linestatus(uint8_t status, size_t numlines); + + /* + * @brief returns the number of lines detected (1, 2 or 3 lines). + * + * @param[in] status status byte of measurement data + * + * @return number of lines detected by OLS or MLS + */ + static int linenumber(uint8_t status); + + /* + * @brief returns true, if MLS measurement status is okay, or false otherwise. + * + * @param[in] measurement MLS measurement data + * + * @return true (sensor status okay), or false (sensor status not okay) + */ + static bool measurementstatus(const sick_line_guidance::MLS_Measurement & measurement); + + /* + * @brief converts and prints a single field of PointCloud2 according to its dataype. + * + * @param[in] datatype enumeration of the datatye, see sensor_msgs::PointField + * @param[in] data pointer to the data to be converted and printed + * @param[in] end pointer to the end of PointCloud2 data + * + * @return data field converted to string + */ + static std::string cloudDataFieldToString(uint8_t datatype, const uint8_t* data, const uint8_t* end); + + /* + * @brief flips the bits of a code, i.e. reverses the bits (output bit 0 := input bit 31, output bit 1 := input bit 30, and so on) + * + * @param[in] code bits to be flipped + * + * @return flipped code + */ + static uint32_t flipBits(uint32_t code); + + }; // class CloudConverter + +} // namespace sick_line_guidance +#endif // __SICK_LINE_GUIDANCE_CLOUD_CONVERTER_H_INCLUDED diff --git a/Devices/Packages/sick_line_guidance/include/sick_line_guidance/sick_line_guidance_diagnostic.h b/Devices/Packages/sick_line_guidance/include/sick_line_guidance/sick_line_guidance_diagnostic.h new file mode 100755 index 0000000..f22d5f1 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/include/sick_line_guidance/sick_line_guidance_diagnostic.h @@ -0,0 +1,160 @@ +/* + * sick_line_guidance_diagnostic publishes diagnostic messages for sick_line_guidance + * + * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim + * Copyright (C) 2019 SICK AG, Waldkirch + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of SICK AG nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Authors: + * Michael Lehning + * + * Copyright 2019 SICK AG + * Copyright 2019 Ing.-Buero Dr. Michael Lehning + * + */ +#ifndef __SICK_LINE_GUIDANCE_DIAGNOSTIC_H_INCLUDED +#define __SICK_LINE_GUIDANCE_DIAGNOSTIC_H_INCLUDED + +#include + +namespace sick_line_guidance +{ + /* + * enum DIAGNOSTIC_STATUS enumerates the possible status values of diagnostic messages. + * Higher values mean more severe failures. + */ + typedef enum DIAGNOSTIC_STATUS_ENUM + { + OK, // status okay, no errors + EXIT, // sick_line_guidance exiting + NO_LINE_DETECTED, // device signaled "no line detected" + ERROR_STATUS, // device signaled an error (error flag set in TPDO) + SDO_COMMUNICATION_ERROR, // error in SDO query, timeout on receiving SDO response + CAN_COMMUNICATION_ERROR, // can communication error, shutdown and reset communication + CONFIGURATION_ERROR, // invalid configuration, check configuration files + INITIALIZATION_ERROR, // initialization of CAN driver failed + INTERNAL_ERROR // internal error, should never happen + + } DIAGNOSTIC_STATUS; + + /* + * class Diagnostic publishes diagnostic messages for sick_line_guidance + */ + class Diagnostic + { + public: + + /* + * Initializes the global diagnostic handler. + * + * @param[in] nh ros::NodeHandle + * @param[in] publish_topic ros topic to publish diagnostic messages + * @param[in] component description of the component reporting + */ + static void init(ros::NodeHandle & nh, const std::string & publish_topic, const std::string & component) + { + g_diagnostic_handler.init(nh, publish_topic, component); + } + + /* + * Updates and reports the current status. + * + * @param[in] status current status (OK, ERROR, ...) + * @param[in] message optional diagnostic message + */ + static void update(DIAGNOSTIC_STATUS status, const std::string & message = "") + { + g_diagnostic_handler.update(status, message); + } + + protected: + + /* + * class DiagnosticImpl implements diagnostics for sick_line_guidance + */ + class DiagnosticImpl + { + public: + + /* + * Constructor. + */ + DiagnosticImpl(); + + /* + * Initialization. + * + * @param[in] nh ros::NodeHandle + * @param[in] publish_topic ros topic to publish diagnostic messages + * @param[in] component description of the component reporting + */ + void init(ros::NodeHandle & nh, const std::string & publish_topic, const std::string & component); + + /* + * Updates and reports the current status. + * + * @param[in] status current status (OK, ERROR, ...) + * @param[in] message optional diagnostic message + */ + void update(DIAGNOSTIC_STATUS status, const std::string & message = ""); + + protected: + + /* + * member data. + */ + + bool m_diagnostic_initialized; // flag indicating proper initialization of diagnostics + ros::Publisher m_diagnostic_publisher; // publishes diagnostic messages + std::string m_diagnostic_component; // name of the component publishing diagnostic messages + + }; // class DiagnosticImpl + + + static DiagnosticImpl g_diagnostic_handler; // singleton, implements the diagnostics for sick_line_guidance + + }; // class Diagnostic + +} // namespace sick_line_guidance +#endif // __SICK_LINE_GUIDANCE_DIAGNOSTIC_H_INCLUDED + diff --git a/Devices/Packages/sick_line_guidance/include/sick_line_guidance/sick_line_guidance_eds_util.h b/Devices/Packages/sick_line_guidance/include/sick_line_guidance/sick_line_guidance_eds_util.h new file mode 100755 index 0000000..c07634f --- /dev/null +++ b/Devices/Packages/sick_line_guidance/include/sick_line_guidance/sick_line_guidance_eds_util.h @@ -0,0 +1,110 @@ +/* + * sick_line_guidance_eds_util implements utility functions for eds-files. + * + * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim + * Copyright (C) 2019 SICK AG, Waldkirch + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of SICK AG nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Authors: + * Michael Lehning + * + * Copyright 2019 SICK AG + * Copyright 2019 Ing.-Buero Dr. Michael Lehning + * + */ +#ifndef __SICK_LINE_GUIDANCE_EDS_UTIL_H_INCLUDED +#define __SICK_LINE_GUIDANCE_EDS_UTIL_H_INCLUDED + +#include + +namespace sick_line_guidance +{ + + /* + * class EdsUtil implements utility functions for eds-files. + */ + class EdsUtil + { + public: + + /* + * @brief returns a full qualified filepath from package name and file name. + * + * @param package package name + * + * @return full qualified filepath (or filename if package is empty or couldn't be found). + */ + static std::string filepathFromPackage(const std::string & package, const std::string & filename); + + /* + * @brief reads and parses an eds-file and prints all objects. + * + * @param eds_filepath path to eds-file + * + * @return object dictionary of a given eds-file printed to string. + */ + static std::string readParsePrintEdsfile(const std::string & eds_filepath); + + /* + * @brief prints the data of a canopen::ObjectDict::Entry value to std::string. + * + * @param entry object dictionary entry to be printed + * + * @return entry converted to string. + */ + static std::string printObjectDictEntry(const canopen::ObjectDict::Entry & entry); + + /* + * @brief prints the data bytes of a canopen::HoldAny value to std::string. + * + * @param parameter_name name of the parameter + * @param holdany value to be printed + * + * @return value converted to string. + */ + static std::string printHoldAny(const std::string & parameter_name, const canopen::HoldAny & holdany); + + }; // class EdsUtil + +} // namespace sick_line_guidance +#endif // __SICK_LINE_GUIDANCE_EDS_UTIL_H_INCLUDED diff --git a/Devices/Packages/sick_line_guidance/include/sick_line_guidance/sick_line_guidance_msg_util.h b/Devices/Packages/sick_line_guidance/include/sick_line_guidance/sick_line_guidance_msg_util.h new file mode 100755 index 0000000..640d085 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/include/sick_line_guidance/sick_line_guidance_msg_util.h @@ -0,0 +1,276 @@ +/* + * sick_line_guidance_msg_util implements utility functions for ros messages. + * + * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim + * Copyright (C) 2019 SICK AG, Waldkirch + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of SICK AG nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Authors: + * Michael Lehning + * + * Copyright 2019 SICK AG + * Copyright 2019 Ing.-Buero Dr. Michael Lehning + * + */ +#ifndef __SICK_LINE_GUIDANCE_MSG_UTIL_H_INCLUDED +#define __SICK_LINE_GUIDANCE_MSG_UTIL_H_INCLUDED + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "sick_line_guidance/MLS_Measurement.h" +#include "sick_line_guidance/OLS_Measurement.h" + +namespace sick_line_guidance +{ + + /* + * class MsgUtil implements utility functions for ros messages. + */ + class MsgUtil + { + public: + + /* + * @brief compares two messages, returns true if content is equal, or false otherwise. + * + * @param[in] msg1 message to be compared to msg2 + * @param[in] msg2 message to be compared to msg1 + * + * @return true if message content is equal, false otherwise. + */ + template static bool msgIdentical(const T & msg1, const T & msg2) + { + std::stringstream s1; + std::stringstream s2; + s1 << msg1; + s2 << msg2; + return s1.str() == s2.str(); + } + + /* + * @brief shortcut to convert an uint8 value to hex string "0x...". + * + * @param[in] value value to be printed + * + * @return hex string . + */ + static std::string toHexString(uint8_t value) + { + return toHexString(static_cast(value & 0xFF), 2); + } + + /* + * @brief shortcut to convert an uint16 value to hex string "0x...". + * + * @param[in] value value to be printed + * + * @return hex string . + */ + static std::string toHexString(uint16_t value) + { + return toHexString(static_cast(value & 0xFFFF), 4); + } + + /* + * @brief shortcut to convert an uint16 value to hex string "0x...". + * + * @param[in] value value to be printed + * + * @return hex string . + */ + static std::string toHexString(int16_t value) + { + std::stringstream str; + str << toHexString(static_cast(value)); + str << "=" << value << "d"; + return str.str(); + } + + /* + * @brief shortcut to convert an uint32 value to hex string "0x...". + * + * @param[in] value value to be printed + * + * @return hex string . + */ + static std::string toHexString(uint32_t value, int w = 8) + { + std::stringstream str; + str << "0x" << std::uppercase << std::hex << std::setfill('0') << std::setw(w) << value; + return str.str(); + } + + /* + * @brief prints and returns a MLS measurement as informational string + * @param[in] measurement_msg MLS measurement to print + * @return informational string containing the MLS measurement data + */ + static std::string toInfo(const sick_line_guidance::MLS_Measurement & measurement_msg); + + /* + * @brief prints and returns a OLS measurement as informational string + * @param[in] measurement_msg OLS measurement to print + * @return informational string containing the OLS measurement data + */ + static std::string toInfo(const sick_line_guidance::OLS_Measurement & measurement_msg); + + /* + * @brief initializes a MLS measurement with zero data + * @param[in+out] measurement_msg MLS measurement + */ + static void zero(sick_line_guidance::MLS_Measurement & measurement_msg); + + /* + * @brief initializes an OLS measurement with zero data + * @param[in+out] measurement_msg OLS measurement + */ + static void zero(sick_line_guidance::OLS_Measurement & measurement_msg); + + /* + * @brief Returns a MLS sensor measurement + * + * @param[in] lcp1 line center point LCP1 in meter, object 0x2021sub1 in object dictionary + * @param[in] lcp2 line center point LCP2 in meter, object 0x2021sub2 in object dictionary + * @param[in] lcp3 line center point LCP3 in meter, object 0x2021sub3 in object dictionary + * @param[in] status status of measurement, object 0x2022 in object dictionary (Bit7=MSBit to Bit0=LSBit): Bit7: 0, Bit6: ReadingCode, Bit5: Polarity, Bit4: SensorFlipped, Bit3: LineLevBit2, Bit2: LineLevBit1, Bit1: LineLevBit0, Bit0: Linegood(1:LineGood,0:NotGood) + * @param[in] lcp LCP-flags (signs and line assignment), in bits (MSB to LSB): Bit7: MarkerBit4, Bit6: MarkerBit3, Bit5: MarkerBit2, Bit4: MarkerBit1, Bit3: MarkerBit0, Bit2: #LCPBit2, Bit1: #LCPBit1, Bit0: #LCPBit0 + * @param[in] error error register, object 0x1001 in object dictionary + * @param[in] msg_frame_id frame id of OLS_Measurement message + * + * @return parameter converted to MLS_Measurement + */ + static sick_line_guidance::MLS_Measurement convertMLSMessage(float lcp1, float lcp2, float lcp3, uint8_t status, uint8_t lcp, uint8_t error, const std::string & msg_frame_id); + + /* + * @brief Returns an OLS sensor measurement + * + * @param[in] lcp1 line center point LCP1 in meter, object 0x2021sub1 in object dictionary + * @param[in] lcp2 line center point LCP2 in meter, object 0x2021sub2 in object dictionary + * @param[in] lcp3 line center point LCP3 in meter, object 0x2021sub3 in object dictionary + * @param[in] width1 width of line 1 in meter, object 0x2021sub5 in object dictionary + * @param[in] width2 width of line 2 in meter, object 0x2021sub6 in object dictionary + * @param[in] width3 width of line 3 in meter, object 0x2021sub7 in object dictionary + * @param[in] status status of measurement, object 0x2021sub4 in object dictionary, in bits (MSB to LSB): Bit7: CodeValid, Bit6: CodeFlipped, Bit5: x, Bit4: DeviceStatus, Bit3: x, Bit2: #LCPBit2, Bit1: #LCPBit1, Bit0: #LCPBit0 + * @param[in] barcode Barcode (> 255: extended barcode), , object 0x2021sub8 and 0x2021sub9 in object dictionary + * @param[in] dev_status Device status, object 0x2018 in object dictionary + * @param[in] error error register, object 0x1001 in object dictionary + * @param[in] barcodecenter barcode_center_point, OLS20 only (0x2021subA), OLS10: always 0 + * @param[in] linequality quality_of_lines, OLS20 only (0x2021subB), OLS10: always 0 + * @param[in] lineintensity1 intensity_of_lines[0], OLS20 only (0x2023sub1), OLS10: always 0 + * @param[in] lineintensity2 intensity_of_lines[1], OLS20 only (0x2023sub2), OLS10: always 0 + * @param[in] lineintensity3 intensity_of_lines[2], OLS20 only (0x2023sub3), OLS10: always 0 + * @param[in] msg_frame_id frame id of OLS_Measurement message + * + * @return parameter converted to OLS_Measurement + */ + static sick_line_guidance::OLS_Measurement convertOLSMessage(float lcp1, float lcp2, float lcp3, float width1, float width2, float width3, uint8_t status, uint32_t barcode, uint8_t dev_status, uint8_t error, + float barcodecenter, uint8_t linequality, uint8_t lineintensity1, uint8_t lineintensity2, uint8_t lineintensity3, const std::string & msg_frame_id); + + /* + * @brief returns true, if OLS device status is okay, or false otherwise. + * + * @param[in] measurement OLS measurement data + * + * @return true (sensor status okay), or false (sensor status not okay) + */ + static bool statusOK(const sick_line_guidance::OLS_Measurement & measurement) + { + // OLS status bit 4: 0 => Sensor ok, 1 => Sensor not ok, see 0x2018 (measurement.dev_status) + return ((measurement.status & (0x1 << 4)) == 0); + } + + /* + * @brief returns true, if OLS device detected a line, or false otherwise. + * + * @param[in] measurement OLS measurement data + * + * @return true (line good), or false (no line detected) + */ + static bool lineOK(const sick_line_guidance::OLS_Measurement & measurement) + { + return ((measurement.status & 0x7) != 0); // Bit 0-2 OLS status == 0 => no line found + } + + /* + * @brief returns true, if MLS device detected a line, or false otherwise. + * + * @param[in] measurement MLS measurement data + * + * @return true (line good), or false (no line detected) + */ + static bool lineOK(const sick_line_guidance::MLS_Measurement & measurement) + { + // MLS status bit 0 ("Line good") == 0 => no line detected or line too weak, 1 => line detected, MLS #lcp (bit 0-2 == 0) => no line detected + return ((measurement.status & 0x1) != 0) && ((measurement.lcp & 0x7) != 0); + } + + /* + * @brief returns a gaussian destributed random line center position (lcp) + * + * @param stddev standard deviation of gaussian random generator + * + * @return random line center position + */ + static float randomLCP(double stddev); + + /* + * @brief returns a gaussian destributed random line width + * + * @param stddev standard deviation of gaussian random generator + * + * @return random line width + */ + static float randomLW(double min_linewidth, double max_linewidth); + + }; // class MsgUtil + +} // namespace sick_line_guidance +#endif // __SICK_LINE_GUIDANCE_MSG_UTIL_H_INCLUDED diff --git a/Devices/Packages/sick_line_guidance/include/sick_line_guidance/sick_line_guidance_version.h b/Devices/Packages/sick_line_guidance/include/sick_line_guidance/sick_line_guidance_version.h new file mode 100755 index 0000000..60b7c1b --- /dev/null +++ b/Devices/Packages/sick_line_guidance/include/sick_line_guidance/sick_line_guidance_version.h @@ -0,0 +1,89 @@ +/* + * sick_line_guidance_version handles the version of sick_line_guidance ros driver. + * The version concates major version, minor version and patch level, each with three digits. + * + * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim + * Copyright (C) 2019 SICK AG, Waldkirch + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of SICK AG nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Authors: + * Michael Lehning + * + * Copyright 2019 SICK AG + * Copyright 2019 Ing.-Buero Dr. Michael Lehning + * + */ +#ifndef __SICK_LINE_GUIDANCE_VERSION_H_INCLUDED +#define __SICK_LINE_GUIDANCE_VERSION_H_INCLUDED + +#define SICK_LINE_GUIDANCE_MAJOR_VER 1 +#define SICK_LINE_GUIDANCE_MINOR_VER 0 +#define SICK_LINE_GUIDANCE_PATCH_LEVEL 0 + +namespace sick_line_guidance +{ + + /* + * class Version handles the version of sick_line_guidance ros driver. + */ + class Version + { + public: + + /* + * @brief returns the version as a string, f.e. "001.002.003". + * + * @return version string. + */ + static std::string getVersionInfo(void) + { + std::stringstream str; + str << std::uppercase << std::setfill('0') << std::setw(3) << SICK_LINE_GUIDANCE_MAJOR_VER << "."; + str << std::uppercase << std::setfill('0') << std::setw(3) << SICK_LINE_GUIDANCE_MINOR_VER << "."; + str << std::uppercase << std::setfill('0') << std::setw(3) << SICK_LINE_GUIDANCE_PATCH_LEVEL; + return str.str(); + } + + }; // class Version +} // namespace sick_line_guidance +#endif // __SICK_LINE_GUIDANCE_VERSION_H_INCLUDED diff --git a/Devices/Packages/sick_line_guidance/launch/sick_canopen_simu.launch b/Devices/Packages/sick_line_guidance/launch/sick_canopen_simu.launch new file mode 100755 index 0000000..f3aa6cf --- /dev/null +++ b/Devices/Packages/sick_line_guidance/launch/sick_canopen_simu.launch @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + + diff --git a/Devices/Packages/sick_line_guidance/launch/sick_line_guidance.launch b/Devices/Packages/sick_line_guidance/launch/sick_line_guidance.launch new file mode 100755 index 0000000..0743553 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/launch/sick_line_guidance.launch @@ -0,0 +1,35 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Devices/Packages/sick_line_guidance/launch/sick_line_guidance_can2ros_node.launch b/Devices/Packages/sick_line_guidance/launch/sick_line_guidance_can2ros_node.launch new file mode 100755 index 0000000..96581b3 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/launch/sick_line_guidance_can2ros_node.launch @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/Devices/Packages/sick_line_guidance/launch/sick_line_guidance_ols20_twin.launch b/Devices/Packages/sick_line_guidance/launch/sick_line_guidance_ols20_twin.launch new file mode 100755 index 0000000..75f8849 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/launch/sick_line_guidance_ols20_twin.launch @@ -0,0 +1,45 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Devices/Packages/sick_line_guidance/launch/sick_line_guidance_ros2can_node.launch b/Devices/Packages/sick_line_guidance/launch/sick_line_guidance_ros2can_node.launch new file mode 100755 index 0000000..af49cea --- /dev/null +++ b/Devices/Packages/sick_line_guidance/launch/sick_line_guidance_ros2can_node.launch @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/Devices/Packages/sick_line_guidance/mls/SICK-MLS-MLS.eds b/Devices/Packages/sick_line_guidance/mls/SICK-MLS-MLS.eds new file mode 100755 index 0000000..48102d9 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/mls/SICK-MLS-MLS.eds @@ -0,0 +1,483 @@ +[FileInfo] +CreatedBy=Tim Vogt +ModifiedBy=Tim Vogt +Description=Magnetic Lane Sensor +CreationTime=11:15AM +CreationDate=10-10-2017 +ModificationTime=11:15AM +ModificationDate=10-10-2017 +FileName=MLS.eds +FileVersion=0x01 +FileRevision=0x01 +EDSVersion=4.0 + +[DeviceInfo] +VendorName=SICK AG +VendorNumber=0x01000056 +ProductName=Magnetic Lane Sensor +BaudRate_10=0 +BaudRate_20=0 +BaudRate_50=0 +BaudRate_125=1 +BaudRate_250=0 +BaudRate_500=0 +BaudRate_800=0 +BaudRate_1000=0 +SimpleBootUpMaster=0 +SimpleBootUpSlave=1 +Granularity=0 +DynamicChannelsSupported=0 +CompactPDO=0 +GroupMessaging=0 +NrOfRXPDO=0 +NrOfTXPDO=1 +LSS_Supported=1 + +[DummyUsage] +Dummy0001=0 +Dummy0002=1 +Dummy0003=1 +Dummy0004=1 +Dummy0005=1 +Dummy0006=1 +Dummy0007=1 + +[Comments] +Lines=0 + +[MandatoryObjects] +SupportedObjects=3 +1=0x1000 +2=0x1001 +3=0x1018 + +[1000] +ParameterName=Device Type +ObjectType=0x7 +DataType=0x0007 +AccessType=ro +DefaultValue=0x00 +PDOMapping=0 + +[1001] +ParameterName=Error Register +ObjectType=0x7 +DataType=0x0005 +AccessType=ro +PDOMapping=0 + +[1018] +ParameterName=Identity Object +ObjectType=0x9 +SubNumber=5 + +[1018sub0] +ParameterName=Number of entries +ObjectType=0x7 +DataType=0x0005 +AccessType=ro +DefaultValue=4 +PDOMapping=0 + +[1018sub1] +ParameterName=Vendor Id +ObjectType=0x7 +DataType=0x0007 +AccessType=ro +DefaultValue=0x01000056 +PDOMapping=0 + +[1018sub2] +ParameterName=Product Code +ObjectType=0x7 +DataType=0x0007 +AccessType=ro +DefaultValue=0x00001100 +PDOMapping=0 + +[1018sub3] +ParameterName=Revision number +ObjectType=0x7 +DataType=0x0007 +AccessType=ro +DefaultValue=0x00000002 +PDOMapping=0 + +[1018sub4] +ParameterName=Serial number +ObjectType=0x7 +DataType=0x0007 +AccessType=ro +PDOMapping=0 + +[OptionalObjects] +SupportedObjects=10 +1=0x1008 +2=0x1009 +3=0x100A +4=0x100C +5=0x100D +6=0x1017 +7=0x1200 +8=0x1800 +9=0x1A00 +10=0x1F80 + +[1008] +ParameterName=Manufacturer Device Name +ObjectType=0x7 +DataType=0x0009 +AccessType=const +PDOMapping=0 + +[1009] +ParameterName=Manufacturer Hardware Version +ObjectType=0x7 +DataType=0x0009 +AccessType=const +PDOMapping=0 + +[100A] +ParameterName=Manufacturer Software Version +ObjectType=0x7 +DataType=0x0009 +AccessType=const +PDOMapping=0 + +[100C] +ParameterName=Guard Time +ObjectType=0x7 +DataType=0x0006 +AccessType=rw +DefaultValue=0 +PDOMapping=0 + +[100D] +ParameterName=Life Time Factor +ObjectType=0x7 +DataType=0x0005 +AccessType=rw +DefaultValue=0x00 +PDOMapping=0 + +[1017] +ParameterName=Producer Heartbeat Time +ObjectType=0x7 +DataType=0x0006 +AccessType=rw +DefaultValue=0 +PDOMapping=0 + +[1200] +ParameterName=Server SDO Parameter 0 +ObjectType=0x9 +SubNumber=3 + +[1200sub0] +ParameterName=Number of entries +ObjectType=0x7 +DataType=0x0005 +AccessType=ro +DefaultValue=2 +PDOMapping=0 +LowLimit=0x02 +HighLimit=0x02 + +[1200sub1] +ParameterName=COB ID Client to Server +ObjectType=0x7 +DataType=0x0007 +AccessType=ro +DefaultValue=$NODEID+0x600 +PDOMapping=0 + +[1200sub2] +ParameterName=COB ID Server to Client +ObjectType=0x7 +DataType=0x0007 +AccessType=ro +DefaultValue=$NODEID+0x580 +PDOMapping=0 + +[1800] +ParameterName=Transmit PDO Communication Parameter 0 +ObjectType=0x9 +SubNumber=5 + +[1800sub0] +ParameterName=Number of entries +ObjectType=0x7 +DataType=0x0005 +AccessType=ro +DefaultValue=5 +PDOMapping=0 + +[1800sub1] +ParameterName=COB ID +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=$NODEID+0x180 +PDOMapping=0 +LowLimit=0x00000181 +HighLimit=0xFFFFFFFF + +[1800sub2] +ParameterName=Transmission Type +ObjectType=0x7 +DataType=0x0005 +AccessType=ro +DefaultValue=0xFF +PDOMapping=0 + +[1800sub4] +ParameterName=Compatibility Entry +ObjectType=0x7 +DataType=0x0005 +AccessType=rw +PDOMapping=0 + +[1800sub5] +ParameterName=Event Timer +ObjectType=0x7 +DataType=0x0006 +AccessType=rw +PDOMapping=0 + +[1A00] +ParameterName=Transmit PDO Mapping Parameter 0 +ObjectType=0x9 +SubNumber=6 + +[1A00sub0] +ParameterName=Number of entries +ObjectType=0x7 +DataType=0x0005 +AccessType=const +DefaultValue=5 +PDOMapping=0 +LowLimit=0 +HighLimit=5 + +[1A00sub1] +ParameterName=PDO Mapping Entry +ObjectType=0x7 +DataType=0x0007 +AccessType=const +DefaultValue=0x20210110 +PDOMapping=0 + +[1A00sub2] +ParameterName=PDO Mapping Entry_2 +ObjectType=0x7 +DataType=0x0007 +AccessType=const +DefaultValue=0x20210210 +PDOMapping=0 + +[1A00sub3] +ParameterName=PDO Mapping Entry_3 +ObjectType=0x7 +DataType=0x0007 +AccessType=const +DefaultValue=0x20210310 +PDOMapping=0 + +[1A00sub4] +ParameterName=PDO Mapping Entry_4 +ObjectType=0x7 +DataType=0x0007 +AccessType=const +DefaultValue=0x20210408 +PDOMapping=0 + +[1A00sub5] +ParameterName=PDO Mapping Entry_5 +ObjectType=0x7 +DataType=0x0007 +AccessType=const +DefaultValue=0x20220008 +PDOMapping=0 + +[1F80] +ParameterName=NMTStartup +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +PDOMapping=0 + +[ManufacturerObjects] +SupportedObjects=14 +1=0x2019 +2=0x2020 +3=0x2021 +4=0x2022 +5=0x2023 +6=0x2024 +7=0x2025 +8=0x2026 +9=0x2027 +10=0x2028 +11=0x2029 +12=0x202A +13=0x202B +14=0x202C + +[2019] +ParameterName=Order Number +ObjectType=0x7 +DataType=0x0007 +AccessType=ro +PDOMapping=0 + +[2020] +ParameterName=Passwort +ObjectType=0x7 +DataType=0x0009 +AccessType=wo +PDOMapping=0 + +[2021] +ParameterName=SSPs +ObjectType=0x9 +SubNumber=5 + +[2021sub0] +ParameterName=Number of SSPs +ObjectType=0x7 +DataType=0x0005 +AccessType=ro +DefaultValue=0x04 +PDOMapping=0 + +[2021sub1] +ParameterName=SSP1 +ObjectType=0x7 +DataType=0x0003 +AccessType=ro +PDOMapping=1 + +[2021sub2] +ParameterName=SSP2 +ObjectType=0x7 +DataType=0x0003 +AccessType=ro +PDOMapping=1 + +[2021sub3] +ParameterName=SSP3 +ObjectType=0x7 +DataType=0x0003 +AccessType=ro +PDOMapping=1 + +[2021sub4] +ParameterName=#SSP +ObjectType=0x7 +DataType=0x0005 +AccessType=ro +PDOMapping=1 + +[2022] +ParameterName=Status +ObjectType=0x7 +DataType=0x0005 +AccessType=ro +PDOMapping=1 + +[2023] +ParameterName=Track Level +ObjectType=0x7 +DataType=0x0006 +AccessType=ro +PDOMapping=0 + +[2024] +ParameterName=Field Level +ObjectType=0x7 +DataType=0x0006 +AccessType=ro +PDOMapping=0 + +[2025] +ParameterName=Min Level +ObjectType=0x7 +DataType=0x0006 +AccessType=rw +PDOMapping=0 + +[2026] +ParameterName=Offset +ObjectType=0x7 +DataType=0x0003 +AccessType=rw +PDOMapping=0 + +[2027] +ParameterName=Sensor flipped +ObjectType=0x7 +DataType=0x0001 +AccessType=rw +PDOMapping=0 + +[2028] +ParameterName=Markers +ObjectType=0x9 +SubNumber=4 + +[2028sub0] +ParameterName=NrOfObjects +ObjectType=0x7 +DataType=0x0005 +AccessType=ro +DefaultValue=3 +PDOMapping=0 + +[2028sub1] +ParameterName=Use Markers +ObjectType=0x7 +DataType=0x0001 +AccessType=rw +PDOMapping=0 + +[2028sub2] +ParameterName=Marker Style +ObjectType=0x7 +DataType=0x0005 +AccessType=rw +PDOMapping=0 + +[2028sub3] +ParameterName=Fail Safe Mode +ObjectType=0x7 +DataType=0x0001 +AccessType=rw +PDOMapping=0 + +[2029] +ParameterName=Lock Teach +ObjectType=0x7 +DataType=0x0001 +AccessType=rw +PDOMapping=0 + +[202A] +ParameterName=Set Param to Default +ObjectType=0x7 +DataType=0x0001 +AccessType=wo +PDOMapping=0 + +[202B] +ParameterName=Trigger User Offset Calibration +ObjectType=0x7 +DataType=0x0001 +AccessType=wo +PDOMapping=0 + +[202C] +ParameterName=Trigger Zero Position Teach +ObjectType=0x7 +DataType=0x0001 +AccessType=wo +PDOMapping=0 diff --git a/Devices/Packages/sick_line_guidance/mls/SICK-MLS.eds b/Devices/Packages/sick_line_guidance/mls/SICK-MLS.eds new file mode 100755 index 0000000..0eb5520 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/mls/SICK-MLS.eds @@ -0,0 +1,483 @@ +[FileInfo] +CreatedBy=Tim Vogt +ModifiedBy=Tim Vogt +Description=Magnetic Lane Sensor +CreationTime=11:15AM +CreationDate=10-10-2017 +ModificationTime=11:15AM +ModificationDate=10-10-2017 +FileName=MLS.eds +FileVersion=0x01 +FileRevision=0x01 +EDSVersion=4.0 + +[DeviceInfo] +VendorName=SICK AG +VendorNumber=0x01000056 +ProductName=Magnetic Lane Sensor +BaudRate_10=0 +BaudRate_20=0 +BaudRate_50=0 +BaudRate_125=1 +BaudRate_250=0 +BaudRate_500=0 +BaudRate_800=0 +BaudRate_1000=0 +SimpleBootUpMaster=0 +SimpleBootUpSlave=1 +Granularity=0 +DynamicChannelsSupported=0 +CompactPDO=0 +GroupMessaging=0 +NrOfRXPDO=0 +NrOfTXPDO=1 +LSS_Supported=1 + +[DummyUsage] +Dummy0001=0 +Dummy0002=1 +Dummy0003=1 +Dummy0004=1 +Dummy0005=1 +Dummy0006=1 +Dummy0007=1 + +[Comments] +Lines=0 + +[MandatoryObjects] +SupportedObjects=3 +1=0x1000 +2=0x1001 +3=0x1018 + +[1000] +ParameterName=Device Type +ObjectType=0x7 +DataType=0x0007 +AccessType=ro +DefaultValue=0x00 +PDOMapping=0 + +[1001] +ParameterName=Error Register +ObjectType=0x7 +DataType=0x0005 +AccessType=ro +PDOMapping=0 + +[1018] +ParameterName=Identity Object +ObjectType=0x9 +SubNumber=5 + +[1018sub0] +ParameterName=Number of entries +ObjectType=0x7 +DataType=0x0005 +AccessType=ro +DefaultValue=4 +PDOMapping=0 + +[1018sub1] +ParameterName=Vendor Id +ObjectType=0x7 +DataType=0x0007 +AccessType=ro +DefaultValue=0x01000056 +PDOMapping=0 + +[1018sub2] +ParameterName=Product Code +ObjectType=0x7 +DataType=0x0007 +AccessType=ro +DefaultValue=0x00001100 +PDOMapping=0 + +[1018sub3] +ParameterName=Revision number +ObjectType=0x7 +DataType=0x0007 +AccessType=ro +DefaultValue=0x00000002 +PDOMapping=0 + +[1018sub4] +ParameterName=Serial number +ObjectType=0x7 +DataType=0x0007 +AccessType=ro +PDOMapping=0 + +[OptionalObjects] +SupportedObjects=10 +1=0x1008 +2=0x1009 +3=0x100A +4=0x100C +5=0x100D +6=0x1017 +7=0x1200 +8=0x1800 +9=0x1A00 +10=0x1F80 + +[1008] +ParameterName=Manufacturer Device Name +ObjectType=0x7 +DataType=0x0009 +AccessType=const +PDOMapping=0 + +[1009] +ParameterName=Manufacturer Hardware Version +ObjectType=0x7 +DataType=0x0009 +AccessType=const +PDOMapping=0 + +[100A] +ParameterName=Manufacturer Software Version +ObjectType=0x7 +DataType=0x0009 +AccessType=const +PDOMapping=0 + +[100C] +ParameterName=Guard Time +ObjectType=0x7 +DataType=0x0006 +AccessType=rw +DefaultValue=0 +PDOMapping=0 + +[100D] +ParameterName=Life Time Factor +ObjectType=0x7 +DataType=0x0005 +AccessType=rw +DefaultValue=0x00 +PDOMapping=0 + +[1017] +ParameterName=Producer Heartbeat Time +ObjectType=0x7 +DataType=0x0006 +AccessType=rw +DefaultValue=0 +PDOMapping=0 + +[1200] +ParameterName=Server SDO Parameter 0 +ObjectType=0x9 +SubNumber=3 + +[1200sub0] +ParameterName=Number of entries +ObjectType=0x7 +DataType=0x0005 +AccessType=ro +DefaultValue=2 +PDOMapping=0 +LowLimit=0x02 +HighLimit=0x02 + +[1200sub1] +ParameterName=COB ID Client to Server +ObjectType=0x7 +DataType=0x0007 +AccessType=ro +DefaultValue=$NODEID+0x600 +PDOMapping=0 + +[1200sub2] +ParameterName=COB ID Server to Client +ObjectType=0x7 +DataType=0x0007 +AccessType=ro +DefaultValue=$NODEID+0x580 +PDOMapping=0 + +[1800] +ParameterName=Transmit PDO Communication Parameter 0 +ObjectType=0x9 +SubNumber=5 + +[1800sub0] +ParameterName=Number of entries +ObjectType=0x7 +DataType=0x0005 +AccessType=ro +DefaultValue=5 +PDOMapping=0 + +[1800sub1] +ParameterName=COB ID +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=$NODEID+0x180 +PDOMapping=0 +LowLimit=0x00000181 +HighLimit=0xFFFFFFFF + +[1800sub2] +ParameterName=Transmission Type +ObjectType=0x7 +DataType=0x0005 +AccessType=ro +DefaultValue=0xFF +PDOMapping=0 + +[1800sub4] +ParameterName=Compatibility Entry +ObjectType=0x7 +DataType=0x0005 +AccessType=rw +PDOMapping=0 + +[1800sub5] +ParameterName=Event Timer +ObjectType=0x7 +DataType=0x0006 +AccessType=rw +PDOMapping=0 + +[1A00] +ParameterName=Transmit PDO Mapping Parameter 0 +ObjectType=0x9 +SubNumber=6 + +[1A00sub0] +ParameterName=Number of entries +ObjectType=0x7 +DataType=0x0005 +AccessType=const +DefaultValue=5 +PDOMapping=0 +LowLimit=0 +HighLimit=5 + +[1A00sub1] +ParameterName=PDO Mapping Entry +ObjectType=0x7 +DataType=0x0007 +AccessType=const +DefaultValue=0x20210110 +PDOMapping=0 + +[1A00sub2] +ParameterName=PDO Mapping Entry_2 +ObjectType=0x7 +DataType=0x0007 +AccessType=const +DefaultValue=0x20210210 +PDOMapping=0 + +[1A00sub3] +ParameterName=PDO Mapping Entry_3 +ObjectType=0x7 +DataType=0x0007 +AccessType=const +DefaultValue=0x20210310 +PDOMapping=0 + +[1A00sub4] +ParameterName=PDO Mapping Entry_4 +ObjectType=0x7 +DataType=0x0007 +AccessType=const +DefaultValue=0x20210408 +PDOMapping=0 + +[1A00sub5] +ParameterName=PDO Mapping Entry_5 +ObjectType=0x7 +DataType=0x0007 +AccessType=const +DefaultValue=0x20220008 +PDOMapping=0 + +[1F80] +ParameterName=NMTStartup +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +PDOMapping=0 + +[ManufacturerObjects] +SupportedObjects=14 +1=0x2019 +2=0x2020 +3=0x2021 +4=0x2022 +5=0x2023 +6=0x2024 +7=0x2025 +8=0x2026 +9=0x2027 +10=0x2028 +11=0x2029 +12=0x202A +13=0x202B +14=0x202C + +[2019] +ParameterName=Order Number +ObjectType=0x7 +DataType=0x0007 +AccessType=ro +PDOMapping=0 + +[2020] +ParameterName=Passwort +ObjectType=0x7 +DataType=0x0009 +AccessType=wo +PDOMapping=0 + +[2021] +ParameterName=SSPs +ObjectType=0x9 +SubNumber=5 + +[2021sub0] +ParameterName=Number of SSPs +ObjectType=0x7 +DataType=0x0005 +AccessType=ro +DefaultValue=0x04 +PDOMapping=0 + +[2021sub1] +ParameterName=SSP1 +ObjectType=0x7 +DataType=0x0003 +AccessType=ro +PDOMapping=1 + +[2021sub2] +ParameterName=SSP2 +ObjectType=0x7 +DataType=0x0003 +AccessType=ro +PDOMapping=1 + +[2021sub3] +ParameterName=SSP3 +ObjectType=0x7 +DataType=0x0003 +AccessType=ro +PDOMapping=1 + +[2021sub4] +ParameterName=#SSP +ObjectType=0x7 +DataType=0x0005 +AccessType=ro +PDOMapping=1 + +[2022] +ParameterName=Status +ObjectType=0x7 +DataType=0x0005 +AccessType=ro +PDOMapping=1 + +[2023] +ParameterName=Track Level +ObjectType=0x7 +DataType=0x0006 +AccessType=ro +PDOMapping=0 + +[2024] +ParameterName=Field Level +ObjectType=0x7 +DataType=0x0006 +AccessType=ro +PDOMapping=0 + +[2025] +ParameterName=Min Level +ObjectType=0x7 +DataType=0x0006 +AccessType=rw +PDOMapping=0 + +[2026] +ParameterName=Offset +ObjectType=0x7 +DataType=0x0003 +AccessType=rw +PDOMapping=0 + +[2027] +ParameterName=Sensor flipped +ObjectType=0x7 +DataType=0x0005 +AccessType=rw +PDOMapping=0 + +[2028] +ParameterName=Markers +ObjectType=0x9 +SubNumber=4 + +[2028sub0] +ParameterName=NrOfObjects +ObjectType=0x7 +DataType=0x0005 +AccessType=ro +DefaultValue=3 +PDOMapping=0 + +[2028sub1] +ParameterName=Use Markers +ObjectType=0x7 +DataType=0x0005 +AccessType=rw +PDOMapping=0 + +[2028sub2] +ParameterName=Marker Style +ObjectType=0x7 +DataType=0x0005 +AccessType=rw +PDOMapping=0 + +[2028sub3] +ParameterName=Fail Safe Mode +ObjectType=0x7 +DataType=0x0005 +AccessType=rw +PDOMapping=0 + +[2029] +ParameterName=Lock Teach +ObjectType=0x7 +DataType=0x0005 +AccessType=rw +PDOMapping=0 + +[202A] +ParameterName=Set Param to Default +ObjectType=0x7 +DataType=0x0005 +AccessType=wo +PDOMapping=0 + +[202B] +ParameterName=Trigger User Offset Calibration +ObjectType=0x7 +DataType=0x0005 +AccessType=wo +PDOMapping=0 + +[202C] +ParameterName=Trigger Zero Position Teach +ObjectType=0x7 +DataType=0x0005 +AccessType=wo +PDOMapping=0 diff --git a/Devices/Packages/sick_line_guidance/mls/sick_line_guidance_mls.yaml b/Devices/Packages/sick_line_guidance/mls/sick_line_guidance_mls.yaml new file mode 100755 index 0000000..65bf259 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/mls/sick_line_guidance_mls.yaml @@ -0,0 +1,35 @@ +bus: + device: can0 + driver_plugin: can::SocketCANInterface + master_allocator: canopen::SimpleMaster::Allocator +sync: + interval_ms: 10 + overflow: 0 +# +# heartbeat: # simple heartbeat producer (optional, not supported by OLS or MLS, do not enable heartbeats) +# rate: 100 # heartbeat rate (1/rate in seconds) +# msg: "77f#05" # message to send, cansend format: heartbeat of node 127 with status 5=Started +nodes: + node1: + id: 0x01 # CAN-Node-ID of can device, default: Node-ID 10=0x0A for OLS and MLS + eds_pkg: sick_line_guidance # package name for relative path + eds_file: SICK-MLS.eds # path to EDS/DCF file + publish: ["1001","1018sub1","1018sub4","2021sub1!","2021sub2!","2021sub3!","2021sub4!","2022!"] + # MLS: 1001 = error register, 1018sub1 = VendorID, 1018sub4 = SerialNumber, TPDO1 = 0x2021sub1 to 0x2021sub4 and 0x2022 + + # sick_line_guidance configuration of this node: + sick_device_family: "MLS" # can devices of OLS10, OLS20 or MLS family currently supported + sick_topic: "mls" # MLS_Measurement messages are published in topic "/mls" + sick_frame_id: "mls_measurement_frame" # MLS_Measurement messages are published frame_id "mls_measurement_frame" + + # device configuration of writable parameter by dcf_overlay: "objectindex": "parametervalue" + # example: "2027": "0x01" # Object 2027 (sensorFlipped, defaultvalue 0x00) will be configured with value 0x01 + # dcf_overlay: + # "2028sub1": "0x01" # UseMarkers (0 = disable marker detection, 1 = enable marker detection), UINT8, DataType=0x0005, defaultvalue=0 + # "2028sub2": "0x02" # MarkerStyle (0 = disable marker detection, 1 = standard mode, 2 = extended mode), UINT8, DataType=0x0005, defaultvalue=0 + # "2028sub3": "0x01" # FailSafeMode (0 = disabled, 1 = enabled), UINT8, DataType=0x0005, defaultvalue=0 + # "2025": "1000" # Min.Level, UINT16, DataType=0x0006, defaultvalue=600 + # "2026": "1" # Offset [mm] Nullpunkt, INT16, DataType=0x0003, defaultvalue=0 + # "2027": "0x01" # sensorFlipped, UINT8, DataType=0x0005, defaultvalue=0 + # "2029": "0x01" # LockTeach, UINT8, DataType=0x0005, defaultvalue=0 + # diff --git a/Devices/Packages/sick_line_guidance/msg/MLS_Measurement.msg b/Devices/Packages/sick_line_guidance/msg/MLS_Measurement.msg new file mode 100755 index 0000000..4dff85d --- /dev/null +++ b/Devices/Packages/sick_line_guidance/msg/MLS_Measurement.msg @@ -0,0 +1,21 @@ +# MLS_Measurement.msg defines a single measurement of a SICK MLS. +# See operation instructions for details (www.sick.com/mls). +# + +# Header with sequence id, timestamp of the measurement and frame id +Header header + +# Array of measured positions for up to 3 lines. +# Each position is the distance to the center of a line (line center point) in meter. +# More than one line is detected in case of junctions. +float32[] position # distance to the line center point [m] + +# LCP-flags (signs and line assignment) +uint8 lcp # flags (signs and assignment, see chap. 8 of operation instructions) + +# Detection status +uint8 status # status (see chap. 8 of operation instructions) + +# Error register +uint8 error # error register (0x1001, value 0 = okay, see chap. 8 of operation instructions) + diff --git a/Devices/Packages/sick_line_guidance/msg/OLS_Measurement.msg b/Devices/Packages/sick_line_guidance/msg/OLS_Measurement.msg new file mode 100755 index 0000000..0d6ecf3 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/msg/OLS_Measurement.msg @@ -0,0 +1,39 @@ +# OLS_Measurement.msg defines a single measurement of a SICK OLS. +# See operation instructions for details (www.sick.com/ols). + +# Header with sequence id, timestamp of the measurement and frame id +Header header + +# Array of measured positions for up to 3 lines. +# Each position is the distance to the center of a line (line center point) in meter. +# More than one line is detected in case of junctions. +float32[] position # distance to the line center point [m] + +# Array of up to 3 line widths. +# For each detected line, its width is measured. +float32[] width # width of line [m] + +# Detection status +uint8 status # status (see chap. 8 of operation instructions) + +# Barcode data +uint8 barcode # barcode data (0 indicates no barcode) + +# Device status +uint16 dev_status # device status (0x2018, value 0 = okay, see chap. 8 of operation instructions) + +# Extended code +uint32 extended_code # extended code (0x2021sub9, see chap. 8 of operation instructions) + +# Error register +uint8 error # error register (0x1001, value 0 = okay, see chap. 8 of operation instructions) + +# barcode center point [m] (OLS20 only) +float32 barcode_center_point # OLS20 only (0x2021subA), OLS10: always 0 + +# quality of lines +uint8 quality_of_lines # OLS20 only (0x2021subB), OLS10: always 0 + +# Intensity of lines 1 to 3 +uint8[] intensity_of_lines # OLS20 only (0x2023sub1 to 0x2023sub3), OLS10: always 0 + diff --git a/Devices/Packages/sick_line_guidance/ols/SICK_OLS10_CO.eds b/Devices/Packages/sick_line_guidance/ols/SICK_OLS10_CO.eds new file mode 100755 index 0000000..a6f1dc6 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/ols/SICK_OLS10_CO.eds @@ -0,0 +1,602 @@ +[FileInfo] +CreatedBy=Tim Vogt +ModifiedBy=Alexander Bierbaum +Description=Optical Line Guidance Sensor +CreationTime=09:47AM +CreationDate=12-15-2017 +ModificationTime=02:25PM +ModificationDate=03-13-2019 +FileName=SICK_OLS10_CO.eds +FileVersion=0x02 +FileRevision=0x0A +EDSVersion=4.0 + +[DeviceInfo] +VendorName=SICK AG +VendorNumber=0x01000056 +ProductName=Optical Line Guidance Sensor +ProductNumber=0x00001003 +RevisionNumber=0x00010001 +OrderCode=- +BaudRate_10=1 +BaudRate_20=1 +BaudRate_50=1 +BaudRate_125=1 +BaudRate_250=1 +BaudRate_500=1 +BaudRate_800=1 +BaudRate_1000=1 +SimpleBootUpMaster=0 +SimpleBootUpSlave=1 +Granularity=0 +DynamicChannelsSupported=0 +CompactPDO=0 +GroupMessaging=0 +NrOfRXPDO=0 +NrOfTXPDO=2 +LSS_Supported=1 + +[DummyUsage] +Dummy0001=0 +Dummy0002=1 +Dummy0003=1 +Dummy0004=1 +Dummy0005=1 +Dummy0006=1 +Dummy0007=1 + +[Comments] +Lines=0 + +[MandatoryObjects] +SupportedObjects=3 +1=0x1000 +2=0x1001 +3=0x1018 + +[1000] +ParameterName=Device Type +ObjectType=0x7 +DataType=0x0007 +AccessType=ro +DefaultValue=0x00 +PDOMapping=0 + +[1001] +ParameterName=Error Register +ObjectType=0x7 +DataType=0x0005 +AccessType=ro +DefaultValue=0 +PDOMapping=0 + +[1018] +ParameterName=Identity Object +ObjectType=0x9 +SubNumber=5 + +[1018sub0] +ParameterName=Number of entries +ObjectType=0x7 +DataType=0x0005 +AccessType=ro +DefaultValue=4 +PDOMapping=0 +LowLimit=1 +HighLimit=4 + +[1018sub1] +ParameterName=Vendor Id +ObjectType=0x7 +DataType=0x0007 +AccessType=ro +DefaultValue=0x01000056 +PDOMapping=0 + +[1018sub2] +ParameterName=Product Code +ObjectType=0x7 +DataType=0x0007 +AccessType=ro +DefaultValue=0x00001003 +PDOMapping=0 + +[1018sub3] +ParameterName=Revision number +ObjectType=0x7 +DataType=0x0007 +AccessType=ro +DefaultValue=0x00010001 +PDOMapping=0 + +[1018sub4] +ParameterName=Serial number +ObjectType=0x7 +DataType=0x0007 +AccessType=ro +PDOMapping=0 + +[OptionalObjects] +SupportedObjects=13 +1=0x1005 +2=0x1008 +3=0x1009 +4=0x100A +5=0x100C +6=0x100D +7=0x1017 +8=0x1200 +9=0x1800 +10=0x1801 +11=0x1A00 +12=0x1A01 +13=0x1F80 + +[1005] +ParameterName=COB ID SYNC +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000080 +PDOMapping=0 +LowLimit=0x00000080 + +[1008] +ParameterName=Manufacturer Device Name +ObjectType=0x7 +DataType=0x0009 +AccessType=const +DefaultValue=OLS10-BP112311 +PDOMapping=0 + +[1009] +ParameterName=Manufacturer Hardware Version +ObjectType=0x7 +DataType=0x0009 +AccessType=const +DefaultValue=1.0 +PDOMapping=0 + +[100A] +ParameterName=Manufacturer Software Version +ObjectType=0x7 +DataType=0x0009 +AccessType=const +DefaultValue=Firmwareversion +PDOMapping=0 + +[100C] +ParameterName=Guard Time +ObjectType=0x7 +DataType=0x0006 +AccessType=rw +DefaultValue=0 +PDOMapping=0 + +[100D] +ParameterName=Life Time Factor +ObjectType=0x7 +DataType=0x0005 +AccessType=rw +DefaultValue=0x00 +PDOMapping=0 + +[1017] +ParameterName=Producer Heartbeat Time +ObjectType=0x7 +DataType=0x0006 +AccessType=rw +DefaultValue=0 +PDOMapping=0 + +[1200] +ParameterName=Server SDO Parameter 0 +ObjectType=0x9 +SubNumber=3 + +[1200sub0] +ParameterName=Number of entries +ObjectType=0x7 +DataType=0x0005 +AccessType=ro +DefaultValue=2 +PDOMapping=0 +LowLimit=0x02 +HighLimit=0x02 + +[1200sub1] +ParameterName=COB ID Client to Server +ObjectType=0x7 +DataType=0x0007 +AccessType=ro +DefaultValue=$NODEID+0x600 +PDOMapping=0 + +[1200sub2] +ParameterName=COB ID Server to Client +ObjectType=0x7 +DataType=0x0007 +AccessType=ro +DefaultValue=$NODEID+0x580 +PDOMapping=0 + +[1800] +ParameterName=Transmit PDO Communication Parameter 0 +ObjectType=0x9 +SubNumber=5 + +[1800sub0] +ParameterName=Number of entries +ObjectType=0x7 +DataType=0x0005 +AccessType=ro +DefaultValue=5 +PDOMapping=0 + +[1800sub1] +ParameterName=COB ID +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=$NODEID+0x180 +PDOMapping=0 + +[1800sub2] +ParameterName=Transmission Type +ObjectType=0x7 +DataType=0x0005 +AccessType=rw +DefaultValue=0xFF +PDOMapping=0 + +[1800sub4] +ParameterName=Compatibility Entry +ObjectType=0x7 +DataType=0x0005 +AccessType=rw +DefaultValue=0x00 +PDOMapping=0 + +[1800sub5] +ParameterName=Event Timer +ObjectType=0x7 +DataType=0x0006 +AccessType=rw +DefaultValue=20 +PDOMapping=0 + +[1801] +ParameterName=Transmit PDO Communication Parameter 1 +ObjectType=0x9 +SubNumber=5 + +[1801sub0] +ParameterName=Number of entries +ObjectType=0x7 +DataType=0x0005 +AccessType=ro +DefaultValue=5 +PDOMapping=0 + +[1801sub1] +ParameterName=COB ID +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +DefaultValue=$NODEID+0x280 +PDOMapping=0 + +[1801sub2] +ParameterName=Transmission Type +ObjectType=0x7 +DataType=0x0005 +AccessType=rw +DefaultValue=0xFF +PDOMapping=0 + +[1801sub4] +ParameterName=Compatibility Entry +ObjectType=0x7 +DataType=0x0005 +AccessType=rw +DefaultValue=0x00 +PDOMapping=0 + +[1801sub5] +ParameterName=Event Timer +ObjectType=0x7 +DataType=0x0006 +AccessType=rw +DefaultValue=0 +PDOMapping=0 + +[1A00] +ParameterName=Transmit PDO Mapping Parameter 0 +ObjectType=0x9 +SubNumber=6 + +[1A00sub0] +ParameterName=Number of entries +ObjectType=0x7 +DataType=0x0005 +AccessType=const +DefaultValue=5 +PDOMapping=0 +LowLimit=0 +HighLimit=5 + +[1A00sub1] +ParameterName=PDO Mapping Entry +ObjectType=0x7 +DataType=0x0007 +AccessType=const +DefaultValue=0x20210110 +PDOMapping=0 + +[1A00sub2] +ParameterName=PDO Mapping Entry_2 +ObjectType=0x7 +DataType=0x0007 +AccessType=const +DefaultValue=0x20210210 +PDOMapping=0 + +[1A00sub3] +ParameterName=PDO Mapping Entry_3 +ObjectType=0x7 +DataType=0x0007 +AccessType=const +DefaultValue=0x20210310 +PDOMapping=0 + +[1A00sub4] +ParameterName=PDO Mapping Entry_4 +ObjectType=0x7 +DataType=0x0007 +AccessType=const +DefaultValue=0x20210408 +PDOMapping=0 + +[1A00sub5] +ParameterName=PDO Mapping Entry_5 +ObjectType=0x7 +DataType=0x0007 +AccessType=const +DefaultValue=0x20210808 +PDOMapping=0 + +[1A01] +ParameterName=Transmit PDO Mapping Parameter 1 +ObjectType=0x9 +SubNumber=4 + +[1A01sub0] +ParameterName=Number of entries +ObjectType=0x7 +DataType=0x0005 +AccessType=const +DefaultValue=3 +PDOMapping=0 +LowLimit=0 +HighLimit=3 + +[1A01sub1] +ParameterName=PDO Mapping Entry +ObjectType=0x7 +DataType=0x0007 +AccessType=const +DefaultValue=0x20210510 +PDOMapping=0 + +[1A01sub2] +ParameterName=PDO Mapping Entry_2 +ObjectType=0x7 +DataType=0x0007 +AccessType=const +DefaultValue=0x20210610 +PDOMapping=0 + +[1A01sub3] +ParameterName=PDO Mapping Entry_3 +ObjectType=0x7 +DataType=0x0007 +AccessType=const +DefaultValue=0x20210710 +PDOMapping=0 + +[1F80] +ParameterName=NMTStartup +ObjectType=0x7 +DataType=0x0007 +AccessType=rw +PDOMapping=0 + +[ManufacturerObjects] +SupportedObjects=7 +1=0x2001 +2=0x2002 +3=0x2003 +4=0x2018 +5=0x2019 +6=0x2020 +7=0x2021 + +[2001] +ParameterName=Mounting parameters +ObjectType=0x9 +SubNumber=2 + +[2001sub0] +ParameterName=Number of mounting parameters +ObjectType=0x7 +DataType=0x0005 +AccessType=ro +DefaultValue=5 +PDOMapping=0 + +[2001sub5] +ParameterName=Flipped upside down +ObjectType=0x7 +DataType=0x0005 +AccessType=rw +DefaultValue=0 +PDOMapping=0 + +[2002] +ParameterName=Tape parameters +ObjectType=0x9 +SubNumber=4 + +[2002sub0] +ParameterName=Number of tape parameters +ObjectType=0x7 +DataType=0x0005 +AccessType=ro +DefaultValue=3 +PDOMapping=0 + +[2002sub1] +ParameterName=Typ. width [m] +ObjectType=0x7 +DataType=0x0008 +AccessType=rw +PDOMapping=0 + +[2002sub2] +ParameterName=Min. width [m] +ObjectType=0x7 +DataType=0x0008 +AccessType=rw +PDOMapping=0 + +[2002sub3] +ParameterName=Max. width [m] +ObjectType=0x7 +DataType=0x0008 +AccessType=rw +PDOMapping=0 + +[2003] +ParameterName=Advanced settings +ObjectType=0x9 +SubNumber=3 + +[2003sub0] +ParameterName=Number of advanced settings +ObjectType=0x7 +DataType=0x0005 +AccessType=ro +DefaultValue=3 +PDOMapping=0 + +[2003sub1] +ParameterName=Max. number of missing track readings +ObjectType=0x7 +DataType=0x0006 +AccessType=rw +DefaultValue=10 +PDOMapping=0 + +[2003sub3] +ParameterName=Position smoothing filter coefficient (dt/tau) +ObjectType=0x7 +DataType=0x0008 +AccessType=rw +PDOMapping=0 + +[2018] +ParameterName=Device status +ObjectType=0x7 +DataType=0x0006 +AccessType=ro +PDOMapping=0 + +[2019] +ParameterName=Order number +ObjectType=0x7 +DataType=0x0007 +AccessType=ro +PDOMapping=0 + +[2020] +ParameterName=Login +ObjectType=0x7 +DataType=0x000a +AccessType=wo +PDOMapping=0 + +[2021] +ParameterName=Result data (LCPs) +ObjectType=0x9 +SubNumber=10 + +[2021sub0] +ParameterName=Number of result data +ObjectType=0x7 +DataType=0x0005 +AccessType=ro +DefaultValue=9 +PDOMapping=0 + +[2021sub1] +ParameterName=LCP1 +ObjectType=0x7 +DataType=0x0003 +AccessType=ro +PDOMapping=1 + +[2021sub2] +ParameterName=LCP2 +ObjectType=0x7 +DataType=0x0003 +AccessType=ro +PDOMapping=1 + +[2021sub3] +ParameterName=LCP3 +ObjectType=0x7 +DataType=0x0003 +AccessType=ro +PDOMapping=1 + +[2021sub4] +ParameterName=Status +ObjectType=0x7 +DataType=0x0005 +AccessType=ro +PDOMapping=1 + +[2021sub5] +ParameterName=Width line1 +ObjectType=0x7 +DataType=0x0003 +AccessType=ro +PDOMapping=1 + +[2021sub6] +ParameterName=Width line2 +ObjectType=0x7 +DataType=0x0003 +AccessType=ro +PDOMapping=1 + +[2021sub7] +ParameterName=Width line3 +ObjectType=0x7 +DataType=0x0003 +AccessType=ro +PDOMapping=1 + +[2021sub8] +ParameterName=Code +ObjectType=0x7 +DataType=0x0005 +AccessType=ro +PDOMapping=1 + +[2021sub9] +ParameterName=Extended Code +ObjectType=0x7 +DataType=0x0007 +AccessType=ro +PDOMapping=0 diff --git a/Devices/Packages/sick_line_guidance/ols/SICK_OLS20.eds b/Devices/Packages/sick_line_guidance/ols/SICK_OLS20.eds new file mode 100755 index 0000000..813d3e6 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/ols/SICK_OLS20.eds @@ -0,0 +1,797 @@ + +; This EDS file was created by the CANopen Design Tool 2.3.19.0. +; port GmbH Halle/Saale Germany, http://www.port.de, mailto:service@port.de +; DefaultValues added by Michael Lehning. + +[FileInfo] +FileName=OLS20.eds +FileVersion=1 +FileRevision=1 +EDSVersion=4.0 +Description=Optical Line Guidance +CreationTime=09:08AM +CreationDate=08-15-2018 +CreatedBy=herrmra +ModificationTime=10:00AM +ModificationDate=15-02-2019 +ModifiedBy=Michael Lehning + +[DeviceInfo] +VendorName=SICK AG +VendorNumber=0x01000056 +ProductName=OLS20 +ProductNumber=0x00001004 +RevisionNumber=1 +OrderCode=- +BaudRate_10=1 +BaudRate_20=1 +BaudRate_50=1 +BaudRate_125=1 +BaudRate_250=1 +BaudRate_500=1 +BaudRate_800=0 +BaudRate_1000=1 +DynamicChannelsSupported=0 +GroupMessaging=0 +LSS_Supported=1 +Granularity=8 +SimpleBootUpSlave=1 +SimpleBootUpMaster=0 +NrOfRXPDO=0 +NrOfTXPDO=2 + +[Comments] +Lines=0 + +[DummyUsage] +Dummy0001=0 +Dummy0002=0 +Dummy0003=0 +Dummy0004=0 +Dummy0005=0 +Dummy0006=0 +Dummy0007=0 + +[MandatoryObjects] +SupportedObjects=3 +1=0x1000 +2=0x1001 +3=0x1018 + +[OptionalObjects] +SupportedObjects=16 +1=0x1005 +2=0x1008 +3=0x1009 +4=0x100A +5=0x100C +6=0x100D +7=0x1014 +8=0x1015 +9=0x1016 +10=0x1017 +11=0x1200 +12=0x1800 +13=0x1801 +14=0x1A00 +15=0x1A01 +16=0x1F80 + +[ManufacturerObjects] +SupportedObjects=6 +1=0x2001 +2=0x2002 +3=0x2003 +4=0x2018 +5=0x2019 +6=0x2021 + +[1000] +ParameterName=Device Type +ObjectType=0x07 +DataType=0x0007 +AccessType=const +DefaultValue=0x0000000 +PDOMapping=0 + +[1001] +ParameterName=Error Register +ObjectType=0x07 +DataType=0x0005 +AccessType=ro +DefaultValue=0x00 +PDOMapping=0 + +[1005] +ParameterName=COB-ID SYNC +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000080 +PDOMapping=0 + +[1008] +ParameterName=Manufacturer Device Name +ObjectType=0x07 +DataType=0x0009 +AccessType=const +DefaultValue=OLS20 +PDOMapping=0 + +[1009] +ParameterName=Manufacturer Hardware Version +ObjectType=0x07 +DataType=0x0009 +AccessType=const +PDOMapping=0 + +[100A] +ParameterName=Manufacturer Software Version +ObjectType=0x07 +DataType=0x0009 +AccessType=const +PDOMapping=0 + +[100C] +ParameterName=Guard Time +ObjectType=0x07 +DataType=0x0006 +AccessType=rw +DefaultValue=0x0000 +PDOMapping=0 + +[100D] +ParameterName=Life Time Factor +ObjectType=0x07 +DataType=0x0005 +AccessType=rw +DefaultValue=0x00 +PDOMapping=0 + +[1014] +ParameterName=COB-ID EMCY +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=$NODEID+0x80 +PDOMapping=0 + +[1015] +ParameterName=Inhibit Time Emergency +ObjectType=0x07 +DataType=0x0006 +AccessType=rw +DefaultValue=0x0000 +PDOMapping=0 + +[1016] +SubNumber=3 +ParameterName=Heartbeat Consumer Entries +ObjectType=0x08 + +[1016sub0] +ParameterName=Highest sub-index supported +ObjectType=0x07 +DataType=0x0005 +AccessType=const +DefaultValue=0x02 +PDOMapping=0 + +[1016sub1] +ParameterName=Consumer Heartbeat Time 1 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +PDOMapping=0 + +[1016sub2] +ParameterName=Consumer Heartbeat Time 2 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +PDOMapping=0 + +[1017] +ParameterName=Producer Heartbeat Time +ObjectType=0x07 +DataType=0x0006 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1018] +SubNumber=5 +ParameterName=Identity Object +ObjectType=0x09 + +[1018sub0] +ParameterName=number of entries +ObjectType=0x07 +DataType=0x0005 +AccessType=const +DefaultValue=0x4 +PDOMapping=0 + +[1018sub1] +ParameterName=Vendor Id +ObjectType=0x07 +DataType=0x0007 +AccessType=ro +DefaultValue=0x01000056 +PDOMapping=0 + +[1018sub2] +ParameterName=Product Code +ObjectType=0x07 +DataType=0x0007 +AccessType=ro +DefaultValue=0x00001004 +PDOMapping=0 + +[1018sub3] +ParameterName=Revision number +ObjectType=0x07 +DataType=0x0007 +AccessType=ro +DefaultValue=0x00000001 +PDOMapping=0 + +[1018sub4] +ParameterName=Serial number +ObjectType=0x07 +DataType=0x0007 +AccessType=ro +PDOMapping=0 + +[1200] +SubNumber=3 +ParameterName=Server SDO Parameter 1 +ObjectType=0x09 + +[1200sub0] +ParameterName=Highest sub-index supported +ObjectType=0x07 +DataType=0x0005 +AccessType=const +DefaultValue=0x02 +PDOMapping=0 + +[1200sub1] +ParameterName=COB-ID Client -> Server +ObjectType=0x07 +DataType=0x0007 +AccessType=ro +DefaultValue=$NODEID+0x600 +PDOMapping=0 + +[1200sub2] +ParameterName=COB-ID Server -> Client +ObjectType=0x07 +DataType=0x0007 +AccessType=ro +DefaultValue=$NODEID+0x580 +PDOMapping=0 + +[1800] +SubNumber=6 +ParameterName=Transmit PDO Communication Parameter 1 +ObjectType=0x09 + +[1800sub0] +ParameterName=Highest sub-index supported +ObjectType=0x07 +DataType=0x0005 +AccessType=const +DefaultValue=0x05 +PDOMapping=0 +LowLimit=0x2 +HighLimit=0x6 + +[1800sub1] +ParameterName=COB-ID +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=$NODEID+0x180 +PDOMapping=0 +LowLimit=0x00000181 +HighLimit=0xFFFFFFFF + +[1800sub2] +ParameterName=Transmission Type +ObjectType=0x07 +DataType=0x0005 +AccessType=rw +DefaultValue=0xFF +PDOMapping=0 +LowLimit=0x0 +HighLimit=0xFF + +[1800sub3] +ParameterName=Inhibit Time +ObjectType=0x07 +DataType=0x0006 +AccessType=rw +DefaultValue=0x0 +PDOMapping=0 +LowLimit=0x0000 +HighLimit=0xFFFF + +[1800sub4] +ParameterName=Compatibility Entry +ObjectType=0x07 +DataType=0x0005 +AccessType=rw +DefaultValue=0x00 +PDOMapping=0 +LowLimit=0x00 +HighLimit=0xFF + +[1800sub5] +ParameterName=Event Timer +ObjectType=0x07 +DataType=0x0006 +AccessType=rw +DefaultValue=0x14 +PDOMapping=0 +LowLimit=0x0 +HighLimit=0xFFFF + +[1801] +SubNumber=6 +ParameterName=Transmit PDO Communication Parameter 2 +ObjectType=0x09 + +[1801sub0] +ParameterName=Highest sub-index supported +ObjectType=0x07 +DataType=0x0005 +AccessType=const +DefaultValue=0x05 +PDOMapping=0 +LowLimit=0x02 +HighLimit=0x06 + +[1801sub1] +ParameterName=COB-ID +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=$NODEID+0x280 +PDOMapping=0 +LowLimit=0x00000281 +HighLimit=0xFFFFFFFF + +[1801sub2] +ParameterName=Transmission Type +ObjectType=0x07 +DataType=0x0005 +AccessType=rw +DefaultValue=0xFF +PDOMapping=0 +LowLimit=0x0 +HighLimit=0xFF + +[1801sub3] +ParameterName=Inhibit Time +ObjectType=0x07 +DataType=0x0006 +AccessType=rw +DefaultValue=0x0 +PDOMapping=0 +LowLimit=0x0000 +HighLimit=0xFFFF + +[1801sub4] +ParameterName=Compatibility Entry +ObjectType=0x07 +DataType=0x0005 +AccessType=rw +DefaultValue=0x00 +PDOMapping=0 +LowLimit=0x00 +HighLimit=0xFF + +[1801sub5] +ParameterName=Event Timer +ObjectType=0x07 +DataType=0x0006 +AccessType=rw +DefaultValue=0x14 +PDOMapping=0 +LowLimit=0x0 +HighLimit=0xFFFF + +[1A00] +SubNumber=9 +ParameterName=Transmit PDO Mapping Parameter 1 +ObjectType=0x09 + +[1A00sub0] +ParameterName=Number of mapped objects +ObjectType=0x07 +DataType=0x0005 +AccessType=rw +DefaultValue=0x05 +PDOMapping=0 +LowLimit=0x00 +HighLimit=0x08 + +[1A00sub1] +ParameterName=Mapping Entry 1 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x20210110 +PDOMapping=0 + +[1A00sub2] +ParameterName=Mapping Entry 2 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x20210210 +PDOMapping=0 + +[1A00sub3] +ParameterName=Mapping Entry 3 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x20210310 +PDOMapping=0 + +[1A00sub4] +ParameterName=Mapping Entry 4 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x20210408 +PDOMapping=0 + +[1A00sub5] +ParameterName=Mapping Entry 5 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x20210808 +PDOMapping=0 + +[1A00sub6] +ParameterName=Mapping Entry 6 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1A00sub7] +ParameterName=Mapping Entry 7 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1A00sub8] +ParameterName=Mapping Entry 8 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1A01] +SubNumber=9 +ParameterName=Transmit PDO Mapping Parameter 2 +ObjectType=0x09 + +[1A01sub0] +ParameterName=Number of mapped objects +ObjectType=0x07 +DataType=0x0005 +AccessType=rw +DefaultValue=0x03 +PDOMapping=0 +LowLimit=0x0 +HighLimit=0x8 + +[1A01sub1] +ParameterName=Mapping Entry 1 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x20210510 +PDOMapping=0 + +[1A01sub2] +ParameterName=Mapping Entry 2 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x20210610 +PDOMapping=0 + +[1A01sub3] +ParameterName=Mapping Entry 3 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x20210710 +PDOMapping=0 + +[1A01sub4] +ParameterName=Mapping Entry 4 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1A01sub5] +ParameterName=Mapping Entry 5 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1A01sub6] +ParameterName=Mapping Entry 6 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1A01sub7] +ParameterName=Mapping Entry 7 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1A01sub8] +ParameterName=Mapping Entry 8 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1F80] +ParameterName=NMT Startup +ObjectType=0x07 +DataType=0x0007 +AccessType=const +PDOMapping=0 +LowLimit=0x0 +HighLimit=0x3F + +[2001] +SubNumber=6 +ParameterName=Mounting parameters +ObjectType=0x09 + +[2001sub0] +ParameterName=numOfEntries +ObjectType=0x07 +DataType=0x0005 +AccessType=const +DefaultValue=0x5 +PDOMapping=0 + +[2001sub1] +ParameterName=reserved1 +ObjectType=0x07 +DataType=0x0007 +AccessType=ro +PDOMapping=0 + +[2001sub2] +ParameterName=reserved2 +ObjectType=0x07 +DataType=0x0007 +AccessType=ro +PDOMapping=0 + +[2001sub3] +ParameterName=reserved3 +ObjectType=0x07 +DataType=0x0007 +AccessType=ro +PDOMapping=0 + +[2001sub4] +ParameterName=reserved4 +ObjectType=0x07 +DataType=0x0007 +AccessType=ro +PDOMapping=0 + +[2001sub5] +ParameterName=sensorFlipped +ObjectType=0x07 +DataType=0x0005 +AccessType=rw +DefaultValue=0x00 +PDOMapping=0 +LowLimit=0x0 +HighLimit=0xFF + +[2002] +SubNumber=4 +ParameterName=Tape Parameters +ObjectType=0x09 + +[2002sub0] +ParameterName=Number of mapped objects +ObjectType=0x07 +DataType=0x0005 +AccessType=const +DefaultValue=0x3 +PDOMapping=0 + +[2002sub1] +ParameterName=Typ. Width +ObjectType=0x07 +DataType=0x0008 +AccessType=rw +PDOMapping=0 + +[2002sub2] +ParameterName=Min. Width +ObjectType=0x07 +DataType=0x0008 +AccessType=rw +PDOMapping=0 + +[2002sub3] +ParameterName=Max. Width +ObjectType=0x07 +DataType=0x0008 +AccessType=rw +PDOMapping=0 + +[2003] +SubNumber=3 +ParameterName=Advanced Settings +ObjectType=0x09 + +[2003sub0] +ParameterName=Number of mapped objects +ObjectType=0x07 +DataType=0x0005 +AccessType=const +DefaultValue=0x2 +PDOMapping=0 + +[2003sub1] +ParameterName=Off Delay Track Detection +ObjectType=0x07 +DataType=0x0006 +AccessType=rw +DefaultValue=0x0064 +PDOMapping=0 +LowLimit=0x0 +HighLimit=0xFFFF + +[2003sub2] +ParameterName=Off Delay Code Detection +ObjectType=0x07 +DataType=0x0006 +AccessType=rw +DefaultValue=0x0064 +PDOMapping=0 +LowLimit=0x0 +HighLimit=0xFFFF + +[2018] +ParameterName=Device Status +ObjectType=0x07 +DataType=0x0005 +AccessType=ro +PDOMapping=0 + +[2019] +ParameterName=Order number +ObjectType=0x07 +DataType=0x0007 +AccessType=ro +PDOMapping=0 + +[2021] +SubNumber=10 +ParameterName=Result data (LCPs) +ObjectType=0x09 + +[2021sub0] +ParameterName=Number of mapped objects +ObjectType=0x07 +DataType=0x0005 +AccessType=const +DefaultValue=0x9 +PDOMapping=0 + +[2021sub1] +ParameterName=LCP1 +ObjectType=0x07 +DataType=0x0003 +AccessType=ro +PDOMapping=1 +LowLimit=0x8000 +HighLimit=0x7FFF + +[2021sub2] +ParameterName=LCP2 +ObjectType=0x07 +DataType=0x0003 +AccessType=ro +PDOMapping=1 +LowLimit=0x8000 +HighLimit=0x7FFF + +[2021sub3] +ParameterName=LCP3 +ObjectType=0x07 +DataType=0x0003 +AccessType=ro +PDOMapping=1 +LowLimit=0x8000 +HighLimit=0x7FFF + +[2021sub4] +ParameterName=Status +ObjectType=0x07 +DataType=0x0005 +AccessType=ro +PDOMapping=1 +LowLimit=0x00 +HighLimit=0xFF + +[2021sub5] +ParameterName=Width LCP1 +ObjectType=0x07 +DataType=0x0003 +AccessType=ro +PDOMapping=1 +LowLimit=0x8000 +HighLimit=0x7FFF + +[2021sub6] +ParameterName=Width LCP2 +ObjectType=0x07 +DataType=0x0003 +AccessType=ro +PDOMapping=1 +LowLimit=0x8000 +HighLimit=0x7FFF + +[2021sub7] +ParameterName=Width LCP3 +ObjectType=0x07 +DataType=0x0003 +AccessType=ro +PDOMapping=1 +LowLimit=0x8000 +HighLimit=0x7FFF + +[2021sub8] +ParameterName=Code +ObjectType=0x07 +DataType=0x0005 +AccessType=ro +PDOMapping=1 +LowLimit=0x0 +HighLimit=0xFF + +[2021sub9] +ParameterName=Extended Code +ObjectType=0x07 +DataType=0x0007 +AccessType=ro +PDOMapping=0 +LowLimit=0x0 +HighLimit=0xFFFFFFFF + diff --git a/Devices/Packages/sick_line_guidance/ols/SICK_OLS20_CO.eds b/Devices/Packages/sick_line_guidance/ols/SICK_OLS20_CO.eds new file mode 100755 index 0000000..498dc23 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/ols/SICK_OLS20_CO.eds @@ -0,0 +1,867 @@ + +; This EDS file was created by the CANopen Design Tool 2.3.26.0. +; port GmbH Halle/Saale Germany, http://www.port.de, mailto:service@port.de + +[FileInfo] +FileName=OLS20.eds +FileVersion=1 +FileRevision=2 +EDSVersion=4.0 +Description=Optical Line Guidance +CreationTime=09:08AM +CreationDate=08-15-2018 +CreatedBy=herrmra +ModificationTime=04:29PM +ModificationDate=04-10-2019 +ModifiedBy=herrmra + +[DeviceInfo] +VendorName=SICK AG +VendorNumber=0x01000056 +ProductName=OLS20 +ProductNumber=0x00001004 +RevisionNumber=1 +OrderCode=- +BaudRate_10=1 +BaudRate_20=1 +BaudRate_50=1 +BaudRate_125=1 +BaudRate_250=1 +BaudRate_500=1 +BaudRate_800=0 +BaudRate_1000=1 +DynamicChannelsSupported=0 +GroupMessaging=0 +LSS_Supported=1 +Granularity=8 +SimpleBootUpSlave=1 +SimpleBootUpMaster=0 +NrOfRXPDO=0 +NrOfTXPDO=2 + +[Comments] +Lines=0 + +[DummyUsage] +Dummy0001=0 +Dummy0002=0 +Dummy0003=0 +Dummy0004=0 +Dummy0005=0 +Dummy0006=0 +Dummy0007=0 + +[MandatoryObjects] +SupportedObjects=3 +1=0x1000 +2=0x1001 +3=0x1018 + +[OptionalObjects] +SupportedObjects=16 +1=0x1005 +2=0x1008 +3=0x1009 +4=0x100A +5=0x100C +6=0x100D +7=0x1014 +8=0x1015 +9=0x1016 +10=0x1017 +11=0x1200 +12=0x1800 +13=0x1801 +14=0x1A00 +15=0x1A01 +16=0x1F80 + +[ManufacturerObjects] +SupportedObjects=7 +1=0x2001 +2=0x2002 +3=0x2003 +4=0x2018 +5=0x2019 +6=0x2021 +7=0x2023 + +[1000] +ParameterName=Device Type +ObjectType=0x07 +DataType=0x0007 +AccessType=const +DefaultValue=0x0000000 +PDOMapping=0 + +[1001] +ParameterName=Error Register +ObjectType=0x07 +DataType=0x0005 +AccessType=ro +DefaultValue=0 +PDOMapping=0 + +[1005] +ParameterName=COB-ID SYNC +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000080 +PDOMapping=0 + +[1008] +ParameterName=Manufacturer Device Name +ObjectType=0x07 +DataType=0x0009 +AccessType=const +DefaultValue=OLS20 +PDOMapping=0 + +[1009] +ParameterName=Manufacturer Hardware Version +ObjectType=0x07 +DataType=0x0009 +AccessType=const +PDOMapping=0 + +[100A] +ParameterName=Manufacturer Software Version +ObjectType=0x07 +DataType=0x0009 +AccessType=const +PDOMapping=0 + +[100C] +ParameterName=Guard Time +ObjectType=0x07 +DataType=0x0006 +AccessType=rw +DefaultValue=0x00 +PDOMapping=0 + +[100D] +ParameterName=Life Time Factor +ObjectType=0x07 +DataType=0x0005 +AccessType=rw +DefaultValue=0x00 +PDOMapping=0 + +[1014] +ParameterName=COB-ID EMCY +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=$NODEID+0x80 +PDOMapping=0 + +[1015] +ParameterName=Inhibit Time Emergency +ObjectType=0x07 +DataType=0x0006 +AccessType=rw +DefaultValue=0x0000 +PDOMapping=0 + +[1016] +SubNumber=3 +ParameterName=Heartbeat Consumer Entries +ObjectType=0x08 + +[1016sub0] +ParameterName=Highest sub-index supported +ObjectType=0x07 +DataType=5 +AccessType=const +DefaultValue=0x02 +PDOMapping=0 + +[1016sub1] +ParameterName=Consumer Heartbeat Time 1 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +PDOMapping=0 + +[1016sub2] +ParameterName=Consumer Heartbeat Time 2 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +PDOMapping=0 + +[1017] +ParameterName=Producer Heartbeat Time +ObjectType=0x07 +DataType=0x0006 +AccessType=rw +DefaultValue=0x00 +PDOMapping=0 + +[1018] +SubNumber=5 +ParameterName=Identity Object +ObjectType=0x09 + +[1018sub0] +ParameterName=Highest sub-index supported +ObjectType=0x07 +DataType=0x0005 +AccessType=const +DefaultValue=0x4 +PDOMapping=0 + +[1018sub1] +ParameterName=Vendor Id +ObjectType=0x07 +DataType=0x0007 +AccessType=ro +DefaultValue=0x01000056 +PDOMapping=0 + +[1018sub2] +ParameterName=Product Code +ObjectType=0x07 +DataType=0x0007 +AccessType=ro +DefaultValue=0x00001004 +PDOMapping=0 + +[1018sub3] +ParameterName=Revision number +ObjectType=0x07 +DataType=0x0007 +AccessType=ro +DefaultValue=1 +PDOMapping=0 + +[1018sub4] +ParameterName=Serial number +ObjectType=0x07 +DataType=0x0007 +AccessType=ro +PDOMapping=0 + +[1200] +SubNumber=3 +ParameterName=Server SDO Parameter 1 +ObjectType=0x09 + +[1200sub0] +ParameterName=Highest sub-index supported +ObjectType=0x07 +DataType=0x0005 +AccessType=const +DefaultValue=0x02 +PDOMapping=0 + +[1200sub1] +ParameterName=COB-ID Client -> Server +ObjectType=0x07 +DataType=0x0007 +AccessType=ro +DefaultValue=$NODEID+0x600 +PDOMapping=0 + +[1200sub2] +ParameterName=COB-ID Server -> Client +ObjectType=0x07 +DataType=0x0007 +AccessType=ro +DefaultValue=$NODEID+0x580 +PDOMapping=0 + +[1800] +SubNumber=6 +ParameterName=Transmit PDO Communication Parameter 1 +ObjectType=0x09 + +[1800sub0] +ParameterName=Highest sub-index supported +ObjectType=0x07 +DataType=0x0005 +AccessType=const +DefaultValue=0x05 +PDOMapping=0 +LowLimit=0x2 +HighLimit=0x6 + +[1800sub1] +ParameterName=COB-ID +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=$NODEID+0x180 +PDOMapping=0 +LowLimit=0x00000181 +HighLimit=0xFFFFFFFF + +[1800sub2] +ParameterName=Transmission Type +ObjectType=0x07 +DataType=0x0005 +AccessType=rw +DefaultValue=0xFF +PDOMapping=0 +LowLimit=0x0 +HighLimit=0xFF + +[1800sub3] +ParameterName=Inhibit Time +ObjectType=0x07 +DataType=0x0006 +AccessType=rw +DefaultValue=0x0 +PDOMapping=0 +LowLimit=0x0000 +HighLimit=0xFFFF + +[1800sub4] +ParameterName=Compatibility Entry +ObjectType=0x07 +DataType=0x0005 +AccessType=rw +DefaultValue=0x00 +PDOMapping=0 +LowLimit=0x00 +HighLimit=0xFF + +[1800sub5] +ParameterName=Event Timer +ObjectType=0x07 +DataType=0x0006 +AccessType=rw +DefaultValue=0x14 +PDOMapping=0 +LowLimit=0x0 +HighLimit=0xFFFF + +[1801] +SubNumber=6 +ParameterName=Transmit PDO Communication Parameter 2 +ObjectType=0x09 + +[1801sub0] +ParameterName=Highest sub-index supported +ObjectType=0x07 +DataType=0x0005 +AccessType=const +DefaultValue=0x05 +PDOMapping=0 +LowLimit=0x02 +HighLimit=0x06 + +[1801sub1] +ParameterName=COB-ID +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=$NODEID+0x280 +PDOMapping=0 +LowLimit=0x00000281 +HighLimit=0xFFFFFFFF + +[1801sub2] +ParameterName=Transmission Type +ObjectType=0x07 +DataType=0x0005 +AccessType=rw +DefaultValue=0xFF +PDOMapping=0 +LowLimit=0x0 +HighLimit=0xFF + +[1801sub3] +ParameterName=Inhibit Time +ObjectType=0x07 +DataType=0x0006 +AccessType=rw +DefaultValue=0x0 +PDOMapping=0 +LowLimit=0x0000 +HighLimit=0xFFFF + +[1801sub4] +ParameterName=Compatibility Entry +ObjectType=0x07 +DataType=0x0005 +AccessType=rw +DefaultValue=0x00 +PDOMapping=0 +LowLimit=0x00 +HighLimit=0xFF + +[1801sub5] +ParameterName=Event Timer +ObjectType=0x07 +DataType=0x0006 +AccessType=rw +DefaultValue=0x14 +PDOMapping=0 +LowLimit=0x0 +HighLimit=0xFFFF + +[1A00] +SubNumber=9 +ParameterName=Transmit PDO Mapping Parameter 1 +ObjectType=0x09 + +[1A00sub0] +ParameterName=Number of mapped objects +ObjectType=0x07 +DataType=0x0005 +AccessType=rw +DefaultValue=0x05 +PDOMapping=0 +LowLimit=0x00 +HighLimit=0x08 + +[1A00sub1] +ParameterName=Mapping Entry 1 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x20210110 +PDOMapping=0 + +[1A00sub2] +ParameterName=Mapping Entry 2 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x20210210 +PDOMapping=0 + +[1A00sub3] +ParameterName=Mapping Entry 3 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x20210310 +PDOMapping=0 + +[1A00sub4] +ParameterName=Mapping Entry 4 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x20210408 +PDOMapping=0 + +[1A00sub5] +ParameterName=Mapping Entry 5 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x20210808 +PDOMapping=0 + +[1A00sub6] +ParameterName=Mapping Entry 6 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1A00sub7] +ParameterName=Mapping Entry 7 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1A00sub8] +ParameterName=Mapping Entry 8 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1A01] +SubNumber=9 +ParameterName=Transmit PDO Mapping Parameter 2 +ObjectType=0x09 + +[1A01sub0] +ParameterName=Number of mapped objects +ObjectType=0x07 +DataType=0x0005 +AccessType=rw +DefaultValue=0x03 +PDOMapping=0 +LowLimit=0x0 +HighLimit=0x8 + +[1A01sub1] +ParameterName=Mapping Entry 1 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x20210510 +PDOMapping=0 + +[1A01sub2] +ParameterName=Mapping Entry 2 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x20210610 +PDOMapping=0 + +[1A01sub3] +ParameterName=Mapping Entry 3 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x20210710 +PDOMapping=0 + +[1A01sub4] +ParameterName=Mapping Entry 4 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x0 +PDOMapping=0 + +[1A01sub5] +ParameterName=Mapping Entry 5 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x0 +PDOMapping=0 + +[1A01sub6] +ParameterName=Mapping Entry 6 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x0 +PDOMapping=0 + +[1A01sub7] +ParameterName=Mapping Entry 7 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x0 +PDOMapping=0 + +[1A01sub8] +ParameterName=Mapping Entry 8 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x0 +PDOMapping=0 + +[1F80] +ParameterName=NMT Startup +ObjectType=0x07 +DataType=0x0007 +AccessType=const +PDOMapping=0 +LowLimit=0x0 +HighLimit=0x3F + +[2001] +SubNumber=6 +ParameterName=Mounting parameters +ObjectType=0x09 + +[2001sub0] +ParameterName=Highest sub-index supported +ObjectType=0x07 +DataType=0x0005 +AccessType=const +DefaultValue=0x5 +PDOMapping=0 + +[2001sub1] +ParameterName=Reserved 1 +ObjectType=0x07 +DataType=0x0007 +AccessType=ro +PDOMapping=0 + +[2001sub2] +ParameterName=Reserved 2 +ObjectType=0x07 +DataType=0x0007 +AccessType=ro +PDOMapping=0 + +[2001sub3] +ParameterName=Reserved 3 +ObjectType=0x07 +DataType=0x0007 +AccessType=ro +PDOMapping=0 + +[2001sub4] +ParameterName=Reserved 4 +ObjectType=0x07 +DataType=0x0007 +AccessType=ro +PDOMapping=0 + +[2001sub5] +ParameterName=Sensor flipped +ObjectType=0x07 +DataType=0x0005 +AccessType=rw +DefaultValue=0x0 +PDOMapping=0 +LowLimit=0x0 +HighLimit=0xFF + +[2002] +SubNumber=4 +ParameterName=Tape parameters +ObjectType=0x09 + +[2002sub0] +ParameterName=Highest sub-index supported +ObjectType=0x07 +DataType=0x0005 +AccessType=const +DefaultValue=0x3 +PDOMapping=0 + +[2002sub1] +ParameterName=Typ. width +ObjectType=0x07 +DataType=0x0008 +AccessType=rw +PDOMapping=0 + +[2002sub2] +ParameterName=Min. width +ObjectType=0x07 +DataType=0x0008 +AccessType=rw +PDOMapping=0 + +[2002sub3] +ParameterName=Max. width +ObjectType=0x07 +DataType=0x0008 +AccessType=rw +PDOMapping=0 + +[2003] +SubNumber=4 +ParameterName=Advanced settings +ObjectType=0x09 + +[2003sub0] +ParameterName=Highest sub-index supported +ObjectType=0x07 +DataType=0x0005 +AccessType=const +DefaultValue=0x3 +PDOMapping=0 + +[2003sub1] +ParameterName=Off delay track detection +ObjectType=0x07 +DataType=0x0006 +AccessType=rw +DefaultValue=0x64 +PDOMapping=0 +LowLimit=0x0 +HighLimit=0xFFFF + +[2003sub2] +ParameterName=Off delay code detection +ObjectType=0x07 +DataType=0x0006 +AccessType=rw +DefaultValue=0x64 +PDOMapping=0 +LowLimit=0x0 +HighLimit=0xFFFF + +[2003sub3] +ParameterName=Position smoothing filter coefficient +ObjectType=0x07 +DataType=0x0008 +AccessType=rw +DefaultValue=0.0 +PDOMapping=0 + +[2018] +ParameterName=Device status +ObjectType=0x07 +DataType=0x0005 +AccessType=ro +PDOMapping=0 + +[2019] +ParameterName=Order number +ObjectType=0x07 +DataType=0x0007 +AccessType=ro +PDOMapping=0 + +[2021] +SubNumber=12 +ParameterName=Result data (LCPs) +ObjectType=0x09 + +[2021sub0] +ParameterName=Highest sub-index supported +ObjectType=0x07 +DataType=0x0005 +AccessType=const +DefaultValue=0x0B +PDOMapping=0 + +[2021sub1] +ParameterName=LCP 1 +ObjectType=0x07 +DataType=0x0003 +AccessType=ro +PDOMapping=1 +LowLimit=0x8000 +HighLimit=0x7FFF + +[2021sub2] +ParameterName=LCP 2 +ObjectType=0x07 +DataType=0x0003 +AccessType=ro +PDOMapping=1 +LowLimit=0x8000 +HighLimit=0x7FFF + +[2021sub3] +ParameterName=LCP 3 +ObjectType=0x07 +DataType=0x0003 +AccessType=ro +PDOMapping=1 +LowLimit=0x8000 +HighLimit=0x7FFF + +[2021sub4] +ParameterName=Status +ObjectType=0x07 +DataType=0x0005 +AccessType=ro +PDOMapping=1 +LowLimit=0x00 +HighLimit=0xFF + +[2021sub5] +ParameterName=Width LCP 1 +ObjectType=0x07 +DataType=0x0003 +AccessType=ro +PDOMapping=1 +LowLimit=0x8000 +HighLimit=0x7FFF + +[2021sub6] +ParameterName=Width LCP 2 +ObjectType=0x07 +DataType=0x0003 +AccessType=ro +PDOMapping=1 +LowLimit=0x8000 +HighLimit=0x7FFF + +[2021sub7] +ParameterName=Width LCP 3 +ObjectType=0x07 +DataType=0x0003 +AccessType=ro +PDOMapping=1 +LowLimit=0x8000 +HighLimit=0x7FFF + +[2021sub8] +ParameterName=Code +ObjectType=0x07 +DataType=0x0005 +AccessType=ro +PDOMapping=1 +LowLimit=0x0 +HighLimit=0xFF + +[2021sub9] +ParameterName=Extended code +ObjectType=0x07 +DataType=0x0007 +AccessType=ro +PDOMapping=0 +LowLimit=0x0 +HighLimit=0xFFFFFFFF + +[2021subA] +ParameterName=Barcode center point +ObjectType=0x07 +DataType=0x0003 +AccessType=ro +DefaultValue=0x0000 +PDOMapping=0 +LowLimit=0x8000 +HighLimit=0x7FFF + +[2021subB] +ParameterName=Quality of lines +ObjectType=0x07 +DataType=0x0005 +AccessType=ro +DefaultValue=0x64 +PDOMapping=0 +LowLimit=0x00 +HighLimit=0x64 + +[2023] +SubNumber=4 +ParameterName=Line level +ObjectType=0x09 + +[2023sub0] +ParameterName=Highest sub-index supported +ObjectType=0x07 +DataType=0x0005 +AccessType=const +DefaultValue=0x3 +PDOMapping=0 +LowLimit=0x1 +HighLimit=0x3 + +[2023sub1] +ParameterName=Intensity line 1 +ObjectType=0x07 +DataType=0x0005 +AccessType=ro +PDOMapping=0 +LowLimit=0x0 +HighLimit=0x82 + +[2023sub2] +ParameterName=Intensity line 2 +ObjectType=0x07 +DataType=0x0005 +AccessType=ro +PDOMapping=0 +LowLimit=0x0 +HighLimit=0x82 + +[2023sub3] +ParameterName=Intensity line 3 +ObjectType=0x07 +DataType=0x0005 +AccessType=ro +PDOMapping=0 +LowLimit=0x0 +HighLimit=0x82 + diff --git a/Devices/Packages/sick_line_guidance/ols/sick_line_guidance_ols10.yaml b/Devices/Packages/sick_line_guidance/ols/sick_line_guidance_ols10.yaml new file mode 100755 index 0000000..d3bc115 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/ols/sick_line_guidance_ols10.yaml @@ -0,0 +1,32 @@ +bus: + device: can0 + driver_plugin: can::SocketCANInterface + master_allocator: canopen::SimpleMaster::Allocator +sync: + interval_ms: 10 + overflow: 0 +# +# heartbeat: # simple heartbeat producer (optional, not supported by OLS or MLS, do not enable heartbeats) +# rate: 100 # heartbeat rate (milliseconds) +# msg: "77f#05" # message to send, cansend format: heartbeat of node 127 with status 5=Started +nodes: + node1: + id: 0x0A # CAN-Node-ID of can device, default: Node-ID 10=0x0A for OLS and MLS + eds_pkg: sick_line_guidance # package name for relative path + eds_file: SICK_OLS10_CO.eds # path to EDS/DCF file + publish: ["1001","1018sub1","1018sub4","2021sub1!","2021sub2!","2021sub3!","2021sub4!","2021sub5!","2021sub6!","2021sub7!","2021sub8!"] + # OLS10: 1001 = error register, 1018sub1 = VendorID, 1018sub4 = SerialNumber, TPDOs 0x2021sub1 to 0x2021sub8 + + # sick_line_guidance configuration of this node: + sick_device_family: "OLS10" # can devices currently supported: "OLS10", "OLS20" or "MLS" + sick_topic: "ols" # OLS_Measurement messages are published in topic "/ols" + sick_frame_id: "ols_measurement_frame" # OLS_Measurement messages are published with frame_id "ols_measurement_frame" + + # device configuration of writable parameter by dcf_overlay: "objectindex": "parametervalue" + # example: "2001sub5": "0x01" # Object 2001sub5 (sensorFlipped, defaultvalue 0x00) will be configured with value 0x01 + # dcf_overlay: + # "2001sub5": "0x01" # sensorFlipped, UINT8, DataType=0x0005, defaultvalue=0 + # "2002sub1": "0.015" # Typ. Width, FLOAT32, DataType=0x0008 + # "2002sub2": "0.001" # Min. Width, FLOAT32, DataType=0x0008 + # "2002sub3": "0.050" # Max. Width, FLOAT32, DataType=0x0008 + diff --git a/Devices/Packages/sick_line_guidance/ols/sick_line_guidance_ols20.yaml b/Devices/Packages/sick_line_guidance/ols/sick_line_guidance_ols20.yaml new file mode 100755 index 0000000..d698c38 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/ols/sick_line_guidance_ols20.yaml @@ -0,0 +1,32 @@ +bus: + device: can0 + driver_plugin: can::SocketCANInterface + master_allocator: canopen::SimpleMaster::Allocator +sync: + interval_ms: 10 + overflow: 0 +# +# heartbeat: # simple heartbeat producer (optional, not supported by OLS or MLS, do not enable heartbeats) +# rate: 100 # heartbeat rate (milliseconds) +# msg: "77f#05" # message to send, cansend format: heartbeat of node 127 with status 5=Started +nodes: + node1: + id: 0x0A # CAN-Node-ID of can device, default: Node-ID 10=0x0A for OLS and MLS + eds_pkg: sick_line_guidance # package name for relative path + eds_file: SICK_OLS20_CO.eds # path to EDS/DCF file + publish: ["1001","1018sub1","1018sub4","2021sub1!","2021sub2!","2021sub3!","2021sub4!","2021sub5!","2021sub6!","2021sub7!","2021sub8!"] + # OLS20: 1001 = error register, 1018sub1 = VendorID, 1018sub4 = SerialNumber, TPDOs 0x2021sub1 to 0x2021sub8 + + # sick_line_guidance configuration of this node: + sick_device_family: "OLS20" # can devices currently supported: "OLS10", "OLS20" or "MLS" + sick_topic: "ols" # OLS_Measurement messages are published in topic "/ols" + sick_frame_id: "ols_measurement_frame" # OLS_Measurement messages are published with frame_id "ols_measurement_frame" + + # device configuration of writable parameter by dcf_overlay: "objectindex": "parametervalue" + # example: "2001sub5": "0x01" # Object 2001sub5 (sensorFlipped, defaultvalue 0x00) will be configured with value 0x01 + # dcf_overlay: + # "2001sub5": "0x01" # sensorFlipped, UINT8, DataType=0x0005, defaultvalue=0 + # "2002sub1": "0.015" # Typ. Width, FLOAT32, DataType=0x0008 + # "2002sub2": "0.001" # Min. Width, FLOAT32, DataType=0x0008 + # "2002sub3": "0.050" # Max. Width, FLOAT32, DataType=0x0008 + diff --git a/Devices/Packages/sick_line_guidance/ols/sick_line_guidance_ols20_twin.yaml b/Devices/Packages/sick_line_guidance/ols/sick_line_guidance_ols20_twin.yaml new file mode 100755 index 0000000..74a3200 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/ols/sick_line_guidance_ols20_twin.yaml @@ -0,0 +1,44 @@ +bus: + device: can0 + driver_plugin: can::SocketCANInterface + master_allocator: canopen::SimpleMaster::Allocator +sync: + interval_ms: 10 + overflow: 0 +# +# heartbeat: # simple heartbeat producer (optional, not supported by OLS or MLS, do not enable heartbeats) +# rate: 100 # heartbeat rate (milliseconds) +# msg: "77f#05" # message to send, cansend format: heartbeat of node 127 with status 5=Started +nodes: + node1: + id: 0x0A # CAN-Node-ID of can device, default: Node-ID 10=0x0A for OLS and MLS + eds_pkg: sick_line_guidance # package name for relative path + eds_file: SICK_OLS20_CO.eds # path to EDS/DCF file + publish: ["1001","1018sub1","1018sub4","2021sub1!","2021sub2!","2021sub3!","2021sub4!","2021sub5!","2021sub6!","2021sub7!","2021sub8!"] + # OLS20: 1001 = error register, 1018sub1 = VendorID, 1018sub4 = SerialNumber, TPDOs 0x2021sub1 to 0x2021sub8 + + # sick_line_guidance configuration of this node: + sick_device_family: "OLS20" # can devices currently supported: "OLS10", "OLS20" or "MLS" + sick_topic: "olsA" # OLS_Measurement messages are published in topic "/ols" + sick_frame_id: "ols20A_measurement_frame" # OLS_Measurement messages are published with frame_id "ols_measurement_frame" + + # device configuration of writable parameter by dcf_overlay: "objectindex": "parametervalue" + # example: "2001sub5": "0x01" # Object 2001sub5 (sensorFlipped, defaultvalue 0x00) will be configured with value 0x01 + # dcf_overlay: + # "2001sub5": "0x01" # sensorFlipped, UINT8, DataType=0x0005, defaultvalue=0 + # "2002sub1": "0.015" # Typ. Width, FLOAT32, DataType=0x0008 + # "2002sub2": "0.001" # Min. Width, FLOAT32, DataType=0x0008 + # "2002sub3": "0.050" # Max. Width, FLOAT32, DataType=0x0008 + + node2: + id: 0x0B # CAN-Node-ID of can device, default: Node-ID 10=0x0A for OLS and MLS + eds_pkg: sick_line_guidance # package name for relative path + eds_file: SICK_OLS20_CO.eds # path to EDS/DCF file + publish: ["1001","1018sub1","1018sub4","2021sub1!","2021sub2!","2021sub3!","2021sub4!","2021sub5!","2021sub6!","2021sub7!","2021sub8!"] + # OLS20: 1001 = error register, 1018sub1 = VendorID, 1018sub4 = SerialNumber, TPDOs 0x2021sub1 to 0x2021sub8 + + # sick_line_guidance configuration of this node: + sick_device_family: "OLS20" # can devices currently supported: "OLS10", "OLS20" or "MLS" + sick_topic: "olsB" # OLS_Measurement messages are published in topic "/ols" + sick_frame_id: "ols20B_measurement_frame" # OLS_Measurement messages are published with frame_id "ols_measurement_frame" + diff --git a/Devices/Packages/sick_line_guidance/package.xml b/Devices/Packages/sick_line_guidance/package.xml new file mode 100755 index 0000000..0cd7928 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/package.xml @@ -0,0 +1,81 @@ + + + sick_line_guidance + 0.0.0 + SICK Line Guidance ROS Support + + + + + rostest + + + + + Apache + + + + + + https://github.com/SICKAG/sick_line_guidance + https://www.sick.com/ols + https://www.sick.com/mls + + + + + + Michael Lehning + + + + + + + + + + + + + + + + + + + + + + catkin + cmake_modules + roscpp + rospy + std_msgs + sensor_msgs + message_generation + random_numbers + can_msgs + canopen_chain_node + roscpp + rospy + std_msgs + sensor_msgs + message_generation + random_numbers + can_msgs + canopen_chain_node + roscpp + rospy + std_msgs + message_runtime + random_numbers + can_msgs + canopen_chain_node + + + + + + diff --git a/Devices/Packages/sick_line_guidance/src/sick_canopen_simu_canstate.cpp b/Devices/Packages/sick_line_guidance/src/sick_canopen_simu_canstate.cpp new file mode 100755 index 0000000..12b9f13 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/src/sick_canopen_simu_canstate.cpp @@ -0,0 +1,133 @@ +/* + * sick_canopen_simu_canstate implements a state engine for can. + * + * Depending on input messages of type can_msgs::Frame, + * the current state is switched between INITIALIZATION, + * PRE_OPERATIONAL, OPERATIONAL and STOPPED. + * + * class CanState: handles can nmt messages and implements the state engine for can. + * + * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim + * Copyright (C) 2019 SICK AG, Waldkirch + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of SICK AG nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Authors: + * Michael Lehning + * + * Copyright 2019 SICK AG + * Copyright 2019 Ing.-Buero Dr. Michael Lehning + * + */ +#include +#include +#include "sick_line_guidance/sick_canopen_simu_canstate.h" + +/* + * Constructor. + * + * @param[in] can_node_id node id of OLS or MLS, default: 0x0A + */ +sick_canopen_simu::CanState::CanState(int can_node_id) : m_can_node_id(can_node_id), m_can_state(INITIALIZATION) +{ +} + +/* + * Destructor. + */ +sick_canopen_simu::CanState::~CanState() +{ +} + +/* + * @brief Callbacks for ros messages. Switches the state, and returns true, if msg_in is a can nmt message. + * Otherwise, false is returned. + * + * param[in] msg_in input message of type can_msgs::Frame + * param[out] msg_out output message of type can_msgs::Frame (if input message requires a response to send) + * param[out] send_msg true, if input message msg_in requires the response message msg_out to be send + * + * @return true, if input message handled, otherwise false. + */ +bool sick_canopen_simu::CanState::messageHandler(const can_msgs::Frame &msg_in, can_msgs::Frame & msg_out, bool & send_msg) +{ + bool msg_handled = false; + send_msg = false; + // 000#... (msg_in.id == 0 && msg_in.dlc >= ): nmt message from can master (network manager) with 2 byte (command and nodeid) + // msg_in.data[0] : nmt command (0x1, 0x2, 0x80, 0x81 or 0x82) + // msg_in.data[1] : nodeid, message has to be handled if nodeid==0 (broadcast to all devices), or nodeid==m_can_node_id + if (msg_in.id == 0 && msg_in.dlc >= 2 && (msg_in.data[1] == 0 || msg_in.data[1] == m_can_node_id)) + { + msg_out = msg_in; + switch (msg_in.data[0]) + { + case 0x80: // 000#0x80...: Pre-operational -> switch to PRE_OPERATIONAL and send bootup message (0x700+$NODEID)#0x00 + case 0x81: // 000#0x81...: Reset node -> switch to PRE_OPERATIONAL and send bootup message (0x700+$NODEID)#0x00 + case 0x82: // 000#0x82...: Reset communication -> switch to PRE_OPERATIONAL and send bootup message (0x700+$NODEID)#0x00 + msg_out.id = 0x700 + m_can_node_id; + msg_out.dlc = 1; + msg_out.data[0] = 0; + msg_out.header.stamp = ros::Time::now(); + send_msg = true; // msg_out contains the bootup message to send + m_can_state = PRE_OPERATIONAL; + ROS_INFO_STREAM("sick_canopen_simu::CanState::messageHandler(): switched to state PRE_OPERATIONAL."); + msg_handled = true; + break; + + case 0x01: // 000#0x01: switch to OPERATIONAL + m_can_state = OPERATIONAL; + ROS_INFO_STREAM("sick_canopen_simu::CanState::messageHandler(): switched to state OPERATIONAL."); + msg_handled = true; + break; + + case 0x02: // 000#0x02: switch to STOPPED + m_can_state = STOPPED; + ROS_INFO_STREAM("sick_canopen_simu::CanState::messageHandler(): switched to state STOPPED."); + msg_handled = true; + break; + + default: + break; + } + } + return msg_handled; +} diff --git a/Devices/Packages/sick_line_guidance/src/sick_canopen_simu_device.cpp b/Devices/Packages/sick_line_guidance/src/sick_canopen_simu_device.cpp new file mode 100755 index 0000000..8d8b2d9 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/src/sick_canopen_simu_device.cpp @@ -0,0 +1,763 @@ +/* + * Class SimulatorBase implements the base class for simulation of SICK can devices (OLS20 and MLS). + * + * ROS messages of type can_msgs::Frame are read from ros topic "can0", + * handled to simulate an OLS20 or MLS device, and resulting can_msgs::Frame + * messages are published on ros topic "ros2can0". + * + * MsgType can be sick_line_guidance::OLS_Measurement (for simulation of OLS devices), or + * or sick_line_guidance::MLS_Measurement (for simulation of MLS devices). + * + * Assumption: ros nodes sick_line_guidance_ros2can_node and sick_line_guidance_can2ros_node + * have to be running to convert ros messages from and to canbus messages. + * sick_line_guidance_ros2can_node converts ros messages of type can_msgs::Frame to can frames, + * sick_line_guidance_can2ros_node converts can frames to ros messages of type can_msgs::Frame. + * + * Subclass MLSSimulator extends class SimulatorBase to simulate an MLS device. + * + * Subclass OLS20Simulator extends class SimulatorBase to simulate an OLS20 device. + * + * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim + * Copyright (C) 2019 SICK AG, Waldkirch + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of SICK AG nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Authors: + * Michael Lehning + * + * Copyright 2019 SICK AG + * Copyright 2019 Ing.-Buero Dr. Michael Lehning + * + */ +#include +#include +#include "sick_line_guidance/sick_canopen_simu_device.h" +#include "sick_line_guidance/sick_line_guidance_msg_util.h" + +/* + * Constructor + * + * @param[in] nh ros node handle + * @param[in] config_file configuration file with testcases for OLS and MLS simulation + * @param[in] can_node_id node id of OLS or MLS, default: 0x0A + * @param[in] subscribe_topic ros topic to receive input messages of type can_msgs::Frame, default: "can0" + * @param[in] publish_topic ros topic to publish output messages of type can_msgs::Frame, default: "ros2can0" + * @param[in] pdo_rate rate of PDO (default: pdo_rate = 50, i.e. 20 ms between two PDOs or 50 PDOs per second) + * @param[in] pdo_repeat_cnt each sensor state spefied in sick_canopen_simu_cfg.xml is repeated N times before switching to the next state (default: 5 times) + * @param[in] subscribe_queue_size buffer size for ros messages + */ +template +sick_canopen_simu::SimulatorBase::SimulatorBase(ros::NodeHandle & nh, const std::string & config_file, int can_node_id, const std::string & subscribe_topic, const std::string & publish_topic, const ros::Rate & pdo_rate, int pdo_repeat_cnt, int subscribe_queue_size) +: m_can_state(can_node_id), m_can_node_id(can_node_id), m_pdo_rate(pdo_rate), m_pdo_repeat_cnt(pdo_repeat_cnt), m_pdo_publisher_thread(0), m_pdo_publisher_thread_running(false), m_subscribe_queue_size(subscribe_queue_size), m_send_tpdo_immediately(false) +{ + m_sdo_response_dev_state = 0x4F18200000000000; // response to sdo request for dev_status (object 0x2018): MLS and OLS20: 0x4F18200000000000 (sdo response with UINT8 data), OLS10: 0x4B18200000000000 (sdo response with UINT16 data) + m_ros_publisher = nh.advertise(publish_topic, 1); + if(!readSDOconfig(config_file)) + { + ROS_ERROR_STREAM("SimulatorBase: readSDOconfig(" << config_file << ") failed"); + } +} + +/* + * Destructor + * + */ +template +sick_canopen_simu::SimulatorBase::~SimulatorBase() +{ + m_pdo_publisher_thread_running = false; + if(m_pdo_publisher_thread) + { + m_pdo_publisher_thread->join(); + delete(m_pdo_publisher_thread); + m_pdo_publisher_thread = 0; + } +} + +/* + * @brief Callbacks for ros messages. Converts incoming messages of type can_msgs::Frame to simulate a can device + * and publishes simulation results to the configured ros topic. + * + * param[in] msg ros message of type can_msgs::Frame + */ +template +void sick_canopen_simu::SimulatorBase::messageHandler(const can_msgs::Frame & msg_in) +{ + // Handle NMT messages + bool send_msg = false; + can_msgs::Frame msg_out = msg_in; + if(m_can_state.messageHandler(msg_in, msg_out, send_msg)) + { + if(send_msg) + { + boost::lock_guard publish_lockguard(m_ros_publisher_mutex); + m_ros_publisher.publish(msg_out); + } + return; // nmt message handled in m_can_state + } + // Ignore sync frames and bootup messages (node guarding frames) sent by can devices + if((msg_in.id == 0x80) || (msg_in.id >= 0x700 && msg_in.id <= 0x77F)) + { + return; + } + // Ignore TPDO1 and TPDO2 frames sent by can devices + if((msg_in.id >= 0x180 && msg_in.id <= 0x1FF) || (msg_in.id >= 0x280 && msg_in.id <= 0x2FF)) + { + return; + } + // Handle SDOs + if(handleSDO(msg_in)) + { + return; // SDO request handled, SDO response sent + } + ROS_ERROR_STREAM("SimulatorBase::messageHandler(): message " << tostring(msg_in) << " not handled"); +} + + +/* + * @brief Registers a listener to a simulation. Whenever the simulation sends a PDO, the listener is notified about the current sensor state. + * After receiving both the sensor state and the following OLS/MLS-Measurement ros message from the driver, the listener can check + * the correctness by comparing the sensor state from simulation and the OLS/MLS-Measurement from the driver. Both must be identical, + * otherwise some failure occured. + * + * param[in] pListener listener to current sensor states sent by PDO. + */ +template +void sick_canopen_simu::SimulatorBase::registerSimulationListener(sick_canopen_simu::SimulatorBase::SimulationListener* pListener) +{ + m_vec_simu_listener.push_back(pListener); // append pListener to the list of registered listeners +} + +/* + * @brief Unregisters a listener from a simulation. The listener will not be notified about simulated sensor states. + * This function is the opposite to registerSimulationListener. + * + * param[in] pListener listener to current sensor states sent by PDO. + */ +template +void sick_canopen_simu::SimulatorBase::unregisterSimulationListener(sick_canopen_simu::SimulatorBase::SimulationListener* pListener) +{ + for(typename std::vector::iterator iter = m_vec_simu_listener.begin(); iter != m_vec_simu_listener.end(); iter++) + { + if(*iter == pListener) + { + m_vec_simu_listener.erase(iter); // remove pListener from the list of registered listeners + break; + } + } +} + +/* + * reads SDO configuration from xml-file + * + * @param[in] config_file configuration file with testcases for OLS and MLS simulation + */ +template +bool sick_canopen_simu::SimulatorBase::readSDOconfig(const std::string & config_file) +{ + try + { + TiXmlDocument xml_config(config_file); + if(xml_config.LoadFile()) + { + ROS_INFO_STREAM("SimulatorBase::readSDOconfig(): reading SDO map from xml-file " << config_file); + TiXmlElement* xml_sdo_config = xml_config.FirstChild("sick_canopen_simu")->FirstChild("SDO_config")->ToElement(); + if(xml_sdo_config) + { + TiXmlElement* xml_sdo = xml_sdo_config->FirstChildElement("sdo"); + while(xml_sdo) + { + const std::string sdo_request = xml_sdo->Attribute("request"); + const std::string sdo_response = xml_sdo->Attribute("response"); + uint64_t u64_sdo_request = std::stoull(sdo_request, 0, 0); + uint64_t u64_sdo_response = std::stoull(sdo_response, 0, 0); + m_sdo_request_response_map[u64_sdo_request] = u64_sdo_response; + xml_sdo = xml_sdo->NextSiblingElement("sdo"); + ROS_DEBUG_STREAM("SimulatorBase::readSDOconfig(): sdo_request_response_map[0x" << std::hex << u64_sdo_request << "] = 0x" << std::hex << u64_sdo_response + << " (sdo_request_response_map[\"" << sdo_request << "\"] = \"" << sdo_response<< "\")"); + } + if(!m_sdo_request_response_map.empty()) + { + ROS_INFO_STREAM("SimulatorBase::readSDOconfig(" << config_file << "): " << m_sdo_request_response_map.size() << " sdo elements found"); + return true; + } + else + { + ROS_ERROR_STREAM("SimulatorBase::readSDOconfig(" << config_file << "): no sdo elements found"); + } + } + else + { + ROS_ERROR_STREAM("SimulatorBase::readSDOconfig(" << config_file << "): can't read element sick_canopen_simu/SDO_config"); + } + } + else + { + ROS_ERROR_STREAM("SimulatorBase: can't read xml-file " << config_file); + } + } + catch(const std::exception & exc) + { + ROS_ERROR_STREAM("SimulatorBase::readSDOconfig("<< config_file << ") failed: exception " << exc.what()); + } + return false; +} + +/* + * reads the PDO configuration from xml-file + * + * @param[in] config_file configuration file with testcases for simulation + * @param[in] sick_device_family "OLS10", OLS20" or "MLS" + */ +template +bool sick_canopen_simu::SimulatorBase::readPDOconfig(const std::string & config_file, const std::string & sick_device_family) +{ + try + { + TiXmlDocument xml_config(config_file); + if(xml_config.LoadFile()) + { + ROS_INFO_STREAM("SimulatorBase::readPDOconfig(): reading PDO map from xml-file " << config_file); + TiXmlElement* xml_device_config = xml_config.FirstChild("sick_canopen_simu")->FirstChild(sick_device_family)->ToElement(); + if(!xml_device_config) + { + ROS_ERROR_STREAM("SimulatorBase::readPDOconfig(" << config_file << "): element sick_canopen_simu/" << sick_device_family << " not found"); + return false; + } + TiXmlElement* xml_pdo_config = xml_device_config->FirstChildElement("PDO_config")->ToElement(); + if(xml_pdo_config) + { + TiXmlElement* xml_pdo = xml_pdo_config->FirstChildElement("pdo"); + while(xml_pdo) + { + parseXmlPDO(xml_pdo); + xml_pdo = xml_pdo->NextSiblingElement("pdo"); + ROS_DEBUG_STREAM("SimulatorBase::readPDOconfig(): " << sick_line_guidance::MsgUtil::toInfo(m_vec_pdo_measurements.back())); + } + if(!m_vec_pdo_measurements.empty()) + { + ROS_INFO_STREAM("SimulatorBase::readPDOconfig(" << config_file << "): " << m_vec_pdo_measurements.size() << " pdo elements found"); + return true; + } + else + { + ROS_ERROR_STREAM("SimulatorBase::readPDOconfig(" << config_file << "): no pdo elements found"); + } + } + else + { + ROS_ERROR_STREAM("SimulatorBase::readPDOconfig(" << config_file << "): can't read element sick_canopen_simu/" << sick_device_family << "/PDO_config"); + } + } + else + { + ROS_ERROR_STREAM("SimulatorBase::readPDOconfig(): can't read xml-file " << config_file); + } + } + catch(const std::exception & exc) + { + ROS_ERROR_STREAM("SimulatorBase::readPDOconfig("<< config_file << ") failed: exception " << exc.what()); + } + return false; +} + +/* + * @brief handles SDOs, i.e. sends a SDO response and returns true, if can frame is a SDO request. + * Otherwise, the can frame is not handled and false is returned. + * Note: The SDO response is simply taken from a lookup-table (input: SDO request frame data, output: SDO response frame data). + * If the SDO request frame data (frame.data) is not found in the lookup-table, the can frame is not handled and false is returned, too. + * + * @param[in] msg_in received can frame + * + * @return true if can frame was handled and a SDO request is sent, false otherwise. + */ +template +bool sick_canopen_simu::SimulatorBase::handleSDO(const can_msgs::Frame & msg_in) +{ + // can id == (0x600+$NODEID) with 8 byte data => SDO request => send SDO response (0x580+$NODEID)#... + if(msg_in.id == 0x600 + m_can_node_id && msg_in.dlc == 8) + { + boost::lock_guard publish_lockguard(m_ros_publisher_mutex); + can_msgs::Frame msg_out = msg_in; + uint64_t sdo_request_data = frameDataToUInt64(msg_in); // convert msg_in.data to uint64_t + uint64_t sdo_response_data = m_sdo_request_response_map[sdo_request_data]; // lookup table: sdo response data := m_sdo_request_response_map[] + if(sdo_response_data != 0) // SDO response found in lookup table + { + // send SDO response (0x580+$NODEID)# + msg_out.id = 0x580 + m_can_node_id; + uint64ToFrameData(sdo_response_data, msg_out); + msg_out.header.stamp = ros::Time::now(); + m_ros_publisher.publish(msg_out); + ROS_INFO_STREAM("SimulatorBase::handleSDO(): SDO request " << tostring(msg_in) << " handled, SDO response " << tostring(msg_out) << " published."); + return true; + } + else if( (sdo_response_data = m_sdo_request_response_map[(sdo_request_data & 0xFFFFFFFF00000000ULL)]) != 0) + { + // Handle SDO response for a download request, f.e. SDO request = 0x23022003CDCC4C3D = 0x2302200300000000 + <4 byte data>: + // Set value 0xCDCC4C3D in object [2002sub03] -> SDO response = 0x6002200300000000, SDO request = 0x4002200300000000 -> SDO response = 0x43022003CDCC4C3D + uint64_t sdo_object_index = (sdo_request_data & 0x00FFFFFF00000000ULL); + uint64_t sdo_object_value = (sdo_request_data & 0x00000000FFFFFFFFULL); + uint64_t sdo_parameter_bytes = (sdo_request_data & 0x0F00000000000000ULL); + m_sdo_request_response_map[0x4000000000000000 | sdo_object_index] = (0x4000000000000000 | sdo_parameter_bytes | sdo_object_index | sdo_object_value); + // send SDO response (0x580+$NODEID)# + msg_out.id = 0x580 + m_can_node_id; + uint64ToFrameData(sdo_response_data, msg_out); + msg_out.header.stamp = ros::Time::now(); + m_ros_publisher.publish(msg_out); + ROS_INFO_STREAM("SimulatorBase::handleSDO(): SDO request " << tostring(msg_in) << " handled, SDO response " << tostring(msg_out) << " published."); + return true; + } + else + { + ROS_ERROR_STREAM("SimulatorBase::handleSDO(): SDO " << tostring(msg_in) << " not handled"); + return false; + } + } + // Ignore SDO requests (0x600+$NODEID) or SDO responses (0x580+$NODEID) from other can devices + if((msg_in.id >= 0x600 && msg_in.id <= 0x67F) || (msg_in.id >= 0x580 && msg_in.id <= 0x5FF)) + { + return true; + } + return false; +} + +/* + * @brief Publishes PDOs to simulate a MLS or OLS20 device while can state is operational + */ +template +void sick_canopen_simu::SimulatorBase::runPDOthread(void) +{ + size_t pdo_cnt = 0; + while(ros::ok() && m_pdo_publisher_thread_running) + { + m_pdo_rate.sleep(); // ros::Duration(0.01).sleep(); + if(m_can_state.state() == OPERATIONAL || m_send_tpdo_immediately ) // OLS10, MLS: send TPDOs immediately in all states (pre-operational and operational), OLS20: send TPDOs only in state operational + { + // Note: Depending on the transmission type, PDOs are transmitted either asynchronously or synchronously (https://www.canopensolutions.com/english/about_canopen/pdo.shtml): + // "Asynchronous PDOs are event-controlled ... when at least one of the process variables mapped in a PDO is altered, for example an input value, the PDO is immediately transmitted." + // "Synchronous PDOs are only transmitted after prior reception of a synchronization message (Sync Object)". + // Default transmission type for OLS and MLS is 0xFF (asynchronous). + boost::lock_guard publish_lockguard(m_ros_publisher_mutex); + can_msgs::Frame tpdo_msg[2]; + int measurement_idx = ((pdo_cnt/m_pdo_repeat_cnt) % (m_vec_pdo_measurements.size())); // simulate a different measurement every 500 milliseconds (25 times, 20 milliseconds each) + MsgType & pdo_measurement = m_vec_pdo_measurements[measurement_idx]; + // convert sensor measurement to can frames + int tpdo_msg_cnt = convertToCanFrame(pdo_measurement, tpdo_msg[0], tpdo_msg[1]); + // send can frames + for(int n = 0; n < tpdo_msg_cnt; n++) + { + tpdo_msg[n].header.stamp = ros::Time::now(); + m_ros_publisher.publish(tpdo_msg[n]); + ros::Duration(0.001).sleep(); + ROS_INFO_STREAM("SimulatorBase::runPDOthread(): pdo frame " << tostring(tpdo_msg[n]) << " published."); + } + // Notify all registered listener + for(typename std::vector::iterator iter = m_vec_simu_listener.begin(); iter != m_vec_simu_listener.end(); iter++) + { + (*iter)->pdoSent(pdo_measurement); + } + pdo_cnt++; + } + } + m_pdo_publisher_thread_running = false; +} + +/* + * @brief Convertes an MLS_Measurement into a can_msgs::Frame TPDO1. + * @param[in] measurement MLS_Measurement to be converted + * @param[out] tpdo1_msg output can frame TPDO1, + * @param[out] tpdo2_msg dummy frame TPDO2, unused + * @return Returns the number of TPDOs, i.e. 1 for MLS devices. + */ +template +int sick_canopen_simu::SimulatorBase::convertToCanFrame(const sick_line_guidance::MLS_Measurement & measurement, can_msgs::Frame & tpdo1_msg, can_msgs::Frame & tpdo2_msg) +{ + assert(tpdo1_msg.data.size() == 8); + assert(measurement.position.size() == 3); + // convert position and width in meter to lcp and width in millimeter + int lcp1 = convertLCP(measurement.position[0]); + int lcp2 = convertLCP(measurement.position[1]); + int lcp3 = convertLCP(measurement.position[2]); + // set TPDO1: (180+$NODEID)#<8 byte data> + tpdo1_msg.id = 0x180 + m_can_node_id; + tpdo1_msg.dlc = 8; + tpdo1_msg.is_error = false; + tpdo1_msg.is_rtr = false; + tpdo1_msg.is_extended = false; + tpdo1_msg.data[0] = (lcp1 & 0xFF); // LSB LCP1 + tpdo1_msg.data[1] = ((lcp1 >> 8) & 0xFF); // MSB LCP1 + tpdo1_msg.data[2] = (lcp2 & 0xFF); // LSB LCP2 + tpdo1_msg.data[3] = ((lcp2 >> 8) & 0xFF); // MSB LCP2 + tpdo1_msg.data[4] = (lcp3 & 0xFF); // LSB LCP3 + tpdo1_msg.data[5] = ((lcp3 >> 8) & 0xFF); // MSB LCP3 + tpdo1_msg.data[6] = measurement.lcp; + tpdo1_msg.data[7] = measurement.status; + tpdo1_msg.header.stamp = ros::Time::now(); + // set error register 1001 in object dictionary + uint32_t sdo_data_error = ((measurement.error & 0xFFUL) << 24); + m_sdo_request_response_map[0x4001100000000000] = (0x4F01100000000000 | (sdo_data_error & 0xFF000000ULL)); // measurement.error -> set 0x1001 in object dictionary + return 1; // one TPDO set +} + +/* + * @brief Convertes an OLS_Measurement into two can_msgs::Frame TPDO1 and TPDO2. + * @param[in] measurement OLS_Measurement to be converted + * @param[out] tpdo1_msg output can frame TPDO1, Byte 1-8 := LCP1(LSB,MSB,0x2021sub1), LCP2(LSB,MSB,0x2021sub2), LCP3(LSB,MSB,0x2021sub3), status(UINT8,0x2021sub4), barcode(UINT8,0x2021sub8) + * @param[out] tpdo2_msg output can frame TPDO2, Byte 1-6 := Width1(LSB,MSB,0x2021sub5), Width2(LSB,MSB,0x2021sub6), Width3(LSB,MSB,0x2021sub7) + * @return Returns the number of TPDOs, i.e. 2 for OLS devices. + */ +template +int sick_canopen_simu::SimulatorBase::convertToCanFrame(const sick_line_guidance::OLS_Measurement & measurement, can_msgs::Frame & tpdo1_msg, can_msgs::Frame & tpdo2_msg) +{ + assert(tpdo1_msg.data.size() == 8); + assert(tpdo2_msg.data.size() == 8); + assert(measurement.position.size() == 3); + assert(measurement.width.size() == 3); + assert(revertByteorder(0x12345678) == 0x78563412); + // convert position and width in meter to lcp and width in millimeter + int lcp1 = convertLCP(measurement.position[0]); + int lcp2 = convertLCP(measurement.position[1]); + int lcp3 = convertLCP(measurement.position[2]); + int width1 = convertLCP(measurement.width[0]); + int width2 = convertLCP(measurement.width[1]); + int width3 = convertLCP(measurement.width[2]); + // set TPDO1: (180+$NODEID)#<8 byte data> + tpdo1_msg.id = 0x180 + m_can_node_id; + tpdo1_msg.dlc = 8; + tpdo1_msg.is_error = false; + tpdo1_msg.is_rtr = false; + tpdo1_msg.is_extended = false; + tpdo1_msg.data[0] = (lcp1 & 0xFF); // LSB LCP1 + tpdo1_msg.data[1] = ((lcp1 >> 8) & 0xFF); // MSB LCP1 + tpdo1_msg.data[2] = (lcp2 & 0xFF); // LSB LCP2 + tpdo1_msg.data[3] = ((lcp2 >> 8) & 0xFF); // MSB LCP2 + tpdo1_msg.data[4] = (lcp3 & 0xFF); // LSB LCP3 + tpdo1_msg.data[5] = ((lcp3 >> 8) & 0xFF); // MSB LCP3 + tpdo1_msg.data[6] = measurement.status; + tpdo1_msg.data[7] = measurement.barcode; + tpdo1_msg.header.stamp = ros::Time::now(); + // set TPDO2: (280+$NODEID)#<6 byte data> + tpdo2_msg.id = 0x280 + m_can_node_id; + tpdo2_msg.dlc = 6; + tpdo2_msg.is_error = false; + tpdo2_msg.is_rtr = false; + tpdo2_msg.is_extended = false; + tpdo2_msg.data[0] = (width1 & 0xFF); // LSB width1 + tpdo2_msg.data[1] = ((width1 >> 8) & 0xFF); // MSB width1 + tpdo2_msg.data[2] = (width2 & 0xFF); // LSB width2 + tpdo2_msg.data[3] = ((width2 >> 8) & 0xFF); // MSB width2 + tpdo2_msg.data[4] = (width3 & 0xFF); // LSB width3 + tpdo2_msg.data[5] = ((width3 >> 8) & 0xFF); // MSB width3 + tpdo2_msg.data[6] = 0; + tpdo2_msg.data[7] = 0; + tpdo2_msg.header.stamp = ros::Time::now(); + // set objects in dictionary + uint32_t sdo_data_error = ((measurement.error & 0xFFUL) << 24); + uint32_t sdo_data_dev_status = ((measurement.dev_status & 0xFFUL) << 24); + uint32_t sdo_data_extended_code = revertByteorder(measurement.extended_code); + m_sdo_request_response_map[0x4001100000000000] = (0x4F01100000000000 | (sdo_data_error & 0xFF000000ULL)); // measurement.error -> set 0x1001 in object dictionary + m_sdo_request_response_map[0x4018200000000000] = (m_sdo_response_dev_state | (sdo_data_dev_status & 0xFF000000ULL)); // measurement.dev_status -> set 0x2018 in object dictionary (OLS20: UINT8, OLS10: UINT16) + m_sdo_request_response_map[0x4021200900000000] = (0x4321200900000000 | (sdo_data_extended_code & 0xFFFFFFFFULL)); // measurement.extended_code -> set 0x2021sub9 in object dictionary + // OLS20 only: simulate barcode center point, object 0x2021subA (INT16), OLS10: always 0 + int16_t barcodecenter = (int16_t)(measurement.barcode_center_point * 1000); + uint32_t sdo_data = ((barcodecenter & 0xFFUL) << 24) | ((barcodecenter & 0xFF00UL) << 8); + m_sdo_request_response_map[0x4021200A00000000] = (0x4B21200A00000000 | (sdo_data & 0xFFFF0000ULL)); + // OLS20 only: simulate quality of lines, object 0x2021subB (UINT8), OLS10: always 0 + sdo_data = ((measurement.quality_of_lines & 0xFFUL) << 24); + m_sdo_request_response_map[0x4021200B00000000] = (0x4F21200B00000000 | (sdo_data & 0xFF000000ULL)); + // OLS20 only: simulate intensity of lines, object 0x2023sub1 to 0x2023sub3 (UINT8), OLS10: always 0 + sdo_data = ((measurement.intensity_of_lines[0] & 0xFFUL) << 24); + m_sdo_request_response_map[0x4023200100000000] = (0x4F23200100000000 | (sdo_data & 0xFF000000ULL)); + sdo_data = ((measurement.intensity_of_lines[1] & 0xFFUL) << 24); + m_sdo_request_response_map[0x4023200200000000] = (0x4F23200200000000 | (sdo_data & 0xFF000000ULL)); + sdo_data = ((measurement.intensity_of_lines[2] & 0xFFUL) << 24); + m_sdo_request_response_map[0x4023200300000000] = (0x4F23200300000000 | (sdo_data & 0xFF000000ULL)); + return 2; // both TPDOs set +} + +/* + * @brief Converts a position or width (float value in meter) to lcp (INT16 value in millimeter), + * shortcut for std::round(lcp * 1000) with clipping to range INT16_MIN ... INT16_MAX. + * @param[in] lcp position or width (float value in meter) + * @return INT16 value in millimeter + */ +template +int sick_canopen_simu::SimulatorBase::convertLCP(float lcp) +{ + return boost::algorithm::clamp((int)(std::round(lcp * 1000)), INT16_MIN, INT16_MAX); +} + +/* + * @brief Converts frame.data to uint64_t + * @param[in] frame can frame + * @return frame.data converted to uint64_t + */ +template +uint64_t sick_canopen_simu::SimulatorBase::frameDataToUInt64(const can_msgs::Frame & frame) +{ + assert(frame.data.size() == 8); + uint64_t u64data = 0; + for(size_t n = 0; n < std::min(frame.data.size(), (size_t)(frame.dlc & 0xFF)); n++) + { + u64data = ((u64data << 8) | (frame.data[n] & 0xFF)); + } + return u64data; +} + +/* + * @brief Converts uint64_t data to frame.data + * @param[in] u64data frame data (uint64_t) + * @param[in+out] frame can frame + */ +template +void sick_canopen_simu::SimulatorBase::uint64ToFrameData(uint64_t u64data, can_msgs::Frame & frame) +{ + assert(frame.data.size() == 8); + frame.dlc = 8; + for(int n = frame.data.size() - 1; n >= 0; n--) + { + frame.data[n] = (u64data & 0xFF); + u64data = (u64data >> 8); + } +} + +/* + * @brief prints and returns a can_msgs::Frame in short format like candump (f.a. "18A#B4FFCCFF00000300") + */ +template +std::string sick_canopen_simu::SimulatorBase::tostring(const can_msgs::Frame & canframe) +{ + std::stringstream str; + str << std::uppercase << std::hex << std::setfill('0') << std::setw(3) << (unsigned)(canframe.id) << "#"; + for(size_t n = 0; n < std::min((size_t)(canframe.dlc & 0xFF), canframe.data.size()); n++) + str << std::uppercase << std::hex << std::setfill('0') << std::setw(2) << (unsigned)(canframe.data[n] & 0xFF); + return str.str(); +} + +/* + * Subclass MLSSimulator extends class SimulatorBase to simulate a MLS device. + * + */ + +/* + * Constructor + * + * @param[in] nh ros node handle + * @param[in] config_file configuration file with testcases for OLS and MLS simulation + * @param[in] sick_device_family "OLS10", "OLS20" or "MLS" + * @param[in] can_node_id node id of OLS or MLS, default: 0x0A + * @param[in] subscribe_topic ros topic to receive input messages of type can_msgs::Frame, default: "can0" + * @param[in] publish_topic ros topic to publish output messages of type can_msgs::Frame, default: "ros2can0" + * @param[in] pdo_rate rate of PDO (default: pdo_rate = 50, i.e. 20 ms between two PDOs or 50 PDOs per second) + * @param[in] pdo_repeat_cnt each sensor state spefied in sick_canopen_simu_cfg.xml is repeated N times before switching to the next state (default: 5 times) + * @param[in] subscribe_queue_size buffer size for ros messages + */ +sick_canopen_simu::MLSSimulator::MLSSimulator(ros::NodeHandle & nh, const std::string & config_file, const std::string & sick_device_family, int can_node_id, const std::string & subscribe_topic, const std::string & publish_topic, const ros::Rate & pdo_rate, int pdo_repeat_cnt, int subscribe_queue_size) + : SimulatorBase(nh, config_file, can_node_id, subscribe_topic, publish_topic, pdo_rate, pdo_repeat_cnt, subscribe_queue_size) +{ + m_send_tpdo_immediately = true; // true (OLS10, MLS): send TPDOs immediately in all states (pre-operational and operational), false (default, OLS20): send TPDOs only in state operational + m_ros_subscriber = nh.subscribe(subscribe_topic, m_subscribe_queue_size, &sick_canopen_simu::MLSSimulator::messageHandler, this); + if(!readPDOconfig(config_file, sick_device_family)) + { + ROS_ERROR_STREAM("MLSSimulator: readPDOconfig(" << config_file << ") failed"); + } + // Start thread for publishing PDOs + m_pdo_publisher_thread_running = true; + m_pdo_publisher_thread = new boost::thread(&sick_canopen_simu::MLSSimulator::runPDOthread, this); +} + +/* + * @brief Callbacks for ros messages. Converts incoming messages of type can_msgs::Frame to simulate a MLS device + * and publishes simulation results to the configured ros topic. + * + * param[in] msg ros message of type can_msgs::Frame + */ +void sick_canopen_simu::MLSSimulator::messageHandler(const can_msgs::Frame & msg_in) +{ + SimulatorBase::messageHandler(msg_in); +} + +/* + * @brief Parses an pdo element from config file and appends it to m_vec_pdo_measurements. + * + * param[in] xml_pdo pdo element from config file + */ +bool sick_canopen_simu::MLSSimulator::parseXmlPDO(TiXmlElement* xml_pdo) +{ + try + { + m_vec_pdo_measurements.push_back(sick_line_guidance::MsgUtil::convertMLSMessage( + std::stof(xml_pdo->Attribute("lcp1")), std::stof(xml_pdo->Attribute("lcp2")), std::stof(xml_pdo->Attribute("lcp3")), + std::stoul(xml_pdo->Attribute("status"), 0, 0), + std::stoul(xml_pdo->Attribute("lcp"), 0, 0), + std::stoul(xml_pdo->Attribute("error"), 0, 0), + xml_pdo->Attribute("frame_id"))); + return true; + } + catch(const std::exception & exc) + { + ROS_ERROR_STREAM("MLSSimulator::parseXmlPDO() failed: exception " << exc.what()); + } + return false; +} + +/* + * Subclass OLSSimulator extends class SimulatorBase to simulate an OLS devices. + * + */ + +/* + * Constructor + * + * @param[in] nh ros node handle + * @param[in] config_file configuration file with testcases for OLS and MLS simulation + * @param[in] sick_device_family "OLS10", "OLS20" or "MLS" + * @param[in] can_node_id node id of OLS, default: 0x0A + * @param[in] subscribe_topic ros topic to receive input messages of type can_msgs::Frame, default: "can0" + * @param[in] publish_topic ros topic to publish output messages of type can_msgs::Frame, default: "ros2can0" + * @param[in] pdo_rate rate of PDO (default: pdo_rate = 50, i.e. 20 ms between two PDOs or 50 PDOs per second) + * @param[in] pdo_repeat_cnt each sensor state spefied in sick_canopen_simu_cfg.xml is repeated N times before switching to the next state (default: 5 times) + * @param[in] subscribe_queue_size buffer size for ros messages + */ +sick_canopen_simu::OLSSimulator::OLSSimulator(ros::NodeHandle & nh, const std::string & config_file, const std::string & sick_device_family, int can_node_id, const std::string & subscribe_topic, const std::string & publish_topic, const ros::Rate & pdo_rate, int pdo_repeat_cnt, int subscribe_queue_size) + : SimulatorBase(nh, config_file, can_node_id, subscribe_topic, publish_topic, pdo_rate, pdo_repeat_cnt, subscribe_queue_size) +{ + m_ros_subscriber = nh.subscribe(subscribe_topic, m_subscribe_queue_size, &sick_canopen_simu::OLSSimulator::messageHandler, this); + if(!readPDOconfig(config_file, sick_device_family)) + { + ROS_ERROR_STREAM("OLSSimulator: readPDOconfig(" << config_file << ") failed"); + } + // Start thread for publishing PDOs + m_pdo_publisher_thread_running = true; + m_pdo_publisher_thread = new boost::thread(&sick_canopen_simu::OLSSimulator::runPDOthread, this); +} + +/* + * @brief Callbacks for ros messages. Converts incoming messages of type can_msgs::Frame to simulate an OLS device + * and publishes simulation results to the configured ros topic. + * + * param[in] msg ros message of type can_msgs::Frame + */ +void sick_canopen_simu::OLSSimulator::messageHandler(const can_msgs::Frame & msg_in) +{ + SimulatorBase::messageHandler(msg_in); +} + +/* + * @brief Parses an pdo element from config file and appends it to m_vec_pdo_measurements. + * + * param[in] xml_pdo pdo element from config file + */ +bool sick_canopen_simu::OLSSimulator::parseXmlPDO(TiXmlElement* xml_pdo) +{ + try + { + m_vec_pdo_measurements.push_back(sick_line_guidance::MsgUtil::convertOLSMessage( + std::stof(xml_pdo->Attribute("lcp1")), std::stof(xml_pdo->Attribute("lcp2")), std::stof(xml_pdo->Attribute("lcp3")), + std::stof(xml_pdo->Attribute("width1")), std::stof(xml_pdo->Attribute("width2")), std::stof(xml_pdo->Attribute("width3")), + std::stoul(xml_pdo->Attribute("status"), 0, 0), + std::stoul(xml_pdo->Attribute("barcode"), 0, 0), + std::stoul(xml_pdo->Attribute("devstatus"), 0, 0), + std::stoul(xml_pdo->Attribute("error"), 0, 0), + std::stof(xml_pdo->Attribute("barcodecenter")), // OLS20 only: simulate barcode center point, object 0x2021subA (INT16), OLS10: always 0 + std::stoul(xml_pdo->Attribute("linequality"), 0, 0), // OLS20 only: simulate quality of lines, object 0x2021subB (UINT8), OLS10: always 0 + std::stoul(xml_pdo->Attribute("lineintensity1"), 0, 0), // OLS20 only: simulate intensity of lines, object 0x2023sub1 (UINT8), OLS10: always 0 + std::stoul(xml_pdo->Attribute("lineintensity2"), 0, 0), // OLS20 only: simulate intensity of lines, object 0x2023sub2 (UINT8), OLS10: always 0 + std::stoul(xml_pdo->Attribute("lineintensity3"), 0, 0), // OLS20 only: simulate intensity of lines, object 0x2023sub3 (UINT8), OLS10: always 0 + xml_pdo->Attribute("frame_id"))); + } + catch(const std::exception & exc) + { + ROS_ERROR_STREAM("OLSSimulator::parseXmlPDO() failed: exception " << exc.what()); + } + return false; +} + +/* + * Subclass OLS10Simulator extends class OLSSimulator to simulate an OLS10 device. + * + */ + +/* + * Constructor + * + * @param[in] nh ros node handle + * @param[in] config_file configuration file with testcases for OLS and MLS simulation + * @param[in] sick_device_family "OLS10", "OLS20" or "MLS" + * @param[in] can_node_id node id of OLS or MLS, default: 0x0A + * @param[in] subscribe_topic ros topic to receive input messages of type can_msgs::Frame, default: "can0" + * @param[in] publish_topic ros topic to publish output messages of type can_msgs::Frame, default: "ros2can0" + * @param[in] pdo_rate rate of PDO (default: pdo_rate = 50, i.e. 20 ms between two PDOs or 50 PDOs per second) + * @param[in] pdo_repeat_cnt each sensor state spefied in sick_canopen_simu_cfg.xml is repeated N times before switching to the next state (default: 5 times) + * @param[in] subscribe_queue_size buffer size for ros messages + */ +sick_canopen_simu::OLS10Simulator::OLS10Simulator(ros::NodeHandle & nh, const std::string & config_file, const std::string & sick_device_family, int can_node_id, const std::string & subscribe_topic, const std::string & publish_topic, const ros::Rate & pdo_rate, int pdo_repeat_cnt, int subscribe_queue_size) + : OLSSimulator(nh, config_file, sick_device_family, can_node_id, subscribe_topic, publish_topic, pdo_rate, pdo_repeat_cnt, subscribe_queue_size) +{ + m_send_tpdo_immediately = true; // true (OLS10, MLS): send TPDOs immediately in all states (pre-operational and operational), false (default, OLS20): send TPDOs only in state operational + m_sdo_response_dev_state = 0x4B18200000000000; // response to sdo request for dev_status (object 0x2018): MLS and OLS20: 0x4F18200000000000 (sdo response with UINT8 data), OLS10: 0x4B18200000000000 (sdo response with UINT16 data) +} + +/* + * Subclass OLS20Simulator extends class OLSSimulator to simulate an OLS20 device. + * + */ + +/* + * Constructor + * + * @param[in] nh ros node handle + * @param[in] config_file configuration file with testcases for OLS and MLS simulation + * @param[in] sick_device_family "OLS10", "OLS20" or "MLS" + * @param[in] can_node_id node id of OLS or MLS, default: 0x0A + * @param[in] subscribe_topic ros topic to receive input messages of type can_msgs::Frame, default: "can0" + * @param[in] publish_topic ros topic to publish output messages of type can_msgs::Frame, default: "ros2can0" + * @param[in] pdo_rate rate of PDO (default: pdo_rate = 50, i.e. 20 ms between two PDOs or 50 PDOs per second) + * @param[in] pdo_repeat_cnt each sensor state spefied in sick_canopen_simu_cfg.xml is repeated N times before switching to the next state (default: 5 times) + * @param[in] subscribe_queue_size buffer size for ros messages + */ +sick_canopen_simu::OLS20Simulator::OLS20Simulator(ros::NodeHandle & nh, const std::string & config_file, const std::string & sick_device_family, int can_node_id, const std::string & subscribe_topic, const std::string & publish_topic, const ros::Rate & pdo_rate, int pdo_repeat_cnt, int subscribe_queue_size) + : OLSSimulator(nh, config_file, sick_device_family, can_node_id, subscribe_topic, publish_topic, pdo_rate, pdo_repeat_cnt, subscribe_queue_size) +{ + m_send_tpdo_immediately = false; // true (OLS10, MLS): send TPDOs immediately in all states (pre-operational and operational), false (default, OLS20): send TPDOs only in state operational +} + diff --git a/Devices/Packages/sick_line_guidance/src/sick_canopen_simu_node.cpp b/Devices/Packages/sick_line_guidance/src/sick_canopen_simu_node.cpp new file mode 100755 index 0000000..6c51ef3 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/src/sick_canopen_simu_node.cpp @@ -0,0 +1,148 @@ +/* + * sick_canopen_simu_node: subscribes to ros topic "can0" (message type can_msgs::Frame), + * simulates an OLS20 or MLS device and publishes can_msgs::Frame messages on ros topic "ros2can0" + * for further transmission to the can bus. + * + * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim + * Copyright (C) 2019 SICK AG, Waldkirch + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of SICK AG nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Authors: + * Michael Lehning + * + * Copyright 2019 SICK AG + * Copyright 2019 Ing.-Buero Dr. Michael Lehning + * + */ +#include +#include "sick_line_guidance/sick_line_guidance_version.h" +#include "sick_line_guidance/sick_canopen_simu_device.h" +#include "sick_line_guidance/sick_canopen_simu_verify.h" + +int main(int argc, char** argv) +{ + // Setup and configuration + ros::init(argc, argv, "sick_canopen_simu_node"); + ros::NodeHandle nh; + int can_node_id = 0x0A; // default node id for OLS and MLS + int can_message_queue_size = 16; // buffer size for ros messages + int sensor_state_queue_size = 2; // buffer size for simulated sensor states + double pdo_rate = 50; // rate of PDOs (default: pdo_rate = 50, i.e. 20 ms between two PDOs or 50 PDOs per second) + int pdo_repeat_cnt = 25; // each sensor state spefied in sick_canopen_simu_cfg.xml is repeated 25 times before switching to the next state (sensor state changes after 0.5 seconds) + std::string sick_device_family = "OLS20"; // simulation of OLS10, OLS20 or MLS device + std::string can_subscribe_topic = "can0", publish_topic = "ros2can0"; // default can topics + std::string mls_subscribe_topic = "mls", ols_subscribe_topic = "ols"; // default measurement topics + std::string sick_canopen_simu_cfg_file = "sick_canopen_simu_cfg.xml"; // configuration file and testcases for OLS and MLS simulation + nh.param("sick_canopen_simu_cfg_file", sick_canopen_simu_cfg_file, sick_canopen_simu_cfg_file); + nh.param("can_node_id", can_node_id, can_node_id); + nh.param("sick_device_family", sick_device_family, sick_device_family); + nh.param("can_subscribe_topic", can_subscribe_topic, can_subscribe_topic); + nh.param("mls_subscribe_topic", mls_subscribe_topic, mls_subscribe_topic); + nh.param("ols_subscribe_topic", ols_subscribe_topic, ols_subscribe_topic); + nh.param("publish_topic", publish_topic, publish_topic); + nh.param("pdo_rate", pdo_rate, pdo_rate); + nh.param("pdo_repeat_cnt", pdo_repeat_cnt, pdo_repeat_cnt); + nh.param("can_message_queue_size", can_message_queue_size, can_message_queue_size); + nh.param("sensor_state_queue_size", sensor_state_queue_size, sensor_state_queue_size); + + ROS_INFO_STREAM("sick_canopen_simu_node: version " << sick_line_guidance::Version::getVersionInfo()); + ROS_INFO_STREAM("sick_canopen_simu_node: initializing..."); + sick_canopen_simu::MLSMeasurementVerification* mls_measurement_verification = 0; + sick_canopen_simu::OLSMeasurementVerification* ols_measurement_verification = 0; + sick_canopen_simu::MLSSimulator* mls_simulator = 0; + sick_canopen_simu::OLSSimulator* ols_simulator = 0; + if(sick_device_family == "MLS") + { + // Init MLS simulation + mls_simulator = new sick_canopen_simu::MLSSimulator(nh, sick_canopen_simu_cfg_file, sick_device_family, can_node_id, can_subscribe_topic, publish_topic, ros::Rate(pdo_rate), pdo_repeat_cnt,can_message_queue_size); + ROS_INFO_STREAM("sick_canopen_simu_node: MLSSimulator started, can node_id " << can_node_id << ", listening to can topic \"" + << can_subscribe_topic << "\", measurement topic \"" << mls_subscribe_topic << "\", publishing on ros topic \"" << publish_topic << "\" ..."); + mls_measurement_verification = new sick_canopen_simu::MLSMeasurementVerification(nh, mls_subscribe_topic, sensor_state_queue_size, sick_device_family); + mls_simulator->registerSimulationListener(mls_measurement_verification); + } + else if(sick_device_family == "OLS10") + { + // Init OLS10 simulation + ols_simulator = new sick_canopen_simu::OLS10Simulator(nh, sick_canopen_simu_cfg_file, sick_device_family, can_node_id, can_subscribe_topic, publish_topic, ros::Rate(pdo_rate), pdo_repeat_cnt, can_message_queue_size); + ROS_INFO_STREAM("sick_canopen_simu_node: OLS10Simulator started, can node_id " << can_node_id << ", listening to can topic \"" + << can_subscribe_topic << "\", measurement topic \"" << ols_subscribe_topic << "\", publishing on ros topic \"" << publish_topic << "\" ..."); + ols_measurement_verification = new sick_canopen_simu::OLSMeasurementVerification(nh, ols_subscribe_topic, sensor_state_queue_size, sick_device_family); + ols_simulator->registerSimulationListener(ols_measurement_verification); + } + else if(sick_device_family == "OLS20") + { + // Init OLS20 simulation + ols_simulator = new sick_canopen_simu::OLS20Simulator(nh, sick_canopen_simu_cfg_file, sick_device_family, can_node_id, can_subscribe_topic, publish_topic, ros::Rate(pdo_rate), pdo_repeat_cnt, can_message_queue_size); + ROS_INFO_STREAM("sick_canopen_simu_node: OLS20Simulator started, can node_id " << can_node_id << ", listening to can topic \"" + << can_subscribe_topic << "\", measurement topic \"" << ols_subscribe_topic << "\", publishing on ros topic \"" << publish_topic << "\" ..."); + ols_measurement_verification = new sick_canopen_simu::OLSMeasurementVerification(nh, ols_subscribe_topic, sensor_state_queue_size, sick_device_family); + ols_simulator->registerSimulationListener(ols_measurement_verification); + } + else + { + ROS_ERROR_STREAM("sick_canopen_simu_node: sick_device_family \"" << sick_device_family << "\" not supported, aborting ..."); + return 1; + } + + // Run ros event loop + ROS_INFO_STREAM("sick_canopen_simu_node: running..."); + ros::spin(); + std::cout << "sick_canopen_simu_node: exiting..." << std::endl; + ROS_INFO_STREAM("sick_canopen_simu_node: exiting..."); + if(mls_simulator) + { + mls_simulator->unregisterSimulationListener(mls_measurement_verification); + mls_measurement_verification->printStatistic(); + delete(mls_measurement_verification); + delete(mls_simulator); + } + if(ols_simulator) + { + ols_simulator->unregisterSimulationListener(ols_measurement_verification); + ols_measurement_verification->printStatistic(); + delete(ols_measurement_verification); + delete(ols_simulator); + } + return 0; +} + diff --git a/Devices/Packages/sick_line_guidance/src/sick_canopen_simu_verify.cpp b/Devices/Packages/sick_line_guidance/src/sick_canopen_simu_verify.cpp new file mode 100755 index 0000000..6aa7740 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/src/sick_canopen_simu_verify.cpp @@ -0,0 +1,320 @@ +/* + * sick_canopen_simu_verify tests and verifies the sick_line_guidance ros driver. + * + * sick_canopen_simu_verify listenes to both the simulation (interface sick_canopen_simu::SimulatorBase::SimulationListener) + * and to the ros messages of the sick_line_guidance driver. Whenever a MLS_Measurement or OLS_Measurement message + * is received, the measurement is compared to the current sensor state of the simulation. + * Measurement messages from the driver and sensor states from the simulation should be identical, otherwise an error occured. + * The sick_line_guidance driver test is passed, if no error occured (i.e. all measurement messages from the driver have been + * handled correctly, no mismatches between simulated sensor states and published measurement messages). + * + * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim + * Copyright (C) 2019 SICK AG, Waldkirch + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of SICK AG nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Authors: + * Michael Lehning + * + * Copyright 2019 SICK AG + * Copyright 2019 Ing.-Buero Dr. Michael Lehning + * + */ +#include +#include "sick_line_guidance/sick_canopen_simu_verify.h" +#include "sick_line_guidance/sick_line_guidance_msg_util.h" +#include "sick_line_guidance/sick_canopen_simu_compare.h" + +/* + * Constructor + * + * @param[in] nh ros node handle + * @param[in] measurement_subscribe_topic ros topic for measurement messages, default: "mls" resp. "ols" + * @param[in] sensor_state_queue_size buffer size for simulated sensor states, default: 2 + * @param[in] devicename descriptional device name, f.e. "OLS20" or "MLS" + */ +template +sick_canopen_simu::MeasurementVerification::MeasurementVerification(ros::NodeHandle & nh, const std::string & measurement_subscribe_topic, int sensor_state_queue_size, const std::string & devicename) +{ + m_devicename = devicename; + m_sensor_states.resize(sensor_state_queue_size); + m_measurement_messages.resize(sensor_state_queue_size); + m_measurement_verification_error_cnt = 0; + m_measurement_verification_ignored_cnt = 0; + m_measurement_verification_failed = 0; + m_measurement_verification_jitter = 4; // max. 4 consecutive errors tolerated, since measurement messages can be sent while a SDO response is still pending (OLS20: up to 9 SDO queries required per TPDO measurement) + m_measurement_messages_cnt = -sensor_state_queue_size; // start verification after the first two measurements + for(typename std::list::iterator iter = m_sensor_states.begin(); iter != m_sensor_states.end(); iter++) + sick_line_guidance::MsgUtil::zero(*iter); + for (typename std::list::iterator iter = m_measurement_messages.begin(); iter != m_measurement_messages.end(); iter++) + sick_line_guidance::MsgUtil::zero(*iter); +} + +/* + * Destructor + * + */ +template +sick_canopen_simu::MeasurementVerification::~MeasurementVerification() +{ +} + +/* + * @brief Implements the notification callback for SimulationListener. Whenever the simulation sends a PDO, + * this function is called by the simulation to notify SimulationListeners about the current sensor state. + */ +template +void sick_canopen_simu::MeasurementVerification::pdoSent(const MsgType & sensor_state) +{ + ROS_INFO_STREAM("MeasurementVerification::pdoSent(" << sick_line_guidance::MsgUtil::toInfo(sensor_state) << ")"); + // push sensor_state to m_sensor_states (const list size, pop first element and push new sensor state at the back) + boost::lock_guard state_lockguard(m_sensor_states_mutex); + m_sensor_states.pop_front(); + m_sensor_states.push_back(sensor_state); +} + +/* + * @brief Prints the number of verified measuremente and the number of verification failures. + * @return true in case of no verification failures (all measurements verified successfully), false otherwise. + */ +template +bool sick_canopen_simu::MeasurementVerification::printStatistic(void) +{ + std::stringstream message; + if(m_measurement_messages_cnt > 0 && m_measurement_verification_error_cnt > 0) + { + message << m_devicename << " MeasurementVerificationStatistic failures: " << m_measurement_verification_error_cnt << " of " << m_measurement_messages_cnt << " measurements failed, " + << std::fixed << std::setprecision(2) << (m_measurement_verification_error_cnt*100.00/m_measurement_messages_cnt) << " % errors, " << m_measurement_verification_ignored_cnt << " measurements ignored."; + std::cerr << message.str() << std::endl; + ROS_ERROR_STREAM(message.str()); + } + else if(m_measurement_messages_cnt > 0) + { + message << m_devicename << " MeasurementVerificationStatistic okay: " << m_measurement_verification_failed << " of " << m_measurement_messages_cnt << " measurements failed, " << m_measurement_verification_ignored_cnt << " measurements ignored."; + std::cout << message.str() << std::endl; + ROS_INFO_STREAM(message.str()); + } + return (m_measurement_verification_error_cnt == 0); +} + +/* + * @brief Callback for measurement messages, called whenever the sick_line_guidance ros driver publishes a + * new measurement message. Compares the measurement message with the sensor states from simulation, + * and reports an error, if an equivalent sensor state hasn't been sent by the simulation. + * + * @param[in] measurement measurement message from sick_line_guidance ros driver + * + * @return true, if the measurement message is equivalent to sensor states (verification passed), or false otherwise (verification failed). + */ +template +bool sick_canopen_simu::MeasurementVerification::verifyMeasurement(const MsgType & measurement) +{ + ROS_INFO_STREAM(m_devicename << " MeasurementVerification::verifyMeasurement(" << sick_line_guidance::MsgUtil::toInfo(measurement) << ")"); + // push measurement to m_measurement_messages (const list size, pop first element and push new measurement at the back) + boost::lock_guard state_lockguard(m_sensor_states_mutex); + m_measurement_messages.pop_front(); + m_measurement_messages.push_back(measurement); + bool measurement_verified = true; + if(m_measurement_messages_cnt > 0) // start verification after 2 measurements + { + measurement_verified = verifyMeasurements(m_measurement_messages, m_sensor_states); + if(measurement_verified) + { + ROS_INFO_STREAM(m_devicename << " MeasurementVerification::verifyMeasurement(" << sick_line_guidance::MsgUtil::toInfo(measurement) << ") succeeded."); + m_measurement_verification_failed = 0; + } + else + { + std::stringstream errormsg; + errormsg << m_devicename << " MeasurementVerification::verifyMeasurement(" << sick_line_guidance::MsgUtil::toInfo(measurement) << ") failed."; + for (typename std::list::iterator iter_measurement = m_measurement_messages.begin(); iter_measurement != m_measurement_messages.end(); iter_measurement++) + errormsg << m_devicename << " MeasurementVerification: measurement " << sick_line_guidance::MsgUtil::toInfo(*iter_measurement); + for(typename std::list::iterator iter_state = m_sensor_states.begin(); iter_state != m_sensor_states.end(); iter_state++) + errormsg << m_devicename << " MeasurementVerification: sensor state " << sick_line_guidance::MsgUtil::toInfo(*iter_state); + m_measurement_verification_failed++; + if(m_measurement_verification_failed > m_measurement_verification_jitter) + { + m_measurement_verification_error_cnt++; // error: 2 consecutive failures (measurement message different to simulated sensor state) + ROS_ERROR_STREAM(errormsg.str()); + } + else + { + m_measurement_verification_ignored_cnt++; // possible error (max. 1 error tolerated, since measurement messages can be sent while a SDO response is still pending) + ROS_WARN_STREAM(errormsg.str()); + } + } + } + m_measurement_messages_cnt++; + return measurement_verified; +} + +/* + * @brief Compares and verifies MLS measurement messages from ros driver against sensor states from simulation. + * + * @param[in] measurement_messages list of measurement messages from ros driver (to be compared to sensor_states) + * @param[in] sensor_states list of sensor states from simulation (to be compared to measurement_messages) + * + * @return true, if the measurement messages are equivalent to sensor states (verification passed), or false otherwise (verification failed). + */ +template +bool sick_canopen_simu::MeasurementVerification::verifyMeasurements(std::list & measurement_messages, std::list & sensor_states) +{ + // If the sensor state changed between two PDOs, sensor data from PDOs (position and width) might represent the new state, while sensor data from SDOs (barcode) + // are still from the previous sensor state (SDO still pending or SDO response not yet received). Or vice versa, after receiving the PDOs, the simuulation + // switches to a new state, which is returned by the next SDO (but before the new state is published by a new PDO). In this case, measurement verification + // might fail exactly once when SDO based sensor data (barcodes) are switched right between two PDOs. To avoid verification problems when entries in the + // object dictionary are modified between two PDOs, we compare the current and the last sensor state. One of them always has to be verified, either the + // current or the previous state. This way, we tolerate a 10-milliseconds-jitter in our verification. + sick_canopen_simu::MeasurementComparator comparator; + return verifyMeasurementData(measurement_messages, sensor_states, comparator.cmpPosition) + && verifyMeasurementData(measurement_messages, sensor_states, comparator.cmpLcp) + && verifyMeasurementData(measurement_messages, sensor_states, comparator.cmpStatus) + && verifyMeasurementData(measurement_messages, sensor_states, comparator.cmpError); +} + +/* + * @brief Compares and verifies OLS measurement messages from ros driver against sensor states from simulation. + * + * @param[in] measurement_messages list of measurement messages from ros driver (to be compared to sensor_states) + * @param[in] sensor_states list of sensor states from simulation (to be compared to measurement_messages) + * + * @return true, if the measurement messages are equivalent to sensor states (verification passed), or false otherwise (verification failed). + */ +template +bool sick_canopen_simu::MeasurementVerification::verifyMeasurements(std::list & measurement_messages, std::list & sensor_states) +{ + // If the sensor state changed between two PDOs, sensor data from PDOs (position and width) might represent the new state, while sensor data from SDOs (barcode) + // are still from the previous sensor state (SDO still pending or SDO response not yet received). Or vice versa, after receiving the PDOs, the simuulation + // switches to a new state, which is returned by the next SDO (but before the new state is published by a new PDO). In this case, measurement verification + // might fail exactly once when SDO based sensor data (barcodes) are switched right between two PDOs. To avoid verification problems when entries in the + // object dictionary are modified between two PDOs, we compare the current and the last sensor state. One of them always has to be verified, either the + // current or the previous state. This way, we tolerate a 10-milliseconds-jitter in our verification. + sick_canopen_simu::MeasurementComparator comparator; + return verifyMeasurementData(measurement_messages, sensor_states, comparator.cmpPosition) + && verifyMeasurementData(measurement_messages, sensor_states, comparator.cmpLinewidth) + && verifyMeasurementData(measurement_messages, sensor_states, comparator.cmpStatus) + && verifyMeasurementData(measurement_messages, sensor_states, comparator.cmpBarcode) + && verifyMeasurementData(measurement_messages, sensor_states, comparator.cmpDevStatus) + && verifyMeasurementData(measurement_messages, sensor_states, comparator.cmpExtendedCode) + && verifyMeasurementData(measurement_messages, sensor_states, comparator.cmpError) + && verifyMeasurementData(measurement_messages, sensor_states, comparator.cmpBarcodeCenter) + && verifyMeasurementData(measurement_messages, sensor_states, comparator.cmpLineQuality) + && verifyMeasurementData(measurement_messages, sensor_states, comparator.cmpLineIntensity); +} + +/* + * @brief Compares measurement messages from ros driver against sensor states from simulation, using a specified compare function + * (f.e. floating point comparsion for positions with fabs(x-y) < 1 mm and integer comparsion with x == y for barcodes). + * + * @param[in] measurement_messages list of measurement messages from ros driver (to be compared to sensor_states) + * @param[in] sensor_states list of sensor states from simulation (to be compared to measurement_messages) + * @param[in] cmpfunction compare function, called to compare measurement message A to sensor state B + * + * @return true, if the measurement messages are equivalent to sensor states (verification passed), or false otherwise (verification failed). +*/ +template template +bool sick_canopen_simu::MeasurementVerification::verifyMeasurementData(std::list & measurement_messages, std::list & sensor_states, bool(*cmpfunction)(const T & A, const T & B)) +{ + for (typename std::list::iterator iter_measurement = measurement_messages.begin(); iter_measurement != measurement_messages.end(); iter_measurement++) + { + for (typename std::list::iterator iter_state = sensor_states.begin(); iter_state != sensor_states.end(); iter_state++) + { + if (cmpfunction(*iter_state, *iter_measurement)) + { + return true; + } + } + } + return false; +} + +/* + * Constructor. Subclass MLSMeasurementVerification extends class MeasurementVerification to verify MLS_Measurement messages. + * + * @param[in] nh ros node handle + * @param[in] measurement_subscribe_topic ros topic for measurement messages, default: "mls" + * @param[in] sensor_state_queue_size buffer size for simulated sensor states, default: 2 + * @param[in] devicename descriptional device name, f.e. "MLS" + */ +sick_canopen_simu::MLSMeasurementVerification::MLSMeasurementVerification(ros::NodeHandle & nh, const std::string & measurement_subscribe_topic, int sensor_state_queue_size, const std::string & devicename) + : sick_canopen_simu::MeasurementVerification::MeasurementVerification(nh, measurement_subscribe_topic, sensor_state_queue_size, devicename) +{ + m_measurement_subscriber = nh.subscribe(measurement_subscribe_topic, sensor_state_queue_size, &sick_canopen_simu::MLSMeasurementVerification::measurementCb, this); +} + +/* + * @brief Callback for measurement messages, called whenever the sick_line_guidance ros driver publishes a + * new measurement message. Compares the measurement message with the sensor states from simulation, + * and reports an error, if an equivalent sensor state hasn't been sent by the simulation. + * + * @param[in] measurement measurement message from sick_line_guidance ros driver + */ +void sick_canopen_simu::MLSMeasurementVerification::measurementCb(const sick_line_guidance::MLS_Measurement & measurement) +{ + verifyMeasurement(measurement); +} + +/* + * Constructor. Subclass OLSMeasurementVerification extends class MeasurementVerification to verify OLS_Measurement messages. + * + * @param[in] nh ros node handle + * @param[in] measurement_subscribe_topic ros topic for measurement messages, default: "ols" + * @param[in] sensor_state_queue_size buffer size for simulated sensor states, default: 2 + * @param[in] devicename descriptional device name, f.e. "OLS20" + */ +sick_canopen_simu::OLSMeasurementVerification::OLSMeasurementVerification(ros::NodeHandle & nh, const std::string & measurement_subscribe_topic, int sensor_state_queue_size, const std::string & devicename) + : sick_canopen_simu::MeasurementVerification::MeasurementVerification(nh, measurement_subscribe_topic, sensor_state_queue_size, devicename) +{ + m_measurement_subscriber = nh.subscribe(measurement_subscribe_topic, sensor_state_queue_size, &sick_canopen_simu::OLSMeasurementVerification::measurementCb, this); +} + +/* + * @brief Callback for measurement messages, called whenever the sick_line_guidance ros driver publishes a + * new measurement message. Compares the measurement message with the sensor states from simulation, + * and reports an error, if an equivalent sensor state hasn't been sent by the simulation. + * + * @param[in] measurement measurement message from sick_line_guidance ros driver + */ +void sick_canopen_simu::OLSMeasurementVerification::measurementCb(const sick_line_guidance::OLS_Measurement & measurement) +{ + verifyMeasurement(measurement); +} diff --git a/Devices/Packages/sick_line_guidance/src/sick_line_guidance_can2ros_node.cpp b/Devices/Packages/sick_line_guidance/src/sick_line_guidance_can2ros_node.cpp new file mode 100755 index 0000000..3bd8d17 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/src/sick_line_guidance_can2ros_node.cpp @@ -0,0 +1,242 @@ +/* + * sick_line_guidance_can2ros_node: implements a ros node, which publishes all can messages. + * A simple candump to ros messages. + * + * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim + * Copyright (C) 2019 SICK AG, Waldkirch + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of SICK AG nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Authors: + * Michael Lehning + * + * Copyright 2019 SICK AG + * Copyright 2019 Ing.-Buero Dr. Michael Lehning + * + */ +#include +#include +#include +#include +#include +#include "sick_line_guidance/sick_line_guidance_version.h" + +namespace sick_line_guidance +{ + /* + * class SocketCANListener: implements a simple listener to a SocketCANInterface. + */ + class SocketCANListener + { + public: + + /* + * Constructor + */ + SocketCANListener() : m_socketcan_interface(0), m_socketcan_thread(0), m_socketcan_running(false), m_socketcan_state_listener(0), m_socketcan_frame_listener(0), m_filter_sync(true) + { + } + + /* + * Register state and frame listener to the SocketCANInterface and create ros publisher. + * param[in] nh ros handle + * param[in] topic topic for ros messages, message type can_msgs::Frame + * param[in] p_socketcan_interface pointer to SocketCANInterface + * param[in] filter_sync if true (default), can sync messages are not published + * @return true on success, otherwise false; + */ + bool init(ros::NodeHandle & nh, const std::string & topic, can::DriverInterfaceSharedPtr p_socketcan_interface, bool filter_sync = true) + { + m_socketcan_interface = p_socketcan_interface; + m_filter_sync = filter_sync; + m_ros_publisher = nh.advertise(topic, 1); + m_socketcan_state_listener = m_socketcan_interface->createStateListener(can::StateInterface::StateDelegate(this, &SocketCANListener::socketcanStateCallback)); + m_socketcan_frame_listener = m_socketcan_interface->createMsgListener(can::CommInterface::FrameDelegate(this, &SocketCANListener::socketcanFrameCallback)); + return m_socketcan_state_listener != 0 && m_socketcan_frame_listener != 0; + } + + /* + * @brief create a thread and run can::SocketCANInterface::run() in a background task + */ + void start() + { + m_socketcan_running = true; + m_socketcan_thread = new boost::thread(&sick_line_guidance::SocketCANListener::run, this); + } + + /* + * @brief shuts down can::SocketCANInterface and stops the thread running can::SocketCANInterface::run() + */ + void stop() + { + m_socketcan_running = false; + if(m_socketcan_thread) + { + m_socketcan_interface->shutdown(); + m_socketcan_thread->join(); + delete(m_socketcan_thread); + } + m_socketcan_thread = 0; + } + + protected: + + /* + * member variables and functions + */ + + can::DriverInterfaceSharedPtr m_socketcan_interface; // can::SocketCANInterface instance + boost::thread* m_socketcan_thread; // thread to run can::SocketCANInterface in background + bool m_socketcan_running; // flag indicating start and stop of m_socketcan_thread + can::FrameListenerConstSharedPtr m_socketcan_frame_listener; // can frame listener + can::StateListenerConstSharedPtr m_socketcan_state_listener; // can state listener + ros::Publisher m_ros_publisher; // publishes a ros message for each received can frame + bool m_filter_sync; // if true (default), can sync messages are not published + + /* + * Callback for can state messages, called by SocketCANInterface. + * param[in] canstate can state message + */ + void socketcanStateCallback(const can::State & canstate) + { + // ROS_DEBUG_STREAM("sick_line_guidance::SocketCANListener::socketcanStateCallback(): can state message: " << canstate.driver_state << " (error code: " << canstate.error_code << ")"); + if(canstate.error_code) + ROS_ERROR_STREAM("sick_line_guidance::SocketCANListener::socketcanStateCallback(): can error code: " << canstate.error_code); + } + + /* + * Callback for can frame messages, called by SocketCANInterface. + * param[in] canframe can frame message + */ + void socketcanFrameCallback(const can::Frame & canframe) + { + if(canframe.isValid()) + { + if(m_filter_sync && canframe.id == 0x80) // can sync message + { + ROS_DEBUG_STREAM("sick_line_guidance::SocketCANListener::socketcanFrameCallback(): can sync message: " << can::tostring(canframe, false)); + return; + } + ROS_DEBUG_STREAM("sick_line_guidance::SocketCANListener::socketcanFrameCallback(): can frame message: " << can::tostring(canframe, false)); + } + else + { + ROS_ERROR_STREAM("sick_line_guidance::SocketCANListener::socketcanFrameCallback(): invalid can frame message: " << can::tostring(canframe, false)); + } + can_msgs::Frame canframe_msg; + canframe_msg.id = canframe.id; + canframe_msg.dlc = std::min(static_cast(canframe_msg.data.size()), canframe.dlc); + canframe_msg.is_error = canframe.is_error; + canframe_msg.is_rtr = canframe.is_rtr; + canframe_msg.is_extended = canframe.is_extended; + canframe_msg.data.assign(0); + for(size_t n = 0, n_max = std::min(canframe_msg.data.size(),canframe.data.size()); n < n_max; n++) + canframe_msg.data[n] = canframe.data[n]; + canframe_msg.header.stamp = ros::Time::now(); + m_ros_publisher.publish(canframe_msg); + } + + /* + * @brief runs can::SocketCANInterface::run() in an endless loop + */ + void run() + { + while(m_socketcan_running && ros::ok()) + { + m_socketcan_interface->run(); + } + } + + }; // class SocketCANListener +} // namespace sick_line_guidance + +/* + * sick_line_guidance_can2ros_node: implements a ros node, which publishes all can messages. + * A simple candump to ros messages. + */ +int main(int argc, char** argv) +{ + // Setup and configuration + ros::init(argc, argv, "sick_line_guidance_can2ros_node"); + ros::NodeHandle nh; + std::string can_device = "can0", ros_topic = "can0"; + nh.param("can_device", can_device, can_device); // name of can net device (socketcan interface) + nh.param("ros_topic", ros_topic, ros_topic); // topic for ros messages, message type can_msgs::Frame + ROS_INFO_STREAM("sick_line_guidance_can2ros_node: version " << sick_line_guidance::Version::getVersionInfo()); + ROS_INFO_STREAM("sick_line_guidance_can2ros_node: starting..."); + + // Create the SocketCANInterface + sick_line_guidance::SocketCANListener socketcan_listener; + can::DriverInterfaceSharedPtr p_socketcan_interface = 0; + canopen::GuardedClassLoader driver_loader("socketcan_interface", "can::DriverInterface"); + try + { + ROS_INFO("sick_line_guidance_can2ros_node: initializing SocketCANInterface..."); + p_socketcan_interface = driver_loader.createInstance("can::SocketCANInterface"); + if(!p_socketcan_interface->init(can_device, false, can::NoSettings::create())) + ROS_ERROR("sick_line_guidance_can2ros_node: SocketCANInterface::init() failed."); + ROS_INFO("sick_line_guidance_can2ros_node: initializing socketcan listener ..."); + if(!socketcan_listener.init(nh, ros_topic, p_socketcan_interface, true)) + ROS_ERROR("sick_line_guidance_can2ros_node: SocketCANListener::registerListener() failed."); + socketcan_listener.start(); + } + catch(pluginlib::PluginlibException& ex) + { + ROS_ERROR_STREAM("sick_line_guidance_can2ros_node: createInstance(\"can::SocketCANInterface\") failed: " << ex.what() << ", exit with error"); + return 1; + } + ROS_INFO_STREAM("sick_line_guidance_can2ros_node: SocketCANInterface created, state = " << p_socketcan_interface->getState().driver_state); + + // Run interface event loop + // while(ros::ok()) + // p_socketcan_interface->run(); + + // Run ros event loop + ros::spin(); + + std::cout << "sick_line_guidance_can2ros_node: exiting..." << std::endl; + ROS_INFO("sick_line_guidance_can2ros_node: exiting..."); + socketcan_listener.stop(); + p_socketcan_interface = 0; + return 0; +} + diff --git a/Devices/Packages/sick_line_guidance/src/sick_line_guidance_can_chain_node.cpp b/Devices/Packages/sick_line_guidance/src/sick_line_guidance_can_chain_node.cpp new file mode 100755 index 0000000..e06b513 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/src/sick_line_guidance_can_chain_node.cpp @@ -0,0 +1,94 @@ +/* + * sick_line_guidance_can_chain_node wraps CanopenChain for sick_line_guidance ros driver. + * + * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim + * Copyright (C) 2019 SICK AG, Waldkirch + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of SICK AG nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Authors: + * Michael Lehning + * + * Copyright 2019 SICK AG + * Copyright 2019 Ing.-Buero Dr. Michael Lehning + * + */ +#include +#include +#include +#include +#include "sick_line_guidance/sick_line_guidance_version.h" +#include "sick_line_guidance/sick_line_guidance_canopen_chain.h" +#include "sick_line_guidance/sick_line_guidance_diagnostic.h" + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "sick_line_guidance_can_chain_node"); + // log4cxx::LoggerPtr node_logger = log4cxx::Logger::getLogger(ROSCONSOLE_DEFAULT_NAME); + // node_logger->setLevel(ros::console::g_level_lookup[ros::console::levels::Debug]); + // console_bridge::setLogLevel(console_bridge::CONSOLE_BRIDGE_LOG_DEBUG); + + ros::NodeHandle nh; + ros::NodeHandle nh_priv("~"); + ROS_INFO_STREAM("sick_line_guidance_can_chain_node: version " << sick_line_guidance::Version::getVersionInfo()); + + // Start canopen_chain_node + std::string diagnostic_topic = "diagnostics"; + nh.param("/sick_line_guidance_can_chain_node/diagnostic_topic", diagnostic_topic, diagnostic_topic); + sick_line_guidance::Diagnostic::init(nh, diagnostic_topic, "sick_line_guidance_can_chain_node"); + sick_line_guidance::CanopenChain canopen_chain(nh, nh_priv); + + ROS_INFO_STREAM("sick_line_guidance_can_chain_node: canopen setup..."); + if(!canopen_chain.setup()) + { + ROS_ERROR_STREAM("sick_line_guidance_can_chain_node: CanopenChain::setup() failed."); + sick_line_guidance::Diagnostic::update(sick_line_guidance::DIAGNOSTIC_STATUS::INITIALIZATION_ERROR, "CanopenChain::setup() failed"); + return 1; + } + ROS_INFO_STREAM("sick_line_guidance_can_chain_node: canopen setup successfull."); + sick_line_guidance::Diagnostic::update(sick_line_guidance::DIAGNOSTIC_STATUS::OK); + + // Run ros event loop + ros::spin(); + sick_line_guidance::Diagnostic::update(sick_line_guidance::DIAGNOSTIC_STATUS::EXIT); + return 0; +} diff --git a/Devices/Packages/sick_line_guidance/src/sick_line_guidance_can_cia401_subscriber.cpp b/Devices/Packages/sick_line_guidance/src/sick_line_guidance_can_cia401_subscriber.cpp new file mode 100755 index 0000000..315aeaf --- /dev/null +++ b/Devices/Packages/sick_line_guidance/src/sick_line_guidance_can_cia401_subscriber.cpp @@ -0,0 +1,194 @@ +/* + * class CanCiA401Subscriber implements the ros subscriber to canopen ols messages (example CiA401 device for testing). + * + * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim + * Copyright (C) 2019 SICK AG, Waldkirch + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of SICK AG nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Authors: + * Michael Lehning + * + * Copyright 2019 SICK AG + * Copyright 2019 Ing.-Buero Dr. Michael Lehning + * + */ +#include +#include +#include "sick_line_guidance/sick_line_guidance_can_cia401_subscriber.h" + +/* + * Constructor. + * @param[in] nh ros::NodeHandle + * @param[in] can_nodeid can id for canopen_chain_node, f.e. "node1" + * @param[in] max_num_retries_after_sdo_error After SDO error, the query is repeated max. N times (default: N=2). If the SDO error persists, the can driver is shutdown and restarted. + * @param[in] max_sdo_rate max. sdo query and publish rate + * @param[in] ros_topic topic for ros messages, f.e. "mls" or "ols" + * @param[in] ros_frameid frameid for ros messages, f.e. "mls_measurement_frame" or "ols_measurement_frame" + * @param[in] initial_sensor_state initial sensor state (f.e. 0x07 for 3 detected lines, or (1 << 4) to indicate sensor error) + * @param[in] subscribe_queue_size buffer size for ros messages + */ +sick_line_guidance::CanCiA401Subscriber::CanCiA401Subscriber(ros::NodeHandle & nh, const std::string & can_nodeid, int max_num_retries_after_sdo_error, double max_sdo_rate, + const std::string & ros_topic, const std::string & ros_frameid, int initial_sensor_state, int subscribe_queue_size) + : sick_line_guidance::CanOlsSubscriber::CanOlsSubscriber(nh, can_nodeid, max_num_retries_after_sdo_error, max_sdo_rate, ros_topic, ros_frameid, initial_sensor_state, subscribe_queue_size) +{ +} + +/* + * Destructor. + */ +sick_line_guidance::CanCiA401Subscriber::~CanCiA401Subscriber() +{ +} + +/* + * @brief subsribes to canopen topics for ols messages and sets the callbacks to handle messages from canopen_chain_node + * after PDO messages (LCP = line center point): + * + * Mapping for OLS: + * + * canopenchain-OLS -> CanSubscriber-callback -> OLS sensor state + * ----------------------------------------------------------------------- + * +"_1001" -> cancallbackError(UINT8) -> Error register + * +"_2018" -> cancallbackDevState(UINT8) -> Device state + * +"_2021sub1" -> cancallbackLCP1(INT16) -> LCP1 + * +"_2021sub2" -> cancallbackLCP2(INT16) -> LCP2 + * +"_2021sub3" -> cancallbackLCP3(INT16) -> LCP3 + * +"_2021sub4" -> cancallbackState(UINT8) -> State + * +"_2021sub5" -> cancallbackWidthLCP1(INT16) -> Width LCP1 + * +"_2021sub6" -> cancallbackWidthLCP2(INT16) -> Width LCP2 + * +"_2021sub7" -> cancallbackWidthLCP3(INT16) -> Width LCP3 + * +"_2021sub8" -> cancallbackCode(UINT8) -> Code + * +"_2021sub9" -> cancallbackExtCode(UINT32) -> Extended Code + * + * Mapping for MLS: + * + * canopenchain-MLS -> CanSubscriber-callback -> MLS sensor state + * ----------------------------------------------------------------------- + * +"_1001" -> cancallbackError(UINT8) -> Error register + * +"_2021sub1" -> cancallbackLCP1(INT16) -> LCP1 + * +"_2021sub2" -> cancallbackLCP2(INT16) -> LCP2 + * +"_2021sub3" -> cancallbackLCP3(INT16) -> LCP3 + * +"_2021sub4" -> cancallbackMarker(UINT8) -> #LCP and marker + * +"_2022" -> cancallbackState(UINT8) -> State + * + * Mapping for CiA402 (example, testing only): + * + * canopenchain-CiA402 -> CanSubscriber-callback -> CiA402 state + * --------------------------------------------------------------------- + * +"_6000sub1" -> cancallbackError(UINT8) -> Error register + * +"_6000sub2" -> cancallbackState(UINT8) -> State + * +"_6000sub3" -> cancallbackCode(UINT8) -> Code + * +"_6000sub4" -> cancallbackExtCode(UINT8) -> LSB Extended Code + * +"_6401sub1" -> cancallbackLCP1(INT16) -> LCP1 + * +"_6401sub2" -> cancallbackLCP2(INT16) -> LCP2 + * +"_6401sub3" -> cancallbackLCP3(INT16) -> LCP3 + * +"_6401sub4" -> cancallbackWidthLCP1(INT16) -> Width LCP1 + * +"_6401sub5" -> cancallbackWidthLCP2(INT16) -> Width LCP2 + * +"_6401sub6" -> cancallbackWidthLCP3(INT16) -> Width LCP3 + * + * See operation manuals for details (file SICK-OLS-Operating_instructions_OLS10_de_IM0076955.PDF for OLS + * and file SICK-MLS-Operating_instructions_MLS_de_IM0076567.PDF for MLS) + * + * @return true on success, otherwise false. + */ +bool sick_line_guidance::CanCiA401Subscriber::subscribeCanTopics(void) +{ + m_can_subscriber.push_back(m_measurement.m_nh.subscribe(m_measurement.m_can_nodeid + "_6000sub1", m_subscribe_queue_size, &sick_line_guidance::CanCiA401Subscriber::cancallbackError, this)); + m_can_subscriber.push_back(m_measurement.m_nh.subscribe(m_measurement.m_can_nodeid + "_6401sub1", m_subscribe_queue_size, &sick_line_guidance::CanCiA401Subscriber::cancallbackLCP1, this)); + m_can_subscriber.push_back(m_measurement.m_nh.subscribe(m_measurement.m_can_nodeid + "_6401sub2", m_subscribe_queue_size, &sick_line_guidance::CanCiA401Subscriber::cancallbackLCP2, this)); + m_can_subscriber.push_back(m_measurement.m_nh.subscribe(m_measurement.m_can_nodeid + "_6401sub3", m_subscribe_queue_size, &sick_line_guidance::CanCiA401Subscriber::cancallbackLCP3, this)); + m_can_subscriber.push_back(m_measurement.m_nh.subscribe(m_measurement.m_can_nodeid + "_6000sub2", m_subscribe_queue_size, &sick_line_guidance::CanCiA401Subscriber::cancallbackState, this)); + m_can_subscriber.push_back(m_measurement.m_nh.subscribe(m_measurement.m_can_nodeid + "_6401sub4", m_subscribe_queue_size, &sick_line_guidance::CanCiA401Subscriber::cancallbackWidthLCP1, this)); + m_can_subscriber.push_back(m_measurement.m_nh.subscribe(m_measurement.m_can_nodeid + "_6401sub5", m_subscribe_queue_size, &sick_line_guidance::CanCiA401Subscriber::cancallbackWidthLCP2, this)); + m_can_subscriber.push_back(m_measurement.m_nh.subscribe(m_measurement.m_can_nodeid + "_6401sub6", m_subscribe_queue_size, &sick_line_guidance::CanCiA401Subscriber::cancallbackWidthLCP3, this)); + m_can_subscriber.push_back(m_measurement.m_nh.subscribe(m_measurement.m_can_nodeid + "_6000sub3", m_subscribe_queue_size, &sick_line_guidance::CanCiA401Subscriber::cancallbackCode, this)); + return true; +} + +/* + * Callbacks for ros messages from canopen_chain_node. There's one callback for each ros topic + * published by canopen_chain_node. Each callback updates the sensor state and publishes an + * OLS-Measurement message. + */ + +void sick_line_guidance::CanCiA401Subscriber::cancallbackError(const boost::shared_ptr& msg) +{ + // Note: Object 0x1001 (error register) is NOT PDO mapped -> Query 0x1001 ((error register) in case of error flag in m_ols_state.status + // In CanCiA401Subscriber we simulate error status by PDO mapped object 6000sub1 + boost::lock_guard publish_lockguard(m_measurement.m_measurement_mutex); + m_measurement.m_ols_state.error = convertIntegerMessage(msg, m_measurement.m_ols_state.error, 0xFF, "OLS/error"); + publishOLSMeasurement(); +} +void sick_line_guidance::CanCiA401Subscriber::cancallbackLCP1(const boost::shared_ptr& msg) +{ + sick_line_guidance::CanOlsSubscriber::cancallbackLCP1(msg); +} +void sick_line_guidance::CanCiA401Subscriber::cancallbackLCP2(const boost::shared_ptr& msg) +{ + sick_line_guidance::CanOlsSubscriber::cancallbackLCP2(msg); +} +void sick_line_guidance::CanCiA401Subscriber::cancallbackLCP3(const boost::shared_ptr& msg) +{ + sick_line_guidance::CanOlsSubscriber::cancallbackLCP3(msg); +} +void sick_line_guidance::CanCiA401Subscriber::cancallbackState(const boost::shared_ptr& msg) +{ + sick_line_guidance::CanOlsSubscriber::cancallbackState(msg); +} +void sick_line_guidance::CanCiA401Subscriber::cancallbackWidthLCP1(const boost::shared_ptr& msg) +{ + sick_line_guidance::CanOlsSubscriber::cancallbackWidthLCP1(msg); +} +void sick_line_guidance::CanCiA401Subscriber::cancallbackWidthLCP2(const boost::shared_ptr& msg) +{ + sick_line_guidance::CanOlsSubscriber::cancallbackWidthLCP2(msg); +} +void sick_line_guidance::CanCiA401Subscriber::cancallbackWidthLCP3(const boost::shared_ptr& msg) +{ + sick_line_guidance::CanOlsSubscriber::cancallbackWidthLCP3(msg); +} +void sick_line_guidance::CanCiA401Subscriber::cancallbackCode(const boost::shared_ptr& msg) +{ + sick_line_guidance::CanOlsSubscriber::cancallbackCode(msg); +} + diff --git a/Devices/Packages/sick_line_guidance/src/sick_line_guidance_can_mls_subscriber.cpp b/Devices/Packages/sick_line_guidance/src/sick_line_guidance_can_mls_subscriber.cpp new file mode 100755 index 0000000..e8de6ff --- /dev/null +++ b/Devices/Packages/sick_line_guidance/src/sick_line_guidance_can_mls_subscriber.cpp @@ -0,0 +1,200 @@ +/* + * class CanMlsSubscriber implements the ros subscriber to canopen mls messages. + * + * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim + * Copyright (C) 2019 SICK AG, Waldkirch + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of SICK AG nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Authors: + * Michael Lehning + * + * Copyright 2019 SICK AG + * Copyright 2019 Ing.-Buero Dr. Michael Lehning + * + */ +#include +#include +#include "sick_line_guidance/sick_line_guidance_can_mls_subscriber.h" + +/* + * Constructor. + * @param[in] nh ros::NodeHandle + * @param[in] can_nodeid can id for canopen_chain_node, f.e. "node1" + * @param[in] max_num_retries_after_sdo_error After SDO error, the query is repeated max. N times (default: N=2). If the SDO error persists, the can driver is shutdown and restarted. + * @param[in] max_sdo_rate max. sdo query and publish rate + * @param[in] ros_topic topic for ros messages, f.e. "mls" or "ols" + * @param[in] ros_frameid frameid for ros messages, f.e. "mls_measurement_frame" or "ols_measurement_frame" + * @param[in] initial_sensor_state initial sensor state (f.e. 0x07 for 3 detected lines, or (1 << 4) to indicate sensor error) + * @param[in] subscribe_queue_size buffer size for ros messages + */ +sick_line_guidance::CanMlsSubscriber::CanMlsSubscriber(ros::NodeHandle & nh, const std::string & can_nodeid, int max_num_retries_after_sdo_error, double max_sdo_rate, + const std::string & ros_topic, const std::string & ros_frameid, int initial_sensor_state, int subscribe_queue_size) + : sick_line_guidance::CanSubscriber::CanSubscriber(nh, can_nodeid, max_num_retries_after_sdo_error, max_sdo_rate, initial_sensor_state, subscribe_queue_size) +{ + m_measurement.m_mls_state.header.frame_id = ros_frameid; + m_measurement.m_ros_publisher = nh.advertise(ros_topic, 1); +} + +/* + * Destructor. + */ +sick_line_guidance::CanMlsSubscriber::~CanMlsSubscriber() +{ +} + +/* + * @brief subsribes to canopen topics for mls messages and sets the callbacks to handle messages from canopen_chain_node + * after PDO messages (LCP = line center point): + * + * Mapping for OLS: + * + * canopenchain-OLS -> CanSubscriber-callback -> OLS sensor state + * ----------------------------------------------------------------------- + * +"_1001" -> cancallbackError(UINT8) -> Error register + * +"_2018" -> cancallbackDevState(UINT8) -> Device state + * +"_2021sub1" -> cancallbackLCP1(INT16) -> LCP1 + * +"_2021sub2" -> cancallbackLCP2(INT16) -> LCP2 + * +"_2021sub3" -> cancallbackLCP3(INT16) -> LCP3 + * +"_2021sub4" -> cancallbackState(UINT8) -> State + * +"_2021sub5" -> cancallbackWidthLCP1(INT16) -> Width LCP1 + * +"_2021sub6" -> cancallbackWidthLCP2(INT16) -> Width LCP2 + * +"_2021sub7" -> cancallbackWidthLCP3(INT16) -> Width LCP3 + * +"_2021sub8" -> cancallbackCode(UINT8) -> Code + * +"_2021sub9" -> cancallbackExtCode(UINT32) -> Extended Code + * + * Mapping for MLS: + * + * canopenchain-MLS -> CanSubscriber-callback -> MLS sensor state + * ----------------------------------------------------------------------- + * +"_1001" -> cancallbackError(UINT8) -> Error register + * +"_2021sub1" -> cancallbackLCP1(INT16) -> LCP1 + * +"_2021sub2" -> cancallbackLCP2(INT16) -> LCP2 + * +"_2021sub3" -> cancallbackLCP3(INT16) -> LCP3 + * +"_2021sub4" -> cancallbackMarker(UINT8) -> #LCP and marker + * +"_2022" -> cancallbackState(UINT8) -> State + * + * Mapping for CiA402 (example, testing only): + * + * canopenchain-CiA402 -> CanSubscriber-callback -> CiA402 state + * --------------------------------------------------------------------- + * +"_6000sub1" -> cancallbackError(UINT8) -> Error register + * +"_6000sub2" -> cancallbackState(UINT8) -> State + * +"_6000sub3" -> cancallbackCode(UINT8) -> Code + * +"_6000sub4" -> cancallbackExtCode(UINT8) -> LSB Extended Code + * +"_6401sub1" -> cancallbackLCP1(INT16) -> LCP1 + * +"_6401sub2" -> cancallbackLCP2(INT16) -> LCP2 + * +"_6401sub3" -> cancallbackLCP3(INT16) -> LCP3 + * +"_6401sub4" -> cancallbackWidthLCP1(INT16) -> Width LCP1 + * +"_6401sub5" -> cancallbackWidthLCP2(INT16) -> Width LCP2 + * +"_6401sub6" -> cancallbackWidthLCP3(INT16) -> Width LCP3 + * + * See operation manuals for details (file SICK-OLS-Operating_instructions_OLS10_de_IM0076955.PDF for OLS + * and file SICK-MLS-Operating_instructions_MLS_de_IM0076567.PDF for MLS) + * + * @return true on success, otherwise false. + */ +bool sick_line_guidance::CanMlsSubscriber::subscribeCanTopics(void) +{ + // m_can_subscriber.push_back(m_measurement.m_nh.subscribe(m_measurement.m_can_nodeid + "_1001", m_subscribe_queue_size, &sick_line_guidance::CanMlsSubscriber::cancallbackError, this)); + m_can_subscriber.push_back(m_measurement.m_nh.subscribe(m_measurement.m_can_nodeid + "_2021sub1", m_subscribe_queue_size, &sick_line_guidance::CanMlsSubscriber::cancallbackLCP1, this)); + m_can_subscriber.push_back(m_measurement.m_nh.subscribe(m_measurement.m_can_nodeid + "_2021sub2", m_subscribe_queue_size, &sick_line_guidance::CanMlsSubscriber::cancallbackLCP2, this)); + m_can_subscriber.push_back(m_measurement.m_nh.subscribe(m_measurement.m_can_nodeid + "_2021sub3", m_subscribe_queue_size, &sick_line_guidance::CanMlsSubscriber::cancallbackLCP3, this)); + m_can_subscriber.push_back(m_measurement.m_nh.subscribe(m_measurement.m_can_nodeid + "_2021sub4", m_subscribe_queue_size, &sick_line_guidance::CanMlsSubscriber::cancallbackMarker, this)); + m_can_subscriber.push_back(m_measurement.m_nh.subscribe(m_measurement.m_can_nodeid + "_2022", m_subscribe_queue_size, &sick_line_guidance::CanMlsSubscriber::cancallbackState, this)); + return true; +} + +/* + * Callbacks for ros messages from canopen_chain_node. There's one callback for each ros topic + * published by canopen_chain_node. Each callback updates the sensor state and publishes an + * OLS-Measurement message. + */ + +void sick_line_guidance::CanMlsSubscriber::cancallbackLCP1(const boost::shared_ptr& msg) +{ + boost::lock_guard publish_lockguard(m_measurement.m_measurement_mutex); + m_measurement.m_mls_state.position[0] = convertToLCP(msg, m_measurement.m_mls_state.position[0], "MLS/LCP1"); + publishMLSMeasurement(); +} + +void sick_line_guidance::CanMlsSubscriber::cancallbackLCP2(const boost::shared_ptr& msg) +{ + boost::lock_guard publish_lockguard(m_measurement.m_measurement_mutex); + m_measurement.m_mls_state.position[1] = convertToLCP(msg, m_measurement.m_mls_state.position[1], "MLS/LCP2"); + publishMLSMeasurement(); +} + +void sick_line_guidance::CanMlsSubscriber::cancallbackLCP3(const boost::shared_ptr& msg) +{ + boost::lock_guard publish_lockguard(m_measurement.m_measurement_mutex); + m_measurement.m_mls_state.position[2] = convertToLCP(msg, m_measurement.m_mls_state.position[2], "MLS/LCP3"); + publishMLSMeasurement(); +} + +void sick_line_guidance::CanMlsSubscriber::cancallbackMarker(const boost::shared_ptr& msg) +{ + boost::lock_guard publish_lockguard(m_measurement.m_measurement_mutex); + m_measurement.m_mls_state.lcp = convertIntegerMessage(msg, m_measurement.m_mls_state.lcp, 0xFF, "MLS/lcp"); + publishMLSMeasurement(); +} + +void sick_line_guidance::CanMlsSubscriber::cancallbackState(const boost::shared_ptr& msg) +{ + boost::lock_guard publish_lockguard(m_measurement.m_measurement_mutex); + m_measurement.m_mls_state.status = convertIntegerMessage(msg, m_measurement.m_mls_state.status, 0xFF, "MLS/status"); + // m_measurement.m_mls_state.error = 0; + // m_measurement.m_mls_query_error_register = false; + // if((m_measurement.m_mls_state.status & 0x0F) == 0) // Bit0 to Bit 3 == 0: bad line, no magnetic field recognized -> SDO request 0x1001 (error register, UINT8) + // { + // m_measurement.m_mls_query_error_register = true; // query object 0x1001 (error register, UINT8) in object dictionary by SDO (query runs in m_measurement in a background thread) + // } + publishMLSMeasurement(); +} + +// void sick_line_guidance::CanMlsSubscriber::cancallbackError(const boost::shared_ptr& msg) +// { +// // Note: Object 0x1001 (error register) is NOT PDO mapped -> Query 0x1001 ((error register) in case of error flag in m_mls_state.status +// boost::lock_guard publish_lockguard(m_measurement.m_measurement_mutex); +// m_measurement.m_mls_state.error = convertIntegerMessage(msg, m_measurement.m_mls_state.error, 0xFF, "MLS/error"); +// publishMLSMeasurement(); +// } diff --git a/Devices/Packages/sick_line_guidance/src/sick_line_guidance_can_ols_subscriber.cpp b/Devices/Packages/sick_line_guidance/src/sick_line_guidance_can_ols_subscriber.cpp new file mode 100755 index 0000000..19247ff --- /dev/null +++ b/Devices/Packages/sick_line_guidance/src/sick_line_guidance_can_ols_subscriber.cpp @@ -0,0 +1,367 @@ +/* + * class CanOlsSubscriber implements the ros subscriber to canopen ols messages. + * + * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim + * Copyright (C) 2019 SICK AG, Waldkirch + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of SICK AG nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Authors: + * Michael Lehning + * + * Copyright 2019 SICK AG + * Copyright 2019 Ing.-Buero Dr. Michael Lehning + * + */ +#include +#include +#include "sick_line_guidance/sick_line_guidance_canopen_chain.h" +#include "sick_line_guidance/sick_line_guidance_can_ols_subscriber.h" +#include "sick_line_guidance/sick_line_guidance_msg_util.h" + +/* + * class CanOlsSubscriber implements the base ros subscriber to canopen ols messages. + */ + +/* + * Constructor. + * @param[in] nh ros::NodeHandle + * @param[in] can_nodeid can id for canopen_chain_node, f.e. "node1" + * @param[in] max_num_retries_after_sdo_error After SDO error, the query is repeated max. N times (default: N=2). If the SDO error persists, the can driver is shutdown and restarted. + * @param[in] max_sdo_rate max. sdo query and publish rate + * @param[in] ros_topic topic for ros messages, f.e. "mls" or "ols" + * @param[in] ros_frameid frameid for ros messages, f.e. "mls_measurement_frame" or "ols_measurement_frame" + * @param[in] initial_sensor_state initial sensor state (f.e. 0x07 for 3 detected lines, or (1 << 4) to indicate sensor error) + * @param[in] subscribe_queue_size buffer size for ros messages + */ +sick_line_guidance::CanOlsSubscriber::CanOlsSubscriber(ros::NodeHandle & nh, const std::string & can_nodeid, int max_num_retries_after_sdo_error, double max_sdo_rate, + const std::string & ros_topic, const std::string & ros_frameid, int initial_sensor_state, int subscribe_queue_size) + : sick_line_guidance::CanSubscriber::CanSubscriber(nh, can_nodeid, max_num_retries_after_sdo_error, max_sdo_rate, initial_sensor_state, subscribe_queue_size) +{ + m_bOLS20queries = false; + m_measurement.m_ols_state.header.frame_id = ros_frameid; + m_measurement.m_ros_publisher = nh.advertise(ros_topic, 1); +} + +/* + * Destructor. + */ +sick_line_guidance::CanOlsSubscriber::~CanOlsSubscriber() +{ +} + +/* + * @brief subsribes to canopen topics for ols messages and sets the callbacks to handle messages from canopen_chain_node + * after PDO messages (LCP = line center point): + * + * Mapping for OLS: + * + * canopenchain-OLS -> CanSubscriber-callback -> OLS sensor state + * ----------------------------------------------------------------------- + * +"_1001" -> cancallbackError(UINT8) -> Error register + * +"_2018" -> cancallbackDevState(UINT8) -> Device state + * +"_2021sub1" -> cancallbackLCP1(INT16) -> LCP1 + * +"_2021sub2" -> cancallbackLCP2(INT16) -> LCP2 + * +"_2021sub3" -> cancallbackLCP3(INT16) -> LCP3 + * +"_2021sub4" -> cancallbackState(UINT8) -> State + * +"_2021sub5" -> cancallbackWidthLCP1(INT16) -> Width LCP1 + * +"_2021sub6" -> cancallbackWidthLCP2(INT16) -> Width LCP2 + * +"_2021sub7" -> cancallbackWidthLCP3(INT16) -> Width LCP3 + * +"_2021sub8" -> cancallbackCode(UINT8) -> Code + * +"_2021sub9" -> cancallbackExtCode(UINT32) -> Extended Code + * + * Mapping for MLS: + * + * canopenchain-MLS -> CanSubscriber-callback -> MLS sensor state + * ----------------------------------------------------------------------- + * +"_1001" -> cancallbackError(UINT8) -> Error register + * +"_2021sub1" -> cancallbackLCP1(INT16) -> LCP1 + * +"_2021sub2" -> cancallbackLCP2(INT16) -> LCP2 + * +"_2021sub3" -> cancallbackLCP3(INT16) -> LCP3 + * +"_2021sub4" -> cancallbackMarker(UINT8) -> #LCP and marker + * +"_2022" -> cancallbackState(UINT8) -> State + * + * Mapping for CiA402 (example, testing only): + * + * canopenchain-CiA402 -> CanSubscriber-callback -> CiA402 state + * --------------------------------------------------------------------- + * +"_6000sub1" -> cancallbackError(UINT8) -> Error register + * +"_6000sub2" -> cancallbackState(UINT8) -> State + * +"_6000sub3" -> cancallbackCode(UINT8) -> Code + * +"_6000sub4" -> cancallbackExtCode(UINT8) -> LSB Extended Code + * +"_6401sub1" -> cancallbackLCP1(INT16) -> LCP1 + * +"_6401sub2" -> cancallbackLCP2(INT16) -> LCP2 + * +"_6401sub3" -> cancallbackLCP3(INT16) -> LCP3 + * +"_6401sub4" -> cancallbackWidthLCP1(INT16) -> Width LCP1 + * +"_6401sub5" -> cancallbackWidthLCP2(INT16) -> Width LCP2 + * +"_6401sub6" -> cancallbackWidthLCP3(INT16) -> Width LCP3 + * + * See operation manuals for details (file SICK-OLS-Operating_instructions_OLS10_de_IM0076955.PDF for OLS + * and file SICK-MLS-Operating_instructions_MLS_de_IM0076567.PDF for MLS) + * + * @return true on success, otherwise false. + */ +bool sick_line_guidance::CanOlsSubscriber::subscribeCanTopics(void) +{ + // m_can_subscriber.push_back(m_measurement.m_nh.subscribe(m_measurement.m_can_nodeid + "_1001", m_subscribe_queue_size, &sick_line_guidance::CanOlsSubscriber::cancallbackError, this)); + // m_can_subscriber.push_back(m_measurement.m_nh.subscribe(m_measurement.m_can_nodeid + "_2018", m_subscribe_queue_size, &sick_line_guidance::CanOlsSubscriber::cancallbackDevState, this)); + m_can_subscriber.push_back(m_measurement.m_nh.subscribe(m_measurement.m_can_nodeid + "_2021sub1", m_subscribe_queue_size, &sick_line_guidance::CanOlsSubscriber::cancallbackLCP1, this)); + m_can_subscriber.push_back(m_measurement.m_nh.subscribe(m_measurement.m_can_nodeid + "_2021sub2", m_subscribe_queue_size, &sick_line_guidance::CanOlsSubscriber::cancallbackLCP2, this)); + m_can_subscriber.push_back(m_measurement.m_nh.subscribe(m_measurement.m_can_nodeid + "_2021sub3", m_subscribe_queue_size, &sick_line_guidance::CanOlsSubscriber::cancallbackLCP3, this)); + m_can_subscriber.push_back(m_measurement.m_nh.subscribe(m_measurement.m_can_nodeid + "_2021sub4", m_subscribe_queue_size, &sick_line_guidance::CanOlsSubscriber::cancallbackState, this)); + m_can_subscriber.push_back(m_measurement.m_nh.subscribe(m_measurement.m_can_nodeid + "_2021sub5", m_subscribe_queue_size, &sick_line_guidance::CanOlsSubscriber::cancallbackWidthLCP1, this)); + m_can_subscriber.push_back(m_measurement.m_nh.subscribe(m_measurement.m_can_nodeid + "_2021sub6", m_subscribe_queue_size, &sick_line_guidance::CanOlsSubscriber::cancallbackWidthLCP2, this)); + m_can_subscriber.push_back(m_measurement.m_nh.subscribe(m_measurement.m_can_nodeid + "_2021sub7", m_subscribe_queue_size, &sick_line_guidance::CanOlsSubscriber::cancallbackWidthLCP3, this)); + m_can_subscriber.push_back(m_measurement.m_nh.subscribe(m_measurement.m_can_nodeid + "_2021sub8", m_subscribe_queue_size, &sick_line_guidance::CanOlsSubscriber::cancallbackCode, this)); + // m_can_subscriber.push_back(m_measurement.m_nh.subscribe(m_measurement.m_can_nodeid + "_2021sub9", m_subscribe_queue_size, &sick_line_guidance::CanOlsSubscriber::cancallbackExtCode, this)); + return true; +} + +/* + * Callbacks for ros messages from canopen_chain_node. There's one callback for each ros topic + * published by canopen_chain_node. Each callback updates the sensor state and publishes an + * OLS-Measurement message. + */ + +void sick_line_guidance::CanOlsSubscriber::cancallbackLCP1(const boost::shared_ptr& msg) +{ + boost::lock_guard publish_lockguard(m_measurement.m_measurement_mutex); + m_measurement.m_ols_state.position[0] = convertToLCP(msg, m_measurement.m_ols_state.position[0], "OLS/LCP1"); + if(m_bOLS20queries) + { + m_measurement.m_ols_query_quality_of_lines.pending() = true; // OLS20 only: query object 2021subB (quality of lines, UINT8) in object dictionary by SDO (query runs in m_measurement in a background thread) + m_measurement.m_ols_query_intensity_of_lines[0].pending() = true; // OLS20 only: query object 2023sub1 (intensity line 1, UINT8) in object dictionary by SDO (query runs in m_measurement in a background thread) + } + else + { + m_measurement.m_ols_state.quality_of_lines = 0; + m_measurement.m_ols_state.intensity_of_lines[0] = 0; + m_measurement.m_ols_query_quality_of_lines.pending() = false; + m_measurement.m_ols_query_intensity_of_lines[0].pending() = false; + } + publishOLSMeasurement(); +} + +void sick_line_guidance::CanOlsSubscriber::cancallbackLCP2(const boost::shared_ptr& msg) +{ + boost::lock_guard publish_lockguard(m_measurement.m_measurement_mutex); + m_measurement.m_ols_state.position[1] = convertToLCP(msg, m_measurement.m_ols_state.position[1], "OLS/LCP2"); + if(m_bOLS20queries) + { + m_measurement.m_ols_query_quality_of_lines.pending() = true; // OLS20 only: query object 2021subB (quality of lines, UINT8) in object dictionary by SDO (query runs in m_measurement in a background thread) + m_measurement.m_ols_query_intensity_of_lines[1].pending() = true; // OLS20 only: query object 2023sub2 (intensity line 2, UINT8) in object dictionary by SDO (query runs in m_measurement in a background thread) + } + else + { + m_measurement.m_ols_state.quality_of_lines = 0; + m_measurement.m_ols_state.intensity_of_lines[1] = 0; + m_measurement.m_ols_query_quality_of_lines.pending() = false; + m_measurement.m_ols_query_intensity_of_lines[1].pending() = false; + } + publishOLSMeasurement(); +} + +void sick_line_guidance::CanOlsSubscriber::cancallbackLCP3(const boost::shared_ptr& msg) +{ + boost::lock_guard publish_lockguard(m_measurement.m_measurement_mutex); + m_measurement.m_ols_state.position[2] = convertToLCP(msg, m_measurement.m_ols_state.position[2], "OLS/LCP3"); + if(m_bOLS20queries) + { + m_measurement.m_ols_query_quality_of_lines.pending() = true; // OLS20 only: query object 2021subB (quality of lines, UINT8) in object dictionary by SDO (query runs in m_measurement in a background thread) + m_measurement.m_ols_query_intensity_of_lines[2].pending() = true; // OLS20 only: query object 2023sub3 (intensity line 3, UINT8) in object dictionary by SDO (query runs in m_measurement in a background thread) + } + else + { + m_measurement.m_ols_state.quality_of_lines = 0; + m_measurement.m_ols_state.intensity_of_lines[2] = 0; + m_measurement.m_ols_query_quality_of_lines.pending() = false; + m_measurement.m_ols_query_intensity_of_lines[2].pending() = false; + } + publishOLSMeasurement(); +} + +void sick_line_guidance::CanOlsSubscriber::cancallbackState(const boost::shared_ptr& msg) +{ + boost::lock_guard publish_lockguard(m_measurement.m_measurement_mutex); + m_measurement.m_ols_state.status = convertIntegerMessage(msg, m_measurement.m_ols_state.status, 0xFF, "OLS/status"); + // If Bit 4 of m_ols_state.status is set (i.e. device status != 0), an error occured. In this case, we have to query SDOs: + // a) object 0x1001 in object dictionary -> measurement.error + // b) object 0x2018 in object dictionary -> measurement.dev_status + if(!sick_line_guidance::MsgUtil::statusOK(m_measurement.m_ols_state)) // Bit 4 OLS status != 0 -> SDO request 0x1001 (error register, UINT8) and 0x2018 (device status register, UINT8) + { + m_measurement.m_ols_query_error_register.pending() = true; // query object 0x1001 (error register, UINT8) in object dictionary by SDO (query runs in m_measurement in a background thread) + if(m_bOLS20queries) // OLS20: query object 0x2018 (device status register, UINT8) in object dictionary by SDO (query runs in m_measurement in a background thread) + { + m_measurement.m_ols_query_device_status_u8.pending() = true; + m_measurement.m_ols_query_device_status_u16.pending() = false; + } + else // OLS10: query object 0x2018 (device status register, UINT16) in object dictionary by SDO (query runs in m_measurement in a background thread) + { + m_measurement.m_ols_query_device_status_u8.pending() = false; + m_measurement.m_ols_query_device_status_u16.pending() = true; + } + } + else + { + m_measurement.m_ols_state.error = 0; + m_measurement.m_ols_state.dev_status = 0; + m_measurement.m_ols_query_device_status_u8.pending() = false; + m_measurement.m_ols_query_device_status_u16.pending() = false; + m_measurement.m_ols_query_error_register.pending() = false; + } + publishOLSMeasurement(); +} + +void sick_line_guidance::CanOlsSubscriber::cancallbackWidthLCP1(const boost::shared_ptr& msg) +{ + boost::lock_guard publish_lockguard(m_measurement.m_measurement_mutex); + m_measurement.m_ols_state.width[0] = convertToLCP(msg, m_measurement.m_ols_state.width[0], "OLS/WidthLCP1"); + publishOLSMeasurement(); +} + +void sick_line_guidance::CanOlsSubscriber::cancallbackWidthLCP2(const boost::shared_ptr& msg) +{ + boost::lock_guard publish_lockguard(m_measurement.m_measurement_mutex); + m_measurement.m_ols_state.width[1] = convertToLCP(msg, m_measurement.m_ols_state.width[1], "OLS/WidthLCP2"); + publishOLSMeasurement(); +} + +void sick_line_guidance::CanOlsSubscriber::cancallbackWidthLCP3(const boost::shared_ptr& msg) +{ + boost::lock_guard publish_lockguard(m_measurement.m_measurement_mutex); + m_measurement.m_ols_state.width[2] = convertToLCP(msg, m_measurement.m_ols_state.width[2], "OLS/WidthLCP3"); + publishOLSMeasurement(); +} + +void sick_line_guidance::CanOlsSubscriber::cancallbackCode(const boost::shared_ptr& msg) +{ + boost::lock_guard publish_lockguard(m_measurement.m_measurement_mutex); + m_measurement.m_ols_state.barcode = convertIntegerMessage(msg, m_measurement.m_ols_state.barcode, 0xFF, "OLS/barcode"); + // If barcode >= 255, we have to query object 0x2021sub9 (extended code, UINT32) in object dictionary by SDOs + if(m_measurement.m_ols_state.barcode >= 255) + { + m_measurement.m_ols_query_extended_code.pending() = true; // query object 0x2021sub9 (extended code, UINT32) in object dictionary by SDO (query runs in m_measurement in a background thread) + } + else + { + m_measurement.m_ols_query_extended_code.pending() = false; + m_measurement.m_ols_state.extended_code = 0; + } + // OLS20 only: query object 2021subA (barcode center point, INT16) in object dictionary by SDO (query runs in m_measurement in a background thread) + if(m_measurement.m_ols_state.barcode > 0 && m_bOLS20queries) + { + m_measurement.m_ols_query_barcode_center_point.pending() = true; + } + else + { + m_measurement.m_ols_state.barcode_center_point = 0; + m_measurement.m_ols_query_barcode_center_point.pending() = false; + } + publishOLSMeasurement(); +} + +// void sick_line_guidance::CanOlsSubscriber::cancallbackError(const boost::shared_ptr& msg) +// { +// // Note: Object 0x1001 (error register) is NOT PDO mapped -> Query 0x1001 ((error register) in case of error flag in m_ols_state.status +// boost::lock_guard publish_lockguard(m_measurement.m_measurement_mutex); +// m_measurement.m_ols_state.error = convertIntegerMessage(msg, m_measurement.m_ols_state.error, 0xFF, "OLS/error"); +// publishOLSMeasurement(); +// } + +// void sick_line_guidance::CanOlsSubscriber::cancallbackDevState(const boost::shared_ptr& msg) +// { +// // Note: Object 0x2018 is NOT PDO mapped -> Query 0x2018 (device state) in case of error flag in m_ols_state.status +// boost::lock_guard publish_lockguard(m_measurement.m_measurement_mutex); +// m_measurement.m_ols_state.dev_status = convertIntegerMessage(msg, m_measurement.m_ols_state.dev_status, 0xFF, "OLS/DevState"); +// publishOLSMeasurement(); +// } + +// void sick_line_guidance::CanOlsSubscriber::cancallbackExtCode(const boost::shared_ptr& msg) +// { +// // Note: Object 0x2021sub9 is NOT PDO mapped -> Query 0x2021sub9 (extended code) in case of m_ols_state.barcode == 0xFF +// boost::lock_guard publish_lockguard(m_measurement.m_measurement_mutex); +// m_measurement.m_ols_state.extended_code = convertIntegerMessage(msg, m_measurement.m_ols_state.extended_code, 0xFFFFFFFF, "OLS/extcode"); +// publishOLSMeasurement(); +// } + +/* + * class CanOls10Subscriber derives from CanOlsSubscriber to implements OLS10 specific handling of the ros canopen messages. + */ + +/* + * Constructor. + * @param[in] nh ros::NodeHandle + * @param[in] can_nodeid can id for canopen_chain_node, f.e. "node1" + * @param[in] max_num_retries_after_sdo_error After SDO error, the query is repeated max. N times (default: N=2). If the SDO error persists, the can driver is shutdown and restarted. + * @param[in] max_sdo_rate max. sdo query and publish rate + * @param[in] ros_topic topic for ros messages, f.e. "mls" or "ols" + * @param[in] ros_frameid frameid for ros messages, f.e. "mls_measurement_frame" or "ols_measurement_frame" + * @param[in] initial_sensor_state initial sensor state (f.e. 0x07 for 3 detected lines, or (1 << 4) to indicate sensor error) + * @param[in] subscribe_queue_size buffer size for ros messages + */ +sick_line_guidance::CanOls10Subscriber::CanOls10Subscriber(ros::NodeHandle & nh, const std::string & can_nodeid, int max_num_retries_after_sdo_error, double max_sdo_rate, + const std::string & ros_topic, const std::string & ros_frameid, int initial_sensor_state, int subscribe_queue_size) + : sick_line_guidance::CanOlsSubscriber::CanOlsSubscriber(nh, can_nodeid, max_num_retries_after_sdo_error, max_sdo_rate, ros_topic, ros_frameid, initial_sensor_state, subscribe_queue_size) +{ +} + +/* + * class CanOls20Subscriber derives from CanOlsSubscriber to implements OLS20 specific handling of the ros canopen messages. + */ + +/* + * Constructor. + * @param[in] nh ros::NodeHandle + * @param[in] can_nodeid can id for canopen_chain_node, f.e. "node1" + * @param[in] max_num_retries_after_sdo_error After SDO error, the query is repeated max. N times (default: N=2). If the SDO error persists, the can driver is shutdown and restarted. + * @param[in] max_sdo_rate max. sdo query and publish rate + * @param[in] ros_topic topic for ros messages, f.e. "mls" or "ols" + * @param[in] ros_frameid frameid for ros messages, f.e. "mls_measurement_frame" or "ols_measurement_frame" + * @param[in] initial_sensor_state initial sensor state (f.e. 0x07 for 3 detected lines, or (1 << 4) to indicate sensor error) + * @param[in] subscribe_queue_size buffer size for ros messages + */ +sick_line_guidance::CanOls20Subscriber::CanOls20Subscriber(ros::NodeHandle & nh, const std::string & can_nodeid, int max_num_retries_after_sdo_error, double max_sdo_rate, + const std::string & ros_topic, const std::string & ros_frameid, int initial_sensor_state, int subscribe_queue_size) + : sick_line_guidance::CanOlsSubscriber::CanOlsSubscriber(nh, can_nodeid, max_num_retries_after_sdo_error, max_sdo_rate, ros_topic, ros_frameid, initial_sensor_state, subscribe_queue_size) +{ + m_bOLS20queries = true; // OLS20 only: query objects 2021subA (barcode center point, INT16), 2021subB (quality of lines, UINT8) and 2023sub1 to 2023sub3 (intensity line 1 - 3, UINT8) +} diff --git a/Devices/Packages/sick_line_guidance/src/sick_line_guidance_can_subscriber.cpp b/Devices/Packages/sick_line_guidance/src/sick_line_guidance_can_subscriber.cpp new file mode 100755 index 0000000..1b2aa73 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/src/sick_line_guidance_can_subscriber.cpp @@ -0,0 +1,523 @@ +/* + * class CanSubscriber: base class for CanOlsSubscriber and CanMlsSubscriber, + * implements the base class for ros subscriber to canopen messages for OLS and MLS. + * Converts canopen messages to MLS/OLS measurement messages and publishes + * MLS/OLS measurement messages on the configured ros topic ("/mls" or "/ols"). + * + * class CanSubscriber::MeasurementHandler: queries SDOs (if required) and publishes MLS/OLS measurement messages in a background thread + * + * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim + * Copyright (C) 2019 SICK AG, Waldkirch + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of SICK AG nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Authors: + * Michael Lehning + * + * Copyright 2019 SICK AG + * Copyright 2019 Ing.-Buero Dr. Michael Lehning + * + */ +#include +#include "sick_line_guidance/sick_line_guidance_canopen_chain.h" +#include "sick_line_guidance/sick_line_guidance_can_subscriber.h" +#include "sick_line_guidance/sick_line_guidance_diagnostic.h" +#include "sick_line_guidance/sick_line_guidance_msg_util.h" + +/* + * Constructor. + * @param[in] nh ros::NodeHandle + * @param[in] can_nodeid can id for canopen_chain_node, f.e. "node1" + * @param[in] initial_sensor_state initial sensor state (f.e. 0x07 for 3 detected lines, or (1 << 4) to indicate sensor error) + * @param[in] max_publish_rate max rate to publish OLS/MLS measurement messages (default: min. 1 ms between two measurement messages) + * @param[in] max_query_rate max rate to query SDOs if required (default: min. 1 ms between sdo queries) + * @param[in] schedule_publish_delay MLS and OLS measurement message are scheduled to be published 5 milliseconds after first PDO is received + * @param[in] max_publish_delay MLS and OLS measurement message are scheduled to be published max. 2*20 milliseconds after first PDO is received, even if a sdo request is pending (max. 2 * tpdo rate) + * @param[in] query_jitter jitter in seconds (default: 10 ms), i.e. a sdo is requested, if the query is pending and the last successful query is out of the time jitter. + * @param[in] max_num_retries_after_sdo_error After SDO error, the query is repeated max. N times (default: N=2). If the SDO error persists, the can driver is shutdown and restarted. + * By default, a sdo request is send, if the query is pending and not done within the last 10 ms. + */ +sick_line_guidance::CanSubscriber::MeasurementHandler::MeasurementHandler(ros::NodeHandle &nh, const std::string &can_nodeid, int max_num_retries_after_sdo_error, int initial_sensor_state, double max_publish_rate, double max_query_rate, double schedule_publish_delay, double max_publish_delay, double query_jitter) + : m_nh(nh), m_can_nodeid(can_nodeid), m_max_publish_rate(ros::Rate(max_publish_rate)), m_max_sdo_query_rate(ros::Rate(max_query_rate)), + m_schedule_publish_delay(ros::Duration(schedule_publish_delay)), m_max_publish_delay(ros::Duration(max_publish_delay)), m_max_num_retries_after_sdo_error(max_num_retries_after_sdo_error) +{ + // initialize MLS/OLS sensor states + sick_line_guidance::MsgUtil::zero(m_mls_state); + m_mls_state.header.stamp = ros::Time::now(); + m_mls_state.lcp = static_cast(initial_sensor_state); + m_mls_state.status = ((initial_sensor_state & 0x7) ? 1 : 0); + sick_line_guidance::MsgUtil::zero(m_ols_state); + m_ols_state.header.stamp = ros::Time::now(); + m_ols_state.status = initial_sensor_state; + // initialize publisher thread + m_publish_mls_measurement = ros::Time(0); + m_publish_ols_measurement = ros::Time(0); + m_publish_measurement_latest = ros::Time(0); + m_ols_query_extended_code = QuerySupport(query_jitter); + m_ols_query_device_status_u8 = QuerySupport(query_jitter); + m_ols_query_device_status_u16 = QuerySupport(query_jitter); + m_ols_query_error_register = QuerySupport(query_jitter); + m_ols_query_barcode_center_point = QuerySupport(query_jitter); + m_ols_query_quality_of_lines = QuerySupport(query_jitter); + m_ols_query_intensity_of_lines[0] = QuerySupport(query_jitter); + m_ols_query_intensity_of_lines[1] = QuerySupport(query_jitter); + m_ols_query_intensity_of_lines[2] = QuerySupport(query_jitter); + m_measurement_publish_thread = new boost::thread(&sick_line_guidance::CanSubscriber::MeasurementHandler::runMeasurementPublishThread, this); + m_measurement_sdo_query_thread = new boost::thread(&sick_line_guidance::CanSubscriber::MeasurementHandler::runMeasurementSDOqueryThread, this); +} + +/* + * Destructor. + */ +sick_line_guidance::CanSubscriber::MeasurementHandler::~MeasurementHandler() +{ + if (m_measurement_sdo_query_thread) + { + m_measurement_sdo_query_thread->join(); + delete (m_measurement_sdo_query_thread); + m_measurement_sdo_query_thread = 0; + } + if (m_measurement_publish_thread) + { + m_measurement_publish_thread->join(); + delete (m_measurement_publish_thread); + m_measurement_publish_thread = 0; + } +} + +/* + * @brief Runs the background thread to publish MLS/OLS measurement messages + */ +void sick_line_guidance::CanSubscriber::MeasurementHandler::runMeasurementPublishThread(void) +{ + while(ros::ok()) + { + // Publish mls measurement (if one has been triggered, i.e. a pdo has been received recently) + if(isMLSMeasurementTriggered()) + { + sick_line_guidance::MLS_Measurement measurement_msg; + { + boost::lock_guard publish_lockguard(m_measurement_mutex); + m_mls_state.header.stamp = ros::Time::now(); + measurement_msg = m_mls_state; + } + m_ros_publisher.publish(measurement_msg); + schedulePublishMLSMeasurement(false); + bool line_good = sick_line_guidance::MsgUtil::lineOK(measurement_msg); // MLS status bit 0 ("Line good") == 0 => no line detected or line too weak, 1 => line detected, MLS #lcp (bit 0-2 == 0) => no line detected + ROS_INFO_STREAM("sick_line_guidance::MLS_Measurement: {" << sick_line_guidance::MsgUtil::toInfo(measurement_msg) << ",line_good=" << line_good << "}"); + /* if(line_good) + sick_line_guidance::Diagnostic::update(sick_line_guidance::DIAGNOSTIC_STATUS::OK, "MLS Measurement published"); + else + sick_line_guidance::Diagnostic::update(sick_line_guidance::DIAGNOSTIC_STATUS::NO_LINE_DETECTED, "MLS Measurement published, no line"); */ + } + // Publish ols measurement (if one has been triggered, i.e. a pdo has been received recently) + if(isOLSMeasurementTriggered() && (!isSDOQueryPending() || isLatestTimeForMeasurementPublishing())) // no sdo requests are pending or latest time to publish a new measurement is reached + { + sick_line_guidance::OLS_Measurement measurement_msg; + { + boost::lock_guard publish_lockguard(m_measurement_mutex); + m_ols_state.header.stamp = ros::Time::now(); + measurement_msg = m_ols_state; + } + m_ros_publisher.publish(measurement_msg); + schedulePublishOLSMeasurement(false); + bool status_ok = sick_line_guidance::MsgUtil::statusOK(measurement_msg); // OLS status bit 4: 0 => Sensor ok, 1 => Sensor not ok => 0x2018 (measurement_msg.dev_status) + bool line_good = status_ok && sick_line_guidance::MsgUtil::lineOK(measurement_msg); // Bit 0-2 OLS status == 0 => no line found + ROS_INFO_STREAM("sick_line_guidance::OLS_Measurement: {" << sick_line_guidance::MsgUtil::toInfo(measurement_msg) << ",status_ok=" << status_ok << ",line_good=" << line_good << "}"); + if(!status_ok) + sick_line_guidance::Diagnostic::update(sick_line_guidance::DIAGNOSTIC_STATUS::ERROR_STATUS, "OLS Measurement published, status error " + sick_line_guidance::MsgUtil::toHexString(measurement_msg.dev_status)); + /* else if(!line_good) + sick_line_guidance::Diagnostic::update(sick_line_guidance::DIAGNOSTIC_STATUS::NO_LINE_DETECTED, "OLS Measurement published, no line"); + else + sick_line_guidance::Diagnostic::update(sick_line_guidance::DIAGNOSTIC_STATUS::OK, "OLS Measurement published"); */ + } + m_max_publish_rate.sleep(); + } +} + +/* + * @brief Runs the background thread to query SDOs, if required + */ +void sick_line_guidance::CanSubscriber::MeasurementHandler::runMeasurementSDOqueryThread(void) +{ + while(ros::ok()) + { + // Query SDOs if required + querySDOifPending(m_ols_query_extended_code, "2021sub9", m_max_num_retries_after_sdo_error, m_ols_state.extended_code, 1); // OLS: query object 0x2021sub9 (extended code, UINT32) in object dictionary by SDO + querySDOifPending(m_ols_query_device_status_u8, "2018", m_max_num_retries_after_sdo_error, m_ols_state.dev_status, 1); // OLS20: query object 0x2018 (device status register, UINT8) in object dictionary by SDO + querySDOifPending(m_ols_query_device_status_u16, "2018", m_max_num_retries_after_sdo_error, m_ols_state.dev_status, 1); // OLS10: query object 0x2018 (device status register, UINT16) in object dictionary by SDO + querySDOifPending (m_ols_query_error_register, "1001", m_max_num_retries_after_sdo_error, m_ols_state.error, 1); // OLS: query object 0x1001 (error register, UINT8) in object dictionary by SDO + querySDOifPending (m_ols_query_barcode_center_point, "2021subA", m_max_num_retries_after_sdo_error, m_ols_state.barcode_center_point, 0.001f); // OLS20 only: query object 2021subA (barcode center point, INT16) in object dictionary by SDO + querySDOifPending (m_ols_query_quality_of_lines, "2021subB", m_max_num_retries_after_sdo_error, m_ols_state.quality_of_lines, 1); // OLS20 only: query object 2021subB (quality of lines, UINT8) in object dictionary by SDO + querySDOifPending (m_ols_query_intensity_of_lines[0], "2023sub1", m_max_num_retries_after_sdo_error, m_ols_state.intensity_of_lines[0], 1); // OLS20 only: query object 2023sub1 (intensity line 1, UINT8) + querySDOifPending (m_ols_query_intensity_of_lines[1], "2023sub2", m_max_num_retries_after_sdo_error, m_ols_state.intensity_of_lines[1], 1); // OLS20 only: query object 2023sub2 (intensity line 2, UINT8) + querySDOifPending (m_ols_query_intensity_of_lines[2], "2023sub3", m_max_num_retries_after_sdo_error, m_ols_state.intensity_of_lines[2], 1); // OLS20 only: query object 2023sub3 (intensity line 3, UINT8) + // Clear all pending status + m_ols_query_extended_code.pending() = false; + m_ols_query_device_status_u8.pending() = false; + m_ols_query_device_status_u16.pending() = false; + m_ols_query_error_register.pending() = false; + m_ols_query_barcode_center_point.pending() = false; + m_ols_query_quality_of_lines.pending() = false; + m_ols_query_intensity_of_lines[0].pending() = false; + m_ols_query_intensity_of_lines[1].pending() = false; + m_ols_query_intensity_of_lines[2].pending() = false; + m_max_sdo_query_rate.sleep(); + } +} + +/* + * @brief queries an object in the object dictionary by SDO and returns its value. + * @param[in] can_object_idx object index in object dictionary, f.e. "2018" (OLS device status) or "2021sub9" (OLS extended code) + * @param[in] max_num_retries_after_sdo_error After SDO error, the query is repeated max. N times (default: N=2). If the SDO error persists, the can driver is shutdown and restarted. + * @param[out] can_object_value object value from SDO response + * @return true on success (can_object_value set to objects value), false otherwise (can_object_value not set) + */ +bool sick_line_guidance::CanSubscriber::MeasurementHandler::querySDO(const std::string & can_object_idx, int max_num_retries_after_sdo_error, uint8_t & can_object_value) +{ + std::string can_object_entry = ""; + if(querySDO(can_object_idx, max_num_retries_after_sdo_error, can_object_entry) && convertSDOresponse(can_object_entry, can_object_value)) + return true; + ROS_ERROR_STREAM("querySDO(" << can_object_idx << ",uint8_t) failed, value=" << sick_line_guidance::MsgUtil::toHexString(can_object_value) ); + return false; +} + +/* + * @brief queries an object in the object dictionary by SDO and returns its value. + * @param[in] can_object_idx object index in object dictionary, f.e. "2018" (OLS device status) or "2021sub9" (OLS extended code) + * @param[in] max_num_retries_after_sdo_error After SDO error, the query is repeated max. N times (default: N=2). If the SDO error persists, the can driver is shutdown and restarted. + * @param[out] can_object_value object value from SDO response + * @return true on success (can_object_value set to objects value), false otherwise (can_object_value not set) + */ +bool sick_line_guidance::CanSubscriber::MeasurementHandler::querySDO(const std::string & can_object_idx, int max_num_retries_after_sdo_error, int16_t & can_object_value) +{ + int32_t value = 0; + std::string can_object_entry = ""; + if(querySDO(can_object_idx, max_num_retries_after_sdo_error, can_object_entry) && convertSDOresponse(can_object_entry, value) && value >= INT16_MIN && value <= INT16_MAX) + { + can_object_value = (int16_t)value; + return true; + } + ROS_ERROR_STREAM("querySDO(" << can_object_idx << ",int16_t) failed, value=" << sick_line_guidance::MsgUtil::toHexString(can_object_value) ); + return false; +} + +/* + * @brief queries an object in the object dictionary by SDO and returns its value. + * @param[in] can_object_idx object index in object dictionary, f.e. "2018" (OLS device status) or "2021sub9" (OLS extended code) + * @param[in] max_num_retries_after_sdo_error After SDO error, the query is repeated max. N times (default: N=2). If the SDO error persists, the can driver is shutdown and restarted. + * @param[out] can_object_value object value from SDO response + * @return true on success (can_object_value set to objects value), false otherwise (can_object_value not set) + */ +bool sick_line_guidance::CanSubscriber::MeasurementHandler::querySDO(const std::string & can_object_idx, int max_num_retries_after_sdo_error, uint16_t & can_object_value) +{ + uint32_t value = 0; + std::string can_object_entry = ""; + if(querySDO(can_object_idx, max_num_retries_after_sdo_error, can_object_entry) && convertSDOresponse(can_object_entry, value) && value <= UINT16_MAX) + { + can_object_value = (uint16_t)value; + return true; + } + ROS_ERROR_STREAM("querySDO(" << can_object_idx << ",uint16_t) failed, value=" << sick_line_guidance::MsgUtil::toHexString(can_object_value) ); + return false; +} + +/* + * @brief queries an object in the object dictionary by SDO and returns its value. + * @param[in] can_object_idx object index in object dictionary, f.e. "2018" (OLS device status) or "2021sub9" (OLS extended code) + * @param[in] max_num_retries_after_sdo_error After SDO error, the query is repeated max. N times (default: N=2). If the SDO error persists, the can driver is shutdown and restarted. + * @param[out] can_object_value object value from SDO response + * @return true on success (can_object_value set to objects value), false otherwise (can_object_value not set) + */ +bool sick_line_guidance::CanSubscriber::MeasurementHandler::querySDO(const std::string & can_object_idx, int max_num_retries_after_sdo_error, uint32_t & can_object_value) +{ + std::string can_object_entry = ""; + if(querySDO(can_object_idx, max_num_retries_after_sdo_error, can_object_entry) && convertSDOresponse(can_object_entry, can_object_value)) + return true; + ROS_ERROR_STREAM("querySDO(" << can_object_idx << ",uint32_t) failed, value=" << sick_line_guidance::MsgUtil::toHexString(can_object_value) ); + return false; +} + +/* + * @brief queries an object in the object dictionary by SDO and returns its value. + * @param[in] can_object_idx object index in object dictionary, f.e. "2018" (OLS device status) or "2021sub9" (OLS extended code) + * @param[in] max_num_retries_after_sdo_error After SDO error, the query is repeated max. N times (default: N=2). If the SDO error persists, the can driver is shutdown and restarted. + * @param[out] can_object_value object value from SDO response + * @return true on success (can_object_value set to objects value), false otherwise (can_object_value not set) + */ +bool sick_line_guidance::CanSubscriber::MeasurementHandler::querySDO(const std::string & can_object_idx, int max_num_retries_after_sdo_error, std::string & can_object_value) +{ + std::string can_msg = ""; + can_object_value = ""; + bool sdo_success = sick_line_guidance::CanopenChain::queryCanObject(m_nh, m_can_nodeid, can_object_idx, max_num_retries_after_sdo_error, can_msg, can_object_value); + if(sdo_success) + { + ROS_INFO_STREAM("sick_line_guidance::CanopenChain::queryCanObject(" << m_can_nodeid << "): [" << can_object_idx << "]=" << can_object_value ); + /* sick_line_guidance::Diagnostic::update(sick_line_guidance::DIAGNOSTIC_STATUS::OK, "SDO"); */ + } + else + { + ROS_ERROR_STREAM("sick_line_guidance::CanopenChain::queryCanObject(" << m_can_nodeid << "): [" << can_object_idx << "]=\"" << can_object_value << "\" failed " << can_msg); + sick_line_guidance::Diagnostic::update(sick_line_guidance::DIAGNOSTIC_STATUS::SDO_COMMUNICATION_ERROR, "querySDO failed"); + } + return sdo_success; +} + +/* + * @brief converts a sdo response to uint8. + * Note: nh.serviceClient returns SDO responses as strings. + * In case of 1 Byte (UINT8) values, the returned "string" contains this byte (uint8 value streamed to std::stringstream). + * Example: UINT8 with value 0 are returned as "\0", not "0". Parsing the SDO response has to handle the UINT8 case, + * which is done by this function + * Note: std::exception are caught (error message and return false in this case) + * @param[in] response sdo response as string + * @param[out] value uint8 value converted from SDO response + * @return true on success, false otherwise + */ +bool sick_line_guidance::CanSubscriber::MeasurementHandler::convertSDOresponse(const std::string & response, uint8_t & value) +{ + value = 0; + try + { + if(response.empty()) + value = 0; + else if(response.size() == 1) + value = static_cast(response[0]); + else + { + uint32_t u32val = std::stoul(response, 0, 0); + if((u32val & 0xFFFFFF00) != 0) + ROS_WARN_STREAM("convertSDOresponse(" << response << ") to UINT8: value " << u32val << " out of UINT8 range, possible loss of data."); + value = static_cast(u32val & 0xFF); + } + return true; + } + catch(const std::exception & exc) + { + ROS_ERROR_STREAM("convertSDOresponse(" << response << ") to UINT8 failed: exception = " << exc.what()); + } + return false; +} + +/* + * @brief converts a sdo response to uint32. + * Note: std::exception are caught (error message and return false in this case) + * @param[in] response sdo response as string + * @param[out] value uint32 value converted from SDO response + * @return true on success, false otherwise + */ +bool sick_line_guidance::CanSubscriber::MeasurementHandler::convertSDOresponse(const std::string & response, uint32_t & value) +{ + value = 0; + try + { + value = std::stoul(response, 0, 0); + return true; + } + catch(const std::exception & exc) + { + ROS_ERROR_STREAM("convertSDOresponse(" << response << ") to UINT32 failed: exception = " << exc.what()); + } + return false; +} + +/* + * @brief converts a sdo response to int32. + * Note: std::exception are caught (error message and return false in this case) + * @param[in] response sdo response as string + * @param[out] value uint32 value converted from SDO response + * @return true on success, false otherwise + */ +bool sick_line_guidance::CanSubscriber::MeasurementHandler::convertSDOresponse(const std::string & response, int32_t & value) +{ + value = 0; + try + { + value = std::stol(response, 0, 0); + return true; + } + catch(const std::exception & exc) + { + ROS_ERROR_STREAM("convertSDOresponse(" << response << ") to INT32 failed: exception = " << exc.what()); + } + return false; +} + +/* + * @brief returns true, if publishing of a MLS measurement is scheduled and time has been reached for publishing the current MLS measurement. + */ +bool sick_line_guidance::CanSubscriber::MeasurementHandler::isMLSMeasurementTriggered(void) +{ + boost::lock_guard schedule_lockguard(m_publish_measurement_mutex); + return !m_publish_mls_measurement.isZero() && ros::Time::now() > m_publish_mls_measurement; +} + +/* + * @brief returns true, if publishing of a OLS measurement is scheduled and time has been reached for publishing the current OLS measurement. + */ +bool sick_line_guidance::CanSubscriber::MeasurementHandler::isOLSMeasurementTriggered(void) +{ + boost::lock_guard schedule_lockguard(m_publish_measurement_mutex); + return !m_publish_ols_measurement.isZero() && ros::Time::now() > m_publish_ols_measurement; +} + +/* + * @brief returns true, if publishing of a measurement is scheduled and latest time for publishing has been reached. + */ +bool sick_line_guidance::CanSubscriber::MeasurementHandler::isLatestTimeForMeasurementPublishing(void) +{ + boost::lock_guard schedule_lockguard(m_publish_measurement_mutex); + return !m_publish_measurement_latest.isZero() && ros::Time::now() > m_publish_measurement_latest; +} + + +/* + * @brief returns true, if sdo query is pending, i.e. measurement is not yet completed (sdo request or sdo response still pending) + */ +bool sick_line_guidance::CanSubscriber::MeasurementHandler::isSDOQueryPending(void) +{ + boost::lock_guard schedule_lockguard(m_publish_measurement_mutex); + return (m_ols_query_extended_code.pending() || m_ols_query_device_status_u8.pending() || m_ols_query_device_status_u16.pending() || m_ols_query_error_register.pending() || m_ols_query_barcode_center_point.pending() + || m_ols_query_quality_of_lines.pending() || m_ols_query_intensity_of_lines[0].pending() || m_ols_query_intensity_of_lines[1].pending() || m_ols_query_intensity_of_lines[2].pending()); +} + +/* + * @brief schedules the publishing of the current MLS measurement message. + * @param[in] schedule if true, publishing is scheduled, otherwise a possibly pending schedule is removed. + */ +void sick_line_guidance::CanSubscriber::MeasurementHandler::schedulePublishMLSMeasurement(bool schedule) +{ + boost::lock_guard schedule_lockguard(m_publish_measurement_mutex); + if(schedule && m_publish_mls_measurement.isZero()) // otherwise publishing the measurement is already scheduled + { + m_publish_mls_measurement = ros::Time::now() + m_schedule_publish_delay; + m_publish_measurement_latest = ros::Time::now() + m_max_publish_delay; + } + if(!schedule && !m_publish_mls_measurement.isZero()) // remove pending schedule + { + m_publish_mls_measurement = ros::Time(0); + m_publish_measurement_latest = ros::Time(0); + } +} + +/* + * @brief schedules the publishing of the current OLS measurement message. + * @param[in] schedule if true, publishing is scheduled, otherwise a possibly pending schedule is removed. + */ +void sick_line_guidance::CanSubscriber::MeasurementHandler::schedulePublishOLSMeasurement(bool schedule) +{ + boost::lock_guard schedule_lockguard(m_publish_measurement_mutex); + if(schedule && m_publish_ols_measurement.isZero()) // otherwise publishing the measurement is already scheduled + { + m_publish_ols_measurement = ros::Time::now() + m_schedule_publish_delay; + m_publish_measurement_latest = ros::Time::now() + m_max_publish_delay; + } + if(!schedule && !m_publish_ols_measurement.isZero()) // remove pending schedule + { + m_publish_ols_measurement = ros::Time(0); + m_publish_measurement_latest = ros::Time(0); + } +} + +/* + * Constructor. + * @param[in] nh ros::NodeHandle + * @param[in] can_nodeid can id for canopen_chain_node, f.e. "node1" + * @param[in] max_num_retries_after_sdo_error After SDO error, the query is repeated max. N times (default: N=2). If the SDO error persists, the can driver is shutdown and restarted. + * @param[in] max_sdo_rate max. sdo query and publish rate + * @param[in] initial_sensor_state initial sensor state (f.e. 0x07 for 3 detected lines, or (1 << 4) to indicate sensor error) + * @param[in] subscribe_queue_size buffer size for ros messages + */ +sick_line_guidance::CanSubscriber::CanSubscriber(ros::NodeHandle & nh, const std::string & can_nodeid, int max_num_retries_after_sdo_error, double max_sdo_rate, int initial_sensor_state, int subscribe_queue_size) + : m_measurement(nh, can_nodeid, max_num_retries_after_sdo_error, initial_sensor_state, max_sdo_rate, max_sdo_rate), m_subscribe_queue_size(subscribe_queue_size) +{ +} + +/* + * Destructor. + */ +sick_line_guidance::CanSubscriber::~CanSubscriber() +{ +} + +/* + * @brief converts an INT16 message (line center point lcp in millimeter) to a float lcp in meter. + * + * @param[in] msg INT16 message (line center point lcp in millimeter) + * @param[in] defaultvalue default, is returned in case of an invalid message + * @param[in] dbg_info informal debug message (ROS_INFO/ROS_ERROR) + * + * @return float lcp in meter + */ +float sick_line_guidance::CanSubscriber::convertToLCP(const boost::shared_ptr& msg, + const float & defaultvalue, const std::string & dbg_info) +{ + float value = defaultvalue; + if(msg) + { + int16_t data = ((msg->data)&0xFFFF); + value = data / 1000.0; + ROS_DEBUG("sick_line_guidance::CanSubscriber(%s): data: 0x%04x -> %.3f", dbg_info.c_str(), (int)(data), value); + } + else + { + ROS_ERROR("## ERROR sick_line_guidance::CanSubscriber(%s): invalid message (%s:%d)", dbg_info.c_str(), __FILE__, __LINE__); + } + return value; +} + +/* + * @brief schedules the current MLS measurement message for publishing. + */ +void sick_line_guidance::CanSubscriber::publishMLSMeasurement(void) +{ + m_measurement.schedulePublishMLSMeasurement(true); +} + +/* + * @brief schedules the current OLS measurement message for publishing. + */ +void sick_line_guidance::CanSubscriber::publishOLSMeasurement(void) +{ + m_measurement.schedulePublishOLSMeasurement(true); +} + diff --git a/Devices/Packages/sick_line_guidance/src/sick_line_guidance_canopen_chain.cpp b/Devices/Packages/sick_line_guidance/src/sick_line_guidance_canopen_chain.cpp new file mode 100755 index 0000000..b3d1560 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/src/sick_line_guidance_canopen_chain.cpp @@ -0,0 +1,550 @@ +/* + * sick_line_guidance_canopen_chain wraps and adapts RosChain of package canopen_chain_node for sick_line_guidance. + * + * class CanopenChain implements canopen support for sick_line_guidance + * based on RosChain of package canopen_chain_node (package ros_canopen). + * + * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim + * Copyright (C) 2019 SICK AG, Waldkirch + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of SICK AG nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Authors: + * Michael Lehning + * + * Copyright 2019 SICK AG + * Copyright 2019 Ing.-Buero Dr. Michael Lehning + * + */ +#include +#include +#include "sick_line_guidance/sick_line_guidance_canopen_chain.h" +#include "sick_line_guidance/sick_line_guidance_diagnostic.h" + +/* + * CanopenChain constructor + * + * @param[in] nh ros::NodeHandle + * @param[in] nh_priv ros::NodeHandle + */ +sick_line_guidance::CanopenChain::CanopenChain(const ros::NodeHandle &nh, const ros::NodeHandle &nh_priv) + : canopen::RosChain(nh, nh_priv) +{ +} + +/* + * CanopenChain destructor + */ +sick_line_guidance::CanopenChain::~CanopenChain() +{ +} + +/* + * Connects to CAN bus by calling "init" of canopen_chain_node ros-service + * + * @param[in] nh ros::NodeHandle + * + * @return true, if initialization successful, otherwise false. + */ +bool sick_line_guidance::CanopenChain::connectInitService(ros::NodeHandle &nh) +{ + bool success = false; + try + { + ros::ServiceClient servclient = nh.serviceClient("/driver/init"); + std_srvs::Trigger servtrigger; + success = servclient.call(servtrigger); + if(!success) + { + ROS_ERROR_STREAM("sick_line_guidance::CanopenChain::connectInitService(): ServiceClient::call(\"/driver/init\") failed."); + sick_line_guidance::Diagnostic::update(sick_line_guidance::DIAGNOSTIC_STATUS::CAN_COMMUNICATION_ERROR, "CanopenChain: \"/driver/init\" failed"); + } + else + { + ROS_INFO_STREAM("sick_line_guidance::CanopenChain::connectInitService(): ServiceClient::call(\"/driver/init\") successfull."); + sick_line_guidance::Diagnostic::update(sick_line_guidance::DIAGNOSTIC_STATUS::OK, "CanopenChain: \"/driver/init\" successfull"); + } + } + catch(const std::exception & exc) + { + success = false; + ROS_ERROR_STREAM("sick_line_guidance::CanopenChain::connectInitService(): ServiceClient::call(\"/driver/init\") failed: exception = " << exc.what()); + sick_line_guidance::Diagnostic::update(sick_line_guidance::DIAGNOSTIC_STATUS::CAN_COMMUNICATION_ERROR, "CanopenChain: \"/driver/init\" failed"); + } + return success; +} + +/* + * Sets all dcf overlays in the can devices. All dcf values are reread from the can device and checked to be identical + * to the configured object value. If all dcf overlays have been set successfully (i.e. successfull write operation, + * successfull re-read operation and object value identical to the configured value), true is returned. + * Otherwise, false is returned (i.e. some dcf overlays could not be set). + * + * dcf overlays are a list of key value pairs with "object_index" as key and "object_value" as value. + * Examples for dcf overlays can be found in the yaml configuration file. Example from sick_line_guidance_ols20.yaml: + * node1: + * id: 0x0A # CAN-Node-ID of can device, default: Node-ID 10=0x0A for OLS and MLS + * eds_pkg: sick_line_guidance # package name for relative path + * eds_file: SICK_OLS20.eds # path to EDS/DCF file + * ... + * dcf_overlay: + * "2001sub5": "0x01" # sensorFlipped, UINT8, DataType=0x0005, defaultvalue 0x00 + * "2002sub1": "0.015" # Typ. Width, FLOAT32, DataType=0x0008 + * "2002sub2": "0.001" # Min. Width, FLOAT32, DataType=0x0008 + * "2002sub3": "0.050" # Max. Width, FLOAT32, DataType=0x0008 + * + * @param[in] nh ros::NodeHandle + * @param[in] node_id can node id of the can device + * @param[in] dcf_overlays list of dcf overlays, format "object_index":"object_value" + * + * @return true, if dcf overlays set successfully, otherwise false (write error, read error + * or queried value differs from configured object value). + */ +bool sick_line_guidance::CanopenChain::writeDCFoverlays(ros::NodeHandle &nh, const std::string & node_id, XmlRpc::XmlRpcValue & dcf_overlays) +{ + // int32_t i32_val = 0; + // uint32_t u32_val = 0; + // float f32_val = 0; + // assert(tryParseUINT32("1", u32_val) && u32_val == 1); + // assert(tryParseUINT32("0x01", u32_val) && u32_val == 1); + // assert(tryParseUINT32("255", u32_val) && u32_val == 255); + // assert(tryParseUINT32("0xFF", u32_val) && u32_val == 0xFF); + // assert(tryParseUINT32("0xFFFFFFFF", u32_val) && u32_val == 0xFFFFFFFF); + // assert(tryParseINT32("1", i32_val) && i32_val == 1); + // assert(tryParseINT32("-1", i32_val) && i32_val == -1); + // assert(tryParseINT32("0x7FFFFFFF", i32_val) && i32_val == 0x7FFFFFFF); + // assert(tryParseINT32("0xFFFFFFFF", i32_val) && i32_val == -1); + // assert(tryParseFLOAT("1.0", f32_val) && std::fabs(f32_val - 1) < 1.0e-6); + // assert(tryParseFLOAT("-1.0", f32_val) && std::fabs(f32_val + 1) < 1.0e-6); + // assert(tryParseFLOAT("1e6", f32_val) && std::fabs(f32_val - 1.0E6) < 1.0e-6); + // assert(tryParseFLOAT("1.0E06", f32_val) && std::fabs(f32_val - 1.0E6) < 1.0e-6); + // assert(tryParseFLOAT("1e-6", f32_val) && std::fabs(f32_val - 1.0E-6) < 1.0e-6); + // assert(tryParseFLOAT("1.0E-06", f32_val) && std::fabs(f32_val - 1.0E-6) < 1.0e-6); + bool success = true; + for(XmlRpc::XmlRpcValue::iterator dcf_overlay_iter = dcf_overlays.begin(); dcf_overlay_iter != dcf_overlays.end(); ++dcf_overlay_iter) + { + bool dcf_success = true; + std::string dcf_overlay_object_index = dcf_overlay_iter->first; + std::string dcf_overlay_object_value = dcf_overlay_iter->second; + std::string sdo_message = "", sdo_response = ""; + // Set value in object dictionary + if(!setCanObjectValue(nh, node_id, dcf_overlay_object_index, dcf_overlay_object_value, sdo_response)) + { + dcf_success = false; + ROS_ERROR_STREAM("sick_line_guidance::CanopenChain::writeDCFoverlays(): setCanObjectValue(" << node_id << "," << dcf_overlay_object_index + << ") failed: sdo_response:\"" << sdoResponseToString(sdo_response) << "\"(\"" << sdo_response << "\")"); + sick_line_guidance::Diagnostic::update(sick_line_guidance::DIAGNOSTIC_STATUS::SDO_COMMUNICATION_ERROR, "writeDCFoverlays failed"); + } + // Re-read value in object dictionary + if(!queryCanObject(nh, node_id, dcf_overlay_object_index, 0, sdo_message, sdo_response)) + { + dcf_success = false; + ROS_ERROR_STREAM("sick_line_guidance::CanopenChain::writeDCFoverlays(): queryCanObject(" << node_id << "," << dcf_overlay_object_index + << ") failed: sdo_message:\"" << sdo_message << "\", sdo_response:\"" << sdoResponseToString(sdo_response) << "\"(\"" << sdo_response << "\")"); + sick_line_guidance::Diagnostic::update(sick_line_guidance::DIAGNOSTIC_STATUS::SDO_COMMUNICATION_ERROR, "writeDCFoverlays failed"); + } + else + { + ROS_INFO_STREAM("sick_line_guidance::CanopenChain::writeDCFoverlays(): queryCanObject(" << node_id << "," << dcf_overlay_object_index + << ") successfull re-read, sdo_response:\"" << sdoResponseToString(sdo_response) << "\"(\"" << sdo_response << "\")"); + } + // Compare destination value dcf_overlay_object_value with queried sdo response + if(!cmpDCFoverlay(dcf_overlay_object_value, sdo_response)) + { + ROS_WARN_STREAM("sick_line_guidance::CanopenChain::writeDCFoverlays(): dcf_overlay \"" << dcf_overlay_object_index<< "\": \"" + << dcf_overlay_object_value << "\" possibly failed: queried value=\"" << sdoResponseToString(sdo_response) << "\", expected value=\"" << dcf_overlay_object_value << "\""); + } + if(dcf_success) + { + ROS_INFO_STREAM("sick_line_guidance::CanopenChain::writeDCFoverlays(): dcf_overlay \"" << dcf_overlay_object_index<< "\": \"" + << dcf_overlay_object_value << "\" sucessfull, value=\"" << sdoResponseToString(sdo_response) << "\"(\"" << sdo_response << "\")"); + } + else + { + ROS_ERROR_STREAM("sick_line_guidance::CanopenChain::writeDCFoverlays(): dcf_overlay \"" << dcf_overlay_object_index<< "\": \"" + << dcf_overlay_object_value << "\" failed, value=\"" << sdoResponseToString(sdo_response) << "\"(\"" << sdo_response << "\")"); + } + success = dcf_success && success; + } + if(success) + sick_line_guidance::Diagnostic::update(sick_line_guidance::DIAGNOSTIC_STATUS::OK); + else + sick_line_guidance::Diagnostic::update(sick_line_guidance::DIAGNOSTIC_STATUS::SDO_COMMUNICATION_ERROR, "writeDCFoverlays failed"); + return success; +} + +/* + * Queries an object of a can node from canopen service by its object index. + * + * @param[in] nh ros::NodeHandle + * @param[in] node_id can node id of the can device + * @param[in] object index in the object dictonary of the can device, f.e. "1001sub", "1018sub4" + * @param[in] max_num_retries_after_sdo_error After SDO error, the query is repeated max. N times (default: N=2). If the SDO error persists, the can driver is shutdown and restarted. + * @param[out] output_message informational message in case of errors (responce from canopen service) + * @param[out] output_value value of the object (responce from canopen service) + * + * @return true, if query successful, otherwise false. + */ +bool sick_line_guidance::CanopenChain::queryCanObject(ros::NodeHandle &nh, + const std::string & node_id, const std::string & objectidx, int max_num_retries_after_sdo_error, + std::string & output_message, std::string & output_value) +{ + bool sdo_success = false; + for(int retry_cnt = 0; sdo_success == false; retry_cnt++) + { + try + { + output_message = ""; + output_value = ""; + ros::ServiceClient servclient = nh.serviceClient("/driver/get_object"); + canopen_chain_node::GetObject servobject; + servobject.request.node = node_id; + servobject.request.object = objectidx; + servobject.request.cached = false; + if(servclient.call(servobject)) + { + output_message = servobject.response.message; + output_value = servobject.response.value; + sdo_success = servobject.response.success; + if(!sdo_success) + { + ROS_ERROR_STREAM("sick_line_guidance::CanopenChain::queryCanObject(): ServiceClient::call(\"/driver/get_object\") failed: " << output_message); + } + } + else + { + ROS_ERROR_STREAM("sick_line_guidance::CanopenChain::queryCanObject(): ServiceClient::call(\"/driver/get_object\") failed."); + } + } + catch(const std::exception & exc) + { + sdo_success = false; + ROS_ERROR_STREAM("sick_line_guidance::CanopenChain::queryCanObject(" << node_id << "): [" << objectidx << "]=\"" << output_value << "\" failed: " << output_message << " exception = " << exc.what()); + } + if(!sdo_success) + { + // SDO failed + sick_line_guidance::Diagnostic::update(sick_line_guidance::DIAGNOSTIC_STATUS::SDO_COMMUNICATION_ERROR, "CanopenChain::queryCanObject failed"); + if (retry_cnt < max_num_retries_after_sdo_error) + { + ROS_ERROR_STREAM("sick_line_guidance::CanopenChain::queryCanObject(" << node_id << ") failed: SDO error, retrying..."); + continue; + } + else + { + // SDO failed, reset communication ("driver/recover") + // ROS_ERROR_STREAM("sick_line_guidance::CanopenChain::queryCanObject(\" << node_id << \"): resetting can communication after SDO error"); + // if(recoverCanDriver(nh)) + // ROS_INFO_STREAM("sick_line_guidance::CanopenChain::queryCanObject(\" << node_id << \"): recoverCanDriver() successfull."); + // else + // ROS_ERROR_STREAM("sick_line_guidance::CanopenChain::queryCanObject(\" << node_id << \"): recoverCanDriver() failed."); + // SDO failed, shutdown communication ("driver/shutdown") and re-initialize ("driver/init") + ROS_ERROR_STREAM("sick_line_guidance::CanopenChain::queryCanObject(" << node_id << "): unrecoverable SDO error, retries not successful, giving up, initiating can driver shutdown and restart after SDO error"); + if(shutdownCanDriver(nh)) + ROS_INFO_STREAM("sick_line_guidance::CanopenChain::queryCanObject(" << node_id << "): shutdownCanDriver() successfull."); + else + ROS_ERROR_STREAM("sick_line_guidance::CanopenChain::queryCanObject(" << node_id << "): shutdownCanDriver() failed."); + if(connectInitService(nh)) + ROS_INFO_STREAM("sick_line_guidance::CanopenChain::queryCanObject(" << node_id << "): connectInitService() successfull."); + else + ROS_ERROR_STREAM("sick_line_guidance::CanopenChain::queryCanObject(" << node_id << "): connectInitService() failed."); + break; + } + } + } + return sdo_success; +} + +/* + * Sets the value of an object in the object dictionary of a can device. + * + * @param[in] nh ros::NodeHandle + * @param[in] node_id can node id of the can device + * @param[in] object index in the object dictonary of the can device, f.e. "1001sub", "1018sub4" + * @param[in] object_value value of the object + * @param[out] response value of the object (responce from canopen service) + * + * @return true, if SDO successful, otherwise false. + */ +bool sick_line_guidance::CanopenChain::setCanObjectValue(ros::NodeHandle &nh, const std::string & node_id, const std::string & objectidx, + const std::string & object_value, std::string & response) +{ + bool sdo_success = false; + try + { + response = ""; + ros::ServiceClient servclient = nh.serviceClient("/driver/set_object"); + canopen_chain_node::SetObject servobject; + servobject.request.node = node_id; + servobject.request.object = objectidx; + servobject.request.value = object_value; + servobject.request.cached = false; + if(servclient.call(servobject)) + { + response = servobject.response.message; + sdo_success = servobject.response.success; + if(!sdo_success) + { + ROS_ERROR_STREAM("sick_line_guidance::CanopenChain::setCanObjectValue(): ServiceClient::call(\"/driver/set_object\") failed: " << response); + } + } + else + { + ROS_ERROR_STREAM("sick_line_guidance::CanopenChain::setCanObjectValue(): ServiceClient::call(\"/driver/set_object\") failed."); + } + } + catch(const std::exception & exc) + { + sdo_success = false; + ROS_ERROR_STREAM("sick_line_guidance::CanopenChain::setCanObjectValue(" << node_id << ", " << objectidx << ") failed: exception = " << exc.what()); + } + if(!sdo_success) + sick_line_guidance::Diagnostic::update(sick_line_guidance::DIAGNOSTIC_STATUS::SDO_COMMUNICATION_ERROR, "CanopenChain::setCanObjectValue failed"); + /* else + sick_line_guidance::Diagnostic::update(sick_line_guidance::DIAGNOSTIC_STATUS::OK); */ + return sdo_success; +} + +/* + * Recovers can driver after emergency or other errors. Calls "/driver/recover" from canopen_chain_node + * + * @param[in] nh ros::NodeHandle + * + * @return true, if recover command returned with success, otherwise false. + */ +bool sick_line_guidance::CanopenChain::recoverCanDriver(ros::NodeHandle & nh) +{ + bool success = false; + try + { + ROS_WARN_STREAM("sick_line_guidance::CanopenChain::recoverCanDriver(): resetting can communication by ServiceClient::call(\"/driver/recover\")..."); + ros::ServiceClient servclient = nh.serviceClient("/driver/recover"); + std_srvs::Trigger servobject; + if(servclient.call(servobject)) + { + success = servobject.response.success; + if(success) + { + ROS_INFO_STREAM("sick_line_guidance::CanopenChain::recoverCanDriver(): ServiceClient::call(\"/driver/recover\") successfull " << servobject.response.message.c_str()); + } + else + { + ROS_ERROR_STREAM("sick_line_guidance::CanopenChain::recoverCanDriver(): ServiceClient::call(\"/driver/recover\") failed: " << servobject.response.message.c_str()); + } + } + else + { + ROS_ERROR_STREAM("sick_line_guidance::CanopenChain::recoverCanDriver(): ServiceClient::call(\"/driver/recover\") failed."); + } + } + catch(const std::exception & exc) + { + success = false; + ROS_ERROR_STREAM("sick_line_guidance::CanopenChain::recoverCanDriver() failed: exception = " << exc.what()); + } + if(success) + sick_line_guidance::Diagnostic::update(sick_line_guidance::DIAGNOSTIC_STATUS::OK); + else + sick_line_guidance::Diagnostic::update(sick_line_guidance::DIAGNOSTIC_STATUS::SDO_COMMUNICATION_ERROR, "CanopenChain::recoverCanDriver failed"); + return success; +} + +/* + * Shutdown can driver. Calls "/driver/shutdown" from canopen_chain_node + * + * @param[in] nh ros::NodeHandle + * + * @return true, if shutdown command returned with success, otherwise false. + */ +bool sick_line_guidance::CanopenChain::shutdownCanDriver(ros::NodeHandle & nh) +{ + bool success = false; + try + { + ROS_WARN_STREAM("sick_line_guidance::CanopenChain::shutdownCanDriver(): shutdown can communication by ServiceClient::call(\"/driver/shutdown\")..."); + ros::ServiceClient servclient = nh.serviceClient("/driver/shutdown"); + std_srvs::Trigger servobject; + if(servclient.call(servobject)) + { + success = servobject.response.success; + if(success) + { + ROS_INFO_STREAM("sick_line_guidance::CanopenChain::shutdownCanDriver(): ServiceClient::call(\"/driver/shutdown\") successfull " << servobject.response.message.c_str()); + } + else + { + ROS_ERROR_STREAM("sick_line_guidance::CanopenChain::shutdownCanDriver(): ServiceClient::call(\"/driver/shutdown\") failed: " << servobject.response.message.c_str()); + } + } + else + { + ROS_ERROR_STREAM("sick_line_guidance::CanopenChain::shutdownCanDriver(): ServiceClient::call(\"/driver/shutdown\") failed."); + } + } + catch(const std::exception & exc) + { + success = false; + ROS_ERROR_STREAM("sick_line_guidance::CanopenChain::shutdownCanDriver() failed: exception = " << exc.what()); + } + return success; +} + +/* + * Compares a destination value configured by dcf_overlay with the sdo response. + * Comparison by string, integer or float comparison. + * Returns true, if both values are identical, or otherwise false. + * Note: dcf_overlay_value and sdo_response may represent identical numbers, but different strings (different number formatting) + * Therefor, both string and numbers are compared in this function. + * + * @param[in] dcf_overlay_value configured value + * @param[in] sdo_response received value + * + * @return true, if dcf_overlay_value is identical to sdo_response or has identical values. + */ +bool sick_line_guidance::CanopenChain::cmpDCFoverlay(const std::string & dcf_overlay_value, const std::string & sdo_response) +{ + // compare strings + if(dcf_overlay_value == sdo_response) + return true; + float f_dcf_val = 0, f_sdo_val = 0; + int32_t i32_dcf_val = 0, i32_sdo_val = 0; + uint32_t u32_dcf_val = 0, u32_sdo_val = 0; + // try UINT8 comparison (1 byte sdo response) + if(sdo_response.empty()) + return tryParseUINT32(dcf_overlay_value, u32_dcf_val) && u32_dcf_val == 0; + if(sdo_response.size() == 1) + return tryParseUINT32(dcf_overlay_value, u32_dcf_val) && u32_dcf_val == ((static_cast(sdo_response[0])) & 0xFF); + // try UINT32, INT32 and FLOAT comparison (2, 3, or 4 byte sdo response) + return (tryParseUINT32(dcf_overlay_value, u32_dcf_val) && tryParseUINT32(sdo_response, u32_sdo_val) && u32_dcf_val == u32_sdo_val) // identical uint32 + || (tryParseINT32(dcf_overlay_value, i32_dcf_val) && tryParseINT32(sdo_response, i32_sdo_val) && i32_dcf_val == i32_sdo_val) // identical int32 + || (tryParseFLOAT(dcf_overlay_value, f_dcf_val) && tryParseFLOAT(sdo_response, f_sdo_val) && std::fabs(f_dcf_val - f_sdo_val) < 0.001); // float diff < 1 mm (OLS/MLS resolution) +} + +/* + * Converts a string to UINT32. Catches conversion exceptions and returns false in case of number parsing errors. + * + * @param[in] str input string to be parsed + * @param[out] value output value (== input converted to number in case of success) + * + * @return true in case of success, false otherwise + */ +uint32_t sick_line_guidance::CanopenChain::tryParseUINT32(const std::string & str, uint32_t & value) +{ + bool success = false; + try + { + value = std::stoul(str, 0, 0); + success = true; + } + catch(const std::exception & exc) + { + success = false; + } + return success; +} + +/* + * Converts a string to INT32. Catches conversion exceptions and returns false in case of number parsing errors. + * + * @param[in] str input string to be parsed + * @param[out] value output value (== input converted to number in case of success) + * + * @return true in case of success, false otherwise + */ +int32_t sick_line_guidance::CanopenChain::tryParseINT32(const std::string & str, int32_t & value) +{ + bool success = false; + try + { + value = std::stol(str, 0, 0); + success = true; + } + catch(const std::exception & exc) + { + success = false; + } + return success; +} + +/* + * Converts a string to FLOAT32. Catches conversion exceptions and returns false in case of number parsing errors. + * + * @param[in] str input string to be parsed + * @param[out] value output value (== input converted to number in case of success) + * + * @return true in case of success, false otherwise + */ +int32_t sick_line_guidance::CanopenChain::tryParseFLOAT(const std::string & str, float & value) +{ + bool success = false; + try + { + value = std::stof(str); + success = true; + } + catch(const std::exception & exc) + { + success = false; + } + return success; +} + +/* + * Prints and converts a sdo response. 1 byte datatypes (UINT8) are converted to decimal (f.e. "\0" is returned as "0"). + * + * @param[in] response sdo response + * + * @return printed response. + */ +std::string sick_line_guidance::CanopenChain::sdoResponseToString(const std::string & response) +{ + if(response.empty()) + return "0"; + if(response.size() == 1) + { + uint32_t value = static_cast((static_cast(response[0])) & 0xFF); + std::stringstream str; + str << value; + return str.str(); + } + return response; +} diff --git a/Devices/Packages/sick_line_guidance/src/sick_line_guidance_cloud_converter.cpp b/Devices/Packages/sick_line_guidance/src/sick_line_guidance_cloud_converter.cpp new file mode 100755 index 0000000..26ba90a --- /dev/null +++ b/Devices/Packages/sick_line_guidance/src/sick_line_guidance_cloud_converter.cpp @@ -0,0 +1,413 @@ +/* + * class CloudConverter implements utility functions to convert measurement data to PointCloud2. + * + * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim + * Copyright (C) 2019 SICK AG, Waldkirch + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of SICK AG nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Authors: + * Michael Lehning + * + * Copyright 2019 SICK AG + * Copyright 2019 Ing.-Buero Dr. Michael Lehning + * + */ +#include +#include +#include +#include +#include + +#include "sick_line_guidance/sick_line_guidance_cloud_converter.h" +#include "sick_line_guidance/sick_line_guidance_msg_util.h" + +/* + * @brief returns the status for each line (detected or not detected). + * + * @param[in] status status byte of measurement data + * @param[in] numlines number of lined (normally 3 lines) + * + * @return detection status for each line + */ +std::vector sick_line_guidance::CloudConverter::linestatus(uint8_t status, size_t numlines) +{ + std::vector lines_valid; + numlines = std::max((size_t)3,numlines); + lines_valid.resize(numlines); + // Spezifikation OLS+MLS: Es gilt folgende Zuordnung: + // 0=000b => keine Spur gefunden + // 2=010b => eine Spur gefunden + // 3=011b => zwei Spuren gefunden: Weiche links + // 6=110b => zwei Spuren gefunden: Weiche rechts + // 7=111b => drei Spuren gefunden + lines_valid[0] = ((status & 0x01) > 0); + lines_valid[1] = ((status & 0x02) > 0); + lines_valid[2] = ((status & 0x04) > 0); + for(size_t n = 3; n < numlines; n++) + lines_valid[n] = false; + return lines_valid; +} + +/* + * @brief returns the number of lines detected (1, 2 or 3 lines). + * + * @param[in] status status byte of measurement data + * + * @return number of lines detected by OLS or MLS + */ +int sick_line_guidance::CloudConverter::linenumber(uint8_t status) +{ + std::vector lines_valid = linestatus(status, 3); + int linecnt = 0; + for(size_t n = 0; n < lines_valid.size(); n++) + { + if(lines_valid[n]) + linecnt++; + } + return linecnt; +} + +/* + * @brief returns true, if MLS measurement status is okay, or false otherwise. + * + * @param[in] measurement MLS measurement data + * + * @return true (measurement status okay), or false (measurement status not okay) + */ +bool sick_line_guidance::CloudConverter::measurementstatus(const sick_line_guidance::MLS_Measurement & measurement) +{ + // Spezifikation status bit 0 MLS ("Line good"): + // 0 => keine Spur oder Spur zu schwach + // 1 => ausreichend starke Spur erkannt + return ((measurement.status & 0x1) != 0); +} + +/* + * @brief converts OLS measurement data to PointCloud2. + * + * @param[in] measurement OLS measurement data + * @param[in]frame_id frame_id of PointCloud2 message + * + * @return sensor_msgs::PointCloud2 data converted from measurement + */ +sensor_msgs::PointCloud2 sick_line_guidance::CloudConverter::convert(const sick_line_guidance::OLS_Measurement &measurement, const std::string & frame_id) +{ + ROS_DEBUG("CloudConverter::convert(OLS_Measurement)"); + assert(sizeof(float) == sizeof(uint32_t)); + // #ifdef DEBUG + // assert(flipBits(0x04030201) == 0x8040C020); + // assert(flipBits(0xFFAA5500) == 0x00AA55FF); + // #endif + sensor_msgs::PointCloud2 cloud; + // set header + cloud.header.stamp = measurement.header.stamp; // timestamp of measurement + cloud.header.frame_id = frame_id; + cloud.header.seq = 0; + // clear cloud data + cloud.height = 0; + cloud.width = 0; + cloud.data.clear(); + // set data header + bool sensorOkay = sick_line_guidance::MsgUtil::statusOK(measurement); + int numMeasuredLines = linenumber(measurement.status); + if(!sensorOkay || numMeasuredLines < 1) // no lines detected + return cloud; // return empty PointCloud2 + int numChannels = 12; // "x", "y", "z", "linewidth", "lineidx", "barcode", "status", "dev_status", "error", "barcode_center", "line_quality", "line_intensity" + std::string channelId[] = { "x", "y", "z", "linewidth", "lineidx", "barcode", "status", "dev_status", "error", "barcode_center", "line_quality", "line_intensity" }; + cloud.height = 1; + cloud.width = numMeasuredLines; // normally we have 3 positions (3 lines, one position for each line) + cloud.is_bigendian = false; + cloud.is_dense = true; + cloud.point_step = numChannels * sizeof(float); + cloud.row_step = cloud.point_step * cloud.width; + cloud.fields.resize(numChannels); + for (int i = 0; i < numChannels; i++) + { + cloud.fields[i].name = channelId[i]; + cloud.fields[i].offset = i * sizeof(float); + cloud.fields[i].count = 1; + } + for (int i = 0; i < numChannels; i++) + { + cloud.fields[i].datatype = sensor_msgs::PointField::UINT32; // default: datatype UINT32 + } + cloud.fields[0].datatype = sensor_msgs::PointField::FLOAT32; // "x" + cloud.fields[1].datatype = sensor_msgs::PointField::FLOAT32; // "y" + cloud.fields[2].datatype = sensor_msgs::PointField::FLOAT32; // "z" + cloud.fields[3].datatype = sensor_msgs::PointField::FLOAT32; // "linewidth" + cloud.fields[9].datatype = sensor_msgs::PointField::FLOAT32; // "barcode_center" + // get barcode: barcode valid, if bit 7 of measurement.status is set + uint32_t barcode = 0; + if((measurement.status & 0x80) != 0) + { + barcode = ((measurement.barcode < 255) ? (measurement.barcode) : (measurement.extended_code)); + } + // if((measurement.status & 0x40) != 0) // barcode flipped, if bit 6 of measurement.status is set) + // { + // uint32_t flipped_barcode = barcode; + // barcode = flipBits(flipped_barcode); + // #ifdef DEBUG + // assert(flipBits(barcode) == flipped_barcode); + // #endif + // } + // set data values + cloud.data.resize(cloud.row_step * cloud.height); + float* pfdata = reinterpret_cast(&cloud.data[0]); + uint32_t* pidata = reinterpret_cast(&cloud.data[0]); + std::vector lines_valid = linestatus(measurement.status, measurement.position.size()); + for(size_t cloudIdx = 0, meaIdx = 0; meaIdx < measurement.position.size(); meaIdx++) + { + if(lines_valid[meaIdx]) + { + pfdata[cloudIdx++] = 0; // "x" := 0 + pfdata[cloudIdx++] = measurement.position[meaIdx]; // "y" := measurement.position[i] + pfdata[cloudIdx++] = 0; // "z" := 0 + pfdata[cloudIdx++] = measurement.width[meaIdx]; // "linewidth" := measurement.width[i] + pidata[cloudIdx++] = meaIdx + 1; // "lineidx": 1:=LCP1, 2:=LCP2, 3:=LCP3 + pidata[cloudIdx++] = barcode; // "barcode" + pidata[cloudIdx++] = measurement.status; // "status" + pidata[cloudIdx++] = measurement.dev_status; // "dev_status" + pidata[cloudIdx++] = measurement.error; // "error" + pfdata[cloudIdx++] = measurement.barcode_center_point; // "barcode_center" (OLS20 only, OLS10: always 0) + pidata[cloudIdx++] = measurement.quality_of_lines; // "line_quality" (OLS20 only, OLS10: always 0) + pidata[cloudIdx++] = measurement.intensity_of_lines[meaIdx]; // "line_intensity" (OLS20 only, OLS10: always 0) + } + } + ROS_DEBUG("CloudConverter::convert(OLS_Measurement): sensorOkay=%d, numMeasuredLines=%d, numChannels=%d, numBytes=%d", + (int)sensorOkay, numMeasuredLines, numChannels, (int)cloud.data.size()); + return cloud; +} + +/* +* @brief converts OLS measurement data to PointCloud2. +* +* @param[in] measurement OLS measurement data +* @param[in]frame_id frame_id of PointCloud2 message +* +* @return sensor_msgs::PointCloud2 data converted from measurement +*/ +sensor_msgs::PointCloud2 sick_line_guidance::CloudConverter::convert(const sick_line_guidance::MLS_Measurement &measurement, const std::string & frame_id) +{ + ROS_DEBUG("CloudConverter::convert(MLS_Measurement)"); + assert(sizeof(float) == sizeof(uint32_t)); + sensor_msgs::PointCloud2 cloud; + // set header + cloud.header.stamp = measurement.header.stamp; // timestamp of measurement + cloud.header.frame_id = frame_id; + cloud.header.seq = 0; + // clear cloud data + cloud.height = 0; + cloud.width = 0; + cloud.data.clear(); + // set data header + bool sensorOkay = measurementstatus(measurement); + int numMeasuredLines = linenumber(measurement.lcp); + if(!sensorOkay || numMeasuredLines < 1) // no lines detected + return cloud; // return empty PointCloud2 + int numChannels = 8; // "x", "y", "z", "lineidx", "barcode", "lcp", "status", "error" + std::string channelId[] = { "x", "y", "z", "lineidx", "marker", "lcp", "status", "error" }; + cloud.height = 1; + cloud.width = numMeasuredLines; // normally we have 3 positions (3 lines, one position for each line) + cloud.is_bigendian = false; + cloud.is_dense = true; + cloud.point_step = numChannels * sizeof(float); // length of a point in bytes + cloud.row_step = cloud.point_step * cloud.width; // length of a row in bytes + cloud.fields.resize(numChannels); + for (int i = 0; i < numChannels; i++) + { + cloud.fields[i].name = channelId[i]; + cloud.fields[i].offset = i * sizeof(float); + cloud.fields[i].count = 1; + } + for (int i = 0; i < 3; i++) + { + cloud.fields[i].datatype = sensor_msgs::PointField::FLOAT32; // "x", "y", "z" + } + cloud.fields[3].datatype = sensor_msgs::PointField::UINT32;// "lineidx" + cloud.fields[4].datatype = sensor_msgs::PointField::INT32;// "marker" + for (int i = 5; i < numChannels; i++) + { + cloud.fields[i].datatype = sensor_msgs::PointField::UINT32; // "lcp", "status", "error" + } + // get marker: marker valid, if bit 6 of measurement.status is set + int32_t marker = 0; + if((measurement.status & 0x40) != 0) + { + marker = ((measurement.lcp >> 4) & 0x0F); // Bit 4 - bit 7 of lcp: marker bit + if((measurement.lcp & 0x08) != 0) // // Bit 3 of lcp: sign bit of marker + marker = -marker; + } + // set data values + cloud.data.resize(cloud.row_step * cloud.height); + float* pfdata = reinterpret_cast(&cloud.data[0]); + uint32_t* pidata = reinterpret_cast(&cloud.data[0]); + std::vector lines_valid = linestatus(measurement.lcp, measurement.position.size()); + for(size_t cloudIdx = 0, meaIdx = 0; meaIdx < measurement.position.size(); meaIdx++) + { + if(lines_valid[meaIdx]) + { + pfdata[cloudIdx++] = 0; // "x" := 0 + pfdata[cloudIdx++] = measurement.position[meaIdx]; // "y" := measurement.position[i] + pfdata[cloudIdx++] = 0; // "z" := 0 + pidata[cloudIdx++] = meaIdx + 1; // "lineidx": 1:=LCP1, 2:=LCP2, 3:=LCP3 + pidata[cloudIdx++] = marker; // "marker" + pidata[cloudIdx++] = measurement.lcp; // "lcp" + pidata[cloudIdx++] = measurement.status; // "status" + pidata[cloudIdx++] = measurement.error; // "error" + } + } + ROS_DEBUG("CloudConverter::convert(MLS_Measurement): sensorOkay=%d, numMeasuredLines=%d, numPositions=%d, numChannels=%d, numBytes=%d", + (int)sensorOkay, numMeasuredLines, (int)measurement.position.size(), numChannels, (int)cloud.data.size()); + return cloud; +} + +template static std::string convertCloudDataElement(const uint8_t* data, const uint8_t* end, bool printhex) +{ + std::stringstream out; + const T* pElement = reinterpret_cast(data); + if(data + sizeof(*pElement) <= end) + { + if(printhex) + { + out << "0x" << std::uppercase << std::hex << std::setfill('0') << std::setw(2*sizeof(*pElement)) << (uint32_t)(*pElement); + } + else + { + out << (double)(*pElement); + } + data += sizeof(*pElement); + } + return out.str(); +} + +/* + * @brief converts and prints a single field of PointCloud2 according to its dataype. + * + * @param[in] datatype enumeration of the datatye, see sensor_msgs::PointField + * @param[in] data pointer to the data to be converted and printed + * @param[in] end pointer to the end of PointCloud2 data + * + * @return data field converted to string + */ +std::string sick_line_guidance::CloudConverter::cloudDataFieldToString(uint8_t datatype, const uint8_t* data, const uint8_t* end) +{ + switch(datatype) + { + case sensor_msgs::PointField::INT8: + return convertCloudDataElement(data, end, true); + case sensor_msgs::PointField::UINT8: + return convertCloudDataElement(data, end, true); + case sensor_msgs::PointField::INT16: + return convertCloudDataElement(data, end, true); + case sensor_msgs::PointField::UINT16: + return convertCloudDataElement(data, end, true); + case sensor_msgs::PointField::INT32: + return convertCloudDataElement(data, end, true); + case sensor_msgs::PointField::UINT32: + return convertCloudDataElement(data, end, true); + case sensor_msgs::PointField::FLOAT32: + return convertCloudDataElement(data, end, false); + case sensor_msgs::PointField::FLOAT64: + return convertCloudDataElement(data, end, false); + default: + break; + } + return ""; +} + +/* +* @brief prints a PointCloud2 data field to string. +* +* @param[in] cloud PointCloud2 data +* +* @return PointCloud2 data converted string +*/ +std::string sick_line_guidance::CloudConverter::cloudDataToString(const sensor_msgs::PointCloud2 & cloud) +{ + std::stringstream out; + size_t numChannels = cloud.fields.size(); + size_t numDataBytes = cloud.data.size(); + const uint8_t* pCloudData = reinterpret_cast(&cloud.data[0]); + const uint8_t* pCloudLast = &pCloudData[numDataBytes]; + ROS_DEBUG("CloudConverter::cloudDataToString(): cloud.height=%d, cloud.width=%d, numChannels=%d, numDataBytes=%d",(int)cloud.height, (int)cloud.width, (int)numChannels, (int)numDataBytes); + if(cloud.height > 0 && cloud.width > 0 && numChannels > 0 && numDataBytes > 0 && pCloudData != 0) + { + for(size_t y = 0; y < cloud.height; y++) + { + for (size_t x = 0; x < cloud.width; x++) + { + const uint8_t* pFieldData = pCloudData + y * cloud.row_step + x * cloud.point_step; + for (size_t n = 0; n < numChannels && pFieldData < pCloudLast; n++) + { + out << cloud.fields[n].name << ":"; + const uint8_t* pChannelData = pFieldData + cloud.fields[n].offset; + for (size_t m = 0; m < cloud.fields[n].count && pChannelData < pCloudLast; m++) + { + std::string element = cloudDataFieldToString(cloud.fields[n].datatype, pChannelData, pCloudLast); + out << element << ","; + } + } + } + } + } + return out.str(); +} + +/* + * @brief flips the bits of a code, i.e. reverses the bits (output bit 0 := input bit 31, output bit 1 := input bit 30, and so on) + * + * @param[in] code bits to be flipped + * + * @return flipped code + */ +uint32_t sick_line_guidance::CloudConverter::flipBits(uint32_t code) +{ + uint32_t flipped = 0; + for(uint32_t shift = 0; shift < 32; shift++) + { + flipped = (flipped << 1); + flipped = flipped | (code & 0x1); + code = code >> 1; + } + return flipped; +} diff --git a/Devices/Packages/sick_line_guidance/src/sick_line_guidance_cloud_publisher.cpp b/Devices/Packages/sick_line_guidance/src/sick_line_guidance_cloud_publisher.cpp new file mode 100755 index 0000000..3c8ad71 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/src/sick_line_guidance_cloud_publisher.cpp @@ -0,0 +1,162 @@ +/* + * sick_line_guidance_cloud_publisher implements a ros-node to convert sensor measurement data to PointCloud2 data. + * Subscribes topics "/ols" and "/mls", converts the sensor data and publishes sensor positions on topic "/cloud". + * + * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim + * Copyright (C) 2019 SICK AG, Waldkirch + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of SICK AG nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Authors: + * Michael Lehning + * + * Copyright 2019 SICK AG + * Copyright 2019 Ing.-Buero Dr. Michael Lehning + * + */ +#include +#include +#include + +#include "sick_line_guidance/sick_line_guidance_version.h" +#include "sick_line_guidance/sick_line_guidance_cloud_converter.h" +#include "sick_line_guidance/sick_line_guidance_msg_util.h" + +ros::Publisher cloud_publisher; +std::string mls_cloud_frame_id ="mls_frame"; +std::string ols_cloud_frame_id ="ols_frame"; + +/* + * Callback for OLS measurement messages (topic ("/ols"). + * Converts the sensor data of type OLS_Measurement and publishes a PointCloud2 message on topic "/cloud". + * + * @param[in] msg OLS measurement messages (topic ("/ols") + */ +void olsMessageCb(const boost::shared_ptr& msg) +{ + if(msg) + { + sensor_msgs::PointCloud2 cloud = sick_line_guidance::CloudConverter::convert(*msg, ols_cloud_frame_id); + if(!cloud.data.empty()) + { + cloud_publisher.publish(cloud); + std::string cloudmsg = sick_line_guidance::CloudConverter::cloudDataToString(cloud); + ROS_INFO("sick_line_guidance_cloud_publisher: OLS PointCloud data: {%s}", cloudmsg.c_str()); + } + else + { + ROS_INFO("sick_line_guidance_cloud_publisher: invalid OLS measurement data, no line detected."); + } + } + else + { + ROS_ERROR("## ERROR sick_line_guidance_cloud_publisher::olsMessageCb(): invalid message (%s:%d)", __FILE__, __LINE__); + } +} + +/* + * Callback for MLS measurement messages (topic ("/mls"). + * Converts the sensor data of type MLS_Measurement and publishes a PointCloud2 message on topic "/cloud". + * + * @param[in] msg MLS measurement messages (topic ("/mls") + */ +void mlsMessageCb(const boost::shared_ptr& msg) +{ + if(msg) + { + sensor_msgs::PointCloud2 cloud = sick_line_guidance::CloudConverter::convert(*msg, mls_cloud_frame_id); + if(!cloud.data.empty()) + { + cloud_publisher.publish(cloud); + std::string cloudmsg = sick_line_guidance::CloudConverter::cloudDataToString(cloud); + ROS_INFO("sick_line_guidance_cloud_publisher: MLS PointCloud data: {%s}", cloudmsg.c_str()); + } + else + { + ROS_INFO("sick_line_guidance_cloud_publisher: invalid MLS measurement data, no line detected."); + } + } + else + { + ROS_ERROR("## ERROR sick_line_guidance_cloud_publisher::mlsMessageCb(): invalid message (%s:%d)", __FILE__, __LINE__); + } +} + +/* + * sick_line_guidance_cloud_publisher implements a ros-node to convert sensor measurement data to PointCloud2 data. + * Subscribes topics "/ols" and "/mls", converts the sensor data and publishes sensor positions on topic "/cloud". + */ +int main(int argc, char** argv) +{ + ros::init(argc, argv, "sick_line_guidance_cloud_publisher"); + ros::NodeHandle nh; + ros::NodeHandle nh_priv("~"); + + int subscribe_queue_size = 1; + std::string ols_topic_publish = "ols", mls_topic_publish = "mls", cloud_topic_publish = "cloud"; + nh_priv.param("mls_topic_publish", mls_topic_publish, mls_topic_publish); + nh_priv.param("ols_topic_publish", ols_topic_publish, ols_topic_publish); + nh_priv.param("cloud_topic_publish", cloud_topic_publish, cloud_topic_publish); + nh_priv.param("mls_cloud_frame_id", mls_cloud_frame_id, mls_cloud_frame_id); + nh_priv.param("ols_cloud_frame_id", ols_cloud_frame_id, ols_cloud_frame_id); + nh_priv.param("subscribe_queue_size", subscribe_queue_size, subscribe_queue_size); // buffer size for ros messages + + + ROS_INFO_STREAM("sick_line_guidance_cloud_publisher: version " << sick_line_guidance::Version::getVersionInfo()); + ROS_INFO_STREAM("sick_line_guidance_cloud_publisher started."); + ROS_INFO_STREAM("sick_line_guidance_cloud_publisher: mls_topic=" << mls_topic_publish); + ROS_INFO_STREAM("sick_line_guidance_cloud_publisher: ols_topic=" << ols_topic_publish); + ROS_INFO_STREAM("sick_line_guidance_cloud_publisher: cloud_topic=" << cloud_topic_publish); + ROS_INFO_STREAM("sick_line_guidance_cloud_publisher: mls_frame_id=" << mls_cloud_frame_id); + ROS_INFO_STREAM("sick_line_guidance_cloud_publisher: ols_frame_id=" << ols_cloud_frame_id); + ROS_INFO_STREAM("sick_line_guidance_cloud_publisher: queue_size=" << subscribe_queue_size); + cloud_publisher = nh.advertise(cloud_topic_publish, 1); + ros::Subscriber ols_subscriber = nh.subscribe(ols_topic_publish, subscribe_queue_size, olsMessageCb); + ros::Subscriber mls_subscriber = nh.subscribe(mls_topic_publish, subscribe_queue_size, mlsMessageCb); + + ros::spin(); + + std::cout << "sick_line_guidance_cloud_publisher finished." << std::endl; + ROS_INFO_STREAM("sick_line_guidance_cloud_publisher finished."); + return 0; +} + diff --git a/Devices/Packages/sick_line_guidance/src/sick_line_guidance_diagnostic.cpp b/Devices/Packages/sick_line_guidance/src/sick_line_guidance_diagnostic.cpp new file mode 100755 index 0000000..9679d17 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/src/sick_line_guidance_diagnostic.cpp @@ -0,0 +1,134 @@ +/* + * sick_line_guidance_diagnostic publishes diagnostic messages for sick_line_guidance + * + * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim + * Copyright (C) 2019 SICK AG, Waldkirch + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of SICK AG nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Authors: + * Michael Lehning + * + * Copyright 2019 SICK AG + * Copyright 2019 Ing.-Buero Dr. Michael Lehning + * + */ +#include +#include +#include +#include "sick_line_guidance/sick_line_guidance_diagnostic.h" + +/* + * g_diagnostic_handler: singleton, implements the diagnostics for sick_line_guidance + */ +sick_line_guidance::Diagnostic::DiagnosticImpl sick_line_guidance::Diagnostic::g_diagnostic_handler; + +/* + * Constructor. + */ +sick_line_guidance::Diagnostic::DiagnosticImpl::DiagnosticImpl() : m_diagnostic_initialized(false), m_diagnostic_component("") +{ +} + +/* + * Initialization. + * + * @param[in] nh ros::NodeHandle + * @param[in] publish_topic ros topic to publish diagnostic messages + * @param[in] component description of the component reporting + */ +void sick_line_guidance::Diagnostic::DiagnosticImpl::init(ros::NodeHandle &nh, const std::string & publish_topic, const std::string & component) +{ + m_diagnostic_publisher = nh.advertise(publish_topic, 1); + m_diagnostic_component = component; + m_diagnostic_initialized = true; +} + +/* + * Updates and reports the current status. + * + * @param[in] status current status (OK, ERROR, ...) + * @param[in] message optional diagnostic message + */ +void sick_line_guidance::Diagnostic::DiagnosticImpl::update(DIAGNOSTIC_STATUS status, const std::string &message) +{ + if (m_diagnostic_initialized) + { + static std::map status_description = { + {DIAGNOSTIC_STATUS::OK, "OK"}, + {DIAGNOSTIC_STATUS::EXIT, "EXIT"}, + {DIAGNOSTIC_STATUS::NO_LINE_DETECTED, "NO_LINE_DETECTED"}, + {DIAGNOSTIC_STATUS::ERROR_STATUS, "ERROR_STATUS"}, + {DIAGNOSTIC_STATUS::SDO_COMMUNICATION_ERROR, "SDO_COMMUNICATION_ERROR"}, + {DIAGNOSTIC_STATUS::CAN_COMMUNICATION_ERROR, "CAN_COMMUNICATION_ERROR"}, + {DIAGNOSTIC_STATUS::CONFIGURATION_ERROR, "CONFIGURATION_ERROR"}, + {DIAGNOSTIC_STATUS::INITIALIZATION_ERROR, "INITIALIZATION_ERROR"}, + {DIAGNOSTIC_STATUS::INTERNAL_ERROR, "INTERNAL_ERROR"} + }; + + + // create DiagnosticStatus + diagnostic_msgs::DiagnosticStatus msg; + msg.level = (status == DIAGNOSTIC_STATUS::OK) ? (diagnostic_msgs::DiagnosticStatus::OK) : (diagnostic_msgs::DiagnosticStatus::ERROR); // Level of operation + msg.name = m_diagnostic_component; // description of the test/component reporting + msg.hardware_id = ""; // hardware unique string (tbd) + msg.values.clear(); // array of values associated with the status + // description of the status + msg.message = status_description[status]; + if(msg.message.empty()) + { + msg.message = "ERROR"; + } + if (!message.empty()) + { + msg.message = msg.message + ": " + message; + } + // publish DiagnosticStatus in DiagnosticArray + diagnostic_msgs::DiagnosticArray msg_array; + msg_array.header.stamp = ros::Time::now(); + msg_array.header.frame_id = ""; + msg_array.status.clear(); + msg_array.status.push_back(msg); + m_diagnostic_publisher.publish(msg_array); + } +} + diff --git a/Devices/Packages/sick_line_guidance/src/sick_line_guidance_eds_util.cpp b/Devices/Packages/sick_line_guidance/src/sick_line_guidance_eds_util.cpp new file mode 100755 index 0000000..f6cc4d3 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/src/sick_line_guidance_eds_util.cpp @@ -0,0 +1,170 @@ +/* + * class EdsUtil implements utility functions for eds-files. + * + * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim + * Copyright (C) 2019 SICK AG, Waldkirch + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of SICK AG nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Authors: + * Michael Lehning + * + * Copyright 2019 SICK AG + * Copyright 2019 Ing.-Buero Dr. Michael Lehning + * + */ +#include +#include +#include +#include +#include "sick_line_guidance/sick_line_guidance_eds_util.h" + +/* + * @brief returns a full qualified filepath from package name and file name. + * + * @param package package name + * + * @return full qualified filepath (or filename if package is empty or couldn't be found). + */ +std::string sick_line_guidance::EdsUtil::filepathFromPackage(const std::string & package, const std::string & filename) +{ + std::string filepath = filename; + if(!package.empty()) + { + std::string pkg_path = ros::package::getPath(package); + if(!pkg_path.empty()) + { + filepath = (boost::filesystem::path(pkg_path)/filepath).make_preferred().native(); + } + } + return filepath; +} + +/* + * @brief reads and parses an eds-file and prints all objects. + * + * @param eds_filepath path to eds-file + * + * @return object dictionary of a given eds-file printed to string + */ +std::string sick_line_guidance::EdsUtil::readParsePrintEdsfile(const std::string & eds_filepath) +{ + std::stringstream object_dictionary_msg; + try + { + // ROS_DEBUG("sick_line_guidance: object_dictionary from eds_file \"%s\":", eds_filepath.c_str()); + canopen::ObjectDict::Overlay object_overlay; + canopen::ObjectDictSharedPtr object_dictionary = canopen::ObjectDict::fromFile(eds_filepath, object_overlay); + canopen::ObjectDict::ObjectDictMap::const_iterator object_dictionary_iter; + while (object_dictionary->iterate(object_dictionary_iter)) + { + const canopen::ObjectDict::Key &key = object_dictionary_iter->first; + const canopen::ObjectDict::Entry &entry = *object_dictionary_iter->second; + object_dictionary_msg << key << "=(" << printObjectDictEntry(entry) << ")\n"; + } + // ROS_DEBUG("%s", object_dictionary_msg.str().c_str()); + } + catch (const canopen::ParseException & err) + { + std::stringstream err_msg; + err_msg << "sick_line_guidance::readParsePrintEdsfile(" << eds_filepath << ") failed: " << err.what(); + ROS_ERROR("%s", err_msg.str().c_str()); + } + return object_dictionary_msg.str(); +} + +/* + * @brief prints the data of a canopen::ObjectDict::Entry value to std::string. + * + * @param entry object dictionary entry to be printed + * + * @return entry converted to string. + */ +std::string sick_line_guidance::EdsUtil::printObjectDictEntry(const canopen::ObjectDict::Entry & entry) +{ + std::stringstream msg; + msg << "desc:" << entry.desc << ","; + msg << "data_type:" << (int)(entry.data_type) << ","; + msg << "index:" << canopen::ObjectDict::Key(entry.index, entry.sub_index) << ","; + msg << "constant:" << entry.constant << ","; + msg << "readable:" << entry.readable << ","; + msg << "writable:" << entry.writable << ","; + msg << "mappable:" << entry.mappable << ","; + msg << "obj_code:" << (int)(entry.obj_code) << ","; + msg << printHoldAny("value", entry.value()) << ","; + msg << printHoldAny("def_val", entry.def_val) << ","; + msg << printHoldAny("init_val", entry.init_val); + return msg.str(); +} + +/* + * @brief prints the data bytes of a canopen::HoldAny value to std::string. + * + * @param parameter_name name of the parameter + * @param holdany value to be printed + * + * @return value converted to string. + */ +std::string sick_line_guidance::EdsUtil::printHoldAny(const std::string & parameter_name, const canopen::HoldAny & holdany) +{ + std::stringstream msg; + if(!holdany.is_empty()) + { + size_t value_size = holdany.type().get_size(); + msg << parameter_name << "_size:" << value_size << "," << parameter_name << "_data:0x"; + const canopen::String & value_data = holdany.data(); + canopen::String::const_iterator iter=value_data.cbegin(); + for(size_t n = 0; iter != value_data.cend() && n < value_size; iter++, n++) + { + msg << std::uppercase << std::hex << std::setfill('0') << std::setw(2) << (unsigned)((*iter)&0xFF); + } + if(iter != value_data.cend()) + { + msg << "..."; + } + } + else + { + msg << parameter_name << ":none"; + } + return msg.str(); +} diff --git a/Devices/Packages/sick_line_guidance/src/sick_line_guidance_msg_util.cpp b/Devices/Packages/sick_line_guidance/src/sick_line_guidance_msg_util.cpp new file mode 100755 index 0000000..a6b33e7 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/src/sick_line_guidance_msg_util.cpp @@ -0,0 +1,238 @@ +/* + * class EdsUtil implements utility functions for eds-files. + * + * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim + * Copyright (C) 2019 SICK AG, Waldkirch + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of SICK AG nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Authors: + * Michael Lehning + * + * Copyright 2019 SICK AG + * Copyright 2019 Ing.-Buero Dr. Michael Lehning + * + */ +#include +#include +#include "sick_line_guidance/sick_line_guidance_msg_util.h" + +/* + * @brief prints and returns a MLS measurement as informational string + * @param[in] measurement_msg MLS measurement to print + * @return informational string containing the MLS measurement data +*/ +std::string sick_line_guidance::MsgUtil::toInfo(const sick_line_guidance::MLS_Measurement & measurement_msg) +{ + std::stringstream info; + info << "position:[" << std::fixed << std::setprecision(3) << measurement_msg.position[0] + << "," << std::fixed << std::setprecision(3) << measurement_msg.position[1] + << "," << std::fixed << std::setprecision(3) << measurement_msg.position[2] + << "],status=" << sick_line_guidance::MsgUtil::toHexString(measurement_msg.status) + << ",lcpflags=" << sick_line_guidance::MsgUtil::toHexString(measurement_msg.lcp) + << ",error=" << sick_line_guidance::MsgUtil::toHexString(measurement_msg.error); + return info.str(); +} + +/* + * @brief prints and returns a OLS measurement as informational string + * @param[in] measurement_msg OLS measurement to print + * @return informational string containing the OLS measurement data +*/ +std::string sick_line_guidance::MsgUtil::toInfo(const sick_line_guidance::OLS_Measurement & measurement_msg) +{ + uint32_t barcode = measurement_msg.barcode; + if(measurement_msg.barcode >= 255) + barcode = measurement_msg.extended_code; + std::stringstream info; + info << "position:[" << std::fixed << std::setprecision(3) << measurement_msg.position[0] + << "," << std::fixed << std::setprecision(3) << measurement_msg.position[1] + << "," << std::fixed << std::setprecision(3) << measurement_msg.position[2] + << "],width:[" << std::fixed << std::setprecision(3) << measurement_msg.width[0] + << "," << std::fixed << std::setprecision(3) << measurement_msg.width[1] + << "," << std::fixed << std::setprecision(3) << measurement_msg.width[2] + << "],status=" << sick_line_guidance::MsgUtil::toHexString(measurement_msg.status) + << ",devstatus=" << sick_line_guidance::MsgUtil::toHexString(measurement_msg.dev_status) + << ",error=" << sick_line_guidance::MsgUtil::toHexString(measurement_msg.error) + << ",barcode=0x" << std::uppercase << std::hex << std::setfill('0') << std::setw(2) << barcode + << ",barcodecenter=" << std::fixed << std::setprecision(3) << measurement_msg.barcode_center_point + << ",linequality=" << sick_line_guidance::MsgUtil::toHexString(measurement_msg.quality_of_lines) + << ",lineintensity=[" << sick_line_guidance::MsgUtil::toHexString(measurement_msg.intensity_of_lines[0]) + << "," << sick_line_guidance::MsgUtil::toHexString(measurement_msg.intensity_of_lines[1]) + << "," << sick_line_guidance::MsgUtil::toHexString(measurement_msg.intensity_of_lines[2]) << "]"; + return info.str(); +} + +/* + * @brief initializes a MLS measurement with zero data + * @param[in+out] measurement_msg MLS measurement + */ +void sick_line_guidance::MsgUtil::zero(sick_line_guidance::MLS_Measurement & measurement_msg) +{ + measurement_msg.header.stamp = ros::Time(0); + measurement_msg.header.frame_id = ""; + measurement_msg.position = { 0, 0, 0 }; + measurement_msg.status = 0; + measurement_msg.lcp = 0; + measurement_msg.error = 0; +} + +/* + * @brief initializes an OLS measurement with zero data + * @param[in+out] measurement_msg OLS measurement + */ +void sick_line_guidance::MsgUtil::zero(sick_line_guidance::OLS_Measurement & measurement_msg) +{ + measurement_msg.header.stamp = ros::Time(0); + measurement_msg.header.frame_id = ""; + measurement_msg.position = { 0, 0, 0 }; + measurement_msg.width = { 0, 0, 0 }; + measurement_msg.status = 0; + measurement_msg.barcode = 0; + measurement_msg.extended_code = 0; + measurement_msg.dev_status = 0; + measurement_msg.error = 0; + measurement_msg.barcode_center_point = 0; + measurement_msg.quality_of_lines = 0; + measurement_msg.intensity_of_lines = { 0, 0, 0 }; +} + +/* + * @brief Returns a MLS sensor measurement + * + * @param[in] lcp1 line center point LCP1 in meter, object 0x2021sub1 in object dictionary + * @param[in] lcp2 line center point LCP2 in meter, object 0x2021sub2 in object dictionary + * @param[in] lcp3 line center point LCP3 in meter, object 0x2021sub3 in object dictionary + * @param[in] status status of measurement, object 0x2022 in object dictionary (Bit7=MSBit to Bit0=LSBit): Bit7: 0, Bit6: ReadingCode, Bit5: Polarity, Bit4: SensorFlipped, Bit3: LineLevBit2, Bit2: LineLevBit1, Bit1: LineLevBit0, Bit0: Linegood(1:LineGood,0:NotGood) + * @param[in] lcp LCP-flags (signs and line assignment), in bits (MSB to LSB): Bit7: MarkerBit4, Bit6: MarkerBit3, Bit5: MarkerBit2, Bit4: MarkerBit1, Bit3: MarkerBit0, Bit2: #LCPBit2, Bit1: #LCPBit1, Bit0: #LCPBit0 + * @param[in] error error register, object 0x1001 in object dictionary + * @param[in] msg_frame_id frame id of OLS_Measurement message + * + * @return parameter converted to MLS_Measurement + */ +sick_line_guidance::MLS_Measurement sick_line_guidance::MsgUtil::convertMLSMessage(float lcp1, float lcp2, float lcp3, uint8_t status, uint8_t lcp, uint8_t error, const std::string & msg_frame_id) +{ + sick_line_guidance::MLS_Measurement mls_message; + mls_message.header.stamp = ros::Time::now(); + mls_message.header.frame_id = msg_frame_id; + mls_message.position = { lcp1, lcp2, lcp3 }; + mls_message.status = status; + mls_message.lcp = lcp; + mls_message.error = error; + return mls_message; +} + +/* + * @brief Returns an OLS sensor measurement + * + * @param[in] lcp1 line center point LCP1 in meter, object 0x2021sub1 in object dictionary + * @param[in] lcp2 line center point LCP2 in meter, object 0x2021sub2 in object dictionary + * @param[in] lcp3 line center point LCP3 in meter, object 0x2021sub3 in object dictionary + * @param[in] width1 width of line 1 in meter, object 0x2021sub5 in object dictionary + * @param[in] width2 width of line 2 in meter, object 0x2021sub6 in object dictionary + * @param[in] width3 width of line 3 in meter, object 0x2021sub7 in object dictionary + * @param[in] status status of measurement, object 0x2021sub4 in object dictionary, in bits (MSB to LSB): Bit7: CodeValid, Bit6: CodeFlipped, Bit5: x, Bit4: DeviceStatus, Bit3: x, Bit2: #LCPBit2, Bit1: #LCPBit1, Bit0: #LCPBit0 + * @param[in] barcode Barcode (> 255: extended barcode), , object 0x2021sub8 and 0x2021sub9 in object dictionary + * @param[in] dev_status Device status, object 0x2018 in object dictionary + * @param[in] error error register, object 0x1001 in object dictionary + * @param[in] barcodecenter barcode_center_point, OLS20 only (0x2021subA), OLS10: always 0 + * @param[in] linequality quality_of_lines, OLS20 only (0x2021subB), OLS10: always 0 + * @param[in] lineintensity1 intensity_of_lines[0], OLS20 only (0x2023sub1), OLS10: always 0 + * @param[in] lineintensity2 intensity_of_lines[1], OLS20 only (0x2023sub2), OLS10: always 0 + * @param[in] lineintensity3 intensity_of_lines[2], OLS20 only (0x2023sub3), OLS10: always 0 + * @param[in] msg_frame_id frame id of OLS_Measurement message + * + * @return parameter converted to OLS_Measurement + */ +sick_line_guidance::OLS_Measurement sick_line_guidance::MsgUtil::convertOLSMessage(float lcp1, float lcp2, float lcp3, float width1, float width2, float width3, uint8_t status, + uint32_t barcode, uint8_t dev_status, uint8_t error, float barcodecenter, uint8_t linequality, uint8_t lineintensity1, uint8_t lineintensity2, uint8_t lineintensity3, const std::string & msg_frame_id) +{ + sick_line_guidance::OLS_Measurement ols_message; + ols_message.header.stamp = ros::Time::now(); + ols_message.header.frame_id = msg_frame_id; + ols_message.position = { lcp1, lcp2, lcp3 }; + ols_message.width = { width1, width2, width3 }; + ols_message.status = status; + if(barcode > 255) + { + ols_message.barcode = 255; + ols_message.extended_code = barcode; + } + else + { + ols_message.barcode = barcode; + ols_message.extended_code = 0; + } + ols_message.dev_status = dev_status; + ols_message.error = error; + ols_message.barcode_center_point = barcodecenter; + ols_message.quality_of_lines = linequality; + ols_message.intensity_of_lines = { lineintensity1, lineintensity2, lineintensity3 }; + return ols_message; +} + + +/* + * @brief returns a gaussian destributed random line center position (lcp) + * + * @param stddev standard deviation of gaussian random generator + * + * @return random line center position + */ +float sick_line_guidance::MsgUtil::randomLCP(double stddev) +{ + static random_numbers::RandomNumberGenerator mea_random_generator; + return (float)mea_random_generator.gaussian(0.0, stddev); +} + +/* + * @brief returns a gaussian destributed random line width + * + * @param stddev standard deviation of gaussian random generator + * + * @return random line width + */ +float sick_line_guidance::MsgUtil::randomLW(double min_linewidth, double max_linewidth) +{ + static random_numbers::RandomNumberGenerator mea_random_generator; + return (float)mea_random_generator.uniformReal(min_linewidth, max_linewidth); +} diff --git a/Devices/Packages/sick_line_guidance/src/sick_line_guidance_node.cpp b/Devices/Packages/sick_line_guidance/src/sick_line_guidance_node.cpp new file mode 100755 index 0000000..6c52989 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/src/sick_line_guidance_node.cpp @@ -0,0 +1,224 @@ +/* + * @brief sick_line_guidance_node implements ros driver for OLS20 and MLS devices. + * + * It implements the can master using CanopenChain in package ros_canopen, + * parses and converts the can messages from OLS20 and MLS devices (SDOs and PDOs) + * and publishes sensor messages (type OLS_Measurement or MLS_Measurement) + * + * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim + * Copyright (C) 2019 SICK AG, Waldkirch + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of SICK AG nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Authors: + * Michael Lehning + * + * Copyright 2019 SICK AG + * Copyright 2019 Ing.-Buero Dr. Michael Lehning + * + */ +#include +#include +#include +#include +#include +#include "sick_line_guidance/sick_line_guidance_version.h" +#include "sick_line_guidance/sick_line_guidance_canopen_chain.h" +#include "sick_line_guidance/sick_line_guidance_can_mls_subscriber.h" +#include "sick_line_guidance/sick_line_guidance_can_ols_subscriber.h" +#include "sick_line_guidance/sick_line_guidance_can_cia401_subscriber.h" +#include "sick_line_guidance/sick_line_guidance_diagnostic.h" +#include "sick_line_guidance/sick_line_guidance_eds_util.h" +#include "sick_line_guidance/sick_line_guidance_msg_util.h" + +typedef enum SICK_LINE_GUIDANCE_EXIT_CODES_ENUM +{ + EXIT_OK = 0, + EXIT_ERROR = 1, + EXIT_CONFIG_ERROR +} SICK_LINE_GUIDANCE_EXIT_CODES; + +// shortcut to read a member from a XmlRpcValue, or to return a defaultvalue, it the member does not exist +template static T readMember(XmlRpc::XmlRpcValue & value, const std::string & member, const T & defaultvalue) +{ + if(value.hasMember(member)) + return value[member]; + return defaultvalue; +} + +/* + * @brief sick_line_guidance_node implements ros driver for OLS20 and MLS devices. + * It implements the can master using CanopenChain in package ros_canopen, + * parses and converts the can messages from OLS20 and MLS devices (SDOs and PDOs) + * and publishes sensor messages (type OLS_Measurement or MLS_Measurement) + */ +int main(int argc, char** argv) +{ + ros::init(argc, argv, "sick_line_guidance_node"); + + // Configuration + ros::NodeHandle nh; + bool can_connect_init_at_startup = true; // Connect and initialize canopen service + int initial_sensor_state = 0x07; // initial sensor state (f.e. 0x07 for 3 detected lines, or 8 to indicate sensor error) + int subscribe_queue_size = 16; // buffer size for ros messages + int max_num_retries_after_sdo_error = 2; // After SDO error, the query is repeated max. N times (default: N=2). If the SDO error persists, the can driver is shutdown and restarted. + double max_sdo_rate = 1000; // max_sdo_rate max. sdo query and publish rate + std::string diagnostic_topic = "diagnostics"; + nh.param("/sick_line_guidance_node/can_connect_init_at_startup", can_connect_init_at_startup, can_connect_init_at_startup); // Connect and initialize canopen service at startup + nh.param("/sick_line_guidance_node/initial_sensor_state", initial_sensor_state, initial_sensor_state); // initial sensor state + nh.param("/sick_line_guidance_node/subscribe_queue_size", subscribe_queue_size, subscribe_queue_size); + nh.param("/sick_line_guidance_node/diagnostic_topic", diagnostic_topic, diagnostic_topic); + nh.param("/sick_line_guidance_node/max_num_retries_after_sdo_error", max_num_retries_after_sdo_error, max_num_retries_after_sdo_error); + nh.param("/sick_line_guidance_node/max_sdo_rate", max_sdo_rate, max_sdo_rate); + ROS_INFO_STREAM("sick_line_guidance_node: version " << sick_line_guidance::Version::getVersionInfo()); + sick_line_guidance::Diagnostic::init(nh, diagnostic_topic, "sick_line_guidance_node"); + if(can_connect_init_at_startup) + { + ROS_INFO_STREAM("sick_line_guidance_node: connect and init canopen ros-service..."); + do + { + ros::Duration(1.0).sleep(); + } while (!sick_line_guidance::CanopenChain::connectInitService(nh)); + ROS_INFO_STREAM("sick_line_guidance_node: canopen ros-service connected and initialized."); + } + else + { + ROS_INFO_STREAM("sick_line_guidance_node: starting..."); + } + + // Read configuration of can nodes from yaml-file (there can be more than just "node1") + std::vector vecCanSubscriber; + XmlRpc::XmlRpcValue nodes; + if(!nh.getParam("nodes", nodes) || nodes.size() < 1) + { + ROS_ERROR("sick_line_guidance_node: no can nodes found in yaml-file, please check configuration. Aborting..."); + sick_line_guidance::Diagnostic::update(sick_line_guidance::DIAGNOSTIC_STATUS::CONFIGURATION_ERROR, "no can nodes found in yaml-file"); + return EXIT_CONFIG_ERROR; + } + for(XmlRpc::XmlRpcValue::iterator can_node_iter = nodes.begin(); can_node_iter != nodes.end(); ++can_node_iter) + { + std::string can_node_id = readMember(can_node_iter->second, "name", can_node_iter->first); + ROS_INFO("sick_line_guidance_node: initializing can_node_id \"%s\"...", can_node_id.c_str()); + + // Test: Query some common objects from can device + std::string can_msg = "", can_object_value = ""; + std::vector vec_object_idx = {"1001", "1018sub1", "1018sub4"}; + for (std::vector::iterator object_iter = vec_object_idx.begin(); object_iter != vec_object_idx.end(); object_iter++) + { + if (!sick_line_guidance::CanopenChain::queryCanObject(nh, can_node_id, *object_iter, max_num_retries_after_sdo_error, can_msg, can_object_value)) + ROS_ERROR("sick_line_guidance_node: CanopenChain::queryCanObject(%s, %s) failed: %s", can_node_id.c_str(), object_iter->c_str(), can_msg.c_str()); + else + ROS_INFO("sick_line_guidance_node: can %s[%s]=%s", can_node_id.c_str(), object_iter->c_str(), can_object_value.c_str()); + } + + // Test: read eds-file + std::string eds_file = sick_line_guidance::EdsUtil::filepathFromPackage( + readMember(can_node_iter->second, "eds_pkg", ""), + readMember(can_node_iter->second, "eds_file", "")); + ROS_INFO("sick_line_guidance_node: eds_file \"%s\"", eds_file.c_str()); + std::string eds_file_content = sick_line_guidance::EdsUtil::readParsePrintEdsfile(eds_file); + if(eds_file_content == "") + { + ROS_ERROR("sick_line_guidance_node: read/parse eds file \"%s\" failed", eds_file.c_str()); + sick_line_guidance::Diagnostic::update(sick_line_guidance::DIAGNOSTIC_STATUS::CONFIGURATION_ERROR, "read/parse error in eds file"); + } + else + ROS_DEBUG("%s", eds_file_content.c_str()); + + // Parse dcf_overlays, read and overwrite parameter specified in dcf_overlays + if (can_node_iter->second.hasMember("dcf_overlay") && + !sick_line_guidance::CanopenChain::writeDCFoverlays(nh, can_node_id, can_node_iter->second["dcf_overlay"])) + { + ROS_ERROR("sick_line_guidance: failed to set one or more dcf overlays."); + } + + // Subscribe to canopen_chain_node topics, handle PDO messages and publish OLS/MLS measurement messages + std::string sick_device_family = readMember(can_node_iter->second, "sick_device_family", ""); + std::string sick_topic = readMember(can_node_iter->second, "sick_topic", ""); + std::string sick_frame_id = readMember(can_node_iter->second, "sick_frame_id", ""); + boost::to_upper(sick_device_family); + if(sick_device_family.empty() || sick_topic.empty() || sick_frame_id.empty()) + { + sick_line_guidance::Diagnostic::update(sick_line_guidance::DIAGNOSTIC_STATUS::CONFIGURATION_ERROR, "invalid node configuration in yaml file"); + ROS_ERROR("sick_line_guidance_node: invalid node configuration: sick_device_family=%s, sick_topic=%s, sick_frame_id=%s", sick_device_family.c_str(), sick_topic.c_str(), sick_frame_id.c_str()); + continue; + } + sick_line_guidance::Diagnostic::update(sick_line_guidance::DIAGNOSTIC_STATUS::OK, ""); + sick_line_guidance::CanSubscriber * p_can_subscriber = NULL; + if (sick_device_family == "MLS") + p_can_subscriber = new sick_line_guidance::CanMlsSubscriber(nh, can_node_id, max_num_retries_after_sdo_error, max_sdo_rate, sick_topic, sick_frame_id, initial_sensor_state, subscribe_queue_size); + else if (sick_device_family == "OLS10") + p_can_subscriber = new sick_line_guidance::CanOls10Subscriber(nh, can_node_id, max_num_retries_after_sdo_error, max_sdo_rate, sick_topic, sick_frame_id, initial_sensor_state, subscribe_queue_size); + else if (sick_device_family == "OLS20") + p_can_subscriber = new sick_line_guidance::CanOls20Subscriber(nh, can_node_id, max_num_retries_after_sdo_error, max_sdo_rate, sick_topic, sick_frame_id, initial_sensor_state, subscribe_queue_size); + else if (sick_device_family == "CIA401") + p_can_subscriber = new sick_line_guidance::CanCiA401Subscriber(nh, can_node_id, max_num_retries_after_sdo_error, max_sdo_rate, sick_topic, sick_frame_id, initial_sensor_state, subscribe_queue_size); + if(p_can_subscriber && p_can_subscriber->subscribeCanTopics()) + { + vecCanSubscriber.push_back(p_can_subscriber); + } + else + { + ROS_ERROR("sick_line_guidance_node: sick_device_family \"%s\" not supported.", sick_device_family.c_str()); + } + + } + if(vecCanSubscriber.size() < 1) + { + sick_line_guidance::Diagnostic::update(sick_line_guidance::DIAGNOSTIC_STATUS::CONFIGURATION_ERROR, " no can nodes for sick_line_guidance found in yaml-file"); + ROS_ERROR("sick_line_guidance_node: no can nodes for sick_line_guidance found in yaml-file, please check configuration. Aborting..."); + return EXIT_CONFIG_ERROR; + } + + // Run ros event loop + ros::spin(); + + // Deallocate resources + std::cout << "sick_line_guidance_node: exiting..." << std::endl; + ROS_INFO("sick_line_guidance_node: exiting..."); + sick_line_guidance::Diagnostic::update(sick_line_guidance::DIAGNOSTIC_STATUS::EXIT); + for(std::vector::iterator iter = vecCanSubscriber.begin(); iter != vecCanSubscriber.end(); iter++) + delete(*iter); + + return EXIT_OK; +} + diff --git a/Devices/Packages/sick_line_guidance/src/sick_line_guidance_ros2can_node.cpp b/Devices/Packages/sick_line_guidance/src/sick_line_guidance_ros2can_node.cpp new file mode 100755 index 0000000..22f3958 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/src/sick_line_guidance_ros2can_node.cpp @@ -0,0 +1,206 @@ +/* + * sick_line_guidance_ros2can_node: subscribes to ros topic (message type can_msgs::Frame), + * converts all messages to can frames (type can::Frame) and writes them to can bus. + * A simple cansend with input by ros messages. + * + * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim + * Copyright (C) 2019 SICK AG, Waldkirch + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of SICK AG nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Authors: + * Michael Lehning + * + * Copyright 2019 SICK AG + * Copyright 2019 Ing.-Buero Dr. Michael Lehning + * + */ +#include +#include +#include +#include +#include +#include "sick_line_guidance/sick_line_guidance_version.h" + +namespace sick_line_guidance +{ + /* + * class SocketCANSender implements a callback for ros messages, converts them to can frames and writes the data to can bus. + */ + class SocketCANSender + { + public: + + /* + * Constructor + */ + SocketCANSender(can::DriverInterfaceSharedPtr p_socketcan_interface) : m_socketcan_interface(p_socketcan_interface), m_socketcan_thread(0), m_socketcan_running(false) + { + } + + /* + * @brief create a thread and run can::SocketCANInterface::run() in a background task + */ + void start() + { + m_socketcan_running = true; + m_socketcan_thread = new boost::thread(&sick_line_guidance::SocketCANSender::run, this); + } + + /* + * @brief shuts down can::SocketCANInterface and stops the thread running can::SocketCANInterface::run() + */ + void stop() + { + m_socketcan_running = true; + if(m_socketcan_thread) + { + m_socketcan_interface->shutdown(); + m_socketcan_thread->join(); + delete(m_socketcan_thread); + } + m_socketcan_thread = 0; + } + + /* + * @brief Callbacks for ros messages. Converts a ros message to datatype can::Frame and writes it to can bus. + * param[in] msg ros message of type can_msgs::Frame + */ + void messageCallback(const can_msgs::Frame & msg) + { + can::Frame canframe; + canframe.id = msg.id; + canframe.dlc = msg.dlc; + canframe.is_error = msg.is_error; + canframe.is_rtr = msg.is_rtr; + canframe.is_extended = msg.is_extended; + size_t n = 0, n_max = std::min(msg.data.size(),canframe.data.size()); + for(n = 0; n < n_max; n++) + canframe.data[n] = msg.data[n]; + for(; n < canframe.data.size(); n++) + canframe.data[n] = 0; + if(!canframe.isValid()) + { + ROS_ERROR_STREAM("sick_line_guidance::SocketCANSender::messageCallback(): received invalid can_msgs::Frame message: " << msg); + ROS_ERROR_STREAM("sick_line_guidance::SocketCANSender::messageCallback(): sending invalid can frame " << can::tostring(canframe, false) << " ..."); + } + if(m_socketcan_interface->send(canframe)) + { + ROS_DEBUG_STREAM("sick_line_guidance::SocketCANSender::messageCallback(): sent can message " << can::tostring(canframe, false)); + } + else + { + ROS_ERROR_STREAM("sick_line_guidance::SocketCANSender::messageCallback(): send can message " << can::tostring(canframe, false) << " failed."); + } + } + + protected: + + can::DriverInterfaceSharedPtr m_socketcan_interface; // can::SocketCANInterface instance + boost::thread* m_socketcan_thread; // thread to run can::SocketCANInterface in background + bool m_socketcan_running; // flag indicating start and stop of m_socketcan_thread + + /* + * @brief runs can::SocketCANInterface::run() in an endless loop + */ + void run() + { + while(m_socketcan_running && ros::ok()) + { + m_socketcan_interface->run(); + } + } + + }; // SocketCANSender +} // sick_line_guidance + +/* + * sick_line_guidance_ros2can_node: subscribes to ros topic (message type can_msgs::Frame), + * converts all messages to can frames (type can::Frame) and writes them to can bus. + * A simple cansend with input by ros messages. + */ +int main(int argc, char** argv) +{ + // Setup and configuration + ros::init(argc, argv, "sick_line_guidance_ros2can_node"); + ros::NodeHandle nh; + std::string can_device = "can0", ros_topic = "ros2can0"; + int subscribe_queue_size = 32; + nh.param("can_device", can_device, can_device); // name of can net device (socketcan interface) + nh.param("ros_topic", ros_topic, ros_topic); // topic for ros messages, message type can_msgs::Frame + nh.param("subscribe_queue_size", subscribe_queue_size, subscribe_queue_size); // buffer size for ros messages + ROS_INFO_STREAM("sick_line_guidance_ros2can_node: version " << sick_line_guidance::Version::getVersionInfo()); + ROS_INFO_STREAM("sick_line_guidance_ros2can_node: starting..."); + + // Create the SocketCANInterface + can::DriverInterfaceSharedPtr p_socketcan_interface = 0; + canopen::GuardedClassLoader driver_loader("socketcan_interface", "can::DriverInterface"); + try + { + ROS_INFO_STREAM("sick_line_guidance_ros2can_node: initializing SocketCANInterface..."); + p_socketcan_interface = driver_loader.createInstance("can::SocketCANInterface"); + if(!p_socketcan_interface->init(can_device, false, can::NoSettings::create())) + ROS_ERROR_STREAM("sick_line_guidance_ros2can_node: SocketCANInterface::init() failed."); + } + catch(pluginlib::PluginlibException& ex) + { + ROS_ERROR_STREAM("sick_line_guidance_ros2can_node: createInstance(\"can::SocketCANInterface\") failed: " << ex.what() << ", exit with error"); + return 1; + } + ROS_INFO_STREAM("sick_line_guidance_ros2can_node: SocketCANInterface created, state = " << p_socketcan_interface->getState().driver_state); + + // Subscribe to ros topic + sick_line_guidance::SocketCANSender socketcan_sender(p_socketcan_interface); + ros::Subscriber ros_subscriber = nh.subscribe(ros_topic, subscribe_queue_size, &sick_line_guidance::SocketCANSender::messageCallback, &socketcan_sender); + socketcan_sender.start(); + ROS_INFO_STREAM("sick_line_guidance_ros2can_node: SocketCANSender started, listening to ros topic \"" << ros_topic << "\" ..."); + + // Run ros event loop + ros::spin(); + + std::cout << "sick_line_guidance_ros2can_node: exiting..." << std::endl; + ROS_INFO_STREAM("sick_line_guidance_ros2can_node: exiting..."); + socketcan_sender.stop(); + p_socketcan_interface = 0; + return 0; +} + diff --git a/Devices/Packages/sick_line_guidance/test/cfg/sick_canopen_simu_cfg.xml b/Devices/Packages/sick_line_guidance/test/cfg/sick_canopen_simu_cfg.xml new file mode 100755 index 0000000..01c5a87 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/test/cfg/sick_canopen_simu_cfg.xml @@ -0,0 +1,133 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Devices/Packages/sick_line_guidance/test/eds/CiA401_IO_Node3.bin b/Devices/Packages/sick_line_guidance/test/eds/CiA401_IO_Node3.bin new file mode 100755 index 0000000..119ad30 Binary files /dev/null and b/Devices/Packages/sick_line_guidance/test/eds/CiA401_IO_Node3.bin differ diff --git a/Devices/Packages/sick_line_guidance/test/eds/CiA401_IO_Node3.eds b/Devices/Packages/sick_line_guidance/test/eds/CiA401_IO_Node3.eds new file mode 100755 index 0000000..1ef774c --- /dev/null +++ b/Devices/Packages/sick_line_guidance/test/eds/CiA401_IO_Node3.eds @@ -0,0 +1,2282 @@ +[FileInfo] +FileName=C:\Users\Andy\Documents\ESA Projects\CANopenIA\EDS_shared\CiA401_IO_Node3.eds +FileVersion=1 +FileRevision=10 +EDSVersion=4.0 +Description=CANopenIA CiA401 I/O Configuration Example +CreatedBy=ESAcademy +CreationTime=03:24PM +CreationDate=02-07-2017 +ModifiedBy=ESAcademy +ModificationTime=09:21AM +ModificationDate=02-01-2019 + +[Comments] +Lines=2 +Line1=ESAcademy EDS example for an Input/Output device. +Line2=For more information, see www.CANopenIA.com and www.CANgineBerry.com + +[DeviceInfo] +Vendorname=ESAcademy +ProductName=www.cangineberry.com +OrderCode=ESS-CAG-BRY EAN:0610098078999 +VendorNumber=0x01455341 +ProductNumber=0xC01A0401 +RevisionNumber=0xFFFFFFFF +BaudRate_10=0 +BaudRate_20=0 +BaudRate_50=0 +BaudRate_125=1 +BaudRate_250=1 +BaudRate_500=1 +BaudRate_800=0 +BaudRate_1000=1 +SimpleBootupMaster=0 +SimpleBootupSlave=1 +GroupMessaging=0 +LSS_Supported=0 +Granularity=0 +DynamicChannelsSupported=0 +NrOfRXPDO=4 +NrOfTXPDO=4 +CompactPDO=0x00 + +[DummyUsage] +Dummy0001=0 +Dummy0002=0 +Dummy0003=0 +Dummy0004=0 +Dummy0005=1 +Dummy0006=1 +Dummy0007=1 + +[MandatoryObjects] +SupportedObjects=3 +1=0x1000 +2=0x1001 +3=0x1018 + +[OptionalObjects] +SupportedObjects=28 +1=0x1002 +2=0x1003 +3=0x1005 +4=0x1008 +5=0x1009 +6=0x100A +7=0x1016 +8=0x1017 +9=0x1400 +10=0x1401 +11=0x1402 +12=0x1403 +13=0x1600 +14=0x1601 +15=0x1602 +16=0x1603 +17=0x1800 +18=0x1801 +19=0x1802 +20=0x1803 +21=0x1A00 +22=0x1A01 +23=0x1A02 +24=0x1A03 +25=0x6000 +26=0x6200 +27=0x6401 +28=0x6411 + +[ManufacturerObjects] +SupportedObjects=3 +1=0x5F00 +2=0x5F01 +3=0x5FF5 + +[1000] +ParameterName=Device Type +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=const +DefaultValue=0x000F0191 +PDOMapping=0 +ObjFlags=0x00 + +[1001] +ParameterName=Error Register +ObjectType=0x07 +DataType=0x0005 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue= +PDOMapping=0 +ObjFlags=0x00 + +[1002] +ParameterName=Manufacturer Status Register +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue= +PDOMapping=0 +ObjFlags=0x00 + +[1003] +ParameterName=Pre-Defined Error Field +SubNumber=5 +ObjectType=0x08 + +[1003sub0] +ParameterName=Number of Errors +ObjectType=0x07 +DataType=0x0005 +LowLimit=0x00 +HighLimit=0x04 +AccessType=rw +DefaultValue=0x00 +PDOMapping=0 +ObjFlags=0x00 + +[1003sub1] +ParameterName=Pre-Defined Error Field 1 +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue= +PDOMapping=0 +ObjFlags=0x00 + +[1003sub2] +ParameterName=Pre-Defined Error Field 2 +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue= +PDOMapping=0 +ObjFlags=0x00 + +[1003sub3] +ParameterName=Pre-Defined Error Field 3 +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue= +PDOMapping=0 +ObjFlags=0x00 + +[1003sub4] +ParameterName=Pre-Defined Error Field 4 +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue= +PDOMapping=0 +ObjFlags=0x00 + +[1005] +ParameterName=COB-ID SYNC +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue=0x00000080 +PDOMapping=0 +ObjFlags=0x00 + +[1008] +ParameterName=Manufacturer Device Name +ObjectType=0x07 +DataType=0x0009 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue=CANopenIA CiA401 Demo +PDOMapping=0 +ObjFlags=0x00 + +[1009] +ParameterName=Manufacturer Hardware Version +ObjectType=0x07 +DataType=0x0009 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue=HW V1.00 +PDOMapping=0 +ObjFlags=0x00 + +[100A] +ParameterName=Manufacturer Software Version +ObjectType=0x07 +DataType=0x0009 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue=SW V1.00 +PDOMapping=0 +ObjFlags=0x00 + +[1016] +ParameterName=Consumer Heartbeat Time +SubNumber=4 +ObjectType=0x08 + +[1016sub0] +ParameterName=Number of Entries +ObjectType=0x07 +DataType=0x0005 +LowLimit= +HighLimit= +AccessType=const +DefaultValue=3 +PDOMapping=0 +ObjFlags=0x00 + +[1016sub1] +ParameterName=Consumer Heartbeat Time 1 +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=rw +DefaultValue=0 +PDOMapping=0 +ObjFlags=0x00 + +[1016sub2] +ParameterName=Consumer Heartbeat Time 2 +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=rw +DefaultValue=0 +PDOMapping=0 +ObjFlags=0x00 + +[1016sub3] +ParameterName=Consumer Heartbeat Time 3 +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=rw +DefaultValue=0 +PDOMapping=0 +ObjFlags=0x00 + +[1017] +ParameterName=Producer Heartbeat Time +ObjectType=0x07 +DataType=0x0006 +LowLimit= +HighLimit= +AccessType=rw +DefaultValue=0 +PDOMapping=0 +ObjFlags=0x00 + +[1018] +ParameterName=Identity Object +SubNumber=5 +ObjectType=0x08 + +[1018sub0] +ParameterName=number of entries +ObjectType=0x07 +DataType=0x0005 +LowLimit=1 +HighLimit=4 +AccessType=const +DefaultValue=4 +PDOMapping=0 +ObjFlags=0x00 + +[1018sub1] +ParameterName=Vendor ID +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=const +DefaultValue=0x01455341 +PDOMapping=0 +ObjFlags=0x00 + +[1018sub2] +ParameterName=Product Code +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=const +DefaultValue=0xC01A0401 +PDOMapping=0 +ObjFlags=0x00 + +[1018sub3] +ParameterName=Revision number +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue=0xFFFFFFFF +PDOMapping=0 +ObjFlags=0x00 + +[1018sub4] +ParameterName=Serial number +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue=0xFFFFFFFF +PDOMapping=0 +ObjFlags=0x00 + +[1400] +ParameterName=Receive PDO1 Communication Parameter +SubNumber=3 +ObjectType=0x09 + +[1400sub0] +ParameterName=Number of Entries +ObjectType=0x07 +DataType=0x0005 +LowLimit=0x02 +HighLimit=0x05 +AccessType=const +DefaultValue=2 +PDOMapping=0 +ObjFlags=0x00 + +[1400sub1] +ParameterName=COB-ID +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit=0xFFFFFFFF +AccessType=rw +DefaultValue=$NODEID+0x200 +PDOMapping=0 +ObjFlags=0x00 + +[1400sub2] +ParameterName=Transmission Type +ObjectType=0x07 +DataType=0x0005 +LowLimit= +HighLimit= +AccessType=rw +DefaultValue=255 +PDOMapping=0 +ObjFlags=0x00 + +[1401] +ParameterName=Receive PDO2 Communication Parameter +SubNumber=3 +ObjectType=0x09 + +[1401sub0] +ParameterName=Number of Entries +ObjectType=0x07 +DataType=0x0005 +LowLimit=0x02 +HighLimit=0x05 +AccessType=const +DefaultValue=2 +PDOMapping=0 +ObjFlags=0x00 + +[1401sub1] +ParameterName=COB-ID +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit=0xFFFFFFFF +AccessType=rw +DefaultValue=$NODEID+0x300 +PDOMapping=0 +ObjFlags=0x00 + +[1401sub2] +ParameterName=Transmission Type +ObjectType=0x07 +DataType=0x0005 +LowLimit= +HighLimit= +AccessType=rw +DefaultValue=255 +PDOMapping=0 +ObjFlags=0x00 + +[1402] +ParameterName=Receive PDO3 Communication Parameter +SubNumber=3 +ObjectType=0x09 + +[1402sub0] +ParameterName=Number of Entries +ObjectType=0x07 +DataType=0x0005 +LowLimit=0x02 +HighLimit=0x05 +AccessType=const +DefaultValue=0x02 +PDOMapping=0 +ObjFlags=0x00 + +[1402sub1] +ParameterName=COB-ID +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit=0xFFFFFFFF +AccessType=rw +DefaultValue=$NODEID+0x400 +PDOMapping=0 +ObjFlags=0x00 + +[1402sub2] +ParameterName=Transmission Type +ObjectType=0x07 +DataType=0x0005 +LowLimit= +HighLimit= +AccessType=rw +DefaultValue=255 +PDOMapping=0 +ObjFlags=0x00 + +[1403] +ParameterName=Receive PDO4 Communication Parameter +SubNumber=3 +ObjectType=0x09 + +[1403sub0] +ParameterName=Number of Entries +ObjectType=0x07 +DataType=0x0005 +LowLimit=0x02 +HighLimit=0x05 +AccessType=const +DefaultValue=0x02 +PDOMapping=0 +ObjFlags=0x00 + +[1403sub1] +ParameterName=COB-ID +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit=0xFFFFFFFF +AccessType=rw +DefaultValue=$NODEID+0x500 +PDOMapping=0 +ObjFlags=0x00 + +[1403sub2] +ParameterName=Transmission Type +ObjectType=0x07 +DataType=0x0005 +LowLimit= +HighLimit= +AccessType=rw +DefaultValue=255 +PDOMapping=0 +ObjFlags=0x00 + +[1600] +ParameterName=Receive PDO1 Mapping Parameter +SubNumber=9 +ObjectType=0x08 + +[1600sub0] +ParameterName=Number of Entries +ObjectType=0x07 +DataType=0x0005 +LowLimit=0 +HighLimit=64 +AccessType=const +DefaultValue=0x08 +PDOMapping=0 +ObjFlags=0x00 + +[1600sub1] +ParameterName=PDO Mapping Entry +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=const +DefaultValue=0x62000108 +PDOMapping=0 +ObjFlags=0x00 + +[1600sub2] +ParameterName=PDO Mapping Entry +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=const +DefaultValue=0x62000208 +PDOMapping=0 +ObjFlags=0x00 + +[1600sub3] +ParameterName=PDO Mapping Entry +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=const +DefaultValue=0x62000308 +PDOMapping=0 +ObjFlags=0x00 + +[1600sub4] +ParameterName=PDO Mapping Entry +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=const +DefaultValue=0x62000408 +PDOMapping=0 +ObjFlags=0x00 + +[1600sub5] +ParameterName=PDO Mapping Entry +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=const +DefaultValue=0x62000508 +PDOMapping=0 +ObjFlags=0x00 + +[1600sub6] +ParameterName=PDO Mapping Entry +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=const +DefaultValue=0x62000608 +PDOMapping=0 +ObjFlags=0x00 + +[1600sub7] +ParameterName=PDO Mapping Entry +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=const +DefaultValue=0x62000708 +PDOMapping=0 +ObjFlags=0x00 + +[1600sub8] +ParameterName=PDO Mapping Entry +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=const +DefaultValue=0x62000808 +PDOMapping=0 +ObjFlags=0x00 + +[1601] +ParameterName=Receive PDO2 Mapping Parameter +SubNumber=5 +ObjectType=0x08 + +[1601sub0] +ParameterName=Number of Entries +ObjectType=0x07 +DataType=0x0005 +LowLimit=0 +HighLimit=64 +AccessType=const +DefaultValue=0x04 +PDOMapping=0 +ObjFlags=0x00 + +[1601sub1] +ParameterName=PDO Mapping Entry +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=const +DefaultValue=0x64110110 +PDOMapping=0 +ObjFlags=0x00 + +[1601sub2] +ParameterName=PDO Mapping Entry +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=const +DefaultValue=0x64110210 +PDOMapping=0 +ObjFlags=0x00 + +[1601sub3] +ParameterName=PDO Mapping Entry +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=const +DefaultValue=0x64110310 +PDOMapping=0 +ObjFlags=0x00 + +[1601sub4] +ParameterName=PDO Mapping Entry +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=const +DefaultValue=0x64110410 +PDOMapping=0 +ObjFlags=0x00 + +[1602] +ParameterName=Receive PDO3 Mapping Parameter +SubNumber=5 +ObjectType=0x08 + +[1602sub0] +ParameterName=Number of Entries +ObjectType=0x07 +DataType=0x0005 +LowLimit=0 +HighLimit=64 +AccessType=const +DefaultValue=0x04 +PDOMapping=0 +ObjFlags=0x00 + +[1602sub1] +ParameterName=PDO Mapping Entry +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=const +DefaultValue=0x64110510 +PDOMapping=0 +ObjFlags=0x00 + +[1602sub2] +ParameterName=PDO Mapping Entry +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=const +DefaultValue=0x64110610 +PDOMapping=0 +ObjFlags=0x00 + +[1602sub3] +ParameterName=PDO Mapping Entry +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=const +DefaultValue=0x64110710 +PDOMapping=0 +ObjFlags=0x00 + +[1602sub4] +ParameterName=PDO Mapping Entry +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=const +DefaultValue=0x64110810 +PDOMapping=0 +ObjFlags=0x00 + +[1603] +ParameterName=Receive PDO4 Mapping Parameter +SubNumber=5 +ObjectType=0x08 + +[1603sub0] +ParameterName=Number of Entries +ObjectType=0x07 +DataType=0x0005 +LowLimit=0 +HighLimit=64 +AccessType=const +DefaultValue=0x04 +PDOMapping=0 +ObjFlags=0x00 + +[1603sub1] +ParameterName=PDO Mapping Entry +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=const +DefaultValue=0x64110910 +PDOMapping=0 +ObjFlags=0x00 + +[1603sub2] +ParameterName=PDO Mapping Entry +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=const +DefaultValue=0x64110A10 +PDOMapping=0 +ObjFlags=0x00 + +[1603sub3] +ParameterName=PDO Mapping Entry +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=const +DefaultValue=0x64110B10 +PDOMapping=0 +ObjFlags=0x00 + +[1603sub4] +ParameterName=PDO Mapping Entry +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=const +DefaultValue=0x64110C10 +PDOMapping=0 +ObjFlags=0x00 + +[1800] +ParameterName=Transmit PDO1 Communication Parameter +SubNumber=5 +ObjectType=0x09 + +[1800sub0] +ParameterName=Number of Entries +ObjectType=0x07 +DataType=0x0005 +LowLimit=0x02 +HighLimit=0x05 +AccessType=const +DefaultValue=5 +PDOMapping=0 +ObjFlags=0x00 + +[1800sub1] +ParameterName=COB-ID +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit=0xFFFFFFFF +AccessType=rw +DefaultValue=$NODEID+0x40000180 +PDOMapping=0 +ObjFlags=0x00 + +[1800sub2] +ParameterName=Transmission Type +ObjectType=0x07 +DataType=0x0005 +LowLimit= +HighLimit= +AccessType=rw +DefaultValue=255 +PDOMapping=0 +ObjFlags=0x00 + +[1800sub3] +ParameterName=Inhibit Time +ObjectType=0x07 +DataType=0x0006 +LowLimit= +HighLimit= +AccessType=rw +DefaultValue=0 +PDOMapping=0 +ObjFlags=0x00 + +[1800sub5] +ParameterName=Event Timer +ObjectType=0x07 +DataType=0x0006 +LowLimit= +HighLimit= +AccessType=rw +DefaultValue=100 +PDOMapping=0 +ObjFlags=0x00 + +[1801] +ParameterName=Transmit PDO2 Communication Parameter +SubNumber=5 +ObjectType=0x09 + +[1801sub0] +ParameterName=Number of Entries +ObjectType=0x07 +DataType=0x0005 +LowLimit=0x02 +HighLimit=0x05 +AccessType=const +DefaultValue=5 +PDOMapping=0 +ObjFlags=0x00 + +[1801sub1] +ParameterName=COB-ID +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit=0xFFFFFFFF +AccessType=rw +DefaultValue=$NODEID+0x40000280 +PDOMapping=0 +ObjFlags=0x00 + +[1801sub2] +ParameterName=Transmission Type +ObjectType=0x07 +DataType=0x0005 +LowLimit= +HighLimit= +AccessType=rw +DefaultValue=255 +PDOMapping=0 +ObjFlags=0x00 + +[1801sub3] +ParameterName=Inhibit Time +ObjectType=0x07 +DataType=0x0006 +LowLimit= +HighLimit= +AccessType=rw +DefaultValue=2000 +PDOMapping=0 +ObjFlags=0x00 + +[1801sub5] +ParameterName=Event Timer +ObjectType=0x07 +DataType=0x0006 +LowLimit= +HighLimit= +AccessType=rw +DefaultValue=1000 +PDOMapping=0 +ObjFlags=0x00 + +[1802] +ParameterName=Transmit PDO3 Communication Parameter +SubNumber=5 +ObjectType=0x09 + +[1802sub0] +ParameterName=Number of Entries +ObjectType=0x07 +DataType=0x0005 +LowLimit=0x02 +HighLimit=0x05 +AccessType=const +DefaultValue=0x05 +PDOMapping=0 +ObjFlags=0x00 + +[1802sub1] +ParameterName=COB-ID +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit=0xFFFFFFFF +AccessType=rw +DefaultValue=$NODEID+0x40000380 +PDOMapping=0 +ObjFlags=0x00 + +[1802sub2] +ParameterName=Transmission Type +ObjectType=0x07 +DataType=0x0005 +LowLimit= +HighLimit= +AccessType=rw +DefaultValue=255 +PDOMapping=0 +ObjFlags=0x00 + +[1802sub3] +ParameterName=Inhibit Time +ObjectType=0x07 +DataType=0x0006 +LowLimit= +HighLimit= +AccessType=rw +DefaultValue=2000 +PDOMapping=0 +ObjFlags=0x00 + +[1802sub5] +ParameterName=Event Timer +ObjectType=0x07 +DataType=0x0006 +LowLimit= +HighLimit= +AccessType=rw +DefaultValue=1000 +PDOMapping=0 +ObjFlags=0x00 + +[1803] +ParameterName=Transmit PDO4 Communication Parameter +SubNumber=5 +ObjectType=0x09 + +[1803sub0] +ParameterName=Number of Entries +ObjectType=0x07 +DataType=0x0005 +LowLimit=0x02 +HighLimit=0x05 +AccessType=const +DefaultValue=0x05 +PDOMapping=0 +ObjFlags=0x00 + +[1803sub1] +ParameterName=COB-ID +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit=0xFFFFFFFF +AccessType=rw +DefaultValue=$NODEID+0x40000480 +PDOMapping=0 +ObjFlags=0x00 + +[1803sub2] +ParameterName=Transmission Type +ObjectType=0x07 +DataType=0x0005 +LowLimit= +HighLimit= +AccessType=rw +DefaultValue=255 +PDOMapping=0 +ObjFlags=0x00 + +[1803sub3] +ParameterName=Inhibit Time +ObjectType=0x07 +DataType=0x0006 +LowLimit= +HighLimit= +AccessType=rw +DefaultValue=2000 +PDOMapping=0 +ObjFlags=0x00 + +[1803sub5] +ParameterName=Event Timer +ObjectType=0x07 +DataType=0x0006 +LowLimit= +HighLimit= +AccessType=rw +DefaultValue=1000 +PDOMapping=0 +ObjFlags=0x00 + +[1A00] +ParameterName=Transmit PDO1 Mapping Parameter +SubNumber=9 +ObjectType=0x08 + +[1A00sub0] +ParameterName=Number of Entries +ObjectType=0x07 +DataType=0x0005 +LowLimit=0 +HighLimit=64 +AccessType=const +DefaultValue=0x08 +PDOMapping=0 +ObjFlags=0x00 + +[1A00sub1] +ParameterName=PDO Mapping Entry +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=const +DefaultValue=0x60000108 +PDOMapping=0 +ObjFlags=0x00 + +[1A00sub2] +ParameterName=PDO Mapping Entry +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=const +DefaultValue=0x60000208 +PDOMapping=0 +ObjFlags=0x00 + +[1A00sub3] +ParameterName=PDO Mapping Entry +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=const +DefaultValue=0x60000308 +PDOMapping=0 +ObjFlags=0x00 + +[1A00sub4] +ParameterName=PDO Mapping Entry +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=const +DefaultValue=0x60000408 +PDOMapping=0 +ObjFlags=0x00 + +[1A00sub5] +ParameterName=PDO Mapping Entry +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=const +DefaultValue=0x60000508 +PDOMapping=0 +ObjFlags=0x00 + +[1A00sub6] +ParameterName=PDO Mapping Entry +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=const +DefaultValue=0x60000608 +PDOMapping=0 +ObjFlags=0x00 + +[1A00sub7] +ParameterName=PDO Mapping Entry +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=const +DefaultValue=0x60000708 +PDOMapping=0 +ObjFlags=0x00 + +[1A00sub8] +ParameterName=PDO Mapping Entry +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=const +DefaultValue=0x60000808 +PDOMapping=0 +ObjFlags=0x00 + +[1A01] +ParameterName=Transmit PDO2 Mapping Parameter +SubNumber=5 +ObjectType=0x08 + +[1A01sub0] +ParameterName=Number of Entries +ObjectType=0x07 +DataType=0x0005 +LowLimit=0 +HighLimit=64 +AccessType=const +DefaultValue=0x04 +PDOMapping=0 +ObjFlags=0x00 + +[1A01sub1] +ParameterName=PDO Mapping Entry +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=const +DefaultValue=0x64010110 +PDOMapping=0 +ObjFlags=0x00 + +[1A01sub2] +ParameterName=PDO Mapping Entry +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=const +DefaultValue=0x64010210 +PDOMapping=0 +ObjFlags=0x00 + +[1A01sub3] +ParameterName=PDO Mapping Entry +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=const +DefaultValue=0x64010310 +PDOMapping=0 +ObjFlags=0x00 + +[1A01sub4] +ParameterName=PDO Mapping Entry +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=const +DefaultValue=0x64010410 +PDOMapping=0 +ObjFlags=0x00 + +[1A02] +ParameterName=Transmit PDO3 Mapping Parameter +SubNumber=5 +ObjectType=0x08 + +[1A02sub0] +ParameterName=Number of Entries +ObjectType=0x07 +DataType=0x0005 +LowLimit=0 +HighLimit=64 +AccessType=const +DefaultValue=0x04 +PDOMapping=0 +ObjFlags=0x00 + +[1A02sub1] +ParameterName=PDO Mapping Entry +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=const +DefaultValue=0x64010510 +PDOMapping=0 +ObjFlags=0x00 + +[1A02sub2] +ParameterName=PDO Mapping Entry +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=const +DefaultValue=0x64010610 +PDOMapping=0 +ObjFlags=0x00 + +[1A02sub3] +ParameterName=PDO Mapping Entry +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=const +DefaultValue=0x64010710 +PDOMapping=0 +ObjFlags=0x00 + +[1A02sub4] +ParameterName=PDO Mapping Entry +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=const +DefaultValue=0x64010810 +PDOMapping=0 +ObjFlags=0x00 + +[1A03] +ParameterName=Transmit PDO4 Mapping Parameter +SubNumber=5 +ObjectType=0x08 + +[1A03sub0] +ParameterName=Number of Entries +ObjectType=0x07 +DataType=0x0005 +LowLimit=0 +HighLimit=64 +AccessType=const +DefaultValue=0x04 +PDOMapping=0 +ObjFlags=0x00 + +[1A03sub1] +ParameterName=PDO Mapping Entry +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=const +DefaultValue=0x64010910 +PDOMapping=0 +ObjFlags=0x00 + +[1A03sub2] +ParameterName=PDO Mapping Entry +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=const +DefaultValue=0x64010A10 +PDOMapping=0 +ObjFlags=0x00 + +[1A03sub3] +ParameterName=PDO Mapping Entry +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=const +DefaultValue=0x64010B10 +PDOMapping=0 +ObjFlags=0x00 + +[1A03sub4] +ParameterName=PDO Mapping Entry +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=const +DefaultValue=0x64010C10 +PDOMapping=0 +ObjFlags=0x00 + +[5F00] +ParameterName=Device Status +SubNumber=6 +ObjectType=0x09 + +[5F00sub0] +ParameterName=Highest Subindex +ObjectType=0x07 +DataType=0x0005 +LowLimit= +HighLimit= +AccessType=const +DefaultValue=0x05 +PDOMapping=0 +ObjFlags=0x00 + +[5F00sub1] +ParameterName=Own Node ID +ObjectType=0x07 +DataType=0x0005 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue=0x00 +PDOMapping=0 +ObjFlags=0x00 + +[5F00sub2] +ParameterName=Own NMT status +ObjectType=0x07 +DataType=0x0005 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue=0x00 +PDOMapping=0 +ObjFlags=0x00 + +[5F00sub3] +ParameterName=Own HW status +ObjectType=0x07 +DataType=0x0005 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue=0x00 +PDOMapping=0 +ObjFlags=0x00 + +[5F00sub4] +ParameterName=Version +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue=0x01010201 +PDOMapping=0 +ObjFlags=0x00 + +[5F00sub5] +ParameterName=Chip ID +ObjectType=0x07 +DataType=0x000F +LowLimit= +HighLimit= +AccessType=ro +DefaultValue= +PDOMapping=0 +ObjFlags=0x00 + +[5F01] +ParameterName=Device Control +SubNumber=12 +ObjectType=0x09 + +[5F01sub0] +ParameterName=Highest Subindex +ObjectType=0x07 +DataType=0x0005 +LowLimit= +HighLimit= +AccessType=const +DefaultValue=0x0B +PDOMapping=0 +ObjFlags=0x00 + +[5F01sub1] +ParameterName=Reset +ObjectType=0x07 +DataType=0x0005 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue=0x00 +PDOMapping=0 +ObjFlags=0x00 + +[5F01sub2] +ParameterName=Sleep Objection +ObjectType=0x07 +DataType=0x0005 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue=0x00 +PDOMapping=0 +ObjFlags=0x00 + +[5F01sub3] +ParameterName=Ignore VD PDO +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue=0x00000000 +PDOMapping=0 +ObjFlags=0x00 + +[5F01sub4] +ParameterName=Manager Control +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue=0x2020001F +PDOMapping=0 +ObjFlags=0x00 + +[5F01sub5] +ParameterName=Default HB producer +ObjectType=0x07 +DataType=0x0006 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue=1000 +PDOMapping=0 +ObjFlags=0x00 + +[5F01sub6] +ParameterName=Default HB consumer +ObjectType=0x07 +DataType=0x0006 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue=2500 +PDOMapping=0 +ObjFlags=0x00 + +[5F01sub7] +ParameterName=PDO update time +ObjectType=0x07 +DataType=0x0006 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue=70 +PDOMapping=0 +ObjFlags=0x00 + +[5F01sub8] +ParameterName=PDO event time +ObjectType=0x07 +DataType=0x0006 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue=0 +PDOMapping=0 +ObjFlags=0x00 + +[5F01sub9] +ParameterName=PDO inhibit time +ObjectType=0x07 +DataType=0x0006 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue=0 +PDOMapping=0 +ObjFlags=0x00 + +[5F01subA] +ParameterName=Manager re-scan +ObjectType=0x07 +DataType=0x0005 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue=0 +PDOMapping=0 +ObjFlags=0x00 + +[5F01subB] +ParameterName=Generic RxTx +ObjectType=0x07 +DataType=0x0005 +LowLimit= +HighLimit= +AccessType=rw +DefaultValue=0 +PDOMapping=0 +ObjFlags=0x00 + +[5FF5] +ParameterName=MCOP Diagnostics +SubNumber=23 +ObjectType=0x09 + +[5FF5sub0] +ParameterName=Highest Subindex +ObjectType=0x07 +DataType=0x0005 +LowLimit= +HighLimit= +AccessType=const +DefaultValue=0x16 +PDOMapping=0 +ObjFlags=0x00 + +[5FF5sub1] +ParameterName=Identification +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=const +DefaultValue=0x01355341 +PDOMapping=0 +ObjFlags=0x00 + +[5FF5sub2] +ParameterName=Version +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue=0xFFFFFFFF +PDOMapping=0 +ObjFlags=0x00 + +[5FF5sub3] +ParameterName=Functionality +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue=0xFFFFFFFF +PDOMapping=0 +ObjFlags=0x00 + +[5FF5sub4] +ParameterName=Status +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue=0xFFFFFFFF +PDOMapping=0 +ObjFlags=0x00 + +[5FF5sub5] +ParameterName=TxFIFOstatus +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue=0 +PDOMapping=0 +ObjFlags=0x00 + +[5FF5sub6] +ParameterName=RxFIFOstatus +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue=0 +PDOMapping=0 +ObjFlags=0x00 + +[5FF5sub7] +ParameterName=RxMgrFIFOstatus +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue=0 +PDOMapping=0 +ObjFlags=0x00 + +[5FF5sub8] +ParameterName=ProcPerSecCur +ObjectType=0x07 +DataType=0x0006 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue=0x0000 +PDOMapping=0 +ObjFlags=0x00 + +[5FF5sub9] +ParameterName=ProcPerSecMin +ObjectType=0x07 +DataType=0x0006 +LowLimit= +HighLimit= +AccessType=rw +DefaultValue=0xFFFF +PDOMapping=0 +ObjFlags=0x00 + +[5FF5subA] +ParameterName=ProcPerSecMax +ObjectType=0x07 +DataType=0x0006 +LowLimit= +HighLimit= +AccessType=rw +DefaultValue=0x0000 +PDOMapping=0 +ObjFlags=0x00 + +[5FF5subB] +ParameterName=ProcBurstMax +ObjectType=0x07 +DataType=0x0006 +LowLimit= +HighLimit= +AccessType=rw +DefaultValue=0x0000 +PDOMapping=0 +ObjFlags=0x00 + +[5FF5subC] +ParameterName=ProcRxPerSecCur +ObjectType=0x07 +DataType=0x0006 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue=0x0000 +PDOMapping=0 +ObjFlags=0x00 + +[5FF5subD] +ParameterName=ProcRxPerSecMin +ObjectType=0x07 +DataType=0x0006 +LowLimit= +HighLimit= +AccessType=rw +DefaultValue=0xFFFF +PDOMapping=0 +ObjFlags=0x00 + +[5FF5subE] +ParameterName=ProcRxPerSecMax +ObjectType=0x07 +DataType=0x0006 +LowLimit= +HighLimit= +AccessType=rw +DefaultValue=0x0000 +PDOMapping=0 +ObjFlags=0x00 + +[5FF5subF] +ParameterName=Reserved +ObjectType=0x07 +DataType=0x0005 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue=0xFF +PDOMapping=0 +ObjFlags=0x00 + +[5FF5sub10] +ParameterName=MgrPerSecCur +ObjectType=0x07 +DataType=0x0006 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue=0xFFFF +PDOMapping=0 +ObjFlags=0x00 + +[5FF5sub11] +ParameterName=MgrPerSecMin +ObjectType=0x07 +DataType=0x0006 +LowLimit= +HighLimit= +AccessType=rw +DefaultValue=0xFFFF +PDOMapping=0 +ObjFlags=0x00 + +[5FF5sub12] +ParameterName=MgrPerSecMax +ObjectType=0x07 +DataType=0x0006 +LowLimit= +HighLimit= +AccessType=rw +DefaultValue=0x0000 +PDOMapping=0 +ObjFlags=0x00 + +[5FF5sub13] +ParameterName=MgrBurstMax +ObjectType=0x07 +DataType=0x0006 +LowLimit= +HighLimit= +AccessType=rw +DefaultValue=0x0000 +PDOMapping=0 +ObjFlags=0x00 + +[5FF5sub14] +ParameterName=MgrRxPerSecCur +ObjectType=0x07 +DataType=0x0006 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue=0x0000 +PDOMapping=0 +ObjFlags=0x00 + +[5FF5sub15] +ParameterName=MgrRxPerSecMin +ObjectType=0x07 +DataType=0x0006 +LowLimit= +HighLimit= +AccessType=rw +DefaultValue=0x0000 +PDOMapping=0 +ObjFlags=0x00 + +[5FF5sub16] +ParameterName=MgrRxPerSecMax +ObjectType=0x07 +DataType=0x0006 +LowLimit= +HighLimit= +AccessType=rw +DefaultValue=0x0000 +PDOMapping=0 +ObjFlags=0x00 + +[6000] +ParameterName=Read State 8 Input Lines +SubNumber=9 +ObjectType=0x08 + +[6000sub0] +ParameterName=Number of Elements +ObjectType=0x07 +DataType=0x0005 +LowLimit= +HighLimit= +AccessType=const +DefaultValue=0x08 +PDOMapping=0 +ObjFlags=0x00 + +[6000sub1] +ParameterName=DigInput8_1 +ObjectType=0x07 +DataType=0x0005 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue=0x12 +PDOMapping=1 +ObjFlags=0x00 + +[6000sub2] +ParameterName=DigInput8_2 +ObjectType=0x07 +DataType=0x0005 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue=0x23 +PDOMapping=1 +ObjFlags=0x00 + +[6000sub3] +ParameterName=DigInput8_3 +ObjectType=0x07 +DataType=0x0005 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue=0x34 +PDOMapping=1 +ObjFlags=0x00 + +[6000sub4] +ParameterName=DigInput8_4 +ObjectType=0x07 +DataType=0x0005 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue=0x45 +PDOMapping=1 +ObjFlags=0x00 + +[6000sub5] +ParameterName=DigInput8_5 +ObjectType=0x07 +DataType=0x0005 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue=0x56 +PDOMapping=1 +ObjFlags=0x00 + +[6000sub6] +ParameterName=DigInput8_6 +ObjectType=0x07 +DataType=0x0005 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue=0x67 +PDOMapping=1 +ObjFlags=0x00 + +[6000sub7] +ParameterName=DigInput8_7 +ObjectType=0x07 +DataType=0x0005 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue=0x78 +PDOMapping=1 +ObjFlags=0x00 + +[6000sub8] +ParameterName=DigInput8_8 +ObjectType=0x07 +DataType=0x0005 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue=0x89 +PDOMapping=1 +ObjFlags=0x00 + +[6200] +ParameterName=Write State 8 Output Lines +SubNumber=9 +ObjectType=0x08 + +[6200sub0] +ParameterName=Number of Elements +ObjectType=0x07 +DataType=0x0005 +LowLimit= +HighLimit= +AccessType=const +DefaultValue=0x08 +PDOMapping=0 +ObjFlags=0x00 + +[6200sub1] +ParameterName=DigOutput8_1 +ObjectType=0x07 +DataType=0x0005 +LowLimit= +HighLimit= +AccessType=rww +DefaultValue= +PDOMapping=1 +ObjFlags=0x00 + +[6200sub2] +ParameterName=DigOutput8_2 +ObjectType=0x07 +DataType=0x0005 +LowLimit= +HighLimit= +AccessType=rww +DefaultValue= +PDOMapping=1 +ObjFlags=0x00 + +[6200sub3] +ParameterName=DigOutput8_3 +ObjectType=0x07 +DataType=0x0005 +LowLimit= +HighLimit= +AccessType=rww +DefaultValue= +PDOMapping=1 +ObjFlags=0x00 + +[6200sub4] +ParameterName=DigOutput8_4 +ObjectType=0x07 +DataType=0x0005 +LowLimit= +HighLimit= +AccessType=rww +DefaultValue= +PDOMapping=1 +ObjFlags=0x00 + +[6200sub5] +ParameterName=DigOutput8_5 +ObjectType=0x07 +DataType=0x0005 +LowLimit= +HighLimit= +AccessType=rww +DefaultValue= +PDOMapping=1 +ObjFlags=0x00 + +[6200sub6] +ParameterName=DigOutput8_6 +ObjectType=0x07 +DataType=0x0005 +LowLimit= +HighLimit= +AccessType=rww +DefaultValue= +PDOMapping=1 +ObjFlags=0x00 + +[6200sub7] +ParameterName=DigOutput8_7 +ObjectType=0x07 +DataType=0x0005 +LowLimit= +HighLimit= +AccessType=rww +DefaultValue= +PDOMapping=1 +ObjFlags=0x00 + +[6200sub8] +ParameterName=DigOutput8_8 +ObjectType=0x07 +DataType=0x0005 +LowLimit= +HighLimit= +AccessType=rww +DefaultValue= +PDOMapping=1 +ObjFlags=0x00 + +[6401] +ParameterName=Read Analog Input 16-bit +SubNumber=13 +ObjectType=0x08 + +[6401sub0] +ParameterName=Number of Elements +ObjectType=0x07 +DataType=0x0005 +LowLimit= +HighLimit= +AccessType=const +DefaultValue=0x0C +PDOMapping=0 +ObjFlags=0x00 + +[6401sub1] +ParameterName=AnalogInput16_1 +ObjectType=0x07 +DataType=0x0003 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue=0x015A +PDOMapping=1 +ObjFlags=0x00 + +[6401sub2] +ParameterName=AnalogInput16_2 +ObjectType=0x07 +DataType=0x0003 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue=0x025A +PDOMapping=1 +ObjFlags=0x00 + +[6401sub3] +ParameterName=AnalogInput16_3 +ObjectType=0x07 +DataType=0x0003 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue=0x035A +PDOMapping=1 +ObjFlags=0x00 + +[6401sub4] +ParameterName=AnalogInput16_4 +ObjectType=0x07 +DataType=0x0003 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue=0x045A +PDOMapping=1 +ObjFlags=0x00 + +[6401sub5] +ParameterName=AnalogInput16_5 +ObjectType=0x07 +DataType=0x0003 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue=0x055A +PDOMapping=1 +ObjFlags=0x00 + +[6401sub6] +ParameterName=AnalogInput16_6 +ObjectType=0x07 +DataType=0x0003 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue=0x065A +PDOMapping=1 +ObjFlags=0x00 + +[6401sub7] +ParameterName=AnalogInput16_7 +ObjectType=0x07 +DataType=0x0003 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue=0x075A +PDOMapping=1 +ObjFlags=0x00 + +[6401sub8] +ParameterName=AnalogInput16_8 +ObjectType=0x07 +DataType=0x0003 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue=0x085A +PDOMapping=1 +ObjFlags=0x00 + +[6401sub9] +ParameterName=AnalogInput16_9 +ObjectType=0x07 +DataType=0x0003 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue=0x095A +PDOMapping=1 +ObjFlags=0x00 + +[6401subA] +ParameterName=AnalogInput16_10 +ObjectType=0x07 +DataType=0x0003 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue=0x0A5A +PDOMapping=1 +ObjFlags=0x00 + +[6401subB] +ParameterName=AnalogInput16_11 +ObjectType=0x07 +DataType=0x0003 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue=0x0B5A +PDOMapping=1 +ObjFlags=0x00 + +[6401subC] +ParameterName=AnalogInput16_12 +ObjectType=0x07 +DataType=0x0003 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue=0x0C5A +PDOMapping=1 +ObjFlags=0x00 + +[6411] +ParameterName=Write Analog Output 16-bit +SubNumber=13 +ObjectType=0x08 + +[6411sub0] +ParameterName=Number of Elements +ObjectType=0x07 +DataType=0x0005 +LowLimit= +HighLimit= +AccessType=const +DefaultValue=0x0C +PDOMapping=0 +ObjFlags=0x00 + +[6411sub1] +ParameterName=AnalogOutput16_1 +ObjectType=0x07 +DataType=0x0003 +LowLimit= +HighLimit= +AccessType=rww +DefaultValue= +PDOMapping=1 +ObjFlags=0x00 + +[6411sub2] +ParameterName=AnalogOutput16_2 +ObjectType=0x07 +DataType=0x0003 +LowLimit= +HighLimit= +AccessType=rww +DefaultValue= +PDOMapping=1 +ObjFlags=0x00 + +[6411sub3] +ParameterName=AnalogOutput16_3 +ObjectType=0x07 +DataType=0x0003 +LowLimit= +HighLimit= +AccessType=rww +DefaultValue= +PDOMapping=1 +ObjFlags=0x00 + +[6411sub4] +ParameterName=AnalogOutput16_4 +ObjectType=0x07 +DataType=0x0003 +LowLimit= +HighLimit= +AccessType=rww +DefaultValue= +PDOMapping=1 +ObjFlags=0x00 + +[6411sub5] +ParameterName=AnalogOutput16_5 +ObjectType=0x07 +DataType=0x0003 +LowLimit= +HighLimit= +AccessType=rww +DefaultValue= +PDOMapping=1 +ObjFlags=0x00 + +[6411sub6] +ParameterName=AnalogOutput16_6 +ObjectType=0x07 +DataType=0x0003 +LowLimit= +HighLimit= +AccessType=rww +DefaultValue= +PDOMapping=1 +ObjFlags=0x00 + +[6411sub7] +ParameterName=AnalogOutput16_7 +ObjectType=0x07 +DataType=0x0003 +LowLimit= +HighLimit= +AccessType=rww +DefaultValue= +PDOMapping=1 +ObjFlags=0x00 + +[6411sub8] +ParameterName=AnalogOutput16_8 +ObjectType=0x07 +DataType=0x0003 +LowLimit= +HighLimit= +AccessType=rww +DefaultValue= +PDOMapping=1 +ObjFlags=0x00 + +[6411sub9] +ParameterName=AnalogOutput16_9 +ObjectType=0x07 +DataType=0x0003 +LowLimit= +HighLimit= +AccessType=rww +DefaultValue= +PDOMapping=1 +ObjFlags=0x00 + +[6411subA] +ParameterName=AnalogOutput16_10 +ObjectType=0x07 +DataType=0x0003 +LowLimit= +HighLimit= +AccessType=rww +DefaultValue= +PDOMapping=1 +ObjFlags=0x00 + +[6411subB] +ParameterName=AnalogOutput16_11 +ObjectType=0x07 +DataType=0x0003 +LowLimit= +HighLimit= +AccessType=rww +DefaultValue= +PDOMapping=1 +ObjFlags=0x00 + +[6411subC] +ParameterName=AnalogOutput16_12 +ObjectType=0x07 +DataType=0x0003 +LowLimit= +HighLimit= +AccessType=rww +DefaultValue= +PDOMapping=1 +ObjFlags=0x00 + diff --git a/Devices/Packages/sick_line_guidance/test/eds/CiA401_minimal.eds b/Devices/Packages/sick_line_guidance/test/eds/CiA401_minimal.eds new file mode 100755 index 0000000..e55a7ad --- /dev/null +++ b/Devices/Packages/sick_line_guidance/test/eds/CiA401_minimal.eds @@ -0,0 +1,143 @@ +[FileInfo] +FileName=CiA401_minimal.eds +FileVersion=1 +FileRevision=1 +EDSVersion=4.0 +Description=CANopenIA CiA401 Minimal Example +CreatedBy=rostest +CreationTime=11:00AM +CreationDate=13-02-2019 +ModifiedBy=rostest +ModificationTime=11:00AM +ModificationDate=13-02-2019 + +[DeviceInfo] +VendorName=rostest +VendorNumber=0x01000056 +ProductName=rostest +ProductNumber=0x00001004 +RevisionNumber=1 +OrderCode=- +BaudRate_10=1 +BaudRate_20=1 +BaudRate_50=1 +BaudRate_125=1 +BaudRate_250=1 +BaudRate_500=1 +BaudRate_800=0 +BaudRate_1000=1 +DynamicChannelsSupported=0 +GroupMessaging=0 +LSS_Supported=0 +Granularity=0 +SimpleBootUpSlave=1 +SimpleBootUpMaster=0 +NrOfRXPDO=0 +NrOfTXPDO=2 +CompactPDO=0x00 + +[Comments] +Lines=0 + +[DummyUsage] +Dummy0001=0 +Dummy0002=0 +Dummy0003=0 +Dummy0004=0 +Dummy0005=0 +Dummy0006=0 +Dummy0007=0 + +[MandatoryObjects] +SupportedObjects=3 +1=0x1000 +2=0x1001 +3=0x1018 + +[OptionalObjects] +SupportedObjects=0 + +[ManufacturerObjects] +SupportedObjects=0 + +[1000] +ParameterName=Device Type +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=const +DefaultValue=0x000F0191 +PDOMapping=0 +ObjFlags=0x00 + +[1001] +ParameterName=Error Register +ObjectType=0x07 +DataType=0x0005 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue= +PDOMapping=0 +ObjFlags=0x00 + +[1018] +ParameterName=Identity Object +SubNumber=5 +ObjectType=0x08 + +[1018sub0] +ParameterName=number of entries +ObjectType=0x07 +DataType=0x0005 +LowLimit=1 +HighLimit=4 +AccessType=const +DefaultValue=4 +PDOMapping=0 +ObjFlags=0x00 + +[1018sub1] +ParameterName=Vendor ID +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=const +DefaultValue=0x01455341 +PDOMapping=0 +ObjFlags=0x00 + +[1018sub2] +ParameterName=Product Code +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=const +DefaultValue=0xC01A0401 +PDOMapping=0 +ObjFlags=0x00 + +[1018sub3] +ParameterName=Revision number +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue=0xFFFFFFFF +PDOMapping=0 +ObjFlags=0x00 + +[1018sub4] +ParameterName=Serial number +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue=0xFFFFFFFF +PDOMapping=0 +ObjFlags=0x00 diff --git a/Devices/Packages/sick_line_guidance/test/eds/CiA402_Stepper_Node8.bin b/Devices/Packages/sick_line_guidance/test/eds/CiA402_Stepper_Node8.bin new file mode 100755 index 0000000..09e8900 Binary files /dev/null and b/Devices/Packages/sick_line_guidance/test/eds/CiA402_Stepper_Node8.bin differ diff --git a/Devices/Packages/sick_line_guidance/test/eds/CiA402_Stepper_Node8.eds b/Devices/Packages/sick_line_guidance/test/eds/CiA402_Stepper_Node8.eds new file mode 100755 index 0000000..8eb71d4 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/test/eds/CiA402_Stepper_Node8.eds @@ -0,0 +1,1760 @@ +[FileInfo] +FileName=C:\Users\Andy\Documents\ESA Projects\CANopenIA\EDS_shared\CiA402_Stepper_Node8.eds +FileVersion=1 +FileRevision=10 +EDSVersion=4.0 +Description=CANopenIA CiA402 Stepper Drive Configuration Example +CreatedBy=ESAcademy +CreationTime=11:18AM +CreationDate=07-03-2003 +ModifiedBy=EScademy +ModificationTime=09:21AM +ModificationDate=02-01-2019 + +[Comments] +Lines=2 +Line1=ESAcademy EDS example for a Stepper Drive. +Line2=For more information, see www.CANopenIA.com and www.CANgineBerry.com + +[DeviceInfo] +Vendorname=ESAcademy +ProductName=www.cangineberry.com +OrderCode=ESS-CAG-BRY EAN:0610098078999 +VendorNumber=0x01455341 +ProductNumber=0xC01A0402 +RevisionNumber=0xFFFFFFFF +BaudRate_10=0 +BaudRate_20=0 +BaudRate_50=0 +BaudRate_125=1 +BaudRate_250=1 +BaudRate_500=1 +BaudRate_800=0 +BaudRate_1000=1 +SimpleBootupMaster=0 +SimpleBootupSlave=1 +GroupMessaging=0 +LSS_Supported=0 +Granularity=0 +DynamicChannelsSupported=0 +NrOfRXPDO=6 +NrOfTXPDO=2 +CompactPDO=0x00 + +[DummyUsage] +Dummy0001=0 +Dummy0002=0 +Dummy0003=0 +Dummy0004=0 +Dummy0005=1 +Dummy0006=1 +Dummy0007=1 + +[MandatoryObjects] +SupportedObjects=3 +1=0x1000 +2=0x1001 +3=0x1018 + +[OptionalObjects] +SupportedObjects=45 +1=0x1002 +2=0x1005 +3=0x1008 +4=0x1009 +5=0x100A +6=0x100C +7=0x100D +8=0x1016 +9=0x1017 +10=0x1400 +11=0x1401 +12=0x1402 +13=0x1403 +14=0x1404 +15=0x1405 +16=0x1600 +17=0x1601 +18=0x1602 +19=0x1603 +20=0x1604 +21=0x1605 +22=0x1800 +23=0x1801 +24=0x1A00 +25=0x1A01 +26=0x6040 +27=0x6041 +28=0x6042 +29=0x6043 +30=0x6044 +31=0x6046 +32=0x6048 +33=0x6049 +34=0x6060 +35=0x6061 +36=0x607A +37=0x607C +38=0x6081 +39=0x6083 +40=0x6084 +41=0x6086 +42=0x6095 +43=0x6097 +44=0x6402 +45=0x6502 + +[ManufacturerObjects] +SupportedObjects=3 +1=0x5F00 +2=0x5F01 +3=0x5FF5 + +[1000] +ParameterName=Device Type +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=const +DefaultValue=0x00000192 +PDOMapping=0 +ObjFlags=0x00 + +[1001] +ParameterName=Error Register +ObjectType=0x07 +DataType=0x0005 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue=0 +PDOMapping=0 +ObjFlags=0x00 + +[1002] +ParameterName=Manufacturer Status Register +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue= +PDOMapping=0 +ObjFlags=0x00 + +[1005] +ParameterName=COB-ID SYNC +ObjectType=0x07 +DataType=0x0007 +LowLimit=0x00000001 +HighLimit= +AccessType=rw +DefaultValue=0x00000080 +PDOMapping=0 +ObjFlags=0x00 + +[1008] +ParameterName=Manufacturer Device Name +ObjectType=0x07 +DataType=0x0009 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue=CANopenIA CiA402 Demo +PDOMapping=0 +ObjFlags=0x00 + +[1009] +ParameterName=Manufacturer Hardware Version +ObjectType=0x07 +DataType=0x0009 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue=V1.0 +PDOMapping=0 +ObjFlags=0x00 + +[100A] +ParameterName=Manufacturer Software Version +ObjectType=0x07 +DataType=0x0009 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue=V1.0 +PDOMapping=0 +ObjFlags=0x00 + +[100C] +ParameterName=Guard Time +ObjectType=0x07 +DataType=0x0006 +LowLimit= +HighLimit= +AccessType=rw +DefaultValue=0 +PDOMapping=0 +ObjFlags=0x00 + +[100D] +ParameterName=Life Time Factor +ObjectType=0x07 +DataType=0x0005 +LowLimit= +HighLimit= +AccessType=rw +DefaultValue=0x00 +PDOMapping=0 +ObjFlags=0x00 + +[1016] +ParameterName=Consumer Heartbeat Time +SubNumber=4 +ObjectType=0x08 + +[1016sub0] +ParameterName=Number of Entries +ObjectType=0x07 +DataType=0x0005 +LowLimit= +HighLimit= +AccessType=const +DefaultValue=0x03 +PDOMapping=0 +ObjFlags=0x00 + +[1016sub1] +ParameterName=Consumer Heartbeat Time 1 +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=rw +DefaultValue=0 +PDOMapping=0 +ObjFlags=0x00 + +[1016sub2] +ParameterName=Consumer Heartbeat Time 2 +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=rw +DefaultValue=0 +PDOMapping=0 +ObjFlags=0x00 + +[1016sub3] +ParameterName=Consumer Heartbeat Time 3 +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=rw +DefaultValue=0 +PDOMapping=0 +ObjFlags=0x00 + +[1017] +ParameterName=Producer Heartbeat Time +ObjectType=0x07 +DataType=0x0006 +LowLimit= +HighLimit= +AccessType=rw +DefaultValue=0 +PDOMapping=0 +ObjFlags=0x00 + +[1018] +ParameterName=Identity Object +SubNumber=5 +ObjectType=0x08 + +[1018sub0] +ParameterName=Number of entries +ObjectType=0x07 +DataType=0x0005 +LowLimit=1 +HighLimit=4 +AccessType=const +DefaultValue=4 +PDOMapping=0 +ObjFlags=0x00 + +[1018sub1] +ParameterName=Vendor ID +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=const +DefaultValue=0x01455341 +PDOMapping=0 +ObjFlags=0x00 + +[1018sub2] +ParameterName=Product code +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=const +DefaultValue=0xC01A0402 +PDOMapping=0 +ObjFlags=0x00 + +[1018sub3] +ParameterName=Revision number +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue=0xFFFFFFFF +PDOMapping=0 +ObjFlags=0x00 + +[1018sub4] +ParameterName=Serial number +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue=0xFFFFFFFF +PDOMapping=0 +ObjFlags=0x00 + +[1400] +ParameterName=Receive PDO Communication Parameter +SubNumber=3 +ObjectType=0x09 + +[1400sub0] +ParameterName=Number of entries +ObjectType=0x07 +DataType=0x0005 +LowLimit=0x02 +HighLimit=0x05 +AccessType=ro +DefaultValue=2 +PDOMapping=0 +ObjFlags=0x00 + +[1400sub1] +ParameterName=COB-ID +ObjectType=0x07 +DataType=0x0007 +LowLimit=0x00000001 +HighLimit=0xFFFFFFFF +AccessType=rw +DefaultValue=$NODEID+0x200 +PDOMapping=0 +ObjFlags=0x00 + +[1400sub2] +ParameterName=Transmission Type +ObjectType=0x07 +DataType=0x0005 +LowLimit= +HighLimit= +AccessType=rw +DefaultValue=255 +PDOMapping=0 +ObjFlags=0x00 + +[1401] +ParameterName=Receive PDO Communication Parameter +SubNumber=3 +ObjectType=0x09 + +[1401sub0] +ParameterName=Number of entries +ObjectType=0x07 +DataType=0x0005 +LowLimit=0x02 +HighLimit=0x05 +AccessType=ro +DefaultValue=2 +PDOMapping=0 +ObjFlags=0x00 + +[1401sub1] +ParameterName=COB-ID +ObjectType=0x07 +DataType=0x0007 +LowLimit=0x00000001 +HighLimit=0xFFFFFFFF +AccessType=rw +DefaultValue=$NODEID+0x300 +PDOMapping=0 +ObjFlags=0x00 + +[1401sub2] +ParameterName=Transmission Type +ObjectType=0x07 +DataType=0x0005 +LowLimit= +HighLimit= +AccessType=rw +DefaultValue=255 +PDOMapping=0 +ObjFlags=0x00 + +[1402] +ParameterName=Receive PDO Communication Parameter +SubNumber=3 +ObjectType=0x09 + +[1402sub0] +ParameterName=Number of entries +ObjectType=0x07 +DataType=0x0005 +LowLimit=0x02 +HighLimit=0x05 +AccessType=ro +DefaultValue=2 +PDOMapping=0 +ObjFlags=0x00 + +[1402sub1] +ParameterName=COB-ID +ObjectType=0x07 +DataType=0x0007 +LowLimit=0x00000001 +HighLimit=0xFFFFFFFF +AccessType=rw +DefaultValue=$NODEID+0x400 +PDOMapping=0 +ObjFlags=0x00 + +[1402sub2] +ParameterName=Transmission Type +ObjectType=0x07 +DataType=0x0005 +LowLimit= +HighLimit= +AccessType=rw +DefaultValue=255 +PDOMapping=0 +ObjFlags=0x00 + +[1403] +ParameterName=Receive PDO Communication Parameter +SubNumber=3 +ObjectType=0x09 + +[1403sub0] +ParameterName=Number of entries +ObjectType=0x07 +DataType=0x0005 +LowLimit=0x02 +HighLimit=0x05 +AccessType=ro +DefaultValue=2 +PDOMapping=0 +ObjFlags=0x00 + +[1403sub1] +ParameterName=COB-ID +ObjectType=0x07 +DataType=0x0007 +LowLimit=0x00000001 +HighLimit=0xFFFFFFFF +AccessType=rw +DefaultValue=$NODEID+0x500 +PDOMapping=0 +ObjFlags=0x00 + +[1403sub2] +ParameterName=Transmission Type +ObjectType=0x07 +DataType=0x0005 +LowLimit= +HighLimit= +AccessType=rw +DefaultValue=255 +PDOMapping=0 +ObjFlags=0x00 + +[1404] +ParameterName=Receive PDO Communication Parameter +SubNumber=3 +ObjectType=0x09 + +[1404sub0] +ParameterName=Number of Entries +ObjectType=0x07 +DataType=0x0005 +LowLimit=0x02 +HighLimit=0x05 +AccessType=ro +DefaultValue=2 +PDOMapping=0 +ObjFlags=0x00 + +[1404sub1] +ParameterName=COB-ID +ObjectType=0x07 +DataType=0x0007 +LowLimit=0x00000001 +HighLimit=0xFFFFFFFF +AccessType=rw +DefaultValue=0x80000000 +PDOMapping=0 +ObjFlags=0x00 + +[1404sub2] +ParameterName=Transmission Type +ObjectType=0x07 +DataType=0x0005 +LowLimit= +HighLimit= +AccessType=rw +DefaultValue= +PDOMapping=0 +ObjFlags=0x00 + +[1405] +ParameterName=Receive PDO Communication Parameter +SubNumber=3 +ObjectType=0x09 + +[1405sub0] +ParameterName=Number of Entries +ObjectType=0x07 +DataType=0x0005 +LowLimit=0x02 +HighLimit=0x05 +AccessType=ro +DefaultValue=2 +PDOMapping=0 +ObjFlags=0x00 + +[1405sub1] +ParameterName=COB-ID +ObjectType=0x07 +DataType=0x0007 +LowLimit=0x00000001 +HighLimit=0xFFFFFFFF +AccessType=rw +DefaultValue=0x80000000 +PDOMapping=0 +ObjFlags=0x00 + +[1405sub2] +ParameterName=Transmission Type +ObjectType=0x07 +DataType=0x0005 +LowLimit= +HighLimit= +AccessType=rw +DefaultValue= +PDOMapping=0 +ObjFlags=0x00 + +[1600] +ParameterName=Receive PDO Mapping Parameter +SubNumber=2 +ObjectType=0x08 + +[1600sub0] +ParameterName=Number of entries +ObjectType=0x07 +DataType=0x0005 +LowLimit=0 +HighLimit=64 +AccessType=ro +DefaultValue=1 +PDOMapping=0 +ObjFlags=0x00 + +[1600sub1] +ParameterName=PDO Mapping Entry +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=rw +DefaultValue=0x60400010 +PDOMapping=0 +ObjFlags=0x00 + +[1601] +ParameterName=Receive PDO Mapping Parameter +SubNumber=3 +ObjectType=0x08 + +[1601sub0] +ParameterName=Number of entries +ObjectType=0x07 +DataType=0x0005 +LowLimit=0 +HighLimit=64 +AccessType=ro +DefaultValue=2 +PDOMapping=0 +ObjFlags=0x00 + +[1601sub1] +ParameterName=PDO Mapping Entry +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=rw +DefaultValue=0x60400010 +PDOMapping=0 +ObjFlags=0x00 + +[1601sub2] +ParameterName=PDO Mapping Entry +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=rw +DefaultValue=0x60600008 +PDOMapping=0 +ObjFlags=0x00 + +[1602] +ParameterName=Receive PDO Mapping Parameter +SubNumber=3 +ObjectType=0x08 + +[1602sub0] +ParameterName=Number of entries +ObjectType=0x07 +DataType=0x0005 +LowLimit=0 +HighLimit=64 +AccessType=ro +DefaultValue=2 +PDOMapping=0 +ObjFlags=0x00 + +[1602sub1] +ParameterName=PDO Mapping Entry +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=rw +DefaultValue=0x60400010 +PDOMapping=0 +ObjFlags=0x00 + +[1602sub2] +ParameterName=PDO Mapping Entry +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=rw +DefaultValue=0x607A0020 +PDOMapping=0 +ObjFlags=0x00 + +[1603] +ParameterName=Receive PDO Mapping Parameter +SubNumber=2 +ObjectType=0x08 + +[1603sub0] +ParameterName=Number of entries +ObjectType=0x07 +DataType=0x0005 +LowLimit=0 +HighLimit=64 +AccessType=rw +DefaultValue=1 +PDOMapping=0 +ObjFlags=0x00 + +[1603sub1] +ParameterName=PDO Mapping Entry +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=rw +DefaultValue=0x60400010 +PDOMapping=0 +ObjFlags=0x00 + +[1604] +ParameterName=Receive PDO Mapping Parameter +SubNumber=2 +ObjectType=0x08 + +[1604sub0] +ParameterName=Number of Entries +ObjectType=0x07 +DataType=0x0005 +LowLimit=0 +HighLimit=64 +AccessType=rw +DefaultValue=1 +PDOMapping=0 +ObjFlags=0x00 + +[1604sub1] +ParameterName=PDO Mapping Entry +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=rw +DefaultValue=0x60400010 +PDOMapping=0 +ObjFlags=0x00 + +[1605] +ParameterName=Receive PDO Mapping Parameter +SubNumber=3 +ObjectType=0x08 + +[1605sub0] +ParameterName=Number of Entries +ObjectType=0x07 +DataType=0x0005 +LowLimit=0 +HighLimit=64 +AccessType=rw +DefaultValue=2 +PDOMapping=0 +ObjFlags=0x00 + +[1605sub1] +ParameterName=PDO Mapping Entry +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=rw +DefaultValue=0x60400010 +PDOMapping=0 +ObjFlags=0x00 + +[1605sub2] +ParameterName=PDO Mapping Entry +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=rw +DefaultValue=0x60420010 +PDOMapping=0 +ObjFlags=0x00 + +[1800] +ParameterName=Transmit PDO Communication Parameter +SubNumber=5 +ObjectType=0x09 + +[1800sub0] +ParameterName=Number of entries +ObjectType=0x07 +DataType=0x0005 +LowLimit=0x02 +HighLimit=0x05 +AccessType=ro +DefaultValue=5 +PDOMapping=0 +ObjFlags=0x00 + +[1800sub1] +ParameterName=COB-ID +ObjectType=0x07 +DataType=0x0007 +LowLimit=0x00000001 +HighLimit=0xFFFFFFFF +AccessType=rw +DefaultValue=$NODEID+0x40000180 +PDOMapping=0 +ObjFlags=0x00 + +[1800sub2] +ParameterName=Transmission Type +ObjectType=0x07 +DataType=0x0005 +LowLimit= +HighLimit= +AccessType=rw +DefaultValue=255 +PDOMapping=0 +ObjFlags=0x00 + +[1800sub3] +ParameterName=Inhibit Time +ObjectType=0x07 +DataType=0x0006 +LowLimit= +HighLimit= +AccessType=rw +DefaultValue=0 +PDOMapping=0 +ObjFlags=0x00 + +[1800sub5] +ParameterName=Event Timer +ObjectType=0x07 +DataType=0x0006 +LowLimit= +HighLimit= +AccessType=rw +DefaultValue=0 +PDOMapping=0 +ObjFlags=0x00 + +[1801] +ParameterName=Transmit PDO Communication Parameter +SubNumber=5 +ObjectType=0x09 + +[1801sub0] +ParameterName=Number of entries +ObjectType=0x07 +DataType=0x0005 +LowLimit=0x02 +HighLimit=0x05 +AccessType=ro +DefaultValue=5 +PDOMapping=0 +ObjFlags=0x00 + +[1801sub1] +ParameterName=COB-ID +ObjectType=0x07 +DataType=0x0007 +LowLimit=0x00000001 +HighLimit=0xFFFFFFFF +AccessType=rw +DefaultValue=$NODEID+0x40000280 +PDOMapping=0 +ObjFlags=0x00 + +[1801sub2] +ParameterName=Transmission Type +ObjectType=0x07 +DataType=0x0005 +LowLimit= +HighLimit= +AccessType=rw +DefaultValue=255 +PDOMapping=0 +ObjFlags=0x00 + +[1801sub3] +ParameterName=Inhibit Time +ObjectType=0x07 +DataType=0x0006 +LowLimit= +HighLimit= +AccessType=rw +DefaultValue=0 +PDOMapping=0 +ObjFlags=0x00 + +[1801sub5] +ParameterName=Event Timer +ObjectType=0x07 +DataType=0x0006 +LowLimit= +HighLimit= +AccessType=rw +DefaultValue=0 +PDOMapping=0 +ObjFlags=0x00 + +[1A00] +ParameterName=Transmit PDO Mapping Parameter +SubNumber=2 +ObjectType=0x08 + +[1A00sub0] +ParameterName=Number of entries +ObjectType=0x07 +DataType=0x0005 +LowLimit=0 +HighLimit=64 +AccessType=ro +DefaultValue=1 +PDOMapping=0 +ObjFlags=0x00 + +[1A00sub1] +ParameterName=PDO Mapping Entry +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=rw +DefaultValue=0x60410010 +PDOMapping=0 +ObjFlags=0x00 + +[1A01] +ParameterName=Transmit PDO Mapping Parameter +SubNumber=3 +ObjectType=0x08 + +[1A01sub0] +ParameterName=Number of entries +ObjectType=0x07 +DataType=0x0005 +LowLimit=0 +HighLimit=64 +AccessType=ro +DefaultValue=2 +PDOMapping=0 +ObjFlags=0x00 + +[1A01sub1] +ParameterName=PDO Mapping Entry +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=rw +DefaultValue=0x60410010 +PDOMapping=0 +ObjFlags=0x00 + +[1A01sub2] +ParameterName=PDO Mapping Entry +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=rw +DefaultValue=0x60610008 +PDOMapping=0 +ObjFlags=0x00 + +[5F00] +ParameterName=Device Status +SubNumber=6 +ObjectType=0x09 + +[5F00sub0] +ParameterName=Highest Subindex +ObjectType=0x07 +DataType=0x0005 +LowLimit= +HighLimit= +AccessType=const +DefaultValue=0x05 +PDOMapping=0 +ObjFlags=0x00 + +[5F00sub1] +ParameterName=Own Node ID +ObjectType=0x07 +DataType=0x0005 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue=0x00 +PDOMapping=0 +ObjFlags=0x00 + +[5F00sub2] +ParameterName=Own NMT status +ObjectType=0x07 +DataType=0x0005 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue=0x00 +PDOMapping=0 +ObjFlags=0x00 + +[5F00sub3] +ParameterName=Own HW status +ObjectType=0x07 +DataType=0x0005 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue=0x00 +PDOMapping=0 +ObjFlags=0x00 + +[5F00sub4] +ParameterName=Version +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue=0x01010201 +PDOMapping=0 +ObjFlags=0x00 + +[5F00sub5] +ParameterName=Chip ID +ObjectType=0x07 +DataType=0x000F +LowLimit= +HighLimit= +AccessType=ro +DefaultValue= +PDOMapping=0 +ObjFlags=0x00 + +[5F01] +ParameterName=Device Control +SubNumber=12 +ObjectType=0x09 + +[5F01sub0] +ParameterName=Highest Subindex +ObjectType=0x07 +DataType=0x0005 +LowLimit= +HighLimit= +AccessType=const +DefaultValue=0x0B +PDOMapping=0 +ObjFlags=0x00 + +[5F01sub1] +ParameterName=Reset +ObjectType=0x07 +DataType=0x0005 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue=0x00 +PDOMapping=0 +ObjFlags=0x00 + +[5F01sub2] +ParameterName=Sleep Objection +ObjectType=0x07 +DataType=0x0005 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue=0x00 +PDOMapping=0 +ObjFlags=0x00 + +[5F01sub3] +ParameterName=Ignore VD PDO +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue=0x00000000 +PDOMapping=0 +ObjFlags=0x00 + +[5F01sub4] +ParameterName=Manager Control +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue=0x2020001F +PDOMapping=0 +ObjFlags=0x00 + +[5F01sub5] +ParameterName=Default HB producer +ObjectType=0x07 +DataType=0x0006 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue=1000 +PDOMapping=0 +ObjFlags=0x00 + +[5F01sub6] +ParameterName=Default HB consumer +ObjectType=0x07 +DataType=0x0006 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue=2500 +PDOMapping=0 +ObjFlags=0x00 + +[5F01sub7] +ParameterName=PDO update time +ObjectType=0x07 +DataType=0x0006 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue=70 +PDOMapping=0 +ObjFlags=0x00 + +[5F01sub8] +ParameterName=PDO event time +ObjectType=0x07 +DataType=0x0006 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue=0 +PDOMapping=0 +ObjFlags=0x00 + +[5F01sub9] +ParameterName=PDO inhibit time +ObjectType=0x07 +DataType=0x0006 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue=0 +PDOMapping=0 +ObjFlags=0x00 + +[5F01subA] +ParameterName=Manager re-scan +ObjectType=0x07 +DataType=0x0005 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue=0 +PDOMapping=0 +ObjFlags=0x00 + +[5F01subB] +ParameterName=Generic RxTx +ObjectType=0x07 +DataType=0x0005 +LowLimit= +HighLimit= +AccessType=rw +DefaultValue=0 +PDOMapping=0 +ObjFlags=0x00 + +[5FF5] +ParameterName=MCOP Diagnostics +SubNumber=23 +ObjectType=0x09 + +[5FF5sub0] +ParameterName=Highest Subindex +ObjectType=0x07 +DataType=0x0005 +LowLimit= +HighLimit= +AccessType=const +DefaultValue=0x16 +PDOMapping=0 +ObjFlags=0x00 + +[5FF5sub1] +ParameterName=Identification +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=const +DefaultValue=0x01355341 +PDOMapping=0 +ObjFlags=0x00 + +[5FF5sub2] +ParameterName=Version +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue=0xFFFFFFFF +PDOMapping=0 +ObjFlags=0x00 + +[5FF5sub3] +ParameterName=Functionality +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue=0xFFFFFFFF +PDOMapping=0 +ObjFlags=0x00 + +[5FF5sub4] +ParameterName=Status +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue=0xFFFFFFFF +PDOMapping=0 +ObjFlags=0x00 + +[5FF5sub5] +ParameterName=TxFIFOstatus +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue=0 +PDOMapping=0 +ObjFlags=0x00 + +[5FF5sub6] +ParameterName=RxFIFOstatus +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue=0 +PDOMapping=0 +ObjFlags=0x00 + +[5FF5sub7] +ParameterName=RxMgrFIFOstatus +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue=0 +PDOMapping=0 +ObjFlags=0x00 + +[5FF5sub8] +ParameterName=ProcPerSecCur +ObjectType=0x07 +DataType=0x0006 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue=0x0000 +PDOMapping=0 +ObjFlags=0x00 + +[5FF5sub9] +ParameterName=ProcPerSecMin +ObjectType=0x07 +DataType=0x0006 +LowLimit= +HighLimit= +AccessType=rw +DefaultValue=0xFFFF +PDOMapping=0 +ObjFlags=0x00 + +[5FF5subA] +ParameterName=ProcPerSecMax +ObjectType=0x07 +DataType=0x0006 +LowLimit= +HighLimit= +AccessType=rw +DefaultValue=0x0000 +PDOMapping=0 +ObjFlags=0x00 + +[5FF5subB] +ParameterName=ProcBurstMax +ObjectType=0x07 +DataType=0x0006 +LowLimit= +HighLimit= +AccessType=rw +DefaultValue=0x0000 +PDOMapping=0 +ObjFlags=0x00 + +[5FF5subC] +ParameterName=ProcRxPerSecCur +ObjectType=0x07 +DataType=0x0006 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue=0x0000 +PDOMapping=0 +ObjFlags=0x00 + +[5FF5subD] +ParameterName=ProcRxPerSecMin +ObjectType=0x07 +DataType=0x0006 +LowLimit= +HighLimit= +AccessType=rw +DefaultValue=0xFFFF +PDOMapping=0 +ObjFlags=0x00 + +[5FF5subE] +ParameterName=ProcRxPerSecMax +ObjectType=0x07 +DataType=0x0006 +LowLimit= +HighLimit= +AccessType=rw +DefaultValue=0x0000 +PDOMapping=0 +ObjFlags=0x00 + +[5FF5subF] +ParameterName=Reserved +ObjectType=0x07 +DataType=0x0005 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue=0xFF +PDOMapping=0 +ObjFlags=0x00 + +[5FF5sub10] +ParameterName=MgrPerSecCur +ObjectType=0x07 +DataType=0x0006 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue=0xFFFF +PDOMapping=0 +ObjFlags=0x00 + +[5FF5sub11] +ParameterName=MgrPerSecMin +ObjectType=0x07 +DataType=0x0006 +LowLimit= +HighLimit= +AccessType=rw +DefaultValue=0xFFFF +PDOMapping=0 +ObjFlags=0x00 + +[5FF5sub12] +ParameterName=MgrPerSecMax +ObjectType=0x07 +DataType=0x0006 +LowLimit= +HighLimit= +AccessType=rw +DefaultValue=0x0000 +PDOMapping=0 +ObjFlags=0x00 + +[5FF5sub13] +ParameterName=MgrBurstMax +ObjectType=0x07 +DataType=0x0006 +LowLimit= +HighLimit= +AccessType=rw +DefaultValue=0x0000 +PDOMapping=0 +ObjFlags=0x00 + +[5FF5sub14] +ParameterName=MgrRxPerSecCur +ObjectType=0x07 +DataType=0x0006 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue=0x0000 +PDOMapping=0 +ObjFlags=0x00 + +[5FF5sub15] +ParameterName=MgrRxPerSecMin +ObjectType=0x07 +DataType=0x0006 +LowLimit= +HighLimit= +AccessType=rw +DefaultValue=0x0000 +PDOMapping=0 +ObjFlags=0x00 + +[5FF5sub16] +ParameterName=MgrRxPerSecMax +ObjectType=0x07 +DataType=0x0006 +LowLimit= +HighLimit= +AccessType=rw +DefaultValue=0x0000 +PDOMapping=0 +ObjFlags=0x00 + +[6040] +ParameterName=Control Word +ObjectType=0x07 +DataType=0x0006 +LowLimit= +HighLimit= +AccessType=rww +DefaultValue= +PDOMapping=1 +ObjFlags=0x00 + +[6041] +ParameterName=Status Word +ObjectType=0x07 +DataType=0x0006 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue= +PDOMapping=1 +ObjFlags=0x00 + +[6042] +ParameterName=Target Velocity +ObjectType=0x07 +DataType=0x0003 +LowLimit= +HighLimit= +AccessType=rww +DefaultValue= +PDOMapping=1 +ObjFlags=0x00 + +[6043] +ParameterName=Velocity Demand +ObjectType=0x07 +DataType=0x0003 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue= +PDOMapping=1 +ObjFlags=0x00 + +[6044] +ParameterName=Control Effort +ObjectType=0x07 +DataType=0x0003 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue= +PDOMapping=1 +ObjFlags=0x00 + +[6046] +ParameterName=Velocity Min Max Amount +SubNumber=3 +ObjectType=0x08 + +[6046sub0] +ParameterName=Number of Entries +ObjectType=0x07 +DataType=0x0005 +LowLimit= +HighLimit= +AccessType=const +DefaultValue=2 +PDOMapping=0 +ObjFlags=0x00 + +[6046sub1] +ParameterName=Velocity Min Amount +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=rww +DefaultValue= +PDOMapping=1 +ObjFlags=0x00 + +[6046sub2] +ParameterName=Velocity Max Amount +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=rww +DefaultValue= +PDOMapping=1 +ObjFlags=0x00 + +[6048] +ParameterName=Velocity Acceleration +SubNumber=3 +ObjectType=0x09 + +[6048sub0] +ParameterName=Number of Entries +ObjectType=0x07 +DataType=0x0005 +LowLimit= +HighLimit= +AccessType=const +DefaultValue=2 +PDOMapping=0 +ObjFlags=0x00 + +[6048sub1] +ParameterName=Delta Speed +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=rww +DefaultValue= +PDOMapping=1 +ObjFlags=0x00 + +[6048sub2] +ParameterName=Delta Time +ObjectType=0x07 +DataType=0x0006 +LowLimit= +HighLimit= +AccessType=rww +DefaultValue= +PDOMapping=1 +ObjFlags=0x00 + +[6049] +ParameterName=Velocity Deceleration +SubNumber=3 +ObjectType=0x09 + +[6049sub0] +ParameterName=Number of Entries +ObjectType=0x07 +DataType=0x0005 +LowLimit= +HighLimit= +AccessType=const +DefaultValue=2 +PDOMapping=0 +ObjFlags=0x00 + +[6049sub1] +ParameterName=Delta Speed +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=rww +DefaultValue= +PDOMapping=1 +ObjFlags=0x00 + +[6049sub2] +ParameterName=Delta Time +ObjectType=0x07 +DataType=0x0006 +LowLimit= +HighLimit= +AccessType=rww +DefaultValue= +PDOMapping=1 +ObjFlags=0x00 + +[6060] +ParameterName=Modes of Operation +ObjectType=0x07 +DataType=0x0002 +LowLimit= +HighLimit= +AccessType=rww +DefaultValue= +PDOMapping=1 +ObjFlags=0x00 + +[6061] +ParameterName=Modes of Operation Display +ObjectType=0x07 +DataType=0x0002 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue= +PDOMapping=1 +ObjFlags=0x00 + +[607A] +ParameterName=Target Position +ObjectType=0x07 +DataType=0x0004 +LowLimit= +HighLimit= +AccessType=rww +DefaultValue= +PDOMapping=1 +ObjFlags=0x00 + +[607C] +ParameterName=Home Offset +ObjectType=0x07 +DataType=0x0004 +LowLimit= +HighLimit= +AccessType=rww +DefaultValue= +PDOMapping=1 +ObjFlags=0x00 + +[6081] +ParameterName=Profile Velocity +ObjectType=0x07 +DataType=0x0004 +LowLimit= +HighLimit= +AccessType=rww +DefaultValue= +PDOMapping=1 +ObjFlags=0x00 + +[6083] +ParameterName=Profile Acceleration +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=rww +DefaultValue= +PDOMapping=1 +ObjFlags=0x00 + +[6084] +ParameterName=Profile Deceleration +ObjectType=0x07 +DataType=0x0004 +LowLimit= +HighLimit= +AccessType=rww +DefaultValue= +PDOMapping=1 +ObjFlags=0x00 + +[6086] +ParameterName=Motion Profile Type +ObjectType=0x07 +DataType=0x0003 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue= +PDOMapping=1 +ObjFlags=0x00 + +[6095] +ParameterName=Velocity Factor 1 +SubNumber=3 +ObjectType=0x08 + +[6095sub0] +ParameterName=Number of Entries +ObjectType=0x07 +DataType=0x0005 +LowLimit= +HighLimit= +AccessType=const +DefaultValue=2 +PDOMapping=0 +ObjFlags=0x00 + +[6095sub1] +ParameterName=Numerator +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=rww +DefaultValue= +PDOMapping=1 +ObjFlags=0x00 + +[6095sub2] +ParameterName=Divisor +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=rww +DefaultValue= +PDOMapping=1 +ObjFlags=0x00 + +[6097] +ParameterName=Acceleration Factor +SubNumber=3 +ObjectType=0x08 + +[6097sub0] +ParameterName=Number of Entries +ObjectType=0x07 +DataType=0x0005 +LowLimit= +HighLimit= +AccessType=const +DefaultValue=2 +PDOMapping=0 +ObjFlags=0x00 + +[6097sub1] +ParameterName=Numerator +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=rww +DefaultValue= +PDOMapping=1 +ObjFlags=0x00 + +[6097sub2] +ParameterName=Divisor +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=rww +DefaultValue= +PDOMapping=1 +ObjFlags=0x00 + +[6402] +ParameterName=Motor Type +ObjectType=0x07 +DataType=0x0006 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue= +PDOMapping=1 +ObjFlags=0x00 + +[6502] +ParameterName=Supported Drive Modes +ObjectType=0x07 +DataType=0x0007 +LowLimit= +HighLimit= +AccessType=ro +DefaultValue= +PDOMapping=1 +ObjFlags=0x00 + diff --git a/Devices/Packages/sick_line_guidance/test/eds/OLS20_experimental.eds b/Devices/Packages/sick_line_guidance/test/eds/OLS20_experimental.eds new file mode 100755 index 0000000..203a615 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/test/eds/OLS20_experimental.eds @@ -0,0 +1,787 @@ + +; This EDS file was created by the CANopen Design Tool 2.3.19.0. +; port GmbH Halle/Saale Germany, http://www.port.de, mailto:service@port.de +; DefaultValues added by rostest. + +[FileInfo] +FileName=OLS20.eds +FileVersion=1 +FileRevision=1 +EDSVersion=4.0 +Description=Optical Line Guidance +CreationTime=09:08AM +CreationDate=08-15-2018 +CreatedBy=herrmra +ModificationTime=10:51AM +ModificationDate=12-03-2018 +ModifiedBy=rostest + +[DeviceInfo] +VendorName=SICK AG +VendorNumber=0x01000056 +ProductName=OLS20 +ProductNumber=0x00001004 +RevisionNumber=1 +OrderCode=- +BaudRate_10=1 +BaudRate_20=1 +BaudRate_50=1 +BaudRate_125=1 +BaudRate_250=1 +BaudRate_500=1 +BaudRate_800=0 +BaudRate_1000=1 +DynamicChannelsSupported=0 +GroupMessaging=0 +LSS_Supported=1 +Granularity=8 +SimpleBootUpSlave=1 +SimpleBootUpMaster=0 +NrOfRXPDO=0 +NrOfTXPDO=2 + +[Comments] +Lines=0 + +[DummyUsage] +Dummy0001=0 +Dummy0002=0 +Dummy0003=0 +Dummy0004=0 +Dummy0005=0 +Dummy0006=0 +Dummy0007=0 + +[MandatoryObjects] +SupportedObjects=3 +1=0x1000 +2=0x1001 +3=0x1018 + +[OptionalObjects] +SupportedObjects=16 +1=0x1005 +2=0x1008 +3=0x1009 +4=0x100A +5=0x100C +6=0x100D +7=0x1014 +8=0x1015 +9=0x1016 +10=0x1017 +11=0x1200 +12=0x1800 +13=0x1801 +14=0x1A00 +15=0x1A01 +16=0x1F80 + +[ManufacturerObjects] +SupportedObjects=6 +1=0x2001 +2=0x2002 +3=0x2003 +4=0x2018 +5=0x2019 +6=0x2021 + +[1000] +ParameterName=Device Type +ObjectType=0x07 +DataType=0x0007 +AccessType=const +DefaultValue=0x0000000 +PDOMapping=0 + +[1001] +ParameterName=Error Register +ObjectType=0x07 +DataType=0x0005 +AccessType=ro +PDOMapping=0 + +[1005] +ParameterName=COB-ID SYNC +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +PDOMapping=0 + +[1008] +ParameterName=Manufacturer Device Name +ObjectType=0x07 +DataType=0x0009 +AccessType=const +PDOMapping=0 + +[1009] +ParameterName=Manufacturer Hardware Version +ObjectType=0x07 +DataType=0x0009 +AccessType=const +PDOMapping=0 + +[100A] +ParameterName=Manufacturer Software Version +ObjectType=0x07 +DataType=0x0009 +AccessType=const +PDOMapping=0 + +[100C] +ParameterName=Guard Time +ObjectType=0x07 +DataType=0x0006 +AccessType=rw +PDOMapping=0 + +[100D] +ParameterName=Life Time Factor +ObjectType=0x07 +DataType=0x0005 +AccessType=rw +PDOMapping=0 + +[1014] +ParameterName=COB-ID EMCY +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +PDOMapping=0 + +[1015] +ParameterName=Inhibit Time Emergency +ObjectType=0x07 +DataType=0x0006 +AccessType=rw +DefaultValue=0x0000 +PDOMapping=0 + +[1016] +SubNumber=3 +ParameterName=Heartbeat Consumer Entries +ObjectType=0x08 + +[1016sub0] +ParameterName=Highest sub-index supported +ObjectType=0x07 +DataType=5 +AccessType=const +DefaultValue=0x02 +PDOMapping=0 + +[1016sub1] +ParameterName=Consumer Heartbeat Time 1 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +PDOMapping=0 + +[1016sub2] +ParameterName=Consumer Heartbeat Time 2 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +PDOMapping=0 + +[1017] +ParameterName=Producer Heartbeat Time +ObjectType=0x07 +DataType=0x0006 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1018] +SubNumber=5 +ParameterName=Identity Object +ObjectType=0x09 + +[1018sub0] +ParameterName=number of entries +ObjectType=0x07 +DataType=0x0005 +AccessType=const +DefaultValue=0x4 +PDOMapping=0 + +[1018sub1] +ParameterName=Vendor Id +ObjectType=0x07 +DataType=0x0007 +AccessType=ro +DefaultValue=0x01000056 +PDOMapping=0 + +[1018sub2] +ParameterName=Product Code +ObjectType=0x07 +DataType=0x0007 +AccessType=ro +PDOMapping=0 + +[1018sub3] +ParameterName=Revision number +ObjectType=0x07 +DataType=0x0007 +AccessType=ro +PDOMapping=0 + +[1018sub4] +ParameterName=Serial number +ObjectType=0x07 +DataType=0x0007 +AccessType=ro +PDOMapping=0 + +[1200] +SubNumber=3 +ParameterName=Server SDO Parameter 1 +ObjectType=0x09 + +[1200sub0] +ParameterName=Highest sub-index supported +ObjectType=0x07 +DataType=0x0005 +AccessType=const +DefaultValue=0x02 +PDOMapping=0 + +[1200sub1] +ParameterName=COB-ID Client -> Server +ObjectType=0x07 +DataType=0x0007 +AccessType=ro +PDOMapping=0 + +[1200sub2] +ParameterName=COB-ID Server -> Client +ObjectType=0x07 +DataType=0x0007 +AccessType=ro +PDOMapping=0 + +[1800] +SubNumber=6 +ParameterName=Transmit PDO Communication Parameter 1 +ObjectType=0x09 + +[1800sub0] +ParameterName=Highest sub-index supported +ObjectType=0x07 +DataType=0x0005 +AccessType=const +DefaultValue=0x05 +PDOMapping=0 +LowLimit=0x2 +HighLimit=0x6 + +[1800sub1] +ParameterName=COB-ID +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=$NODEID+0x180 +PDOMapping=0 +LowLimit=0x00000001 +HighLimit=0xFFFFFFFF + +[1800sub2] +ParameterName=Transmission Type +ObjectType=0x07 +DataType=0x0005 +AccessType=rw +DefaultValue=0xFF +PDOMapping=0 +LowLimit=0x0 +HighLimit=0xFF + +[1800sub3] +ParameterName=Inhibit Time +ObjectType=0x07 +DataType=0x0006 +AccessType=rw +DefaultValue=0x0 +PDOMapping=0 +LowLimit=0x0000 +HighLimit=0xFFFF + +[1800sub4] +ParameterName=Compatibility Entry +ObjectType=0x07 +DataType=0x0005 +AccessType=rw +DefaultValue=0x00 +PDOMapping=0 +LowLimit=0x00 +HighLimit=0xFF + +[1800sub5] +ParameterName=Event Timer +ObjectType=0x07 +DataType=0x0006 +AccessType=rw +DefaultValue=0x14 +PDOMapping=0 +LowLimit=0x0 +HighLimit=0xFFFF + +[1801] +SubNumber=6 +ParameterName=Transmit PDO Communication Parameter 2 +ObjectType=0x09 + +[1801sub0] +ParameterName=Highest sub-index supported +ObjectType=0x07 +DataType=0x0005 +AccessType=const +DefaultValue=0x05 +PDOMapping=0 +LowLimit=0x02 +HighLimit=0x06 + +[1801sub1] +ParameterName=COB-ID +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=$NODEID+0x280 +PDOMapping=0 +LowLimit=0x00000001 +HighLimit=0xFFFFFFFF + +[1801sub2] +ParameterName=Transmission Type +ObjectType=0x07 +DataType=0x0005 +AccessType=rw +DefaultValue=0xFF +PDOMapping=0 +LowLimit=0x0 +HighLimit=0xFF + +[1801sub3] +ParameterName=Inhibit Time +ObjectType=0x07 +DataType=0x0006 +AccessType=rw +DefaultValue=0x0 +PDOMapping=0 +LowLimit=0x0000 +HighLimit=0xFFFF + +[1801sub4] +ParameterName=Compatibility Entry +ObjectType=0x07 +DataType=0x0005 +AccessType=rw +DefaultValue=0x00 +PDOMapping=0 +LowLimit=0x00 +HighLimit=0xFF + +[1801sub5] +ParameterName=Event Timer +ObjectType=0x07 +DataType=0x0006 +AccessType=rw +DefaultValue=0x14 +PDOMapping=0 +LowLimit=0x0 +HighLimit=0xFFFF + +[1A00] +SubNumber=9 +ParameterName=Transmit PDO Mapping Parameter 1 +ObjectType=0x09 + +[1A00sub0] +ParameterName=Number of mapped objects +ObjectType=0x07 +DataType=0x0005 +AccessType=rw +DefaultValue=0x00 +PDOMapping=0 +LowLimit=0x00 +HighLimit=0x08 + +[1A00sub1] +ParameterName=Mapping Entry 1 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1A00sub2] +ParameterName=Mapping Entry 2 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1A00sub3] +ParameterName=Mapping Entry 3 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1A00sub4] +ParameterName=Mapping Entry 4 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1A00sub5] +ParameterName=Mapping Entry 5 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1A00sub6] +ParameterName=Mapping Entry 6 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1A00sub7] +ParameterName=Mapping Entry 7 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1A00sub8] +ParameterName=Mapping Entry 8 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1A01] +SubNumber=9 +ParameterName=Transmit PDO Mapping Parameter 2 +ObjectType=0x09 + +[1A01sub0] +ParameterName=Number of mapped objects +ObjectType=0x07 +DataType=0x0005 +AccessType=rw +DefaultValue=0x00 +PDOMapping=0 +LowLimit=0x0 +HighLimit=0x8 + +[1A01sub1] +ParameterName=Mapping Entry 1 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1A01sub2] +ParameterName=Mapping Entry 2 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1A01sub3] +ParameterName=Mapping Entry 3 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1A01sub4] +ParameterName=Mapping Entry 4 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1A01sub5] +ParameterName=Mapping Entry 5 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1A01sub6] +ParameterName=Mapping Entry 6 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1A01sub7] +ParameterName=Mapping Entry 7 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1A01sub8] +ParameterName=Mapping Entry 8 +ObjectType=0x07 +DataType=0x0007 +AccessType=rw +DefaultValue=0x00000000 +PDOMapping=0 + +[1F80] +ParameterName=NMT Startup +ObjectType=0x07 +DataType=0x0007 +AccessType=const +PDOMapping=0 +LowLimit=0x0 +HighLimit=0x3F + +[2001] +SubNumber=6 +ParameterName=Mounting parameters +ObjectType=0x09 + +[2001sub0] +ParameterName=numOfEntries +ObjectType=0x07 +DataType=0x0005 +AccessType=const +DefaultValue=0x5 +PDOMapping=0 + +[2001sub1] +ParameterName=reserved1 +ObjectType=0x07 +DataType=0x0007 +AccessType=ro +PDOMapping=0 + +[2001sub2] +ParameterName=reserved2 +ObjectType=0x07 +DataType=0x0007 +AccessType=ro +PDOMapping=0 + +[2001sub3] +ParameterName=reserved3 +ObjectType=0x07 +DataType=0x0007 +AccessType=ro +PDOMapping=0 + +[2001sub4] +ParameterName=reserved4 +ObjectType=0x07 +DataType=0x0007 +AccessType=ro +PDOMapping=0 + +[2001sub5] +ParameterName=sensorFlipped +ObjectType=0x07 +DataType=0x0005 +AccessType=rw +DefaultValue=0x00 +PDOMapping=0 +LowLimit=0x0 +HighLimit=0xFF + +[2002] +SubNumber=4 +ParameterName=Tape Parameters +ObjectType=0x09 + +[2002sub0] +ParameterName=Number of mapped objects +ObjectType=0x07 +DataType=0x0005 +AccessType=const +DefaultValue=0x3 +PDOMapping=0 + +[2002sub1] +ParameterName=Typ. Width +ObjectType=0x07 +DataType=0x0008 +AccessType=rw +PDOMapping=0 + +[2002sub2] +ParameterName=Min. Width +ObjectType=0x07 +DataType=0x0008 +AccessType=rw +PDOMapping=0 + +[2002sub3] +ParameterName=Max. Width +ObjectType=0x07 +DataType=0x0008 +AccessType=rw +PDOMapping=0 + +[2003] +SubNumber=3 +ParameterName=Advanced Settings +ObjectType=0x09 + +[2003sub0] +ParameterName=Number of mapped objects +ObjectType=0x07 +DataType=0x0005 +AccessType=const +DefaultValue=0x2 +PDOMapping=0 + +[2003sub1] +ParameterName=Off Delay Track Detection +ObjectType=0x07 +DataType=0x0006 +AccessType=rw +DefaultValue=0x0064 +PDOMapping=0 +LowLimit=0x0 +HighLimit=0xFFFF + +[2003sub2] +ParameterName=Off Delay Code Detection +ObjectType=0x07 +DataType=0x0006 +AccessType=rw +DefaultValue=0x0064 +PDOMapping=0 +LowLimit=0x0 +HighLimit=0xFFFF + +[2018] +ParameterName=Device Status +ObjectType=0x07 +DataType=0x0005 +AccessType=ro +PDOMapping=0 + +[2019] +ParameterName=Order number +ObjectType=0x07 +DataType=0x0007 +AccessType=ro +PDOMapping=0 + +[2021] +SubNumber=10 +ParameterName=Result data (LCPs) +ObjectType=0x09 + +[2021sub0] +ParameterName=Number of mapped objects +ObjectType=0x07 +DataType=0x0005 +AccessType=const +DefaultValue=0x9 +PDOMapping=0 + +[2021sub1] +ParameterName=LCP1 +ObjectType=0x07 +DataType=0x0003 +AccessType=ro +PDOMapping=1 +LowLimit=0x8000 +HighLimit=0x7FFF + +[2021sub2] +ParameterName=LCP2 +ObjectType=0x07 +DataType=0x0003 +AccessType=ro +PDOMapping=1 +LowLimit=0x8000 +HighLimit=0x7FFF + +[2021sub3] +ParameterName=LCP3 +ObjectType=0x07 +DataType=0x0003 +AccessType=ro +PDOMapping=1 +LowLimit=0x8000 +HighLimit=0x7FFF + +[2021sub4] +ParameterName=Status +ObjectType=0x07 +DataType=0x0005 +AccessType=ro +PDOMapping=1 +LowLimit=0x00 +HighLimit=0xFF + +[2021sub5] +ParameterName=Width LCP1 +ObjectType=0x07 +DataType=0x0003 +AccessType=ro +PDOMapping=1 +LowLimit=0x8000 +HighLimit=0x7FFF + +[2021sub6] +ParameterName=Width LCP2 +ObjectType=0x07 +DataType=0x0003 +AccessType=ro +PDOMapping=1 +LowLimit=0x8000 +HighLimit=0x7FFF + +[2021sub7] +ParameterName=Width LCP3 +ObjectType=0x07 +DataType=0x0003 +AccessType=ro +PDOMapping=1 +LowLimit=0x8000 +HighLimit=0x7FFF + +[2021sub8] +ParameterName=Code +ObjectType=0x07 +DataType=0x0005 +AccessType=ro +PDOMapping=1 +LowLimit=0x0 +HighLimit=0xFF + +[2021sub9] +ParameterName=Extended Code +ObjectType=0x07 +DataType=0x0007 +AccessType=ro +PDOMapping=0 +LowLimit=0x0 +HighLimit=0xFFFFFFFF + diff --git a/Devices/Packages/sick_line_guidance/test/scripts/cleanup.bash b/Devices/Packages/sick_line_guidance/test/scripts/cleanup.bash new file mode 100755 index 0000000..286f9e4 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/test/scripts/cleanup.bash @@ -0,0 +1,13 @@ +#!/bin/bash +pushd ../../../../ +echo -e "\n# cleanup.bash: Stopping rosmaster and all rosnodes...\n# rosnode kill -a ; sleep 5 ; killall rosmaster ; sleep 5" +rosnode kill -a ; sleep 5 ; killall roslaunch ; sleep 5 ; killall rosmaster ; sleep 5 +echo -e "\n# cleanup.bash: Deleting ros cache and logfiles and catkin folders ./build ./devel ./install" +rosclean purge -y +rm -rf ./build ./devel ./install +rm -rf ~/.ros/* +# rm -rf ~/.ros/log/* +catkin clean --yes --all-profiles --verbose +catkin_make clean +popd + diff --git a/Devices/Packages/sick_line_guidance/test/scripts/clion.bash b/Devices/Packages/sick_line_guidance/test/scripts/clion.bash new file mode 100755 index 0000000..752f02a --- /dev/null +++ b/Devices/Packages/sick_line_guidance/test/scripts/clion.bash @@ -0,0 +1,16 @@ +#!/bin/bash + +# init ros environment +source /opt/ros/melodic/setup.bash +source ../../../../devel/setup.bash + +# start edit resource-files +gedit ./run.bash ./runsimu.bash ../../../sick_line_guidance/launch/sick_canopen_simu.launch ../../../sick_line_guidance/launch/sick_line_guidance.launch ../../../sick_line_guidance/ols/sick_line_guidance_ols10.yaml ../../../sick_line_guidance/ols/sick_line_guidance_ols20.yaml ../../../sick_line_guidance/mls/sick_line_guidance_mls.yaml & + +# start clion +# pushd ../../../sick_line_guidance +# ~/Public/clion-2018.3.3/bin/clion.sh & +pushd ../../../.. +~/Public/clion-2018.3.3/bin/clion.sh ./src & +popd + diff --git a/Devices/Packages/sick_line_guidance/test/scripts/echo_topics.bash b/Devices/Packages/sick_line_guidance/test/scripts/echo_topics.bash new file mode 100755 index 0000000..5091fed --- /dev/null +++ b/Devices/Packages/sick_line_guidance/test/scripts/echo_topics.bash @@ -0,0 +1,21 @@ +#!/bin/bash +source ../../../../install/setup.bash +printf "\033c" + +# display PointCloud2 messages +rosrun rviz rviz & + +# plot sensor positions +rqt_plot /ols/position[0] /ols/position[1] /ols/position[2] & +# rqt_plot /cloud & + +# print some values from the object directory of the can device +rostopic list +rostopic echo -n 1 /node1_1001 & rostopic echo -n 1 /node1_1018sub1 & rostopic echo -n 1 /node1_1018sub4 +rosservice call /driver/get_object node1 1001sub false +rosservice call /driver/get_object node1 1018sub1 false +rosservice call /driver/get_object node1 1018sub4 false + +# print ros topics for ols and mls +rostopic echo /mls & rostopic echo /ols & rostopic echo /cloud & rostopic echo /diagnostics + diff --git a/Devices/Packages/sick_line_guidance/test/scripts/make.bash b/Devices/Packages/sick_line_guidance/test/scripts/make.bash new file mode 100755 index 0000000..9b45406 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/test/scripts/make.bash @@ -0,0 +1,30 @@ +#!/bin/bash + +# delete old logfiles +pushd ../../../.. +rm -rf install/lib/sick_line_guidance/* +rm -rf install/share/sick_line_guidance/* +rm -f build/catkin_make_install.log + +# make install +catkin_make install 2>&1 | tee -a build/catkin_make_install.log +source ./install/setup.bash + +# lint, install by running +# sudo apt-get install python-catkin-lint +# catkin_lint -W1 src/sick_line_guidance + +# print warnings and errors +echo -e "\nmake.bash finished.\n" +echo -e "catkin_make warnings:" +cat build/catkin_make_install.log | grep -i "warning:" +echo -e "\ncatkin_make errors:" +cat build/catkin_make_install.log | grep -i "error:" + +# print sick_line_guidance install files, libraries, executables +echo -e "\ninstall/lib/sick_line_guidance:" +ls -al install/lib/sick_line_guidance/* +echo -e "\ninstall/share/sick_line_guidance:" +ls install/share/sick_line_guidance/*.* +popd + diff --git a/Devices/Packages/sick_line_guidance/test/scripts/makeall.bash b/Devices/Packages/sick_line_guidance/test/scripts/makeall.bash new file mode 100755 index 0000000..5ed231b --- /dev/null +++ b/Devices/Packages/sick_line_guidance/test/scripts/makeall.bash @@ -0,0 +1,9 @@ +#!/bin/bash +echo -e "makeall.bash: build and install ros sick_line_guidance driver" +sudo echo -e "makeall.bash started." +source /opt/ros/noetic/setup.bash +./cleanup.bash +./makepcan.bash +./make.bash +echo -e "makeall.bash finished." + diff --git a/Devices/Packages/sick_line_guidance/test/scripts/makepcan.bash b/Devices/Packages/sick_line_guidance/test/scripts/makepcan.bash new file mode 100755 index 0000000..395b7a3 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/test/scripts/makepcan.bash @@ -0,0 +1,41 @@ +#!/bin/bash + +# install packages required for peak can device driver + +if [ -f ../../../../../peak_systems/pcanview-ncurses_0.8.7-0_amd64.deb ] ; then + pushd ../../../../../peak_systems + sudo apt-get install libncurses5 + sudo dpkg --install pcanview-ncurses_0.8.7-0_amd64.deb + popd +fi + +# build and install peak can device driver + +if [ -d ../../../../../peak_systems/peak-linux-driver-8.17.0 ] ; then + pushd ../../../../../peak_systems/peak-linux-driver-8.17.0 + # install required packages + sudo apt-get install can-utils + sudo apt-get install gcc-multilib + sudo apt-get install libelf-dev + sudo apt-get install libpopt-dev + sudo apt-get install tree + # build and install pcan driver + make clean + make NET=NETDEV_SUPPORT + sudo make install + # install the modules + sudo modprobe pcan + sudo modprobe can + sudo modprobe vcan + sudo modprobe slcan + # setup and configure "can0" net device + sudo ip link set can0 type can + sudo ip link set can0 up type can bitrate 125000 # configure the CAN bitrate, f.e. 125000 bit/s + # check installation + # ./driver/lspcan --all # should print "pcanusb32" and pcan version + # tree /dev/pcan-usb # should show a pcan-usb device + # ip -a link # should print some "can0: ..." messages + ip -details link show can0 # should print some details about "can0" net device + popd +fi + diff --git a/Devices/Packages/sick_line_guidance/test/scripts/run.bash b/Devices/Packages/sick_line_guidance/test/scripts/run.bash new file mode 100755 index 0000000..f02fc39 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/test/scripts/run.bash @@ -0,0 +1,69 @@ +#!/bin/bash + +# set environment, clear screen and logfiles +if [ ! -d ~/.ros/log ] ; then mkdir -p ~/.ros/log ; fi +printf "\033c" +source ../../../../install/setup.bash +echo -e "\n# run.bash: Stopping rosmaster and all rosnodes...\n# rosnode kill -a ; sleep 5 ; killall rosmaster ; sleep 5" +rosnode kill -a ; sleep 5 ; killall rosmaster ; sleep 5 +rm -rf ~/.ros/log/* + +# initialize can net device, can0 by peak can adapter +# sudo ip link set can0 up type can bitrate 125000 +NUM_BITRATE_125000=$(ip -details link show can0 | grep "bitrate 125000" | wc -l) +if [ "$NUM_BITRATE_125000" = "0" ] ; then + echo -e "# run sick_line_guidance:\n# sudo ip link set can0 up type can bitrate 125000" + sudo ip link set can0 up type can bitrate 125000 +fi +echo -e "# run sick_line_guidance:\n# ip -details link show can0" +ip -details link show can0 + +# check can net device by sending 000#0x820A (can command reset communication to device 0x0A), should be answered by 70A#00 (boot message from device 0x0A) +# for ((n=0;n<=2;n++)) ; do +# candump -ta -n 2 can0 & +# cansend can0 000#820A +# sleep 1 +# done + +# Start OLS20 simulation +# ./runsimu.bash + +# start can logging +# printf "\033c" ; candump -ta can0 2>&1 | tee ~/.ros/log/candump.log +candump -ta can0 2>&1 | tee ~/.ros/log/candump.log & + +# get/set values from object directory with canopen_chain_node service, f.e. Object 2001sub5 (sensorFlipped, UINT8, defaultvalue: 0x01) +# rosservice call /driver/get_object "{node: 'node1', object: '2001sub5', cached: false}" +# rosservice call /driver/set_object "{node: 'node1', object: '2001sub5', value: '0x01', cached: false}" + +# Start ros driver canopen_chain_node, sick_line_guidance_node and sick_line_guidance_cloud_publisher + +# echo -e "\n# run sick_line_guidance:\n# roslaunch -v --screen sick_line_guidance sick_line_guidance.launch yaml:=sick_line_guidance_ols10.yaml\n" +# roslaunch -v --screen sick_line_guidance sick_line_guidance.launch yaml:=sick_line_guidance_ols10.yaml 2>&1 | tee ~/.ros/log/sick_line_guidance_ols10.log # start OLS10 driver + +echo -e "\n# run sick_line_guidance:\n# roslaunch -v --screen sick_line_guidance sick_line_guidance.launch yaml:=sick_line_guidance_ols20.yaml\n" +roslaunch -v --screen sick_line_guidance sick_line_guidance.launch yaml:=sick_line_guidance_ols20.yaml 2>&1 | tee ~/.ros/log/sick_line_guidance_ols20.log # start OLS20 driver + +# echo -e "\n# run sick_line_guidance:\n# roslaunch -v --screen sick_line_guidance sick_line_guidance.launch yaml:=sick_line_guidance_mls.yaml\n" +# roslaunch -v --screen sick_line_guidance sick_line_guidance.launch yaml:=sick_line_guidance_mls.yaml 2>&1 | tee ~/.ros/log/sick_line_guidance_mls.log # start MLS driver + +# read some object indices (f.e. 1001=ErrorRegister, 1018sub1=VendorID, 1018sub4=SerialNumber) +# rostopic echo -n 1 /node1_1001 & rostopic echo -n 1 /node1_1018sub1 & rostopic echo -n 1 /node1_1018sub4 +# rosservice call /driver/get_object node1 1001sub false +# rosservice call /driver/get_object node1 1018sub1 false +# rosservice call /driver/get_object node1 1018sub4 false + +# Check errors and warnings in ros logfiles +killall candump +grep "\[ WARN\]" ~/.ros/log/*.log ~/.ros/log/*/ros*.log ~/.ros/log/*/sick*.log >> ~/.ros/log/ros_log_warnings.txt +grep "\[ERROR\]" ~/.ros/log/*.log ~/.ros/log/*/ros*.log ~/.ros/log/*/sick*.log >> ~/.ros/log/ros_log_errors.txt +cat ~/.ros/log/ros_log_warnings.txt +cat ~/.ros/log/ros_log_errors.txt + +# Zip all logfiles +mkdir ./tmp +cp -rf ~/.ros/log ./tmp +now=$(date +"%Y%m%d_%H%M%S") +tar -cvzf ./$now-ros-logfiles.tgz ./tmp/log +rm -rf ./tmp + diff --git a/Devices/Packages/sick_line_guidance/test/scripts/runsimu.bash b/Devices/Packages/sick_line_guidance/test/scripts/runsimu.bash new file mode 100755 index 0000000..e6336fc --- /dev/null +++ b/Devices/Packages/sick_line_guidance/test/scripts/runsimu.bash @@ -0,0 +1,112 @@ +#!/bin/bash + +# set environment, clear screen and logfiles +if [ ! -d ~/.ros/log ] ; then mkdir -p ~/.ros/log ; fi +printf "\033c" +source ../../../../install/setup.bash +echo -e "\n# runsimu.bash: Stopping rosmaster and all rosnodes...\n# rosnode kill -a ; sleep 5 ; killall rosmaster ; sleep 5" +rosnode kill -a ; sleep 5 ; killall roslaunch ; sleep 5 ; killall rosmaster ; sleep 5 +rm -rf ~/.ros/log/* + +# Initialize can net device, can0 by peak can adapter +# sudo ip link set can0 type can loopback on # loopback not supported +# sudo ip link set can0 up type can bitrate 125000 # set default bitrate 125000 bit/s + +# Reset can0 net device +# sudo ip link set can0 down +# sleep 1 +# sudo ip link set can0 up type can bitrate 125000 +# sleep 1 + +# Set default bitrate 125000 bit/s +NUM_BITRATE_125000=$(ip -details link show can0 | grep "bitrate 125000" | wc -l) +if [ "$NUM_BITRATE_125000" = "0" ] ; then + echo -e "# run ols/mls simulation:\n# sudo ip link set can0 up type can bitrate 125000" + sudo ip link set can0 up type can bitrate 125000 +fi +echo -e "# run ols/mls simulation:\n# ip -details link show can0" +ip -details link show can0 +sleep 1 + +# Start can logging +# candump -ta can0 & +# candump -ta can0 2>&1 | tee ~/.ros/log/candump.log & + +# Simulation with cangineberry: Upload firmware (BEDS slave) and beds-file CiA401_IO_Node3 +# ./coiaupdater -p /dev/ttyS0 -f ../APPS/CANopenIA-BEDS/CgB_COIA_BEDS1.3_sec.bin # install firmware CANopen IA BEDS slave +# ./coiaupdater -p /dev/ttyS0 -d ../APPS/CANopenIA-BEDS/EDS/CiA401_IO_Node3.bin +# Simulation with cangineberry: Read/write objects 6401sub1 (LCP1), 6401sub2 (LCP2), 6401sub3 (LCP3), 6401sub4 (WidthLCP1), 6401sub5 (WidthLCP2), 6401sub6 (WidthLCP3) +# ./coia -p /dev/ttyS0 --read=0x6401,0x01 +# ./coia -p /dev/ttyS0 --write=0x6401,0x01,2,0x1234 +# ./coia -p /dev/ttyS0 --write=0x6401,0x01,2,0xABCD + +# get/set values from object directory with canopen_chain_node service, f.e. Object 2001sub5 (sensorFlipped, UINT8, defaultvalue: 0x01) +# rosservice call /driver/get_object "{node: 'node1', object: '2001sub5', cached: false}" +# rosservice call /driver/set_object "{node: 'node1', object: '2001sub5', value: '0x01', cached: false}" + +# Run simulation for OLS and MLS by yaml-file configuration +device_settings_map=( "OLS20:sick_line_guidance_ols20.yaml;120" "OLS10:sick_line_guidance_ols10.yaml;30" "MLS:sick_line_guidance_mls.yaml;30" ) +for((device_cnt=0;device_cnt<=2;device_cnt++)) ; do + device=${device_settings_map[$device_cnt]%%:*} + settings_str=${device_settings_map[$device_cnt]##*:} + IFS=';' settings_list=($settings_str) + yaml_file=${settings_list[0]} + run_seconds=${settings_list[1]} + echo -e "runsimu.bash: settings for $device device: yaml_file $yaml_file, run for $run_seconds seconds." + + # Start can2ros converter + # echo -e "\n# run ols/mls simulation:\n# roslaunch -v --screen sick_line_guidance sick_line_guidance_can2ros_node.launch\n" + # roslaunch -v --screen sick_line_guidance sick_line_guidance_can2ros_node.launch & + echo -e "\n# run ols/mls simulation:\n# roslaunch -v sick_line_guidance sick_line_guidance_can2ros_node.launch\n" + roslaunch -v sick_line_guidance sick_line_guidance_can2ros_node.launch & + sleep 5 + + # Start ros2can converter + # echo -e "\n# run ols/mls simulation:\n# roslaunch -v --screen sick_line_guidance sick_line_guidance_ros2can_node.launch\n" + # roslaunch -v --screen sick_line_guidance sick_line_guidance_ros2can_node.launch & + echo -e "\n# run ols/mls simulation:\n# roslaunch -v sick_line_guidance sick_line_guidance_ros2can_node.launch\n" + roslaunch -v sick_line_guidance sick_line_guidance_ros2can_node.launch & + sleep 5 + + # Start OLS20 simulation + echo -e "\n# run ols/mls simulation:\n# roslaunch -v sick_line_guidance sick_canopen_simu.launch device:=$device\n" + roslaunch -v --screen sick_line_guidance sick_canopen_simu.launch device:=$device 2>&1 | tee ~/.ros/log/sick_canopen_simu_$device.log & + # echo -e "\n# run ols/mls simulation:\n# roslaunch -v sick_line_guidance sick_canopen_simu.launch device:=$device\n" + # roslaunch -v sick_line_guidance sick_canopen_simu.launch device:=$device & + sleep 5 + + # check can net device by sending 000#0x820A (can command reset communication to device 0x0A), should be answered by 70A#00 (boot message from device 0x0A) + for ((n=0;n<1;n++)) ; do + candump -ta -n 2 can0 & + cansend can0 000#820A + sleep 2 + done + + # Start ros driver for MLS or OLS, incl. canopen_chain_node, sick_line_guidance_node and sick_line_guidance_cloud_publisher + echo -e "\n# run sick_line_guidance:\n# roslaunch -v sick_line_guidance sick_line_guidance.launch yaml:=$yaml_file\n" + roslaunch -v --screen sick_line_guidance sick_line_guidance.launch yaml:=$yaml_file 2>&1 | tee ~/.ros/log/$yaml_file.log & + # echo -e "\n# run sick_line_guidance:\n# roslaunch -v sick_line_guidance sick_line_guidance.launch yaml:=$yaml_file\n" + # roslaunch -v sick_line_guidance sick_line_guidance.launch yaml:=$yaml_file & + + # Run simulation for a while + sleep $run_seconds + + # Exit simulation, shutdown all ros nodes + echo -e "\n# runsimu.bash: Stopping rosmaster and all rosnodes...\n# rosnode kill -a ; sleep 5 ; killall rosmaster ; sleep 5" + rosnode kill -a ; sleep 5 ; killall rosmaster ; sleep 5 + +done + + +# Check errors and warnings in ros logfiles +killall candump +echo -e "\n#\n# OLS/MLS simulation finished.\n#\n" +grep "\[ WARN\]" ~/.ros/log/*.log ~/.ros/log/*/ros*.log ~/.ros/log/*/sick*.log >> ~/.ros/log/ros_log_warnings.txt +grep "\[ERROR\]" ~/.ros/log/*.log ~/.ros/log/*/ros*.log ~/.ros/log/*/sick*.log >> ~/.ros/log/ros_log_errors.txt +grep "MeasurementVerificationStatistic" ~/.ros/log/*.log ~/.ros/log/*/ros*.log ~/.ros/log/*/sick*.log >> ~/.ros/log/simulation_statistic.txt +echo -e "\nSimulation warnings and errors:\n" +cat ~/.ros/log/ros_log_warnings.txt +cat ~/.ros/log/ros_log_errors.txt +echo -e "\nSimulation statistic:\n" +cat ~/.ros/log/simulation_statistic.txt + diff --git a/Devices/Packages/sick_line_guidance/test/scripts/vs_code.bash b/Devices/Packages/sick_line_guidance/test/scripts/vs_code.bash new file mode 100755 index 0000000..d88b8af --- /dev/null +++ b/Devices/Packages/sick_line_guidance/test/scripts/vs_code.bash @@ -0,0 +1,15 @@ +#!/bin/bash + + +if [ -f /opt/ros/noetic/setup.bash ] ; then + source /opt/ros/noetic/setup.bash +elif [ -f /opt/ros/foxy/setup.bash ] ; then + source /opt/ros/foxy/setup.bash +fi +pushd ../../../.. +if [ -f ./install/setup.bash ] ; then + source ./install/setup.bash +fi + +code ./sick_line_guidance_vscode.code-workspace +popd diff --git a/Devices/Packages/sick_line_guidance/test/yaml/sick_line_guidance_example.yaml b/Devices/Packages/sick_line_guidance/test/yaml/sick_line_guidance_example.yaml new file mode 100755 index 0000000..187cdcf --- /dev/null +++ b/Devices/Packages/sick_line_guidance/test/yaml/sick_line_guidance_example.yaml @@ -0,0 +1,30 @@ +bus: + device: can0 + driver_plugin: can::SocketCANInterface + master_allocator: canopen::SimpleMaster::Allocator +sync: + interval_ms: 10 + overflow: 0 +# heartbeat: # simple heartbeat producer (optional, not supported by OLS or MLS, do not enable heartbeats, do not configure heartbeat rate or message) +# rate: 100 # heartbeat rate (milliseconds) +# msg: "77f#05" # message to send, cansend format: heartbeat of node 127 with status 5=Started +nodes: + node1: + id: 0x03 # CAN-Node-ID of can device, default: Node-ID 10=0x0A for OLS10 and MLS (CiA401_IO_Node3: 0x03, CiA402_Stepper_Node8: 0x08) + eds_pkg: sick_line_guidance # package name for relative path + eds_file: CiA401_IO_Node3.eds # path to EDS/DCF file + publish: ["1001","1018sub1","1018sub4","6000sub1!","6000sub2!","6000sub3!","6000sub4!","6401sub1!","6401sub2!","6401sub3!","6401sub4!","6401sub5!","6401sub6!"] + # Common objects (CiA401_IO_Node3.eds, CiA402_Stepper_node8.eds, SICK_OLS20.eds, SICK-MLS-MLS.eds): 1001 = ErrorRegister, 1018sub1 = VendorID, 1018sub4 = SerialNumber + # objects SICK_OLS20.eds: 1001 = ErrorRegister, 1018sub1 = VendorID, 1018sub4 = SerialNumber, 0x2018 = DeviceStatus(0=OK), 0x2021sub1 bis 0x2021sub9 = measurementdata + # TPDOs SICK_OLS20.eds: 0x2021sub1 bis 0x2021sub8 + # SICK-MLS.eds: 0x2021sub1 bis 0x2021sub4 und 0x2022 = measurementdata + # CiA401_IO_Node3.eds: TPDO mapped: 6000sub1 bis 6000sub8 (jeweils 1 byte), 6401sub1 bis 6401subC (jeweils 2 byte) + # Note: publish with "!" (f.e. publish: ["1001!"]) means reread from object directory + # (not cached, i.e. query by SDO, if no actual PDO is transmitted), + # otherwise cached (use last received SDO or PDO). + + # sick_line_guidance configuration of this node: + sick_device_family: "CIA401" # can devices of OLS or MLS family currently supported (or CIA401 for testing) + sick_topic: "ols" # OLS_Measurement messages are published in topic "/ols" + sick_frame_id: "ols_measurement_frame" # OLS_Measurement messages are published frame_id "ols_measurement_frame" + diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/CMakeLists.txt b/Devices/Packages/sick_line_guidance/turtlebotDemo/CMakeLists.txt new file mode 100755 index 0000000..b1cf04c --- /dev/null +++ b/Devices/Packages/sick_line_guidance/turtlebotDemo/CMakeLists.txt @@ -0,0 +1,67 @@ +# toplevel CMakeLists.txt for a catkin workspace +# catkin/cmake/toplevel.cmake + +cmake_minimum_required(VERSION 2.8.3) + +set(CATKIN_TOPLEVEL TRUE) + +# search for catkin within the workspace +set(_cmd "catkin_find_pkg" "catkin" "${CMAKE_SOURCE_DIR}") +execute_process(COMMAND ${_cmd} + RESULT_VARIABLE _res + OUTPUT_VARIABLE _out + ERROR_VARIABLE _err + OUTPUT_STRIP_TRAILING_WHITESPACE + ERROR_STRIP_TRAILING_WHITESPACE +) +if(NOT _res EQUAL 0 AND NOT _res EQUAL 2) + # searching fot catkin resulted in an error + string(REPLACE ";" " " _cmd_str "${_cmd}") + message(FATAL_ERROR "Search for 'catkin' in workspace failed (${_cmd_str}): ${_err}") +endif() + +# include catkin from workspace or via find_package() +if(_res EQUAL 0) + set(catkin_EXTRAS_DIR "${CMAKE_SOURCE_DIR}/${_out}/cmake") + # include all.cmake without add_subdirectory to let it operate in same scope + include(${catkin_EXTRAS_DIR}/all.cmake NO_POLICY_SCOPE) + add_subdirectory("${_out}") + +else() + # use either CMAKE_PREFIX_PATH explicitly passed to CMake as a command line argument + # or CMAKE_PREFIX_PATH from the environment + if(NOT DEFINED CMAKE_PREFIX_PATH) + if(NOT "$ENV{CMAKE_PREFIX_PATH}" STREQUAL "") + if(NOT WIN32) + string(REPLACE ":" ";" CMAKE_PREFIX_PATH $ENV{CMAKE_PREFIX_PATH}) + else() + set(CMAKE_PREFIX_PATH $ENV{CMAKE_PREFIX_PATH}) + endif() + endif() + endif() + + # list of catkin workspaces + set(catkin_search_path "") + foreach(path ${CMAKE_PREFIX_PATH}) + if(EXISTS "${path}/.catkin") + list(FIND catkin_search_path ${path} _index) + if(_index EQUAL -1) + list(APPEND catkin_search_path ${path}) + endif() + endif() + endforeach() + + # search for catkin in all workspaces + set(CATKIN_TOPLEVEL_FIND_PACKAGE TRUE) + find_package(catkin QUIET + NO_POLICY_SCOPE + PATHS ${catkin_search_path} + NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH) + unset(CATKIN_TOPLEVEL_FIND_PACKAGE) + + if(NOT catkin_FOUND) + message(FATAL_ERROR "find_package(catkin) failed. catkin was neither found in the workspace nor in the CMAKE_PREFIX_PATH. One reason may be that no ROS setup.sh was sourced before.") + endif() +endif() + +catkin_workspace() diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/LICENSE b/Devices/Packages/sick_line_guidance/turtlebotDemo/LICENSE new file mode 100755 index 0000000..261eeb9 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/turtlebotDemo/LICENSE @@ -0,0 +1,201 @@ + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/README.md b/Devices/Packages/sick_line_guidance/turtlebotDemo/README.md new file mode 100755 index 0000000..4d133a8 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/turtlebotDemo/README.md @@ -0,0 +1,45 @@ +# TurtleBot demonstration + +Demonstration of SICK line guidance demonstration with TurtleBot + +## Build and install + +To build and install a TurtleBot demonstration (sick_line_guidance_demo), follow the description [doc/build_install.md](doc/build_install.md) + +## Build and run simulation + +To build and run the sick_line_guidance_demo simulation, run the following commands: + +```console +cd ~/catkin_ws/src +git clone https://github.com/SICKAG/sick_line_guidance.git +cd ./sick_line_guidance/turtlebotDemo/test/scripts +# ./gitCloneInstall.bash # install all packages for sick_line_guidance_demo (required once) +./makeall.bash # build everything for sick_line_guidance_demo +./runsimu.bash # run simulation of sick_line_guidance_demo +``` + +Note: ``` catkin_make ``` resp. ``` catkin_make install ``` will only build the SICK line guidance ros driver. +To build the TurtleBot demonstration, an additional option ``` --cmake-args -DTURTLEBOT_DEMO="ON" ``` is required: + +```console +cd ~/catkin_ws +catkin_make install --cmake-args -DTURTLEBOT_DEMO="ON" +source ./install/setup.bash +``` + +TurtleBot packages are not required unless this option is enabled (``` --cmake-args -DTURTLEBOT_DEMO="ON" ```). + +## Setup TurtleBot + +To setup the TurtleBot for sick_line_guidance_demo, follow the description [doc/setup_turtlebot.md](doc/setup_turtlebot.md) + + +## Run TurtleBot demonstration + +To run the SICK line guidance demonstration with a TurtleBot, run the following commands: + +```console +cd ~/catkin_ws/src/sick_line_guidance/turtlebotDemo/test/scripts +./runturtlebot.bash +``` diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/agc_radar/CMakeLists.txt b/Devices/Packages/sick_line_guidance/turtlebotDemo/agc_radar/CMakeLists.txt new file mode 100755 index 0000000..d178794 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/turtlebotDemo/agc_radar/CMakeLists.txt @@ -0,0 +1,49 @@ +cmake_minimum_required(VERSION 2.8.3) +project(agc_radar) + +## Compile as C++11, supported in ROS Kinetic and newer +add_compile_options(-std=c++17) + +find_package(catkin REQUIRED COMPONENTS + roscpp + std_msgs + message_generation +) + + add_message_files( + FILES + agc_radar_msg.msg + ) + + add_service_files( + FILES + agc_radar_config.srv +) + +# Generate added messages and services with any dependencies listed here + generate_messages( + DEPENDENCIES + std_msgs + ) + +#catkin_package( +# #INCLUDE_DIRS include +# #LIBRARIES agc_radar +# CATKIN_DEPENDS message_generation roscpp std_msgs message_runtime gpio_handling +# #DEPENDS system_lib +#) +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( + # include + ${catkin_INCLUDE_DIRS} + # $HOME/catkin_ws/devel/include +) + +add_executable(agc_radar src/agc_radar.cpp) +add_dependencies(agc_radar ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(agc_radar ${catkin_LIBRARIES}) diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/agc_radar/msg/agc_radar_msg.msg b/Devices/Packages/sick_line_guidance/turtlebotDemo/agc_radar/msg/agc_radar_msg.msg new file mode 100755 index 0000000..c4ca07e --- /dev/null +++ b/Devices/Packages/sick_line_guidance/turtlebotDemo/agc_radar/msg/agc_radar_msg.msg @@ -0,0 +1,4 @@ +Header header +float64 Schutzzeit +float64 Stopzeit +bool obsctacle_stop diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/agc_radar/package.xml b/Devices/Packages/sick_line_guidance/turtlebotDemo/agc_radar/package.xml new file mode 100755 index 0000000..90c5bdb --- /dev/null +++ b/Devices/Packages/sick_line_guidance/turtlebotDemo/agc_radar/package.xml @@ -0,0 +1,35 @@ + + + agc_radar + 0.0.0 + The agc_radar package + + tsprifl + + TODO + + catkin + + roscpp + std_msgs + message_generation + gpio_handling + + + roscpp + std_msgs + message_generation + gpio_handling + + + roscpp + std_msgs + message_generation + message_runtime + gpio_handling + + + + + + diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/agc_radar/src/agc_radar.cpp b/Devices/Packages/sick_line_guidance/turtlebotDemo/agc_radar/src/agc_radar.cpp new file mode 100755 index 0000000..f3317f7 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/turtlebotDemo/agc_radar/src/agc_radar.cpp @@ -0,0 +1,134 @@ +#include +#include +#include "agc_radar/agc_radar_msg.h" // for publishing +#include "agc_radar/agc_radar_config.h" // for service +#include "gpio_handling/gpio_get_config.h" +#include "gpio_handling/gpio_set_config.h" +#include "gpio_handling/gpio_get_pin.h" +#include "gpio_handling/gpio_set_pin.h" +#include + +/************************************************************************************************* +*** Service Client (GPIO-handling) +**************************************************************************************************/ +ros::ServiceClient gpio_get_config_client; +ros::ServiceClient gpio_set_config_client; +ros::ServiceClient gpio_get_pin_client; +ros::ServiceClient gpio_set_pin_client; + +/************************************************************************************************* +*** Services +**************************************************************************************************/ +gpio_handling::gpio_get_config gpio_get_config_srv; +gpio_handling::gpio_set_config gpio_set_config_srv; +gpio_handling::gpio_get_pin gpio_get_pin_srv; +gpio_handling::gpio_set_pin gpio_set_pin_srv; + +/************************************************************************************************* +*** Messages +**************************************************************************************************/ +agc_radar::agc_radar_msg agc_radar_msg; + +bool config_Service_callback(agc_radar::agc_radar_config::Request &req, + agc_radar::agc_radar_config::Request &res){ + agc_radar_msg.Schutzzeit = req.Schutzzeit; + ROS_INFO("set Schutzzeit to: %f", req.Schutzzeit); + return true; +} + +int main(int argc, char **argv) +{ + //defaults + agc_radar_msg.Schutzzeit = 1; + agc_radar_msg.Stopzeit = 0.01; + + ros::init(argc, argv, "agc_radar"); + ros::NodeHandle nh; + ROS_INFO("Node %s started.", ros::this_node::getName().c_str()); + + ros::Publisher agc_radar_pub = nh.advertise("agc_radar", 10); + ROS_INFO("Publishing on topic %s", agc_radar_pub.getTopic().c_str()); + ros::Rate loop_rate(100); + + ros::ServiceServer config_server = nh.advertiseService("agc_radar_config", config_Service_callback); + ROS_INFO("Service Server for radar configuration started. Call it with \"rosservice call %s\"", config_server.getService().c_str()); + + /* Configure Service Clients */ + gpio_get_config_client = nh.serviceClient("gpio_get_config"); + gpio_set_config_client = nh.serviceClient("gpio_set_config"); + gpio_get_pin_client = nh.serviceClient("gpio_get_pin"); + gpio_set_pin_client = nh.serviceClient("gpio_set_pin"); + + uint64_t begin = (uint64_t)ros::Time::now().sec *1e9 + (uint64_t)ros::Time::now().nsec; + uint64_t begin_stop = (uint64_t)ros::Time::now().sec *1e9 + (uint64_t)ros::Time::now().nsec; + gpio_get_pin_srv.request.pinNumber = {4}; + + while(ros::ok()) + { + gpio_get_pin_client.call(gpio_get_pin_srv); + + if(gpio_get_pin_srv.response.pinValue[0] == true) // radar detected an Object + { + if ((uint64_t)ros::Time::now().sec *1e9 + (uint64_t)ros::Time::now().nsec >= begin + agc_radar_msg.Stopzeit * 1e9) + { + agc_radar_msg.obsctacle_stop = true; + } + begin_stop = (uint64_t)ros::Time::now().sec *1e9 + (uint64_t)ros::Time::now().nsec; + } + else + { + if ((uint64_t)ros::Time::now().sec *1e9 + (uint64_t)ros::Time::now().nsec >= begin_stop + agc_radar_msg.Schutzzeit * 1e9) + { + agc_radar_msg.obsctacle_stop = false; + } + begin = (uint64_t)ros::Time::now().sec *1e9 + (uint64_t)ros::Time::now().nsec; + } + + /* + if(gpio_get_msg.pin_value[4] == true) // radar detected an Object + { + sum = (0.99*sum + 0.01); + } + else + { + sum = (0.99*sum - 0.01); + } + + if (sum < 0) + { + sum = 0; + } + if( sum > 1) + { + sum = 1; + } + + ROS_INFO("sum: %lf", sum); + + if (sum > 0.5) + { + agc_radar_msg.obsctacle_stop = true; + begin = (uint64_t)ros::Time::now().sec *1e9 + (uint64_t)ros::Time::now().nsec; + } + else + { + if( ((uint64_t)ros::Time::now().sec *1e9 + (uint64_t)ros::Time::now().nsec) >= (begin + agc_radar_msg.Schutzzeit) ) + { + agc_radar_msg.obsctacle_stop = false; + } + } +*/ + + + + agc_radar_msg.header.stamp = ros::Time::now(); + agc_radar_pub.publish(agc_radar_msg); + + ros::spinOnce(); + loop_rate.sleep(); + } + + + return 0; +} + diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/agc_radar/src/agc_radar_config.cpp b/Devices/Packages/sick_line_guidance/turtlebotDemo/agc_radar/src/agc_radar_config.cpp new file mode 100755 index 0000000..87b7e67 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/turtlebotDemo/agc_radar/src/agc_radar_config.cpp @@ -0,0 +1,28 @@ +#include "ros/ros.h" +#include "agc_radar/agc_radar_config.h" + +int main(int argc, char **argv) // Node Main Function +{ +ros::init(argc, argv, "agc_radar_config"); // Initializes Node Name +if (argc != 2) +{ + ROS_INFO("cmd : rosrun own own_client arg0 "); + ROS_INFO("arg0: Schutzzeit, type: float64\n arg1: Stopzeit, type: float64"); +return 1; +} + +char buffer [128]; +sprintf(buffer, "%s/agc_radar_config", ros::this_node::getNamespace().c_str()); +ros::NodeHandle nh; +ros::ServiceClient config_client = nh.serviceClient(buffer); + +// Declares the 'srv' service that uses the 'SrvTutorial' service file +agc_radar::agc_radar_config srv; + +// Parameters entered when the node is executed as a service request value are stored at 'a' and 'b' +srv.request.Schutzzeit = atoll(argv[1]); +srv.request.Stopzeit = atoll(argv[2]); +config_client.call(srv); + + return 0; +} \ No newline at end of file diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/agc_radar/srv/agc_radar_config.srv b/Devices/Packages/sick_line_guidance/turtlebotDemo/agc_radar/srv/agc_radar_config.srv new file mode 100755 index 0000000..84e5192 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/turtlebotDemo/agc_radar/srv/agc_radar_config.srv @@ -0,0 +1,3 @@ +float64 Schutzzeit +float64 Stopzeit +--- \ No newline at end of file diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/custom_messages/CMakeLists.txt b/Devices/Packages/sick_line_guidance/turtlebotDemo/custom_messages/CMakeLists.txt new file mode 100755 index 0000000..ad43af0 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/turtlebotDemo/custom_messages/CMakeLists.txt @@ -0,0 +1,29 @@ +cmake_minimum_required(VERSION 2.8.3) +project(custom_messages) + +find_package(catkin REQUIRED COMPONENTS + message_generation + std_msgs + roscpp + ) + + add_message_files( + FILES + gpio.msg + ) + + generate_messages( + DEPENDENCIES + std_msgs + ) + +#catkin_package( +#LIBRARIES custom_messages +#CATKIN_DEPENDS std_msgs roscpp +#) + +include_directories( +# include + ${catkin_INCLUDE_DIRS} +) + diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/custom_messages/msg/gpio.msg b/Devices/Packages/sick_line_guidance/turtlebotDemo/custom_messages/msg/gpio.msg new file mode 100755 index 0000000..dbd7cc2 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/turtlebotDemo/custom_messages/msg/gpio.msg @@ -0,0 +1,2 @@ +uint8[18] pin_config +bool[18] pin_value \ No newline at end of file diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/custom_messages/package.xml b/Devices/Packages/sick_line_guidance/turtlebotDemo/custom_messages/package.xml new file mode 100755 index 0000000..f7c65fe --- /dev/null +++ b/Devices/Packages/sick_line_guidance/turtlebotDemo/custom_messages/package.xml @@ -0,0 +1,27 @@ + + + custom_messages + 0.0.0 + The custom_messages package + tsprifl + TODO + + catkin + + message_generation + roscpp + std_msgs + + message_generation + message_runtime + roscpp + std_msgs + + message_generation + roscpp + std_msgs + + + + + diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/doc/OLS_barcode_0101.bmp b/Devices/Packages/sick_line_guidance/turtlebotDemo/doc/OLS_barcode_0101.bmp new file mode 100755 index 0000000..a995e9a Binary files /dev/null and b/Devices/Packages/sick_line_guidance/turtlebotDemo/doc/OLS_barcode_0101.bmp differ diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/doc/OLS_barcode_0102.bmp b/Devices/Packages/sick_line_guidance/turtlebotDemo/doc/OLS_barcode_0102.bmp new file mode 100755 index 0000000..33d4628 Binary files /dev/null and b/Devices/Packages/sick_line_guidance/turtlebotDemo/doc/OLS_barcode_0102.bmp differ diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/doc/OLS_barcodes.jpg b/Devices/Packages/sick_line_guidance/turtlebotDemo/doc/OLS_barcodes.jpg new file mode 100755 index 0000000..d2f8129 Binary files /dev/null and b/Devices/Packages/sick_line_guidance/turtlebotDemo/doc/OLS_barcodes.jpg differ diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/doc/backlog.md b/Devices/Packages/sick_line_guidance/turtlebotDemo/doc/backlog.md new file mode 100755 index 0000000..6c12f17 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/turtlebotDemo/doc/backlog.md @@ -0,0 +1,13 @@ +# Backlog + + * Makrolon bekleben nach Vorlage + * Simulation umsetzen + * Störungssimulation definieren (Bandbegrenztes normalverteiles Rauschen, systematischer Fehler und Aussetzer (Salt/Pepper)) + * Kidnapping-Problem mit Sick besprechen + * Beschreibung für Setup der Simulation + * Beschreibung der Durchführung der Simulation + + +Merker: OLS10-Simulation + Spg.-Versorgung 24V an OLS10 anklemmen und Linux booten (20.06.2019) + diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/doc/build_install.md b/Devices/Packages/sick_line_guidance/turtlebotDemo/doc/build_install.md new file mode 100755 index 0000000..a0d562e --- /dev/null +++ b/Devices/Packages/sick_line_guidance/turtlebotDemo/doc/build_install.md @@ -0,0 +1,226 @@ +# Build and install TurtleBot demonstration (sick_line_guidance_demo) + +## Prerequisites + +To run the demonstrations, Gazebo must be installed in advance. Also the keys for the access to the ROS-Repos. must be updated. + +```console +sudo apt-key adv --keyserver keyserver.ubuntu.com --recv-keys F42ED6FBAB17C654 +sudo apt-get update +sudo apt-get install ros-melodic-gazebo-ros-pkgs ros-melodic-gazebo-ros-control +``` + + +## Build and install + +To build and install the TurtleBot demonstration (sick_line_guidance_demo), run the following commands: + +```console +cd ~/catkin_ws/src +git clone https://github.com/SICKAG/sick_line_guidance.git +cd ./sick_line_guidance/turtlebotDemo/test/scripts +./gitCloneInstall.bash +``` + +This will install sick_line_guidance_demo and all required packages. +Alternatively, checkout the following packages for a manual installation: +```console +# Get ros packages for turtlebot +cd ~/catkin_ws/src +git clone https://github.com/ROBOTIS-GIT/turtlebot3.git +git clone https://github.com/ROBOTIS-GIT/turtlebot3_msgs.git +git clone https://github.com/ROBOTIS-GIT/hls_lfcd_lds_driver.git +git clone https://github.com/ros-drivers/rosserial.git +# Get ros packages for turtlebot simulation +git clone https://github.com/ROBOTIS-GIT/turtlebot3_simulations +git clone https://github.com/ROBOTIS-GIT/turtlebot3_gazebo_plugin.git +# Get can_open packages +git clone https://github.com/ros-industrial/ros_canopen.git +git clone https://github.com/CANopenNode/CANopenSocket.git +git clone https://github.com/linux-can/can-utils.git +# Get sick_line_guidance package +git clone https://github.com/ros-planning/random_numbers.git +git clone https://github.com/SICKAG/sick_line_guidance.git +# Get ros packages required for robot_fsm +git clone https://github.com/uos/sick_tim.git +# Install video support for sick_line_guidance_demo +sudo apt-get install ffmpeg +sudo apt-get install vlc +# Install profiling and performance tools +git clone https://github.com/catkin/catkin_simple.git +sudo svn export https://github.com/ethz-asl/schweizer_messer/trunk/sm_common # common utilities from ethz-asl "schweizer messer" toolbox +sudo svn export https://github.com/ethz-asl/schweizer_messer/trunk/sm_timing # timing utilities from ethz-asl "schweizer messer" toolbox +sudo apt-get install google-perftools libgoogle-perftools-dev graphviz # libprofiler for profiling +``` + +Modify file ~/.ignition/fuel/config.yaml: Replace "url: https://api.ignitionrobotics.org" by "url: https://api.ignitionfuel.org": +``` +url: https://api.ignitionrobotics.org # https://api.ignitionfuel.org +``` + +Build and install by running + +```console +cd ~/catkin_ws +catkin_make install --cmake-args -DTURTLEBOT_DEMO="ON" +source ./install/setup.bash +``` + +## Run simulation + +To test build and install, run a standalone simulation (no Turtlebot or additional hardware required): +```console +cd ~/catkin_ws/src/sick_line_guidance/turtlebotDemo/test/scripts +./runsimu.bash +``` +You should see robots positions (marked by a green dot), following the black lines: +![ ](simu01.jpg "screenshot simulation") + +## Troubleshooting + +### Versioncheck + +Depending on OS, ROS and gcc versions, errors during build or run may occur. You can determine your versions by the following commands: +```console +cat /etc/os-release # displays the Linux distribution version +lsb_release -a # displays the Linux/OS version (linux standard base) +uname -a # displays the Linux kernel version +gcc --version # displays the compiler version +rosversion -d # displays the ros distro version +rosversion roscpp # displays the version of ros package roscpp +catkin --version # displays the version of catkin tools +``` + +### Compiler and build errors + +#### "cout is not a member of std" + +:question: SynchCout.h: "error: cout is not a member of std" in package decision_making: + +:white_check_mark: Include \ in file ~/catkin_ws/src/decision_making/decision_making/include/decision_making/SynchCout.h + +#### "cmake: project 'XXX' tried to find library -lpthread" + +:question: cmake fails with error: "Project 'XXX' tried to find library '-lpthread'": + +:white_check_mark: delete component 'thread' from find_package(Boost ... COMPONENTS ...) in CMakeLists.txt of project 'XXX' + +#### "include does not exist"" + +:question: "CMake Error at /opt/ros/melodic/share/catkin/cmake/catkin_package.cmake:302 (message): catkin_package() include dir include does not exist" + +:white_check_mark: Uncommment ~/catkin_ws/src/robot_fsm/gpio_handling/CMakeLists.txt line 30 (#INCLUDE_DIRS include) + +#### "No launch file Robot_FSM.launch" + +:question: "RLException: \[Robot_FSM.launch\] is neither a launch file in package \[iam\] nor is \[iam\] a launch file name" + +:white_check_mark: Append the following lines in ~/catkin_ws/src/robot_fsm/iam/CMakeLists.txt +``` +install(FILES + launch/Robot_FSM.launch + yaml/AGC.yaml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) +``` + +#### "Could NOT find urdf" + +:question: cmake error "Could NOT find urdf": +``` +-- ==> add_subdirectory(turtlebot3/turtlebot3_description) +-- Could NOT find urdf (missing: urdf_DIR) +-- Could not find the required component 'urdf'. The following CMake error indicates that you either need to install the package with the same name or change your environment so that it can be found. +CMake Error at /opt/ros/melodic/share/catkin/cmake/catkinConfig.cmake:83 (find_package): + Could not find a package configuration file provided by "urdf" with any of the following names: + urdfConfig.cmake + urdf-config.cmake + Add the installation prefix of "urdf" to CMAKE_PREFIX_PATH or set + "urdf_DIR" to a directory containing one of the above files. If "urdf" + provides a separate development package or SDK, be sure it has been + installed. +Call Stack (most recent call first): + turtlebot3/turtlebot3_description/CMakeLists.txt:10 (find_package) +``` + +:white_check_mark: Update ros dependencies by +``` +cd ~/catkin_ws +rosdep update +rosdep install --from-paths ~/catkin_ws/src/turtlebot3 --ignore-src +rosdep install --from-paths ~/catkin_ws/src/ros_canopen --ignore-src +catkin_make install +``` + +### Runtime errors + +#### "Resource not found: IAM" + +:question: "roslaunch iam Robot_FSM.launch" causes "Resource not found: IAM" + +:white_check_mark: Modify paths in ~/catkin_ws/src/robot_fsm/iam/launch/Robot_FSM.launch : +replace \ by \ and +replace \ by \ + +#### "libdecision_making_ros.so: No such file or directory" + +:question: "catkin_ws/install/lib/iam/robot_fsm: error while loading shared libraries: libdecision_making_ros.so: cannot open shared object file: No such file or directory" + +:white_check_mark: Uncomment line 181-185 in file ~/catkin_ws/src/decision_making/decision_making/CMakeLists.txt: +``` +install(TARGETS decision_making_ros # decision_making decision_making_node + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) +``` + +#### "invalid topic type: OLS_Measurement" + +:question: "ERROR: invalid topic type: OLS_Measurement" when sending an OLS_Measurement message to robot_fsm + +:white_check_mark: Package iam (part of the robot_fsm) redefines message type OLS_Measurement. Message type iam/OLS_Measurement may differ from OLS_Measurement +definition in package sick_line_guidance. To avoid OLS_Measurement type conflicts, only OLS_Measurement messages defined in sick_line_guidance should be used. +Replace iam::OLS_Measurement by sick_line_guidance::OLS_Measurement in the following files: + +``` +// replace iam::OLS_Measurement by sick_line_guidance::OLS_Measurement in file catkin_ws/src/robot_fsm/iam/include/iam/robot_fsm.h: +#include "sick_line_guidance/OLS_Measurement.h" // line 14: "iam/OLS_Measurement.h" +void olsCallback(const sick_line_guidance::OLS_Measurement::ConstPtr& msg); // line 84: void olsCallback(const iam::OLS_Measurement::ConstPtr& msg); +sick_line_guidance::OLS_Measurement::Type ols_msg; // line 99: iam::OLS_Measurement::Type ols_msg; + +// replace iam::OLS_Measurement by sick_line_guidance::OLS_Measurement in file catkin_ws/src/robot_fsm/iam/src/robot_fsm.cpp: +void olsCallback(const sick_line_guidance::OLS_Measurement::ConstPtr& msg){ // line 498: void olsCallback(const iam::OLS_Measurement::ConstPtr& msg){ + +// deactivate iam::OLS_Measurement in add_message_files(...) in file catkin_ws/src/robot_fsm/iam/CMakeLists.txt: + # OLS_Measurement.msg +``` + +#### "turtlebot3_gazebo: Error in REST request" + +:question: "roslaunch turtlebot3_gazebo turtlebot3_world.launch" results in error message +"\[Err\] \[REST.cc:205\] Error in REST request libcurl: (51) SSL: no alternative certificate subject name matches target host name 'api.ignitionfuel.org'" + +:white_check_mark: Replace url: https://api.ignitionrobotics.org by https://api.ignitionfuel.org in file ~/.ignition/fuel/config.yaml: +``` +url: https://api.ignitionrobotics.org # https://api.ignitionfuel.org +``` + +#### "turtlebot3_gazebo: VMware: vmw_ioctl_command error Invalid argument. Aborted (core dumped)" + +:question: "roslaunch turtlebot3_gazebo turtlebot3_world.launch" results in a core dump with an error message +"VMware: vmw_ioctl_command error Invalid argument. Aborted (core dumped)" + +:white_check_mark: Uncheck "Accelerate 3D Graphics" in VMware settings and set +```console +export SVGA_VGPU10=0 +export LIBGL_ALWAYS_SOFTWARE=1 +``` +before launching turtlebot3_gazebo. + + +#### Connect to bot: +```console +ssh turtlebot@192.168.178.45 +``` +ps turtlebot3 diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/doc/demo_map.pptx b/Devices/Packages/sick_line_guidance/turtlebotDemo/doc/demo_map.pptx new file mode 100755 index 0000000..700d9d1 Binary files /dev/null and b/Devices/Packages/sick_line_guidance/turtlebotDemo/doc/demo_map.pptx differ diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/doc/demo_map_01.png b/Devices/Packages/sick_line_guidance/turtlebotDemo/doc/demo_map_01.png new file mode 100755 index 0000000..7b51dba Binary files /dev/null and b/Devices/Packages/sick_line_guidance/turtlebotDemo/doc/demo_map_01.png differ diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/doc/demo_map_02.png b/Devices/Packages/sick_line_guidance/turtlebotDemo/doc/demo_map_02.png new file mode 100755 index 0000000..9d7d803 Binary files /dev/null and b/Devices/Packages/sick_line_guidance/turtlebotDemo/doc/demo_map_02.png differ diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/doc/faq.md b/Devices/Packages/sick_line_guidance/turtlebotDemo/doc/faq.md new file mode 100755 index 0000000..77ddd31 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/turtlebotDemo/doc/faq.md @@ -0,0 +1,77 @@ +# Interne Sammlung von FAQ + +## Zugriff auf Turtlebot über WLAN-Lehning + +Der Zugriff erfolgt über SSH per WLAN. Der Turtlebot holt sich via DHCP (eigentlich) immer dieselbe IP-Adresse. +Deswegen wurde vorerst auf Static-IP verzichtet. +Die IP-Adresse lautet: 192.168.178.45 + +Mit dem Zugriff über + +```bash +ssh -X turtlebot@192.168.178.45 +``` + +kann die Remote-Verbindung zum Roboter aufgebaut werden. + + +## PC: + + PC-Typ: Ist es der Typ „EC700-BT4051-454-64WT“, den Sie in Ihrer Master-Arbeit erwähnen? -> Ja + Ist ein Account/Ubuntu eingerichtet? -> Ja + Annahme: Ubuntu: 18.04 Betriebssystem mit der ROS-Distribution ROS-Melodic -> Richtig + Wenn „Ja“: Username und Passwort? -> Username: turtlebot, Passwort: turtlebot3 + Kann man den PC mit einem der beigefügten Netzteile betreiben? -> Könnte man theoretisch. Ich würde Ihnen empfehlen den Stecker (roter Pfeil) einzustecken. (Aufgrund der Länge passt er nur in die auf dem Bild rechte Buchse). Dazu können Sie das Netzteil für die Powerbank in die linke Buchse einstecken. Dann haben Sie einen "quasi Netzbetrieb". + +## TiM: + + Eingestellte IP-Adresse? -> Am TiM habe ich nichts konfiguriert. Daher müsste noch die Standard-IP-Adresse konfiguriert sein. + +## Spg.-Versorgung: + + Batterie wird mit dem Standardnetzteil aufgeladen? Blauer Pfeil? -> Richtig + Spg.-Versorgung zum PC erfolgt einfach durch Einstecken des Hohlsteckers in die Batterie? (roter Pfeil) -> Korrekt + + +## Miniatur-Lichtschranken: + + Ich nehme an, dass die Miniatur-Lichtschranken von Typ "GL2S- E1311" sind (vgl. Masterarbeit) -> korrekt + Sind die Lichtschranken an das OpenCR-Board wie in der Masterarbeit angeschlossen? -> Auf dem "Version 0" Dokument, das Sie haben finden Sie auf Seite 16 die Abbildung 5.1. Die Sensoren sind wie dort abgebildet am OpenCR-Board angeschlossen. Die Einweglichtschranken (GRSE18S) sind auf Ihrem Turtlebot jedoch nicht vorhanden. Zwischen dem Schaltausgang der Miniatur-Lichtschranken GL2S-E1311 und dem GPIO-Pin befindet sich noch ein Serienwiderstand von 57k, um die Spannung an den GPIO-Pins auf 3,3V anzupassen (Thesis S. 20 Abb. 5.6). + + +## Allgemein: + + Sollte ich beim Laden, Betreiben etc. auf irgendetwas besonders achten? -> Ich habe in dem Git-Repo, in dem ich Sie als Collaborator hinzugefügt habe eine Readme.md erstellt. Mit dieser Datei könnte es Ihnen evtl. etwas leichter fallen, das System -- so wie ich es immer in Betrieb genommen habe -- in Betrieb zu nehmen. + +## Herunterfahren + +```bash +sudo shutdown -h now +# Alternativ: +sudo init 0 +``` + +## Turtlebot: USB stick mounten + +```bash +# list all devices: +sudo fdisk -l +# usbstick-device finden , z.B. /dev/sdb1 +sudo mkdir /media/usbstick +# usb stick mounten, /dev/sdb1 ggfs ersetzen +sudo mount -t vfat /dev/sdb1 /media/usbstick -o uid=1000,gid=1000,utf8,dmask=027,fmask=137 +# usb stick gemounted unter /media/usbstick: +ls /media/usbstick +``` + +## Turtlebot: Filetransfer mit scp + +```bash +# ros logfiles mit scp vom Turtlebot in ein lokales Verzeichnis auf dem Linux-Rechner kopieren: +rostest@ROS-NB: +mkdir -p ~/tmp +cd ~/tmp +scp -r turtlebot@192.168.178.45:/home/turtlebot/.ros/log scp -r turtlebot@192.168.178.45:/home/turtlebot/catkin_ws/src/sick_line_guidance/turtlebotDemo/test/scripts/log . +# scp -r turtlebot@192.168.178.45:/home/turtlebot/.ros/log . +# scp -r turtlebot@192.168.178.45:/home/turtlebot/catkin_ws/src/sick_line_guidance_demo/test/scripts/log . +``` diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/doc/inbetriebnahme.md b/Devices/Packages/sick_line_guidance/turtlebotDemo/doc/inbetriebnahme.md new file mode 100755 index 0000000..4893166 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/turtlebotDemo/doc/inbetriebnahme.md @@ -0,0 +1,69 @@ +# Inbetriebnahme + +Die vorliegende Anleitung zeigt die Inbetriebnahme der Hardware für den Turtlebot. + +Ausführliche Details findet man unter: +https://github.com/tsprifl/catkin_src + +# Lager +Der Turtlebot und die Unterlagen (neben Ladegerät etc.) lagen im Labor in den Kisten +# Schritte + +1. Powerverbindung mit XTPower-Akku herstellen. +2. Ggf. Ladegerät "XTPower TurtleBot Sick" anschließen. +3. Gerät startet +4. Gerät mit Monitor (HDMI an der Frontseite) und USB über HUB mit Tastatur und Maus verbinden. +5. Bei Sick wurde das Gerät hinter einem Proxy betrieben. Der ROS-Master lief auf einem Remote-PC. + Möchte man bei SICK eine Demo starten, muss man den Proxy gemäß Proxy Config einstellen. +6. ROS-Master: Bei einer lokalen Entwicklung auf dem System ROS_MASTER und ROS_HOSTNAME gem. u.a. config eingestellt werden. + Für eine Remote-Verbindung muss die IP-Adresse etc. für Remote-Rechner mit laufendem Ros-Core bekannt sein. + (siehe u.a. Punkte) +7. Nun der o.a. Anleitung folgen (ACHTUNG: In der Anleitungen befinden sich Fehler. Siehe "Start des Bots". Statt + "rosrun iam" muss man "roslaunch iam Robot_FSM.launch" angeben (s.u.). + In Kurzform: + * Terminator: 4 Terminalfenstser starten + * Terminal 1: roslaunch turtlebot3_bringup turtlebot3_robot.launch + * Terminal 2: ols + * Terminal 3: rosrun gpio_handling gpio_handler + * Terminal 4: rosparam load ~/catkin_ws/src/iam/yaml/AGC.yam + * Terminal 4 (erneut): roslaunch iam Robot_FSM.launch + +8. Spurführungsband verlegen bzw. Robot auf Demo-System setzen. +9. terminator starten + +## Proxy Config +* Bei Lehning : keine +* Bei Sick : + * Host/Port: cloudproxy-sickag.sickcn.net:10415 + * Details siehe: + * Webproxy: https://wiki.ubuntuusers.de/Proxyserver/#Unity-und-GNOME-3 + * apt proxy: https://askubuntu.com/questions/257290/configure-proxy-for-apt + +## ros config remote/local master +IP Config Bash RC +In der Datei ~/.bashrc muss die IP-Adresse des Ros-Master (Remote-PC), sowie die IP-Adresse des Turtlebots eingetragen werden. (Ändern der letzten beiden Zeilen). +für lokalen master 127.0.0.1 +export ROS_MASTER_URI=http://127.0.0.1:11311 +export ROS_HOSTNAME=127.0.0.1 + +## Start des Bots + +Für jedes Terminal wie gewohnt: +```console +cd catkin_ws +source devel/setup.bash +``` + +Bei Punkt 6: +statt +```console +rosrun iam +``` +```console +roslaunch iam Robot_FSM.launch +``` + +siehe Anleitung unter +https://github.com/tsprifl/catkin_src + + diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/doc/ols_sensor_properties.md b/Devices/Packages/sick_line_guidance/turtlebotDemo/doc/ols_sensor_properties.md new file mode 100755 index 0000000..223b401 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/turtlebotDemo/doc/ols_sensor_properties.md @@ -0,0 +1,57 @@ +# Properties of OLS line guidance sensor for sick_line_guidance_demo + +The OLS sensor on the demo system is mounted 65 mm over ground. Both line distances (lcp, line center points) and line width measured by the sensor +depend on its height over ground. LineSensorConfig configures a scaling between sensor measurement and physical world: + +| parameter | default value | details | +| --- | --- | --- | +| line_sensor_scaling_dist | 180.0 / 133.0 | Scaling between physical distance to the line center and measured line center point, depending on mounted sensor height (measurement: lcp = 180 mm, physical: lcp = 133 mm) | +| line_sensor_scaling_width | 29.0 / 20.0 | Scaling between physical line width (20 mm) and measured line width (29 mm), depending on mounted sensor height (sensor mounted 100 mm over ground: scaling = 1, sensor mounted 65 mm over ground: scaling = 100/65 = 1.5) | + + +## Line distance scaling + +Line center points measured with the demo system (OLS mounted 65 mm over ground): + +| lcp measured | physical line distance | scaling | +| --- | --- | --- | +| 180 mm | 133 mm | 180/133 = 1.35 | + +Line offset: The sensor is not completely centered. If the sensor is centered over the right side of the line, +a line center point with value 0 is detected. + +## Line width scaling + +Line width measured with the demo system (OLS mounted 65 mm over ground): + +| angle | measured width | physical width | scaling | +| --- | --- | --- | --- | +| 0 degree | 29 mm | 20 mm | 29/29 = 1.45 | +| 45 degree | 43 mm | 28 mm | 43/28 = 1.52 | +| max. | 75 mm | | | + +## Line switching at junctions + +Lines detected by the sensor can switch at junctions depending on robots movement. When taking the left turnout, the left line will become the center line +and the previous main line will become the right line. + +The following line switches were observed under a test of the demo system: + +Sensor moves from left to right at a junction (2 lines, lcp\[0\], lcp\[1\], lcp\[2\] measured in mm): + +| lcp\[0\] | lcp\[1\] | lcp\[2\] | description | +| --- | --- | --- | --- | +| - | 88 | - | start, one line visible | +| - | 0 | 46 | switched to 2 lines, when sensor in the center of left line | +| -32 | 12 | - | next line switch | + +Sensor moves from right to left at a junction (2 lines, lcp\[0\], lcp\[1\], lcp\[2\] measured in mm): + + +## Line detection at barcodes + +If a barcode is placed over the line, lines are detected in different ways: +1. There are no lines detected within the lower textzone (the area with human readable numbers). +2. Within the label zone (machine readable barcode label), a line with a width of round about the barcode is detected (line width >= 80 mm). +3. If the yaw angle between barcode and sensor is above 30 degrees, two or three lines are detected with small width (i.e. the markers are detected as lines). +4. No lines are detected within the small upper gap between the label area and the upper barcode border. diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/doc/planung_demo.pptx b/Devices/Packages/sick_line_guidance/turtlebotDemo/doc/planung_demo.pptx new file mode 100755 index 0000000..fb62723 Binary files /dev/null and b/Devices/Packages/sick_line_guidance/turtlebotDemo/doc/planung_demo.pptx differ diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/doc/setup_turtlebot.md b/Devices/Packages/sick_line_guidance/turtlebotDemo/doc/setup_turtlebot.md new file mode 100755 index 0000000..2574871 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/turtlebotDemo/doc/setup_turtlebot.md @@ -0,0 +1,46 @@ +Hostname des Turtlebots: turtlebot +Password: turtlebot3 + + +Setup: + +Zunächst muss sich der Turtlebot im gleichen Netz wie der Remote-PC befinden. Im Auslieferungszustand wird sich der EC700-BT Rechner in ein WLAN mit der SSID "Ubuntu-Hotspot" mit dynamischer IP einloggen, da dies meine Konfiguration war. +Die Empfehlung ist, bei der ersten Inbetriebnahme Maus und Tastatur am hinteren Ende des AGC, sowie HDMI-Kabel vorne neben dem OLS20 in den Rechner einzustecken. Dann kann manuell ein WLAN-Netzwerk gespeichert werden, in welches sich der Rechner nach dem booten einloggt (vorzugsweise mit fester IP-Adresse). + +In der Datei ~/.bashrc muss die IP-Adresse des Ros-Master (Remote-PC), sowie die IP-Adresse des Turtlebots eingetragen werden. (Ändern der letzten beiden Zeilen). + +Nachdem sichergestellt ist, dass sich der Turtlebot-PC beim Neustart in das gewünschte Netz einloggt, kann Maus, Tastatur und HDMI-Kabel abgezogen werden und der Turtlebot-PC neu gestartet werden. + + +Damit im folgenden Abschnitt nicht bei jeder ssh-Verbindung das Password eingegeben werden muss, empfielt es sich, am Remote-PC ein rsa-key zu generieren und diesen auf dem Turtlebot-PC zu speichern. Dazu auf dem Remote-PC folgende Befehle ausführen: + $ ssh-keygen -t rsa #Den Key ohne Password durch zweimaliges drücken von Enter speichern. + $ ssh-copy-id -i ~/.ssh/id_rsa.pub turtlebot@Turtlebot-IP-Adresse + + +Starten der Robot-Fsm: + +1. Turtlebot an Powerbank einstecken und ca. 1 min warten, bis sich der Turtlebot-PC in das konfigurierte Netzwerk eingeloggt hat. + +2. roscore vom Remote-PC aus starten. + +3. Konsolenfenster öffnen und eine ssh-Verbindung zum Turtlebot-PC erstellen. Anschließend zum OpenCR-Board verbinden. Dies geschieht Normalerweise durch den Befehl "roslaunch turtlebot3_bringup turtlebot3_robot.launch". Der Befehl ist jedoch durch den Kurzbefehl "rl" hinterlegt. Somit folgende Befehle ausführen: + $ ssh turtlebot@IP-Adresse #Danach besteht das Konsolenfenster auf dem Turtlebot-PC + $ rl # Am Anfang wird ein Fehler auftauchen, da der Standard-Laserscanner nicht verbunden ist. Der Fehler kann ignoriert werden. + +4. Konsolenfenster öffnen und eine ssh-Verbindung zum Turtlebot-PC erstellen. Anschließend den ols-Treiber starten. Dies geschieht Normalerweise durch den Kurzbefehl "ols". Dieser konfiguriert die can0-Schnittstelle und führt das sick_line_guidance launch-file mit dem ols-yaml-file aus. Somit folgende Befehle ausführen: + $ ssh turtlebot@IP-Adresse #Danach besteht das Konsolenfenster auf dem Turtlebot-PC + $ ols + +5. Die gpio-handler Node vom Remote-PC aus starten: + $ rosrun gpio_handling gpio_handler + +### ACHTUNG: Beim ausführen der nächsten Befehle wird sich der Turtlebot geradeaus bewegen. ### +6. Das yaml-Parameter-File für die robot-fsm-Node laden und anschließend die Node starten: + $ rosparam load ~/catkin_ws/src/iam/yaml/AGC.yaml + $ rosrun iam + +7. Durch starten der Teleop-Node kann der Turtlebot gestoppt werden. + (Achtung hier publishen nun zwei Nodes auf dem gleichen Topic (cmd_vel). Deshalb muss die robot_fsm Node kurz nach dem starten der teleop-Node mit strg+c beendet werden): + $ roslaunch turtlebot3_teleop turtlebot3_teleop_key.launch # Durch drücken der Taste s auf der Tastatur, kann der Turtlebot sofort gestoppt werden. + +Der Turtlebot fährt nun geradeaus. Wird eine Linie erkannt erfolgt ein Wechsel in den State follow_line. Der Turtlebot folgt der Linie. Geht die Linie verloren, wird der Turtlebot nach 1 Sekunde stoppen. diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/doc/simu01.jpg b/Devices/Packages/sick_line_guidance/turtlebotDemo/doc/simu01.jpg new file mode 100755 index 0000000..3126efc Binary files /dev/null and b/Devices/Packages/sick_line_guidance/turtlebotDemo/doc/simu01.jpg differ diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/gpio_handling/CMakeLists.txt b/Devices/Packages/sick_line_guidance/turtlebotDemo/gpio_handling/CMakeLists.txt new file mode 100755 index 0000000..6f0a674 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/turtlebotDemo/gpio_handling/CMakeLists.txt @@ -0,0 +1,53 @@ +cmake_minimum_required(VERSION 2.8.3) +project(gpio_handling) + +find_package(catkin REQUIRED COMPONENTS + message_generation + roscpp + std_msgs +) + +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +add_service_files( + FILES + gpio_set_config.srv + gpio_get_config.srv + gpio_set_pin.srv + gpio_get_pin.srv + ) + +generate_messages( + DEPENDENCIES + std_msgs + custom_messages + ) + +#catkin_package( +# # INCLUDE_DIRS include +# LIBRARIES gpio_handling +# CATKIN_DEPENDS message_generation message_runtime roscpp std_msgs +#) + +include_directories( ${catkin_INCLUDE_DIRS} ) + +# Service Server +add_executable(gpio_handler src/gpio_handler.cpp) +add_dependencies(gpio_handler ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(gpio_handler ${catkin_LIBRARIES} ) + +install(TARGETS gpio_handler + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE +) diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/gpio_handling/msg/gpio.msg b/Devices/Packages/sick_line_guidance/turtlebotDemo/gpio_handling/msg/gpio.msg new file mode 100755 index 0000000..dbd7cc2 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/turtlebotDemo/gpio_handling/msg/gpio.msg @@ -0,0 +1,2 @@ +uint8[18] pin_config +bool[18] pin_value \ No newline at end of file diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/gpio_handling/package.xml b/Devices/Packages/sick_line_guidance/turtlebotDemo/gpio_handling/package.xml new file mode 100755 index 0000000..d3912e6 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/turtlebotDemo/gpio_handling/package.xml @@ -0,0 +1,34 @@ + + + gpio_handling + 0.0.0 + The gpio_handling package + tsprifl + + TODO + + catkin + + roscpp + + message_generation + std_msgs + + roscpp + + message_generation + std_msgs + + roscpp + message_runtime + std_msgs + + + + + + diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/gpio_handling/src/gpio_handler.cpp b/Devices/Packages/sick_line_guidance/turtlebotDemo/gpio_handling/src/gpio_handler.cpp new file mode 100755 index 0000000..451c65d --- /dev/null +++ b/Devices/Packages/sick_line_guidance/turtlebotDemo/gpio_handling/src/gpio_handler.cpp @@ -0,0 +1,82 @@ +#include "gpio_handler.h" + +int main(int argc, char **argv) // node main function +{ + ros::init(argc, argv, "gpio_handler"); // initialize node with given name + ROS_INFO("Node %s started", ros::this_node::getName().c_str()); // print info to ros command line + ros::NodeHandle nh; // init node handle + + // init servive servers + ros::ServiceServer set_config_server = nh.advertiseService("gpio_set_config", gpio_set_config); + ROS_INFO("gpio set_config_server ready. Call it with: rosservice call %s", set_config_server.getService().c_str()); + + ros::ServiceServer get_config_server = nh.advertiseService("gpio_get_config", gpio_get_config); + ROS_INFO("gpio get_config_server ready. Call it with: rosservice call %s", get_config_server.getService().c_str()); + + ros::ServiceServer set_server = nh.advertiseService("gpio_set_pin", gpio_set_pin); + ROS_INFO("gpio set_server ready. Call it with: rosservice call %s", set_server.getService().c_str()); + + ros::ServiceServer get_server = nh.advertiseService("gpio_get_pin", gpio_get_pin); + ROS_INFO("gpio get_server ready. Call it with: rosservice call %s", get_server.getService().c_str()); + + // init publisher and subscriber + ros::Publisher set_publisher = nh.advertise("/gpio_set", 1, false); + ros::Subscriber get_subscriber = nh.subscribe("/gpio_get", 1, gpioCallback); + + /* gpio-message default values */ + for (int i = 0; i <= 17; i++) + { + gpio_set_msg.pin_config[i] = 4; //input pulldown (high impedance) + gpio_set_msg.pin_value[i] = 0; + gpio_get_msg.pin_value[i] = 4; + gpio_get_msg.pin_value[i] = 0; + } + + ros::Rate loop_rate(200); // publish message rate = 200 Hz + + while(ros::ok()) + { + ros::spinOnce(); // handle service requests and subscriber callbacks + set_publisher.publish(gpio_set_msg); // publish messages + loop_rate.sleep(); // sleep for loop_rate + } + return 0; +} + +void gpioCallback(const custom_messages::gpioConstPtr &msg){ + gpio_get_msg = *msg; +} + +bool gpio_set_config(gpio_handling::gpio_set_config::Request &req, gpio_handling::gpio_set_config::Response &res){ + for(uint8_t i = 0; i < req.pinNumber.size(); i++) + { + gpio_set_msg.pin_config[req.pinNumber[i]] = req.pinMode[i]; + } + return true; +} + +bool gpio_get_config(gpio_handling::gpio_get_config::Request &req, gpio_handling::gpio_get_config::Response &res){ + res.pinMode.resize(req.pinNumber.size()); + for(uint8_t i = 0; i < req.pinNumber.size(); i++) + { + res.pinMode[i] = gpio_get_msg.pin_config[req.pinNumber[i]]; + } + return true; +} + +bool gpio_set_pin(gpio_handling::gpio_set_pin::Request &req, gpio_handling::gpio_set_pin::Response &res){ + for(uint8_t i = 0; i < req.pinNumber.size(); i++) + { + gpio_set_msg.pin_value[req.pinNumber[i]] = req.pinValue[i]; + } + return true; +} + +bool gpio_get_pin(gpio_handling::gpio_get_pin::Request &req, gpio_handling::gpio_get_pin::Response &res){ + res.pinValue.resize(req.pinNumber.size()); + for(uint8_t i = 0; i < req.pinNumber.size(); i++) + { + res.pinValue[i] = gpio_get_msg.pin_value[req.pinNumber[i]]; + } + return true; +} \ No newline at end of file diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/gpio_handling/src/gpio_handler.h b/Devices/Packages/sick_line_guidance/turtlebotDemo/gpio_handling/src/gpio_handler.h new file mode 100755 index 0000000..8915ee9 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/turtlebotDemo/gpio_handling/src/gpio_handler.h @@ -0,0 +1,19 @@ +#include "ros/ros.h" +#include "custom_messages/gpio.h" +#include "gpio_handling/gpio_set_config.h" +#include "gpio_handling/gpio_get_config.h" +#include "gpio_handling/gpio_set_pin.h" +#include "gpio_handling/gpio_get_pin.h" +#include +#include + +// messages for publish and subscribing for communication to openCR-board +custom_messages::gpio gpio_set_msg; +custom_messages::gpio gpio_get_msg; + +// function Declaration +void gpioCallback(const custom_messages::gpioConstPtr &msg); +bool gpio_set_config(gpio_handling::gpio_set_config::Request &req, gpio_handling::gpio_set_config::Response &res); +bool gpio_get_config(gpio_handling::gpio_get_config::Request &req, gpio_handling::gpio_get_config::Response &res); +bool gpio_set_pin(gpio_handling::gpio_set_pin::Request &req, gpio_handling::gpio_set_pin::Response &res); +bool gpio_get_pin(gpio_handling::gpio_get_pin::Request &req, gpio_handling::gpio_get_pin::Response &res); diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/gpio_handling/srv/gpio_get_config.srv b/Devices/Packages/sick_line_guidance/turtlebotDemo/gpio_handling/srv/gpio_get_config.srv new file mode 100755 index 0000000..7c6e97e --- /dev/null +++ b/Devices/Packages/sick_line_guidance/turtlebotDemo/gpio_handling/srv/gpio_get_config.srv @@ -0,0 +1,3 @@ +uint8[] pinNumber +--- +uint8[] pinMode \ No newline at end of file diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/gpio_handling/srv/gpio_get_pin.srv b/Devices/Packages/sick_line_guidance/turtlebotDemo/gpio_handling/srv/gpio_get_pin.srv new file mode 100755 index 0000000..8f37327 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/turtlebotDemo/gpio_handling/srv/gpio_get_pin.srv @@ -0,0 +1,3 @@ +uint8[] pinNumber +--- +bool[] pinValue \ No newline at end of file diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/gpio_handling/srv/gpio_set_config.srv b/Devices/Packages/sick_line_guidance/turtlebotDemo/gpio_handling/srv/gpio_set_config.srv new file mode 100755 index 0000000..064ef9f --- /dev/null +++ b/Devices/Packages/sick_line_guidance/turtlebotDemo/gpio_handling/srv/gpio_set_config.srv @@ -0,0 +1,3 @@ +uint8[] pinNumber +uint8[] pinMode +--- \ No newline at end of file diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/gpio_handling/srv/gpio_set_pin.srv b/Devices/Packages/sick_line_guidance/turtlebotDemo/gpio_handling/srv/gpio_set_pin.srv new file mode 100755 index 0000000..1aceab6 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/turtlebotDemo/gpio_handling/srv/gpio_set_pin.srv @@ -0,0 +1,3 @@ +uint8[] pinNumber +bool[] pinValue +--- \ No newline at end of file diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/lidar_obstacle_detection/CMakeLists.txt b/Devices/Packages/sick_line_guidance/turtlebotDemo/lidar_obstacle_detection/CMakeLists.txt new file mode 100755 index 0000000..8439a9e --- /dev/null +++ b/Devices/Packages/sick_line_guidance/turtlebotDemo/lidar_obstacle_detection/CMakeLists.txt @@ -0,0 +1,51 @@ +cmake_minimum_required(VERSION 2.8.3) +project(lidar_obstacle_detection) + +## Compile as C++11, supported in ROS Kinetic and newer +add_compile_options(-std=c++17) + +find_package(catkin REQUIRED COMPONENTS + message_generation + roscpp + std_msgs +) + + add_message_files( + FILES + obstacle_detector_msg.msg + ) + + add_service_files( + FILES + obstacle_detector_config.srv +) + +# Generate added messages and services with any dependencies listed here + generate_messages( + DEPENDENCIES + std_msgs + ) + +#catkin_package( +# #INCLUDE_DIRS include +# LIBRARIES lidar_obstacle_detection +# CATKIN_DEPENDS message_generation roscpp std_msgs message_runtime +# #DEPENDS system_lib +#) + +########### +## Build ## +########### + +include_directories( +# include + ${catkin_INCLUDE_DIRS} +) + +add_executable(obstacle_detector src/obstacle_detector.cpp) +add_dependencies(obstacle_detector ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(obstacle_detector ${catkin_LIBRARIES}) + +add_executable(obstacle_detector_config src/obstacle_detector_config.cpp) +add_dependencies(obstacle_detector_config ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(obstacle_detector_config ${catkin_LIBRARIES}) diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/lidar_obstacle_detection/msg/obstacle_detector_msg.msg b/Devices/Packages/sick_line_guidance/turtlebotDemo/lidar_obstacle_detection/msg/obstacle_detector_msg.msg new file mode 100755 index 0000000..a8c4f59 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/turtlebotDemo/lidar_obstacle_detection/msg/obstacle_detector_msg.msg @@ -0,0 +1,8 @@ +Header header +bool obsctacle_stop +float32 obstacle_stop_range +int16 angle_min +int16 angle_max +float32 range_min +float32 range_max +float32 object_distance_min \ No newline at end of file diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/lidar_obstacle_detection/package.xml b/Devices/Packages/sick_line_guidance/turtlebotDemo/lidar_obstacle_detection/package.xml new file mode 100755 index 0000000..ecd13f0 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/turtlebotDemo/lidar_obstacle_detection/package.xml @@ -0,0 +1,31 @@ + + + lidar_obstacle_detection + 0.0.0 + The lidar_obstacle_detection package + tsprifl + TODO + + catkin + + message_generation + roscpp + std_msgs + message_runtime + + message_generation + roscpp + std_msgs + message_runtime + + message_generation + roscpp + std_msgs + message_runtime + + + + + + + diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/lidar_obstacle_detection/src/obstacle_detector.cpp b/Devices/Packages/sick_line_guidance/turtlebotDemo/lidar_obstacle_detection/src/obstacle_detector.cpp new file mode 100755 index 0000000..23c4fd7 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/turtlebotDemo/lidar_obstacle_detection/src/obstacle_detector.cpp @@ -0,0 +1,97 @@ +#include "ros/ros.h" +#include "lidar_obstacle_detection/obstacle_detector_msg.h" // for publishing +#include "lidar_obstacle_detection/obstacle_detector_config.h" // for service +#include "sensor_msgs/LaserScan.h" // for subscribing +#include +#include +#include + +lidar_obstacle_detection::obstacle_detector_msg obstacle_detector_msg; + +void scan_Callback(const sensor_msgs::LaserScan::ConstPtr& msg){ + uint16_t angle; + obstacle_detector_msg.obsctacle_stop = false; + obstacle_detector_msg.object_distance_min = obstacle_detector_msg.range_max; + //ROS_INFO("angle_min: %d angle_max: %d", obstacle_detector_msg.angle_min, obstacle_detector_msg.angle_max); + + for(int16_t i = obstacle_detector_msg.angle_min; i < obstacle_detector_msg.angle_max; i++){ + if(i < 0) + { + angle = i+360; + } + else + { + angle = i; + } + + + if(msg->ranges[angle] > obstacle_detector_msg.range_min && msg->ranges[angle] < obstacle_detector_msg.range_max) // if range value is between range_min and range_max + { + //ROS_INFO("in range value at angle: %d range: %f", angle, msg->ranges[angle]); + if(msg->ranges[angle] < obstacle_detector_msg.object_distance_min) + { + obstacle_detector_msg.object_distance_min = msg->ranges[angle]; + } + + if(msg->ranges[angle] < obstacle_detector_msg.obstacle_stop_range)// object in stop range + { + obstacle_detector_msg.obsctacle_stop = true; + } + } + } +} + +bool config_Service_callback(lidar_obstacle_detection::obstacle_detector_config::Request &req, + lidar_obstacle_detection::obstacle_detector_config::Request &res){ + obstacle_detector_msg.range_min = req.range_min; + ROS_INFO("set range_min to: %f", req.range_min); + obstacle_detector_msg.range_max = req.range_max; + ROS_INFO("set range_max to: %f", req.range_max); + obstacle_detector_msg.angle_min = req.angle_min; + ROS_INFO("set angle_min to: %d", req.angle_min); + obstacle_detector_msg.angle_max = req.angle_max; + ROS_INFO("set angle_max to: %d", req.angle_max); + obstacle_detector_msg.obstacle_stop_range = req.obstacle_stop_range; + ROS_INFO("set obstacle_stop_range to: %f", req.obstacle_stop_range); + + return true; +} + +int main(int argc, char **argv){ + //defaults + obstacle_detector_msg.angle_min = -30; + obstacle_detector_msg.angle_max = 30; + obstacle_detector_msg.range_min = 0.2; + obstacle_detector_msg.range_max = 0.5; + obstacle_detector_msg.obstacle_stop_range = 0.3; + obstacle_detector_msg.header.frame_id = "obstacle_detector_frame"; + + ros::init(argc, argv, "obstacle_detector"); + ros::NodeHandle nh; + ROS_INFO("Node %s started.", ros::this_node::getName().c_str()); + + char buffer [128]; + sprintf(buffer, "%s/obstacle_detector_config", ros::this_node::getNamespace().c_str()); + ros::ServiceServer config_server = nh.advertiseService(buffer, config_Service_callback); + ROS_INFO("Service Server for configuration started. Call it with \"rosservice call %s\"", config_server.getService().c_str()); + + sprintf(buffer, "%s/scan", ros::this_node::getNamespace().c_str()); + ros::Subscriber scan_sub = nh.subscribe(buffer, 1, scan_Callback); + ROS_INFO("Subscribing topic %s", scan_sub.getTopic().c_str()); + + sprintf(buffer, "%s/obstacle", ros::this_node::getNamespace().c_str()); + ros::Publisher obstacle_detector_pub = nh.advertise(buffer, 1); + ROS_INFO("Publishing topic %s", obstacle_detector_pub.getTopic().c_str()); + + ros::Rate loop_rate(100); // Publiziere Nachricht mit 100 Hz + + while(ros::ok()) + { + obstacle_detector_msg.header.stamp = ros::Time::now(); + obstacle_detector_pub.publish(obstacle_detector_msg); + + ros::spinOnce(); + loop_rate.sleep(); // Sleep for 100 ms + } + return 0; +} \ No newline at end of file diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/lidar_obstacle_detection/src/obstacle_detector_config.cpp b/Devices/Packages/sick_line_guidance/turtlebotDemo/lidar_obstacle_detection/src/obstacle_detector_config.cpp new file mode 100755 index 0000000..21b5759 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/turtlebotDemo/lidar_obstacle_detection/src/obstacle_detector_config.cpp @@ -0,0 +1,43 @@ +#include "ros/ros.h" +#include "lidar_obstacle_detection/obstacle_detector_config.h" + +int main(int argc, char **argv) // Node Main Function +{ +ros::init(argc, argv, "obstacle_detector_config"); // Initializes Node Name +if (argc != 5) +{ + ROS_INFO("cmd : rosrun own own_client arg0 arg1 arg2 arg3 arg4"); + ROS_INFO("arg0: angle_min, type: int16 \n arg1: angle_max, type: int16 \n arg2: range_min, type: int16 \n arg3: rang_max, type: int16 \n arg4: obstacle_stop_range, type: int16 \n"); +return 1; +} + +char buffer [128]; +sprintf(buffer, "%s/obstacle_detector_config", ros::this_node::getNamespace().c_str()); +ros::NodeHandle nh; +ros::ServiceClient config_client = nh.serviceClient(buffer); + +// Declares the 'srv' service that uses the 'SrvTutorial' service file +lidar_obstacle_detection::obstacle_detector_config srv; + +// Parameters entered when the node is executed as a service request value are stored at 'a' and 'b' +srv.request.range_min = atoll(argv[1]); +srv.request.range_max = atoll(argv[2]); +srv.request.angle_min = atoll(argv[3]); +srv.request.angle_max = atoll(argv[4]); +srv.request.obstacle_stop_range = atoll(argv[5]); +config_client.call(srv); +/* +// Request the service. If the request is accepted, display the response value +if (config_client.call(srv)) +{ + ROS_INFO("send srv, srv.Request.in: %s", srv.request.in.c_str()); + ROS_INFO("receive srv, srv.Response.result: %s", srv.response.out.c_str()); +} +else +{ +ROS_ERROR("Failed to call service own_service"); +return 1; +} +*/ + return 0; +} \ No newline at end of file diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/lidar_obstacle_detection/srv/obstacle_detector_config.srv b/Devices/Packages/sick_line_guidance/turtlebotDemo/lidar_obstacle_detection/srv/obstacle_detector_config.srv new file mode 100755 index 0000000..15914c5 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/turtlebotDemo/lidar_obstacle_detection/srv/obstacle_detector_config.srv @@ -0,0 +1,6 @@ +float32 range_min +float32 range_max +int16 angle_min +int16 angle_max +float32 obstacle_stop_range +--- \ No newline at end of file diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/CMakeLists.txt b/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/CMakeLists.txt new file mode 100755 index 0000000..30c7ce5 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/CMakeLists.txt @@ -0,0 +1,284 @@ +cmake_minimum_required(VERSION 2.8.3) +project(sick_line_guidance_demo) + +## Compile as C++11, supported in ROS Kinetic and newer +add_compile_options(-std=c++17 -g -Wall -Wno-reorder -Wno-sign-compare -Wno-unused-local-typedefs -Wno-unused-parameter) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + cmake_modules + sick_line_guidance + message_generation + random_numbers + roscpp + rospy + sensor_msgs + std_msgs + nav_msgs + sm_timing +) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) +find_package(TinyXML REQUIRED) +find_package(OpenCV 3 REQUIRED) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a exec_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Measurement1.msg +# Measurement2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# sensor_msgs +# std_msgs +# nav_msgs +# sick_line_guidance +# ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +#catkin_package( +# INCLUDE_DIRS include +# LIBRARIES sick_line_guidance_demo_lib +# CATKIN_DEPENDS sick_line_guidance message_runtime random_numbers roscpp rospy sensor_msgs std_msgs nav_msgs +# DEPENDS TinyXML +#) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${TinyXML_INCLUDE_DIRS} + ${OpenCV_INCLUDE_DIRS} +) + +## Declare a C++ library +add_library(${PROJECT_NAME}_lib + src/adjust_heading.cpp + src/barcodes.cpp + src/explore_line_state.cpp + src/figure_eight_fsm.cpp + src/follow_line_state.cpp + src/image_util.cpp + src/navigation_mapper.cpp + src/navigation_util.cpp + src/ols_measurement_simulator.cpp + src/pid.cpp + src/robot_fsm.cpp + src/stop_go_fsm.cpp + src/turtlebot_test_fsm.cpp + src/velocity_ctr.cpp + src/wait_at_barcode_state.cpp +) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +## With catkin_make all packages are built within a single CMake context +## The recommended prefix ensures that target names across packages don't collide +add_executable(sick_line_guidance_demo_node src/sick_line_guidance_demo_node.cpp) +add_executable(sick_line_guidance_watchdog src/sick_line_guidance_watchdog.cpp) +add_executable(turtlebot_test_fsm_node src/turtlebot_test_fsm_node.cpp) + +## Rename C++ executable without prefix +## The above recommended prefix causes long target names, the following renames the +## target back to the shorter version for ease of user use +## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" +# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") + +## Add cmake target dependencies of the executable +## same as for the library above +add_dependencies(${PROJECT_NAME}_lib + sick_line_guidance_lib + random_numbers + ${${PROJECT_NAME}_EXPORTED_TARGETS} + ${catkin_EXPORTED_TARGETS} +) +add_dependencies(sick_line_guidance_demo_node + sick_line_guidance_lib + random_numbers + ${PROJECT_NAME}_lib + ${${PROJECT_NAME}_EXPORTED_TARGETS} + ${catkin_EXPORTED_TARGETS} +) +add_dependencies(sick_line_guidance_watchdog + sick_line_guidance_lib + ${PROJECT_NAME}_lib + ${${PROJECT_NAME}_EXPORTED_TARGETS} + ${catkin_EXPORTED_TARGETS} +) +add_dependencies(turtlebot_test_fsm_node + sick_line_guidance_lib + ${PROJECT_NAME}_lib + ${${PROJECT_NAME}_EXPORTED_TARGETS} + ${catkin_EXPORTED_TARGETS} +) + +## Specify libraries to link a library or executable target against +target_link_libraries(sick_line_guidance_demo_node + random_numbers + sick_line_guidance_lib + ${PROJECT_NAME}_lib + ${Boost_LIBRARIES} + ${catkin_LIBRARIES} + ${TinyXML_LIBRARIES} + ${OpenCV_LIBRARIES} + profiler +) +target_link_libraries(sick_line_guidance_watchdog + sick_line_guidance_lib + ${PROJECT_NAME}_lib + ${Boost_LIBRARIES} + ${catkin_LIBRARIES} + ${TinyXML_LIBRARIES} + ${OpenCV_LIBRARIES} + profiler +) +target_link_libraries(turtlebot_test_fsm_node + sick_line_guidance_lib + ${PROJECT_NAME}_lib + ${Boost_LIBRARIES} + ${catkin_LIBRARIES} + ${TinyXML_LIBRARIES} + ${OpenCV_LIBRARIES} + profiler +) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# install(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables and/or libraries for installation +install(TARGETS sick_line_guidance_demo_node sick_line_guidance_watchdog turtlebot_test_fsm_node ${PROJECT_NAME}_lib + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +## Mark cpp header files for installation +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE +) + +## Mark other files for installation (e.g. launch and bag files, etc.) +install(FILES + launch/Robot_FSM_demo.launch + launch/sick_line_guidance_demo_node.launch + launch/sick_line_guidance_watchdog.launch + launch/turtlebot_test_fsm_node.launch + maps/cam_intrinsic.xml + maps/demo_barcodes.xml + maps/demo_map_02.png + yaml/sick_line_guidance_demo.yaml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_sick_line_guidance.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/include/sick_line_guidance_demo/adjust_heading.h b/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/include/sick_line_guidance_demo/adjust_heading.h new file mode 100755 index 0000000..8ae5005 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/include/sick_line_guidance_demo/adjust_heading.h @@ -0,0 +1,283 @@ +/* + * AdjustHeading implements a state machine to adjust the robot heading, if the line distance increases over time. + * It searches the line orientation by minimizing the line distance and adjusts the robot heading. + * + * Algorithm: + * - Stop the robot, set linear velocity to 0.0 + * - Minimize the line distance by rotating the robot: + * + Start rotating clockwise + * + If the lined distance increases, rotate anti-clockwise + * + Stop rotation when the line distance is minimal. + * The robot is now orientated parallel to the line. + * - Reset PID and continue moving along the line. + * + * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim + * Copyright (C) 2019 SICK AG, Waldkirch + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of SICK AG nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Authors: + * Michael Lehning + * + * Copyright 2019 SICK AG + * Copyright 2019 Ing.-Buero Dr. Michael Lehning + * + */ +#ifndef __ROBOT_FSM_ADJUST_HEADING_H_INCLUDED +#define __ROBOT_FSM_ADJUST_HEADING_H_INCLUDED + +#include +#include "sick_line_guidance_demo/regression_1d.h" +#include "sick_line_guidance_demo/velocity_ctr.h" +#include "sick_line_guidance_demo/time_format.h" + +namespace sick_line_guidance_demo +{ + + /* + * struct AdjustHeadingConfig collects the configuration and parameter for class AdjustHeading: + */ + typedef struct AdjustHeadingConfigStruct + { + AdjustHeadingConfigStruct() : search_lower_bound_linedistance(0), search_upper_bound_linedistance(0), search_lower_bound_linewidth(0), search_upper_bound_linewidth(0), angular_z(0), + measurement_jitter(0), delta_angle_epsilon(0), sigma_linedistance(0), sigma_linewidth(0) {} + double search_lower_bound_linedistance; // lower limit for line distance: if the current line distance is below search_lower_bound_linedistance, the search will revert direction + double search_upper_bound_linedistance; // upper limit for line distance: if the current line distance is above search_upper_bound_linedistance, the search will revert direction + double search_lower_bound_linewidth; // lower limit for line width: if the current line width is below search_lower_bound_linewidth, the search will revert direction + double search_upper_bound_linewidth; // upper limit for line width: if the current line width is above search_upper_bound_linewidth, the search will revert direction + double search_max_yaw_angle_delta; // upper limit for yaw angle: if abs. difference between current yaw_angle and yaw angle at start, the search will revert direction + double angular_z; // velocity.angular.z under adjustment, default: 0.1 * M_PI / 4 + double measurement_jitter; // tolerate some line measurement jitter when adjusting the heading, default: 0.003 + double delta_angle_epsilon; // search stops, if the difference between current and desired yaw angle is smaller than delta_angle_epsilon, default: 0.1 * M_PI / 180 + double sigma_linedistance; // standard deviation of line distance, used to compute feature = sqrt((linedistance/sigma_linedistance)^2+(linewidth/sigma_linelinewidth)^2) + double sigma_linewidth; // standard deviation of line width, used to compute feature = sqrt((linedistance/sigma_linedistance)^2+(linewidth/sigma_linelinewidth)^2) + double max_state_duration; // test only: max time amount in seconds to stay in current search state; after seconds state increases. Prevents endless adjustment if TurtleBot is mounted fixed under test and odom always returns constant positions + + } AdjustHeadingConfig; + + /* + * class AdjustHeading implements a state machine to adjust the robot heading, if the line distance increases over time. + * It searches the line orientation by minimizing the line distance and adjusts the robot heading. + * Algorithm: + * - Stop the robot, set linear velocity to 0.0 + * - Minimize the line distance by rotating the robot: + * + Start rotating clockwise + * + If the lined distance increases, rotate anti-clockwise + * + Stop rotation when the line distance is minimal. + * The robot is now orientated parallel to the line. + * - Reset PID and continue moving along the line. + */ + class AdjustHeading + { + public: + + /* + * Constructor + */ + AdjustHeading(); + + /* + * Destructor + */ + virtual ~AdjustHeading(); + + /* + * Starts a new adjustment. + * @param[in] line_distance: current distance between robot and line + * @param[in] line_width: current line width + * @param[in] yaw_angle: current yaw angle from odometry + * @param[in] use_line_width: minimize line distance (false,default) or line width (true) + * @param[in] adjust_heading_cfg Parameter (upper and lower bounds) configuring the search + */ + virtual void start(double line_distance, double line_width, double yaw_angle, bool use_line_width, const AdjustHeadingConfig & adjust_heading_cfg); + + /* + * Stops a running adjustment. + */ + virtual void stop(void); + + /* + * Returns true, if an adjustment is currently running. + */ + virtual bool isRunning(void); + + /* + * Updates the current line distance and switches the state, if an upper bound or the minimum line distance has been reached: + * ADJUST_HEADING_STATE_STOPPED -> ADJUST_HEADING_STATE_SEARCH_FORWARD_1 -> ADJUST_HEADING_STATE_SEARCH_BACKWARD-2 -> ADJUST_HEADING_STATE_SEARCH_FORWARD_3 -> ADJUST_HEADING_STATE_STOPPED + * @param[in] line_detected: true if line detected (line_distance is valid), false if no line detected + * @param[in] line_distance: current distance between robot and line + * @param[in] line_width: current line width + * @param[in] yaw_angle: current yaw angle from odometry + * @return angular velocity (updated value, taken the current line_distance into account) + */ + virtual double update(bool line_detected, double line_distance, double line_width, double yaw_angle); + + /* + * Returns the lowest line dictance found by current search (if isRunning()==true) or by last search (if isRunning()==false) + */ + virtual double getBestLineDistance(void) { return m_line_distance_best; } + + /* + * Returns the lowest line width found by current search (if isRunning()==true) or by last search (if isRunning()==false) + */ + virtual double getBestLineWidth(void) { return m_line_width_best; } + + /* + * Returns the yaw angle at lowest line dictance resp. line width found by current search (if isRunning()==true) or by last search (if isRunning()==false) + */ + virtual double getBestYawAngle(void) { return m_yaw_angle_best; } + + /* + * Returns the difference of two angles in range -PI and +PI + * @param[in] angle1: first angle in range -PI and +PI + * @param[in] angle2: second angle in range -PI and +PI + * @return (angle2 - angle1) in range -PI and +PI + */ + static inline double deltaAngle(double angle1, double angle2) + { + double delta = angle2 - angle1; + while (delta < -M_PI) delta += 2 * M_PI; + while (delta > +M_PI) delta -= 2 * M_PI; + return delta; + } + + /* + * Returns the sign of a value, i.e. a shortcut for + * +1 if value >= epsilon, + * -1 if value <= -epsilon, or + * 0 otherwise + */ + static inline int signum(double value, double epsilon) + { + if(value >= epsilon) + return 1; + else if(value <= -epsilon) + return -1; + return 0; + } + + protected: + + /* + * ADJUST_HEADING_STATE_ENUM enumerates the state machine: + * ADJUST_HEADING_STATE_STOPPED -> ADJUST_HEADING_STATE_SEARCH_FORWARD_1 -> ADJUST_HEADING_STATE_SEARCH_BACKWARD-2 -> ADJUST_HEADING_STATE_SEARCH_FORWARD_3 -> ADJUST_HEADING_STATE_STOPPED + */ + typedef enum ADJUST_HEADING_STATE_ENUM + { + ADJUST_HEADING_STATE_STOPPED, // initial state + ADJUST_HEADING_STATE_SEARCH_FORWARD_1, // currently searching by rotating forward until upper bound reached (1. path after start) + ADJUST_HEADING_STATE_SEARCH_BACKWARD_2, // currently searching by rotating backward until upper bound or minimum reached (2. path after forward search) + ADJUST_HEADING_STATE_SEARCH_FORWARD_3, // currently searching by rotating forward until minimum reached (final path) + ADJUST_HEADING_MAX_STATES // number of states + + } ADJUST_HEADING_STATE; + + /* + * Returns true, if line distance or line width are above their upper bounds. In this case, search direction is inverted. Otherwise false is returned. + * @param[in] line_distance: current distance between robot and line + * @param[in] line_width: current line width + * @param[in] yaw_angle: current yaw angle from odometry (currently unused) + * @return true, if measurement (line distance or line width) out of configured bounds + */ + bool greaterThanUpperBounds(double line_distance, double line_width, double yaw_angle); + + /* + * Computes the feature value from a new measurement. + * @param[in] line_distance: current distance between robot and line + * @param[in] line_width: current line width + * @param[in] yaw_angle: current yaw angle from odometry (currently unused) + * @return feature value + */ + double computeFeature(double line_distance, double line_width, double yaw_angle); + + /* + * member data + */ + AdjustHeadingConfig m_adjust_heading_cfg; // Parameter (upper and lower bounds) configuring the search + ADJUST_HEADING_STATE m_adjust_heading_state; // simple state machine: ADJUST_HEADING_STATE_STOPPED -> ADJUST_HEADING_STATE_SEARCH_FORWARD_1 -> ADJUST_HEADING_STATE_SEARCH_BACKWARD-2 -> ADJUST_HEADING_STATE_SEARCH_FORWARD_3 -> ADJUST_HEADING_STATE_STOPPED + bool m_use_line_width; // minimize line distance (false,default) or line width (true) + double m_line_distance_at_start; // line distance at start of search + double m_line_width_at_start; // line width at start of search + double m_yaw_angle_at_start; // yaw angle at start of search + double m_line_distance_best; // lowest line distance found while searching + double m_line_width_best; // lowest line width found while searching + double m_feature_value_best; // lowest feature value found while searching + double m_yaw_angle_best; // yaw angle at lowest line distance + double m_final_step_delta_heading; // delta angle at final step (:= - m_yaw_angle_best) + int m_final_step_toggle_cnt; // counts possible toggling around the minimum line distance at the final optimization step + AngularZCtr m_wait_at_state_transition; // at state transitions: increase/decrease angular_z slowly until the desired yaw angle is reached, avoid acceleration and change in velocity + ros::Time m_states_start_time[ADJUST_HEADING_MAX_STATES]; // test only: limit time amount in seconds to stay in current search state; after seconds state increases. Prevents endless adjustment if TurtleBot is mounted fixed under test and odom always returns constant positions + + /* + * class AdjustHeadingAutoPrinter: Utility to print values of class AdjustHeading automatically in destructor. + */ + class AdjustHeadingAutoPrinter + { + public: + AdjustHeadingAutoPrinter(AdjustHeading* adjuster = 0, bool line_detected = 0, double line_distance = 0, double line_width = 0, double cur_dist = 0, double cur_yaw = 0, double* p_angular_z = 0) + : m_adjuster(adjuster), m_line_detected(line_detected), m_line_distance(line_distance), m_line_width(line_width), m_cur_dist(cur_dist), m_cur_yaw(cur_yaw), + m_angular_z(p_angular_z), m_prev_state(adjuster?(adjuster->m_adjust_heading_state):ADJUST_HEADING_STATE_STOPPED) {} + virtual ~AdjustHeadingAutoPrinter() + { + if(m_adjuster && (m_prev_state != ADJUST_HEADING_STATE_STOPPED || m_adjuster->m_adjust_heading_state != ADJUST_HEADING_STATE_STOPPED)) + { + std::string timestamp = sick_line_guidance_demo::TimeFormat::formatDateTime(); + ROS_INFO("%sADJUST HEADING: detected=%d, linedist=%.3lf, linewidth=%.3lf, startdist=%.3lf, startwidth=%.3lf, value=%.6lf, best=%.6lf, curyaw=%.4lf*PI, bestyaw=%.4lf*PI, angular.z=%.2lf*PI, WAITSTATE=%d, PREVSTATE=%d, NEXTSTATE=%d", + timestamp.c_str(), (int)m_line_detected, m_line_distance, m_line_width, m_adjuster->m_line_distance_at_start, m_adjuster->m_line_width_at_start, m_cur_dist, m_adjuster->m_feature_value_best, + m_cur_yaw/M_PI, m_adjuster->m_yaw_angle_best/M_PI, *m_angular_z/M_PI, (int)m_adjuster->m_wait_at_state_transition.isRunning(), (int)m_prev_state, (int)m_adjuster->m_adjust_heading_state); + } + } + protected: + AdjustHeading* m_adjuster; + ADJUST_HEADING_STATE m_prev_state; // previous state (m_adjuster->m_adjust_heading_state at constructor) + bool m_line_detected; // true: sensor detected line, line_distance and line_width valid + double m_line_distance; // current sensor line distance (line center point) + double m_line_width; // current sensor line width + double m_cur_dist; // current distance (line distance or line_width) + double m_cur_yaw; // current heading (yaw angle) + double* m_angular_z; // heading returned by AdjustHeading + }; + + }; // class AdjustHeading + +} // namespace sick_line_guidance_demo +#endif // __ROBOT_FSM_ADJUST_HEADING_H_INCLUDED diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/include/sick_line_guidance_demo/barcodes.h b/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/include/sick_line_guidance_demo/barcodes.h new file mode 100755 index 0000000..b509f36 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/include/sick_line_guidance_demo/barcodes.h @@ -0,0 +1,149 @@ +/* + * barcodes implements a container for barcodes for sick_line_guidance_demo (position and label). + * + * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim + * Copyright (C) 2019 SICK AG, Waldkirch + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of SICK AG nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Authors: + * Michael Lehning + * + * Copyright 2019 SICK AG + * Copyright 2019 Ing.-Buero Dr. Michael Lehning + * + */ +#ifndef __SICK_LINE_GUIDANCE_DEMO_BARCODES_H_INCLUDED +#define __SICK_LINE_GUIDANCE_DEMO_BARCODES_H_INCLUDED + +namespace sick_line_guidance_demo +{ + + /* + * class Barcode implements a container for barcode properties for sick_line_guidance_demo (position and label).. + */ + class Barcode + { + public: + + /* + * Constructor + */ + Barcode() : m_label(""), m_outer_rect_world(0,0,0,0), m_inner_rect_world(0,0,0,0), m_outer_rect_map(0,0,0,0), m_inner_rect_map(0,0,0,0), m_flipped(false){} + + /* + * Get/set barcode label, f.e. "0101" + */ + std::string & label(void) { return m_label; } + + /* + * Retuns the barcode label as number f.e. 101 + */ + size_t labelCode(void) { return m_label.empty() ? 0 : ((size_t)std::atol(m_label.c_str())); } + + /* + * Get/set barcode outer rect in world coordinates [meter] + */ + cv::Rect2d & outerRectWorld(void) { return m_outer_rect_world; } + + /* + * Get/set barcode inner rect (label area) in world coordinates [meter] + */ + cv::Rect2d & innerRectWorld(void) { return m_inner_rect_world; } + + /* + * Get/set barcode rect in image map coordinates [pixel] + */ + cv::Rect & outerRectMap(void) { return m_outer_rect_map; } + + /* + * Get/set barcode rect in image map coordinates [pixel] + */ + cv::Rect & innerRectMap(void) { return m_inner_rect_map; } + + /* + * Get/set barcode flipped (true or false) + */ + bool & flipped(void) { return m_flipped; } + + /* + * returns the barcode center (center of the outer rect) in world coordinates [meter] + */ + cv::Point2d centerWorld(void) { return cv::Point2d(m_outer_rect_world.x + m_outer_rect_world.width/2.0, m_outer_rect_world.y + m_outer_rect_world.height/2.0); } + + /* + * returns the barcode center (center of the outer rect) in map coordinates [pixel] + */ + cv::Point2d centerMap(void) { return cv::Point2d(m_outer_rect_map.x + m_outer_rect_map.width/2.0, m_outer_rect_map.y + m_outer_rect_map.height/2.0); } + + protected: + + /* + * member data + */ + std::string m_label; // barcode label, f.e. "0101" + cv::Rect2d m_outer_rect_world; // barcode outer rect in world coordinates [meter] + cv::Rect2d m_inner_rect_world; // barcode inner rect (label area) in world coordinates [meter] + cv::Rect m_outer_rect_map; // barcode outer rect in image map coordinates [pixel] + cv::Rect m_inner_rect_map; // barcode inner rect (label area) in image map coordinates [pixel] + bool m_flipped; // barcode flipped (true or false) + + }; // class Barcode + + /* + * class BarcodeUtil implements support tools for Barcodes (configuration from xml-file, etc.) + */ + class BarcodeUtil + { + public: + + /* + * reads a list of barcodes from xml-file + * @param[in] barcode_xml_file xml-file, f.e. "demo_barcodes.xml" + * @return list of barcodes (empty list, if xmlfile could not be read) + */ + static std::vector readBarcodeXmlfile(const std::string & barcode_xml_file); + }; + + } // namespace sick_line_guidance_demo +#endif // __SICK_LINE_GUIDANCE_DEMO_BARCODES_H_INCLUDED + diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/include/sick_line_guidance_demo/explore_line_state.h b/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/include/sick_line_guidance_demo/explore_line_state.h new file mode 100755 index 0000000..60b568a --- /dev/null +++ b/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/include/sick_line_guidance_demo/explore_line_state.h @@ -0,0 +1,124 @@ +/* + * ExploreLineState implements the state to explore a line for sick_line_guidance_demo. + * As long as ols does not detect a line, cmd_vel messages are published to search a line. + * Currently, the TurtleBot just moves straight forwared until a line is detected. + * Input: ols messages + * Output: cmd_vel messages + * + * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim + * Copyright (C) 2019 SICK AG, Waldkirch + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of SICK AG nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Authors: + * Michael Lehning + * + * Copyright 2019 SICK AG + * Copyright 2019 Ing.-Buero Dr. Michael Lehning + * + */ +#ifndef __SICK_LINE_GUIDANCE_DEMO_EXPLORE_LINE_STATE_H_INCLUDED +#define __SICK_LINE_GUIDANCE_DEMO_EXPLORE_LINE_STATE_H_INCLUDED + +#include +#include +#include "sick_line_guidance/OLS_Measurement.h" +#include "sick_line_guidance_demo/robot_fsm_context.h" + +namespace sick_line_guidance_demo +{ + /* + * ExploreLineState implements the state to explore a line for sick_line_guidance_demo. + * As long as ols does not detect a line, cmd_vel messages are published to search a line. + * Currently, the TurtleBot just moves straight forwared until a line is detected. + * Input: ols messages + * Output: cmd_vel messages + */ + class ExploreLineState + { + public: + + /* + * Constructor + * @param[in] nh ros handle + * @param[in] context shared fsm context + */ + ExploreLineState(ros::NodeHandle* nh=0, RobotFSMContext* context = 0); + + /* + * Destructor + */ + ~ExploreLineState(); + + /* + * Clears all internal states + */ + void clear(void); + + /* + * Runs the explore line state until line is detected (or a fatal error occures). + * @return FOLLOW_LINE in case of line detected, or EXIT in case ros::ok()==false. + */ + RobotFSMContext::RobotState run(void); + + protected: + + /* + * member data + */ + + RobotFSMContext* m_fsm_context; // shared state context + + /* + * Configuration + */ + + double m_exploreSpeed; // default linear velocity to explore a line + ros::Rate m_explore_line_rate; // frequency to update explort line state, default: 20 Hz + double m_ols_message_timeout; // timeout for ols messages: robot stops and waits, if last ols message was received more than seconds ago + double m_odom_message_timeout; // timeout for odom messages: robot stops and waits, if last ols message was received more than seconds ago + + }; // class ExploreLineState + +} // namespace sick_line_guidance_demo +#endif // __SICK_LINE_GUIDANCE_DEMO_EXPLORE_LINE_STATE_H_INCLUDED + diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/include/sick_line_guidance_demo/figure_eight_fsm.h b/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/include/sick_line_guidance_demo/figure_eight_fsm.h new file mode 100755 index 0000000..b2fa190 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/include/sick_line_guidance_demo/figure_eight_fsm.h @@ -0,0 +1,115 @@ +/* + * FigureEightVelocityFSM implements a simple state machine, creating cmd_vel messages + * to drive a TurtleBot in a figure of eight. + * + * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim + * Copyright (C) 2019 SICK AG, Waldkirch + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of SICK AG nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Authors: + * Michael Lehning + * + * Copyright 2019 SICK AG + * Copyright 2019 Ing.-Buero Dr. Michael Lehning + * + */ +#ifndef __SICK_LINE_GUIDANCE_DEMO_FIGURE_EIGHT_FSM_H_INCLUDED +#define __SICK_LINE_GUIDANCE_DEMO_FIGURE_EIGHT_FSM_H_INCLUDED + +#include + +namespace sick_line_guidance_demo +{ + /* + * class FigureEightVelocityFSM implements a simple state machine, creating cmd_vel messages + * to drive a TurtleBot in a figure of eight. + */ + class FigureEightVelocityFSM + { + public: + + /* + * Constructor + */ + FigureEightVelocityFSM(); + + /* + * Destructor + */ + ~FigureEightVelocityFSM(); + + /* + * Next cycle, internal state is updated, velocity message may switch to next movement + */ + void update(void); + + /* + * Returns the cmd_vel message for the current movement + */ + geometry_msgs::Twist getVelocity(void); + + protected: + + /* + * VelocityState := cmd_vel message and its duration + * class VelocityState is just a container for a cmd_vel message and its duration + */ + class VelocityState + { + public: + geometry_msgs::Twist cmd_vel; + ros::Duration duration; + }; + + /* + * member data + */ + int m_state_cnt; + ros::Time m_next_state_switch; + std::vector m_vec_vel_states; + + }; // class FigureEightVelocityFSM + +} // namespace sick_line_guidance_demo +#endif // __SICK_LINE_GUIDANCE_DEMO_FIGURE_EIGHT_FSM_H_INCLUDED + diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/include/sick_line_guidance_demo/follow_line_state.h b/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/include/sick_line_guidance_demo/follow_line_state.h new file mode 100755 index 0000000..f12b288 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/include/sick_line_guidance_demo/follow_line_state.h @@ -0,0 +1,137 @@ +/* + * FollowLineState implements the state to follow a line for sick_line_guidance_demo. + * As long as ols detects a line, cmd_vel messages are published to follow this line. + * Input: ols and odometry messages + * Output: cmd_vel messages + * + * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim + * Copyright (C) 2019 SICK AG, Waldkirch + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of SICK AG nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Authors: + * Michael Lehning + * + * Copyright 2019 SICK AG + * Copyright 2019 Ing.-Buero Dr. Michael Lehning + * + */ +#ifndef __SICK_LINE_GUIDANCE_DEMO_FOLLOW_LINE_STATE_H_INCLUDED +#define __SICK_LINE_GUIDANCE_DEMO_FOLLOW_LINE_STATE_H_INCLUDED + +#include +#include +#include "sick_line_guidance/OLS_Measurement.h" +#include "sick_line_guidance_demo/robot_fsm_context.h" + +namespace sick_line_guidance_demo +{ + /* + * class FollowLineState implements the state to follow a line for sick_line_guidance_demo. + * As long as ols detects a line, cmd_vel messages are published to follow this line. + * Input: ols and odometry messages + * Output: cmd_vel messages + */ + class FollowLineState + { + public: + + /* + * Constructor + * @param[in] nh ros handle + * @param[in] context shared fsm context + */ + FollowLineState(ros::NodeHandle* nh=0, RobotFSMContext* context = 0); + + /* + * Destructor + */ + ~FollowLineState(); + + /* + * Clears all internal states (pid etc.) + */ + void clear(void); + + /* + * Runs the follow line state until line is lost (or a fatal error occures). + * @return EXPLORE_LINE in case of line lost, or EXIT in case ros::ok()==false. + */ + RobotFSMContext::RobotState run(void); + + protected: + + /* + * member data + */ + + RobotFSMContext* m_fsm_context; // shared state context + ros::Time m_last_barcode_detected_time; // timestamp of last detected barcode + + /* + * Configuration + */ + + double m_pid_kp; // P parameter of PID control + double m_pid_ki; // I parameter of PID control + double m_pid_kd; // D parameter of PID control + double m_pid_setpoint; // setpoint parameter of PID control + double m_followSpeed; // default linear velocity to follow a line + ros::Rate m_followLineRate; // frequency to update follow line state, default: 20 Hz + double m_noLineTime ; // time in seconds before switching to state explore line because of lost line + double m_sensorLineWidth; // measured line width at 0 degree, 29 mm for an OLS mounted 65 mm over ground, 20 mm for an OLS mounted 100 mm over ground + double m_sensorLineMeasurementJitter; // tolerate some line measurement jitter when adjusting the heading + double m_adjustHeadingAngularZ; // velocity.angular.z to adjust robots heading + double m_adjustHeadingLcpDeviationThresh; // start to adjust heading, if the line distance increases over time (deviation of 1D-regression of line center points is above threshold lcpDeviationThresh) + double m_adjustHeadingLcpThresh; // start to adjust heading, if the line distance in meter (abs value) is above this threshold + double m_adjustHeadingDeltaAngleEpsilon; // search stops, if the difference between current and desired yaw angle is smaller than delta_angle_epsilon + double m_adjustHeadingMinDistanceToLastAdjust; // move at least some cm before doing next heading adjustment + double m_olsMessageTimeout; // timeout for ols messages: robot stops and waits, if last ols message was received more than seconds ago + double m_odomMessageTimeout; // timeout for odom messages: robot stops and waits, if last ols message was received more than seconds ago + double m_seconds_at_barcode; // time in seconds to wait at barcode + int m_ols_simu; // ols simulation (default: 0), test only + + }; // class FollowLineState + +} // namespace sick_line_guidance_demo +#endif // __SICK_LINE_GUIDANCE_DEMO_FOLLOW_LINE_STATE_H_INCLUDED + diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/include/sick_line_guidance_demo/image_util.h b/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/include/sick_line_guidance_demo/image_util.h new file mode 100755 index 0000000..aa81e72 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/include/sick_line_guidance_demo/image_util.h @@ -0,0 +1,241 @@ +/* + * ImageUtil implements utility functions for images and maps. + * + * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim + * Copyright (C) 2019 SICK AG, Waldkirch + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of SICK AG nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Authors: + * Michael Lehning + * + * Copyright 2019 SICK AG + * Copyright 2019 Ing.-Buero Dr. Michael Lehning + * + */ +#ifndef __SICK_LINE_GUIDANCE_DEMO_IMAGE_UTIL_H_INCLUDED +#define __SICK_LINE_GUIDANCE_DEMO_IMAGE_UTIL_H_INCLUDED + +#include + +namespace sick_line_guidance_demo +{ + /* + * class LineDetectionResult is a container for a line center point of a detected line, its width and its distance to the robot. + */ + class LineDetectionResult + { + public: + + /* + * LineDetectionResult constructor + */ + LineDetectionResult(const cv::Point2d & center_pos = cv::Point2d(0,0), const cv::Point2d & start_pos = cv::Point2d(0,0), const cv::Point2d & end_pos = cv::Point2d(0,0), double line_width = 0, double center_dist = 0) + : m_center_pos(center_pos), m_start_pos(start_pos), m_end_pos(end_pos), m_line_width(line_width), m_center_dist(center_dist) + { + } + + /* + * get/set line center position (x,y) + */ + cv::Point2d & centerPos(void) { return m_center_pos; } + const cv::Point2d & centerPos(void) const { return m_center_pos; } + + /* + * get/set line center position (x,y) + */ + cv::Point2d & startPos(void) { return m_start_pos; } + const cv::Point2d & startPos(void) const { return m_start_pos; } + + /* + * get/set line center position (x,y) + */ + cv::Point2d & endPos(void) { return m_end_pos; } + const cv::Point2d & endPos(void) const { return m_end_pos; } + + /* + * get/set line width + */ + double & lineWidth(void) { return m_line_width; } + const double & lineWidth(void) const { return m_line_width; } + + /* + * get/set distance to center position + */ + double & centerDistance(void) { return m_center_dist; } + const double & centerDistance(void) const { return m_center_dist; } + + protected: + + cv::Point2d m_center_pos; // line center position (x,y) + cv::Point2d m_start_pos; // line start position (x,y) + cv::Point2d m_end_pos; // line end position (x,y) + double m_line_width; // line width + double m_center_dist; // distance to center position + + }; + + /* + * class ImageUtil implements utility functions for images and maps. + */ + class ImageUtil + { + public: + + /* + * returns the pixel of a map at positions (posx, posy), or a default value (white, BGR=255,255,255) if (posx, posy) is outside the image (red := out of map). + * @param[in] map_img image (navigation map) + * @param[in] posx x-position in image map coordinates [pixel] + * @param[in] posy y-position in image map coordinates [pixel] + * @return pixel (cv::Vec3b) + */ + static inline cv::Vec3b getMapPixel(const cv::Mat & map_img, int posx, int posy, const cv::Vec3b & default_pixel = cv::Vec3b(255,255,255)) + { + if(posx >= 0 && posy >= 0 && posx < map_img.cols && posy < map_img.rows) + { + return map_img.at(posy,posx); + } + return default_pixel; // out of image map + } + + /* + * returns true, if two pixel (type cv::Vec3b) have identical values, or false otherwise. + * shortcut for (a[0] == b[0] && a[1] == b[1] && a[2] == b[2]) + * @param[in] a first pixel, to be compared with b + * @param[in] b seoncd pixel, to be compared with a + * @return true, if a[n] == b[n] for all n, otherwise false. + */ + static inline bool cmpPixel(const cv::Vec3b & a, const cv::Vec3b & b) + { + return (a[0] == b[0] && a[1] == b[1] && a[2] == b[2]); + } + + /* + * returns true, if the pixel at position (x,y) is on a line (i.e. a black pixel), or false otherwise. + * @param[in] map_img image (navigation map) + * @param[in] posx x-position in image map coordinates [pixel] + * @param[in] posy y-position in image map coordinates [pixel] + * @return true, if map at (x,y) is a black line pixel, or otherwise false. + */ + static inline bool isLinePixel(const cv::Mat & map_img, int posx, int posy) + { + return (cmpPixel(getMapPixel(map_img, posx, posy), cv::Vec3b(0,0,0))); + } + + /* + * returns true, if the pixel at position pos is on a line (i.e. a black pixel), or false otherwise. + * @param[in] map_img image (navigation map) + * @param[in] pos (x,y)-position in image map coordinates [pixel] + * @return true, if map at (pos.x,pos.y) is a black line pixel, or otherwise false. + */ + static inline bool isLinePixel(const cv::Mat & map_img, const cv::Point & pos) + { + return isLinePixel(map_img, pos.x, pos.y); + } + + /* + * computes and returns the position of a point with a distance and in direction from a given point . + * @param[in] base_pos start position in world coordinates [meter] + * @param[in] heading angle between given point and the returned point + * @param[in] radius distance of the returned point to + * @return position of a point with a distance and in direction from a given point in world coordinates [meter] + */ + static cv::Point2d getWorldPointInDirection(const cv::Point2d & base_pos, double heading, double radius); + + /* + * computes and returns the position of a point with a distance and in direction from a given point . + * @param[in] base_pos start position in map coordinates [pixel] + * @param[in] heading angle between given point and the returned point + * @param[in] radius distance of the returned point to + * @return position of a point with a distance and in direction from a given point in map coordinates [pixel] + */ + static cv::Point getMapPointInDirection(const cv::Point & base_pos, double heading, double radius); + + /* + * Detects and returns the line center points on a map, which can be seen + * by a robot at position moving in directions . + * Lines are detected +- 90 degree of . + * @param[in] map_img image (navigation map) + * @param[in] robot_map_pos robots position on the navigation map + * @param[in] robot_heading robots moving direction (i.e. robots yaw angle) + * @return list of line detection results (center points etc.) in map coordinates [pixel] + */ + static std::vector detectLineCenterPoints(const cv::Mat & map_img, const cv::Point & robot_map_pos, double robot_heading); + + /* + * Detects and returns the line center points on a map, in front of + * a robot at position moving in directions . + * Lines are detected in heading of . + * @param[in] map_img image (navigation map) + * @param[in] robot_map_pos robots position on the navigation map + * @param[in] robot_heading robots moving direction (i.e. robots yaw angle) + * @return list of line detection results (center points etc.) in map coordinates [pixel] + */ + static std::vector detectLineCenterInHeadingDirection(const cv::Mat & map_img, const cv::Point & robot_map_pos, double robot_heading); + + protected: + + /* + * computes and returns the max possible euclidean distance of a robot position to the corner points of an image. + * @param[in] robot_pos robots position on the navigation map [pixel] + * @param[in] dimx width of navigation map [pixel] + * @param[in] dimy height of navigation map [pixel] + * @return max possible distance [pixel] + */ + static double computeMaxDistanceToCorner(const cv::Point & robot_pos, int dimx, int dimy); + + /* + * detects and returns the line center points on a map, which can be seen + * by a robot at position moving in directions . + * @param[in] map_img image (navigation map) + * @param[in] robot_map_pos robots position on the navigation map + * @param[in] search_start_pos start point for line iteration + * @param[in] search_end_pos end point for line iteration + * @return list of line detection results (center points etc.) in map coordinates [pixel] + */ + static std::vector detectLineCenterPoints(const cv::Mat & map_img, const cv::Point & robot_map_pos, const cv::Point2d & search_start_pos, const cv::Point2d & search_end_pos); + + }; // class ImageUtil + +} // namespace sick_line_guidance_demo +#endif // __SICK_LINE_GUIDANCE_DEMO_IMAGE_UTIL_H_INCLUDED + diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/include/sick_line_guidance_demo/navigation_mapper.h b/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/include/sick_line_guidance_demo/navigation_mapper.h new file mode 100755 index 0000000..a5684b7 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/include/sick_line_guidance_demo/navigation_mapper.h @@ -0,0 +1,249 @@ +/* + * NavigationMapper transforms the robots xy-positions from world/meter into map/pixel position, + * detect lines and barcodes in the map plus their distance to the robot, + * and transforms them invers into world coordinates. + * + * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim + * Copyright (C) 2019 SICK AG, Waldkirch + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of SICK AG nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Authors: + * Michael Lehning + * + * Copyright 2019 SICK AG + * Copyright 2019 Ing.-Buero Dr. Michael Lehning + * + */ +#ifndef __SICK_LINE_GUIDANCE_DEMO_NAVIGATION_MAPPER_H_INCLUDED +#define __SICK_LINE_GUIDANCE_DEMO_NAVIGATION_MAPPER_H_INCLUDED + +#include +#include +#include +#include +#include +#include +#include "sick_line_guidance_demo/barcodes.h" +#include "sick_line_guidance_demo/image_util.h" +#include "sick_line_guidance_demo/ols_measurement_simulator.h" +#include "sick_line_guidance_demo/set_get.h" + +namespace sick_line_guidance_demo +{ + /* + * struct LineSensorConfig collects the line sensor configuration setting (sensor parameter and mounting settings): + * line_sensor_detection_width: default: 0.130, Width of the detection area of the line sensor (meter), 130 mm for sensor mounted 65 mm over ground + * line_sensor_scaling_dist: default: 180.0/133.0, Scaling between physical distance to the line center and measured line center point (measurement: lcp = 180 mm, physical: lcp = 133 mm), depending on mounted sensor height + * line_sensor_scaling_width: default: 29.0/20.0, Scaling between physical line width (20 mm) and measured line width (29 mm) depending on mounted sensor height (sensor mounted 100 mm over ground: scaling = 1, sensor mounted 65 mm over ground: scaling = 100/65 = 1.5) + * line_sensor_mounted_right_to_left: default: true, Sensor mounted from right to left (true, demo robot configuration) or otherwise from left to right + */ + typedef struct LineSensorConfigStruct + { + LineSensorConfigStruct() : line_sensor_detection_width(0.130), line_sensor_scaling_dist(180.0/133.0), line_sensor_scaling_width(29.0/20.0), line_sensor_mounted_right_to_left(true) {} + double line_sensor_detection_width; // default: 0.130, Width of the detection area of the line sensor (meter), 130 mm for sensor mounted 65 mm over ground + double line_sensor_scaling_dist; // default: 180.0 / 133.0, Scaling between physical distance to the line center and measured line center point (measurement: lcp = 180 mm, physical: lcp = 133 mm), depending on mounted sensor height + double line_sensor_scaling_width; // default: 29.0 / 20.0, Scaling between physical line width (20 mm) and measured line width (29 mm) depending on mounted sensor height (sensor mounted 100 mm over ground: scaling = 1, sensor mounted 65 mm over ground: scaling = 100/65 = 1.5) + bool line_sensor_mounted_right_to_left; // default: true, Sensor mounted from right to left (true, demo robot configuration) or otherwise from left to right + + } LineSensorConfig; + + /* + * class NavigationMapper transforms the robots xy-positions from world/meter into map/pixel position, + * detect lines and barcodes in the map plus their distance to the robot, + * and transforms them invers into world coordinates. + */ + class NavigationMapper + { + public: + + /* + * Constructor + * @param[in] map_imagefile navigation map, image file containing the map, f.e. "demo_map_02.png" + * @param[in] intrinsic_xmlfile xmlfile with intrinsic parameter to transform position from navigation map (pixel) to real world (meter) and vice versa, f.e. "cam_intrinsic.xml" with cx=cy=660, fx=fy=1 + * @param[in] barcode_xmlfile xmlfile with a list of barcodes with label and position, f.e. "demo_barcodes.xml" + * @param[in] ros_topic_output_ols_messages ROS topic for simulated OLS_Measurement messages, "/ols" (activated) in simulation and "" (deactivated) in demo application + * @param[in] sensor_config line sensor configuration setting (sensor parameter and mounting settings) + * @param[in] visualize==2: visualization+video enabled, map and navigation plots are displayed in a window, visualize==1: video created but not displayed, visualize==0: visualization and video disabled. + */ + NavigationMapper(ros::NodeHandle* nh=0, const std::string & map_imagefile = "", const std::string & intrinsic_xmlfile = "", const std::string & barcode_xmlfile = "", const std::string & ros_topic_output_ols_messages = "", + const LineSensorConfig & sensor_config = LineSensorConfig(), int visualize = 0); + + /* + * Destructor + */ + ~NavigationMapper(); + + /* + * message callback for odometry messages. This function is called automatically by the ros::Subscriber after subscription of topic "/odom". + * It transforms the robots xy-positions from world/meter into map/pixel position, detect lines and barcodes in the map plus their distance to the robot, + * and transform them invers into world coordinates. + * @param[in] msg odometry message (input) + */ + virtual void messageCallbackOdometry(const nav_msgs::Odometry::ConstPtr& msg); + + /* + * message callback for OLS measurement messages. This function is called automatically by the ros::Subscriber after subscription of topic "/ols". + * It displays the OLS measurement (line info and barcodes), if visualization is enabled. + * @param[in] msg OLS measurement message (input) + */ + virtual void messageCallbackOlsMeasurement(const boost::shared_ptr& msg); + + /* + * starts the message loop to handle ols and odometry messages received. + * Message handling runs in background thread started by this function. + */ + virtual void start(void); + + /* + * stops the message loop. + */ + virtual void stop(void); + + protected: + + /* + * transforms a xy-position in world coordinates [meter] to a xy-position in image map coordinates [pixel] + * @param[in] world_pos xy-position in world coordinates [meter] + * @param[out] map_pos xy-position in image map coordinates [pixel] + */ + virtual void transformPositionWorldToMap(const cv::Point2d & world_pos, cv::Point2d & map_pos); + + /* + * transforms a xy-position in world coordinates [meter] to a xy-position in image map coordinates [pixel] + * @param[in] world_pos xy-position in world coordinates [meter] + * @return xy-position in image map coordinates [pixel] + */ + virtual cv::Point transformPositionWorldToMap(const cv::Point2d & world_pos); + + /* + * transforms a xy-position in image map coordinates [pixel] to a xy-position in world coordinates [meter] + * @param[in] map_pos xy-position in image map coordinates [pixel] + * @return xy-position in world coordinates [meter] + */ + virtual cv::Point2d transformPositionMapToWorld(const cv::Point2d & map_pos); + + /* + * transforms a rectangle in world coordinates [meter] into a rectangle in image map coordinates [pixel] + * @param[in] world_rect rectangle in world coordinates [meter] + * @return rectangle in image map coordinates [pixel] + */ + virtual cv::Rect transformRectWorldToMap(const cv::Rect2d & world_rect); + + /* + * transforms an ols state from world coordinates [meter] to sensor units [meter], i.e. scales line distance (lcp) and line width + * from physical distances in the world to units of a sensor measurement; reverts function unscaleMeasurementToWorld(): + * line distances (lcp) are scaled by line_sensor_scaling_dist: default: 180.0/133.0, Scaling between physical distance to the line center and measured line center point + * (measurement: lcp = 180 mm, physical: lcp = 133 mm), depending on mounted sensor height + * line width are scaled by line_sensor_scaling_width: default: 29.0/20.0, Scaling between physical line width (20 mm) and measured line width (29 mm) depending on mounted sensor height + * (sensor mounted 100 mm over ground: scaling = 1, sensor mounted 65 mm over ground: scaling = 100/65 = 1.5) + * @param[in] ols_state ols state in world coordinates [meter] + * @return ols measurement in sensor units [meter] + */ + virtual sick_line_guidance::OLS_Measurement scaleWorldToMeasurement(const sick_line_guidance::OLS_Measurement & ols_state); + + /* + * transforms an ols measurement from sensor units [meter] to world coordinates [meter], i.e. scales line distance (lcp) and line width + * from sensor units to physical distances; reverts function scaleWorldToMeasurement(). + * line distances (lcp) are scaled by 1 / line_sensor_scaling_dist + * line width are scaled by 1 / line_sensor_scaling_width + * @param[in] ols measurement in sensor units [meter] + * @return ols_state ols state in world coordinates [meter] + */ + virtual sick_line_guidance::OLS_Measurement unscaleMeasurementToWorld(const sick_line_guidance::OLS_Measurement & ols_message); + + /* + * Runs the message loop to handle odometry and ols messages. + * It transforms the robots xy-positions from world/meter into map/pixel position, detect lines and barcodes in the map plus their distance to the robot, + * and transform them invers into world coordinates. + * @param[in] msg odometry message (input) + */ + void messageLoop(void); + + /* + * member data for navigation and mapping + */ + + ros::Rate m_message_rate; // frequency to handle ols and odom messages, default: 20 Hz + cv::Mat m_map_img; // world map from image file + cv::Mat m_intrinsics; // intrinsic parameter to transforms the robots xy-positions from world [meter] into xy-positions in the image map [pixel] + cv::Mat m_intrinsics_inv; // inverted intrinsic parameter to transforms xy-positions in the image map [pixel] into robots xy-positions in the world world [meter] + std::vector m_barcodes; // list of barcodes + LineSensorConfig m_sensor_config; // sensor configuration + OLS_Measurement_Simulator m_ols_measurement_simulator; // Simulator for OLS measurement messages, prediction of expected OLS measurement messages + sick_line_guidance_demo::SetGet m_ols_measurement_sensor; // OLS measurement message received from topic "/ols" (measurement in sensor units) + sick_line_guidance_demo::SetGet m_ols_measurement_world; // OLS measurement message received from topic "/ols" (measurement scaled to world coordinates) + sick_line_guidance_demo::SetGet m_odom_msg; // odometry message received from topic "/odom" + bool m_message_thread_run; // flag to start and stop m_message_thread + boost::thread* m_message_thread; // thread to run message loop + + typedef enum NAVIGATION_STATE_ENUM + { + INITIAL, // navigation: initial state + SEARCH_LINE, // navigation: search for a line (initially or whenever line lost) + FOLLOW_LINE // navigation: follow a line + } NAVIGATION_STATE; + NAVIGATION_STATE m_navigation_state; // enumerates the navigational state: search for or follow a line + + + /* + * member data for error simulation and testing + */ + + double m_error_simulation_burst_no_line_duration; // error simulation: duration of "no line detected" bursts in seconds, default: 0.0 (disabled) + double m_error_simulation_burst_no_line_frequency; // error simulation: frequency of "no line detected" bursts in 1/seconds, default: 0.0 (disabled) + ros::Time m_error_simulation_no_line_start; + ros::Time m_error_simulation_no_line_end; + + /* + * member data for debugging and visualization + */ + + int m_visualize; // visualize==2: visualization+video enabled, map and navigation plots are displayed in a window, visualize==1: video created but not displayed, visualize==0: visualization and video disabled. + cv::Mat m_window_img; // image to plot and visualize, if m_visualize is true + std::string m_window_name; // named window for visualization, if m_visualize is true + cv::VideoWriter m_video_write; // output video writer + + }; // class NavigationMapper + +} // namespace sick_line_guidance_demo +#endif // __SICK_LINE_GUIDANCE_DEMO_NAVIGATION_MAPPER_H_INCLUDED diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/include/sick_line_guidance_demo/navigation_util.h b/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/include/sick_line_guidance_demo/navigation_util.h new file mode 100755 index 0000000..018d5ba --- /dev/null +++ b/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/include/sick_line_guidance_demo/navigation_util.h @@ -0,0 +1,234 @@ +/* + * NavigationUtil implements utility functions for 2D/3D-transforms and navigation. + * + * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim + * Copyright (C) 2019 SICK AG, Waldkirch + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of SICK AG nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Authors: + * Michael Lehning + * + * Copyright 2019 SICK AG + * Copyright 2019 Ing.-Buero Dr. Michael Lehning + * + */ +#ifndef __SICK_LINE_GUIDANCE_DEMO_NAVIGATION_UTIL_H_INCLUDED +#define __SICK_LINE_GUIDANCE_DEMO_NAVIGATION_UTIL_H_INCLUDED + +#include +#include +#include +#include "sick_line_guidance/OLS_Measurement.h" +#include "sick_line_guidance_demo/image_util.h" + +namespace sick_line_guidance_demo +{ + + /* + * class NavigationUtil implements utility functions for 2D/3D-transforms and navigation. + */ + class NavigationUtil + { + public: + + /* + * Returns the difference of two angles in range -PI and +PI + * @param[in] angle1: first angle in range -PI and +PI + * @param[in] angle2: second angle in range -PI and +PI + * @return (angle2 - angle1) in range -PI and +PI + */ + static inline double deltaAngle(double angle1, double angle2) + { + return unwrapAngle(angle2 - angle1); + } + + /* + * Unwraps an angle to range -PI and +PI + * @return angle in range -PI and +PI + */ + static inline double unwrapAngle(double angle) + { + while (angle < -M_PI) angle += 2 * M_PI; + while (angle > +M_PI) angle -= 2 * M_PI; + return angle; + } + + /* + * computes and return the euclidean distance between two points, + * shortcut for sqrt((a.x - b.x) * (a.x - b.x) + (a.y - b.y) * (a.y - b.y)) + * @param[in] a first point + * @param[in] b second point + * @return euclidean distance between a and b + */ + static inline double euclideanDistance(const cv::Point2d & a, const cv::Point2d & b) + { + double dx = a.x - b.x; + double dy = a.y - b.y; + return sqrt(dx * dx + dy * dy); + } + + /* + * computes and return the euclidean distance between between a robot position and a line center point, + * i.e. a negative value if the line_center is left from robots position when moving in robot_heading, and + * a positive value if the line_center is right from robots position when moving in robot_heading + * @param[in] robot_pos first point (robots position) + * @param[in] line_center_pos second point (line center position) + * @param[in] robot_heading robots moving direction (i.e. robots yaw angle) + * @param[in] sensor_mounted_right_to_left Sensor mounted from right to left (true, demo robot configuration) or otherwise from left to right + * @return euclidean distance between a and b + */ + static double euclideanDistanceOrientated(const cv::Point2d & robot_pos, const cv::Point2d & line_center_pos, double robot_heading, bool sensor_mounted_right_to_left); + + /* + * transforms and returns (x,y) position and yaw angle of an odometry message into a readable string. + * @param[in] msg odometry message (input) + * @return (x,y) position and yaw angle of odometry message as a readable string (output) + */ + static std::string toInfo(const nav_msgs::Odometry::ConstPtr& msg); + + /* + * transforms and returns (x,y) position and yaw angle of an odometry message into a readable string. + * @param[in] msg odometry message (input) + * @return (x,y) position and yaw angle of odometry message as a readable string (output) + */ + static std::string toInfo(const nav_msgs::Odometry & msg); + + /* + * transforms and returns a gazebo ModelStates message into a readable string. + * @param[in] msg gazebo ModelStates message (input) + * @param[in] start_idx index to start iteration of names and poses (input, default = 0: print all names and poses) + * @param[in] last_idx index to end iteration of names and poses (input, default = MAX_INT: print all names and poses) + * @return gazebo ModelStates message as a readable string (output) + */ + static std::string toInfo(const gazebo_msgs::ModelStates::ConstPtr& msg, size_t start_idx = 0, size_t last_idx = INT_MAX); + + /* + * gets the robots xy-position and yaw angle from an odometry message + * @param[in] msg odometry message (input) + * @param[out] world_posx x-position in world coordinates [meter] + * @param[out] world_posy y-position in world coordinates [meter] + * @param[out] yaw_angle orientation (z-axis rotation) in rad + */ + static void toWorldPosition(const nav_msgs::Odometry & msg, double & world_posx, double & world_posy, double & yaw_angle); + /* + * gets the robots xy-position and yaw angle from an odometry message + * @param[in] msg odometry message (input) + * @param[out] world_posx x-position in world coordinates [meter] + * @param[out] world_posy y-position in world coordinates [meter] + * @param[out] yaw_angle orientation (z-axis rotation) in rad + */ + static void toWorldPosition(const nav_msgs::Odometry::ConstPtr& msg, double & world_posx, double & world_posy, double & yaw_angle); + + /* + * gets the robots xy-position and yaw angle from a gazebo ModelStates message + * @param[in] msg gazebo ModelStates message (input) + * @param[out] world_posx x-position in world coordinates [meter] + * @param[out] world_posy y-position in world coordinates [meter] + * @param[out] yaw_angle orientation (z-axis rotation) in rad + */ + static void toWorldPosition(const gazebo_msgs::ModelStates::ConstPtr& msg, double & world_posx, double & world_posy, double & yaw_angle); + + /* + * transforms an orientation from Quaternion to angles: roll (x-axis rotation), pitch (y-axis rotation), yaw (z-axis rotation). + * @param[in] quat_msg gazebo orientation (quaternion, input) + * @param[out] roll angle (x-axis rotation, output) + * @param[out] pitch angle (y-axis rotation, output) + * @param[out] yaw angle (z-axis rotation, output) + */ + static void transformOrientationToRollPitchYaw(const geometry_msgs::Quaternion & quat_msg, double & roll, double & pitch, double & yaw); + + /* + * returns +1, if a point is on the right side of the robot, and -1 otherwise. + * see https://math.stackexchange.com/questions/274712/calculate-on-which-side-of-a-straight-line-is-a-given-point-located + * for the math under the hood. + * @param[in] xpoint x-position of the point + * @param[in] ypoint y-position of the point + * @param[in] xrobot x-position of the robot + * @param[in] yrobot y-position of the robot + * @param[in] heading heading of the robot + * @return +1 (point is on the right side) or -1 (point is on the left side) + */ + static int isPointRightFromRobot(double xpoint, double ypoint, double xrobot, double yrobot, double heading); + + /* + * Selects line points within the sensor detection zone from a list of possible line center points + * @param[in] world_line_points list of possible line center points in world coordinates [meter] + * @param[in] robot_world_pos robot position in world coordinates [meter] + * @param[in] line_sensor_detection_width width of sensor detection zone (f.e. 0.180 for an OLS mounted in 0.1m height over ground, 0.130 for the demo system with OLS mounted in 0.065m height) + * @param[in] max_line_cnt max. number of lines (OLS: max. 3 lines) + * @return list of line points within the sensor detection zone + */ + static std::vector selectLinePointsWithinDetectionZone(std::vector & world_line_points, const cv::Point2d & robot_world_pos, double line_sensor_detection_width, size_t max_line_cnt = 3); + + /* + * returns true, if a line is detected by ols measurement (bit 0, 1 or 2 of status is set), or false otherwise. + */ + static bool lineDetected(const sick_line_guidance::OLS_Measurement & ols_msg) + { + return ((ols_msg.status) & 0x7) != 0; // bit 0, 1 or 2 of status is set -> line detected + } + + /* + * returns true, if a barcode is detected by ols measurement (bit 7 of status is set), or false otherwise. + */ + static bool barcodeDetected(const sick_line_guidance::OLS_Measurement & ols_msg) + { + return (((ols_msg.status) & 0x80) != 0); // bit 7 of status is set -> barcode valid + } + + /* + * returns the barcode, if a barcode is detected by ols measurement, or 0 otherwise. + */ + static uint32_t barcode(const sick_line_guidance::OLS_Measurement & ols_msg) + { + if(barcodeDetected(ols_msg)) + return ols_msg.barcode < 255 ? ols_msg.barcode : ols_msg.extended_code; + return 0; + } + + + }; // class NavigationUtil + +} // namespace sick_line_guidance_demo +#endif // __SICK_LINE_GUIDANCE_DEMO_NAVIGATION_UTIL_H_INCLUDED + diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/include/sick_line_guidance_demo/ols_measurement_simulator.h b/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/include/sick_line_guidance_demo/ols_measurement_simulator.h new file mode 100755 index 0000000..5159c03 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/include/sick_line_guidance_demo/ols_measurement_simulator.h @@ -0,0 +1,171 @@ +/* + * ols_measurement_simulator simulates OLS_Measurement messages for sick_line_guidance_demo. + * + * OLS_Measurement_Simulator converts the distance between the simulated robot and + * the optical lines from the navigation map into OLS_Measurement messages. + * + * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim + * Copyright (C) 2019 SICK AG, Waldkirch + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of SICK AG nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Authors: + * Michael Lehning + * + * Copyright 2019 SICK AG + * Copyright 2019 Ing.-Buero Dr. Michael Lehning + * + */ +#ifndef __SICK_LINE_GUIDANCE_DEMO_OLS_MEASUREMENT_SIMULATOR_H_INCLUDED +#define __SICK_LINE_GUIDANCE_DEMO_OLS_MEASUREMENT_SIMULATOR_H_INCLUDED + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "sick_line_guidance/OLS_Measurement.h" + +namespace sick_line_guidance_demo +{ + + /* + * class OLS_Measurement_Simulator converts the distance between the simulated robot and + * the optical lines from the navigation map into OLS_Measurement messages. + */ + class OLS_Measurement_Simulator + { + public: + + /* + * Constructor + * @param[in] ros_topic_ols_messages ros topic for publishing OLS messages (empty: deactivated) + * @param[in] publish_rate rate to publish OLS measurements (if activated, default: 100) + */ + OLS_Measurement_Simulator(ros::NodeHandle* nh=0, const std::string & ros_topic_ols_messages = "", double publish_rate = 100); + + /* + * Destructor + */ + virtual ~OLS_Measurement_Simulator(); + + /* + * Get the current OLS state + */ + virtual sick_line_guidance::OLS_Measurement GetState(void) + { + boost::lock_guard state_lockguard(m_ols_state_mutex); + return m_ols_state; + } + + /* + * Set the current OLS state + */ + virtual void SetState(const sick_line_guidance::OLS_Measurement & ols_state) + { + boost::lock_guard state_lockguard(m_ols_state_mutex); + m_ols_state = ols_state; + } + + /* + * Returns the ros topic for publishing ols messages, configured in the constructor. + * If not running a simulation (the default), this topic is empty (no ols messages published) + */ + std::string getPublishTopic(void){ return m_ros_topic_ols_messages;} + + /* + * publish the current OLS state + */ + void publish(void); + + /* + * publish the current OLS state in background task (publish with fixed rate) + */ + virtual void schedulePublish(void); + + /* + * Initializes ols_state for one line (position, width and status) + */ + static void setLine(sick_line_guidance::OLS_Measurement & ols_state, float position, float width); + + /* + * Initializes ols_state for detected lines (position, width and status for 0, 1, 2 or 3 lines) + */ + static void setLines(sick_line_guidance::OLS_Measurement & ols_state, std::vector & line_points); + + /* + * Sets the barcode of an ols_state + */ + static void setBarcode(sick_line_guidance::OLS_Measurement & ols_state, size_t label, bool flipped); + + /* + * rounds a double value to a given float precision, f.e. roundPrecision(lcp, 0.001) to round a line center point to millimeter precision. + */ + static float roundPrecision(double value, double precision); + + protected: + + /* + * thread callback, just publishes the current OLS state with a const rate + */ + void runPublishThread(void); + + /* + * member data + */ + + std::string m_ros_topic_ols_messages; // ros topic for publishing OLS messages (empty: deactivated) + ros::Publisher m_ols_publisher; // publisher for ols measurement messages on topic "/ols" + boost::thread* m_ols_publish_thread; // thread to publish ols measurement messages + bool m_publish_scheduled; // if true, m_ols_publish_thread will publish m_ols_state + ros::Rate m_ols_publish_rate; // rate to publish the current OLS state (default: 100) + sick_line_guidance::OLS_Measurement m_ols_state; // the current OLS state + boost::mutex m_ols_state_mutex; // lock guard for access to m_ols_state + + }; // class OLS_Measurement_Simulator + +} // namespace sick_line_guidance_demo +#endif // __SICK_LINE_GUIDANCE_DEMO_OLS_MEASUREMENT_SIMULATOR_H_INCLUDED diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/include/sick_line_guidance_demo/pid.h b/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/include/sick_line_guidance_demo/pid.h new file mode 100755 index 0000000..3f7c937 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/include/sick_line_guidance_demo/pid.h @@ -0,0 +1,32 @@ +#ifndef PID_H +#define PID_H + +// using namespace std; + +class PID_Controller +{ + public: + + PID_Controller(double dt, double kp, double ki, double kd); //Contstructor + + double calculate_output(double setpoint_value, double actual_value); + void setParams(double dt, double kp, double ki, double kd); + void getParams(double& dt, double& kp, double& ki, double& kd); + void reset(); + void clear_state(); + ~PID_Controller(); //Destructor + + private: + double _kp; // Proportional Factor + double _ki; // Integrate Factor + double _kd; // Derivation Factor + double _dt; // Sample Time + double _e = 0; // controller deviation + double _e_int = 0; // controller integrator + double _e_diff = 0; // controller derivator + double _e_z = 0; // e * z⁻1 + double _yp = 0; // P-Controller Output + double _yi = 0; // I-Controller Output + double _yd = 0; // D-Controller Output +}; +#endif \ No newline at end of file diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/include/sick_line_guidance_demo/regression_1d.h b/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/include/sick_line_guidance_demo/regression_1d.h new file mode 100755 index 0000000..5f612a4 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/include/sick_line_guidance_demo/regression_1d.h @@ -0,0 +1,176 @@ +/* + * regression_1d estimates a one-dimensional regresssion (value over time). + * + * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim + * Copyright (C) 2019 SICK AG, Waldkirch + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of SICK AG nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Authors: + * Michael Lehning + * + * Copyright 2019 SICK AG + * Copyright 2019 Ing.-Buero Dr. Michael Lehning + * + */ +#ifndef __SICK_LINE_GUIDANCE_DEMO_REGRESSION_1D_H_INCLUDED +#define __SICK_LINE_GUIDANCE_DEMO_REGRESSION_1D_H_INCLUDED + +namespace sick_line_guidance_demo +{ + + /* + * class Regression1D implements a one-dimensional regression over a fixed number of data points. + */ + class Regression1D + { + public: + + /* + * Constructor + * @param[in] n: number of data points + */ + Regression1D(int n = 0) : m_solved(false), m_data_cnt(0) + { + if(n > 0) + { + m_left_mat = cv::Mat(n, 2, CV_64F); + m_right_mat = cv::Mat(n, 1, CV_64F); + m_solved_mat = cv::Mat(2, 1, CV_64F); + clear(); + } + } + + /* + * Updates a value over time and performs a new regression. + */ + inline void update(double value, double time = ros::Time::now().toSec()) + { + if(!m_left_mat.empty() && !m_right_mat.empty() && !m_solved_mat.empty()) + { + assert(m_left_mat.cols == 2); + assert(m_right_mat.cols == 1); + assert(m_solved_mat.cols == 1); + assert(m_solved_mat.rows == 2); + assert(m_left_mat.rows == m_right_mat.rows); + // Move all rows one up + for(size_t y = 1; y < m_left_mat.rows; y++) + m_left_mat.at(y - 1, 0) = m_left_mat.at(y,0); + for(size_t y = 1; y < m_right_mat.rows; y++) + m_right_mat.at(y - 1, 0) = m_right_mat.at(y,0); + // Copy the new data to the last row + m_left_mat.at(m_left_mat.rows - 1, 0) = time; + m_right_mat.at(m_right_mat.rows - 1, 0) = value; + // Compute regression by solving m_left_mat * m_solved_mat = m_right_mat, + // where m_solved_mat contains the regression coefficients + m_data_cnt++; + if(m_data_cnt >= m_left_mat.rows && m_data_cnt >= m_right_mat.rows) + { + cv::Mat left_work; + cv::Mat right_work; + m_left_mat.copyTo(left_work); + m_right_mat.copyTo(right_work); + m_solved = cv::solve(left_work, right_work, m_solved_mat, cv::DECOMP_SVD); + } + } + } + + /* + * Returns offset and deviation of the regression line. + */ + inline bool getRegression(double & offset, double & deviation) + { + if(m_solved && !m_solved_mat.empty()) + { + assert(m_solved_mat.cols == 1); + assert(m_solved_mat.rows == 2); + offset = m_solved_mat.at(1,0); + deviation = m_solved_mat.at(0,0); + } + else + { + offset = 0; + deviation = 0; + } + return m_solved; + } + + /* + * Returns the deviation part of the regression. + */ + inline double getDeviation(void) + { + return (m_solved && !m_solved_mat.empty()) ? (m_solved_mat.at(0,0)) : 0; + } + + /* + * Clears all values. + */ + inline void clear(void) + { + m_solved = false; + m_data_cnt = 0; + m_left_mat = 0; + m_right_mat = 0; + m_solved_mat = 0; + for(size_t y = 0; y < m_left_mat.rows; y++) + { + m_left_mat.at(y,1) = 1; + } + } + + protected: + + /* + * member data + */ + bool m_solved; + size_t m_data_cnt; + cv::Mat m_left_mat; + cv::Mat m_right_mat; + cv::Mat m_solved_mat; + + }; // class Regression1D + +} // namespace sick_line_guidance_demo +#endif // __SICK_LINE_GUIDANCE_DEMO_REGRESSION_1D_H_INCLUDED + diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/include/sick_line_guidance_demo/robot_fsm.h b/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/include/sick_line_guidance_demo/robot_fsm.h new file mode 100755 index 0000000..c0b6cd8 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/include/sick_line_guidance_demo/robot_fsm.h @@ -0,0 +1,156 @@ +/* + * RobotFSM implements the state machine to explore and follow a line + * for sick_line_guidance_demo. + * The following RobotStates are executed: + * INITIAL -> EXPLORE_LINE -> FOLLOW_LINE [ -> WAIT_AT_BARCODE -> FOLLOW_LINE ] -> EXIT + * Theses states are implemented in the following classes: + * EXPLORE_LINE: ExploreLineState + * FOLLOW_LINE: FollowLineState + * WAIT_AT_BARCODE: WaitAtBarcodeState + * + * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim + * Copyright (C) 2019 SICK AG, Waldkirch + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of SICK AG nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Authors: + * Michael Lehning + * + * Copyright 2019 SICK AG + * Copyright 2019 Ing.-Buero Dr. Michael Lehning + * + */ +#ifndef __SICK_LINE_GUIDANCE_DEMO_ROBOT_FSM_H_INCLUDED +#define __SICK_LINE_GUIDANCE_DEMO_ROBOT_FSM_H_INCLUDED + +#include +#include +#include "sick_line_guidance/OLS_Measurement.h" +#include "sick_line_guidance_demo/explore_line_state.h" +#include "sick_line_guidance_demo/follow_line_state.h" +#include "sick_line_guidance_demo/robot_fsm_context.h" +#include "sick_line_guidance_demo/wait_at_barcode_state.h" + +namespace sick_line_guidance_demo +{ + /* + * class RobotFSM implements the state machine to explore and follow a line + * for sick_line_guidance_demo. + * Input: ols and odometry messages + * Output: cmd_vel messages + */ + class RobotFSM + { + public: + + /* + * Constructor + * @param[in] nh ros handle + * @param[in] ros_topic_ols_messages ROS topic for OLS_Measurement messages (input) + * @param[in] ros_topic_odometry ROS topic for odometry incl. robot positions (input) + * @param[in] ros_topic_cmd_vel ROS topic for cmd_vel messages (output) + */ + RobotFSM(ros::NodeHandle* nh=0, const std::string & ros_topic_ols_messages = "/ols", const std::string & ros_topic_odometry = "/odom", const std::string & ros_topic_cmd_vel = "/cmd_vel"); + + /* + * Destructor + */ + ~RobotFSM(); + + /* + * Start thread to run the final state machine. Read messages form ols and odom topics, publish messages to cmd_vel + */ + void startFSM(void); + + /* + * Stops the thread to run the final state machine + */ + void stopFSM(void); + + /* + * message callback for odometry messages. This function is called automatically by the ros::Subscriber after subscription of topic "/odom". + * It transforms the robots xy-positions from world/meter into map/pixel position, detect lines and barcodes in the map plus their distance to the robot, + * and transform them invers into world coordinates. + * @param[in] msg odometry message (input) + */ + virtual void messageCallbackOdometry(const nav_msgs::Odometry::ConstPtr& msg); + + /* + * message callback for OLS measurement messages. This function is called automatically by the ros::Subscriber after subscription of topic "/ols". + * It displays the OLS measurement (line info and barcodes), if visualization is enabled. + * @param[in] msg OLS measurement message (input) + */ + virtual void messageCallbackOlsMeasurement(const boost::shared_ptr& msg); + + protected: + + /* + * thread callback, runs the final state machine for sick_line_guidance_demo. + * Input: ols and odometry messages + * Output: cmd_vel messages + */ + void runFSMthread(void); + + /* + * member data: context and subscriber + */ + + RobotFSMContext m_fsm_context; // shared context + ros::Subscriber m_ols_subscriber; // ros subscriber for ols messages (fsm input) + ros::Subscriber m_odom_subscriber; // ros subscriber for odom messages (fsm input) + ros::Publisher m_cmd_vel_publisher; // ros publisher for cmd_vel messages (fsm output) + + /* + * member data: states and thread to run the state engine + */ + + boost::thread* m_fsm_thread; // thread to run the state machine + bool m_fsm_thread_run; // true: m_fsm_thread is currently running, false: m_fsm_thread stopping + sick_line_guidance_demo::ExploreLineState m_explore_line; // state to explore a line in state RobotState::EXPLORE_LINE + sick_line_guidance_demo::FollowLineState m_follow_line; // state to follow a line in state RobotState::FOLLOW_LINE + sick_line_guidance_demo::WaitAtBarcodeState wait_at_barcode_state; // state to wait at a barcode in state RobotState::WAIT_AT_BARCODE + + }; // class RobotFSM + +} // namespace sick_line_guidance_demo +#endif // __SICK_LINE_GUIDANCE_DEMO_ROBOT_FSM_H_INCLUDED + diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/include/sick_line_guidance_demo/robot_fsm_context.h b/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/include/sick_line_guidance_demo/robot_fsm_context.h new file mode 100755 index 0000000..809b564 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/include/sick_line_guidance_demo/robot_fsm_context.h @@ -0,0 +1,262 @@ +/* + * RobotFSMContext implements a threadsafe context of RobotFSM + * incl. ols, odom, and fsm state. + * + * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim + * Copyright (C) 2019 SICK AG, Waldkirch + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of SICK AG nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Authors: + * Michael Lehning + * + * Copyright 2019 SICK AG + * Copyright 2019 Ing.-Buero Dr. Michael Lehning + * + */ +#ifndef __SICK_LINE_GUIDANCE_DEMO_ROBOT_FSM_CONTEXT_H_INCLUDED +#define __SICK_LINE_GUIDANCE_DEMO_ROBOT_FSM_CONTEXT_H_INCLUDED + +#include "sick_line_guidance/OLS_Measurement.h" +#include "sick_line_guidance_demo/navigation_util.h" +#include "sick_line_guidance_demo/set_get.h" + +namespace sick_line_guidance_demo +{ + + /* + * Container for (x,y) position and yaw angle + */ + class RobotPosition + { + public: + RobotPosition(double x = 0, double y = 0, double a = 0) : pos(x,y), yaw(a) {} + cv::Point2d pos; + double yaw; + }; + + /* + * Threadsafe context of robot_fsm: ols, odom and fsm state + */ + class RobotFSMContext + { + public: + + /* + * Enumerates the states of robot FSM: INITIAL -> EXPLORE_LINE -> FOLLOW_LINE [ -> WAIT_AT_BARCODE -> FOLLOW_LINE ] -> EXIT + */ + typedef enum RobotStateEnum + { + INITIAL, + EXPLORE_LINE, + FOLLOW_LINE, + WAIT_AT_BARCODE, + FATAL_ERROR, + EXIT + + } RobotState; + + /* + * Constructor + */ + RobotFSMContext() : m_velocity_publisher(0) + { + m_follow_left_turnout.set(false); + m_cur_barcode.set(0); + m_last_barcode.set(0); + } + + /* + * set the current ols measurement + */ + void setOlsState(const sick_line_guidance::OLS_Measurement::ConstPtr& msg) + { + if(msg) + { + m_ols_msg.set(*msg); + setCurBarcode(sick_line_guidance_demo::NavigationUtil::barcode(*msg)); + } + } + + /* + * returns the current ols measurement + */ + sick_line_guidance::OLS_Measurement getOlsState(void) + { + return m_ols_msg.get(); + } + + /* + * returns the timestamp of current ols measurement + */ + ros::Time getOlsStateTime(void) + { + return m_ols_msg.getTime(); + } + + /* + * set the current odometry measurement + */ + void setOdomState(const nav_msgs::Odometry::ConstPtr& msg) + { + if(msg) + { + double posx=0, posy=0, yaw = 0; + sick_line_guidance_demo::NavigationUtil::toWorldPosition(msg, posx, posy, yaw); + RobotPosition odom_position(posx, posy, yaw); + setOdomPosition(odom_position); + } + } + + /* + * set the current robot position from odometry measurement + */ + void setOdomPosition(const RobotPosition & odom_position) + { + m_odom_position.set(odom_position); + } + + /* + * returns the current robot position from odometry + */ + RobotPosition getOdomPosition(void) + { + return m_odom_position.get(); + } + + /* + * returns the timestamp of current odometry measurement + */ + ros::Time getOdomPositionTime(void) + { + return m_odom_position.getTime(); + } + + /* + * set follow_left_turnout flag, true: follow a turnout on the left side, flag is toggled at barcode 101 + */ + void setFollowLeftTurnout(bool follow_left_turnout) + { + m_follow_left_turnout.set(follow_left_turnout); + } + + /* + * returns the current follow_left_turnout flag, true: follow a turnout on the left side, flag is toggled at barcode 101 + */ + bool getFollowLeftTurnout(void) + { + return m_follow_left_turnout.get(); + } + + /* + * set the label of current barcode (0: no barcode detected) + */ + void setCurBarcode(uint32_t cur_barcode) + { + if(cur_barcode != 0 && cur_barcode != getLastBarcode()) + m_last_barcode.set(cur_barcode); + m_cur_barcode.set(cur_barcode); + } + + /* + * returns the label of current barcode (0: no barcode detected) + */ + uint32_t getCurBarcode(void) + { + return m_cur_barcode.get(); + } + + /* + * returns the label of last barcode (0: no barcode detected) + */ + uint32_t getLastBarcode(void) + { + return m_last_barcode.get(); + } + + /* + * returns true, if the last ols messages has been received more than seconds ago. + */ + bool hasOlsMessageTimeout(double timeout_sec) + { + ros::Time ols_time = getOlsStateTime(); + return ols_time.isValid() && ((ros::Time::now() - ols_time).toSec() > timeout_sec); + } + + /* + * returns true, if the last odom messages has been received more than seconds ago. + */ + bool hasOdomMessageTimeout(double timeout_sec) + { + ros::Time odom_time = getOdomPositionTime(); + return odom_time.isValid() && ((ros::Time::now() - odom_time).toSec() > timeout_sec); + } + + /* + * publishes a cmd_vel message + */ + void publish(geometry_msgs::Twist & velocityMessage) + { + if(m_velocity_publisher) + m_velocity_publisher->publish(velocityMessage); + } + + void setVelocityPublisher(ros::Publisher* publisher) + { + m_velocity_publisher = publisher; + } + + + protected: + + sick_line_guidance_demo::SetGet m_ols_msg; // ols measurement message + sick_line_guidance_demo::SetGet m_odom_position; // robot position from odometry + sick_line_guidance_demo::SetGet m_follow_left_turnout; // true: follow a turnout on the left side, flag is toggled at barcode 101 + sick_line_guidance_demo::SetGet m_cur_barcode; // label of current barcode (0: no barcode detected) + sick_line_guidance_demo::SetGet m_last_barcode; // label of last detected barcode (0: no barcode detected) + ros::Publisher* m_velocity_publisher; + + }; + +} // namespace sick_line_guidance_demo +#endif // __SICK_LINE_GUIDANCE_DEMO_ROBOT_FSM_CONTEXT_H_INCLUDED + diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/include/sick_line_guidance_demo/set_get.h b/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/include/sick_line_guidance_demo/set_get.h new file mode 100755 index 0000000..b1b0878 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/include/sick_line_guidance_demo/set_get.h @@ -0,0 +1,116 @@ +/* + * set_get implements an utility template class to set and get values threadsafe. + * + * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim + * Copyright (C) 2019 SICK AG, Waldkirch + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of SICK AG nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Authors: + * Michael Lehning + * + * Copyright 2019 SICK AG + * Copyright 2019 Ing.-Buero Dr. Michael Lehning + * + */ +#ifndef __SICK_LINE_GUIDANCE_DEMO_SET_GET_H_INCLUDED +#define __SICK_LINE_GUIDANCE_DEMO_SET_GET_H_INCLUDED + +namespace sick_line_guidance_demo +{ + + /* + * class SetGet implements an utility template to set and get values threadsafe. + */ + template class SetGet + { + public: + + /* + * Constructor + */ + SetGet() : m_timestamp(0){} + + /* + * Constructor + */ + SetGet(const T & value) { set(value); } + + /* + * Set a value + */ + void set(const T & value) + { + boost::lock_guard state_lockguard(m_mutex); + m_value = value; + m_timestamp = ros::Time::now(); + } + + /* + * Get a value + */ + T get(void) + { + boost::lock_guard state_lockguard(m_mutex); + return m_value; + } + + /* + * Returns the timestamp (i.e. the time a value has been set) + */ + ros::Time getTime(void) + { + boost::lock_guard state_lockguard(m_mutex); + return m_timestamp; + } + + protected: + + T m_value; + boost::mutex m_mutex; + ros::Time m_timestamp; + + }; // class SetGet + +} // namespace sick_line_guidance_demo +#endif // __SICK_LINE_GUIDANCE_DEMO_SET_GET_H_INCLUDED + diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/include/sick_line_guidance_demo/stop_go_fsm.h b/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/include/sick_line_guidance_demo/stop_go_fsm.h new file mode 100755 index 0000000..cc8e81b --- /dev/null +++ b/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/include/sick_line_guidance_demo/stop_go_fsm.h @@ -0,0 +1,115 @@ +/* + * StopGoVelocityFSM implements a simple state machine, creating cmd_vel messages + * to drive a TurtleBot in stop and go. + * + * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim + * Copyright (C) 2019 SICK AG, Waldkirch + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of SICK AG nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Authors: + * Michael Lehning + * + * Copyright 2019 SICK AG + * Copyright 2019 Ing.-Buero Dr. Michael Lehning + * + */ +#ifndef __SICK_LINE_GUIDANCE_DEMO_STOP_GO_FSM_H_INCLUDED +#define __SICK_LINE_GUIDANCE_DEMO_STOP_GO_FSM_H_INCLUDED + +#include + +namespace sick_line_guidance_demo +{ + /* + * class StopGoVelocityFSM implements a simple state machine, creating cmd_vel messages + * to drive a TurtleBot in stop and go. + */ + class StopGoVelocityFSM + { + public: + + /* + * Constructor + */ + StopGoVelocityFSM(); + + /* + * Destructor + */ + ~StopGoVelocityFSM(); + + /* + * Next cycle, internal state is updated, velocity message may switch to next movement + */ + void update(void); + + /* + * Returns the cmd_vel message for the current movement + */ + geometry_msgs::Twist getVelocity(void); + + protected: + + /* + * VelocityState := cmd_vel message and its duration + * class VelocityState is just a container for a cmd_vel message and its duration + */ + class VelocityState + { + public: + geometry_msgs::Twist cmd_vel; + ros::Duration duration; + }; + + /* + * member data + */ + int m_state_cnt; + ros::Time m_next_state_switch; + std::vector m_vec_vel_states; + + }; // class StopGoVelocityFSM + +} // namespace sick_line_guidance_demo +#endif // __SICK_LINE_GUIDANCE_DEMO_STOP_GO_FSM_H_INCLUDED + diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/include/sick_line_guidance_demo/time_format.h b/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/include/sick_line_guidance_demo/time_format.h new file mode 100755 index 0000000..315e67f --- /dev/null +++ b/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/include/sick_line_guidance_demo/time_format.h @@ -0,0 +1,105 @@ +/* + * time_format implements utility function to print the current time in different time formats. + * Note: By default, ROS_INFO, ROS_DEBUG and ROS_ERROR prints the walltime like '1563182704.466086322', + * while rosspy uses time format like '2019-07-15 12:23:24,859'. + * For easier debugging, TimeFormat prints both timestamp formats. + * + * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim + * Copyright (C) 2019 SICK AG, Waldkirch + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of SICK AG nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Authors: + * Michael Lehning + * + * Copyright 2019 SICK AG + * Copyright 2019 Ing.-Buero Dr. Michael Lehning + * + */ +#ifndef __SICK_LINE_GUIDANCE_DEMO_TIME_FORMAT_H_INCLUDED +#define __SICK_LINE_GUIDANCE_DEMO_TIME_FORMAT_H_INCLUDED + +#include "boost/date_time/posix_time/posix_time.hpp" + + +namespace sick_line_guidance_demo +{ + + /* + * class TimeFormat + */ + class TimeFormat + { + public: + + /* + * Returns current date and time in a format like "[2019-07-15 12:23:24,859123] " + */ + static std::string formatDateTime(void) + { + boost::posix_time::time_facet* facet = new boost::posix_time::time_facet("%Y-%m-%d %H:%M:%S,%f"); + time_t rawtime; + time(&rawtime); + ros::WallTime local_walltime = ros::WallTime::now() + ros::WallDuration(localtime(&rawtime)->tm_gmtoff); + std::stringstream timestamp; + timestamp.imbue(std::locale(timestamp.getloc(), facet)); + timestamp << "[" << local_walltime.toBoost() << "] "; + return timestamp.str(); + } + + /* + * Returns current date and time in a format like "[2019-07-15 12:23:24,859123] " + */ + static std::string formatDateTime(const ros::Time & time) + { + boost::posix_time::time_facet* facet = new boost::posix_time::time_facet("%Y-%m-%d %H:%M:%S,%f"); + std::stringstream timestamp; + timestamp.imbue(std::locale(timestamp.getloc(), facet)); + timestamp << time.toBoost(); + return timestamp.str(); + } + + }; // class TimeFormat + +} // namespace sick_line_guidance_demo +#endif // __SICK_LINE_GUIDANCE_DEMO_TIME_FORMAT_H_INCLUDED + diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/include/sick_line_guidance_demo/turtlebot_test_fsm.h b/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/include/sick_line_guidance_demo/turtlebot_test_fsm.h new file mode 100755 index 0000000..d161966 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/include/sick_line_guidance_demo/turtlebot_test_fsm.h @@ -0,0 +1,143 @@ +/* + * TurtlebotTestFSM implements a state machine to generate cmd_vel messages + * with varying velocities to test the motor control of a TurtleBot. + * + * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim + * Copyright (C) 2019 SICK AG, Waldkirch + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of SICK AG nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Authors: + * Michael Lehning + * + * Copyright 2019 SICK AG + * Copyright 2019 Ing.-Buero Dr. Michael Lehning + * + */ +#ifndef __SICK_LINE_GUIDANCE_DEMO_TURTLEBOT_TEST_FSM_H_INCLUDED +#define __SICK_LINE_GUIDANCE_DEMO_TURTLEBOT_TEST_FSM_H_INCLUDED + +#include +#include +#include "sick_line_guidance/OLS_Measurement.h" + +namespace sick_line_guidance_demo +{ + /* + * class TurtlebotTestFSM implements a state machine to generate cmd_vel messages + * with varying velocities to test the motor control of a TurtleBot. + * Input: ols and odometry messages + * Output: cmd_vel messages + */ + class TurtlebotTestFSM + { + public: + + /* + * Constructor + * @param[in] nh ros handle + * @param[in] ros_topic_ols_messages ROS topic for OLS_Measurement messages (input) + * @param[in] ros_topic_odometry ROS topic for odometry incl. robot positions (input) + * @param[in] ros_topic_cmd_vel ROS topic for cmd_vel messages (output) + * @param[in] print_errors true (default): print high latency by error message (print by info message otherwise) + */ + TurtlebotTestFSM(ros::NodeHandle* nh=0, const std::string & ros_topic_ols_messages = "/ols", const std::string & ros_topic_odometry = "/odom", const std::string & ros_topic_cmd_vel = "/cmd_vel", bool print_errors = true); + + /* + * Destructor + */ + ~TurtlebotTestFSM(); + + /* + * Start thread to run the final state machine. Read messages form ols and odom topics, publish messages to cmd_vel + */ + void startFSM(void); + + /* + * Stops the thread to run the final state machine + */ + void stopFSM(void); + + /* + * message callback for odometry messages. This function is called automatically by the ros::Subscriber after subscription of topic "/odom". + * It compares odom messages with the current velocity from last cmd_vel command and measures latencies to stop the Turtlebot after moving + * with linear and angular velocitiy. + * @param[in] msg odometry message (input) + */ + virtual void messageCallbackOdometry(const nav_msgs::Odometry::ConstPtr& msg); + + /* + * message callback for OLS measurement messages, empty function + */ + virtual void messageCallbackOlsMeasurement(const boost::shared_ptr& msg); + + protected: + + /* + * thread callback, runs the final state machine. + * Input: ols and odometry messages + * Output: cmd_vel messages + */ + void runFSMthread(void); + + /* + * member data + */ + + ros::Subscriber m_ols_subscriber; // ros subscriber for ols messages (fsm input) + ros::Subscriber m_odom_subscriber; // ros subscriber for odom messages (fsm input) + ros::Publisher m_cmd_vel_publisher; // ros publisher for cmd_vel messages (fsm output) + ros::Rate m_cmd_vel_publish_rate; // cmd_vel messages are published with 20 Hz by default + boost::thread* m_fsm_thread; // thread to run the state machine + bool m_fsm_thread_run; // true: m_fsm_thread is currently running, false: m_fsm_thread stopping + nav_msgs::Odometry m_cur_odom; // last received odom message + ros::Time m_cur_odom_timestamp; // time of last received odom message + geometry_msgs::Twist m_cur_velocity; // last published cmd_vel message + ros::Time m_cur_velocity_timestamp; // time of last published cmd_vel message + ros::Time m_timestamp_stopped; // time of last stop + bool m_print_errors; // true (default): print high latency by error message (print by info message otherwise) + + }; // class TurtlebotTestFSM + +} // namespace sick_line_guidance_demo +#endif // __SICK_LINE_GUIDANCE_DEMO_TURTLEBOT_TEST_FSM_H_INCLUDED + diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/include/sick_line_guidance_demo/velocity_ctr.h b/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/include/sick_line_guidance_demo/velocity_ctr.h new file mode 100755 index 0000000..d37c826 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/include/sick_line_guidance_demo/velocity_ctr.h @@ -0,0 +1,134 @@ +/* + * velocity_ctr controls angular velocity to avoid acceleration and changing velocity. + * velocity_ctr increases/decreases velocity.angular.z smooth and slowly until a desired yaw angle is reached. + * + * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim + * Copyright (C) 2019 SICK AG, Waldkirch + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of SICK AG nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Authors: + * Michael Lehning + * + * Copyright 2019 SICK AG + * Copyright 2019 Ing.-Buero Dr. Michael Lehning + * + */ +#ifndef __ROBOT_FSM_VELOCITY_CTR_H_INCLUDED +#define __ROBOT_FSM_VELOCITY_CTR_H_INCLUDED + +namespace sick_line_guidance_demo +{ + + /* + * class AngularZCtr controls angular velocity to avoid acceleration and changing velocity. + * It increases/decreases velocity.angular.z smooth and slowly until a desired yaw angle is reached. + */ + class AngularZCtr + { + public: + + /* + * Constructor + */ + AngularZCtr(); + + /* + * Destructor + */ + virtual ~AngularZCtr(); + + /* + * Starts a new control cycle with smooth transition from cur_yaw_angle and velocity.angular.z = 0 to + * dest_yaw_angle and dest_vel_angular_z. + * @param[in] cur_yaw_angle: current yaw angle (robots current heading) + * @param[in] dest_yaw_angle: destination yaw angle (robots desired heading) + * @param[in] dest_vel_angular_z: destination velocity.angular.z + * @param[in] line_detected: if true (default), start with 5 cycles velocity.angular.z=0 before increasing velocity.angular.z to dest_vel_angular_z, otherwise there's only one cycle with velocity.angular.z=0 + * @return first value of velocity.angular.z after start, default: 0 + */ + virtual double start(double cur_yaw_angle, double dest_yaw_angle, double dest_vel_angular_z, bool line_detected = true); + + /* + * Stops angular.z control, if currently running. + */ + virtual void stop(); + + /* + * Update velocity.angular.z control with the current yaw angle. + * @param[in] cur_yaw_angle: current yaw angle (robots current heading) + */ + virtual void update(double cur_yaw_angle); + + /* + * Returns true, if AngularZCtr is currently running, i.e. velocity.angular.z is slowly increased/decreased until + * dest_yaw_angle (destination yaw angle, robots heading) and dest_vel_angular_z (destination velocity.angular.z) + * is reached. + */ + virtual bool isRunning(void); + + /* + * Returns true, if AngularZCtr is currently running, i.e. velocity.angular.z is slowly increased/decreased until + * dest_yaw_angle (destination yaw angle, robots heading) and dest_vel_angular_z (destination velocity.angular.z) + * is reached. In this case, output parameter angular_z is set to the new velocity.angular.z to be published, + * and true is returned. Otherwise, false is returned and angular_z remains unchanged. + * @param[out] angular_z: velocity.angular.z to be published (if true returned), otherwise left untouched. + */ + virtual bool isRunning(double & angular_z); + + protected: + + double m_dest_yaw_angle; // robots desired heading + double m_dest_vel_angular_z; // desired velocity.angular.z (max. value to increase/decrease m_cur_vel_angular_z) + double m_cur_yaw_angle; // robots current heading + double m_cur_vel_angular_z; // current velocity.angular.z (increased resp. decreased from 0.0 to m_dest_vel_angular_z) + double m_start_yaw_angle; // robots heading at start time + ros::Time m_start_time; // timestamp at start + int m_update_cnt; // just counts the number of updates (cycles) required to reach the destination heading, or -1 if not started + int m_number_start_cyles_with_zero_angular_z; // number of start cycles velocity.angular.z=0 before increasing velocity.angular.z to dest_vel_angular_z (default: 5) + double m_delta_angular_z; // increase/decrease velocity.angular.z by m_delta_angular_z until delta_angle_now is smaller than some epsilon (default: 0.05 * M_PI / 4) + + }; // class AngularZCtr + +} // namespace sick_line_guidance_demo +#endif // __ROBOT_FSM_VELOCITY_CTR_H_INCLUDED + diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/include/sick_line_guidance_demo/wait_at_barcode_state.h b/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/include/sick_line_guidance_demo/wait_at_barcode_state.h new file mode 100755 index 0000000..164d280 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/include/sick_line_guidance_demo/wait_at_barcode_state.h @@ -0,0 +1,113 @@ +/* + * WaitAtBarcodeState implements the state to wait at a barcode for a configurable amount of time. + * + * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim + * Copyright (C) 2019 SICK AG, Waldkirch + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of SICK AG nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Authors: + * Michael Lehning + * + * Copyright 2019 SICK AG + * Copyright 2019 Ing.-Buero Dr. Michael Lehning + * + */ +#ifndef __SICK_LINE_GUIDANCE_DEMO_WAIT_AT_BARCODE_STATE_H_INCLUDED +#define __SICK_LINE_GUIDANCE_DEMO_WAIT_AT_BARCODE_STATE_H_INCLUDED + +#include +#include +#include "sick_line_guidance_demo/robot_fsm_context.h" + +namespace sick_line_guidance_demo +{ + /* + * WaitAtBarcodeState implements the state to wait at a barcode for a configurable amount of time. + */ + class WaitAtBarcodeState + { + public: + + /* + * Constructor + * @param[in] nh ros handle + * @param[in] context shared fsm context + */ + WaitAtBarcodeState(ros::NodeHandle* nh=0, RobotFSMContext* context = 0); + + /* + * Destructor + */ + ~WaitAtBarcodeState(); + + /* + * Clears all internal states + */ + void clear(void); + + /* + * Runs the wait at barcode state for a configurable amount of time. + * Toggles follow_left_turnout flag at barcode label 101. + * @return FOLLOW_LINE, or EXIT in case ros::ok()==false. + */ + RobotFSMContext::RobotState run(void); + + protected: + + /* + * member data + */ + + RobotFSMContext* m_fsm_context; // shared state context + + /* + * Configuration + */ + + double m_stop_wait_time; // time in seconds to stop at a barcode, default: 10 seconds + + }; // class WaitAtBarcodeState + +} // namespace sick_line_guidance_demo +#endif // __SICK_LINE_GUIDANCE_DEMO_WAIT_AT_BARCODE_STATE_H_INCLUDED + diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/launch/Robot_FSM_demo.launch b/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/launch/Robot_FSM_demo.launch new file mode 100755 index 0000000..509c585 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/launch/Robot_FSM_demo.launch @@ -0,0 +1,11 @@ + + + + + + + + + diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/launch/sick_line_guidance_demo_node.launch b/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/launch/sick_line_guidance_demo_node.launch new file mode 100755 index 0000000..cee1710 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/launch/sick_line_guidance_demo_node.launch @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/launch/sick_line_guidance_watchdog.launch b/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/launch/sick_line_guidance_watchdog.launch new file mode 100755 index 0000000..8f23568 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/launch/sick_line_guidance_watchdog.launch @@ -0,0 +1,13 @@ + + + + + + + + + + + + + diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/launch/turtlebot_test_fsm_node.launch b/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/launch/turtlebot_test_fsm_node.launch new file mode 100755 index 0000000..c816d48 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/launch/turtlebot_test_fsm_node.launch @@ -0,0 +1,6 @@ + + + + + + diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/maps/cam_intrinsic.xml b/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/maps/cam_intrinsic.xml new file mode 100755 index 0000000..331af4e --- /dev/null +++ b/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/maps/cam_intrinsic.xml @@ -0,0 +1,13 @@ + + + + 3 + 3 +
d
+ + 1000. 0. 600. + 0. -1000. 600. + 0. 0. 1. + + + diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/maps/demo_barcodes.xml b/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/maps/demo_barcodes.xml new file mode 100755 index 0000000..de1cfd0 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/maps/demo_barcodes.xml @@ -0,0 +1,7 @@ + + + + + + + diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/maps/demo_map_02.png b/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/maps/demo_map_02.png new file mode 100755 index 0000000..9d7d803 Binary files /dev/null and b/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/maps/demo_map_02.png differ diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/package.xml b/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/package.xml new file mode 100755 index 0000000..4b15775 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/package.xml @@ -0,0 +1,81 @@ + + + sick_line_guidance_demo + 0.0.0 + SICK Line Guidance ROS Demo + + + rostest + + + + + Apache + + + + + + https://github.com/SICKAG/sick_line_guidance + https://github.com/SICKAG/sick_line_guidance_demo + https://www.sick.com/ols + https://www.sick.com/mls + + + + Michael Lehning + + + + + + + + + + + + + + + + + + + + + + catkin + cmake_modules + roscpp + rospy + std_msgs + nav_msgs + sensor_msgs + message_generation + random_numbers + sick_line_guidance + sm_timing + roscpp + rospy + std_msgs + nav_msgs + sensor_msgs + message_generation + random_numbers + sick_line_guidance + sm_timing + roscpp + rospy + std_msgs + nav_msgs + message_runtime + random_numbers + sick_line_guidance + sm_timing + + + + + + diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/src/adjust_heading.cpp b/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/src/adjust_heading.cpp new file mode 100755 index 0000000..cd5ce83 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/src/adjust_heading.cpp @@ -0,0 +1,315 @@ +/* + * AdjustHeading implements a state machine to adjust the robot heading, if the line distance increases over time. + * It searches the line orientation by minimizing the line distance and adjusts the robot heading. + * + * Algorithm: + * - Stop the robot, set linear velocity to 0.0 + * - Minimize the line distance by rotating the robot: + * + Start rotating clockwise + * + If the lined distance increases, rotate anti-clockwise + * + Stop rotation when the line distance is minimal. + * The robot is now orientated parallel to the line. + * - Reset PID and continue moving along the line. + * + * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim + * Copyright (C) 2019 SICK AG, Waldkirch + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of SICK AG nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Authors: + * Michael Lehning + * + * Copyright 2019 SICK AG + * Copyright 2019 Ing.-Buero Dr. Michael Lehning + * + */ +#include +#include +#include +#include "sick_line_guidance_demo/adjust_heading.h" + +/* + * class AdjustHeading implements a state machine to adjust the robot heading, if the line distance increases over time. + * It searches the line orientation by minimizing the line distance and adjusts the robot heading. + * Algorithm: + * - Stop the robot, set linear velocity to 0.0 + * - Minimize the line distance by rotating the robot: + * + Start rotating clockwise + * + If the lined distance increases, rotate anti-clockwise + * + Stop rotation when the line distance is minimal. + * The robot is now orientated parallel to the line. + * - Reset PID and continue moving along the line. + */ + +/* + * Constructor + */ +sick_line_guidance_demo::AdjustHeading::AdjustHeading() +: m_adjust_heading_state(ADJUST_HEADING_STATE_STOPPED), m_use_line_width(false), m_line_distance_at_start(0), m_line_width_at_start(0), + m_line_distance_best(0), m_line_width_best(0), m_feature_value_best(0), m_yaw_angle_best(0), m_final_step_delta_heading(0), m_final_step_toggle_cnt(0) +{ +} + +/* + * Destructor + */ +sick_line_guidance_demo::AdjustHeading::~AdjustHeading() +{ +} + +/* + * Starts a new adjustment. + * @param[in] line_distance: current distance between robot and line + * @param[in] yaw_angle: current yaw angle from odometry + * @param[in] use_line_width: minimize line distance (false,default) or line width (true) + * @param[in] search_lower_bound: if the current line distance is below search_lower_bound, the search will stop + * @param[in] search_upper_bound: if the current line distance is above search_upper_bound, the search will continue in opposite direction (max. detection zone: +- 65 mm) + * @param[in] angular_z: velocity.angular.z under adjustment, default: 0.1 * M_PI / 4 + * @param[in] measurement_jitter: tolerate some line measurement jitter when adjusting the heading, default: 0.003 + * @param[in] lcp_deviation_thresh: stop heading adjustment, if the line distance finally increases again (lcp-deviation above lcp_deviation_thresh) + */ + +/* + * Starts a new adjustment. + * @param[in] line_distance: current distance between robot and line + * @param[in] line_width: current line width + * @param[in] yaw_angle: current yaw angle from odometry + * @param[in] use_line_width: minimize line distance (false,default) or line width (true) + * @param[in] adjust_heading_cfg Parameter (upper and lower bounds) configuring the search + */ +void sick_line_guidance_demo::AdjustHeading::start(double line_distance, double line_width, double yaw_angle, bool use_line_width, const AdjustHeadingConfig & adjust_heading_cfg) +{ + m_adjust_heading_cfg = adjust_heading_cfg; // Parameter (upper and lower bounds) configuring the search + m_use_line_width = use_line_width; // minimize line distance (false,default) or line width (true) + m_adjust_heading_state = ADJUST_HEADING_STATE_SEARCH_FORWARD_1; // forward search until upper bound reached (1. path) + m_final_step_delta_heading = 0; // delta angle at final step (:= - m_yaw_angle_best) + m_final_step_toggle_cnt = 0; // counts possible toggling around the minimum line distance at the final optimization step + m_yaw_angle_best = yaw_angle; // yaw angle at lowest line distance + m_line_distance_at_start = line_distance; // line distance at start of search + m_line_width_at_start = line_width; // line width at start of search + m_yaw_angle_at_start = yaw_angle; // yaw angle at start of search + m_line_distance_best = line_distance; // lowest line distance found while searching + m_line_width_best = line_width; // lowest line width found while searching + m_feature_value_best = computeFeature(line_distance, line_width, yaw_angle); // lowest feature value found while searching + m_wait_at_state_transition.start(yaw_angle, yaw_angle, 0); + for(size_t n = 0; n < ADJUST_HEADING_MAX_STATES; n++) + m_states_start_time[n] = ros::Time(0); + m_states_start_time[m_adjust_heading_state] = ros::Time::now(); // test only: limit time amount in seconds to stay in current search state; after seconds state increases. Prevents endless adjustment if TurtleBot is mounted fixed under test and odom always returns constant positions +} + +/* + * Stops a running adjustment. + */ +void sick_line_guidance_demo::AdjustHeading::stop(void) +{ + m_adjust_heading_state = ADJUST_HEADING_STATE_STOPPED; +} + +/* + * Returns true, if an adjustment is currently running. + */ +bool sick_line_guidance_demo::AdjustHeading::isRunning(void) +{ + return (m_adjust_heading_state != ADJUST_HEADING_STATE_STOPPED) || m_wait_at_state_transition.isRunning(); +} + + +/* + * Returns true, if line distance or line width are above their upper bounds. In this case, search direction is inverted. Otherwise false is returned. + * @param[in] line_distance: current distance between robot and line + * @param[in] line_width: current line width + * @param[in] yaw_angle: current yaw angle from odometry (currently unused) + * @return true, if measurement (line distance or line width) out of configured bounds + */ +bool sick_line_guidance_demo::AdjustHeading::greaterThanUpperBounds(double line_distance, double line_width, double yaw_angle) +{ + return ((std::abs(line_distance) >= m_adjust_heading_cfg.search_upper_bound_linedistance) // line distance beyond limit + || (std::abs(line_width) >= m_adjust_heading_cfg.search_upper_bound_linewidth) // line width beyond limit + || (std::abs(deltaAngle(yaw_angle, m_yaw_angle_at_start)) >= m_adjust_heading_cfg.search_max_yaw_angle_delta) // yaw angle beyond limit + || ((ros::Time::now() - m_states_start_time[m_adjust_heading_state]).toSec() > m_adjust_heading_cfg.max_state_duration)); // timeout +} + +/* + * Computes the feature value from a new measurement. + * @param[in] line_distance: current distance between robot and line + * @param[in] line_width: current line width + * @param[in] yaw_angle: current yaw angle from odometry (currently unused) + * @return feature value + */ +double sick_line_guidance_demo::AdjustHeading::computeFeature(double line_distance, double line_width, double yaw_angle) +{ + if(m_use_line_width) // minimize combination of line_distance and line_width + { + double feature_dist = line_distance / m_adjust_heading_cfg.sigma_linedistance; + double feature_width = line_width / m_adjust_heading_cfg.sigma_linewidth; + return std::sqrt(feature_dist * feature_dist + feature_width * feature_width); + } + return line_distance; // minimize line distance +} + +/* + * Updates the current line distance and switches the state, if an upper bound or the minimum line distance has been reached: + * ADJUST_HEADING_STATE_STOPPED -> ADJUST_HEADING_STATE_SEARCH_FORWARD_1 -> ADJUST_HEADING_STATE_SEARCH_BACKWARD-2 -> ADJUST_HEADING_STATE_SEARCH_FORWARD_3 -> ADJUST_HEADING_STATE_STOPPED + * @param[in] line_detected: true if line detected (line_distance is valid), false if no line detected + * @param[in] line_distance: current distance between robot and line + * @param[in] line_width: current line width + * @param[in] yaw_angle: current yaw angle from odometry + * @return angular velocity (updated value, taken the current line_distance into account) + */ +double sick_line_guidance_demo::AdjustHeading::update(bool line_detected, double sensor_line_distance, double sensor_line_width, double yaw_angle) +{ + // Compute minimization feature (line distance or a combination of line distance and line width + double angular_z = 0; + double feature_value = computeFeature(sensor_line_distance, sensor_line_width, yaw_angle); + double feature_value_at_start = computeFeature(m_line_distance_at_start, m_line_width_at_start, m_yaw_angle_at_start); + AdjustHeadingAutoPrinter auto_printer(this, line_detected, sensor_line_distance, sensor_line_width, feature_value, yaw_angle, &angular_z); + + // Update velocity control for state transition with smooth angular_z + if(!line_detected) + m_wait_at_state_transition.stop(); + else + m_wait_at_state_transition.update(yaw_angle); + if(!isRunning()) + return angular_z; + + // Testmode: limit time amount in seconds to stay in current search state; after seconds state increases. + // Prevents endless adjustment if TurtleBot is mounted fixed under test and odom always returns constant positions + if(!m_states_start_time[m_adjust_heading_state].isValid()) + m_states_start_time[m_adjust_heading_state] = ros::Time::now(); + + // Stop search if lower bound reached (it won't get better) + if(line_detected && std::abs(sensor_line_distance) <= m_adjust_heading_cfg.search_lower_bound_linedistance + 0.001 && std::abs(sensor_line_width) <= m_adjust_heading_cfg.search_lower_bound_linewidth + 0.0001) + { + if(m_adjust_heading_state != ADJUST_HEADING_STATE_STOPPED) + { + m_adjust_heading_state = ADJUST_HEADING_STATE_STOPPED; + angular_z = m_wait_at_state_transition.start(yaw_angle, yaw_angle, 0, line_detected); + } + return angular_z; + } + + // Update best feature value found while searching + if(line_detected && std::abs(feature_value) < std::abs(m_feature_value_best)) + { + m_feature_value_best = feature_value; + m_line_distance_best = sensor_line_distance; + m_line_width_best = sensor_line_width; + m_yaw_angle_best = yaw_angle; + } + + // While waiting at state transitions: increase/decrease angular_z slowly until the desired yaw angle is reached. Avoid acceleration, smooth changes of velocity + if(m_wait_at_state_transition.isRunning(angular_z)) + return angular_z; + angular_z = 0; + + // 1. forward search: if line distance is out of its upper bound, revert search direction (i.e. invert m_heading_angular_z) + if(m_adjust_heading_state == ADJUST_HEADING_STATE_SEARCH_FORWARD_1) + { + if(!line_detected || greaterThanUpperBounds(sensor_line_distance, sensor_line_width, yaw_angle)) + { + // line distance or line width out of bounds, revert search direction + m_adjust_heading_cfg.angular_z = -m_adjust_heading_cfg.angular_z; + m_adjust_heading_state = ADJUST_HEADING_STATE_SEARCH_BACKWARD_2; // default: backward search after forward search + angular_z = m_wait_at_state_transition.start(yaw_angle, m_yaw_angle_best, m_adjust_heading_cfg.angular_z, line_detected); // increase angular_z slowly until the desired yaw angle is reached, avoid acceleration and change in velocity + return angular_z; + } + // forward search: just continue rotation with m_heading_angular_z + angular_z = (std::abs(feature_value) >= feature_value_at_start) ? 2 * m_adjust_heading_cfg.angular_z : m_adjust_heading_cfg.angular_z; + return angular_z; + } + + // 2. backward search: if line distance is out of its upper bound, revert search direction (i.e. invert m_heading_angular_z) + if(m_adjust_heading_state == ADJUST_HEADING_STATE_SEARCH_BACKWARD_2) + { + if(!line_detected) + { + // line lost, revert search direction + m_adjust_heading_cfg.angular_z = -m_adjust_heading_cfg.angular_z; + m_adjust_heading_state = ADJUST_HEADING_STATE_SEARCH_FORWARD_3; + angular_z = m_wait_at_state_transition.start(yaw_angle, m_yaw_angle_best, m_adjust_heading_cfg.angular_z, line_detected); // increase angular_z slowly until the desired yaw angle is reached, avoid acceleration and change in velocity + return angular_z; + } + if(greaterThanUpperBounds(sensor_line_distance, sensor_line_width, yaw_angle)) + { + // line distance or line width out of bounds, revert search direction + m_adjust_heading_cfg.angular_z = -m_adjust_heading_cfg.angular_z; + m_adjust_heading_state = ADJUST_HEADING_STATE_SEARCH_FORWARD_3; + angular_z = m_wait_at_state_transition.start(yaw_angle, m_yaw_angle_best, m_adjust_heading_cfg.angular_z, line_detected); // increase angular_z slowly until the desired yaw angle is reached, avoid acceleration and change in velocity + return angular_z; + } + // backward search: just continue rotation with m_heading_angular_z + angular_z = (std::abs(feature_value) >= feature_value_at_start) ? 2 * m_adjust_heading_cfg.angular_z : m_adjust_heading_cfg.angular_z; + return angular_z; + } + + // 3. backward search: continue with rotation until upper bound reached, or close to minimum + if(m_adjust_heading_state == ADJUST_HEADING_STATE_SEARCH_FORWARD_3) + { + double delta_heading = deltaAngle(yaw_angle, m_yaw_angle_best); + if(std::abs(delta_heading) < m_adjust_heading_cfg.delta_angle_epsilon) // stop search, best angular_z at lowest line distance reached + { + m_adjust_heading_state = ADJUST_HEADING_STATE_STOPPED; + angular_z = m_wait_at_state_transition.start(yaw_angle, m_yaw_angle_best, 0, line_detected); + return angular_z; + } + // Prevent toggling around the minimum line distance at the final optimization step + if(signum(m_final_step_delta_heading, m_adjust_heading_cfg.delta_angle_epsilon) != signum(delta_heading, m_adjust_heading_cfg.delta_angle_epsilon)) + m_final_step_toggle_cnt++; + if(m_final_step_toggle_cnt > 4 || ((ros::Time::now() - m_states_start_time[m_adjust_heading_state]).toSec() > m_adjust_heading_cfg.max_state_duration)) // timeout + { + m_adjust_heading_state = ADJUST_HEADING_STATE_STOPPED; // stop search, toggling around the minimum + angular_z = m_wait_at_state_transition.start(yaw_angle, m_yaw_angle_best, 0, line_detected); + return angular_z; + } + // continue until closer to the minimum + m_final_step_delta_heading = delta_heading; + if(delta_heading >= 0) + angular_z = std::max(delta_heading, std::abs(m_adjust_heading_cfg.angular_z)); + else + angular_z = std::min(delta_heading, -std::abs(m_adjust_heading_cfg.angular_z)); + return angular_z; // velocity.angular.z = delta_heading per second, i.e. we turn slowly back (desired heading reached in one second), avoiding acceleration + } + + return 0; +} + + diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/src/barcodes.cpp b/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/src/barcodes.cpp new file mode 100755 index 0000000..bc18e19 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/src/barcodes.cpp @@ -0,0 +1,118 @@ +/* + * barcodes implements a container for barcodes for sick_line_guidance_demo (position and label). + * + * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim + * Copyright (C) 2019 SICK AG, Waldkirch + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of SICK AG nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Authors: + * Michael Lehning + * + * Copyright 2019 SICK AG + * Copyright 2019 Ing.-Buero Dr. Michael Lehning + * + */ +#include +#include +#include +#include +#include +#include +#include + +#include "sick_line_guidance_demo/barcodes.h" + +/* + * reads a list of barcodes from xml-file + * @param[in] barcode_xml_file xml-file, f.e. "demo_barcodes.xml" + * @return list of barcodes (empty list, if xmlfile could not be read) + */ +std::vector sick_line_guidance_demo::BarcodeUtil::readBarcodeXmlfile(const std::string & barcode_xml_file) +{ + std::vector barcodes; + // Read list of barcode settings from xml-file. Example ("demo_barcodes.xml"): + // + // + // + // + // + // + try + { + TiXmlDocument xml_config(barcode_xml_file); + if(xml_config.LoadFile()) + { + TiXmlElement* xml_barcodes = xml_config.FirstChild("sick_line_guidance_demo")->FirstChild("barcodes")->ToElement(); + if(xml_barcodes) + { + TiXmlElement* xml_barcode = xml_barcodes->FirstChildElement("barcode"); + while(xml_barcode) + { + Barcode barcode; + barcode.label() = xml_barcode->Attribute("label"); + double pos_x1 = std::stod(xml_barcode->Attribute("outer_pos_x1")); + double pos_y1 = std::stod(xml_barcode->Attribute("outer_pos_y1")); + double pos_x2 = std::stod(xml_barcode->Attribute("outer_pos_x2")); + double pos_y2 = std::stod(xml_barcode->Attribute("outer_pos_y2")); + barcode.outerRectWorld() = cv::Rect2d(pos_x1, pos_y1, pos_x2 - pos_x1, pos_y2 - pos_y1); + pos_x1 = std::stod(xml_barcode->Attribute("inner_pos_x1")); + pos_y1 = std::stod(xml_barcode->Attribute("inner_pos_y1")); + pos_x2 = std::stod(xml_barcode->Attribute("inner_pos_x2")); + pos_y2 = std::stod(xml_barcode->Attribute("inner_pos_y2")); + barcode.innerRectWorld() = cv::Rect2d(pos_x1, pos_y1, pos_x2 - pos_x1, pos_y2 - pos_y1); + std::string flipped = xml_barcode->Attribute("flipped"); + barcode.flipped() = (boost::iequals(flipped, "true") || boost::iequals(flipped.substr(0,1), "y") || std::atoi(flipped.c_str()) > 0); + barcodes.push_back(barcode); + xml_barcode = xml_barcode->NextSiblingElement("barcode"); + } + return barcodes; + } + } + } + catch(const std::exception & exc) + { + ROS_ERROR_STREAM("readBarcodeXmlfile("<< barcode_xml_file << ") failed: exception " << exc.what()); + } + return std::vector(); + +} diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/src/explore_line_state.cpp b/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/src/explore_line_state.cpp new file mode 100755 index 0000000..500f1ee --- /dev/null +++ b/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/src/explore_line_state.cpp @@ -0,0 +1,162 @@ +/* + * ExploreLineState implements the state to explore a line for sick_line_guidance_demo. + * As long as ols does not detect a line, cmd_vel messages are published to search a line. + * Currently, the TurtleBot just moves straight forwared until a line is detected. + * Input: ols messages + * Output: cmd_vel messages + * + * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim + * Copyright (C) 2019 SICK AG, Waldkirch + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of SICK AG nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Authors: + * Michael Lehning + * + * Copyright 2019 SICK AG + * Copyright 2019 Ing.-Buero Dr. Michael Lehning + * + */ +#include +#include "sick_line_guidance_demo/explore_line_state.h" +#include "sick_line_guidance_demo/navigation_util.h" +#include "sick_line_guidance_demo/time_format.h" + +/* + * Constructor + * @param[in] nh ros handle + * @param[in] context shared fsm context + */ +sick_line_guidance_demo::ExploreLineState::ExploreLineState(ros::NodeHandle* nh, RobotFSMContext* context) : m_fsm_context(context), m_explore_line_rate(20) +{ + if(nh) + { + // Configuration of explore line state + double exploreLineRate; + ros::param::getCached("/explore_line_state/exploreLineRate", exploreLineRate); // frequency to update explore line state, default: 20 Hz + ros::param::getCached("/explore_line_state/exploreSpeed", m_exploreSpeed); // default linear velocity to explore a line + ros::param::getCached("/explore_line_state/olsMessageTimeout", m_ols_message_timeout); // timeout for ols messages: robot stops and waits, if last ols message was received more than seconds ago + ros::param::getCached("/explore_line_state/odomMessageTimeout", m_odom_message_timeout); // timeout for odom messages: robot stops and waits, if last ols message was received more than seconds ago + ros::Rate m_explore_line_rate = ros::Rate(exploreLineRate); // frequency to update explort line state, default: 20 Hz + ROS_INFO_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "Configuration explore line state: " + << " exploreLineRate=" << exploreLineRate << " exploreSpeed=" << m_exploreSpeed + << ", olsMessageTimeout=" << m_ols_message_timeout << ", odomMessageTimeout=" << m_odom_message_timeout); + } +} + +/* + * Destructor + */ +sick_line_guidance_demo::ExploreLineState::~ExploreLineState() +{ + m_fsm_context = 0; +} + +/* + * Clears all internal states + */ +void sick_line_guidance_demo::ExploreLineState::clear(void) +{ +} + +/* + * Runs the explore line state until line is detected (or a fatal error occures). + * @return FOLLOW_LINE in case of line detected, or EXIT in case ros::ok()==false. + */ +sick_line_guidance_demo::RobotFSMContext::RobotState sick_line_guidance_demo::ExploreLineState::run(void) +{ + assert(m_fsm_context); + ROS_INFO_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "RobotFSM: entering ExploreLineState"); + + geometry_msgs::Twist velocityMessage; + velocityMessage.linear.x = 0; + velocityMessage.linear.y = 0; + velocityMessage.linear.z = 0; + velocityMessage.angular.x = 0; + velocityMessage.angular.y = 0; + velocityMessage.angular.z = 0; + + while(ros::ok()) + { + m_explore_line_rate.sleep(); + + // Get current ols measurement + sick_line_guidance::OLS_Measurement ols_msg = m_fsm_context->getOlsState(); + //sick_line_guidance_demo::RobotPosition odom_position = m_fsm_context->getOdomPosition(); + bool ols_message_missing = m_fsm_context->hasOlsMessageTimeout(m_ols_message_timeout); + bool odom_message_missing = m_fsm_context->hasOdomMessageTimeout(m_odom_message_timeout); + if(ols_message_missing) + ROS_WARN_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "RobotFSM: no ols message received since " << ((ros::Time::now() - m_fsm_context->getOlsStateTime()).toSec()) << " seconds"); + if(odom_message_missing) + ROS_WARN_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "RobotFSM: no odom message received since " << ((ros::Time::now() - m_fsm_context->getOdomPositionTime()).toSec()) << " seconds"); + if(ols_message_missing || odom_message_missing) + { + velocityMessage.angular.z = 0; + velocityMessage.linear.x = 0; + m_fsm_context->publish(velocityMessage); + continue; + } + + // Switch to FOLLOW_LINE, if line detected + if(sick_line_guidance_demo::NavigationUtil::lineDetected(ols_msg)) + { + ROS_INFO_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "RobotFSM: line detected, switching to state FOLLOW_LINE"); + velocityMessage.angular.z = 0; + velocityMessage.linear.x = 0; + m_fsm_context->publish(velocityMessage); + return RobotFSMContext::RobotState::FOLLOW_LINE; + } + + // Otherwise move straight forward + if(velocityMessage.linear.x < m_exploreSpeed) + { + velocityMessage.linear.x += m_exploreSpeed / 30.0; + } + else + { + velocityMessage.linear.x = m_exploreSpeed; + } + m_fsm_context->publish(velocityMessage); + } + ROS_INFO_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "RobotFSM: leaving ExploreLineState"); + return RobotFSMContext::RobotState::EXIT; // ros::ok() == false +} diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/src/figure_eight_fsm.cpp b/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/src/figure_eight_fsm.cpp new file mode 100755 index 0000000..cc31d02 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/src/figure_eight_fsm.cpp @@ -0,0 +1,158 @@ +/* + * FigureEightVelocityFSM implements a simple state machine, creating cmd_vel messages + * to drive a TurtleBot in a figure of eight. + * + * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim + * Copyright (C) 2019 SICK AG, Waldkirch + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of SICK AG nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Authors: + * Michael Lehning + * + * Copyright 2019 SICK AG + * Copyright 2019 Ing.-Buero Dr. Michael Lehning + * + */ +#include +#include +#include +#include + +#include "sick_line_guidance_demo/figure_eight_fsm.h" +#include "sick_line_guidance_demo/time_format.h" + +/* + * class FigureEightVelocityFSM implements a simple state machine, creating cmd_vel messages + * to drive a TurtleBot in a figure of eight. + */ + +/* + * Constructor + */ +sick_line_guidance_demo::FigureEightVelocityFSM::FigureEightVelocityFSM() +{ + // define the state machine + m_state_cnt = 0; + m_next_state_switch = ros::Time(0); + VelocityState state; + state.cmd_vel.linear.x = 0; + state.cmd_vel.linear.y = 0; + state.cmd_vel.linear.z = 0; + state.cmd_vel.angular.x = 0; + state.cmd_vel.angular.y = 0; + state.cmd_vel.angular.z = 0; + // 1. state: drive anti-clockwise half circle witch 180 degree turn in 5 seconds + m_vec_vel_states.push_back(state); + m_vec_vel_states.back().duration = ros::Duration(5.0); + m_vec_vel_states.back().cmd_vel.linear.x = 0.01; + m_vec_vel_states.back().cmd_vel.angular.z = M_PI / m_vec_vel_states.back().duration.toSec(); + // 2. state: stop for 2 seconds + m_vec_vel_states.push_back(state); + m_vec_vel_states.back().duration = ros::Duration(3.0); + m_vec_vel_states.back().cmd_vel.linear.x = 0.00; + m_vec_vel_states.back().cmd_vel.angular.z = 0.00; + // 3. state: drive anti-clockwise half circle witch 180 degree turn in 5 seconds + m_vec_vel_states.push_back(state); + m_vec_vel_states.back().duration = ros::Duration(5.0); + m_vec_vel_states.back().cmd_vel.linear.x = 0.01; + m_vec_vel_states.back().cmd_vel.angular.z = M_PI / m_vec_vel_states.back().duration.toSec(); + // 4. state: stop for 2 seconds + m_vec_vel_states.push_back(state); + m_vec_vel_states.back().duration = ros::Duration(3.0); + m_vec_vel_states.back().cmd_vel.linear.x = 0.00; + m_vec_vel_states.back().cmd_vel.angular.z = 0.00; + // 5. state: drive clockwise half circle witch 180 degree turn in 5 seconds + m_vec_vel_states.push_back(state); + m_vec_vel_states.back().duration = ros::Duration(5.0); + m_vec_vel_states.back().cmd_vel.linear.x = 0.01; + m_vec_vel_states.back().cmd_vel.angular.z = -M_PI / m_vec_vel_states.back().duration.toSec(); + // 6. state: stop for 2 seconds + m_vec_vel_states.push_back(state); + m_vec_vel_states.back().duration = ros::Duration(3.0); + m_vec_vel_states.back().cmd_vel.linear.x = 0.00; + m_vec_vel_states.back().cmd_vel.angular.z = 0.00; + // 7. state: drive clockwise half circle witch 180 degree turn in 5 seconds + m_vec_vel_states.push_back(state); + m_vec_vel_states.back().duration = ros::Duration(5.0); + m_vec_vel_states.back().cmd_vel.linear.x = 0.01; + m_vec_vel_states.back().cmd_vel.angular.z = -M_PI / m_vec_vel_states.back().duration.toSec(); + // 8. state: stop for 2 seconds + m_vec_vel_states.push_back(state); + m_vec_vel_states.back().duration = ros::Duration(3.0); + m_vec_vel_states.back().cmd_vel.linear.x = 0.00; + m_vec_vel_states.back().cmd_vel.angular.z = 0.00; +} + +/* + * Destructor + */ +sick_line_guidance_demo::FigureEightVelocityFSM::~FigureEightVelocityFSM() +{ +} + +/* + * Next cycle, internal state is updated, velocity message may switch to next movement + */ +void sick_line_guidance_demo::FigureEightVelocityFSM::update(void) +{ + ros::Time cur_time = ros::Time::now(); + if(!m_next_state_switch.isValid()) + { + m_next_state_switch = cur_time + m_vec_vel_states[m_state_cnt].duration; + } + if(cur_time >= m_next_state_switch) + { + m_state_cnt = ((m_state_cnt + 1) % (m_vec_vel_states.size())); + m_next_state_switch = cur_time + m_vec_vel_states[m_state_cnt].duration; + ROS_INFO_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "FigureEightVelocityFSM: state switched, cmd_vel: linear.x=" + << std::fixed << std::setprecision(3) << (m_vec_vel_states[m_state_cnt].cmd_vel.linear.x) << ", angular.z=" << (m_vec_vel_states[m_state_cnt].cmd_vel.angular.z/M_PI) << "*PI"); + } +} + +/* + * Returns the cmd_vel message for the current movement + */ +geometry_msgs::Twist sick_line_guidance_demo::FigureEightVelocityFSM::getVelocity(void) +{ + return m_vec_vel_states[m_state_cnt].cmd_vel; +} diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/src/follow_line_state.cpp b/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/src/follow_line_state.cpp new file mode 100755 index 0000000..cda4be0 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/src/follow_line_state.cpp @@ -0,0 +1,362 @@ +/* + * FollowLineState implements the state to follow a line for sick_line_guidance_demo. + * As long as ols detects a line, cmd_vel messages are published to follow this line. + * Input: ols and odometry messages + * Output: cmd_vel messages + * + * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim + * Copyright (C) 2019 SICK AG, Waldkirch + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of SICK AG nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Authors: + * Michael Lehning + * + * Copyright 2019 SICK AG + * Copyright 2019 Ing.-Buero Dr. Michael Lehning + * + */ +#include +#include "sick_line_guidance_demo/adjust_heading.h" +#include "sick_line_guidance_demo/follow_line_state.h" +#include "sick_line_guidance_demo/navigation_util.h" +#include "sick_line_guidance_demo/pid.h" +#include "sick_line_guidance_demo/regression_1d.h" +#include "sick_line_guidance_demo/time_format.h" + +/* + * Constructor + * @param[in] nh ros handle + * @param[in] context shared fsm context + */ +sick_line_guidance_demo::FollowLineState::FollowLineState(ros::NodeHandle* nh, RobotFSMContext* context) : m_fsm_context(context), m_last_barcode_detected_time(0), m_pid_kp(0), m_pid_ki(0), m_pid_kd(0), m_followLineRate(20) +{ + if(nh) + { + // Configuration of PID controller + ros::param::getCached("/pid_controller/kp", m_pid_kp); + ros::param::getCached("/pid_controller/ki", m_pid_ki); + ros::param::getCached("/pid_controller/kd", m_pid_kd); + ros::param::getCached("/pid_controller/setpoint", m_pid_setpoint); + ROS_INFO_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "Configuration pid_controller: kp=" << m_pid_kp << ", ki=" << m_pid_ki << ", kd=" << m_pid_kd << ", setpoint=" << m_pid_setpoint); + // Configuration of follow line state + double followLineRate; + ros::param::getCached("/follow_line_state/followLineRate", followLineRate); // frequency to update follow line state, default: 20 Hz + ros::param::getCached("/follow_line_state/followSpeed", m_followSpeed); // default linear velocity to follow a line + ros::param::getCached("/follow_line_state/noLineTime", m_noLineTime); // time in seconds before switching to state explore line because of lost line + ros::param::getCached("/follow_line_state/sensorLineWidth", m_sensorLineWidth); // measured line width at 0 degree, 29 mm for an OLS mounted 65 mm over ground, 20 mm for an OLS mounted 100 mm over ground + ros::param::getCached("/follow_line_state/sensorLineMeasurementJitter", m_sensorLineMeasurementJitter); // tolerate some line measurement jitter when adjusting the heading + ros::param::getCached("/follow_line_state/adjustHeadingAngularZ", m_adjustHeadingAngularZ); // velocity.angular.z to adjust robots heading + ros::param::getCached("/follow_line_state/adjustHeadingLcpDeviationThresh", m_adjustHeadingLcpDeviationThresh); // start to adjust heading, if the line distance increases over time (deviation of 1D-regression of line center points is above threshold lcpDeviationThresh) + ros::param::getCached("/follow_line_state/adjustHeadingLcpThresh", m_adjustHeadingLcpThresh); // start to adjust heading, if the line distance in meter (abs value) is above this threshold + ros::param::getCached("/follow_line_state/adjustHeadingDeltaAngleEpsilon", m_adjustHeadingDeltaAngleEpsilon); // search stops, if the difference between current and desired yaw angle is smaller than delta_angle_epsilon + ros::param::getCached("/follow_line_state/adjustHeadingMinDistanceToLastAdjust", m_adjustHeadingMinDistanceToLastAdjust); // move at least some cm before doing next heading adjustment + ros::param::getCached("/follow_line_state/olsMessageTimeout", m_olsMessageTimeout); // timeout for ols messages: robot stops and waits, if last ols message was received more than seconds ago + ros::param::getCached("/follow_line_state/odomMessageTimeout", m_odomMessageTimeout); // timeout for odom messages: robot stops and waits, if last ols message was received more than seconds ago + ros::param::getCached("/wait_at_barcode_state/stopWaitSeconds", m_seconds_at_barcode); // time in seconds to wait at barcode + ROS_INFO_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "Configuration follow line state: " + << " followLineRate=" << followLineRate << " followSpeed=" << m_followSpeed << ", noLineTime=" << m_noLineTime << ", sensorLineWidth=" << m_sensorLineWidth + << ", sensorLineMeasurementJitter=" << m_sensorLineMeasurementJitter << ", adjustHeadingAngularZ=" << m_adjustHeadingAngularZ + << ", adjustHeadingLcpDeviationThresh=" << m_adjustHeadingLcpDeviationThresh << ", adjustHeadingLcpThresh=" << m_adjustHeadingLcpThresh + << ", adjustHeadingDeltaAngleEpsilon=" << m_adjustHeadingDeltaAngleEpsilon << ", adjustHeadingMinDistanceToLastAdjust=" << m_adjustHeadingMinDistanceToLastAdjust + << ", olsMessageTimeout=" << m_olsMessageTimeout << ", odomMessageTimeout=" << m_odomMessageTimeout); + ros::Rate m_followLineRate = ros::Rate(followLineRate); // frequency to update follow line state, default: 20 Hz + ros::param::getCached("/sick_line_guidance_demo/ols_simu", m_ols_simu); // ols simulation (default: 0), test only + } +} + +/* + * Destructor + */ +sick_line_guidance_demo::FollowLineState::~FollowLineState() +{ + m_fsm_context = 0; +} + +/* + * Clears all internal states (pid etc.) + */ +void sick_line_guidance_demo::FollowLineState::clear(void) +{ +} + +/* + * Runs the follow line state until line is lost (or a fatal error occures) + * @return EXPLORE_LINE in case of line lost, or EXIT in case ros::ok()==false. + */ +sick_line_guidance_demo::RobotFSMContext::RobotState sick_line_guidance_demo::FollowLineState::run(void) +{ + assert(m_fsm_context); + ROS_INFO_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "RobotFSM: entering FollowLineState"); + + sick_line_guidance_demo::Regression1D lcp_regression(10); // regression over the last 10 line center points + sick_line_guidance_demo::AdjustHeading adjust_heading1; // state machine to adjust the heading, if the line distance increases over time. + sick_line_guidance_demo::AdjustHeading adjust_heading2; // state machine to adjust the heading, if the line width is out of plausible range. + size_t adjust_heading_cnt = 0; // count the number of heading adjustments required + float ols_msg_position = 0; + float ols_msg_linewidth = 0; + int follow_line_index = 1; // default: follow line with index 1, which is the main (center) line + int last_line_index = 1; // last line index 1, used to detect line switches at junctions and at turnouts + bool line_switched = false; // true if line switched at junctions and at turnouts (left or right line) + double typical_line_with = m_sensorLineWidth; // line width after heading adjustment + sick_line_guidance_demo::RobotPosition pos_at_last_adjustment(-FLT_MAX, -FLT_MAX, 0); + + double pid_dt = m_followLineRate.expectedCycleTime().toSec(); + PID_Controller pid(pid_dt, m_pid_kp, m_pid_ki, m_pid_kd); + double controller_value = 0; // pid-controller output + + double followSpeed_in = 0; + bool line_detected = false; + ros::Time begin = ros::Time::now(); + + geometry_msgs::Twist velocityMessage; + velocityMessage.linear.x = 0; + velocityMessage.linear.y = 0; + velocityMessage.linear.z = 0; + velocityMessage.angular.x = 0; + velocityMessage.angular.y = 0; + velocityMessage.angular.z = 0; + + while(ros::ok()) + { + m_followLineRate.sleep(); + + // get parameters from ROS-parameter-server and configure the pid-controller + pid.setParams(pid_dt, m_pid_kp, m_pid_ki, m_pid_kd); + + /* this is for slow acceleration */ + if(followSpeed_in < m_followSpeed) + { + followSpeed_in += m_followSpeed / 30.0; + } + else + { + followSpeed_in = m_followSpeed; + } + + // if no line detected, wait a time before raise /LOST_LINE, to get to Emergency_Stop state + sick_line_guidance::OLS_Measurement ols_msg = m_fsm_context->getOlsState(); + sick_line_guidance_demo::RobotPosition odom_position = m_fsm_context->getOdomPosition(); + bool ols_message_missing = m_fsm_context->hasOlsMessageTimeout(m_olsMessageTimeout); + bool odom_message_missing = m_fsm_context->hasOdomMessageTimeout(m_odomMessageTimeout); + if(ols_message_missing) + ROS_WARN_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "RobotFSM: no ols message received since " << ((ros::Time::now() - m_fsm_context->getOlsStateTime()).toSec()) << " seconds"); + if(odom_message_missing) + ROS_WARN_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "RobotFSM: no odom message received since " << ((ros::Time::now() - m_fsm_context->getOdomPositionTime()).toSec()) << " seconds"); + if(ols_message_missing || odom_message_missing) + { + velocityMessage.angular.z = 0; + velocityMessage.linear.x = 0; + m_fsm_context->publish(velocityMessage); + lcp_regression.clear(); + pid.clear_state(); + followSpeed_in = 0; + continue; + } + if(!sick_line_guidance_demo::NavigationUtil::lineDetected(ols_msg)) + { + if(line_detected) + { + line_detected = false; + // save the last time, a line was detected + begin = ros::Time::now(); + } + + if(!adjust_heading1.isRunning() &&!adjust_heading2.isRunning() && (ros::Time::now() - begin).toSec() >= m_noLineTime) + { + // if a line is not detected for noLineTime, raise LOST_LINE to get in EmergencyStop state + ROS_INFO_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "LOST LINE -> switch to EXPLORE_LINE"); + return RobotFSMContext::RobotState::EXPLORE_LINE; + } + } + else // if a line is detected, control the line guidance + { + line_detected = true; + } + bool barcode_detected = sick_line_guidance_demo::NavigationUtil::barcodeDetected(ols_msg); + if(!adjust_heading1.isRunning() && !adjust_heading2.isRunning()) + { + // Switch to state WAIT_AT_BARCODE, if barcode detected and not currently adjusting + // uint32_t barcode = sick_line_guidance_demo::NavigationUtil::barcode(ols_msg); + if(barcode_detected && (ros::Time::now() - m_last_barcode_detected_time).toSec() > m_seconds_at_barcode+2) // time in seconds to wait at barcode + { + // new barcode detected -> switch to state WAIT_AT_BARCODE + m_last_barcode_detected_time = ros::Time::now(); + return RobotFSMContext::RobotState::WAIT_AT_BARCODE; + } + if(barcode_detected) + m_last_barcode_detected_time = ros::Time::now(); + } + + if(line_detected) + { + follow_line_index = 1; // default: follow the center line + bool follow_left_turnout = m_fsm_context->getFollowLeftTurnout(); + uint32_t last_barcode = m_fsm_context->getLastBarcode(); + // Bit coding of parallel lines (ols_msg.status & 0x7): + // 2 = 2 : only one (center) line + // 4 | 2 = 6: right line and center line + /// 2 | 1 = 3: left line and center line + // Interpretation gives follow_line_index + // 0: left + // 1: center + // 2: right + if((ols_msg.status & 0x7) == 3 && !barcode_detected && follow_left_turnout && last_barcode == 101) // turnout to the left side detected, follow_left_turnout == true: follow a turnout on the left side, follow_left_turnout flag is toggled + { + follow_line_index = 0; // follow the left trace + } + if((ols_msg.status&0x7) == 6 && !barcode_detected && (!follow_left_turnout || last_barcode == 102)) // the outer (main) line is joining from the right, follow the outer one + { + follow_line_index = 2; // follow the right trace + } + ols_msg_position = ols_msg.position[follow_line_index]; + ols_msg_linewidth = ols_msg.width[follow_line_index]; + line_switched = (follow_line_index != last_line_index); + // if you have to change the line forget your regression history + if(line_switched) + { + lcp_regression.clear(); // line regression with lcp from different lines doesn't make sense + } + else + { + // smTimer sm_timer_lcp("lcp_regression"); + lcp_regression.update(ols_msg_position); // update line regression by current lcp + } + if((ols_msg.status & 0x7) != 2) // junction + { + ROS_INFO_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << ((adjust_heading1.isRunning() || adjust_heading2.isRunning()) ? "ADJUST HEADING" : "FOLLOW LINE") + << " ols_msg.status=" << (int)ols_msg.status << ", follow_left_turnout=" << follow_left_turnout << ", last_barcode=" << last_barcode << ", follow_line_index=" << follow_line_index); + } + } + + // Adjust the robot heading, if the line distance increases over time. AdjustHeading searches the line orientation by minimizing the line distance and adjusts the robot heading. + // If the line distance is small, estimating heading from the line distance is inaccurate. Therefore, we adjust the heading by minimizing the line distance if the robot is outside the line + // (line distance > 10 mm or physicalLineWidth/2), and by minimizing the line width when inside the line (line distance < 10 mm or physicalLineWidth/2). + // smTimer sm_timer_adjust_init("adjust_header_init"); + sick_line_guidance_demo::AdjustHeadingConfig adjust_heading_cfg; + adjust_heading_cfg.search_lower_bound_linedistance = 0.0; // lower limit for line distance: if the current line distance is below search_lower_bound_linedistance, the search will revert direction + adjust_heading_cfg.search_upper_bound_linedistance = std::abs(ols_msg_position) + m_sensorLineMeasurementJitter; // upper limit for line distance: if the current line distance is above search_upper_bound_linedistance, the search will revert direction + adjust_heading_cfg.search_lower_bound_linewidth = m_sensorLineWidth; // lower limit for line width: if the current line width is below search_lower_bound_linewidth, the search will revert direction + adjust_heading_cfg.search_upper_bound_linewidth = std::abs(ols_msg_linewidth) + 2 * m_sensorLineMeasurementJitter; // upper limit for line width: if the current line width is above search_upper_bound_linewidth, the search will revert direction + adjust_heading_cfg.search_max_yaw_angle_delta = 80.0 * M_PI / 180.0; // upper limit for yaw angle: if abs. difference between current yaw_angle and yaw angle at start, the search will revert direction + adjust_heading_cfg.angular_z = (velocityMessage.angular.z > 0) ? -m_adjustHeadingAngularZ : m_adjustHeadingAngularZ; // velocity.angular.z under adjustment, default: 0.1 * M_PI / 4 + adjust_heading_cfg.measurement_jitter = m_sensorLineMeasurementJitter; // tolerate some line measurement jitter when adjusting the heading, default: 0.003 + adjust_heading_cfg.delta_angle_epsilon = m_adjustHeadingDeltaAngleEpsilon; // search stops, if the difference between current and desired yaw angle is smaller than delta_angle_epsilon + adjust_heading_cfg.sigma_linedistance = 1.0; // standard deviation of line distance, used to compute feature = sqrt((linedistance/sigma_linedistance)^2+(linewidth/sigma_linelinewidth)^2) + adjust_heading_cfg.sigma_linewidth = 1.0; // standard deviation of line width, used to compute feature = sqrt((linedistance/sigma_linedistance)^2+(linewidth/sigma_linelinewidth)^2) + adjust_heading_cfg.max_state_duration = 10; // test only: max time amount in seconds to stay in current search state; after seconds state increases. Prevents endless adjustment if TurtleBot is mounted fixed under test and odom always returns constant positions + + bool adjust_too_close_at_last = sick_line_guidance_demo::NavigationUtil::euclideanDistance(odom_position.pos, pos_at_last_adjustment.pos) < m_adjustHeadingMinDistanceToLastAdjust; // move at least some cm before doing next heading adjustment + double lcp_deviation = lcp_regression.getDeviation(); // get slope of deviation line and adjust heading if slope of deviation is above threshold (i.e. lcp increases resp. decreases over time) + bool lcp_deviation_exceed = (ols_msg_position > m_sensorLineWidth/2 && lcp_deviation > m_adjustHeadingLcpDeviationThresh) || (ols_msg_position < -m_sensorLineWidth/2 && lcp_deviation < -m_adjustHeadingLcpDeviationThresh); // slope of deviation line above threshold + bool lcp_max_dist_exceed = (std::abs(ols_msg_position) > m_adjustHeadingLcpThresh); // start to adjust heading, if the line distance in meter (abs value) is above this threshold + bool adjust_after_line_switch = line_switched && (follow_line_index == 1); // adjust heading after a line switch is recommanded, heading can be quite different from the new line + // distance to line center is increasing over time -> rotate and minimize distance to line + bool enable_adjust_heading1 = (barcode_detected ? (std::abs(ols_msg_position) > m_sensorLineWidth/2 + m_sensorLineMeasurementJitter) : (std::abs(ols_msg_position) > m_sensorLineWidth - m_sensorLineMeasurementJitter)); + // robot near line center and line width too high -> rotate and minimize line center and line width + bool enable_adjust_heading2 = std::abs(ols_msg_position) <= m_sensorLineWidth/2 + m_sensorLineMeasurementJitter && std::abs(ols_msg_linewidth) >= std::max(typical_line_with, m_sensorLineWidth) + m_sensorLineMeasurementJitter; + enable_adjust_heading2 |= adjust_after_line_switch; // tbd: testen ... + bool adjust_heading_line_detected = line_detected && (follow_line_index == 1); // When switching to another line at junctions, we wait to adjust heading until the line to follow is the center line + if(!adjust_heading1.isRunning() && !adjust_heading2.isRunning() && adjust_heading_line_detected && (lcp_deviation_exceed || lcp_max_dist_exceed) && !adjust_too_close_at_last && enable_adjust_heading1 && adjust_heading_cnt) + { + // line detected and line distance increases over time -> start adjustment of robots heading + adjust_heading1.start(ols_msg_position, ols_msg_linewidth, odom_position.yaw, false, adjust_heading_cfg); + } + else if(!adjust_heading1.isRunning() && !adjust_heading2.isRunning() && adjust_heading_line_detected && !barcode_detected && !adjust_too_close_at_last && enable_adjust_heading2) // line width not meaningsfull within barcode, do not use for adjust heading + { + // sensor position within the line, but probably not heading in line direction (line width measured > sensorLineWidth) -> start adjust heading + adjust_heading2.start(ols_msg_position, ols_msg_linewidth, odom_position.yaw, true, adjust_heading_cfg); + } + // sm_timer_adjust_init.stop(); + // update line distance and width, if adjust heading is running, and a line is detected + // smTimer sm_timer_adjust_update("adjust_header_update"); + double adjust_heading1_angular_z = adjust_heading1.update(adjust_heading_line_detected, ols_msg_position, ols_msg_linewidth, odom_position.yaw); + double adjust_heading2_angular_z = adjust_heading2.update(adjust_heading_line_detected && !barcode_detected, ols_msg_position, ols_msg_linewidth, odom_position.yaw); + if(adjust_heading1.isRunning() || adjust_heading2.isRunning()) + { + adjust_heading_cnt++; + pos_at_last_adjustment = odom_position; + typical_line_with = (adjust_heading1.isRunning() ? adjust_heading1.getBestLineWidth() : adjust_heading2.getBestLineWidth()); + if(m_ols_simu > 0) // test only (default: 0) + typical_line_with = m_sensorLineWidth; + velocityMessage.linear.x = 0; + velocityMessage.angular.z = (adjust_heading1.isRunning() ? adjust_heading1_angular_z : adjust_heading2_angular_z); + lcp_regression.clear(); + pid.clear_state(); + followSpeed_in = 0; + } + else // if(line_detected) // OLS detects no line when entering the barcode area. So we have to continue nevertheless we have detected a line or not. + { + if(line_detected) // ols_msg_position is valid, deviation = set value - actual value + controller_value = pid.calculate_output(m_pid_setpoint, ols_msg_position); + else + controller_value = 0; // ols_msg_position not valid, we continue straight forward + velocityMessage.angular.z = controller_value; // control angular velocity by pid controller + velocityMessage.linear.x = followSpeed_in; // set linear velocity + } + // sm_timer_adjust_update.stop(); + ROS_INFO_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << ((adjust_heading1.isRunning() || adjust_heading2.isRunning()) ? "ADJUST HEADING" : "FOLLOW LINE") + << ": detected=" << (int)line_detected << ", barcode=" << (int)barcode_detected << "(" << (int)ols_msg.barcode << "), linepos=" << ols_msg_position << ", linewidth=" << ols_msg_linewidth + << std::fixed << std::setprecision(3) << ", velocity.linear.x=" << velocityMessage.linear.x << ", velocity.angular.z=" << velocityMessage.angular.z/M_PI << "*PI" + << ", odom=(x:" << odom_position.pos.x << ",y:" << odom_position.pos.y << ",yaw:" << odom_position.yaw/M_PI << "*PI)"); + + // Hard limits for the velocity, both angular and linear, to prevent oscillation by PID: + // Incorrect robot heading increases the line distance, with causes the PID to compensate by making the heading even worse ... + // If abs(velocityMessage.angular.z) is too high, the robot may turn sharply and hit the line orthogonal, causing movements like a sinus wave with increasing amplitude. + // If abs(velocityMessage.linear.x) is too high, the robot may not be able to turn in time, i.e. before loosing the line. + // smTimer sm_timer_publish("publish_cmd_vel"); + double max_cmd_vel_angular = 0.5 * 0.7854; // 0.7854 = PI/4 + double max_cmd_vel_linear = 0.04; + // Force slow down if we're missing the center of the line + if(std::abs(ols_msg_position) > 0.003) + max_cmd_vel_linear = 0.03; + velocityMessage.angular.z = std::min(velocityMessage.angular.z, max_cmd_vel_angular); + velocityMessage.angular.z = std::max(velocityMessage.angular.z, -max_cmd_vel_angular); + velocityMessage.linear.x = std::min(velocityMessage.linear.x, max_cmd_vel_linear); + velocityMessage.linear.x = std::max(velocityMessage.linear.x, -max_cmd_vel_linear); + m_fsm_context->publish(velocityMessage); // publish controller value to cmd_vel topic + + last_line_index = follow_line_index; + } + ROS_INFO_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "RobotFSM: leaving FollowLineState"); + return RobotFSMContext::RobotState::EXIT; // ros::ok() == false +} diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/src/image_util.cpp b/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/src/image_util.cpp new file mode 100755 index 0000000..ece9392 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/src/image_util.cpp @@ -0,0 +1,197 @@ +/* + * ImageUtil implements utility functions for images and maps. + * + * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim + * Copyright (C) 2019 SICK AG, Waldkirch + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of SICK AG nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Authors: + * Michael Lehning + * + * Copyright 2019 SICK AG + * Copyright 2019 Ing.-Buero Dr. Michael Lehning + * + */ +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include + +#include "sick_line_guidance_demo/image_util.h" +#include "sick_line_guidance_demo/navigation_util.h" + +/* + * computes and returns the max possible euclidean distance of a robot position to the corner points of an image. + * @param[in] robot_pos robots position on the navigation map [pixel] + * @param[in] dimx width of navigation map [pixel] + * @param[in] dimy height of navigation map [pixel] + * @return max possible distance [pixel] + */ +double sick_line_guidance_demo::ImageUtil::computeMaxDistanceToCorner(const cv::Point & robot_pos, int dimx, int dimy) +{ + double r1_sqr = (robot_pos.x - 0) * (robot_pos.x - 0) + (robot_pos.y - 0) * (robot_pos.y - 0); // square distance to upper left corner + double r2_sqr = (robot_pos.x - dimx) * (robot_pos.x - dimx) + (robot_pos.y - 0) * (robot_pos.y - 0); // square distance to upper right corner + double r3_sqr = (robot_pos.x - dimx) * (robot_pos.x - dimx) + (robot_pos.y - dimy) * (robot_pos.y - dimy); // square distance to lower right corner + double r4_sqr = (robot_pos.x - 0) * (robot_pos.x - 0) + (robot_pos.y - dimy) * (robot_pos.y - dimy); // square distance to lower left corner + double r_sqr = std::max(std::max(r1_sqr, r2_sqr), std::max(r3_sqr, r4_sqr)); // max square distance + return sqrt(r_sqr); // max distance +} + +/* + * computes and returns the position of a point with a distance and in direction from a given point . + * @param[in] base_pos start position in world coordinates [meter] + * @param[in] heading angle between given point and the returned point + * @param[in] radius distance of the returned point to + * @return position of a point with a distance and in direction from a given point in world coordinates [meter] + */ +cv::Point2d sick_line_guidance_demo::ImageUtil::getWorldPointInDirection(const cv::Point2d & base_pos, double heading, double radius) +{ + cv::Point2d delta(radius * cos(heading), radius * sin(heading)); + return cv::Point2d(base_pos.x + delta.x, base_pos.y + delta.y); +} + +/* + * computes and returns the position of a point with a distance and in direction from a given point . + * @param[in] base_pos start position in map coordinates [pixel] + * @param[in] heading angle between given point and the returned point + * @param[in] radius distance of the returned point to + * @return position of a point with a distance and in direction from a given point in map coordinates [pixel] + */ +cv::Point sick_line_guidance_demo::ImageUtil::getMapPointInDirection(const cv::Point & base_pos, double heading, double radius) +{ + cv::Point2d delta(radius * cos(heading), radius * sin(heading)); + return cv::Point(base_pos.x + std::lround(delta.x), base_pos.y - std::lround(delta.y)); +} + +/* + * Detects and returns the line center points on a map, which can be seen + * by a robot at position moving in directions . + * Lines are detected +- 90 degree of . + * @param[in] map_img image (navigation map) + * @param[in] robot_map_pos robots position on the navigation map + * @param[in] robot_heading robots moving direction (i.e. robots yaw angle) + * @return list of line detection results (center points etc.) in map coordinates [pixel] + */ +std::vector sick_line_guidance_demo::ImageUtil::detectLineCenterPoints(const cv::Mat & map_img, const cv::Point & robot_map_pos, double robot_heading) +{ + // Get 2 starting points for cv::LineIterator in max. distance to robot_map_pos in orthogonal direction (+- 90 degree) to robot_heading + double r_max = sick_line_guidance_demo::ImageUtil::computeMaxDistanceToCorner(robot_map_pos, map_img.cols, map_img.rows); + cv::Point search_start_pos = getMapPointInDirection(robot_map_pos, robot_heading + CV_PI/2, r_max); + cv::Point search_end_pos = getMapPointInDirection(robot_map_pos, robot_heading - CV_PI/2, r_max); + cv::clipLine(cv::Size(map_img.cols, map_img.rows), search_start_pos, search_end_pos); + return detectLineCenterPoints(map_img, robot_map_pos, search_start_pos, search_end_pos); +} + +/* + * Detects and returns the line center points on a map, in front of + * a robot at position moving in directions . + * Lines are detected in heading of . + * @param[in] map_img image (navigation map) + * @param[in] robot_map_pos robots position on the navigation map + * @param[in] robot_heading robots moving direction (i.e. robots yaw angle) + * @return list of line detection results (center points etc.) in map coordinates [pixel] + */ +std::vector sick_line_guidance_demo::ImageUtil::detectLineCenterInHeadingDirection(const cv::Mat & map_img, const cv::Point & robot_map_pos, double robot_heading) +{ + // Get 2 starting points for cv::LineIterator in max. distance to robot_map_pos in orthogonal direction (+- 90 degree) to robot_heading + double r_max = sick_line_guidance_demo::ImageUtil::computeMaxDistanceToCorner(robot_map_pos, map_img.cols, map_img.rows); + cv::Point search_start_pos = robot_map_pos; + cv::Point search_end_pos = getMapPointInDirection(robot_map_pos, robot_heading, r_max); + cv::clipLine(cv::Size(map_img.cols, map_img.rows), search_start_pos, search_end_pos); + return detectLineCenterPoints(map_img, robot_map_pos, robot_map_pos, search_end_pos); +} + +/* + * detects and returns the line center points on a map, which can be seen + * by a robot at position moving in directions . + * @param[in] map_img image (navigation map) + * @param[in] robot_map_pos robots position on the navigation map + * @param[in] search_start_pos start point for line iteration + * @param[in] search_end_pos end point for line iteration + * @return list of line detection results (center points etc.) in map coordinates [pixel] + */ +std::vector sick_line_guidance_demo::ImageUtil::detectLineCenterPoints(const cv::Mat & map_img, const cv::Point & robot_map_pos, const cv::Point2d & search_start_pos, const cv::Point2d & search_end_pos) +{ + // Iterate from search start to end position and detect the line starts (pixel changes from not-black to black) and the line ends (pixel changes from black to not-black) + std::vector vec_line_detection_result; + bool lastPixelWasLine = false; + cv::Point2d robot_pos2d(robot_map_pos.x, robot_map_pos.y); + cv::Point2d line_start_pos(0, 0); + cv::Point2d line_end_pos(0, 0); + cv::LineIterator search_iter(map_img, search_start_pos, search_end_pos); + for(int cnt_iter = 0; cnt_iter < search_iter.count; cnt_iter++, search_iter++) + { + bool isLinePixel = ImageUtil::isLinePixel(map_img, search_iter.pos()); + if(isLinePixel) // line detected + { + line_end_pos = cv::Point2d(search_iter.pos().x, search_iter.pos().y); + } + if(!lastPixelWasLine && isLinePixel) // start of a line detected + { + line_start_pos = cv::Point2d(search_iter.pos().x, search_iter.pos().y); + } + else if(lastPixelWasLine && !isLinePixel && line_start_pos != line_end_pos) // end of a line detected, append line center point to output result (1 pixel lines ignored) + { + double line_center_posx = 0.5 * (line_start_pos.x + line_end_pos.x); + double line_center_posy = 0.5 * (line_start_pos.y + line_end_pos.y); + cv::Point2d line_center (line_center_posx, line_center_posy); + double line_width = NavigationUtil::euclideanDistance(line_start_pos, line_end_pos) + 1; + double center_dist = NavigationUtil::euclideanDistance(line_center, robot_pos2d); + LineDetectionResult result(line_center, line_start_pos, line_end_pos, line_width, center_dist); + vec_line_detection_result.push_back(result); + } + lastPixelWasLine = isLinePixel; + } + return vec_line_detection_result; +} + + diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/src/navigation_mapper.cpp b/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/src/navigation_mapper.cpp new file mode 100755 index 0000000..ed80e8f --- /dev/null +++ b/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/src/navigation_mapper.cpp @@ -0,0 +1,494 @@ +/* + * NavigationMapper transforms the robots xy-positions from world/meter into map/pixel position, + * detect lines and barcodes in the map plus their distance to the robot, + * and transforms them invers into world coordinates. + * + * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim + * Copyright (C) 2019 SICK AG, Waldkirch + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of SICK AG nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Authors: + * Michael Lehning + * + * Copyright 2019 SICK AG + * Copyright 2019 Ing.-Buero Dr. Michael Lehning + * + */ +#include + +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include + +#include "sick_line_guidance/sick_line_guidance_msg_util.h" +#include "sick_line_guidance_demo/image_util.h" +#include "sick_line_guidance_demo/navigation_mapper.h" +#include "sick_line_guidance_demo/navigation_util.h" +#include "sick_line_guidance_demo/time_format.h" + +/* + * class NavigationMapper transforms the robots xy-positions from world/meter into map/pixel position, + * detect lines and barcodes in the map plus their distance to the robot, + * and transforms them invers into world coordinates. + */ + +/* + * Constructor + * @param[in] map_imagefile navigation map, image file containing the map, f.e. "demo_map_02.png" + * @param[in] intrinsic_xmlfile xmlfile with intrinsic parameter to transform position from navigation map (pixel) to real world (meter) and vice versa, f.e. "cam_intrinsic.xml" with cx=cy=660, fx=fy=1 + * @param[in] barcode_xmlfile xmlfile with a list of barcodes with label and position, f.e. "demo_barcodes.xml" + * @param[in] ros_topic_output_ols_messages ROS topic for simulated OLS_Measurement messages, "/ols" (activated) in simulation and "" (deactivated) in demo application + * @param[in] sensor_config line sensor configuration setting (sensor parameter and mounting settings) + * @param[in] visualize==2: visualization+video enabled, map and navigation plots are displayed in a window, visualize==1: video created but not displayed, visualize==0: visualization and video disabled. + */ +sick_line_guidance_demo::NavigationMapper::NavigationMapper(ros::NodeHandle* nh, const std::string & map_imagefile, const std::string & intrinsic_xmlfile, const std::string & barcode_xmlfile, const std::string & ros_topic_output_ols_messages, + const LineSensorConfig & sensor_config, int visualize) + : m_navigation_state(INITIAL), m_visualize(visualize), m_window_name(""), m_ols_measurement_simulator(nh, ros_topic_output_ols_messages), m_sensor_config(sensor_config), m_message_rate(20), m_message_thread_run(false), m_message_thread(0) +{ + sick_line_guidance::OLS_Measurement zero_ols_msg; + sick_line_guidance::MsgUtil::zero(zero_ols_msg); + m_ols_measurement_sensor.set(zero_ols_msg); + m_ols_measurement_world.set(zero_ols_msg); + m_error_simulation_burst_no_line_duration = 0.0; + m_error_simulation_burst_no_line_frequency = 0.0; + m_error_simulation_no_line_start = ros::Time::now(); + m_error_simulation_no_line_end = m_error_simulation_no_line_start; + if(nh) + { + ros::param::getCached("/error_simulation/burst_no_line_duration", m_error_simulation_burst_no_line_duration); // error simulation: duration of "no line detected" bursts in seconds, default: 0.0 (disabled) + ros::param::getCached("/error_simulation/burst_no_line_frequency", m_error_simulation_burst_no_line_frequency); // error simulation: frequency of "no line detected" bursts in 1/seconds, default: 0.0 (disabled) + ROS_INFO_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "Configuration error simulation: " + << " m_error_simulation_burst_no_line_duration=" << m_error_simulation_burst_no_line_duration + << " m_error_simulation_burst_no_line_frequency=" << m_error_simulation_burst_no_line_frequency); + } + m_message_rate = ros::Rate(20); + if(!map_imagefile.empty()) + { + m_map_img = cv::imread(map_imagefile, cv::IMREAD_COLOR); + if(m_map_img.empty()) + { + ROS_ERROR_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "## ERROR sick_line_guidance_demo::NavigationMapper::NavigationMapper(): file \"" << map_imagefile << "\" not readable (" << __FILE__ << ":" << __LINE__ << ")."); + m_visualize = false; + } + } + if(!intrinsic_xmlfile.empty()) + { + cv::FileStorage cv_intrinsics_file(intrinsic_xmlfile, cv::FileStorage::READ); + cv_intrinsics_file["Camera_Matrix"] >> m_intrinsics; + cv_intrinsics_file.release(); + if(m_intrinsics.empty()) + { + ROS_ERROR_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "## ERROR sick_line_guidance_demo::NavigationMapper::NavigationMapper(): intrisic matrix file \"" << intrinsic_xmlfile << "\" not readable or parameter not found (" << __FILE__ << ":" << __LINE__ << ")."); + } + else + { + ROS_INFO_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "NavigationMapper: intrinsics {cx=" << m_intrinsics.at(0,2) << ",cy=" << m_intrinsics.at(1,2) << ",fx=" << m_intrinsics.at(0,0) << ",fy=" << m_intrinsics.at(1,1) << "} read from configuration file \"" << intrinsic_xmlfile << "\"."); + m_intrinsics_inv = m_intrinsics.inv(); + } + } + if(!barcode_xmlfile.empty()) + { + m_barcodes = BarcodeUtil::readBarcodeXmlfile(barcode_xmlfile); + if(m_barcodes.empty()) + { + ROS_ERROR_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "## ERROR sick_line_guidance_demo::NavigationMapper::NavigationMapper(): barcodes configuration file \"" << barcode_xmlfile << "\" not readable or parameter not found (" << __FILE__ << ":" << __LINE__ << ")."); + } + else + { + ROS_INFO_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "NavigationMapper: " << m_barcodes.size() << " barcodes read from configuration file \"" << barcode_xmlfile << "\"."); + for(std::vector::iterator iter_barcode = m_barcodes.begin(); iter_barcode != m_barcodes.end(); iter_barcode++) + { + iter_barcode->innerRectMap() = transformRectWorldToMap(iter_barcode->innerRectWorld()); + iter_barcode->outerRectMap() = transformRectWorldToMap(iter_barcode->outerRectWorld()); + ROS_DEBUG_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "barcode: label=\"" << iter_barcode->label() << "\", innerRect=" << iter_barcode->innerRectWorld()<< "\", outerRect=" << iter_barcode->outerRectWorld() << ", flipped=" << iter_barcode->flipped()); + } + } + } + if(m_visualize > 0) + { + m_window_img = m_map_img.clone(); + m_video_write = cv::VideoWriter("sick_line_guidance_demo.avi",CV_FOURCC('F','M','P','4'), 30, cv::Size(m_window_img.cols, m_window_img.rows)); + if(m_visualize > 1) + { + m_window_name = "NavigationMap"; + cv::namedWindow(m_window_name, cv::WINDOW_AUTOSIZE); + cv::imshow(m_window_name, m_window_img); + } + } +} + +/* + * Destructor + */ +sick_line_guidance_demo::NavigationMapper::~NavigationMapper() +{ + if(m_visualize > 0) + { + m_video_write.release(); + if(m_visualize > 1) + { + cv::destroyAllWindows(); + } + } +} + +/* + * transforms a xy-position in world coordinates [meter] to a xy-position in image map coordinates [pixel] + * @param[in] world_pos xy-position in world coordinates [meter] + * @param[out] map_pos xy-position in image map coordinates [pixel] + */ +void sick_line_guidance_demo::NavigationMapper::transformPositionWorldToMap(const cv::Point2d & world_pos, cv::Point2d & map_pos) +{ + double world_vec[3] = { world_pos.x, world_pos.y, 1.0 }; + cv::Mat world_mat = cv::Mat(3, 1, CV_64F, world_vec); + cv::Mat map_mat = m_intrinsics * world_mat; + map_pos.x = map_mat.at(0,0); + map_pos.y = map_mat.at(1,0); +} + +/* + * transforms a xy-position in world coordinates [meter] to a xy-position in image map coordinates [pixel] + * @param[in] world_pos xy-position in world coordinates [meter] + * @return xy-position in image map coordinates [pixel] + */ +cv::Point sick_line_guidance_demo::NavigationMapper::transformPositionWorldToMap(const cv::Point2d & world_pos) +{ + cv::Point2d map_pos2d; + transformPositionWorldToMap(world_pos, map_pos2d); + return cv::Point(std::lround(map_pos2d.x), std::lround(map_pos2d.y)); +} + +/* + * transforms a xy-position in image map coordinates [pixel] to a xy-position in world coordinates [meter] + * @param[in] map_pos xy-position in image map coordinates [pixel] + * @return xy-position in world coordinates [meter] + */ +cv::Point2d sick_line_guidance_demo::NavigationMapper::transformPositionMapToWorld(const cv::Point2d & map_pos) +{ + double map_vec[3] = { map_pos.x, map_pos.y, 1.0 }; + cv::Mat map_mat = cv::Mat(3, 1, CV_64F, map_vec); + cv::Mat world_mat = m_intrinsics_inv * map_mat; + return cv::Point2d(world_mat.at(0,0), world_mat.at(1,0)); +} + +/* + * transforms a rectangle in world coordinates [meter] into a rectangle in image map coordinates [pixel] + * @param[in] world_rect rectangle in world coordinates [meter] + * @return rectangle in image map coordinates [pixel] + */ +cv::Rect sick_line_guidance_demo::NavigationMapper::transformRectWorldToMap(const cv::Rect2d & world_rect) +{ + cv::Point map_pos1 = transformPositionWorldToMap(cv::Point2d(world_rect.x, world_rect.y)); + cv::Point map_pos2 = transformPositionWorldToMap(cv::Point2d(world_rect.x + world_rect.width, world_rect.y)); + cv::Point map_pos3 = transformPositionWorldToMap(cv::Point2d(world_rect.x + world_rect.width, world_rect.y + world_rect.height)); + cv::Point map_pos4 = transformPositionWorldToMap(cv::Point2d(world_rect.x, world_rect.y + world_rect.height)); + int x1 = std::min(std::min(map_pos1.x, map_pos2.x), std::min(map_pos3.x, map_pos4.x)); + int y1 = std::min(std::min(map_pos1.y, map_pos2.y), std::min(map_pos3.y, map_pos4.y)); + int x2 = std::max(std::max(map_pos1.x, map_pos2.x), std::max(map_pos3.x, map_pos4.x)); + int y2 = std::max(std::max(map_pos1.y, map_pos2.y), std::max(map_pos3.y, map_pos4.y)); + return cv::Rect(x1, y1, x2 - x1 + 1, y2 - y1 + 1); +} + +/* + * transforms an ols state from world coordinates [meter] to sensor units [meter], i.e. scales line distance (lcp) and line width + * from physical distances in the world to units of a sensor measurement; reverts function unscaleMeasurementToWorld(): + * line distances (lcp) are scaled by line_sensor_scaling_dist: default: 180.0/133.0, Scaling between physical distance to the line center and measured line center point + * (measurement: lcp = 180 mm, physical: lcp = 133 mm), depending on mounted sensor height + * line width are scaled by line_sensor_scaling_width: default: 29.0/20.0, Scaling between physical line width (20 mm) and measured line width (29 mm) depending on mounted sensor height + * (sensor mounted 100 mm over ground: scaling = 1, sensor mounted 65 mm over ground: scaling = 100/65 = 1.5) + * @param[in] ols_state ols state in world coordinates [meter] + * @return ols measurement in sensor units [meter] + */ +sick_line_guidance::OLS_Measurement sick_line_guidance_demo::NavigationMapper::scaleWorldToMeasurement(const sick_line_guidance::OLS_Measurement & ols_state) +{ + sick_line_guidance::OLS_Measurement ols_msg = ols_state; + int status_flags[3] = { 0x1, 0x2, 0x4 }; + for(int line_idx = 0; line_idx < 3; line_idx++) + { + if((ols_msg.status & (status_flags[line_idx])) != 0) + { + ols_msg.position[line_idx] *= static_cast(m_sensor_config.line_sensor_scaling_dist); + ols_msg.width[line_idx] *= static_cast(m_sensor_config.line_sensor_scaling_width); + } + } + return ols_msg; +} + +/* + * transforms an ols measurement from sensor units [meter] to world coordinates [meter], i.e. scales line distance (lcp) and line width + * from sensor units to physical distances; reverts function scaleWorldToMeasurement(). + * line distances (lcp) are scaled by 1 / line_sensor_scaling_dist + * line width are scaled by 1 / line_sensor_scaling_width + * @param[in] ols measurement in sensor units [meter] + * @return ols_state ols state in world coordinates [meter] + */ +sick_line_guidance::OLS_Measurement sick_line_guidance_demo::NavigationMapper::unscaleMeasurementToWorld(const sick_line_guidance::OLS_Measurement & ols_message) +{ + sick_line_guidance::OLS_Measurement ols_state = ols_message; + int status_flags[3] = { 0x1, 0x2, 0x4 }; + for(int line_idx = 0; line_idx < 3; line_idx++) + { + if((ols_state.status & (status_flags[line_idx])) != 0) + { + ols_state.position[line_idx] /= static_cast(m_sensor_config.line_sensor_scaling_dist); + ols_state.width[line_idx] /= static_cast(m_sensor_config.line_sensor_scaling_width); + } + } + return ols_state; +} + +/* + * starts the message loop to handle ols and odometry messages received. + * Message handling runs in background thread started by this function. + */ +void sick_line_guidance_demo::NavigationMapper::start(void) +{ + stop(); + m_message_thread_run = true; + m_message_thread = new boost::thread(&sick_line_guidance_demo::NavigationMapper::messageLoop, this); +} + +/* + * stops the message loop. + */ +void sick_line_guidance_demo::NavigationMapper::stop(void) +{ + if(m_message_thread) + { + m_message_thread_run = false; + m_message_thread->join(); + delete(m_message_thread); + m_message_thread = 0; + } +} + +/* + * message callback for OLS measurement messages. This function is called automatically by the ros::Subscriber after subscription of topic "/ols". + * It displays the OLS measurement (line info and barcodes), if visualization is enabled. + * @param[in] msg OLS measurement message (input) + */ +void sick_line_guidance_demo::NavigationMapper::messageCallbackOlsMeasurement(const boost::shared_ptr& msg) +{ + if(msg) + { + m_ols_measurement_sensor.set(*msg); // OLS measurement message received from topic "/ols" (measurment in sensor units) + m_ols_measurement_world.set(unscaleMeasurementToWorld(*msg)); // OLS measurement message received from topic "/ols" (measurment scaled world coordinates) + } + else + { + ROS_ERROR_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "## ERROR sick_line_guidance_demo::NavigationMapper::messageCallbackOlsMeasurement(): invalid message (" << __FILE__ << ":" << __LINE__ << ")"); + } +} + +/* + * message callback for odometry messages. This function is called automatically by the ros::Subscriber after subscription of topic "/odom". + * @param[in] msg odometry message (input) + */ +void sick_line_guidance_demo::NavigationMapper::messageCallbackOdometry(const nav_msgs::Odometry::ConstPtr& msg) +{ + if(msg) + { + m_odom_msg.set(*msg); + } + else + { + ROS_ERROR_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "## ERROR sick_line_guidance_demo::NavigationMapper::messageCallbackOdometry(): invalid message (" << __FILE__ << ":" << __LINE__ << ")"); + } +} + +/* + * Runs the message loop to handle odometry and ols messages. + * It transforms the robots xy-positions from world/meter into map/pixel position, detect lines and barcodes in the map plus their distance to the robot, + * and transform them invers into world coordinates. + * @param[in] msg odometry message (input) + */ +void sick_line_guidance_demo::NavigationMapper::messageLoop(void) +{ + while(ros::ok()) + { + nav_msgs::Odometry odom_msg = m_odom_msg.get(); + // Convert ModelStates message to robot position in world coordinates [meter] and and image position [pixel] + ROS_INFO_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "NavigationMapper::messageLoop: ols_msg_sensor=( " << sick_line_guidance::MsgUtil::toInfo(m_ols_measurement_sensor.get()) << " )"); + ROS_INFO_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "NavigationMapper::messageLoop: ols_msg_world=( " << sick_line_guidance::MsgUtil::toInfo(m_ols_measurement_world.get()) << " )"); + ROS_INFO_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "NavigationMapper::messageLoop: odom_msg=( " << NavigationUtil::toInfo(odom_msg) << " )"); + double robot_world_posx = 0, robot_world_posy= 0, robot_yaw_angle = 0; + NavigationUtil::toWorldPosition(odom_msg, robot_world_posx, robot_world_posy, robot_yaw_angle); + cv::Point2d robot_world_pos(robot_world_posx, robot_world_posy); + cv::Point robot_map_pos = transformPositionWorldToMap(robot_world_pos); + sick_line_guidance::OLS_Measurement ols_state = unscaleMeasurementToWorld(m_ols_measurement_simulator.GetState()); + + // start quickfix to find a line initially (simulation only): force an initial turn to the left to hit the line for the first time, needs further handling - tbd ... + if(m_navigation_state == INITIAL && (robot_world_pos.x*robot_world_pos.x+robot_world_pos.y*robot_world_pos.y) > (0.26*0.26)) // force initial turn to the left to hit the line for the first time + { + OLS_Measurement_Simulator::setLine(ols_state, 0.001f, 0.02); // force a slight left turn + } + if(m_navigation_state == INITIAL && ImageUtil::isLinePixel(m_map_img,robot_map_pos)) + m_navigation_state = FOLLOW_LINE; // line close to sensor, start to follow this line + // end quicktest + + // Detect possible line center points in map/image coordinates [pixel] + std::vector map_line_points = ImageUtil::detectLineCenterPoints(m_map_img, robot_map_pos, robot_yaw_angle); + + // transform line center points to world coordinate and compute distance in meter + std::vector world_line_points; + for(std::vector::iterator iter_line_points = map_line_points.begin(); iter_line_points != map_line_points.end(); iter_line_points++) + { + LineDetectionResult world_line_pos(*iter_line_points); + world_line_pos.centerPos() = transformPositionMapToWorld(iter_line_points->centerPos()); + world_line_pos.startPos() = transformPositionMapToWorld(iter_line_points->startPos()); + world_line_pos.endPos() = transformPositionMapToWorld(iter_line_points->endPos()); + world_line_pos.lineWidth() = NavigationUtil::euclideanDistance(world_line_pos.startPos(), world_line_pos.endPos()); + world_line_pos.centerDistance() = NavigationUtil::euclideanDistanceOrientated(robot_world_pos, world_line_pos.centerPos(), robot_yaw_angle, m_sensor_config.line_sensor_mounted_right_to_left); + world_line_points.push_back(world_line_pos); + } + // Sort by ascending distance between robot and line center point (note: centerDistance() is orientated, use std::abs(centerDistance()) for comparison) + std::sort(world_line_points.begin(), world_line_points.end(), [](const LineDetectionResult & a, const LineDetectionResult & b){ return (std::abs(a.centerDistance()) < std::abs(b.centerDistance())); }); + + // Get a list of line center points within the detection zone of the sensor + Barcode barcode; + std::vector sensor_line_points = NavigationUtil::selectLinePointsWithinDetectionZone(world_line_points, robot_world_pos, m_sensor_config.line_sensor_detection_width, 3); + if(m_navigation_state > INITIAL) + { + // Set ols measurement from line center points within the detection zone of the sensor + for(std::vector::iterator iter_line_points = sensor_line_points.begin(); iter_line_points != sensor_line_points.end(); iter_line_points++) + { + ROS_INFO_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "OLS-Simulation: robot_world_pos=(" << std::setprecision(3) << std::fixed << robot_world_pos.x << "," << robot_world_pos.y << "), lcp_pos=(" << iter_line_points->centerPos().x << "," << iter_line_points->centerPos().y << "), lcp_dist=" << iter_line_points->centerDistance() << "), linewidth=" << iter_line_points->lineWidth()); + } + // Detect barcodes + for(std::vector::iterator iter_barcodes = m_barcodes.begin(); barcode.label().empty() && iter_barcodes != m_barcodes.end(); iter_barcodes++) + { + if(iter_barcodes->innerRectWorld().contains(robot_world_pos)) + { + barcode = *iter_barcodes; + ROS_INFO_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "NavigationMapper: Barcode \"" << barcode.label() << "\" detected at robot xy-position on world: (" << std::setprecision(3) << std::fixed << robot_world_pos.x << "," << robot_world_pos.y << ")"); + for(std::vector::iterator iter_line_points = sensor_line_points.begin(); iter_line_points != sensor_line_points.end(); iter_line_points++) + iter_line_points->lineWidth() = barcode.innerRectWorld().width; // Line width within label area of the barcode has max. size + } + } + OLS_Measurement_Simulator::setLines(ols_state, sensor_line_points); + OLS_Measurement_Simulator::setBarcode(ols_state, barcode.labelCode(), barcode.flipped()); + } + + // Error simulation and testing: no line detected for some time (line damaged or barcode entered) => fsm must not hang or loose the track! + if(m_navigation_state > INITIAL && m_error_simulation_burst_no_line_frequency > 0 && m_error_simulation_burst_no_line_duration > 0 + && ros::Time::now() > m_error_simulation_no_line_end + ros::Duration(1/m_error_simulation_burst_no_line_frequency)) + { + m_error_simulation_no_line_start = ros::Time::now(); + m_error_simulation_no_line_end = m_error_simulation_no_line_start + ros::Duration(m_error_simulation_burst_no_line_duration); + } + if(ros::Time::now() >= m_error_simulation_no_line_start && ros::Time::now() < m_error_simulation_no_line_end) + { + sensor_line_points.clear(); + OLS_Measurement_Simulator::setLines(ols_state, sensor_line_points); + } + + // Publish ols measurement (if we're running the simulation) + sick_line_guidance::OLS_Measurement ols_msg = scaleWorldToMeasurement(ols_state); + m_ols_measurement_simulator.SetState(ols_msg); + if(!m_ols_measurement_simulator.getPublishTopic().empty()) + { + m_ols_measurement_sensor.set(ols_msg); + m_ols_measurement_world.set(ols_state); + m_ols_measurement_simulator.schedulePublish(); + ROS_INFO_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "NavigationMapper: publishing lcp[1]=" << std::fixed << std::setprecision(3) << ols_msg.position[1] + << ", width[1]=" << ols_msg.width[1] << " on topic \"" << m_ols_measurement_simulator.getPublishTopic() << "\""); + } + + if(m_visualize) + { + m_map_img.copyTo(m_window_img); + // Draw line center points + for(std::vector::iterator iter_lcp = map_line_points.begin(); iter_lcp != map_line_points.end(); iter_lcp++) + cv::circle(m_window_img, iter_lcp->centerPos(), 3, CV_RGB(0,0,255), cv::FILLED); + for(std::vector::iterator iter_lcp = sensor_line_points.begin(); iter_lcp != sensor_line_points.end(); iter_lcp++) + cv::circle(m_window_img, transformPositionWorldToMap(iter_lcp->centerPos()), 1, CV_RGB(0,255,0), cv::FILLED); + // Draw barcode + if(!barcode.label().empty()) + { + cv::rectangle(m_window_img, barcode.outerRectMap(), CV_RGB(255,128,0), cv::FILLED); + cv::rectangle(m_window_img, barcode.outerRectMap(), CV_RGB(0,0,0), 1); + cv::putText(m_window_img, barcode.label(), cv::Point(barcode.centerMap().x - 24, barcode.centerMap().y + 16), cv::FONT_HERSHEY_SIMPLEX, 0.5, 2); + } + // Draw robot position and status (green: line or barcode detected, red otherwise) + std::string map_color = ImageUtil::isLinePixel(m_map_img, robot_map_pos) ? "black" : "white"; + cv::Scalar robot_color = CV_RGB(255,0,0); // default: display robot position in red color + sick_line_guidance::OLS_Measurement ols_measurement_sensor = m_ols_measurement_sensor.get(); + sick_line_guidance::OLS_Measurement ols_measurement_world = m_ols_measurement_world.get(); + if(!sensor_line_points.empty() || ols_measurement_sensor.barcode > 0 || ols_measurement_sensor.extended_code > 0) + robot_color = CV_RGB(0,255,0); // line or barcode detected: display robot position in green color + // Draw robot + cv::Point2d robot_pos1 = ImageUtil::getWorldPointInDirection(robot_world_pos, robot_yaw_angle - CV_PI/2, m_sensor_config.line_sensor_detection_width/2); + cv::Point2d robot_pos2 = ImageUtil::getWorldPointInDirection(robot_world_pos, robot_yaw_angle + CV_PI/2, m_sensor_config.line_sensor_detection_width/2); + cv::line(m_window_img, transformPositionWorldToMap(robot_pos1), transformPositionWorldToMap(robot_pos2), robot_color, 1); + cv::circle(m_window_img, robot_map_pos, 3, robot_color, cv::FILLED); + // Print ros time and lcp of main line + std::stringstream info; + info << "[" << std::fixed << std::setprecision(9) << ros::WallTime::now().toSec() << ", " << ros::Time::now().toSec() << "]"; + if((ols_measurement_sensor.status&0x2) != 0) + info << ": line[1]: " << std::fixed << std::setprecision(3) << ols_measurement_sensor.position[1] << "," << ols_measurement_sensor.width[1] << " (" << ols_measurement_world.position[1] << "," << ols_measurement_world.width[1] << ")"; + cv::putText(m_window_img, info.str(), cv::Point(20, m_map_img.rows-32), cv::FONT_HERSHEY_SIMPLEX, 0.5, 2); + // Display the navigation map + ROS_INFO_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "NavigationMapper: robot xy-position on map:(" << std::setprecision(1) << std::fixed << robot_map_pos.x << "," << robot_map_pos.y << "," << map_color + << "), linestate:" << (int)(ols_measurement_sensor.status & 0x7) << ((m_navigation_state == FOLLOW_LINE) ? ", follow_line" : "")); + m_video_write.write(m_window_img); + if(!m_window_name.empty()) + { + cv::imshow(m_window_name, m_window_img); + cv::waitKey(1); + } + } + m_message_rate.sleep(); + } +} diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/src/navigation_util.cpp b/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/src/navigation_util.cpp new file mode 100755 index 0000000..5a28c71 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/src/navigation_util.cpp @@ -0,0 +1,264 @@ +/* + * NavigationUtil implements utility functions for 2D/3D-transforms and navigation. + * + * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim + * Copyright (C) 2019 SICK AG, Waldkirch + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of SICK AG nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Authors: + * Michael Lehning + * + * Copyright 2019 SICK AG + * Copyright 2019 Ing.-Buero Dr. Michael Lehning + * + */ +#include +#include +#include +#include +#include +#include +#include + +#include "sick_line_guidance_demo/image_util.h" +#include "sick_line_guidance_demo/navigation_util.h" + +/* + * returns +1, if a point is on the right side of the robot, and -1 otherwise. + * see https://math.stackexchange.com/questions/274712/calculate-on-which-side-of-a-straight-line-is-a-given-point-located + * for the math under the hood. + * @param[in] xpoint x-position of the point + * @param[in] ypoint y-position of the point + * @param[in] xrobot x-position of the robot + * @param[in] yrobot y-position of the robot + * @param[in] heading heading of the robot + * @return +1 (point is on the right side) or -1 (point is on the left side) + */ +int sick_line_guidance_demo::NavigationUtil::isPointRightFromRobot(double xpoint, double ypoint, double xrobot, double yrobot, double heading) +{ + double x1 = xrobot; + double y1 = yrobot; + double x2 = x1 + cos(heading); + double y2 = y1 + sin(heading); + double d = (xpoint - x1) * (y2 - y1) - (ypoint - y1) * (x2 - x1); + return (d > 0) ? +1 : -1; +} + +/* + * computes and return the euclidean distance between between a robot position and a line center point, + * i.e. a negative value if the line_center is left from robots position when moving in robot_heading, and + * a positive value if the line_center is right from robots position when moving in robot_heading + * @param[in] robot_pos first point (robots position) + * @param[in] line_center_pos second point (line center position) + * @param[in] robot_heading robots moving direction (i.e. robots yaw angle) + * @param[in] sensor_mounted_right_to_left Sensor mounted from right to left (true, demo robot configuration) or otherwise from left to right + * @return euclidean distance between a and b + */ +double sick_line_guidance_demo::NavigationUtil::euclideanDistanceOrientated(const cv::Point2d & robot_pos, const cv::Point2d & line_center_pos, double robot_heading, bool sensor_mounted_right_to_left) +{ + double dist = euclideanDistance(robot_pos, line_center_pos); + int sign = isPointRightFromRobot(line_center_pos.x, line_center_pos.y, robot_pos.x, robot_pos.y, robot_heading); + if(!sensor_mounted_right_to_left) + sign = -sign; // sign depends on how the sensor is mounted (orientation relative to heading, cable to the right or to the left), default (demo mounting): true + return sign * dist; +} + +/* + * transforms and returns (x,y) position and yaw angle of an odometry message into a readable string. + * @param[in] msg odometry message (input) + * @return (x,y) position and yaw angle of odometry message as a readable string (output) + */ +std::string sick_line_guidance_demo::NavigationUtil::toInfo(const nav_msgs::Odometry::ConstPtr& msg) +{ + assert(msg); + std::stringstream info; + double posx = 0, posy = 0, yaw = 0; + toWorldPosition(msg, posx, posy, yaw); + info << "nav_msgs::Odometry={pose=(x=" << std::setprecision(3) << std::fixed << posx << ",y=" << std::setprecision(3) << std::fixed << posy + << ",yaw=" << std::setprecision(6) << std::fixed << yaw << "=" << std::setprecision(1) << std::fixed << (180.0 * yaw / M_PI) << " deg)}"; + return info.str(); +} + +/* + * transforms and returns (x,y) position and yaw angle of an odometry message into a readable string. + * @param[in] msg odometry message (input) + * @return (x,y) position and yaw angle of odometry message as a readable string (output) + */ +std::string sick_line_guidance_demo::NavigationUtil::toInfo(const nav_msgs::Odometry & msg) +{ + std::stringstream info; + double posx = 0, posy = 0, yaw = 0; + toWorldPosition(msg, posx, posy, yaw); + info << "nav_msgs::Odometry={pose=(x=" << std::setprecision(3) << std::fixed << posx << ",y=" << std::setprecision(3) << std::fixed << posy + << ",yaw=" << std::setprecision(6) << std::fixed << yaw << "=" << std::setprecision(1) << std::fixed << (180.0 * yaw / M_PI) << " deg)}"; + return info.str(); +} + +/* + * transforms and returns a gazebo ModelStates message into a readable string. + * @param[in] msg gazebo ModelStates message (input) + * @param[in] start_idx index to start iteration of names and poses (input, default = 0: print all names and poses) + * @param[in] last_idx index to end iteration of names and poses (input, default = MAX_INT: print all names and poses) + * @return gazebo ModelStates message as a readable string (output) + */ +std::string sick_line_guidance_demo::NavigationUtil::toInfo(const gazebo_msgs::ModelStates::ConstPtr& msg, size_t start_idx, size_t last_idx) +{ + assert(msg && msg->pose.size() > 1); + std::stringstream info; + // Print ModelStates names + info << "gazebo_msgs::ModelStates={name=["; + for(size_t n = std::max((size_t)0,start_idx); n < std::min(msg->name.size(), last_idx+1); n++) + { + if(n > start_idx) + info << ","; + info << "\"" << msg->name[n] << "\""; + } + // Print ModelStates pose positions + info << "],pose=["; + for(size_t n = std::max((size_t)0,start_idx); n < std::min(msg->pose.size(), last_idx+1); n++) + { + const geometry_msgs::Point & position = msg->pose[n].position; + const geometry_msgs::Quaternion & quat_msg = msg->pose[n].orientation; + double roll=0, pitch=0, yaw=0; + // Print xy-position + if(n > start_idx) + info << ","; + info << "{x=" << std::setprecision(3) << std::fixed << position.x << ",y=" << std::setprecision(3) << std::fixed << position.y; // position.z ignored + // transform orientation to angles: roll (x-axis rotation), pitch (y-axis rotation), yaw (z-axis rotation, i.e. robots orientation in the xy-plane) + transformOrientationToRollPitchYaw(quat_msg, roll, pitch, yaw); + // Print orientation (yaw angle, i.e. robots rotation in xy-plane) + info << ",yaw=" << std::setprecision(6) << std::fixed << yaw << "=" << std::setprecision(1) << std::fixed << (180.0 * yaw / M_PI) << " deg}"; // roll and pitch ignored + } + info << "]}"; + return info.str(); +} + +/* + * gets the robots xy-position and yaw angle from an odometry message + * @param[in] msg odometry message (input) + * @param[out] world_posx x-position in world coordinates [meter] + * @param[out] world_posy y-position in world coordinates [meter] + * @param[out] yaw_angle orientation (z-axis rotation) in rad + */ +void sick_line_guidance_demo::NavigationUtil::toWorldPosition(const nav_msgs::Odometry & msg, double & world_posx, double & world_posy, double & yaw_angle) +{ + world_posx = msg.pose.pose.position.x; + world_posy = msg.pose.pose.position.y; + double roll=0, pitch=0; + transformOrientationToRollPitchYaw(msg.pose.pose.orientation, roll, pitch, yaw_angle); +} + +/* + * gets the robots xy-position and yaw angle from an odometry message + * @param[in] msg odometry message (input) + * @param[out] world_posx x-position in world coordinates [meter] + * @param[out] world_posy y-position in world coordinates [meter] + * @param[out] yaw_angle orientation (z-axis rotation) in rad + */ +void sick_line_guidance_demo::NavigationUtil::toWorldPosition(const nav_msgs::Odometry::ConstPtr& msg, double & world_posx, double & world_posy, double & yaw_angle) +{ + assert(msg); + world_posx = msg->pose.pose.position.x; + world_posy = msg->pose.pose.position.y; + double roll=0, pitch=0; + transformOrientationToRollPitchYaw(msg->pose.pose.orientation, roll, pitch, yaw_angle); +} + +/* + * gets the robots xy-position and yaw angle from a gazebo ModelStates message + * @param[in] msg gazebo ModelStates message (input) + * @param[out] world_posx x-position in world coordinates [meter] + * @param[out] world_posy y-position in world coordinates [meter] + * @param[out] yaw_angle orientation (z-axis rotation) in rad + */ +void sick_line_guidance_demo::NavigationUtil::toWorldPosition(const gazebo_msgs::ModelStates::ConstPtr& msg, double & world_posx, double & world_posy, double & yaw_angle) +{ + assert(msg && msg->pose.size() > 1); + world_posx = msg->pose[1].position.x; + world_posy = msg->pose[1].position.y; + double roll=0, pitch=0; + transformOrientationToRollPitchYaw(msg->pose[1].orientation, roll, pitch, yaw_angle); +} + +/* + * transforms an orientation from Quaternion to angles: roll (x-axis rotation), pitch (y-axis rotation), yaw (z-axis rotation). + * @param[in] quat_msg gazebo orientation (quaternion, input) + * @param[out] roll angle (x-axis rotation, output) + * @param[out] pitch angle (y-axis rotation, output) + * @param[out] yaw angle (z-axis rotation, output) + */ +void sick_line_guidance_demo::NavigationUtil::transformOrientationToRollPitchYaw(const geometry_msgs::Quaternion & quat_msg, double & roll, double & pitch, double & yaw) +{ + tf::Quaternion quat_tf(quat_msg.x, quat_msg.y, quat_msg.z, quat_msg.w); + tf::Matrix3x3 quat_mat(quat_tf); + quat_mat.getRPY(roll, pitch, yaw); +} + +/* + * Selects line points within the sensor detection zone from a list of possible line center points + * @param[in] world_line_points list of possible line center points in world coordinates[meter] + * @param[in] robot_world_pos robot position in world coordinates [meter] + * @param[in] line_sensor_detection_width width of sensor detection zone (f.e. 0.180 for an OLS mounted in 0.1m height over ground, 0.130 for the demo system with OLS mounted in 0.065m height) + * @param[in] max_line_cnt max. number of lines (OLS: max. 3 lines) + * @return list of line points within the sensor detection zone + */ +std::vector sick_line_guidance_demo::NavigationUtil::selectLinePointsWithinDetectionZone(std::vector & world_line_points, + const cv::Point2d & robot_world_pos, double line_sensor_detection_width, size_t max_line_cnt) +{ + std::vector sensor_line_points; + for(std::vector::iterator iter_line_points = world_line_points.begin(); iter_line_points != world_line_points.end(); iter_line_points++) + { + if(std::abs(iter_line_points->centerDistance()) < line_sensor_detection_width/2 // line center point within detection zone + && NavigationUtil::euclideanDistance(robot_world_pos, iter_line_points->startPos()) < line_sensor_detection_width/2 // line start point within detection zone + && NavigationUtil::euclideanDistance(robot_world_pos, iter_line_points->endPos()) < line_sensor_detection_width/2) // line end point within detection zone + // && std::abs(iter_line_points->lineWidth()) < line_sensor_detection_width) // line width within detection zone + { + sensor_line_points.push_back(*iter_line_points); + } + } + if(sensor_line_points.size() > max_line_cnt) + { + sensor_line_points.resize(max_line_cnt); + } + return sensor_line_points; +} diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/src/ols_measurement_simulator.cpp b/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/src/ols_measurement_simulator.cpp new file mode 100755 index 0000000..5f83a8b --- /dev/null +++ b/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/src/ols_measurement_simulator.cpp @@ -0,0 +1,234 @@ +/* + * ols_measurement_simulator simulates OLS_Measurement messages for sick_line_guidance_demo. + * + * OLS_Measurement_Simulator converts the distance between the simulated robot and + * the optical lines from the navigation map into OLS_Measurement messages. + * + * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim + * Copyright (C) 2019 SICK AG, Waldkirch + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of SICK AG nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Authors: + * Michael Lehning + * + * Copyright 2019 SICK AG + * Copyright 2019 Ing.-Buero Dr. Michael Lehning + * + */ +#include +#include +#include +#include "sick_line_guidance/sick_line_guidance_msg_util.h" +#include "sick_line_guidance_demo/image_util.h" +#include "sick_line_guidance_demo/ols_measurement_simulator.h" +#include "sick_line_guidance_demo/time_format.h" + +/* + * class OLS_Measurement_Simulator converts the distance between the simulated robot and + * the optical lines from the navigation map into OLS_Measurement messages. + */ + +/* + * Constructor + * @param[in] ros_topic_ols_messages ros topic for publishing OLS messages (empty: deactivated) + * @param[in] publish_rate rate to publish OLS measurements (if activated, default: 100) + */ +sick_line_guidance_demo::OLS_Measurement_Simulator::OLS_Measurement_Simulator(ros::NodeHandle* nh, const std::string & ros_topic_ols_messages, double publish_rate) +: m_ols_publish_thread(0), m_ols_publish_rate(publish_rate) +{ + m_ros_topic_ols_messages = ros_topic_ols_messages; + m_publish_scheduled = false; + sick_line_guidance::MsgUtil::zero(m_ols_state); + if(nh && !m_ros_topic_ols_messages.empty()) + { + m_ols_publish_rate = ros::Rate(publish_rate); + m_ols_publisher = nh->advertise(m_ros_topic_ols_messages, 1); + m_ols_publish_thread = new boost::thread(&sick_line_guidance_demo::OLS_Measurement_Simulator::runPublishThread, this); + ROS_INFO_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "OLS_Measurement_Simulator: publishing \"" << m_ros_topic_ols_messages << "\""); + } + else + { + ROS_INFO_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "OLS_Measurement_Simulator: deactivated (ols topic: \"" << m_ros_topic_ols_messages << "\""); + } +} + +/* + * Destructor + */ +sick_line_guidance_demo::OLS_Measurement_Simulator::~OLS_Measurement_Simulator() +{ +} + +/* + * publish the current OLS state + */ +void sick_line_guidance_demo::OLS_Measurement_Simulator::publish(void) +{ + if(!m_ros_topic_ols_messages.empty()) + { + m_ols_state.header.stamp = ros::Time::now(); + m_ols_publisher.publish(m_ols_state); + } +} + +/* + * publish the current OLS state in background task (publish with fixed rate) + */ +void sick_line_guidance_demo::OLS_Measurement_Simulator::schedulePublish(void) +{ + if(!m_ros_topic_ols_messages.empty()) + { + m_publish_scheduled = true; + } +} + +/* + * thread callback, just publishes the current OLS state with a const rate + */ +void sick_line_guidance_demo::OLS_Measurement_Simulator::runPublishThread(void) +{ + while(ros::ok()) + { + if(!m_ros_topic_ols_messages.empty() && m_publish_scheduled) + { + publish(); + m_publish_scheduled = false; + } + m_ols_publish_rate.sleep(); + } +} + +/* + * rounds a double value to a given float precision, f.e. roundPrecision(lcp, 0.001) to round a line center point to millimeter precision. + */ +float sick_line_guidance_demo::OLS_Measurement_Simulator::roundPrecision(double value, double precision) +{ + return (float)(precision * std::lround(value / precision)); +} + +/* + * Initializes ols_state for one line (position, width and status) + */ +void sick_line_guidance_demo::OLS_Measurement_Simulator::setLine(sick_line_guidance::OLS_Measurement & ols_state, float line_position, float line_width) +{ + ols_state.position = { 0, line_position, 0 }; + ols_state.width = { 0, line_width, 0 }; + ols_state.intensity_of_lines = { 0, 100, 0 }; + ols_state.quality_of_lines = 100; + ols_state.status = 0x2; +} + +/* + * Initializes ols_state for detected lines (position, width and status for 0, 1, 2 or 3 lines) + */ +void sick_line_guidance_demo::OLS_Measurement_Simulator::setLines(sick_line_guidance::OLS_Measurement & ols_state, std::vector & sensor_line_points) +{ + if(sensor_line_points.size() <= 0) // No line detected + { + ols_state.position = { 0, 0, 0 }; + ols_state.width = { 0, 0, 0 }; + ols_state.intensity_of_lines = { 0, 0, 0 }; + ols_state.status = 0; + ols_state.quality_of_lines = 0; + } + else + { + // First (main) line: ols_state.position[1] is always the nearest line, ols_state.position[0] is the line to the left, ols_state.position[2] is the line to the right (in driving direction) + ols_state.position = { 0, roundPrecision(sensor_line_points[0].centerDistance(), 0.001), 0 }; + ols_state.width = { 0, roundPrecision(sensor_line_points[0].lineWidth() + 0.001, 0.001), 0 }; + ols_state.intensity_of_lines = { 0, 100, 0 }; + ols_state.status &= 0xF8; + ols_state.status |= 0x2; + ols_state.quality_of_lines = 100; + if(sensor_line_points.size() > 1) // two or three lines detected -> initialize positions and status depending on the side of the line (left side, right side or both) + { + // Sort from left to right + std::vector line_points_left_to_right; + line_points_left_to_right.push_back(sensor_line_points[1]); // second line + if(sensor_line_points.size() > 2) + { + line_points_left_to_right.push_back(sensor_line_points[2]); // third line + std::sort(line_points_left_to_right.begin(), line_points_left_to_right.end(), [](const LineDetectionResult & a, const LineDetectionResult & b){ return (a.centerDistance() < b.centerDistance()); }); + } + for(std::vector::iterator iter_line = line_points_left_to_right.begin(); iter_line < line_points_left_to_right.end(); iter_line++) + { + int sensor_line_idx = (iter_line->centerDistance() < 0 ? 0 : 2); // OLS: sensor_line_idx==0: line to the left, sensor_line_idx==2: line to the right + int sensor_line_status = (sensor_line_idx == 0 ? 1 : 4); // OLS: (line status & 1) != 0: line to the left, (line status & 4) != 0: line to the left + if((ols_state.status & sensor_line_status) == 0) + { + ols_state.position[sensor_line_idx] = roundPrecision(iter_line->centerDistance(), 0.001); + ols_state.width[sensor_line_idx] = roundPrecision(iter_line->lineWidth() + 0.001, 0.001); + ols_state.intensity_of_lines[sensor_line_idx] = 100; + ols_state.status |= sensor_line_status; + } + } + } + } +} + +/* + * Sets the barcode of an ols_state + */ +void sick_line_guidance_demo::OLS_Measurement_Simulator::setBarcode(sick_line_guidance::OLS_Measurement & ols_state, size_t label, bool flipped) +{ + if(label > 255) + { + ols_state.barcode = 255; + ols_state.extended_code = label; + } + else + { + ols_state.barcode = label; + ols_state.extended_code = 0; + } + // status Bit 6: Code flipped, Bit 7: Code valid + if(flipped) + ols_state.status |= (1 << 6); + else + ols_state.status &= ~(1 << 6); + if(ols_state.barcode > 0) + ols_state.status |= (1 << 7); + else + ols_state.status &= ~(1 << 7); +} + diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/src/pid.cpp b/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/src/pid.cpp new file mode 100755 index 0000000..56d140d --- /dev/null +++ b/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/src/pid.cpp @@ -0,0 +1,62 @@ +#include "sick_line_guidance_demo/pid.h" +//using namespace std; + +/* Object contstructor */ +PID_Controller::PID_Controller(double dt, double kp, double ki, double kd){ + _dt = dt; + _kp = kp; + _ki = ki; + _kd = kd; +} + +/* Function to calculate controller output */ +double PID_Controller::calculate_output(double setpoint_value, double actual_value){ + _e_z = _e; // store old deviation in e_z (e*z⁻1) + _e = setpoint_value - actual_value; // calculate deviation + _e_int += _e; // integrate + _e_diff = _e - _e_z; //derivate + + _yp = _kp * _e; // calculate P-Controller output + _yi = _ki * _dt * _e_int; // calculate I-Controller output + _yd = _kd * _e_diff/_dt; // calculate D-Controller output + + return (_yp + _yi + _yd); // calculate PID-Controller output +} + +/* Function to set controller parameter */ +void PID_Controller::setParams(double dt, double kp, double ki, double kd){ + _dt = dt; + _kp = kp; + _ki = ki; + _kd = kd; +} + +void PID_Controller::getParams(double& dt, double& kp, double& ki, double& kd){ + dt = _dt; + kp = _kp; + ki = _ki; + kd = _kd; +} + +void PID_Controller::reset(){ + _dt = 0; + _kp = 0; + _ki = 0; + _kd = 0; + _e = 0; + _e_int = 0; + _e_diff = 0; + _e_z = 0; + _yp = 0; + _yi = 0; + _yd = 0; +} + +void PID_Controller::clear_state(){ + double params[4]; + getParams(params[0], params[1], params[2], params[3]); + reset(); + setParams(params[0], params[1], params[2], params[3]); +} + +PID_Controller::~PID_Controller(){} \ No newline at end of file diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/src/robot_fsm.cpp b/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/src/robot_fsm.cpp new file mode 100755 index 0000000..383264e --- /dev/null +++ b/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/src/robot_fsm.cpp @@ -0,0 +1,221 @@ +/* + * RobotFSM implements the state machine to explore and follow a line + * for sick_line_guidance_demo. + * The following RobotStates are executed: + * INITIAL -> EXPLORE_LINE -> FOLLOW_LINE [ -> WAIT_AT_BARCODE -> FOLLOW_LINE ] -> EXIT + * Theses states are implemented in the following classes: + * EXPLORE_LINE: ExploreLineState + * FOLLOW_LINE: FollowLineState + * WAIT_AT_BARCODE: WaitAtBarcodeState + * + * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim + * Copyright (C) 2019 SICK AG, Waldkirch + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of SICK AG nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Authors: + * Michael Lehning + * + * Copyright 2019 SICK AG + * Copyright 2019 Ing.-Buero Dr. Michael Lehning + * + */ +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "sick_line_guidance/sick_line_guidance_msg_util.h" +#include "sick_line_guidance_demo/explore_line_state.h" +#include "sick_line_guidance_demo/follow_line_state.h" +#include "sick_line_guidance_demo/navigation_util.h" +#include "sick_line_guidance_demo/robot_fsm.h" +#include "sick_line_guidance_demo/stop_go_fsm.h" +#include "sick_line_guidance_demo/time_format.h" + +/* + * class RobotFSM implements the state machine to explore and follow a line + * for sick_line_guidance_demo. + * Input: ols and odometry messages + * Output: cmd_vel messages + */ + +/* + * Constructor + * @param[in] nh ros handle + * @param[in] ros_topic_ols_messages ROS topic for OLS_Measurement messages (input) + * @param[in] ros_topic_odometry ROS topic for odometry incl. robot positions (input) + * @param[in] ros_topic_cmd_vel ROS topic for cmd_vel messages (output) + */ +sick_line_guidance_demo::RobotFSM::RobotFSM(ros::NodeHandle* nh, const std::string & ros_topic_ols_messages, const std::string & ros_topic_odometry, const std::string & ros_topic_cmd_vel) +: m_fsm_thread(0), m_fsm_thread_run(false) +{ + if(nh && !ros_topic_ols_messages.empty()) + { + m_ols_subscriber = nh->subscribe(ros_topic_ols_messages, 1, &sick_line_guidance_demo::RobotFSM::messageCallbackOlsMeasurement, this); + ROS_INFO_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "RobotFSM: subscribing " << ros_topic_ols_messages); + } + if(nh && !ros_topic_odometry.empty()) + { + m_odom_subscriber = nh->subscribe(ros_topic_odometry, 1, &sick_line_guidance_demo::RobotFSM::messageCallbackOdometry, this); + ROS_INFO_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "RobotFSM: subscribing " << ros_topic_odometry); + } + if(nh && !ros_topic_cmd_vel.empty()) + { + m_cmd_vel_publisher = nh->advertise(ros_topic_cmd_vel, 1); + m_fsm_context.setVelocityPublisher(&m_cmd_vel_publisher); + ROS_INFO_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "RobotFSM: publishing " << m_cmd_vel_publisher.getTopic()); + } + if(nh) + { + m_follow_line = FollowLineState(nh, &m_fsm_context); + m_explore_line = ExploreLineState(nh, &m_fsm_context); + wait_at_barcode_state = WaitAtBarcodeState(nh, &m_fsm_context); + } +} + +/* + * Destructor + */ +sick_line_guidance_demo::RobotFSM::~RobotFSM() +{ + m_fsm_context.setVelocityPublisher(0); +} + +/* + * Start thread to run the final state machine. Read messages form ols and odom topics, publish messages to cmd_vel + */ +void sick_line_guidance_demo::RobotFSM::startFSM(void) +{ + // Stop FSM and start a new thread to run the FSM + stopFSM(); + m_fsm_thread_run = true; + m_fsm_thread = new boost::thread(&sick_line_guidance_demo::RobotFSM::runFSMthread, this); +} + +/* + * Stops the thread to run the final state machine + */ +void sick_line_guidance_demo::RobotFSM::stopFSM(void) +{ + if(m_fsm_thread) + { + m_fsm_thread_run = false; + m_fsm_thread->join(); + delete(m_fsm_thread); + m_fsm_thread = 0; + } +} + +/* + * message callback for odometry messages. This function is called automatically by the ros::Subscriber after subscription of topic "/odom". + * It transforms the robots xy-positions from world/meter into map/pixel position, detect lines and barcodes in the map plus their distance to the robot, + * and transform them invers into world coordinates. + * @param[in] msg odometry message (input) + */ +void sick_line_guidance_demo::RobotFSM::messageCallbackOdometry(const nav_msgs::Odometry::ConstPtr& msg) +{ + m_fsm_context.setOdomState(msg); +} + +/* + * message callback for OLS measurement messages. This function is called automatically by the ros::Subscriber after subscription of topic "/ols". + * It displays the OLS measurement (line info and barcodes), if visualization is enabled. + * @param[in] msg OLS measurement message (input) + */ +void sick_line_guidance_demo::RobotFSM::messageCallbackOlsMeasurement(const boost::shared_ptr& msg) +{ + m_fsm_context.setOlsState(msg); +} + +/* + * thread callback, runs the final state machine for sick_line_guidance_demo. + * Input: ols and odometry messages + * Output: cmd_vel messages + */ +void sick_line_guidance_demo::RobotFSM::runFSMthread(void) +{ + try + { + RobotFSMContext::RobotState robot_state = RobotFSMContext::RobotState::INITIAL; + while(ros::ok() && m_fsm_thread_run && robot_state != RobotFSMContext::RobotState::EXIT && robot_state != RobotFSMContext::RobotState::FATAL_ERROR) + { + switch(robot_state) + { + case RobotFSMContext::RobotState::INITIAL: + robot_state = RobotFSMContext::RobotState::EXPLORE_LINE; + break; + case RobotFSMContext::RobotState::EXPLORE_LINE: + m_explore_line.clear(); + robot_state = m_explore_line.run(); + break; + case RobotFSMContext::RobotState::FOLLOW_LINE: + m_follow_line.clear(); + robot_state = m_follow_line.run(); + break; + case RobotFSMContext::RobotState::WAIT_AT_BARCODE: + wait_at_barcode_state.clear(); + robot_state = wait_at_barcode_state.run(); + break; + case RobotFSMContext::RobotState::FATAL_ERROR: + ROS_ERROR_STREAM("sick_line_guidance_demo::RobotFSM::runFSMthread(): entered state FATAL_ERROR, exiting"); + break; + case RobotFSMContext::RobotState::EXIT: + ROS_INFO_STREAM("sick_line_guidance_demo::RobotFSM::runFSMthread(): switched to state EXIT, exiting"); + break; + default: + ROS_ERROR_STREAM("sick_line_guidance_demo::RobotFSM::runFSMthread(): entered unsupported state " << (int)robot_state); + break; + } + } + ROS_INFO_STREAM("sick_line_guidance_demo::RobotFSM::runFSMthread(): entered state " << (int)robot_state << ", exiting"); + } + catch(const std::exception & exc) + { + std::cerr << "sick_line_guidance_demo::RobotFSM::runFSMthread(): exception \"" << exc.what() << "\"" << std::endl; + } +} diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/src/sick_line_guidance_demo_node.cpp b/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/src/sick_line_guidance_demo_node.cpp new file mode 100755 index 0000000..33a0aa4 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/src/sick_line_guidance_demo_node.cpp @@ -0,0 +1,139 @@ +/* + * @brief sick_line_guidance_demo_node simulates the sick_line_guidance demo application. + * It receives robots (x,y) positions, maps them with the navigation map and creates + * OLS measurement messages for iam::robot_fsm. + * + * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim + * Copyright (C) 2019 SICK AG, Waldkirch + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of SICK AG nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Authors: + * Michael Lehning + * + * Copyright 2019 SICK AG + * Copyright 2019 Ing.-Buero Dr. Michael Lehning + * + */ +#include +#include +#include +#include "sick_line_guidance_demo/navigation_mapper.h" +#include "sick_line_guidance_demo/robot_fsm.h" +#include "sick_line_guidance_demo/time_format.h" +#include "sick_line_guidance_demo/turtlebot_test_fsm.h" + +/* + * @brief sick_line_guidance_demo_node simulates the sick_line_guidance demo application. + * It receives robots (x,y) positions, maps them with the navigation map and creates + * OLS measurement messages for iam::robot_fsm. + */ +int main(int argc, char** argv) +{ + ros::init(argc, argv, "sick_line_guidance_demo_node"); + + // Configuration + ros::NodeHandle nh; + sick_line_guidance_demo::LineSensorConfig sensor_config; + + int ols_simu = 0; // Simulate ols measurement messages + int visualize = 0; // Enable visualization: visualize==2: visualization+video enabled, map and navigation plots are displayed in a window, visualize==1: video created but not displayed, visualize==0: visualization and video disabled + std::string ros_topic_odometry = "/odom"; // ROS topic for odometry incl. robot positions (input) + std::string ros_topic_ols_messages = "/ols"; // ROS topic for OLS_Measurement messages (simulation: input+output, otherwise input) + std::string ros_topic_cmd_vel = "/cmd_vel"; // ROS topic for cmd_vel messages (output) + std::string navigation_map_file = "demo_map_02.png"; // Navigation map (input) + std::string intrinsics_xml_file = "cam_intrinsic.xml"; // Intrinsics parameter to transform position from navigation map (pixel) to real world (meter) and vice versa (input, cx=cy=660, fx=fy=1) + std::string barcodes_xml_file = "demo_barcodes.xml"; // List of barcodes with label and position (input) + sensor_config.line_sensor_detection_width = 0.130; // Width of the detection area of the line sensor (meter) + sensor_config.line_sensor_scaling_dist = 180.0 / 133.0; // Scaling between physical distance to the line center and measured line center point (measurement: lcp = 180 mm, physical: lcp = 133 mm), depending on mounted sensor height + sensor_config.line_sensor_scaling_width = 29.0 / 20.0; // Scaling between physical line width (20 mm) and measured line width (29 mm) depending on mounted sensor height (sensor mounted 100 mm over ground: scaling = 1, sensor mounted 65 mm over ground: scaling = 100/65 = 1.5) + sensor_config.line_sensor_mounted_right_to_left = true; // Sensor mounted from right to left (true, demo robot configuration) or otherwise from left to right + + nh.param("/sick_line_guidance_demo_node/ols_simu", ols_simu, ols_simu); + nh.param("/sick_line_guidance_demo_node/visualize", visualize, visualize); + nh.param("/sick_line_guidance_demo_node/ros_topic_odometry", ros_topic_odometry, ros_topic_odometry); + nh.param("/sick_line_guidance_demo_node/ros_topic_ols_messages", ros_topic_ols_messages, ros_topic_ols_messages); + nh.param("/sick_line_guidance_demo_node/ros_topic_cmd_vel", ros_topic_cmd_vel, ros_topic_cmd_vel); + nh.param("/sick_line_guidance_demo_node/navigation_map_file", navigation_map_file, navigation_map_file); + nh.param("/sick_line_guidance_demo_node/intrinsics_xml_file", intrinsics_xml_file, intrinsics_xml_file); + nh.param("/sick_line_guidance_demo_node/barcodes_xml_file", barcodes_xml_file, barcodes_xml_file); + nh.param("/sick_line_guidance_demo_node/line_sensor_detection_width", sensor_config.line_sensor_detection_width, sensor_config.line_sensor_detection_width); + nh.param("/sick_line_guidance_demo_node/line_sensor_scaling_dist", sensor_config.line_sensor_scaling_dist, sensor_config.line_sensor_scaling_dist); + nh.param("/sick_line_guidance_demo_node/line_sensor_scaling_width", sensor_config.line_sensor_scaling_width, sensor_config.line_sensor_scaling_width); + nh.param("/sick_line_guidance_demo_node/line_sensor_mounted_right_to_left", sensor_config.line_sensor_mounted_right_to_left, sensor_config.line_sensor_mounted_right_to_left); + + // Create navigation mapper (transform the robots xy-positions from world/meter into map/pixel position, + // detect lines and barcodes in the map plus their distance to the robot and transform them invers into world coordinates) + sick_line_guidance_demo::NavigationMapper navigation_mapper(&nh, navigation_map_file, intrinsics_xml_file, barcodes_xml_file, ols_simu ? ros_topic_ols_messages : "", sensor_config, visualize); + + // Final state machine (FSM) to explore and follow a line. Input: ols and odometry messages, Output: cmd_vel messages + sick_line_guidance_demo::RobotFSM robot_fsm(&nh, ros_topic_ols_messages, ros_topic_odometry, ros_topic_cmd_vel /* + "_synth" */ ); + + // Subscribe topics and install callbacks + ros::Subscriber odom_message_subscriber = nh.subscribe(ros_topic_odometry, 1, &sick_line_guidance_demo::NavigationMapper::messageCallbackOdometry, &navigation_mapper); + ros::Subscriber ols_message_subscriber = nh.subscribe(ros_topic_ols_messages, 1, &sick_line_guidance_demo::NavigationMapper::messageCallbackOlsMeasurement, &navigation_mapper); + ROS_INFO_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "sick_line_guidance_demo: subscribing " << odom_message_subscriber.getTopic() << " and " << ols_message_subscriber.getTopic()); + + // Start FSM to explore and follow a line + navigation_mapper.start(); + robot_fsm.startFSM(); + + // Test only: Run a final state machine (FSM) to test Turtlebot with changing velocities, output: geometry_msgs::Twist messages on topic "/cmd_vel" + // sick_line_guidance_demo::TurtlebotTestFSM turtlebot_test_fsm(&nh, "/ols", "/odom", "/cmd_vel", false); + // ROS_INFO_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "sick_line_guidance_demo: starting turtlebot_test_fsm_node"); + // turtlebot_test_fsm.startFSM(); // test only, do not use for demos + + // Run ros event loop + ros::spin(); + + // Exit + std::cout << "sick_line_guidance_demo: exiting (1/2)" << std::endl; + ROS_INFO_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "sick_line_guidance_demo: exiting (1/2)"); + navigation_mapper.stop(); + robot_fsm.stopFSM(); + // turtlebot_test_fsm.stopFSM(); + std::cout << "sick_line_guidance_demo: exiting (2/2)" << std::endl; + ROS_INFO_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "sick_line_guidance_demo: exiting (2/2)"); + + return 0; +} + diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/src/sick_line_guidance_watchdog.cpp b/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/src/sick_line_guidance_watchdog.cpp new file mode 100755 index 0000000..a81d495 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/src/sick_line_guidance_watchdog.cpp @@ -0,0 +1,239 @@ +/* + * @brief sick_line_guidance_watchdog implements a watchdog. + * If no line or barcode is detected for some time (after a watchdog timeout, f.e. 1 second), + * a command or executable is started (f.e. an emergency script to stop a turtlebot + * by killing all nodes). + * Watchdog timeout in seconds and command can be configured in the launchfile. + * Default values are 1 second watchdog timeout and a watchdog command "nohup rosnode kill -a &". + * + * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim + * Copyright (C) 2019 SICK AG, Waldkirch + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of SICK AG nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Authors: + * Michael Lehning + * + * Copyright 2019 SICK AG + * Copyright 2019 Ing.-Buero Dr. Michael Lehning + * + */ +#include +#include +#include +#include +#include +#include "sick_line_guidance/OLS_Measurement.h" +#include "sick_line_guidance_demo/navigation_util.h" + +namespace sick_line_guidance_demo +{ + /* + * class Watchdog implements an emergency kill in case lines and barcodes have been lost for more than 1 second + */ + class Watchdog + { + public: + + /* + * Constructor + * @param[in] command watchdog command, executed in case of watchdog timeouts, f.e. "nohup rosnode kill -a" + * @param[in] timeout watchdog timeout in seconds, f.e. 1 second + * @param[in] check_rate rate to check OLS line detected messages, default: 10 per second + * @param[in] barcode_height height of barcode (i.e. area without valid ols line), default: 0.055 (55 mm) + */ + Watchdog(const std::string & command = "", double timeout = 0, double check_rate = 10, double barcode_height = 0.055) + : m_watchdog_command(command), m_watchdog_timeout(timeout), m_check_rate(check_rate), m_barcode_height(barcode_height) + { + if(timeout > FLT_EPSILON) + { + m_check_rate = ros::Rate(check_rate); + m_time_line_detected = ros::Time::now(); + m_time_ols_message = ros::Time::now(); + m_time_odom_message = ros::Time::now(); + m_pos_line_detected = cv::Point2d(); + m_pos_odom_message = cv::Point2d(); + m_watchdog_thread = new boost::thread(&sick_line_guidance_demo::Watchdog::watchdogThreadCb, this); + } + } + + /* + * Message callback for ols messages. Updates the watchdog, if a line is detected ((ols_msg->status & 0x7) != 0) + * or a barcode has been read ( + * @param[in] ols_msg ols message with line info + */ + virtual void olsMessageCb(const boost::shared_ptr& ols_msg) + { + if(ols_msg) + m_time_ols_message = ros::Time::now(); + if(ols_msg != 0 && (ols_msg->status & 0x7) != 0) // bit 0,1 or 2 of status is set -> line detected + { + m_time_line_detected = m_time_ols_message; + m_pos_line_detected = m_pos_odom_message; + ROS_DEBUG_STREAM("sick_line_guidance_watchdog: ols message received, line detected (ols status: " << (int)ols_msg->status << ")" ); + } + else if(ols_msg != 0 && (ols_msg->status & 0x80) != 0) // bit 7 of status is set -> barcode valid + { + m_time_line_detected = m_time_ols_message; + m_pos_line_detected = m_pos_odom_message; + ROS_DEBUG_STREAM("sick_line_guidance_watchdog: ols message received, line detected (ols status: " << (int)ols_msg->status << ")" ); + } + else + { + if(ols_msg != 0) + ROS_ERROR_STREAM("sick_line_guidance_watchdog: ols message received, no line detected (ols status: " << (int)ols_msg->status << ")" ); + else + ROS_ERROR_STREAM("sick_line_guidance_watchdog: invalid ols message received" ); + } + } + + + /* + * Message callback for odometry messages. Updates the watchdog, whenever an odom message is received + * or a barcode has been read ( + * @param[in] odom_msg odometry message + */ + virtual void odomMessageCb(const nav_msgs::Odometry::ConstPtr& odom_msg) + { + if(odom_msg) + { + m_time_odom_message = ros::Time::now(); + double posx=0, posy=0, yaw = 0; + sick_line_guidance_demo::NavigationUtil::toWorldPosition(odom_msg, posx, posy, yaw); + m_pos_odom_message = cv::Point2d(posx, posy); + } + } + + protected: + + /* + * Thread callback, monitors the time of last line or barcode detection and calls the timeout command in case of a watchdog timeout. + */ + void watchdogThreadCb(void) + { + ROS_INFO_STREAM("sick_line_guidance_watchdog: started" ); + while(ros::ok()) + { + ROS_DEBUG_STREAM("sick_line_guidance_watchdog: running" ); + if((ros::Time::now() - m_time_ols_message).toSec() > m_watchdog_timeout) + { + ROS_ERROR_STREAM("sick_line_guidance_watchdog: WATCHDOG TIMEOUT, no ols message received." ); + break; + } + if((ros::Time::now() - m_time_odom_message).toSec() > m_watchdog_timeout) + { + ROS_ERROR_STREAM("sick_line_guidance_watchdog: WATCHDOG TIMEOUT, no odom message received." ); + break; + } + if((ros::Time::now() - m_time_line_detected).toSec() > m_watchdog_timeout + && NavigationUtil::euclideanDistance(m_pos_odom_message, m_pos_line_detected) > m_barcode_height) + { + ROS_ERROR_STREAM("sick_line_guidance_watchdog: WATCHDOG TIMEOUT, no line detected." ); + break; + } + m_check_rate.sleep(); + } + if(ros::ok() && !m_watchdog_command.empty()) + { + m_watchdog_command += " &"; + ROS_ERROR_STREAM("sick_line_guidance_watchdog: WATCHDOG TIMEOUT, calling system(\"" << m_watchdog_command << "\") ..." ); + system(m_watchdog_command.c_str()); + } + ROS_INFO_STREAM("sick_line_guidance_watchdog: finished" ); + } + + std::string m_watchdog_command; // watchdog command, executed in case of watchdog timeouts, default: "nohup rosnode kill -a" + double m_watchdog_timeout; // watchdog timeout in seconds, default: 1 second + ros::Rate m_check_rate; // rate to check OLS line detected messages, default: 10 per second + ros::Time m_time_line_detected; // timestamp of last detected line or barcode + cv::Point2d m_pos_line_detected; // robot position (world coordinates in meter) at last detected line or barcode + ros::Time m_time_ols_message; // timestamp of last ols message + ros::Time m_time_odom_message; // timestamp of last odom message + cv::Point2d m_pos_odom_message; // robot position (world coordinates in meter) at last odom message + double m_barcode_height; // height of barcode (55 mm, area without valid ols line) + boost::thread* m_watchdog_thread; // thread to monitor time of last line/barcode detection, calls the watchdog command in case of watchdog timeouts + }; +} + +/* + * @brief sick_line_guidance_watchdog subscribes to topic "/ols", monitors the detection of lines and barcodes + * and calls an emergency stop in case of a watchdog timeout. + * Default configuration: system command "nohup rosnode kill -a" (kill all ros nodes) is called + * if no line could be detected for more than 1 second. + * Usage example: "roslaunch sick_line_guidance_demo sick_line_guidance_watchdog.launch" + */ +int main(int argc, char** argv) +{ + ros::init(argc, argv, "sick_line_guidance_watchdog"); + + // Configuration + ros::NodeHandle nh; + std::string ols_topic = "/ols"; // ROS topic for OLS_Measurement messages + std::string odom_topic = "/odom"; // ROS topic for odometry messages + std::string watchdog_command = "nohup rosnode kill -a"; // watchdog command, executed in case of watchdog timeouts + double watchdog_timeout = 1.0; // watchdog timeout in seconds + double watchdog_check_frequency = 10; // rate to check OLS messages + double barcode_height = 0.055; // height of barcode (55 mm, area without valid ols line) + nh.param("/sick_line_guidance_watchdog/ols_topic", ols_topic, ols_topic); + nh.param("/sick_line_guidance_watchdog/odom_topic", odom_topic, odom_topic); + nh.param("/sick_line_guidance_watchdog/watchdog_timeout", watchdog_timeout, watchdog_timeout); + nh.param("/sick_line_guidance_watchdog/watchdog_check_frequency", watchdog_check_frequency, watchdog_check_frequency); + nh.param("/sick_line_guidance_watchdog/barcode_height", barcode_height, barcode_height); + nh.param("/sick_line_guidance_watchdog/watchdog_command", watchdog_command, watchdog_command); + + // Subscribe topic and install callbacks + sick_line_guidance_demo::Watchdog watchdog(watchdog_command, watchdog_timeout, watchdog_check_frequency, barcode_height); + ros::Subscriber ols_message_subscriber = nh.subscribe(ols_topic, 1, &sick_line_guidance_demo::Watchdog::olsMessageCb, &watchdog); + ros::Subscriber odom_message_subscriber = nh.subscribe(odom_topic, 1, &sick_line_guidance_demo::Watchdog::odomMessageCb, &watchdog); + ROS_INFO_STREAM("sick_line_guidance_watchdog: subscribing " << ols_message_subscriber.getTopic()); + ROS_INFO_STREAM("sick_line_guidance_watchdog: subscribing " << odom_message_subscriber.getTopic()); + + // Run ros event loop + ros::spin(); + + // Exit + std::cout << "sick_line_guidance_watchdog: exiting..." << std::endl; + ROS_INFO_STREAM("sick_line_guidance_watchdog: exiting..."); + + return 0; +} + diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/src/stop_go_fsm.cpp b/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/src/stop_go_fsm.cpp new file mode 100755 index 0000000..41516a8 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/src/stop_go_fsm.cpp @@ -0,0 +1,135 @@ +/* + * StopGoVelocityFSM implements a simple state machine, creating cmd_vel messages + * to drive a TurtleBot in stop and go. + * + * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim + * Copyright (C) 2019 SICK AG, Waldkirch + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of SICK AG nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Authors: + * Michael Lehning + * + * Copyright 2019 SICK AG + * Copyright 2019 Ing.-Buero Dr. Michael Lehning + * + */ +#include +#include +#include +#include + +#include "sick_line_guidance_demo/stop_go_fsm.h" +#include "sick_line_guidance_demo/time_format.h" + +/* + * class StopGoVelocityFSM implements a simple state machine, creating cmd_vel messages + * to drive a TurtleBot in stop and go. + */ + +/* + * Constructor + */ +sick_line_guidance_demo::StopGoVelocityFSM::StopGoVelocityFSM() +{ + // define the state machine + m_state_cnt = 0; + m_next_state_switch = ros::Time(0); + VelocityState state; + state.cmd_vel.linear.x = 0.1; + state.cmd_vel.linear.y = 0; + state.cmd_vel.linear.z = 0; + state.cmd_vel.angular.x = 0; + state.cmd_vel.angular.y = 0; + state.cmd_vel.angular.z = -0.5 * M_PI; + // 1. Increase angular velocity from -PI/2 to +PI/2 per second in 0.03*PI steps each 50 ms + while(state.cmd_vel.angular.z <= 0.5 * M_PI) + { + m_vec_vel_states.push_back(state); + m_vec_vel_states.back().duration = ros::Duration(0.05); + state.cmd_vel.angular.z += (0.03 * M_PI); + } + // 2. hold last state for 1 second + m_vec_vel_states.push_back(state); + m_vec_vel_states.back().duration = ros::Duration(1.0); + // 3. stop for 3 seconds + state.cmd_vel.linear.x = 0.0; + state.cmd_vel.angular.z = 0.0; + m_vec_vel_states.push_back(state); + m_vec_vel_states.back().duration = ros::Duration(3.0); + m_vec_vel_states.back().cmd_vel.linear.x = 0.00; + m_vec_vel_states.back().cmd_vel.angular.z = 0.00; +} + +/* + * Destructor + */ +sick_line_guidance_demo::StopGoVelocityFSM::~StopGoVelocityFSM() +{ +} + +/* + * Next cycle, internal state is updated, velocity message may switch to next movement + */ +void sick_line_guidance_demo::StopGoVelocityFSM::update(void) +{ + ros::Time cur_time = ros::Time::now(); + if(!m_next_state_switch.isValid()) + { + m_next_state_switch = cur_time + m_vec_vel_states[m_state_cnt].duration; + } + if(cur_time >= m_next_state_switch) + { + m_state_cnt = ((m_state_cnt + 1) % (m_vec_vel_states.size())); + m_next_state_switch = cur_time + m_vec_vel_states[m_state_cnt].duration; + ROS_INFO_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "StopGoVelocityFSM: state switched, cmd_vel: linear.x=" + << std::fixed << std::setprecision(3) << (m_vec_vel_states[m_state_cnt].cmd_vel.linear.x) << ", angular.z=" << (m_vec_vel_states[m_state_cnt].cmd_vel.angular.z/M_PI) << "*PI"); + } +} + +/* + * Returns the cmd_vel message for the current movement + */ +geometry_msgs::Twist sick_line_guidance_demo::StopGoVelocityFSM::getVelocity(void) +{ + return m_vec_vel_states[m_state_cnt].cmd_vel; +} diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/src/turtlebot_test_fsm.cpp b/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/src/turtlebot_test_fsm.cpp new file mode 100755 index 0000000..1b74b43 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/src/turtlebot_test_fsm.cpp @@ -0,0 +1,217 @@ +/* + * TurtlebotTestFSM implements a state machine to generate cmd_vel messages + * with varying velocities to test the motor control of a TurtleBot. + * + * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim + * Copyright (C) 2019 SICK AG, Waldkirch + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of SICK AG nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Authors: + * Michael Lehning + * + * Copyright 2019 SICK AG + * Copyright 2019 Ing.-Buero Dr. Michael Lehning + * + */ +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "sick_line_guidance/sick_line_guidance_msg_util.h" +#include "sick_line_guidance_demo/figure_eight_fsm.h" +#include "sick_line_guidance_demo/navigation_util.h" +#include "sick_line_guidance_demo/turtlebot_test_fsm.h" +#include "sick_line_guidance_demo/stop_go_fsm.h" +#include "sick_line_guidance_demo/time_format.h" + +/* + * class TurtlebotTestFSM implements a state machine to generate cmd_vel messages + * with varying velocities to test the motor control of a TurtleBot. + * Input: ols and odometry messages + * Output: cmd_vel messages + */ + +/* + * Constructor + * @param[in] nh ros handle + * @param[in] ros_topic_ols_messages ROS topic for OLS_Measurement messages (input) + * @param[in] ros_topic_odometry ROS topic for odometry incl. robot positions (input) + * @param[in] ros_topic_cmd_vel ROS topic for cmd_vel messages (output) + * @param[in] print_errors true (default): print high latency by error message (print by info message otherwise) + */ +sick_line_guidance_demo::TurtlebotTestFSM::TurtlebotTestFSM(ros::NodeHandle* nh, const std::string & ros_topic_ols_messages, const std::string & ros_topic_odometry, const std::string & ros_topic_cmd_vel, bool print_errors) +: m_print_errors(print_errors), m_cmd_vel_publish_rate(20), m_fsm_thread(0), m_fsm_thread_run(false), m_cur_odom_timestamp(0), m_cur_velocity_timestamp(0), m_timestamp_stopped(0) +{ + m_cmd_vel_publish_rate = ros::Rate(20.0); // cmd_vel messages are published with 20 Hz by default + if(nh && !ros_topic_ols_messages.empty()) + { + m_ols_subscriber = nh->subscribe(ros_topic_ols_messages, 1, &sick_line_guidance_demo::TurtlebotTestFSM::messageCallbackOlsMeasurement, this); + ROS_INFO_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "RobotFSM: subscribing " << ros_topic_ols_messages); + } + if(nh && !ros_topic_odometry.empty()) + { + m_odom_subscriber = nh->subscribe(ros_topic_odometry, 1, &sick_line_guidance_demo::TurtlebotTestFSM::messageCallbackOdometry, this); + ROS_INFO_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "RobotFSM: subscribing " << ros_topic_odometry); + } + if(nh && !ros_topic_cmd_vel.empty()) + { + m_cmd_vel_publisher = nh->advertise(ros_topic_cmd_vel, 1); + ROS_INFO_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "RobotFSM: publishing " << m_cmd_vel_publisher.getTopic()); + } +} + +/* + * Destructor + */ +sick_line_guidance_demo::TurtlebotTestFSM::~TurtlebotTestFSM() +{ +} + +/* + * Start thread to run the final state machine. Read messages form ols and odom topics, publish messages to cmd_vel + */ +void sick_line_guidance_demo::TurtlebotTestFSM::startFSM(void) +{ + // Stop FSM and start a new thread to run the FSM + stopFSM(); + m_fsm_thread_run = true; + m_fsm_thread = new boost::thread(&sick_line_guidance_demo::TurtlebotTestFSM::runFSMthread, this); +} + +/* + * Stops the thread to run the final state machine + */ +void sick_line_guidance_demo::TurtlebotTestFSM::stopFSM(void) +{ + if(m_fsm_thread) + { + m_fsm_thread_run = false; + m_fsm_thread->join(); + delete(m_fsm_thread); + m_fsm_thread = 0; + } +} + +/* + * message callback for odometry messages. This function is called automatically by the ros::Subscriber after subscription of topic "/odom". + * It transforms the robots xy-positions from world/meter into map/pixel position, detect lines and barcodes in the map plus their distance to the robot, + * and transform them invers into world coordinates. + * @param[in] msg odometry message (input) + */ +void sick_line_guidance_demo::TurtlebotTestFSM::messageCallbackOdometry(const nav_msgs::Odometry::ConstPtr& msg) +{ + if(m_cur_odom_timestamp.isValid() && m_cur_velocity_timestamp.isValid()) + { + ros::Time time_now = ros::Time::now(); + double posx1 = 0, posx2 = 0, posy1= 0, posy2= 0, yaw1 = 0, yaw2 = 0; + NavigationUtil::toWorldPosition(m_cur_odom, posx1, posy1, yaw1); + NavigationUtil::toWorldPosition(msg, posx2, posy2, yaw2); + double dt = (time_now - m_cur_odom_timestamp).toSec(); + double odom_vel_linear = NavigationUtil::euclideanDistance(cv::Point2d(posx1, posy1), cv::Point2d(posx2, posy2)) / dt; + double odom_vel_yaw = NavigationUtil::deltaAngle(yaw1, yaw2) / dt; + if(std::abs(m_cur_velocity.linear.x) < 0.001 && std::abs(m_cur_velocity.angular.z) < 0.001) + { + ROS_INFO_STREAM("RobotFSM: cmd_vel.linear.x=" << m_cur_velocity.linear.x << ", cmd_vel.angular.z=" << m_cur_velocity.angular.z << ", cmd_vel.time=" << sick_line_guidance_demo::TimeFormat::formatDateTime(m_cur_velocity_timestamp)); + } + if(std::abs(m_cur_velocity.linear.x) < 0.001 && std::abs(m_cur_velocity.angular.z) < 0.001 && (std::abs(odom_vel_linear) >= 0.002 || std::abs(odom_vel_yaw) >= 0.002)) + { + double seconds_stopped = (time_now - m_timestamp_stopped).toSec(); + std::stringstream info; + info << "RobotFSM: cmd_vel.linear.x=" << m_cur_velocity.linear.x << ", cmd_vel.angular.z=" << m_cur_velocity.angular.z << ", cmd_vel.time=" << sick_line_guidance_demo::TimeFormat::formatDateTime(m_cur_velocity_timestamp) + << ", odom_vel_linear=" << odom_vel_linear << ", odom_vel_yaw=" << odom_vel_yaw + << ", stopped " << seconds_stopped << " seconds ago (now: " << sick_line_guidance_demo::TimeFormat::formatDateTime(time_now) + << ", stoppped: " << sick_line_guidance_demo::TimeFormat::formatDateTime(m_timestamp_stopped) << ")"; + if(m_print_errors && std::abs(odom_vel_linear) >= 0.002 && seconds_stopped >= 1.0) + ROS_ERROR_STREAM(info.str()); + else if(m_print_errors) + ROS_WARN_STREAM(info.str()); + else + ROS_INFO_STREAM(info.str()); + } + } + m_cur_odom = *msg; + m_cur_odom_timestamp = ros::Time::now(); +} + +/* + * message callback for OLS measurement messages. This function is called automatically by the ros::Subscriber after subscription of topic "/ols". + * It displays the OLS measurement (line info and barcodes), if visualization is enabled. + * @param[in] msg OLS measurement message (input) + */ +void sick_line_guidance_demo::TurtlebotTestFSM::messageCallbackOlsMeasurement(const boost::shared_ptr& msg) +{ +} + +/* + * thread callback, runs the final state machine for sick_line_guidance_demo. + * Input: ols and odometry messages + * Output: cmd_vel messages + */ +void sick_line_guidance_demo::TurtlebotTestFSM::runFSMthread(void) +{ + // sick_line_guidance_demo::FigureEightVelocityFSM drive_figure_eight_velocity; + sick_line_guidance_demo::StopGoVelocityFSM drive_velocity_fsm; + geometry_msgs::Twist last_velocity; + last_velocity.linear.x = 0; + last_velocity.angular.z = 0; + while(ros::ok() && m_fsm_thread_run) + { + drive_velocity_fsm.update(); + m_cur_velocity = drive_velocity_fsm.getVelocity(); + m_cur_velocity_timestamp = ros::Time::now(); + if((std::abs(last_velocity.linear.x) >= 0.001 || std::abs(last_velocity.angular.z) >= 0.001) + && std::abs(m_cur_velocity.linear.x) < 0.001 && std::abs(m_cur_velocity.angular.z) < 0.001) + { + m_timestamp_stopped = m_cur_velocity_timestamp; + } + m_cmd_vel_publisher.publish(m_cur_velocity); + last_velocity = m_cur_velocity; + m_cmd_vel_publish_rate.sleep(); + } +} diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/src/turtlebot_test_fsm_node.cpp b/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/src/turtlebot_test_fsm_node.cpp new file mode 100755 index 0000000..b022316 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/src/turtlebot_test_fsm_node.cpp @@ -0,0 +1,94 @@ +/* + * @brief turtlebot_test_fsm_node implements a simple state machine, + * generating cmd_vel messages to drive the TurtleBot with varying velocities, + * f.e. a figure of eight. + * A simple test for TurtleBot functionality and stability. + * + * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim + * Copyright (C) 2019 SICK AG, Waldkirch + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of SICK AG nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Authors: + * Michael Lehning + * + * Copyright 2019 SICK AG + * Copyright 2019 Ing.-Buero Dr. Michael Lehning + * + */ +#include +#include +#include +#include "sick_line_guidance_demo/turtlebot_test_fsm.h" +#include "sick_line_guidance_demo/time_format.h" + +/* + * @brief turtlebot_test_fsm_node implements a simple state machine, + * generating cmd_vel messages to drive the TurtleBot with + * varying velocities, f.e. a figure of eight. + * Simple test for TurtleBot functionality and stability. + */ +int main(int argc, char** argv) +{ + // ros init + ros::init(argc, argv, "turtlebot_test_fsm_node"); + ros::NodeHandle nh; + + // Run a final state machine (FSM) to drive a figure of eight. + // Output: geometry_msgs::Twist messages on topic "/cmd_vel" + sick_line_guidance_demo::TurtlebotTestFSM turtlebot_test_fsm(&nh, "/ols", "/odom", "/cmd_vel"); + ROS_INFO_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "turtlebot_test_fsm_node: starting"); + turtlebot_test_fsm.startFSM(); + + // Run ros event loop + ros::spin(); + + // Exit + std::cout << "turtlebot_test_fsm_node: stopping..." << std::endl; + ROS_INFO_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "turtlebot_test_fsm_node: stopping..."); + turtlebot_test_fsm.stopFSM(); + std::cout << "turtlebot_test_fsm_node: exiting..." << std::endl; + ROS_INFO_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "turtlebot_test_fsm_node: exiting..."); + + return 0; +} + diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/src/velocity_ctr.cpp b/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/src/velocity_ctr.cpp new file mode 100755 index 0000000..1c56f36 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/src/velocity_ctr.cpp @@ -0,0 +1,188 @@ +/* + * velocity_ctr controls angular velocity to avoid acceleration and changing velocity. + * velocity_ctr increases/decreases velocity.angular.z smooth and slowly until a desired yaw angle is reached. + * + * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim + * Copyright (C) 2019 SICK AG, Waldkirch + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of SICK AG nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Authors: + * Michael Lehning + * + * Copyright 2019 SICK AG + * Copyright 2019 Ing.-Buero Dr. Michael Lehning + * + */ +#include +#include +#include +#include +#include +#include "sick_line_guidance_demo/adjust_heading.h" +#include "sick_line_guidance_demo/velocity_ctr.h" + +/* + * class AngularZCtr controls angular velocity to avoid acceleration and changing velocity. + * It increases/decreases velocity.angular.z smooth and slowly until a desired yaw angle is reached. + */ + +/* + * Constructor + */ +sick_line_guidance_demo::AngularZCtr::AngularZCtr() +{ + m_dest_yaw_angle = 0; // robots desired heading + m_dest_vel_angular_z = 0; // desired velocity.angular.z (max. value to increase/decrease m_cur_vel_angular_z) + m_cur_yaw_angle = 0; // robots current heading + m_cur_vel_angular_z = 0; // current velocity.angular.z (increased resp. decreased from 0.0 to m_dest_vel_angular_z) + m_start_yaw_angle = 0; // robots heading at start time + m_start_time = ros::Time(0); // timestamp at start + m_update_cnt = -1; // just counts the number of updates (cycles) required to reach the destination heading, or -1 if not started + m_number_start_cyles_with_zero_angular_z = 0; // number of start cycles velocity.angular.z=0 before increasing velocity.angular.z to dest_vel_angular_z + m_delta_angular_z = 0.05 * M_PI / 4; // increase/decrease velocity.angular.z by m_delta_angular_z until delta_angle_now is smaller than some epsilon (default: 0.05 * M_PI / 4) +} + +/* + * Destructor + */ +sick_line_guidance_demo::AngularZCtr::~AngularZCtr() +{ +} + +/* + * Starts a new control cycle with smooth transition from cur_yaw_angle and velocity.angular.z = 0 to + * dest_yaw_angle and dest_vel_angular_z. + * @param[in] cur_yaw_angle: current yaw angle (robots current heading) + * @param[in] dest_yaw_angle: destination yaw angle (robots desired heading) + * @param[in] dest_vel_angular_z: destination velocity.angular.z + * @param[in] line_detected: if true (default), start with 5 cycles velocity.angular.z=0 before increasing velocity.angular.z to dest_vel_angular_z, otherwise there's only one cycle with velocity.angular.z=0 + * @return first value of velocity.angular.z after start, default: 0 + */ +double sick_line_guidance_demo::AngularZCtr::start(double cur_yaw_angle, double dest_yaw_angle, double dest_vel_angular_z, bool line_detected) +{ + if(isRunning()) + stop(); + m_dest_yaw_angle = dest_yaw_angle; + m_dest_vel_angular_z = dest_vel_angular_z; + m_cur_yaw_angle = cur_yaw_angle; + m_cur_vel_angular_z = 0; + m_start_yaw_angle = cur_yaw_angle; + m_number_start_cyles_with_zero_angular_z = line_detected ? 5 : 1; + m_start_time = ros::Time::now(); + m_update_cnt = 0; + return m_cur_vel_angular_z; +} + +/* + * Stops angular.z control, if currently running. + */ +void sick_line_guidance_demo::AngularZCtr::stop() +{ + m_update_cnt = -1; +} + +/* +* Update velocity.angular.z control with the current yaw angle. +* @param[in] cur_yaw_angle: current yaw angle (robots current heading) +*/ +void sick_line_guidance_demo::AngularZCtr::update(double cur_yaw_angle) +{ + if(m_update_cnt >= 0) // otherwise not running + { + // update current heading + m_cur_yaw_angle = cur_yaw_angle; + m_update_cnt++; + // compute new velocity.angular.z + if(m_update_cnt <= m_number_start_cyles_with_zero_angular_z) // currently stopping before increasing/decreasing velocity + { + m_cur_vel_angular_z = 0; + return; + } + if(std::abs(m_dest_vel_angular_z) < FLT_EPSILON) + { + m_update_cnt = -1; // finish after initial wait, nothing to be done + return; + } + // Check: destination heading reached? + double delta_angle_start = AdjustHeading::deltaAngle(m_start_yaw_angle, m_dest_yaw_angle); + double delta_angle_now = AdjustHeading::deltaAngle(cur_yaw_angle, m_dest_yaw_angle); + if((delta_angle_start > 0 && delta_angle_now < 0) || (delta_angle_start < 0 && delta_angle_now > 0) || std::abs(delta_angle_now) < 0.1 * M_PI / 180) + { + m_update_cnt = -1; // m_dest_yaw_angle reached, finished + return; + } + // increase/decrease velocity until delta_angle_now is smaller than some epsilon + m_cur_vel_angular_z += m_delta_angular_z * ((delta_angle_now >= 0) ? 1 : -1); + if(m_cur_vel_angular_z < -std::abs(m_dest_vel_angular_z)) + m_cur_vel_angular_z = -std::abs(m_dest_vel_angular_z); + if(m_cur_vel_angular_z > std::abs(m_dest_vel_angular_z)) + m_cur_vel_angular_z = std::abs(m_dest_vel_angular_z); + } +} + +/* + * Returns true, if AngularZCtr is currently running, i.e. velocity.angular.z is slowly increased/decreased until + * dest_yaw_angle (destination yaw angle, robots heading) and dest_vel_angular_z (destination velocity.angular.z) + * is reached. + */ +bool sick_line_guidance_demo::AngularZCtr::isRunning(void) +{ + return (m_update_cnt >= 0); +} + +/* + * Returns true, if AngularZCtr is currently running, i.e. velocity.angular.z is slowly increased/decreased until + * dest_yaw_angle (destination yaw angle, robots heading) and dest_vel_angular_z (destination velocity.angular.z) + * is reached. In this case, output parameter angular_z is set to the new velocity.angular.z to be published, + * and true is returned. Otherwise, false is returned and angular_z remains unchanged. + * @param[out] angular_z: velocity.angular.z to be published (if true returned), otherwise left untouched. + */ +bool sick_line_guidance_demo::AngularZCtr::isRunning(double & angular_z) +{ + if(m_update_cnt >= 0) // otherwise not running + { + angular_z = m_cur_vel_angular_z; + return true; + } + return false; +} diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/src/wait_at_barcode_state.cpp b/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/src/wait_at_barcode_state.cpp new file mode 100755 index 0000000..300f3a0 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/src/wait_at_barcode_state.cpp @@ -0,0 +1,126 @@ +/* + * WaitAtBarcodeState implements the state to wait at a barcode for a configurable amount of time. + * + * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim + * Copyright (C) 2019 SICK AG, Waldkirch + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of SICK AG nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Authors: + * Michael Lehning + * + * Copyright 2019 SICK AG + * Copyright 2019 Ing.-Buero Dr. Michael Lehning + * + */ +#include +#include "sick_line_guidance_demo/wait_at_barcode_state.h" +#include "sick_line_guidance_demo/navigation_util.h" +#include "sick_line_guidance_demo/time_format.h" + +/* + * Constructor + * @param[in] nh ros handle + * @param[in] context shared fsm context + */ +sick_line_guidance_demo::WaitAtBarcodeState::WaitAtBarcodeState(ros::NodeHandle* nh, RobotFSMContext* context) : m_fsm_context(context), m_stop_wait_time(10) +{ + if(nh) + { + // Configuration of wait at barcode state + ros::param::getCached("/wait_at_barcode_state/stopWaitSeconds", m_stop_wait_time); // time in seconds to stop at a barcode, default: 10 seconds + ROS_INFO_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "Configuration wait at barcode state: stopWaitSeconds=" << m_stop_wait_time); + } +} + +/* + * Destructor + */ +sick_line_guidance_demo::WaitAtBarcodeState::~WaitAtBarcodeState() +{ + m_fsm_context = 0; +} + +/* + * Clears all internal states + */ +void sick_line_guidance_demo::WaitAtBarcodeState::clear(void) +{ +} + +/* + * Runs the wait at barcode state for a configurable amount of time. + * Toggles follow_left_turnout flag at barcode label 101. + * @return FOLLOW_LINE, or EXIT in case ros::ok()==false. + */ +sick_line_guidance_demo::RobotFSMContext::RobotState sick_line_guidance_demo::WaitAtBarcodeState::run(void) +{ + assert(m_fsm_context); + ROS_INFO_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "RobotFSM: entering WaitAtBarcodeState"); + + ros::Time stop_wait_time = ros::Time::now() + ros::Duration(m_stop_wait_time); + geometry_msgs::Twist velocityMessage; + velocityMessage.linear.x = 0; + velocityMessage.linear.y = 0; + velocityMessage.linear.z = 0; + velocityMessage.angular.x = 0; + velocityMessage.angular.y = 0; + velocityMessage.angular.z = 0; + + // toggle follow_left_turnout at barcode 101: if true, follow a turnout on the left side, otherwise keep the main line + if(m_fsm_context->getCurBarcode() == 101) + { + bool follow_left_turnout = m_fsm_context->getFollowLeftTurnout(); + m_fsm_context->setFollowLeftTurnout(follow_left_turnout ? false : true); + } + + ros::Rate wait_rate = ros::Rate(20); + while(ros::ok()) + { + if(ros::Time::now() > stop_wait_time) + return RobotFSMContext::RobotState::FOLLOW_LINE; + wait_rate.sleep(); + m_fsm_context->publish(velocityMessage); + } + ROS_INFO_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "RobotFSM: leaving WaitAtBarcodeState"); + return RobotFSMContext::RobotState::EXIT; // ros::ok() == false +} diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/yaml/sick_line_guidance_demo.yaml b/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/yaml/sick_line_guidance_demo.yaml new file mode 100755 index 0000000..dca3017 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/yaml/sick_line_guidance_demo.yaml @@ -0,0 +1,42 @@ +# +# Configuration file for sick_line_guidance_demo +# + +# Configuration of PID controller +pid_controller: + kp: 30 + ki: 0 + kd: 0 + setpoint: 0.016 + +# Configuration of follow line state +follow_line_state: + followLineRate: 20 # frequency to update follow line state, default 20 Hz + followSpeed: 0.04 # default linear velocity to follow a line + noLineTime: 5 # time in seconds before switching to state explore line because of lost line + sensorLineWidth: 0.029 # measured line width at 0 degree, 29 mm for an OLS mounted 65 mm over ground, 20 mm for an OLS mounted 100 mm over ground + sensorLineMeasurementJitter: 0.003 # tolerate some line measurement jitter when adjusting the heading + adjustHeadingAngularZ: 0.07854 # 0.07854 = 0.1 * M_PI / 4 # velocity.angular.z to adjust robots heading + adjustHeadingLcpDeviationThresh: 0.04 # start to adjust heading, if the line distance increases over time (deviation of 1D-regression of line center points is above threshold lcpDeviationThresh) + adjustHeadingLcpThresh: 0.04 # start to adjust heading, if the line distance in meter (abs value) is above this threshold + adjustHeadingDeltaAngleEpsilon: 0.001745 # 0.001745 = 0.1 * M_PI / 180; # search stops, if the difference between current and desired yaw angle is smaller than delta_angle_epsilon + adjustHeadingMinDistanceToLastAdjust: 0.01 # move at least some cm before doing next heading adjustment + olsMessageTimeout: 0.5 # timeout for ols messages, robot stops and waits, if last ols message was received more than seconds ago + odomMessageTimeout: 0.5 # timeout for odom messages, robot stops and waits, if last ols message was received more than seconds ago + +# Configuration of explore line state +explore_line_state: + exploreLineRate: 20 # frequency to update explore line state, default 20 Hz + exploreSpeed: 0.04 # default linear velocity to explore a line + olsMessageTimeout: 0.5 # timeout for ols messages, robot stops and waits, if last ols message was received more than seconds ago + odomMessageTimeout: 0.5 # timeout for odom messages, robot stops and waits, if last ols message was received more than seconds ago + +# Configuration of wait at barcode state +wait_at_barcode_state: + stopWaitSeconds: 10 # time in seconds to stop at a barcode + +# Configuration of error simulation (test only) +error_simulation: + burst_no_line_duration: 0.0 # 1.0 # error simulation (test only), duration of "no line detected" bursts in seconds, default 0.0 (disabled) + burst_no_line_frequency: 0.0 # 0.1 # error simulation (test only), frequency of "no line detected" bursts in 1/seconds, default 0.0 (disabled) + diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/test/scripts/cleanup.bash b/Devices/Packages/sick_line_guidance/turtlebotDemo/test/scripts/cleanup.bash new file mode 100755 index 0000000..4afb968 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/turtlebotDemo/test/scripts/cleanup.bash @@ -0,0 +1,17 @@ +#!/bin/bash + +pushd ../../../../../ +source /opt/ros/melodic/setup.bash +./src/sick_line_guidance_demo/test/scripts/killall.bash +killall rosmaster ; sleep 1 + +echo -e "\n# cleanup.bash: Deleting ros cache and logfiles and catkin folders ./build ./devel ./install" +rosclean purge -y +rm -rf ./build ./devel ./install +rm -rf ~/.ros/* +# rm -rf ~/.ros/log/* +catkin clean --yes --all-profiles --verbose +catkin_make clean +popd +if [ -f ./sick_line_guidance_demo.avi ] ; then rm -f ./sick_line_guidance_demo.avi ; fi + diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/test/scripts/clion.bash b/Devices/Packages/sick_line_guidance/turtlebotDemo/test/scripts/clion.bash new file mode 100755 index 0000000..ce8d0d1 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/turtlebotDemo/test/scripts/clion.bash @@ -0,0 +1,23 @@ +#!/bin/bash + +# init ros environment +source /opt/ros/melodic/setup.bash +source ../../../../../devel/setup.bash +source ../../../../../install/setup.bash + +# start edit resource-files +gedit ./rungeneric.bash ../../sick_line_guidance_demo/launch/sick_line_guidance_demo_node.launch ../../sick_line_guidance_demo/yaml/sick_line_guidance_demo.yaml & + +# start clion +echo -e "Starting clion...\nNote in case of clion/cmake errors:" +echo -e " Click 'File' -> 'Reload Cmake Project'" +echo -e " cmake/clion: Project 'XXX' tried to find library '-lpthread' -> delete 'thread' from find_package(Boost ... COMPONENTS ...) in CMakeLists.txt" +echo -e " rm -rf ../../../.idea # removes all clion settings" +echo -e " rm -f ~/CMakeCache.txt" +echo -e " 'File' -> 'Settings' -> 'CMake' -> 'CMake options' : CATKIN_DEVEL_PREFIX=~/TASK007_PA0100_ROS_Treiber_Spurfuehrungsssensor/catkin_ws/devel" +echo -e " 'File' -> 'Settings' -> 'CMake' -> 'Generation path' : ../clion_build" + +pushd ../../../../.. +~/Public/clion-2018.3.3/bin/clion.sh ./src & +popd + diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/test/scripts/gitCloneInstall.bash b/Devices/Packages/sick_line_guidance/turtlebotDemo/test/scripts/gitCloneInstall.bash new file mode 100755 index 0000000..1264ca5 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/turtlebotDemo/test/scripts/gitCloneInstall.bash @@ -0,0 +1,46 @@ +#!/bin/bash + +# +# Get all packages for sick_line_guidance_demo, +# build and install. +# + +# Clean up +# rosclean purge -y +# rm -rf ./build ./devel ./install +# rm -rf ~/.ros/* +# catkin clean --yes --all-profiles --verbose +# catkin_make clean +# +# Install ros packages for turtlebot +pushd ../../../../../src +git clone https://github.com/ROBOTIS-GIT/turtlebot3.git +git clone https://github.com/ROBOTIS-GIT/turtlebot3_msgs.git +git clone https://github.com/ROBOTIS-GIT/hls_lfcd_lds_driver.git +git clone https://github.com/ros-drivers/rosserial.git +# Install ros packages for turtlebot simulation +git clone https://github.com/ROBOTIS-GIT/turtlebot3_simulations +git clone https://github.com/ROBOTIS-GIT/turtlebot3_gazebo_plugin.git +# Install can_open packages +git clone https://github.com/ros-industrial/ros_canopen.git +git clone https://github.com/CANopenNode/CANopenSocket.git +git clone https://github.com/linux-can/can-utils.git +# Install sick_line_guidance package +git clone https://github.com/ros-planning/random_numbers.git +git clone https://github.com/SICKAG/sick_line_guidance.git +# Install ros packages required for robot_fsm +git clone https://github.com/uos/sick_tim.git +# Install video support for sick_line_guidance_demo +sudo apt-get install ffmpeg +sudo apt-get install vlc +# Install profiling and performance tools +git clone https://github.com/catkin/catkin_simple.git +# git clone https://github.com/ethz-asl/schweizer_messer.git # toolbox including timing utilities +sudo svn export https://github.com/ethz-asl/schweizer_messer/trunk/sm_common # common utilities from ethz-asl "schweizer messer" toolbox +sudo svn export https://github.com/ethz-asl/schweizer_messer/trunk/sm_timing # timing utilities from ethz-asl "schweizer messer" toolbox +sudo apt-get install google-perftools libgoogle-perftools-dev graphviz # libprofiler for profiling +# Build and install +cd .. +catkin_make install --cmake-args -DTURTLEBOT_DEMO="ON" +source ./install/setup.bash +popd diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/test/scripts/killall.bash b/Devices/Packages/sick_line_guidance/turtlebotDemo/test/scripts/killall.bash new file mode 100755 index 0000000..560693a --- /dev/null +++ b/Devices/Packages/sick_line_guidance/turtlebotDemo/test/scripts/killall.bash @@ -0,0 +1,5 @@ +#!/bin/bash + +echo -e "#\n# runsimu.bash: Stopping all rosnodes...\n# rosnode kill -a ; sleep 1 ; killall robot_fsm rqt_plot gzclient gzserver ; sleep 3\n#" +rosnode kill -a ; sleep 1 ; killall robot_fsm ; killall rqt_plot gzclient ; sleep 1 ; killall gzserver ; sleep 3 # ; killall rosmaster ; sleep 1 + diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/test/scripts/make.bash b/Devices/Packages/sick_line_guidance/turtlebotDemo/test/scripts/make.bash new file mode 100755 index 0000000..a6f1c46 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/turtlebotDemo/test/scripts/make.bash @@ -0,0 +1,41 @@ +#!/bin/bash + +# +# Build and install sick_line_guidance_demo. +# Use ./gitCloneInstall.bash to install required packages. +# + +# delete old logfiles +pushd ../../../../.. +rm -rf install/lib/sick_line_guidance/* +rm -rf install/lib/sick_line_guidance_demo/* +rm -rf install/share/sick_line_guidance/* +rm -rf install/share/sick_line_guidance_demo/* +rm -f build/catkin_make_install.log + +# make install, use cmake option TURTLEBOT_DEMO="ON" to include the sick_line_guidance_demo packages in folder turtlebotDemo +source /opt/ros/melodic/setup.bash +catkin_make --cmake-args -DTURTLEBOT_DEMO="ON" 2>&1 | tee -a build/catkin_make_install.log +catkin_make install --cmake-args -DTURTLEBOT_DEMO="ON" 2>&1 | tee -a build/catkin_make_install.log +source ./install/setup.bash + +# lint, install by running +# sudo apt-get install python-catkin-lint +# catkin_lint -W1 src/sick_line_guidance_demo +# catkin_lint -W1 src/sick_line_guidance +# catkin_lint -W1 src/sick_line_guidance_demo/sick_line_guidance_demo + +# print warnings and errors +echo -e "\nmake.bash finished.\n" +echo -e "catkin_make warnings:" +cat build/catkin_make_install.log | grep -i "warning:" +echo -e "\ncatkin_make errors:" +cat build/catkin_make_install.log | grep -i "error:" + +# print sick_line_guidance install files, libraries, executables +echo -e "\ninstall/lib/sick_line_guidance:" +ls -al install/lib/sick_line_guidance/* +echo -e "\ninstall/share/sick_line_guidance:" +ls install/share/sick_line_guidance/*.* +popd + diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/test/scripts/makeall.bash b/Devices/Packages/sick_line_guidance/turtlebotDemo/test/scripts/makeall.bash new file mode 100755 index 0000000..c321721 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/turtlebotDemo/test/scripts/makeall.bash @@ -0,0 +1,10 @@ +#!/bin/bash +echo -e "makeall.bash: build and install ros sick_line_guidance driver and demo" +sudo echo -e "makeall.bash started." +# sudo apt-get install python-catkin-lint +source /opt/ros/melodic/setup.bash +./cleanup.bash +./makepcan.bash +./make.bash +echo -e "makeall.bash finished." + diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/test/scripts/makepcan.bash b/Devices/Packages/sick_line_guidance/turtlebotDemo/test/scripts/makepcan.bash new file mode 100755 index 0000000..cb3390d --- /dev/null +++ b/Devices/Packages/sick_line_guidance/turtlebotDemo/test/scripts/makepcan.bash @@ -0,0 +1,41 @@ +#!/bin/bash + +# install packages required for peak can device driver + +if [ -f ../../../../../../peak_systems/pcanview-ncurses_0.8.7-0_amd64.deb ] ; then + pushd ../../../../../../peak_systems + sudo apt-get install libncurses5 + sudo dpkg --install pcanview-ncurses_0.8.7-0_amd64.deb + popd +fi + +# build and install peak can device driver + +if [ -d ../../../../../../peak_systems/peak-linux-driver-8.7.0 ] ; then + pushd ../../../../../../peak_systems/peak-linux-driver-8.7.0 + # install required packages + sudo apt-get install can-utils + sudo apt-get install gcc-multilib + sudo apt-get install libelf-dev + sudo apt-get install libpopt-dev + sudo apt-get install tree + # build and install pcan driver + make clean + make NET=NETDEV_SUPPORT + sudo make install + # install the modules + sudo modprobe pcan + sudo modprobe can + sudo modprobe vcan + sudo modprobe slcan + # setup and configure "can0" net device + sudo ip link set can0 type can + sudo ip link set can0 up type can bitrate 125000 # configure the CAN bitrate, f.e. 125000 bit/s + # check installation + # ./driver/lspcan --all # should print "pcanusb32" and pcan version + # tree /dev/pcan-usb # should show a pcan-usb device + # ip -a link # should print some "can0: ..." messages + ip -details link show can0 # should print some details about "can0" net device + popd +fi + diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/test/scripts/plotsimu.bash b/Devices/Packages/sick_line_guidance/turtlebotDemo/test/scripts/plotsimu.bash new file mode 100755 index 0000000..929a3ee --- /dev/null +++ b/Devices/Packages/sick_line_guidance/turtlebotDemo/test/scripts/plotsimu.bash @@ -0,0 +1,18 @@ +#!/bin/bash + +source ./simu_env.bash +source ../../../../../install/setup.bash + +# rosrun rviz rviz & +# roslaunch turtlebot3_gazebo turtlebot3_empty_world.launch & +# rqt_plot /gazebo/model_states/pose[1]/position/x /gazebo/model_states/pose[1]/position/y & # display turtlebots (x,y) position over time + +rostopic list +rostopic echo /cmd_vel & +rostopic echo /fsm & +rostopic echo /iam_state & +# rostopic echo /odom & +# rostopic echo /ols & +# rostopic echo /gazebo/model_states +# rostopic echo /gazebo/link_states + diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/test/scripts/rungeneric.bash b/Devices/Packages/sick_line_guidance/turtlebotDemo/test/scripts/rungeneric.bash new file mode 100755 index 0000000..ecc31a8 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/turtlebotDemo/test/scripts/rungeneric.bash @@ -0,0 +1,142 @@ +#!/bin/bash + +# +# set environment, clear screen and logfiles +# + +if [ ! -d ./log ] ; then mkdir -p ./log ; fi +if [ ! -d ~/.ros/log ] ; then mkdir -p ~/.ros/log ; fi +printf "\033c" +rm -rf ./log/* +source ./simu_env.bash +pushd ../../../../.. +rm -rf ~/.ros/log/* +# printf "\033c" +source /opt/ros/melodic/setup.bash +source ./install/setup.bash +./src/sick_line_guidance/turtlebotDemo/test/scripts/killall.bash +export LIBPROFILER=/usr/lib/x86_64-linux-gnu/libprofiler.so.0 # path to libprofiler.so.0 may depend on os version + +# +# Start fsm, driver, nodes, tools and run the sick_line_guidance_demo simulation +# + +# roscore & +# sleep 10 # wait for roscore startup processes + +if [ "$TURTLEBOT_SIMULATION" == "1" ] ; then + + # Start gazebo / turtlebot simulator + sleep 1 ; echo -e "#\n# runsimu.bash: starting gazebo ..." + sleep 1 ; roslaunch turtlebot3_gazebo turtlebot3_empty_world.launch & + sleep 10 # wait until gazebo has started ... + # Virtual navigation: + # roslaunch turtlebot3_gazebo turtlebot3_world.launch + # roslaunch turtlebot3_navigation turtlebot3_navigation.launch map_file:=$HOME/map.yaml + export SICK_LINE_GUIDANCE_DEMO_LAUNCH_ARGS='ols_simu:=1' + +else # "$TURTLEBOT_SIMULATION" == "0" + + if [ "$TURTLEBOT_STOP_AT_START" == "1" ] ; then # Stop turtlebot (set all velocities to 0 and restart turtlebot) + roslaunch turtlebot3_bringup turtlebot3_robot.launch & + sleep 15 ; rostopic pub --once /cmd_vel geometry_msgs/Twist '{linear: {x: 0.0, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0,z: 0.0}}' # stop the turtlebot + sleep 1 ; rosnode kill turtlebot3_core ; sleep 1 ; rosnode kill turtlebot3_diagnostics + fi + + # Init turtlebot + sleep 5 ; roslaunch turtlebot3_bringup turtlebot3_robot.launch & + sleep 15 + + # Start OLS20 driver or simulated OLS messages + if [ "$TURTLEBOT_OLS_SIMULATION" == "1" ] ; then # Run TurtleBot with simulated OLS messages + export SICK_LINE_GUIDANCE_DEMO_LAUNCH_ARGS='ols_simu:=1' + else # "$TURTLEBOT_OLS_SIMULATION" == "0" # CAN initialization and start OLS20 driver + # Init CAN and start OLS20 driver + sudo ip link set can0 up type can bitrate 125000 # configure can0 + ip -details link show can0 # check status can0 + # Start OLS20 driver + sleep 5 ; roslaunch -v --screen sick_line_guidance sick_line_guidance.launch yaml:=sick_line_guidance_ols20.yaml > ~/.ros/log/sick_line_guidance_ols20.log & + sleep 10 ; export SICK_LINE_GUIDANCE_DEMO_LAUNCH_ARGS='ols_simu:=0' + fi # "$TURTLEBOT_OLS_SIMULATION" + +fi # "$TURTLEBOT_SIMULATION" + +# Start sick_line_guidance_demo simulation +export SICK_LINE_GUIDANCE_DEMO_LAUNCH_ARGS=$SICK_LINE_GUIDANCE_DEMO_LAUNCH_ARGS' visualize:='$TURTLEBOT_VISUALIZE +echo -e "#\n# runsimu.bash: starting simulation:\n# roslaunch sick_line_guidance_demo sick_line_guidance_demo_node.launch $SICK_LINE_GUIDANCE_DEMO_LAUNCH_ARGS \n#" + +if [ "$TURTLEBOT_PROFILING" == "1" ] ; then # debug and profile sick_line_guidance_demo + rm -f /tmp/sick_line_guidance_demo.prof* # remove previous profile logs + env CPUPROFILE=/tmp/sick_line_guidance_demo.prof LD_PRELOAD=$LIBPROFILER roslaunch sick_line_guidance_demo sick_line_guidance_demo_node.launch $SICK_LINE_GUIDANCE_DEMO_LAUNCH_ARGS & +else # run sick_line_guidance_demo + roslaunch -v --screen sick_line_guidance sick_line_guidance_demo_node.launch $SICK_LINE_GUIDANCE_DEMO_LAUNCH_ARGS 2>&1 | tee ~/.ros/log/sick_line_guidance_demo_node.log & +fi +sleep 10 + +# Start sick_line_guidance watchdog run emergency exit to stop the TurtleBot after line lost for more than 1 second +if ! [ "$TURTLEBOT_START_WATCHDOG_AFTER" == "" ] && [ $TURTLEBOT_START_WATCHDOG_AFTER -ge 0 ] ; then + sleep $TURTLEBOT_START_WATCHDOG_AFTER # start watchdog after a simulated OLS is close enough to a line + roslaunch -v --screen sick_line_guidance sick_line_guidance_watchdog.launch 2>&1 | tee ~/.ros/log/watchdog.log & # run watchdog +fi + +# +# Run sick_line_guidance_demo for a while: just wait for key 'q' or 'Q' +# while ros nodes sick_line_guidance_demo_node and Robot_FSM are running +# + +while true ; do + echo -e "FSM and sick_line_guidance_demo simulation running. Press 'q' to exit..." ; read -t 0.1 -n1 -s key + if [[ $key = "q" ]] || [[ $key = "Q" ]]; then break ; fi +done + +# +# Stop turtlebot, fsm and sick_line_guidance_demo +# + +rosnode kill sick_line_guidance_demo_node +sleep 0.5 ; rostopic pub --once /cmd_vel geometry_msgs/Twist '{linear: {x: 0.0, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0,z: 0.0}}' ; sleep 0.5 # stop the turtlebot +./src/sick_line_guidance/turtlebotDemo/test/scripts/killall.bash + +# +# Check errors and warnings in ros logfiles +# +echo -e "\n#\n# sick_line_guidance_demo simulation finished.\n#\n" +grep "\[ WARN\]" ~/.ros/log/*.log ~/.ros/log/*/ros*.log ~/.ros/log/*.log >> ~/.ros/log/ros_log_warnings.txt +grep "\[ERROR\]" ~/.ros/log/*.log ~/.ros/log/*/ros*.log ~/.ros/log/*.log >> ~/.ros/log/ros_log_errors.txt +echo -e "\nSimulation warnings and errors:\n" +cat ~/.ros/log/ros_log_warnings.txt +cat ~/.ros/log/ros_log_errors.txt + +# +# Profiling +# +if [ "$TURTLEBOT_PROFILING" == "1" ] ; then + for proffile in /tmp/robot_fsm.prof* ; do + google-pprof --pdf ./install/lib/iam/robot_fsm $proffile > $proffile.pdf + done + for proffile in /tmp/sick_line_guidance_demo.prof* ; do + google-pprof --pdf ./install/lib/sick_line_guidance_demo/sick_line_guidance_demo_node $proffile > $proffile.pdf + done +fi + +# +# Play video recorded by sick_line_guidance_demo +# +popd +cp ~/.ros/log/watchdog.log ./log +cp ~/.ros/log/robot_fsm.log ./log +cp ~/.ros/log/sick_line_guidance_demo_node.log ./log +if [ "$TURTLEBOT_PROFILING" == "1" ] ; then + cp /tmp/robot_fsm.prof*.pdf ./log + cp /tmp/sick_line_guidance_demo.prof*.pdf ./log +fi +if [ -f ~/.ros/sick_line_guidance_demo.avi ] ; then + mv ~/.ros/sick_line_guidance_demo.avi ./log/sick_line_guidance_demo.avi + echo -e "\nCreated sick_line_guidance_demo.avi:" + ls -al ./log/sick_line_guidance_demo.avi + echo -e "ffplay ./sick_line_guidance_demo.avi ; # Use right mouse button to seek forward/backward, 'p' for pause/resume" + ffplay ./log/sick_line_guidance_demo.avi + echo -e "\n" +fi +ls -al ./log/* + diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/test/scripts/runsimu.bash b/Devices/Packages/sick_line_guidance/turtlebotDemo/test/scripts/runsimu.bash new file mode 100755 index 0000000..045e3cf --- /dev/null +++ b/Devices/Packages/sick_line_guidance/turtlebotDemo/test/scripts/runsimu.bash @@ -0,0 +1,10 @@ +#!/bin/bash + +export TURTLEBOT_SIMULATION=1 +export TURTLEBOT_OLS_SIMULATION=1 +export TURTLEBOT_VISUALIZE=2 +export TURTLEBOT_START_WATCHDOG_AFTER=-1 # 60 +export TURTLEBOT_PROFILING=0 +export TURTLEBOT_STOP_AT_START=0 +./rungeneric.bash + diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/test/scripts/runturtlebot.bash b/Devices/Packages/sick_line_guidance/turtlebotDemo/test/scripts/runturtlebot.bash new file mode 100755 index 0000000..5058e6d --- /dev/null +++ b/Devices/Packages/sick_line_guidance/turtlebotDemo/test/scripts/runturtlebot.bash @@ -0,0 +1,10 @@ +#!/bin/bash + +export TURTLEBOT_SIMULATION=0 +export TURTLEBOT_OLS_SIMULATION=0 +export TURTLEBOT_VISUALIZE=0 +export TURTLEBOT_START_WATCHDOG_AFTER=0 +export TURTLEBOT_PROFILING=0 +export TURTLEBOT_STOP_AT_START=0 +./rungeneric.bash + diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/test/scripts/runturtlebot_olssimu.bash b/Devices/Packages/sick_line_guidance/turtlebotDemo/test/scripts/runturtlebot_olssimu.bash new file mode 100755 index 0000000..66c411a --- /dev/null +++ b/Devices/Packages/sick_line_guidance/turtlebotDemo/test/scripts/runturtlebot_olssimu.bash @@ -0,0 +1,10 @@ +#!/bin/bash + +export TURTLEBOT_SIMULATION=0 +export TURTLEBOT_OLS_SIMULATION=1 +export TURTLEBOT_VISUALIZE=0 +export TURTLEBOT_START_WATCHDOG_AFTER=60 +export TURTLEBOT_PROFILING=0 +export TURTLEBOT_STOP_AT_START=0 +./rungeneric.bash + diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/test/scripts/simu_env.bash b/Devices/Packages/sick_line_guidance/turtlebotDemo/test/scripts/simu_env.bash new file mode 100755 index 0000000..93a0290 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/turtlebotDemo/test/scripts/simu_env.bash @@ -0,0 +1,12 @@ +#!/bin/bash + +# +# set environment for sick_line_guidance_demo simulation +# + +export ROS_MASTER_URI=http://127.0.0.1:11311 +export ROS_HOSTNAME=127.0.0.1 +export TURTLEBOT3_MODEL=waffle +export SVGA_VGPU10=0 # otherwise "roslaunch turtlebot3_gazebo" may fail when running in VMware +export LIBGL_ALWAYS_SOFTWARE=1 # otherwise "roslaunch turtlebot3_gazebo" may fail when running in VMware + diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/test/scripts/stopturtlebot.bash b/Devices/Packages/sick_line_guidance/turtlebotDemo/test/scripts/stopturtlebot.bash new file mode 100755 index 0000000..97d6bf7 --- /dev/null +++ b/Devices/Packages/sick_line_guidance/turtlebotDemo/test/scripts/stopturtlebot.bash @@ -0,0 +1,28 @@ +#!/bin/bash + +# +# stop the turtlebot +# + +# environment +pushd ../../../../.. +source /opt/ros/melodic/setup.bash +source ./install/setup.bash +echo -e "#\n#stop turtlebot...\n#" + +# kill all ros nodes +rosnode kill -a +sleep 0.3 +killall -9 robot_fsm + +# init turtlebot and send velocity 0.0 +roslaunch turtlebot3_bringup turtlebot3_robot.launch & +sleep 1 +for ((i=1;i<=20;i++)) ; do + rostopic pub --once /cmd_vel geometry_msgs/Twist '{linear: {x: 0.0, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0,z: 0.0}}' + sleep 0.5 +done + +# stop turtlebot ros node +rosnode kill -a + diff --git a/Devices/Packages/sick_line_guidance/turtlebotDemo/test/scripts/testturtlebot.bash b/Devices/Packages/sick_line_guidance/turtlebotDemo/test/scripts/testturtlebot.bash new file mode 100755 index 0000000..bfd8f9a --- /dev/null +++ b/Devices/Packages/sick_line_guidance/turtlebotDemo/test/scripts/testturtlebot.bash @@ -0,0 +1,90 @@ +#!/bin/bash + +# +# simple turtlebot test, drives a figure of eight +# + +# +# set environment, clear screen and logfiles +# + +export TURTLEBOT_SIMULATION=0 # 1 +export ROS_MASTER_URI=http://127.0.0.1:11311 +export ROS_HOSTNAME=127.0.0.1 +export TURTLEBOT3_MODEL=waffle +export SVGA_VGPU10=0 # otherwise "roslaunch turtlebot3_gazebo" may fail when running in VMware +export LIBGL_ALWAYS_SOFTWARE=1 # otherwise "roslaunch turtlebot3_gazebo" may fail when running in VMware + +if [ ! -d ./log ] ; then mkdir -p ./log ; fi +if [ ! -d ~/.ros/log ] ; then mkdir -p ~/.ros/log ; fi +printf "\033c" +rm -rf ./log/* +pushd ../../../../.. +rm -rf ~/.ros/log/* +source /opt/ros/melodic/setup.bash +source ./install/setup.bash +rosnode kill -a +sleep 5 + +# +# Start turtlebot resp. turtlebot simulation +# + +if [ "$TURTLEBOT_SIMULATION" == "1" ] ; then + rosnode kill -a ; sleep 1 ; killall rqt_plot gzclient ; sleep 1 ; killall gzserver ; sleep 3 + roslaunch turtlebot3_gazebo turtlebot3_empty_world.launch 2>&1 & +else + roslaunch turtlebot3_bringup turtlebot3_robot.launch 2>&1 | tee ~/.ros/log/turtlebot3_bringup.log & + # Init CAN and start OLS20 driver + sudo ip link set can0 up type can bitrate 125000 # configure can0 + ip -details link show can0 # check status can0 + sleep 1 ; roslaunch -v --screen sick_line_guidance sick_line_guidance.launch yaml:=sick_line_guidance_ols20.yaml > ~/.ros/log/sick_line_guidance_ols20.log & +fi +sleep 15 + +# +# Send some cmd_vel messages to test +# + +# for ((i=1;i<=20;i++)) ; do +# rostopic pub --once /cmd_vel geometry_msgs/Twist '{linear: {x: 0.1, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0,z: -0.4}}' & +# sleep 0.1 +# done +# for ((i=1;i<=20;i++)) ; do +# rostopic pub --once /cmd_vel geometry_msgs/Twist '{linear: {x: 0.0, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0,z: 0.0}}' & +# sleep 0.1 +# done + + +# +# Start turtlebot_test_fsm_node +# + +roslaunch -v --screen sick_line_guidance_demo turtlebot_test_fsm_node.launch & # 2>&1 | tee ~/.ros/log/turtlebot_test_fsm_node.log & +# roslaunch -v --screen sick_line_guidance_demo sick_line_guidance_demo_node.launch ols_simu:=0 visualize:=0 2>&1 | tee ~/.ros/log/sick_line_guidance_demo_node.log & + +# +# Run turtlebot_test_fsm_node for a while: just wait for key 'q' or 'Q' +# while ros node turtlebot_test_fsm_node is running +# + +while true ; do + echo -e "turtlebot_test_fsm_node running. Press 'q' to exit..." ; read -t 1.0 -n1 -s key + if [[ $key = "q" ]] || [[ $key = "Q" ]]; then break ; fi +done + +# +# Stop turtlebot_test_fsm_node +# + +rosnode kill turtlebot_test_fsm_node +sleep 0.5 ; rostopic pub --once /cmd_vel geometry_msgs/Twist '{linear: {x: 0.0, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0,z: 0.0}}' ; sleep 0.5 # stop the turtlebot +rosnode kill -a + +popd +cp ~/.ros/log/turtlebot_test_fsm_node.log ./log +cp ~/.ros/log/sick_line_guidance_ols20.log ./log +cp ~/.ros/log/turtlebot3_bringup.log ./log +ls -al ./log/* + + diff --git a/Devices/Packages/wit_wt901ble_reader/.vscode/c_cpp_properties.json b/Devices/Packages/wit_wt901ble_reader/.vscode/c_cpp_properties.json new file mode 100755 index 0000000..355a7c1 --- /dev/null +++ b/Devices/Packages/wit_wt901ble_reader/.vscode/c_cpp_properties.json @@ -0,0 +1,79 @@ +{ + "configurations": [ + { + "browse": { + "databaseFilename": "${default}", + "limitSymbolsToIncludedHeaders": false + }, + "includePath": [ + "/home/huandt/HuanDT/projects_ws/devel/include/**", + "/opt/ros/noetic/include/**", + "/home/huandt/HuanDT/planner_ws/devel/include/**", + "/home/huandt/HuanDT/planner_ws/src/agv_define/agv_define/include/**", + "/home/huandt/HuanDT/planner_ws/src/agv_define/agv_dynparam/include/**", + "/home/huandt/HuanDT/planner_ws/src/agv_path_smoothing/include/**", + "/home/huandt/HuanDT/planner_ws/src/ROS.AMR_100/navigations/amcl/include/**", + "/home/huandt/HuanDT/planner_ws/src/ROS.AMR_100/navigations/base_local_planner/include/**", + "/home/huandt/HuanDT/planner_ws/src/ROS.AMR_100/navigations/carrot_planner/include/**", + "/home/huandt/HuanDT/planner_ws/src/ROS.AMR_100/navigations/clear_costmap_recovery/include/**", + "/home/huandt/HuanDT/planner_ws/src/ROS.AMR_100/navigations/costmap_2d/include/**", + "/home/huandt/HuanDT/planner_ws/src/ROS.AMR_100/navigations/costmap_converter/include/**", + "/home/huandt/HuanDT/planner_ws/src/ROS.AMR_100/navigations/costmap_queue/include/**", + "/home/huandt/HuanDT/planner_ws/src/custom_planner/include/**", + "/home/huandt/HuanDT/planner_ws/src/ROS.AMR_100/mir_robot/diff_drive_controller/include/**", + "/home/huandt/HuanDT/planner_ws/src/ROS.AMR_100/navigations/dwa_local_planner/include/**", + "/home/huandt/HuanDT/planner_ws/src/ROS.AMR_100/navigations/dwb_critics/include/**", + "/home/huandt/HuanDT/planner_ws/src/ROS.AMR_100/navigations/dwb_local_planner/include/**", + "/home/huandt/HuanDT/planner_ws/src/ROS.AMR_100/navigations/dwb_plugins/include/**", + "/home/huandt/HuanDT/planner_ws/src/ROS.AMR_100/navigations/follow_waypoints/include/**", + "/home/huandt/HuanDT/planner_ws/src/ROS.AMR_100/navigations/geographic_info/geodesy/include/**", + "/home/huandt/HuanDT/planner_ws/src/ROS.AMR_100/navigations/global_planner/include/**", + "/home/huandt/HuanDT/projects_ws/src/ros_basic/libcpp_pkg/include/**", + "/home/huandt/HuanDT/planner_ws/src/make_pathway/include/**", + "/home/huandt/HuanDT/planner_ws/src/ROS.AMR_100/navigations/map_server/include/**", + "/home/huandt/HuanDT/planner_ws/src/ROS.AMR_100/navigations/move_base_flex/mbf_abstract_core/include/**", + "/home/huandt/HuanDT/planner_ws/src/ROS.AMR_100/navigations/move_base_flex/mbf_abstract_nav/include/**", + "/home/huandt/HuanDT/planner_ws/src/ROS.AMR_100/navigations/move_base_flex/mbf_costmap_core/include/**", + "/home/huandt/HuanDT/planner_ws/src/ROS.AMR_100/navigations/move_base_flex/mbf_costmap_nav/include/**", + "/home/huandt/HuanDT/planner_ws/src/ROS.AMR_100/navigations/move_base_flex/mbf_simple_nav/include/**", + "/home/huandt/HuanDT/planner_ws/src/ROS.AMR_100/navigations/move_base_flex/mbf_utility/include/**", + "/home/huandt/HuanDT/planner_ws/src/ROS.AMR_100/mir_robot/mir_dwb_critics/include/**", + "/home/huandt/HuanDT/planner_ws/src/ROS.AMR_100/navigations/move_base/include/**", + "/home/huandt/HuanDT/planner_ws/src/ROS.AMR_100/navigations/move_slow_and_clear/include/**", + "/home/huandt/HuanDT/planner_ws/src/ROS.AMR_100/navigations/nav_2d_utils/include/**", + "/home/huandt/HuanDT/planner_ws/src/ROS.AMR_100/navigations/nav_core/include/**", + "/home/huandt/HuanDT/planner_ws/src/ROS.AMR_100/navigations/nav_core2/include/**", + "/home/huandt/HuanDT/planner_ws/src/ROS.AMR_100/navigations/nav_core_adapter/include/**", + "/home/huandt/HuanDT/planner_ws/src/ROS.AMR_100/navigations/nav_grid/include/**", + "/home/huandt/HuanDT/planner_ws/src/ROS.AMR_100/navigations/nav_grid_iterators/include/**", + "/home/huandt/HuanDT/planner_ws/src/ROS.AMR_100/navigations/navfn/include/**", + "/home/huandt/HuanDT/planner_ws/src/ROS.AMR_100/navigations/pluginlib/include/**", + "/home/huandt/HuanDT/planner_ws/src/ROS.AMR_100/navigations/pose_follower/include/**", + "/home/huandt/HuanDT/planner_ws/src/ROS.AMR_100/navigations/robot_localization/include/**", + "/home/huandt/HuanDT/planner_ws/src/ROS.AMR_100/navigations/rotate_recovery/include/**", + "/home/huandt/HuanDT/planner_ws/src/ROS.AMR_100/navigations/sbpl_lattice_planner/include/**", + "/home/huandt/HuanDT/planner_ws/src/ROS.AMR_100/navigations/sbpl_recovery/include/**", + "/home/huandt/HuanDT/projects_ws/src/serial/include/**", + "/home/huandt/HuanDT/projects_ws/src/sick_safetyscanners/include/**", + "/home/huandt/HuanDT/planner_ws/src/ROS.AMR_100/navigations/teb_local_planner/include/**", + "/home/huandt/HuanDT/planner_ws/src/ROS.AMR_100/navigations/geometry2/tf2_geometry_msgs/include/**", + "/home/huandt/HuanDT/planner_ws/src/ROS.AMR_100/navigations/geometry2/tf2_ros/include/**", + "/home/huandt/HuanDT/planner_ws/src/ROS.AMR_100/navigations/geometry2/tf2_sensor_msgs/include/**", + "/home/huandt/HuanDT/planner_ws/src/ROS.AMR_100/navigations/twist_recovery/include/**", + "/home/huandt/HuanDT/planner_ws/src/ROS.AMR_100/navigations/unique_identifier/unique_id/include/**", + "/home/huandt/HuanDT/planner_ws/src/vda5050_api/vda5050_api/include/**", + "/home/huandt/HuanDT/planner_ws/src/vda5050_api/vda5050_connector/include/**", + "/home/huandt/HuanDT/planner_ws/src/vda5050_api/vda5050_msgs/include/**", + "/home/huandt/HuanDT/projects_ws/src/waveshare_tof_reader/include/**", + "/home/huandt/HuanDT/projects_ws/src/wit_wt901ble_reader/include/**", + "/usr/include/**" + ], + "name": "ROS", + "intelliSenseMode": "gcc-x64", + "compilerPath": "/usr/bin/gcc", + "cStandard": "gnu11", + "cppStandard": "c++14" + } + ], + "version": 4 +} \ No newline at end of file diff --git a/Devices/Packages/wit_wt901ble_reader/.vscode/settings.json b/Devices/Packages/wit_wt901ble_reader/.vscode/settings.json new file mode 100755 index 0000000..362337c --- /dev/null +++ b/Devices/Packages/wit_wt901ble_reader/.vscode/settings.json @@ -0,0 +1,83 @@ +{ + "python.autoComplete.extraPaths": [ + "/home/huandt/HuanDT/projects_ws/devel/lib/python3/dist-packages", + "/home/huandt/HuanDT/planner_ws/devel/lib/python3/dist-packages", + "/opt/ros/noetic/lib/python3/dist-packages" + ], + "python.analysis.extraPaths": [ + "/home/huandt/HuanDT/projects_ws/devel/lib/python3/dist-packages", + "/home/huandt/HuanDT/planner_ws/devel/lib/python3/dist-packages", + "/opt/ros/noetic/lib/python3/dist-packages" + ], + "files.associations": { + "cctype": "cpp", + "clocale": "cpp", + "cmath": "cpp", + "csignal": "cpp", + "cstdarg": "cpp", + "cstddef": "cpp", + "cstdio": "cpp", + "cstdlib": "cpp", + "cstring": "cpp", + "ctime": "cpp", + "cwchar": "cpp", + "cwctype": "cpp", + "array": "cpp", + "atomic": "cpp", + "strstream": "cpp", + "bit": "cpp", + "*.tcc": "cpp", + "bitset": "cpp", + "chrono": "cpp", + "cinttypes": "cpp", + "codecvt": "cpp", + "complex": "cpp", + "condition_variable": "cpp", + "cstdint": "cpp", + "deque": "cpp", + "forward_list": "cpp", + "list": "cpp", + "map": "cpp", + "set": "cpp", + "unordered_map": "cpp", + "unordered_set": "cpp", + "vector": "cpp", + "exception": "cpp", + "algorithm": "cpp", + "functional": "cpp", + "iterator": "cpp", + "memory": "cpp", + "memory_resource": "cpp", + "numeric": "cpp", + "optional": "cpp", + "random": "cpp", + "ratio": "cpp", + "string": "cpp", + "string_view": "cpp", + "system_error": "cpp", + "tuple": "cpp", + "type_traits": "cpp", + "utility": "cpp", + "hash_map": "cpp", + "hash_set": "cpp", + "fstream": "cpp", + "initializer_list": "cpp", + "iomanip": "cpp", + "iosfwd": "cpp", + "iostream": "cpp", + "istream": "cpp", + "limits": "cpp", + "mutex": "cpp", + "new": "cpp", + "ostream": "cpp", + "shared_mutex": "cpp", + "sstream": "cpp", + "stdexcept": "cpp", + "streambuf": "cpp", + "thread": "cpp", + "cfenv": "cpp", + "typeindex": "cpp", + "typeinfo": "cpp", + "variant": "cpp" + } +} \ No newline at end of file diff --git a/Devices/Packages/wit_wt901ble_reader/CMakeLists.txt b/Devices/Packages/wit_wt901ble_reader/CMakeLists.txt new file mode 100755 index 0000000..3f90abc --- /dev/null +++ b/Devices/Packages/wit_wt901ble_reader/CMakeLists.txt @@ -0,0 +1,101 @@ +cmake_minimum_required(VERSION 3.0.2) +project(wit_wt901ble_reader) + +set(CMAKE_CXX_STANDARD 11) +set(CMAKE_CXX_FLAGS "-std=c++11 -g ${CMAKE_CXX_FLAGS}") + +find_package(catkin REQUIRED COMPONENTS + roscpp + std_msgs + serial + diagnostic_msgs +) + +catkin_package( + INCLUDE_DIRS include + LIBRARIES wit_wt901ble_reader + CATKIN_DEPENDS roscpp std_msgs serial diagnostic_msgs +# DEPENDS system_lib +) + +SET(-ludev udev) +## Declare a C++ library +add_library(${PROJECT_NAME} + src/serial.c + src/wit_c_sdk.c + src/wit_wt901ble_reader.cc +) +target_include_directories(${PROJECT_NAME} PUBLIC ${catkin_INCLUDE_DIRS} include) +target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${GSTREAMER_LIBRARIES} ${-ludev}) +add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) + +add_executable(${PROJECT_NAME}_node src/wit_wt901ble_reader_main.cc) +target_include_directories(${PROJECT_NAME}_node PUBLIC ${catkin_INCLUDE_DIRS} include) +target_link_libraries(${PROJECT_NAME}_node PUBLIC ${catkin_LIBRARIES} ${PROJECT_NAME}) +add_dependencies(${PROJECT_NAME}_node ${catkin_EXPORTED_TARGETS}) + + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# catkin_install_python(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html +# install(TARGETS ${PROJECT_NAME}_node +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark libraries for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) + +install(TARGETS ${PROJECT_NAME}_node + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +## Mark cpp header files for installation +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE +) + +install(DIRECTORY + launch + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_waveshare_tof_reader.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) \ No newline at end of file diff --git a/Devices/Packages/wit_wt901ble_reader/README.md b/Devices/Packages/wit_wt901ble_reader/README.md new file mode 100755 index 0000000..8a2b7fd --- /dev/null +++ b/Devices/Packages/wit_wt901ble_reader/README.md @@ -0,0 +1,58 @@ +# Requirements: +- Ubuntu 20.04.6 LTS +- ROS Noetic +- Install SDL library: + ``` + sudo apt-get install libsdl-image1.2-dev + sudo apt-get install libsdl-dev + ``` +- Install libudev-dev: + ``` + sudo apt install libudev-dev + ``` +- Install Serial package: + ``` + sudo apt-get install ros-noetic-serial + ``` +# Wiring and configurations: +- Following on wiki website: https://drive.google.com/drive/folders/1NlOFHSTYNy2bRAfaA0S25BEaXK4uvia9 + +# Build: +- Direct to the workspace and run command: + ``` + catkin_make + ``` +# Run: +- Configure parametters in .launch file: + ``` + + + + + + + + + + + + ``` +- Permit the serial port: + ``` + sudo chmod 777 /dev/USB_IMU + ``` +- Run the lannch file: + ``` + roslaunch wit_wt901ble_reader wit_wt901ble_reader.launch + ``` +# Messages of datas: + +- The output message of the IMU: + ``` + name: custom topic name in wit_wt901ble_reader wit_wt901ble_reader.launch + + + type: sensor_msgs/Imu.msg + + ``` + diff --git a/Devices/Packages/wit_wt901ble_reader/include/wit_wt901ble_reader/REG.h b/Devices/Packages/wit_wt901ble_reader/include/wit_wt901ble_reader/REG.h new file mode 100755 index 0000000..de7b2b2 --- /dev/null +++ b/Devices/Packages/wit_wt901ble_reader/include/wit_wt901ble_reader/REG.h @@ -0,0 +1,284 @@ +#ifndef __AHRSREG_H +#define __AHRSREG_H + +#ifdef __cplusplus +extern "C" { +#endif +#define REGSIZE 0x90 + +#define SAVE 0x00 +#define CALSW 0x01 +#define RSW 0x02 +#define RRATE 0x03 +#define BAUD 0x04 +#define AXOFFSET 0x05 +#define AYOFFSET 0x06 +#define AZOFFSET 0x07 +#define GXOFFSET 0x08 +#define GYOFFSET 0x09 +#define GZOFFSET 0x0a +#define HXOFFSET 0x0b` +#define HYOFFSET 0x0c +#define HZOFFSET 0x0d +#define D0MODE 0x0e +#define D1MODE 0x0f +#define D2MODE 0x10 +#define D3MODE 0x11 +#define D0PWMH 0x12 +#define D1PWMH 0x13 +#define D2PWMH 0x14 +#define D3PWMH 0x15 +#define D0PWMT 0x16 +#define D1PWMT 0x17 +#define D2PWMT 0x18 +#define D3PWMT 0x19 +#define IICADDR 0x1a +#define LEDOFF 0x1b +#define MAGRANGX 0x1c +#define MAGRANGY 0x1d +#define MAGRANGZ 0x1e +#define BANDWIDTH 0x1f +#define GYRORANGE 0x20 +#define ACCRANGE 0x21 +#define SLEEP 0x22 +#define ORIENT 0x23 +#define AXIS6 0x24 +#define FILTK 0x25 +#define GPSBAUD 0x26 +#define READADDR 0x27 +#define BWSCALE 0x28 +#define MOVETHR 0x28 +#define MOVESTA 0x29 +#define ACCFILT 0x2A +#define GYROFILT 0x2b +#define MAGFILT 0x2c +#define POWONSEND 0x2d +#define VERSION 0x2e +#define CCBW 0x2f +#define YYMM 0x30 +#define DDHH 0x31 +#define MMSS 0x32 +#define MS 0x33 +#define AX 0x34 +#define AY 0x35 +#define AZ 0x36 +#define GX 0x37 +#define GY 0x38 +#define GZ 0x39 +#define HX 0x3a +#define HY 0x3b +#define HZ 0x3c +#define Roll 0x3d +#define Pitch 0x3e +#define Yaw 0x3f +#define TEMP 0x40 +#define D0Status 0x41 +#define D1Status 0x42 +#define D2Status 0x43 +#define D3Status 0x44 +#define PressureL 0x45 +#define PressureH 0x46 +#define HeightL 0x47 +#define HeightH 0x48 +#define LonL 0x49 +#define LonH 0x4a +#define LatL 0x4b +#define LatH 0x4c +#define GPSHeight 0x4d +#define GPSYAW 0x4e +#define GPSVL 0x4f +#define GPSVH 0x50 +#define q0 0x51 +#define q1 0x52 +#define q2 0x53 +#define q3 0x54 +#define SVNUM 0x55 +#define PDOP 0x56 +#define HDOP 0x57 +#define VDOP 0x58 +#define DELAYT 0x59 +#define XMIN 0x5a +#define XMAX 0x5b +#define BATVAL 0x5c +#define ALARMPIN 0x5d +#define YMIN 0x5e +#define YMAX 0x5f +#define GYROZSCALE 0x60 +#define GYROCALITHR 0x61 +#define ALARMLEVEL 0x62 +#define GYROCALTIME 0x63 +#define REFROLL 0x64 +#define REFPITCH 0x65 +#define REFYAW 0x66 +#define GPSTYPE 0x67 +#define TRIGTIME 0x68 +#define KEY 0x69 +#define WERROR 0x6a +#define TIMEZONE 0x6b +#define CALICNT 0x6c +#define WZCNT 0x6d +#define WZTIME 0x6e +#define WZSTATIC 0x6f +#define ACCSENSOR 0x70 +#define GYROSENSOR 0x71 +#define MAGSENSOR 0x72 +#define PRESSENSOR 0x73 +#define MODDELAY 0x74 + +#define ANGLEAXIS 0x75 +#define XRSCALE 0x76 +#define YRSCALE 0x77 +#define ZRSCALE 0x78 + +#define XREFROLL 0x79 +#define YREFPITCH 0x7a +#define ZREFYAW 0x7b + +#define ANGXOFFSET 0x7c +#define ANGYOFFSET 0x7d +#define ANGZOFFSET 0x7e + +#define NUMBERID1 0x7f +#define NUMBERID2 0x80 +#define NUMBERID3 0x81 +#define NUMBERID4 0x82 +#define NUMBERID5 0x83 +#define NUMBERID6 0x84 + +#define XA85PSCALE 0x85 +#define XA85NSCALE 0x86 +#define YA85PSCALE 0x87 +#define YA85NSCALE 0x88 +#define XA30PSCALE 0x89 +#define XA30NSCALE 0x8a +#define YA30PSCALE 0x8b +#define YA30NSCALE 0x8c + +#define CHIPIDL 0x8D +#define CHIPIDH 0x8E +#define REGINITFLAG REGSIZE-1 + + +/* AXIS6 */ +#define ALGRITHM9 0 +#define ALGRITHM6 1 + +/************CALSW**************/ +#define NORMAL 0x00 +#define CALGYROACC 0x01 +#define CALMAG 0x02 +#define CALALTITUDE 0x03 +#define CALANGLEZ 0x04 +#define CALACCL 0x05 +#define CALACCR 0x06 +#define CALMAGMM 0x07 +#define CALREFANGLE 0x08 +#define CALMAG2STEP 0x09 +//#define CALACCX 0x09 +//#define ACC45PRX 0x0A +//#define ACC45NRX 0x0B +//#define CALACCY 0x0C +//#define ACC45PRY 0x0D +//#define ACC45NRY 0x0E +//#define CALREFANGLER 0x0F +//#define CALACCINIT 0x10 +//#define CALREFANGLEINIT 0x11 +#define CALHEXAHEDRON 0x12 + +/************OUTPUTHEAD**************/ +#define WIT_TIME 0x50 +#define WIT_ACC 0x51 +#define WIT_GYRO 0x52 +#define WIT_ANGLE 0x53 +#define WIT_MAGNETIC 0x54 +#define WIT_DPORT 0x55 +#define WIT_PRESS 0x56 +#define WIT_GPS 0x57 +#define WIT_VELOCITY 0x58 +#define WIT_QUATER 0x59 +#define WIT_GSA 0x5A +#define WIT_REGVALUE 0x5F + +/************RSW**************/ +#define RSW_TIME 0x01 +#define RSW_ACC 0x02 +#define RSW_GYRO 0x04 +#define RSW_ANGLE 0x08 +#define RSW_MAG 0x10 +#define RSW_PORT 0x20 +#define RSW_PRESS 0x40 +#define RSW_GPS 0x80 +#define RSW_V 0x100 +#define RSW_Q 0x200 +#define RSW_GSA 0x400 +#define RSW_MASK 0xfff + +/**RRATE*****/ +#define RRATE_NONE 0x0d +#define RRATE_02HZ 0x01 +#define RRATE_05HZ 0x02 +#define RRATE_1HZ 0x03 +#define RRATE_2HZ 0x04 +#define RRATE_5HZ 0x05 +#define RRATE_10HZ 0x06 +#define RRATE_20HZ 0x07 +#define RRATE_50HZ 0x08 +#define RRATE_100HZ 0x09 +#define RRATE_125HZ 0x0a //only WT931 +#define RRATE_200HZ 0x0b +#define RRATE_ONCE 0x0c + +/* BAUD */ +#define WIT_BAUD_4800 1 +#define WIT_BAUD_9600 2 +#define WIT_BAUD_19200 3 +#define WIT_BAUD_38400 4 +#define WIT_BAUD_57600 5 +#define WIT_BAUD_115200 6 +#define WIT_BAUD_230400 7 +#define WIT_BAUD_460800 8 +#define WIT_BAUD_921600 9 + +/*CAN BAUD*/ +#define CAN_BAUD_1000000 0 +#define CAN_BAUD_800000 1 +#define CAN_BAUD_500000 2 +#define CAN_BAUD_400000 3 +#define CAN_BAUD_250000 4 +#define CAN_BAUD_200000 5 +#define CAN_BAUD_125000 6 +#define CAN_BAUD_100000 7 +#define CAN_BAUD_80000 8 +#define CAN_BAUD_50000 9 +#define CAN_BAUD_40000 10 +#define CAN_BAUD_20000 11 +#define CAN_BAUD_10000 12 +#define CAN_BAUD_5000 13 +#define CAN_BAUD_3000 14 + +/* KEY */ +#define KEY_UNLOCK 0xB588 + +/* SAVE */ +#define SAVE_PARAM 0x00 +#define SAVE_SWRST 0xFF + +/* ORIENT */ +#define ORIENT_HERIZONE 0 +#define ORIENT_VERTICLE 1 + +/* BANDWIDTH */ +#define BANDWIDTH_256HZ 0 +#define BANDWIDTH_184HZ 1 +#define BANDWIDTH_94HZ 2 +#define BANDWIDTH_44HZ 3 +#define BANDWIDTH_21HZ 4 +#define BANDWIDTH_10HZ 5 +#define BANDWIDTH_5HZ 6 + + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/Devices/Packages/wit_wt901ble_reader/include/wit_wt901ble_reader/serial.h b/Devices/Packages/wit_wt901ble_reader/include/wit_wt901ble_reader/serial.h new file mode 100755 index 0000000..4ad8647 --- /dev/null +++ b/Devices/Packages/wit_wt901ble_reader/include/wit_wt901ble_reader/serial.h @@ -0,0 +1,28 @@ + +#ifndef SERIAL_H +#define SERIAL_H + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +void serial_close(int fd); + +int serial_open(unsigned char* dev, unsigned int baud); + +int serial_read_data(int fd, unsigned char *val, int len); + +int serial_write_data(int fd, unsigned char *val, int len); + + +#endif diff --git a/Devices/Packages/wit_wt901ble_reader/include/wit_wt901ble_reader/udevadm_info.h b/Devices/Packages/wit_wt901ble_reader/include/wit_wt901ble_reader/udevadm_info.h new file mode 100755 index 0000000..64a2e71 --- /dev/null +++ b/Devices/Packages/wit_wt901ble_reader/include/wit_wt901ble_reader/udevadm_info.h @@ -0,0 +1,119 @@ +#ifndef __UDEVADM_INFO_H_INCLUDE__ +#define __UDEVADM_INFO_H_INCLUDE__ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +class udevadmInfo { +public: + udevadmInfo(const char *product_name); + virtual ~udevadmInfo(void); + virtual int init(); + virtual bool scanDevices(void); + const char *port; +protected: + struct udev *udev; + char *product_name_; +}; + +udevadmInfo::udevadmInfo(const char *product_name) + : product_name_((char *)product_name) +{ + +} + +udevadmInfo::~udevadmInfo(void){ + udev_unref(udev); +} + +int udevadmInfo::init() { + udev = udev_new(); + if (!udev) { + printf("Error while initialization!\n"); + return EXIT_FAILURE; + } + sigset_t mask; + + // Set signals we want to catch + sigemptyset(&mask); + sigaddset(&mask, SIGTERM); + sigaddset(&mask, SIGINT); + + // Change the signal mask and check + if (sigprocmask(SIG_BLOCK, &mask, nullptr) < 0) { + ROS_ERROR("UDEV-Error while sigprocmask(): %s\n", std::strerror(errno)); + return EXIT_FAILURE; + } + // Get a signal file descriptor + int signal_fd = signalfd(-1, &mask, 0); + // Check the signal file descriptor + if (signal_fd < 0) { + ROS_ERROR("UDEV-Error while signalfd(): %s\n", std::strerror(errno)); + return EXIT_FAILURE; + } + return EXIT_SUCCESS; +} + +bool udevadmInfo::scanDevices(void) { + struct udev_device *device; + struct udev_enumerate *enumerate; + struct udev_list_entry *devices, *dev_list_entry; + + // Create enumerate object + enumerate = udev_enumerate_new(udev); + if (!enumerate) { + ROS_ERROR("Error while creating udev enumerate\n"); + return false; + } + + // Scan devices + udev_enumerate_scan_devices(enumerate); + + // Fill up device list + devices = udev_enumerate_get_list_entry(enumerate); + if (!devices) { + ROS_ERROR("Error while getting device list\n"); + return false; + } + + udev_list_entry_foreach(dev_list_entry, devices) { + const char* path = udev_list_entry_get_name(dev_list_entry); + // Get the device + device = udev_device_new_from_syspath(udev, udev_list_entry_get_name(dev_list_entry)); + // Print device information + if(udev_device_get_devnode(device) != NULL && strstr(udev_device_get_sysname(device),"ttyUSB") != NULL) { + // ROS_INFO("DEVNODE=%s", udev_device_get_devnode(device)); + struct udev_device* usb = udev_device_get_parent_with_subsystem_devtype(device,"usb","usb_device"); + // ROS_INFO("usb = %s:%s, manufacturer = %s, product = %s, serial = %s, speed = %s, bMaxPower = %s", + // udev_device_get_sysattr_value(usb, "idVendor"), + // udev_device_get_sysattr_value(usb, "idProduct"), + // udev_device_get_sysattr_value(usb, "manufacturer"), + // udev_device_get_sysattr_value(usb, "product"), + // udev_device_get_sysattr_value(usb, "serial"), + // udev_device_get_sysattr_value(usb, "speed"), + // udev_device_get_sysattr_value(usb, "bMaxPower")); + if(strstr(udev_device_get_sysattr_value(usb, "product"), (const char *)product_name_) != NULL) { + port = udev_device_get_devnode(device); + return true; + } + } + else { + continue; + } + // Free the device + udev_device_unref(device); + } + // Free enumerate + udev_enumerate_unref(enumerate); + return false; +} +#endif \ No newline at end of file diff --git a/Devices/Packages/wit_wt901ble_reader/include/wit_wt901ble_reader/wit_c_sdk.h b/Devices/Packages/wit_wt901ble_reader/include/wit_wt901ble_reader/wit_c_sdk.h new file mode 100755 index 0000000..7cff1d7 --- /dev/null +++ b/Devices/Packages/wit_wt901ble_reader/include/wit_wt901ble_reader/wit_c_sdk.h @@ -0,0 +1,135 @@ +#ifndef __WIT_C_SDK_H +#define __WIT_C_SDK_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include +#include "wit_wt901ble_reader/REG.h" + + +#define WIT_HAL_OK (0) /**< There is no error */ +#define WIT_HAL_BUSY (-1) /**< Busy */ +#define WIT_HAL_TIMEOUT (-2) /**< Timed out */ +#define WIT_HAL_ERROR (-3) /**< A generic error happens */ +#define WIT_HAL_NOMEM (-4) /**< No memory */ +#define WIT_HAL_EMPTY (-5) /**< The resource is empty */ +#define WIT_HAL_INVAL (-6) /**< Invalid argument */ + +#define WIT_DATA_BUFF_SIZE 256 + +#define WIT_PROTOCOL_NORMAL 0 +#define WIT_PROTOCOL_MODBUS 1 +#define WIT_PROTOCOL_CAN 2 +#define WIT_PROTOCOL_I2C 3 + + +/* serial function */ +typedef void (*SerialWrite)(uint8_t *p_ucData, uint32_t uiLen); +int32_t WitSerialWriteRegister(SerialWrite write_func); +void WitSerialDataIn(uint8_t ucData); + +/* iic function */ + +/* + i2c write function example + + int32_t WitI2cWrite(uint8_t ucAddr, uint8_t ucReg, uint8_t *p_ucVal, uint32_t uiLen) + { + i2c_start(); + i2c_send(ucAddr); + if(i2c_wait_ask() != SUCCESS)return 0; + i2c_send(ucReg); + if(i2c_wait_ask() != SUCCESS)return 0; + for(uint32_t i = 0; i < uiLen; i++) + { + i2c_send(*p_ucVal++); + if(i2c_wait_ask() != SUCCESS)return 0; + } + i2c_stop(); + return 1; + } +*/ +typedef int32_t (*WitI2cWrite)(uint8_t ucAddr, uint8_t ucReg, uint8_t *p_ucVal, uint32_t uiLen); +/* + i2c read function example + + int32_t WitI2cRead(uint8_t ucAddr, uint8_t ucReg, uint8_t *p_ucVal, uint32_t uiLen) + { + i2c_start(); + i2c_send(ucAddr); + if(i2c_wait_ask() != SUCCESS)return 0; + i2c_send(ucReg); + if(i2c_wait_ask() != SUCCESS)return 0; + + i2c_start(); + i2c_send(ucAddr+1); + for(uint32_t i = 0; i < uiLen; i++) + { + if(i+1 == uiLen)*p_ucVal++ = i2c_read(0); //last byte no ask + else *p_ucVal++ = i2c_read(1); // ask + } + i2c_stop(); + return 1; + } +*/ +typedef int32_t (*WitI2cRead)(uint8_t ucAddr, uint8_t ucReg, uint8_t *p_ucVal, uint32_t uiLen); +int32_t WitI2cFuncRegister(WitI2cWrite write_func, WitI2cRead read_func); + +/* can function */ +typedef void (*CanWrite)(uint8_t ucStdId, uint8_t *p_ucData, uint32_t uiLen); +int32_t WitCanWriteRegister(CanWrite write_func); + +/* Delayms function */ +typedef void (*DelaymsCb)(uint16_t ucMs); +int32_t WitDelayMsRegister(DelaymsCb delayms_func); + + +void WitCanDataIn(uint8_t ucData[8], uint8_t ucLen); + + +typedef void (*RegUpdateCb)(uint32_t uiReg, uint32_t uiRegNum); +int32_t WitRegisterCallBack(RegUpdateCb update_func); +int32_t WitWriteReg(uint32_t uiReg, uint16_t usData); +int32_t WitReadReg(uint32_t uiReg, uint32_t uiReadNum); +int32_t WitInit(uint32_t uiProtocol, uint8_t ucAddr); +void WitDeInit(void); + + + +/** + ****************************************************************************** + * @file wit_c_sdk.h + * @author Wit + * @version V1.0 + * @date 05-May-2022 + * @brief This file provides all Configure sensor function. + ****************************************************************************** + * @attention + * + * http://wit-motion.cn/ + * + ****************************************************************************** + */ +int32_t WitStartAccCali(void); +int32_t WitStopAccCali(void); +int32_t WitStartMagCali(void); +int32_t WitStopMagCali(void); +int32_t WitSetUartBaud(int32_t uiBaudIndex); +int32_t WitSetBandwidth(int32_t uiBaudWidth); +int32_t WitSetOutputRate(int32_t uiRate); +int32_t WitSetContent(int32_t uiRsw); +int32_t WitSetCanBaud(int32_t uiBaudIndex); + +char CheckRange(short sTemp,short sMin,short sMax); + +extern int16_t sReg[REGSIZE]; + +#ifdef __cplusplus +} +#endif + +#endif /* __WIT_C_SDK_H */ diff --git a/Devices/Packages/wit_wt901ble_reader/include/wit_wt901ble_reader/wit_wt901ble_reader.h b/Devices/Packages/wit_wt901ble_reader/include/wit_wt901ble_reader/wit_wt901ble_reader.h new file mode 100755 index 0000000..8b6e1ff --- /dev/null +++ b/Devices/Packages/wit_wt901ble_reader/include/wit_wt901ble_reader/wit_wt901ble_reader.h @@ -0,0 +1,130 @@ +#ifndef _WT901BLE_READER_H_ +#define _WT901BLE_READER_H_ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "sensor_msgs/Imu.h" +#include "geometry_msgs/Quaternion.h" +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "wit_wt901ble_reader/udevadm_info.h" + +using namespace std; + +namespace wt901ble_reader +{ + static bool is_imu_connected = false; + static bool is_imu_data_available = false; + static uint16_t lost_data_count = 0; + const uint8_t wt901ble_data_length = 20; + const uint8_t wt901ble_header_imu[2] = {0x55, 0x61}; + + struct wt901ble_data{ + wt901ble_data() + { + aX = 0; + aY = 0; + aZ = 0; + wX = 0; + wY = 0; + wZ = 0; + Roll = 0; + Pitch = 0; + Yaw = 0; + } + double aX; + double aY; + double aZ; + double wX; + double wY; + double wZ; + double Roll; + double Pitch; + double Yaw; + }; + struct UserParams{ + UserParams(){ + baudrate = 115200; + portname = "/dev/ttyUSB2"; + topic_name = "/imu"; + frame_id = "imu"; + } + uint32_t baudrate; + string portname; + string topic_name; + string frame_id; + }; + + class wt901ble_node + { + public: + wt901ble_node(const string& nodename); + ~wt901ble_node(); + private: + ros::NodeHandle nh_; + ros::Publisher wt901ble_data_pub_; + ros::WallTimer get_and_publish_timer; + ros::WallTimer reconnect_timer; + serial::Serial* serial_; + UserParams* userParams; + wt901ble_data* wt901ble_data_; + bool LoadParams(UserParams* userParams_, const string& node_name); + void GetAndPublishData(const ::ros::WallTimerEvent& timer_event); + void ReconnectSerial(const ::ros::WallTimerEvent& timer_event); + size_t readN(uint8_t *buf, size_t len); + bool receiveData(); + bool recvData(uint8_t *buf); + geometry_msgs::Quaternion getQuaternion(double yaw_, double pitch_, double roll_); + bool detect_port(string& port_name); + sensor_msgs::Imu imu_data; + string nodename_; + bool isConnected_; + bool isDataAvailable; + bool is_try_to_connect; + bool is_reconnected; + uint8_t data_buf[wt901ble_data_length]{0}; + }; + + class Diagnostics + { + public: + Diagnostics(string& nodename); + ~Diagnostics(); + private: + ros::NodeHandle nh_; + ros::Publisher diagnostics_pub; + boost::thread* diagnostics_thread; + boost::mutex mutex; + void run_diagnostics(); + void pubDiagnostics(); + string nodename_; + string imu_diagnostics_topic_name_; + diagnostic_msgs::DiagnosticStatus connection; + diagnostic_msgs::DiagnosticArray prev_diagnostic; + }; +} + +#endif \ No newline at end of file diff --git a/Devices/Packages/wit_wt901ble_reader/launch/wit_wt901ble_reader.launch b/Devices/Packages/wit_wt901ble_reader/launch/wit_wt901ble_reader.launch new file mode 100755 index 0000000..8514d81 --- /dev/null +++ b/Devices/Packages/wit_wt901ble_reader/launch/wit_wt901ble_reader.launch @@ -0,0 +1,26 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/Devices/Packages/wit_wt901ble_reader/package.xml b/Devices/Packages/wit_wt901ble_reader/package.xml new file mode 100755 index 0000000..6e5167d --- /dev/null +++ b/Devices/Packages/wit_wt901ble_reader/package.xml @@ -0,0 +1,69 @@ + + + wit_wt901ble_reader + 0.0.0 + The wit_wt901ble_reader package + + + + + huandt + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + roscpp + std_msgs + serial + roscpp + std_msgs + serial + roscpp + std_msgs + serial + diagnostic_msgs + diagnostic_msgs + + + + + + + diff --git a/Devices/Packages/wit_wt901ble_reader/src/main.c b/Devices/Packages/wit_wt901ble_reader/src/main.c new file mode 100755 index 0000000..5a7ee62 --- /dev/null +++ b/Devices/Packages/wit_wt901ble_reader/src/main.c @@ -0,0 +1,173 @@ +#include "wit_wt901ble_reader/serial.h" +#include "wit_wt901ble_reader/wit_c_sdk.h" +#include "wit_wt901ble_reader/REG.h" +#include + + +#define ACC_UPDATE 0x01 +#define GYRO_UPDATE 0x02 +#define ANGLE_UPDATE 0x04 +#define MAG_UPDATE 0x08 +#define READ_UPDATE 0x80 + + +static int fd, s_iCurBaud = 115200; +static volatile char s_cDataUpdate = 0; + + +const int c_uiBaud[] = {2400 , 4800 , 9600 , 19200 , 38400 , 57600 , 115200 , 230400 , 460800 , 921600}; + + +static void AutoScanSensor(char* dev); +static void SensorDataUpdata(uint32_t uiReg, uint32_t uiRegNum); +static void Delayms(uint16_t ucMs); + + +int main(int argc,char* argv[]){ + + if(argc < 2) + { + printf("please input dev name\n"); + return 0; + } + + + if((fd = serial_open(argv[1] , 115200)<0)) + { + printf("open %s fail\n", argv[1]); + return 0; + } + else printf("open %s success\n", argv[1]); + + + float fAcc[3], fGyro[3], fAngle[3]; + int i , ret; + char cBuff[1]; + + WitInit(WIT_PROTOCOL_NORMAL, 0x50); + WitRegisterCallBack(SensorDataUpdata); + + printf("\r\n********************** wit-motion Normal example ************************\r\n"); + AutoScanSensor(argv[1]); + + while(1) + { + + while(serial_read_data(fd, cBuff, 1)) + { + WitSerialDataIn(cBuff[0]); + } + printf("\n"); + Delayms(500); + + if(s_cDataUpdate) + { + for(i = 0; i < 3; i++) + { + fAcc[i] = sReg[AX+i] / 32768.0f * 16.0f; + fGyro[i] = sReg[GX+i] / 32768.0f * 2000.0f; + fAngle[i] = sReg[Roll+i] / 32768.0f * 180.0f; + } + + if(s_cDataUpdate & ACC_UPDATE) + { + printf("acc:%.3f %.3f %.3f\r\n", fAcc[0], fAcc[1], fAcc[2]); + s_cDataUpdate &= ~ACC_UPDATE; + } + if(s_cDataUpdate & GYRO_UPDATE) + { + printf("gyro:%.3f %.3f %.3f\r\n", fGyro[0], fGyro[1], fGyro[2]); + s_cDataUpdate &= ~GYRO_UPDATE; + } + if(s_cDataUpdate & ANGLE_UPDATE) + { + printf("angle:%.3f %.3f %.3f\r\n", fAngle[0], fAngle[1], fAngle[2]); + s_cDataUpdate &= ~ANGLE_UPDATE; + } + if(s_cDataUpdate & MAG_UPDATE) + { + printf("mag:%d %d %d\r\n", sReg[HX], sReg[HY], sReg[HZ]); + s_cDataUpdate &= ~MAG_UPDATE; + } + } + } + + serial_close(fd); + return 0; +} + + +static void SensorDataUpdata(uint32_t uiReg, uint32_t uiRegNum) +{ + int i; + for(i = 0; i < uiRegNum; i++) + { + switch(uiReg) + { +// case AX: +// case AY: + case AZ: + s_cDataUpdate |= ACC_UPDATE; + break; +// case GX: +// case GY: + case GZ: + s_cDataUpdate |= GYRO_UPDATE; + break; +// case HX: +// case HY: + case HZ: + s_cDataUpdate |= MAG_UPDATE; + break; +// case Roll: +// case Pitch: + case Yaw: + s_cDataUpdate |= ANGLE_UPDATE; + break; + default: + s_cDataUpdate |= READ_UPDATE; + break; + } + uiReg++; + } +} + + +static void Delayms(uint16_t ucMs) +{ + usleep(ucMs*1000); +} + + +static void AutoScanSensor(char* dev) +{ + int i, iRetry; + char cBuff[1]; + + for(i = 1; i < 10; i++) + { + serial_close(fd); + s_iCurBaud = c_uiBaud[i]; + fd = serial_open(dev , c_uiBaud[i]); + + iRetry = 2; + do + { + s_cDataUpdate = 0; + WitReadReg(AX, 3); + Delayms(200); + while(serial_read_data(fd, cBuff, 1)) + { + WitSerialDataIn(cBuff[0]); + } + if(s_cDataUpdate != 0) + { + printf("%d baud find sensor\r\n\r\n", c_uiBaud[i]); + return ; + } + iRetry--; + }while(iRetry); + } + printf("can not find sensor\r\n"); + printf("please check your connection\r\n"); +} diff --git a/Devices/Packages/wit_wt901ble_reader/src/serial.c b/Devices/Packages/wit_wt901ble_reader/src/serial.c new file mode 100755 index 0000000..693eebd --- /dev/null +++ b/Devices/Packages/wit_wt901ble_reader/src/serial.c @@ -0,0 +1,100 @@ +#include "wit_wt901ble_reader/serial.h" + + +int serial_open(unsigned char* dev, unsigned int baud) +{ + int fd; + fd = open(dev, O_RDWR|O_NOCTTY); + if (fd < 0) return fd; + if(isatty(STDIN_FILENO)==0) + { + printf("standard input is not a terminal device\n"); + } + else + { + printf("isatty success!\n"); + } + + struct termios newtio,oldtio; + if (tcgetattr( fd,&oldtio) != 0) + { + perror("SetupSerial 1"); + printf("tcgetattr( fd,&oldtio) -> %d\n",tcgetattr( fd,&oldtio)); + return -1; + } + bzero( &newtio, sizeof( newtio ) ); + newtio.c_cflag |= CLOCAL | CREAD; + newtio.c_cflag |= CS8; + newtio.c_cflag &= ~PARENB; + + /*设置波特率*/ + switch( baud ) + { + case 2400: + cfsetispeed(&newtio, B2400); + cfsetospeed(&newtio, B2400); + break; + case 4800: + cfsetispeed(&newtio, B4800); + cfsetospeed(&newtio, B4800); + break; + case 9600: + cfsetispeed(&newtio, B9600); + cfsetospeed(&newtio, B9600); + break; + case 115200: + cfsetispeed(&newtio, B115200); + cfsetospeed(&newtio, B115200); + break; + case 230400: + cfsetispeed(&newtio, B230400); + cfsetospeed(&newtio, B230400); + break; + case 460800: + cfsetispeed(&newtio, B460800); + cfsetospeed(&newtio, B460800); + break; + default: + cfsetispeed(&newtio, B9600); + cfsetospeed(&newtio, B9600); + break; + } + newtio.c_cflag &= ~CSTOPB; + newtio.c_cc[VTIME] = 0; + newtio.c_cc[VMIN] = 0; + tcflush(fd,TCIFLUSH); + + if((tcsetattr(fd,TCSANOW,&newtio))!=0) + { + perror("com set error"); + return -1; + } + + return fd; +} + +void serial_close(int fd) +{ + close(fd); +} + +int serial_read_data(int fd, unsigned char *val, int len) +{ + + len=read(fd,val,len); + return len; + +} + +int serial_write_data(int fd, unsigned char *val, int len) +{ + len=write(fd,val,len*sizeof(unsigned char)); + return len; +} + + + + + + + diff --git a/Devices/Packages/wit_wt901ble_reader/src/wit_c_sdk.c b/Devices/Packages/wit_wt901ble_reader/src/wit_c_sdk.c new file mode 100755 index 0000000..2c19663 --- /dev/null +++ b/Devices/Packages/wit_wt901ble_reader/src/wit_c_sdk.c @@ -0,0 +1,502 @@ +#include "wit_wt901ble_reader/wit_c_sdk.h" + +static SerialWrite p_WitSerialWriteFunc = NULL; +static WitI2cWrite p_WitI2cWriteFunc = NULL; +static WitI2cRead p_WitI2cReadFunc = NULL; +static CanWrite p_WitCanWriteFunc = NULL; +static RegUpdateCb p_WitRegUpdateCbFunc = NULL; +static DelaymsCb p_WitDelaymsFunc = NULL; + +static uint8_t s_ucAddr = 0xff; +static uint8_t s_ucWitDataBuff[WIT_DATA_BUFF_SIZE]; +static uint32_t s_uiWitDataCnt = 0, s_uiProtoclo = 0, s_uiReadRegIndex = 0; +int16_t sReg[REGSIZE]; + + +#define FuncW 0x06 +#define FuncR 0x03 + +static const uint8_t __auchCRCHi[256] = { + 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, + 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, + 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, + 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, + 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, + 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, + 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, + 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, + 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, + 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, + 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, + 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, + 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, + 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, + 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, + 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, + 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, + 0x40 +}; +static const uint8_t __auchCRCLo[256] = { + 0x00, 0xC0, 0xC1, 0x01, 0xC3, 0x03, 0x02, 0xC2, 0xC6, 0x06, 0x07, 0xC7, 0x05, 0xC5, 0xC4, + 0x04, 0xCC, 0x0C, 0x0D, 0xCD, 0x0F, 0xCF, 0xCE, 0x0E, 0x0A, 0xCA, 0xCB, 0x0B, 0xC9, 0x09, + 0x08, 0xC8, 0xD8, 0x18, 0x19, 0xD9, 0x1B, 0xDB, 0xDA, 0x1A, 0x1E, 0xDE, 0xDF, 0x1F, 0xDD, + 0x1D, 0x1C, 0xDC, 0x14, 0xD4, 0xD5, 0x15, 0xD7, 0x17, 0x16, 0xD6, 0xD2, 0x12, 0x13, 0xD3, + 0x11, 0xD1, 0xD0, 0x10, 0xF0, 0x30, 0x31, 0xF1, 0x33, 0xF3, 0xF2, 0x32, 0x36, 0xF6, 0xF7, + 0x37, 0xF5, 0x35, 0x34, 0xF4, 0x3C, 0xFC, 0xFD, 0x3D, 0xFF, 0x3F, 0x3E, 0xFE, 0xFA, 0x3A, + 0x3B, 0xFB, 0x39, 0xF9, 0xF8, 0x38, 0x28, 0xE8, 0xE9, 0x29, 0xEB, 0x2B, 0x2A, 0xEA, 0xEE, + 0x2E, 0x2F, 0xEF, 0x2D, 0xED, 0xEC, 0x2C, 0xE4, 0x24, 0x25, 0xE5, 0x27, 0xE7, 0xE6, 0x26, + 0x22, 0xE2, 0xE3, 0x23, 0xE1, 0x21, 0x20, 0xE0, 0xA0, 0x60, 0x61, 0xA1, 0x63, 0xA3, 0xA2, + 0x62, 0x66, 0xA6, 0xA7, 0x67, 0xA5, 0x65, 0x64, 0xA4, 0x6C, 0xAC, 0xAD, 0x6D, 0xAF, 0x6F, + 0x6E, 0xAE, 0xAA, 0x6A, 0x6B, 0xAB, 0x69, 0xA9, 0xA8, 0x68, 0x78, 0xB8, 0xB9, 0x79, 0xBB, + 0x7B, 0x7A, 0xBA, 0xBE, 0x7E, 0x7F, 0xBF, 0x7D, 0xBD, 0xBC, 0x7C, 0xB4, 0x74, 0x75, 0xB5, + 0x77, 0xB7, 0xB6, 0x76, 0x72, 0xB2, 0xB3, 0x73, 0xB1, 0x71, 0x70, 0xB0, 0x50, 0x90, 0x91, + 0x51, 0x93, 0x53, 0x52, 0x92, 0x96, 0x56, 0x57, 0x97, 0x55, 0x95, 0x94, 0x54, 0x9C, 0x5C, + 0x5D, 0x9D, 0x5F, 0x9F, 0x9E, 0x5E, 0x5A, 0x9A, 0x9B, 0x5B, 0x99, 0x59, 0x58, 0x98, 0x88, + 0x48, 0x49, 0x89, 0x4B, 0x8B, 0x8A, 0x4A, 0x4E, 0x8E, 0x8F, 0x4F, 0x8D, 0x4D, 0x4C, 0x8C, + 0x44, 0x84, 0x85, 0x45, 0x87, 0x47, 0x46, 0x86, 0x82, 0x42, 0x43, 0x83, 0x41, 0x81, 0x80, + 0x40 +}; + + +static uint16_t __CRC16(uint8_t *puchMsg, uint16_t usDataLen) +{ + uint8_t uchCRCHi = 0xFF; + uint8_t uchCRCLo = 0xFF; + uint8_t uIndex; + int i = 0; + uchCRCHi = 0xFF; + uchCRCLo = 0xFF; + for (; i= 11) + { + ucSum = __CaliSum(s_ucWitDataBuff, 10); + if(ucSum != s_ucWitDataBuff[10]) + { + s_uiWitDataCnt--; + memcpy(s_ucWitDataBuff, &s_ucWitDataBuff[1], s_uiWitDataCnt); + return ; + } + usData[0] = ((uint16_t)s_ucWitDataBuff[3] << 8) | (uint16_t)s_ucWitDataBuff[2]; + usData[1] = ((uint16_t)s_ucWitDataBuff[5] << 8) | (uint16_t)s_ucWitDataBuff[4]; + usData[2] = ((uint16_t)s_ucWitDataBuff[7] << 8) | (uint16_t)s_ucWitDataBuff[6]; + usData[3] = ((uint16_t)s_ucWitDataBuff[9] << 8) | (uint16_t)s_ucWitDataBuff[8]; + CopeWitData(s_ucWitDataBuff[1], usData, 4); + s_uiWitDataCnt = 0; + } + break; + case WIT_PROTOCOL_MODBUS: + if(s_uiWitDataCnt > 2) + { + if(s_ucWitDataBuff[1] != FuncR) + { + s_uiWitDataCnt--; + memcpy(s_ucWitDataBuff, &s_ucWitDataBuff[1], s_uiWitDataCnt); + return ; + } + if(s_uiWitDataCnt < (s_ucWitDataBuff[2] + 5))return ; + usTemp = ((uint16_t)s_ucWitDataBuff[s_uiWitDataCnt-2] << 8) | s_ucWitDataBuff[s_uiWitDataCnt-1]; + usCRC16 = __CRC16(s_ucWitDataBuff, s_uiWitDataCnt-2); + if(usTemp != usCRC16) + { + s_uiWitDataCnt--; + memcpy(s_ucWitDataBuff, &s_ucWitDataBuff[1], s_uiWitDataCnt); + return ; + } + usTemp = s_ucWitDataBuff[2] >> 1; + for(i = 0; i < usTemp; i++) + { + sReg[i+s_uiReadRegIndex] = ((uint16_t)s_ucWitDataBuff[(i<<1)+3] << 8) | s_ucWitDataBuff[(i<<1)+4]; + } + p_WitRegUpdateCbFunc(s_uiReadRegIndex, usTemp); + s_uiWitDataCnt = 0; + } + break; + case WIT_PROTOCOL_CAN: + case WIT_PROTOCOL_I2C: + s_uiWitDataCnt = 0; + break; + } + if(s_uiWitDataCnt == WIT_DATA_BUFF_SIZE)s_uiWitDataCnt = 0; +} +int32_t WitI2cFuncRegister(WitI2cWrite write_func, WitI2cRead read_func) +{ + if(!write_func)return WIT_HAL_INVAL; + if(!read_func)return WIT_HAL_INVAL; + p_WitI2cWriteFunc = write_func; + p_WitI2cReadFunc = read_func; + return WIT_HAL_OK; +} +int32_t WitCanWriteRegister(CanWrite Write_func) +{ + if(!Write_func)return WIT_HAL_INVAL; + p_WitCanWriteFunc = Write_func; + return WIT_HAL_OK; +} +void WitCanDataIn(uint8_t ucData[8], uint8_t ucLen) +{ + uint16_t usData[3]; + if(p_WitRegUpdateCbFunc == NULL)return ; + if(ucLen < 8)return ; + switch(s_uiProtoclo) + { + case WIT_PROTOCOL_CAN: + if(ucData[0] != 0x55)return ; + usData[0] = ((uint16_t)ucData[3] << 8) | ucData[2]; + usData[1] = ((uint16_t)ucData[5] << 8) | ucData[4]; + usData[2] = ((uint16_t)ucData[7] << 8) | ucData[6]; + CopeWitData(ucData[1], usData, 3); + break; + case WIT_PROTOCOL_NORMAL: + case WIT_PROTOCOL_MODBUS: + case WIT_PROTOCOL_I2C: + break; + } +} +int32_t WitRegisterCallBack(RegUpdateCb update_func) +{ + if(!update_func)return WIT_HAL_INVAL; + p_WitRegUpdateCbFunc = update_func; + return WIT_HAL_OK; +} +int32_t WitWriteReg(uint32_t uiReg, uint16_t usData) +{ + uint16_t usCRC; + uint8_t ucBuff[8]; + if(uiReg >= REGSIZE)return WIT_HAL_INVAL; + switch(s_uiProtoclo) + { + case WIT_PROTOCOL_NORMAL: + if(p_WitSerialWriteFunc == NULL)return WIT_HAL_EMPTY; + ucBuff[0] = 0xFF; + ucBuff[1] = 0xAA; + ucBuff[2] = uiReg & 0xFF; + ucBuff[3] = usData & 0xff; + ucBuff[4] = usData >> 8; + p_WitSerialWriteFunc(ucBuff, 5); + break; + case WIT_PROTOCOL_MODBUS: + if(p_WitSerialWriteFunc == NULL)return WIT_HAL_EMPTY; + ucBuff[0] = s_ucAddr; + ucBuff[1] = FuncW; + ucBuff[2] = uiReg >> 8; + ucBuff[3] = uiReg & 0xFF; + ucBuff[4] = usData >> 8; + ucBuff[5] = usData & 0xff; + usCRC = __CRC16(ucBuff, 6); + ucBuff[6] = usCRC >> 8; + ucBuff[7] = usCRC & 0xff; + p_WitSerialWriteFunc(ucBuff, 8); + break; + case WIT_PROTOCOL_CAN: + if(p_WitCanWriteFunc == NULL)return WIT_HAL_EMPTY; + ucBuff[0] = 0xFF; + ucBuff[1] = 0xAA; + ucBuff[2] = uiReg & 0xFF; + ucBuff[3] = usData & 0xff; + ucBuff[4] = usData >> 8; + p_WitCanWriteFunc(s_ucAddr, ucBuff, 5); + break; + case WIT_PROTOCOL_I2C: + if(p_WitI2cWriteFunc == NULL)return WIT_HAL_EMPTY; + ucBuff[0] = usData & 0xff; + ucBuff[1] = usData >> 8; + if(p_WitI2cWriteFunc(s_ucAddr << 1, uiReg, ucBuff, 2) != 1) + { + //printf("i2c write fail\r\n"); + } + break; + default: + return WIT_HAL_INVAL; + } + return WIT_HAL_OK; +} +int32_t WitReadReg(uint32_t uiReg, uint32_t uiReadNum) +{ + uint16_t usTemp, i; + uint8_t ucBuff[8]; + if((uiReg + uiReadNum) >= REGSIZE)return WIT_HAL_INVAL; + switch(s_uiProtoclo) + { + case WIT_PROTOCOL_NORMAL: + if(uiReadNum > 4)return WIT_HAL_INVAL; + if(p_WitSerialWriteFunc == NULL)return WIT_HAL_EMPTY; + ucBuff[0] = 0xFF; + ucBuff[1] = 0xAA; + ucBuff[2] = 0x27; + ucBuff[3] = uiReg & 0xff; + ucBuff[4] = uiReg >> 8; + p_WitSerialWriteFunc(ucBuff, 5); + break; + case WIT_PROTOCOL_MODBUS: + if(p_WitSerialWriteFunc == NULL)return WIT_HAL_EMPTY; + usTemp = uiReadNum << 1; + if((usTemp + 5) > WIT_DATA_BUFF_SIZE)return WIT_HAL_NOMEM; + ucBuff[0] = s_ucAddr; + ucBuff[1] = FuncR; + ucBuff[2] = uiReg >> 8; + ucBuff[3] = uiReg & 0xFF; + ucBuff[4] = uiReadNum >> 8; + ucBuff[5] = uiReadNum & 0xff; + usTemp = __CRC16(ucBuff, 6); + ucBuff[6] = usTemp >> 8; + ucBuff[7] = usTemp & 0xff; + p_WitSerialWriteFunc(ucBuff, 8); + break; + case WIT_PROTOCOL_CAN: + if(uiReadNum > 3)return WIT_HAL_INVAL; + if(p_WitCanWriteFunc == NULL)return WIT_HAL_EMPTY; + ucBuff[0] = 0xFF; + ucBuff[1] = 0xAA; + ucBuff[2] = 0x27; + ucBuff[3] = uiReg & 0xff; + ucBuff[4] = uiReg >> 8; + p_WitCanWriteFunc(s_ucAddr, ucBuff, 5); + break; + case WIT_PROTOCOL_I2C: + if(p_WitI2cReadFunc == NULL)return WIT_HAL_EMPTY; + usTemp = uiReadNum << 1; + if(WIT_DATA_BUFF_SIZE < usTemp)return WIT_HAL_NOMEM; + if(p_WitI2cReadFunc(s_ucAddr << 1, uiReg, s_ucWitDataBuff, usTemp) == 1) + { + if(p_WitRegUpdateCbFunc == NULL)return WIT_HAL_EMPTY; + for(i = 0; i < uiReadNum; i++) + { + sReg[i+uiReg] = ((uint16_t)s_ucWitDataBuff[(i<<1)+1] << 8) | s_ucWitDataBuff[i<<1]; + } + p_WitRegUpdateCbFunc(uiReg, uiReadNum); + } + + break; + default: + return WIT_HAL_INVAL; + } + s_uiReadRegIndex = uiReg; + + return WIT_HAL_OK; +} +int32_t WitInit(uint32_t uiProtocol, uint8_t ucAddr) +{ + if(uiProtocol > WIT_PROTOCOL_I2C)return WIT_HAL_INVAL; + s_uiProtoclo = uiProtocol; + s_ucAddr = ucAddr; + s_uiWitDataCnt = 0; + return WIT_HAL_OK; +} +void WitDeInit(void) +{ + p_WitSerialWriteFunc = NULL; + p_WitI2cWriteFunc = NULL; + p_WitI2cReadFunc = NULL; + p_WitCanWriteFunc = NULL; + p_WitRegUpdateCbFunc = NULL; + s_ucAddr = 0xff; + s_uiWitDataCnt = 0; + s_uiProtoclo = 0; +} + +int32_t WitDelayMsRegister(DelaymsCb delayms_func) +{ + if(!delayms_func)return WIT_HAL_INVAL; + p_WitDelaymsFunc = delayms_func; + return WIT_HAL_OK; +} + +char CheckRange(short sTemp,short sMin,short sMax) +{ + if ((sTemp>=sMin)&&(sTemp<=sMax)) return 1; + else return 0; +} +/*Acceleration calibration demo*/ +int32_t WitStartAccCali(void) +{ +/* + First place the equipment horizontally, and then perform the following operations +*/ + if(WitWriteReg(KEY, KEY_UNLOCK) != WIT_HAL_OK) return WIT_HAL_ERROR;// unlock reg + if(s_uiProtoclo == WIT_PROTOCOL_MODBUS) p_WitDelaymsFunc(20); + else if(s_uiProtoclo == WIT_PROTOCOL_NORMAL) p_WitDelaymsFunc(1); + else ; + if(WitWriteReg(CALSW, CALGYROACC) != WIT_HAL_OK) return WIT_HAL_ERROR; + return WIT_HAL_OK; +} +int32_t WitStopAccCali(void) +{ + if(WitWriteReg(CALSW, NORMAL) != WIT_HAL_OK) return WIT_HAL_ERROR; + if(s_uiProtoclo == WIT_PROTOCOL_MODBUS) p_WitDelaymsFunc(20); + else if(s_uiProtoclo == WIT_PROTOCOL_NORMAL) p_WitDelaymsFunc(1); + else ; + if(WitWriteReg(SAVE, SAVE_PARAM) != WIT_HAL_OK) return WIT_HAL_ERROR; + return WIT_HAL_OK; +} +/*Magnetic field calibration*/ +int32_t WitStartMagCali(void) +{ + if(WitWriteReg(KEY, KEY_UNLOCK) != WIT_HAL_OK) return WIT_HAL_ERROR; + if(s_uiProtoclo == WIT_PROTOCOL_MODBUS) p_WitDelaymsFunc(20); + else if(s_uiProtoclo == WIT_PROTOCOL_NORMAL) p_WitDelaymsFunc(1); + else ; + if(WitWriteReg(CALSW, CALMAGMM) != WIT_HAL_OK) return WIT_HAL_ERROR; + return WIT_HAL_OK; +} +int32_t WitStopMagCali(void) +{ + if(WitWriteReg(KEY, KEY_UNLOCK) != WIT_HAL_OK) return WIT_HAL_ERROR; + if(s_uiProtoclo == WIT_PROTOCOL_MODBUS) p_WitDelaymsFunc(20); + else if(s_uiProtoclo == WIT_PROTOCOL_NORMAL) p_WitDelaymsFunc(1); + else ; + if(WitWriteReg(CALSW, NORMAL) != WIT_HAL_OK) return WIT_HAL_ERROR; + return WIT_HAL_OK; +} +/*change Band*/ +int32_t WitSetUartBaud(int32_t uiBaudIndex) +{ + if(!CheckRange(uiBaudIndex,WIT_BAUD_4800,WIT_BAUD_230400)) + { + return WIT_HAL_INVAL; + } + if(WitWriteReg(KEY, KEY_UNLOCK) != WIT_HAL_OK) return WIT_HAL_ERROR; + if(s_uiProtoclo == WIT_PROTOCOL_MODBUS) p_WitDelaymsFunc(20); + else if(s_uiProtoclo == WIT_PROTOCOL_NORMAL) p_WitDelaymsFunc(1); + else ; + if(WitWriteReg(BAUD, uiBaudIndex) != WIT_HAL_OK) return WIT_HAL_ERROR; + return WIT_HAL_OK; +} +/*change Can Band*/ +int32_t WitSetCanBaud(int32_t uiBaudIndex) +{ + if(!CheckRange(uiBaudIndex,CAN_BAUD_1000000,CAN_BAUD_3000)) + { + return WIT_HAL_INVAL; + } + if(WitWriteReg(KEY, KEY_UNLOCK) != WIT_HAL_OK) return WIT_HAL_ERROR; + if(s_uiProtoclo == WIT_PROTOCOL_MODBUS) p_WitDelaymsFunc(20); + else if(s_uiProtoclo == WIT_PROTOCOL_NORMAL) p_WitDelaymsFunc(1); + else ; + if(WitWriteReg(BAUD, uiBaudIndex) != WIT_HAL_OK) return WIT_HAL_ERROR; + return WIT_HAL_OK; +} +/*change Bandwidth*/ +int32_t WitSetBandwidth(int32_t uiBaudWidth) +{ + if(!CheckRange(uiBaudWidth,BANDWIDTH_256HZ,BANDWIDTH_5HZ)) + { + return WIT_HAL_INVAL; + } + if(WitWriteReg(KEY, KEY_UNLOCK) != WIT_HAL_OK) return WIT_HAL_ERROR; + if(s_uiProtoclo == WIT_PROTOCOL_MODBUS) p_WitDelaymsFunc(20); + else if(s_uiProtoclo == WIT_PROTOCOL_NORMAL) p_WitDelaymsFunc(1); + else ; + if(WitWriteReg(BANDWIDTH, uiBaudWidth) != WIT_HAL_OK) return WIT_HAL_ERROR; + return WIT_HAL_OK; +} + +/*change output rate */ +int32_t WitSetOutputRate(int32_t uiRate) +{ + if(!CheckRange(uiRate,RRATE_02HZ,RRATE_NONE)) + { + return WIT_HAL_INVAL; + } + if(WitWriteReg(KEY, KEY_UNLOCK) != WIT_HAL_OK) return WIT_HAL_ERROR; + if(s_uiProtoclo == WIT_PROTOCOL_MODBUS) p_WitDelaymsFunc(20); + else if(s_uiProtoclo == WIT_PROTOCOL_NORMAL) p_WitDelaymsFunc(1); + else ; + if(WitWriteReg(RRATE, uiRate) != WIT_HAL_OK) return WIT_HAL_ERROR; + return WIT_HAL_OK; +} + +/*change WitSetContent */ +int32_t WitSetContent(int32_t uiRsw) +{ + if(!CheckRange(uiRsw,RSW_TIME,RSW_MASK)) + { + return WIT_HAL_INVAL; + } + if(WitWriteReg(KEY, KEY_UNLOCK) != WIT_HAL_OK) return WIT_HAL_ERROR; + if(s_uiProtoclo == WIT_PROTOCOL_MODBUS) p_WitDelaymsFunc(20); + else if(s_uiProtoclo == WIT_PROTOCOL_NORMAL) p_WitDelaymsFunc(1); + else ; + if(WitWriteReg(RSW, uiRsw) != WIT_HAL_OK) return WIT_HAL_ERROR; + return WIT_HAL_OK; +} + + + diff --git a/Devices/Packages/wit_wt901ble_reader/src/wit_wt901ble_reader.cc b/Devices/Packages/wit_wt901ble_reader/src/wit_wt901ble_reader.cc new file mode 100755 index 0000000..bda9c07 --- /dev/null +++ b/Devices/Packages/wit_wt901ble_reader/src/wit_wt901ble_reader.cc @@ -0,0 +1,387 @@ +#include "wit_wt901ble_reader/wit_wt901ble_reader.h" + +namespace wt901ble_reader +{ + + Diagnostics::Diagnostics(string& nodename):nodename_(nodename), nh_("~") + { + imu_diagnostics_topic_name_ = ""; + if(!ros::param::get(nodename_ + "/imu_diagnostics_topic_name", imu_diagnostics_topic_name_)) + { + imu_diagnostics_topic_name_ = "/imu_diagnostics"; + ROS_WARN("Node: %s cannot get params", nodename_.c_str()); + } + diagnostics_pub = nh_.advertise(imu_diagnostics_topic_name_, 10); + diagnostics_thread = new boost::thread(&Diagnostics::run_diagnostics, this); + } + Diagnostics::~Diagnostics() + { + diagnostics_thread->join(); + delete(diagnostics_thread); + } + + void Diagnostics::run_diagnostics() + { + ros::Duration(0.5).sleep(); + connection.name = "connection"; + connection.hardware_id = "IMU_sensors"; + connection.level = connection.STALE; + connection.message = "Initialized"; + pubDiagnostics(); + ros::Rate r(5); + boost::lock_guard lock(mutex); + while(ros::ok()) + { + ros::spinOnce(); + if(!is_imu_connected) + { + connection.level = connection.ERROR; + connection.message = "Cannot connect to device"; + } + else{ + if(is_imu_data_available) + { + connection.level = connection.OK; + connection.message = "Connected to device, datas is received"; + } + else + { + connection.level = connection.WARN; + connection.message = "Connected to device but datas is not received"; + } + } + pubDiagnostics(); + r.sleep(); + } + } + void Diagnostics::pubDiagnostics() { + diagnostic_msgs::DiagnosticArray diagnostic; + diagnostic.status.push_back(connection); + if(diagnostic.status != prev_diagnostic.status) + { + diagnostic.header.seq = prev_diagnostic.header.seq+1; + diagnostic.header.stamp = ros::Time::now(); + diagnostic.header.frame_id = "frame_id"; + diagnostics_pub.publish(diagnostic); + prev_diagnostic = diagnostic; + } + } + + wt901ble_node::wt901ble_node(const string& nodename):nodename_(nodename), nh_("~") + { + userParams = new UserParams(); + wt901ble_data_ = new wt901ble_data(); + if(LoadParams(userParams, nodename_)){ + ROS_INFO("%s : Params is loaded", nodename_.c_str()); + } + else ROS_ERROR("%s : Fail to load params", nodename_.c_str()); + isDataAvailable = false; + wt901ble_data_pub_ = nh_.advertise(userParams->topic_name,10,false); + isConnected_ = false; + is_imu_connected = false; + is_imu_data_available = false; + is_try_to_connect = false; + is_reconnected = false; + lost_data_count = 0; + reconnect_timer = nh_.createWallTimer(::ros::WallDuration(0.5), &wt901ble_node::ReconnectSerial, this); + get_and_publish_timer = nh_.createWallTimer(::ros::WallDuration(0.02), &wt901ble_node::GetAndPublishData, this); + reconnect_timer.stop(); + if(detect_port(userParams->portname)) + { + ROS_INFO("Port: %s is found",userParams->portname.c_str()); + try { + serial_ = new serial::Serial(userParams->portname, userParams->baudrate, serial::Timeout::simpleTimeout(500)); + } + catch(const std::exception& ex) + { + isConnected_ = false; + is_imu_connected = false; + ROS_WARN("Can not open serial port: %s", userParams->portname.c_str()); + cerr << "Unhandled Exception: " << ex.what() << endl; + } + if(serial_!=NULL) + { + if(serial_-> isOpen()){ + ROS_INFO("%s: Connected to serial port: %s, baudrate: %d", nodename_.c_str(), userParams->portname.c_str(), userParams->baudrate); + isConnected_ = true; + is_imu_connected = true; + } + } + get_and_publish_timer.start(); + } + + } + + wt901ble_node::~wt901ble_node() + { + delete(serial_); + delete(userParams); + delete(wt901ble_data_); + } + + void wt901ble_node::GetAndPublishData(const ::ros::WallTimerEvent& timer_event) + { + if(isConnected_) + { + if(recvData(data_buf)) + { + // ROS_INFO_ONCE("%s: Start to get datas and publish", nodename_.c_str()); + // size_t data_buf_size = sizeof(data_buf)/sizeof(data_buf[0]); + // int len = static_cast(data_buf_size); + // ROS_INFO("len: %d, data_buf: %x, %x, %x, %x, %x, %x, %x, %x, %x, %x, %x, %x, %x, %x, %x, %x, %x, %x, %x, %x", + // len, data_buf[0], data_buf[1], data_buf[2], data_buf[3], data_buf[4], data_buf[5], + // data_buf[6], data_buf[7], data_buf[8], data_buf[9], data_buf[10], data_buf[11], + // data_buf[12], data_buf[13], data_buf[14], data_buf[15], data_buf[16], data_buf[17], data_buf[18], data_buf[19]); + + int16_t aX = (data_buf[3]<<8U)|data_buf[2]; + int16_t aY = (data_buf[5]<<8U)|data_buf[4]; + int16_t aZ = (data_buf[7]<<8U)|data_buf[6]; + int16_t wX = (data_buf[9]<<8U)|data_buf[8]; + int16_t wY = (data_buf[11]<<8U)|data_buf[10]; + int16_t wZ = (data_buf[13]<<8U)|data_buf[12]; + int16_t Roll = (data_buf[15]<<8U)|data_buf[14]; + int16_t Pitch = (data_buf[17]<<8U)|data_buf[16]; + int16_t Yaw = (data_buf[19]<<8U)|data_buf[18]; + + wt901ble_data_->aX = ((double)(aX))/32768*156.9064; + wt901ble_data_->aY = ((double)(aY))/32768*156.9064; + wt901ble_data_->aZ = ((double)(aZ))/32768*156.9064; + wt901ble_data_->wX = ((double)(wX))/32768*2000; + wt901ble_data_->wY = ((double)(wY))/32768*2000; + wt901ble_data_->wZ = ((double)(wZ))/32768*2000; + wt901ble_data_->Roll = ((double)(Roll))/32768*180; + wt901ble_data_->Pitch = ((double)(Pitch))/32768*180; + wt901ble_data_->Yaw = ((double)(Yaw))/32768*180; + + wt901ble_data_->wX = (wt901ble_data_->wX*M_PI)/180; + wt901ble_data_->wY = (wt901ble_data_->wY*M_PI)/180; + wt901ble_data_->wZ = (wt901ble_data_->wZ*M_PI)/180; + + // ROS_INFO("Roll: %f, Pitch: %f, Yaw: %f", + // wt901ble_data_->Roll, wt901ble_data_->Pitch, wt901ble_data_->Yaw); + wt901ble_data_->Roll = (wt901ble_data_->Roll*M_PI)/180; + wt901ble_data_->Pitch = (wt901ble_data_->Pitch*M_PI)/180; + wt901ble_data_->Yaw = (wt901ble_data_->Yaw*M_PI)/180; + + imu_data.header.frame_id = userParams->frame_id; + imu_data.header.stamp = ros::Time::now(); + imu_data.orientation = getQuaternion(wt901ble_data_->Yaw, wt901ble_data_->Pitch, wt901ble_data_->Roll); + // imu_data.orientation_covariance[0] = 2.8900000000000004e-08; + // imu_data.orientation_covariance[4] = 2.8900000000000004e-08; + // imu_data.orientation_covariance[8] = 2.8900000000000004e-08; + imu_data.orientation_covariance[0] = 0.1; + imu_data.orientation_covariance[4] = 0.1; + imu_data.orientation_covariance[8] = 0.1; + imu_data.angular_velocity.x = wt901ble_data_->wX; + imu_data.angular_velocity.y = wt901ble_data_->wY; + imu_data.angular_velocity.z = wt901ble_data_->wZ; + // imu_data.angular_velocity_covariance[0] = 2.8900000000000004e-08; + // imu_data.angular_velocity_covariance[4] = 2.8900000000000004e-08; + // imu_data.angular_velocity_covariance[8] = 2.8900000000000004e-08; + imu_data.angular_velocity_covariance[0] = 8e-6; + imu_data.angular_velocity_covariance[4] = 8e-6; + imu_data.angular_velocity_covariance[8] = 3e-7; + imu_data.linear_acceleration.x = wt901ble_data_->aX; + imu_data.linear_acceleration.y = wt901ble_data_->aY; + imu_data.linear_acceleration.z = wt901ble_data_->aZ; + // imu_data.linear_acceleration_covariance[0] = 0.009604000000000001; + // imu_data.linear_acceleration_covariance[4] = 0.009604000000000001; + // imu_data.linear_acceleration_covariance[8] = 0.009604000000000001; + imu_data.linear_acceleration_covariance[0] = 5e-5; + imu_data.linear_acceleration_covariance[4] = 0.0001; + imu_data.linear_acceleration_covariance[8] = 0.00013; + wt901ble_data_pub_.publish(imu_data); + // ROS_INFO("Roll: %f, Pitch: %f, Yaw: %f", + // wt901ble_data_->Roll, wt901ble_data_->Pitch, wt901ble_data_->Yaw); + // ROS_INFO("aX: %f, aY: %f, aZ: %f, wX: %f, wY: %f, wZ: %f, Roll: %f, Pitch: %f, Yaw: %f", + // wt901ble_data_->aX, wt901ble_data_->aY, wt901ble_data_->aZ, + // wt901ble_data_->wX, wt901ble_data_->wY, wt901ble_data_->wZ, + // wt901ble_data_->Roll, wt901ble_data_->Pitch, wt901ble_data_->Yaw); + is_imu_data_available = true; + lost_data_count = 0; + is_try_to_connect = false; + } + else + { + lost_data_count ++; + if(lost_data_count>30) + { + is_imu_data_available = false; + if(is_try_to_connect ==false) { is_try_to_connect=true; reconnect_timer.start();} + // ROS_WARN("IMU reader node: lost_data"); + lost_data_count = 0; + } + } + } + + } + + void wt901ble_node::ReconnectSerial(const ::ros::WallTimerEvent& timer_event) + { + if(is_try_to_connect) + { + if(!is_reconnected) + { + if(serial_!=NULL) + { + isConnected_ = false; + is_imu_connected = false; + // boost::this_thread::sleep(boost::posix_time::milliseconds(500)); + ros::Duration(0.5).sleep(); + serial_->flush(); + serial_->close(); + delete(serial_); + serial_ = new serial::Serial(userParams->portname, userParams->baudrate, serial::Timeout::simpleTimeout(500)); + } + if(serial_!=NULL) + { + if(serial_-> isOpen()){ + ROS_WARN("%s: Reconnected to serial port (IMU sensor): %s, baudrate: %d", nodename_.c_str(), userParams->portname.c_str(), userParams->baudrate); + isConnected_ = true; + is_imu_connected = true; + } + } + is_reconnected = true; + reconnect_timer.stop(); + } + } + } + + geometry_msgs::Quaternion wt901ble_node::getQuaternion(double yaw, double pitch, double roll) + { + double cy = cos(yaw * 0.5); + double sy = sin(yaw * 0.5); + double cp = cos(pitch * 0.5); + double sp = sin(pitch * 0.5); + double cr = cos(roll * 0.5); + double sr = sin(roll * 0.5); + geometry_msgs::Quaternion q; + q.x = sr * cp * cy - cr * sp * sy; + q.y = cr * sp * cy + sr * cp * sy; + q.z = cr * cp * sy - sr * sp * cy; + q.w = cr * cp * cy + sr * sp * sy; + return q; + } + + bool wt901ble_node::LoadParams(UserParams* userParams_, const string& node_name){ + bool result = true; + try{ + int baudrate; + if(!ros::param::get(node_name + "/baudrate", baudrate)) result = false; + if(!ros::param::get(node_name + "/portname", userParams_->portname)) result = false; + if(!ros::param::get(node_name + "/topic_name", userParams_->topic_name)) result = false; + if(!ros::param::get(node_name + "/frame_id", userParams_->frame_id)) result = false; + userParams_->baudrate = static_cast(baudrate); + } + catch(const std::exception& ex) + { + std::cerr << "Fail to load user params caught: " << ex.what() << std::endl; + result = false; + } + return result; + } + + size_t wt901ble_node::readN(uint8_t *buf, size_t len){ + size_t offset = 0; + offset = serial_->read(buf,len); + return offset; + } + + bool wt901ble_node::recvData(uint8_t *buf){ + bool ret = false; + uint8_t ch[2]; + if(readN(ch, 2) == 2) + { + if(ch[0]==wt901ble_header_imu[0]&& + ch[1]==wt901ble_header_imu[1]) + { + buf[0] = ch[0]; + buf[1] = ch[1]; + if(readN(&buf[2],18)==18) + { + ret = true; + } + } + } + // if (readN(buf, wt901ble_data_length) != 20) { + // // continue; + // return false; + // } + // if (buf[0] == wt901ble_header_imu[0]&& + // buf[1] == wt901ble_header_imu[1]) + // { + // ret = true; + // } + return ret; + } + + bool wt901ble_node::receiveData() + { + bool ret = false; + isDataAvailable = false; + uint8_t ch; + while (!ret) { + + if(readN(&ch, 1) != 1){ + continue; + } + if(ch == wt901ble_header_imu[0]){ + data_buf[0] = ch; + if(readN(&ch, 1) == 1){ + if(ch == wt901ble_header_imu[2]) + { + data_buf[1] = ch; + if(readN(&data_buf[2], 18) == 18) + { + ret = true; + isDataAvailable = true; + } + } + } + } + + } + return ret; + } + + bool wt901ble_node::detect_port(string& port_name) + { + bool result = false; + // vector devices_found = serial::list_ports(); + + // vector::iterator iter = devices_found.begin(); + // bool result = false; + // while( iter != devices_found.end() ) + // { + // serial::PortInfo device = *iter++; + // // ROS_INFO( "(%s, %s, %s)\n", device.port.c_str(), device.description.c_str(), + // // device.hardware_id.c_str() ); + // if(device.port == port_name) + // { + // // ROS_INFO("Found port: %s",device.port.c_str()); + // result = true; + // break; + // } + // } + // return result; + string product = "USB Serial"; + char _port[50] = ""; + udevadmInfo *ludev = new udevadmInfo(product.c_str()); + if(ludev->init() == EXIT_FAILURE) + return false; + if(!ludev->scanDevices()) { return false; } + else { + strcpy(_port, ludev->port); + } + ROS_INFO("Found port: %s",_port); + + if(strstr(_port,"/dev/ttyUSB") != NULL) { + // std::string cmd_ = "sudo chmod 777 " + (std::string)_port; + // system(cmd_.c_str()); + userParams->portname = (std::string)_port; + result = true; + } + return result; + } + +} \ No newline at end of file diff --git a/Devices/Packages/wit_wt901ble_reader/src/wit_wt901ble_reader_main.cc b/Devices/Packages/wit_wt901ble_reader/src/wit_wt901ble_reader_main.cc new file mode 100755 index 0000000..f500131 --- /dev/null +++ b/Devices/Packages/wit_wt901ble_reader/src/wit_wt901ble_reader_main.cc @@ -0,0 +1,19 @@ +#include "wit_wt901ble_reader/wit_wt901ble_reader.h" + +using namespace std; +using namespace wt901ble_reader; + +wt901ble_node* node; +Diagnostics* diagnostics_node; +int main(int argc, char** argv) +{ + ros::init(argc, argv, "wit_wt901ble_reader_node"); + ros::start(); + string nodename = ros::this_node::getName(); + node = new wt901ble_node(nodename); + diagnostics_node = new Diagnostics(nodename); + ros::spin(); + delete(node); + delete(diagnostics_node); + return 0; +} \ No newline at end of file diff --git a/Devices/auto_setup.sh b/Devices/auto_setup.sh new file mode 100755 index 0000000..bfe8685 --- /dev/null +++ b/Devices/auto_setup.sh @@ -0,0 +1,13 @@ +sudo apt-get install -y build-essential libboost-system-dev libboost-thread-dev libboost-program-options-dev libboost-test-dev +sudo apt-get install -y libtinyxml-dev +sudo apt-get install -y libmuparser-dev +sudo apt-get install -y libudev-dev +sudo apt-get install -y ros-noetic-serial +sudo apt-get install -y ros-noetic-realtime-tools +sudo apt-get install -y ros-noetic-diagnostic-updater +sudo apt-get install -y ros-noetic-control-msgs +sudo apt-get install -y ros-noetic-eigen-conversions +sudo apt-get install -y ros-noetic-controller-manager +sudo apt-get install -y ros-noetic-filters +sudo apt-get install -y ros-noetic-joint-limits-interface +sudo apt-get install -y ros-noetic-pcl-ros \ No newline at end of file diff --git a/Localizations/Cores/loc_core/CMakeLists.txt b/Localizations/Cores/loc_core/CMakeLists.txt new file mode 100644 index 0000000..b9968c2 --- /dev/null +++ b/Localizations/Cores/loc_core/CMakeLists.txt @@ -0,0 +1,208 @@ +cmake_minimum_required(VERSION 3.0.2) +project(loc_core) + +## Compile as C++11, supported in ROS Kinetic and newer +# add_compile_options(-std=c++11) +set_directory_properties(PROPERTIES COMPILE_OPTIONS "-std=c++14") + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + geometry_msgs + std_msgs + tf + tf2_ros +) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a exec_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# geometry_msgs# std_msgs +# ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( + INCLUDE_DIRS include +# LIBRARIES loc_core + CATKIN_DEPENDS geometry_msgs std_msgs tf tf2_ros +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( + include + ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +# add_library(${PROJECT_NAME} +# src/${PROJECT_NAME}/loc_core.cpp +# ) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +## With catkin_make all packages are built within a single CMake context +## The recommended prefix ensures that target names across packages don't collide +# add_executable(${PROJECT_NAME}_node src/loc_core_node.cpp) + +## Rename C++ executable without prefix +## The above recommended prefix causes long target names, the following renames the +## target back to the shorter version for ease of user use +## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" +# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +# target_link_libraries(${PROJECT_NAME}_node +# ${catkin_LIBRARIES} +# ) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# catkin_install_python(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html +# install(TARGETS ${PROJECT_NAME}_node +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark libraries for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html +# install(TARGETS ${PROJECT_NAME} +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE +) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_loc_core.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/Localizations/Cores/loc_core/include/loc_core/common.h b/Localizations/Cores/loc_core/include/loc_core/common.h new file mode 100644 index 0000000..90f816c --- /dev/null +++ b/Localizations/Cores/loc_core/include/loc_core/common.h @@ -0,0 +1,42 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Locus Robotics + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#ifndef _LOC_CORE_COMMON_H_INCLUDED_ +#define _LOC_CORE_COMMON_H_INCLUDED_ + +#include +#include + +using TFListenerPtr = std::shared_ptr; + +#endif // _LOC_CORE_COMMON_H_INCLUDED_ diff --git a/Localizations/Cores/loc_core/include/loc_core/localization.h b/Localizations/Cores/loc_core/include/loc_core/localization.h new file mode 100644 index 0000000..08ef05a --- /dev/null +++ b/Localizations/Cores/loc_core/include/loc_core/localization.h @@ -0,0 +1,136 @@ +#ifndef _LOC_CORE_LOCALIZATION_H_INCLUDED_ +#define _LOC_CORE_LOCALIZATION_H_INCLUDED_ +#include +#include +#include +#include +#include +#include +#include + +namespace loc_core +{ + enum slam_state_en + { + Mapping, + Localization, + Calibrations, + Ready, + Error + }; + + /** + * @class BaseLocalization + * @brief Provides an interface for locazation + * @brief All package written as plugins for the localization stack must adhere to this interface. + */ + class BaseLocalization + { + public: + using Ptr = std::shared_ptr; + + /** + * @brief Virtual destructor for the interface + */ + virtual ~BaseLocalization() {} + + /** + * @brief Initialization function for loc base + * @param nh A Node handle + * + */ + virtual void initialize(ros::NodeHandle nh, TFListenerPtr tf) = 0; + + /** + * @brief Loading a Activate Map File name + * @param activated_map_filename The Map File name + * @return True if activated_map_filename is exsits, else Fasle + */ + virtual bool loadActivateMapFileName(std::string& activated_map_filename) = 0; + + /** + * @brief Saving a Activate Map File name + * @param activated_map_filename The Map File name + * @return True if activated_map_filename is exsits, else Fasle + */ + virtual bool saveActivateMapFileName(const std::string& activated_map_filename) = 0; + + /** + * @brief Create any files to a folder of the path + * @param path The path where is be saved any files (png, yaml ...) + * @param map_name The name folder and files + */ + virtual bool createStaticMap(const std::string map_name) = 0; + + /** + * @brief Load a map given a path to a yaml file + * @param filename The file name + * @return True/False If load map succesed or not done + */ + virtual bool changeStaticMap(const std::string filename) = 0; + + /** + * @brief Load a virtual walls map given a path to a yaml file + * @param filename The file name + * @return True/False If load map succesed or not done + */ + virtual bool changeVirtualWallsMap(const std::string filename) = 0; + + /** + * @brief Get the list of map folder in the workingdirection + * @param directories The list map name + */ + virtual void listMapFiles(std::stringstream &directories) = 0; + + /** + * @brief Start the mapping process. + * When the state variable 'SlamState' is 'Ready', this function can be called. + * Upon successful invocation, the state variable 'SlamState' will change to 'Mapping'. + * @return True/False Call fucntion is successed/failed. + */ + virtual bool startMapping() = 0; + + /** + * @brief Turn off the Localization mode. + * At this point, the robot's coordinate information will not be updated. + * If called successfully, the 'SlamState' variable will change to 'Ready'. + * @return True/False Call fucntion is successed/failed + */ + virtual bool stopMapping() = 0; + + /** + * @brief Set the initial position of the robot on the map. + * The condition to call the function is that the 'SlamState' is 'Localization'. + * When the function is called successfully, the 'SlamState' will change to 'Calibrations'. + * When the 'Calibrations' process ends, the 'SlamState' will change back to 'Localization'. + * @return True/False Call fucntion is successed/failed + */ + virtual bool startLocalization() = 0; + + /** + * @brief Turn off the Localization mode. + * At this point, the robot's coordinate information will not be updated. + * If called successfully, the 'SlamState' variable will change to 'Ready'. + * @return True/False Call fucntion is successed/failed + */ + virtual bool stopLocalization() = 0; + + /** + * @brief Get State of Loc Base + */ + virtual loc_core::slam_state_en getState() = 0; + + std::string activated_map_filename_; + std::string working_dir_; + std::string map_topic_; + + protected: + /** + * @brief Constructor for the interface + */ + BaseLocalization() {}; + }; // class BaseLocalization + +} // namespace loc_core + +#endif // _LOC_CORE_LOCALIZATION_H_INCLUDED_ \ No newline at end of file diff --git a/Localizations/Cores/loc_core/package.xml b/Localizations/Cores/loc_core/package.xml new file mode 100644 index 0000000..eeba506 --- /dev/null +++ b/Localizations/Cores/loc_core/package.xml @@ -0,0 +1,71 @@ + + + loc_core + 0.0.0 + The loc_core package + + + + + robotics + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + geometry_msgs + std_msgs + tf + tf2_ros + geometry_msgs + std_msgs + tf + tf2_ros + geometry_msgs + std_msgs + tf + tf2_ros + + + + + + + + diff --git a/Localizations/Libraries/Ros/amcl/CHANGELOG.rst b/Localizations/Libraries/Ros/amcl/CHANGELOG.rst new file mode 100644 index 0000000..2faef83 --- /dev/null +++ b/Localizations/Libraries/Ros/amcl/CHANGELOG.rst @@ -0,0 +1,258 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package amcl +^^^^^^^^^^^^^^^^^^^^^^^^^^ + +1.17.3 (2023-01-10) +------------------- +* [AMCL] Add option to force nomotion update after initialpose (`#1226 `_) + * Adds a new boolean parameter force_update_after_initialpose. When set to true, an update is forced on the next laser scan callback, such as when the /request_nomotion_update service is called. This often results in an improved robot pose after a manual (not very precise) re-localization - without a need for the robot to move. + * Fixes a bunch of compiler warnings (unused variable now, catching exceptions by value), normalizes how tf exceptions are caught +* [ROS-O] various patches (`#1225 `_) + * do not specify obsolete c++11 standard + this breaks with current versions of log4cxx. + * update pluginlib include paths + the non-hpp headers have been deprecated since kinetic + * use lambdas in favor of boost::bind + Using boost's _1 as a global system is deprecated since C++11. + The ROS packages in Debian removed the implicit support for the global symbols, + so this code fails to compile there without the patch. +* Contributors: Michael Görner, Stephan + +1.17.2 (2022-06-20) +------------------- +* Update pf.c (`#1161 `_) + `#1160 `_ AMCL miscalculates orientation covariance for clusters +* Improved Overall readablity (`#1177 `_) +* fix crashes in AMCL (`#1152 `_) + * fix: catch runtime_error from roscore + * ignore malformed message from laser, otherwise it will crash +* Fixes `#1117 `_ (`#1118 `_) +* Fixed the risk of divide by zero. (`#1099 `_) +* (AMCL) add missing test dep on tf2_py (`#1091 `_) +* (AMCL)(Noetic) use robot pose in tests (`#1087 `_) +* (amcl) fix missing '#if NEW_UNIFORM_SAMPLING' (`#1079 `_) +* Contributors: David V. Lu!!, Matthijs van der Burgh, Noriaki Ando, Supernovae, christofschroeter, easylyou + +1.17.1 (2020-08-27) +------------------- +* (AMCL) add resample limit cache [Noetic] (`#1014 `_) +* Contributors: Matthijs van der Burgh + +1.17.0 (2020-04-02) +------------------- +* Merge pull request `#982 `_ from ros-planning/noetic_prep + Noetic Migration +* map is not subscriptable in python3 +* fix python3 errors in basic_localization.py +* use upstream pykdl +* Contributors: Michael Ferguson + +1.16.6 (2020-03-18) +------------------- + +1.16.5 (2020-03-15) +------------------- +* [melodic] updated install for better portability. (`#973 `_) +* Contributors: Sean Yen + +1.16.4 (2020-03-04) +------------------- +* Implement selective resampling (`#921 `_) (`#971 `_) + Co-authored-by: Adi Vardi +* Add CLI option to trigger global localization before processing a bagfile (`#816 `_) (`#970 `_) + Co-authored-by: alain-m +* Fix some reconfigure parameters not being applied [amcl]. (`#952 `_) +* amcl: include missing CMake functions to fix build (`#946 `_) +* Set proper limits for the z-weights [amcl]. (`#953 `_) +* Merge pull request `#965 `_ from nlimpert/nlimpert/fix_missing_cmake_include + Add missing CMake include(CheckSymbolExists) for CMake >= 3.15 +* amcl: add missing CMake include(CheckSymbolExists) + Starting with CMake 3.15 an explicit include(CheckSymbolExists) + is required to use the check_symbol_exists macro. +* Contributors: Ben Wolsieffer, Michael Ferguson, Nicolas Limpert, Patrick Chin + +1.16.3 (2019-11-15) +------------------- +* Merge branch 'melodic-devel' into layer_clear_area-melodic +* Fix typo in amcl_laser model header (`#918 `_) +* Merge pull request `#849 `_ from seanyen/amcl_windows_fix + [Windows][melodic] AMCL Windows build bring up. +* revert unrelated changes. +* AMCL windows build bring up. + * Add HAVE_UNISTD and HAVE_DRAND48 and portable_utils.hpp for better cross compiling. + * Variable length array is not supported in MSVC, conditionally disable it. + * Fix install location for shared lib and executables on Windows. + * Use isfinite for better cross compiling. +* feat: AMCL Diagnostics (`#807 `_) + Diagnostic task that monitors the estimated standard deviation of the filter. + By: reinzor +* fix typo for parameter beam_skip_error_threshold but bandaged for other users in AMCL (`#790 `_) + * fix typo but bandage for other users +* Merge pull request `#785 `_ from mintar/amcl_c++11 + amcl: Add compile option C++11 +* amcl: Set C++ standard 11 if not set + This is required to build the melodic-devel branch of the navigation + stack on kinetic. Melodic sets CMAKE_CXX_STANDARD=14, but kinetic + doesn't set that variable at all. +* Contributors: Hadi Tabatabaee, Martin Günther, Michael Ferguson, Rein Appeldoorn, Sean Yen, Steven Macenski + +1.16.2 (2018-07-31) +------------------- +* Merge pull request `#773 `_ from ros-planning/packaging_fixes + packaging fixes +* update amcl to have proper depends + * add geometry_msgs + * add tf2_msgs + * fix alphabetical order +* Contributors: Michael Ferguson + +1.16.1 (2018-07-28) +------------------- +* Merge pull request `#770 `_ from ros-planning/fix_debians + Fix debian builds (closes `#769 `_) +* make AMCL depend on sensor_msgs + previously, amcl depended on TF, which depended on + sensor_msgs. +* Contributors: Michael Ferguson + +1.16.0 (2018-07-25) +------------------- +* Switch to TF2 `#755 `_ +* Merge pull request `#734 `_ from ros-planning/melodic_731 + AMCL dynamic reconfigure: Extend parameter range (Forward port `#731 `_) +* Merge pull request `#728 `_ from ros-planning/melodic_tf2_conversion + switch AMCL to use TF2 +* fix swapped odom1/4 in omni model, fixes `#499 `_ +* Merge pull request `#730 `_ from Glowcloud/melodic-devel + Fix for Potential Memory Leak in AmclNode::reconfigureCB `#729 `_ +* Fix for Potential Memory Leak in AmclNode::reconfigureCB +* switch AMCL to use TF2 +* Merge pull request `#727 `_ from ros-planning/melodic_668 + Update laser_model_type enum on AMCL.cfg (Melodic port of `#668 `_) +* Update laser_model_type enum on AMCL.cfg + Adding likelihood_field_prob laser model option on AMCL.cfg to be able to control dynamic parameters with this laser sensor model. +* Merge pull request `#723 `_ from moriarty/melodic-buildfarm-errors + Melodic buildfarm errors +* include for std::shared_ptr +* Merge pull request `#718 `_ from moriarty/tf2-buffer-ptr + [melodic] tf2_buffer\_ -> tf2_buffer_ptr\_ +* [melodic] tf2_buffer\_ -> tf2_buffer_ptr\_ + Change required due to changes in upstream dependencies: + `ros/geometry#163 `_: "Maintain & expose tf2 Buffer in shared_ptr for tf" + fixes `ros-planning/navigation#717 `_ (for compile errors at least.) +* Contributors: Alexander Moriarty, Glowcloud, Martin Ganeff, Michael Ferguson, Miguel Cordero, Vincent Rabaud, maracuya-robotics + +1.15.2 (2018-03-22) +------------------- +* Fix minor typo (`#682 `_) + This typo caused some confusion because we were searching for a semicolon in our configuration. +* Merge pull request `#677 `_ from ros-planning/lunar_634 + removing recomputation of cluster stats causing assertion error (`#634 `_) +* Merge pull request `#673 `_ from ros-planning/email_update_lunar + update maintainer email (lunar) +* Remove Dead Code [Lunar] (`#646 `_) + * Clean up navfn + * Cleanup amcl +* Merge pull request `#649 `_ from aaronhoy/lunar_add_ahoy + Add myself as a maintainer. +* Contributors: Aaron Hoy, David V. Lu!!, Michael Ferguson, stevemacenski + +1.15.1 (2017-08-14) +------------------- + +1.15.0 (2017-08-07) +------------------- +* Reference Issue `#592 `_ Added warning to AMCL when map is published on ... (`#604 `_) +* rebase fixups +* convert packages to format2 +* recompute cluster stat when force_publication +* Fix CMakeLists + package.xmls (`#548 `_) +* amcl: fix compilation with gcc v7 +* Added deps to amcl costmap_2d move_base (`#512 `_) +* fix order of parameters (closes `#553 `_) +* Fix potential string overflow and resource leak +* Contributors: Dmitry Rozhkov, Laurent GEORGE, Martin Günther, Michael Ferguson, Mikael Arguedas, Peter Harliman Liem, mryellow, vik748 + +1.14.0 (2016-05-20) +------------------- +* Allow AMCL to run from bag file to allow very fast testing. +* Fixes interpretation of a delayed initialpose message (see `#424 `_). + The tf lookup as it was before this change was very likely to fail as + ros::Time::now() was used to look up a tf without waiting on the tf's + availability. Additionally, the computation of the "new pose" by + multiplying the delta that the robot moved from the initialpose's + timestamp to ros::Time::now() was wrong. That delta has to by multiplied + from the right to the "old pose". + This commit also changes the reference frame to look up this delta to be + the odom frame as this one is supposed to be smooth and therefore the + best reference to get relative robot motion in the robot (base link) frame. +* New unit test for proper interpretation of a delayed initialpose message. + Modifies the set_pose.py script to be able to send an initial pose with + a user defined time stamp at a user defined time. Adds a rostest to + exercise this new option. + This reveals the issues mentioned in `#424 `_ (the new test fails). +* Contributors: Derek King, Stephan Wirth + +1.13.1 (2015-10-29) +------------------- +* adds the set_map service to amcl +* fix pthread_mutex_lock on shutdown +* Contributors: Michael Ferguson, Stephan Wirth + +1.13.0 (2015-03-17) +------------------- +* amcl_node will now save latest pose on shutdown +* Contributors: Ian Danforth + +1.12.0 (2015-02-04) +------------------- +* update maintainer email +* Contributors: Michael Ferguson + +1.11.15 (2015-02-03) +-------------------- + +1.11.14 (2014-12-05) +-------------------- + +1.11.13 (2014-10-02) +-------------------- + +1.11.12 (2014-10-01) +-------------------- +* Bug fix to remove particle weights being reset when motion model is updated +* Integrated new sensor model which calculates the observation likelihood in a probabilistic manner + Also includes the option to do beam-skipping (to better handle observations from dynamic obstacles) +* Pose pulled from parameter server when new map received +* Contributors: Steven Kordell, hes3pal + +1.11.11 (2014-07-23) +-------------------- + +1.11.10 (2014-06-25) +-------------------- + +1.11.9 (2014-06-10) +------------------- + +1.11.8 (2014-05-21) +------------------- + +1.11.7 (2014-05-21) +------------------- +* removes useless this->z_max = z_max assignment +* Fix warning string. +* Contributors: Jeremiah Via, enriquefernandez + +1.11.5 (2014-01-30) +------------------- +* Fix for `#160 `_ +* Download test data from download.ros.org instead of willow +* Change maintainer from Hersh to Lu + +1.11.4 (2013-09-27) +------------------- +* Package URL Updates +* amcl_pose and particle cloud are now published latched +* Fixed or commented out failing amcl tests. + diff --git a/Localizations/Libraries/Ros/amcl/CMakeLists.txt b/Localizations/Libraries/Ros/amcl/CMakeLists.txt new file mode 100644 index 0000000..1c77291 --- /dev/null +++ b/Localizations/Libraries/Ros/amcl/CMakeLists.txt @@ -0,0 +1,184 @@ +cmake_minimum_required(VERSION 3.0.2) +project(amcl) + +include(CheckIncludeFile) +include(CheckSymbolExists) + +find_package(catkin REQUIRED + COMPONENTS + diagnostic_updater + dynamic_reconfigure + geometry_msgs + message_filters + nav_msgs + rosbag + roscpp + sensor_msgs + std_srvs + tf2 + tf2_geometry_msgs + tf2_msgs + tf2_ros +) + +find_package(Boost REQUIRED) + +# dynamic reconfigure +generate_dynamic_reconfigure_options( + cfg/AMCL.cfg +) + +catkin_package( + CATKIN_DEPENDS + diagnostic_updater + dynamic_reconfigure + geometry_msgs + nav_msgs + rosbag + roscpp + sensor_msgs + std_srvs + tf2 + tf2_msgs + tf2_ros + INCLUDE_DIRS include + LIBRARIES amcl_sensors amcl_map amcl_pf amcl_lib +) + +include_directories(include) +include_directories(${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}) +include_directories(src/include) + +check_include_file(unistd.h HAVE_UNISTD_H) +if (HAVE_UNISTD_H) + add_definitions(-DHAVE_UNISTD_H) +endif (HAVE_UNISTD_H) + +check_symbol_exists(drand48 stdlib.h HAVE_DRAND48) +if (HAVE_DRAND48) + add_definitions(-DHAVE_DRAND48) +endif (HAVE_DRAND48) + +add_library(amcl_pf + src/amcl/pf/pf.c + src/amcl/pf/pf_kdtree.c + src/amcl/pf/pf_pdf.c + src/amcl/pf/pf_vector.c + src/amcl/pf/eig3.c + src/amcl/pf/pf_draw.c) + +add_library(amcl_map + src/amcl/map/map.c + src/amcl/map/map_cspace.cpp + src/amcl/map/map_range.c + src/amcl/map/map_store.c + src/amcl/map/map_draw.c) + +add_library(amcl_sensors + src/amcl/sensors/amcl_sensor.cpp + src/amcl/sensors/amcl_odom.cpp + src/amcl/sensors/amcl_laser.cpp) +target_link_libraries(amcl_sensors amcl_map amcl_pf) + + +add_library(amcl_lib + src/amcl.cpp +) +add_dependencies(amcl_lib + amcl_sensors + amcl_map + amcl_pf + ${${PROJECT_NAME}_EXPORTED_TARGETS} + ${catkin_EXPORTED_TARGETS} +) +target_link_libraries(amcl_lib + amcl_sensors + amcl_map + amcl_pf + ${Boost_LIBRARIES} + ${catkin_LIBRARIES} +) + +add_executable(amcl src/amcl_node.cpp) +add_dependencies(amcl ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(amcl + amcl_lib + ${Boost_LIBRARIES} + ${catkin_LIBRARIES} +) + +install(TARGETS + amcl + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(TARGETS + amcl_sensors amcl_map amcl_pf amcl_lib + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) + +install(DIRECTORY include/amcl/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +) + +install(DIRECTORY examples/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/examples +) + +## Configure Tests +if(CATKIN_ENABLE_TESTING) + find_package(rostest REQUIRED) + + # Bags + catkin_download_test_data(${PROJECT_NAME}_basic_localization_stage_indexed.bag + http://download.ros.org/data/amcl/basic_localization_stage_indexed.bag + DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/test + MD5 41fe43af189ec71e5e48eb9ed661a655) + catkin_download_test_data(${PROJECT_NAME}_global_localization_stage_indexed.bag + http://download.ros.org/data/amcl/global_localization_stage_indexed.bag + DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/test + MD5 752f711cf4f6e8d1d660675e2da096b0) + catkin_download_test_data(${PROJECT_NAME}_small_loop_prf_indexed.bag + http://download.ros.org/data/amcl/small_loop_prf_indexed.bag + DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/test + MD5 e4ef0fc006872b43f12ed8a7ce7dcd81) + catkin_download_test_data(${PROJECT_NAME}_small_loop_crazy_driving_prg_indexed.bag + http://download.ros.org/data/amcl/small_loop_crazy_driving_prg_indexed.bag + DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/test + MD5 4a58d1a7962914009d99000d06e5939c) + catkin_download_test_data(${PROJECT_NAME}_texas_greenroom_loop_indexed.bag + http://download.ros.org/data/amcl/texas_greenroom_loop_indexed.bag + DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/test + MD5 6e3432115cccdca1247f6c807038e13d) + catkin_download_test_data(${PROJECT_NAME}_texas_willow_hallway_loop_indexed.bag + http://download.ros.org/data/amcl/texas_willow_hallway_loop_indexed.bag + DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/test + MD5 27deb742fdcd3af44cf446f39f2688a8) + catkin_download_test_data(${PROJECT_NAME}_rosie_localization_stage.bag + http://download.ros.org/data/amcl/rosie_localization_stage.bag + DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/test + MD5 3347bf3835724cfa45e958c5c1846066) + + # Maps + catkin_download_test_data(${PROJECT_NAME}_willow-full.pgm + http://download.ros.org/data/amcl/willow-full.pgm + DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/test + MD5 b84465cdbbfe3e2fb9eb4579e0bcaf0e) + catkin_download_test_data(${PROJECT_NAME}_willow-full-0.05.pgm + http://download.ros.org/data/amcl/willow-full-0.05.pgm + DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/test + MD5 b61694296e08965096c5e78611fd9765) + + # Tests + add_rostest(test/set_initial_pose.xml) + add_rostest(test/set_initial_pose_delayed.xml) + add_rostest(test/basic_localization_stage.xml) + add_rostest(test/global_localization_stage.xml) + add_rostest(test/small_loop_prf.xml) + add_rostest(test/small_loop_crazy_driving_prg.xml) + add_rostest(test/texas_greenroom_loop.xml) + add_rostest(test/rosie_multilaser.xml) + add_rostest(test/texas_willow_hallway_loop.xml) +endif() diff --git a/Localizations/Libraries/Ros/amcl/cfg/AMCL.cfg b/Localizations/Libraries/Ros/amcl/cfg/AMCL.cfg new file mode 100644 index 0000000..4695df7 --- /dev/null +++ b/Localizations/Libraries/Ros/amcl/cfg/AMCL.cfg @@ -0,0 +1,77 @@ +#!/usr/bin/env python + +PACKAGE = 'amcl' + +from math import pi +from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, int_t, double_t, str_t, bool_t + +gen = ParameterGenerator() + +gen.add("min_particles", int_t, 0, "Minimum allowed number of particles.", 100, 0, 1000) +gen.add("max_particles", int_t, 0, "Mamimum allowed number of particles.", 5000, 0, 10000) + +gen.add("kld_err", double_t, 0, "Maximum error between the true distribution and the estimated distribution.", .01, 0, 1) +gen.add("kld_z", double_t, 0, "Upper standard normal quantile for (1 - p), where p is the probability that the error on the estimated distribution will be less than kld_err.", .99, 0, 1) + +gen.add("update_min_d", double_t, 0, "Translational movement required before performing a filter update.", .2, 0, 5) +gen.add("update_min_a", double_t, 0, "Rotational movement required before performing a filter update.", pi/6, 0, 2*pi) + +gen.add("resample_interval", int_t, 0, "Number of filter updates required before resampling.", 2, 0, 20) + +gen.add("transform_tolerance", double_t, 0, "Time with which to post-date the transform that is published, to indicate that this transform is valid into the future.", .1, 0, 2) + +gen.add("recovery_alpha_slow", double_t, 0, "Exponential decay rate for the slow average weight filter, used in deciding when to recover by adding random poses. A good value might be 0.001.", 0, 0, .5) +gen.add("recovery_alpha_fast", double_t, 0, "Exponential decay rate for the fast average weight filter, used in deciding when to recover by adding random poses. A good value might be 0.1.", 0, 0, 1) + +gen.add("do_beamskip", bool_t, 0, "When true skips laser scans when a scan doesnt work for a majority of particles", False) +gen.add("beam_skip_distance", double_t, 0, "Distance from a valid map point before scan is considered invalid", 0.5, 0, 2) +gen.add("beam_skip_threshold", double_t, 0, "Ratio of samples for which the scans are valid to consider as valid scan", 0.3, 0, 1) + +gen.add("tf_broadcast", bool_t, 0, "When true (the default), publish results via TF. When false, do not.", True) +gen.add("force_update_after_initialpose", bool_t, 0, "When true, force a pose update after a new /initialpose is received.", False) +gen.add("force_update_after_set_map", bool_t, 0, "When true, force a pose update after a new map (and pose) has been set via the /set_map service.", False) +gen.add("gui_publish_rate", double_t, 0, "Maximum rate (Hz) at which scans and paths are published for visualization, -1.0 to disable.", -1, -1, 100) +gen.add("save_pose_rate", double_t, 0, "Maximum rate (Hz) at which to store the last estimated pose and covariance to the parameter server, in the variables ~initial_pose_* and ~initial_cov_*. This saved pose will be used on subsequent runs to initialize the filter. -1.0 to disable.", .5, -1, 10) + +gen.add("use_map_topic", bool_t, 0, "When set to true, AMCL will subscribe to the map topic rather than making a service call to receive its map.", False) +gen.add("first_map_only", bool_t, 0, "When set to true, AMCL will only use the first map it subscribes to, rather than updating each time a new one is received.", False) + +# Laser Model Parameters +gen.add("laser_min_range", double_t, 0, "Minimum scan range to be considered; -1.0 will cause the laser's reported minimum range to be used.", -1, -1, 1000) +gen.add("laser_max_range", double_t, 0, "Maximum scan range to be considered; -1.0 will cause the laser's reported maximum range to be used.", -1, -1, 1000) + +gen.add("laser_max_beams", int_t, 0, "How many evenly-spaced beams in each scan to be used when updating the filter.", 30, 0, 250) + +gen.add("laser_z_hit", double_t, 0, "Mixture weight for the z_hit part of the model.", .95, 0, 1) +gen.add("laser_z_short", double_t, 0, "Mixture weight for the z_short part of the model.", .1, 0, 1) +gen.add("laser_z_max", double_t, 0, "Mixture weight for the z_max part of the model.", .05, 0, 1) +gen.add("laser_z_rand", double_t, 0, "Mixture weight for the z_rand part of the model.", .05, 0, 1) + +gen.add("laser_sigma_hit", double_t, 0, "Standard deviation for Gaussian model used in z_hit part of the model.", .2, 0, 10) +gen.add("laser_lambda_short", double_t, 0, "Exponential decay parameter for z_short part of model.", .1, 0, 10) +gen.add("laser_likelihood_max_dist", double_t, 0, "Maximum distance to do obstacle inflation on map, for use in likelihood_field model.", 2, 0, 20) + +lmt = gen.enum([gen.const("beam_const", str_t, "beam", "Use beam laser model"), gen.const("likelihood_field_const", str_t, "likelihood_field", "Use likelihood_field laser model"), gen.const("likelihood_field_prob", str_t, "likelihood_field_prob", "Use likelihood_field_prob laser model")], "Laser Models") +gen.add("laser_model_type", str_t, 0, "Which model to use, either beam, likelihood_field or likelihood_field_prob.", "likelihood_field", edit_method=lmt) + +# Odometry Model Parameters +odt = gen.enum([gen.const("diff_const", str_t, "diff", "Use diff odom model"), + gen.const("omni_const", str_t, "omni", "Use omni odom model"), + gen.const("diff_corrected_const", str_t, "diff-corrected", "Use corrected diff odom model"), + gen.const("omni_corrected_const", str_t, "omni-corrected", "Use corrected omni odom model")], + "Odom Models") +gen.add("odom_model_type", str_t, 0, "Which model to use, diff, omni, diff-corrected, or omni-corrected", "diff", edit_method=odt) + +gen.add("odom_alpha1", double_t, 0, "Specifies the expected noise in odometry's rotation estimate from the rotational component of the robot's motion.", .2, 0, 10) +gen.add("odom_alpha2", double_t, 0, "Specifies the expected noise in odometry's rotation estimate from the translational component of the robot's motion.", .2, 0, 10) +gen.add("odom_alpha3", double_t, 0, "Specifies the expected noise in odometry's translation estimate from the translational component of the robot's motion.", .2, 0, 10) +gen.add("odom_alpha4", double_t, 0, "Specifies the expected noise in odometry's translation estimate from the rotational component of the robot's motion.", .2, 0, 10) +gen.add("odom_alpha5", double_t, 0, "Translation-related noise parameter (only used if model is omni).", .2, 0, 10) + +gen.add("odom_frame_id", str_t, 0, "Which frame to use for odometry.", "odom") +gen.add("base_frame_id", str_t, 0, "Which frame to use for the robot base.", "base_link") +gen.add("global_frame_id", str_t, 0, "The name of the coordinate frame published by the localization system.", "map") + +gen.add("restore_defaults", bool_t, 0, "Retsore the default configuration", False) + +exit(gen.generate(PACKAGE, "amcl_node", "AMCL")) diff --git a/Localizations/Libraries/Ros/amcl/examples/amcl_diff.launch b/Localizations/Libraries/Ros/amcl/examples/amcl_diff.launch new file mode 100644 index 0000000..6e23ebd --- /dev/null +++ b/Localizations/Libraries/Ros/amcl/examples/amcl_diff.launch @@ -0,0 +1,34 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Localizations/Libraries/Ros/amcl/examples/amcl_omni.launch b/Localizations/Libraries/Ros/amcl/examples/amcl_omni.launch new file mode 100644 index 0000000..a9137b2 --- /dev/null +++ b/Localizations/Libraries/Ros/amcl/examples/amcl_omni.launch @@ -0,0 +1,34 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Localizations/Libraries/Ros/amcl/include/amcl/amcl.h b/Localizations/Libraries/Ros/amcl/include/amcl/amcl.h new file mode 100644 index 0000000..2d630fb --- /dev/null +++ b/Localizations/Libraries/Ros/amcl/include/amcl/amcl.h @@ -0,0 +1,293 @@ +#ifndef _AMCL_LIBRARY_H_INCLUDED_ +#define _AMCL_LIBRARY_H_INCLUDED_ + +#include +#include +#include +#include +#include + +#include +#include + + + +#include "amcl/map/map.h" +#include "amcl/pf/pf.h" +#include "amcl/sensors/amcl_odom.h" +#include "amcl/sensors/amcl_laser.h" +#include "amcl/portable_utils.hpp" + +#include "ros/assert.h" + +// roscpp +#include "ros/ros.h" + +// Messages that I need +#include "sensor_msgs/LaserScan.h" +#include "geometry_msgs/PoseWithCovarianceStamped.h" +#include "geometry_msgs/PoseArray.h" +#include "geometry_msgs/Pose.h" +#include "geometry_msgs/PoseStamped.h" +#include "geometry_msgs/PoseWithCovarianceStamped.h" +#include "nav_msgs/GetMap.h" +#include "nav_msgs/SetMap.h" +#include "std_srvs/Empty.h" + +// For transform support +#include "tf2/LinearMath/Transform.h" +#include "tf2/convert.h" +#include "tf2/utils.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.h" +#include "tf2_ros/buffer.h" +#include "tf2_ros/message_filter.h" +#include "tf2_ros/transform_broadcaster.h" +#include "tf2_ros/transform_listener.h" +#include "message_filters/subscriber.h" + +// Dynamic_reconfigure +#include "dynamic_reconfigure/server.h" +#include "amcl/AMCLConfig.h" + +// Allows AMCL to run from bag file +#include +#include +#include + +// For monitoring the estimator +#include + +// using namespace amcl; + +#define NEW_UNIFORM_SAMPLING 1 + +// Pose hypothesis +typedef struct +{ + // Total weight (weights sum to 1) + double weight; + + // Mean of pose esimate + pf_vector_t pf_pose_mean; + + // Covariance of pose estimate + pf_matrix_t pf_pose_cov; + +} amcl_hyp_t; + +static double +normalize(double z) +{ + return atan2(sin(z), cos(z)); +} +static double +angle_diff(double a, double b) +{ + double d1, d2; + a = normalize(a); + b = normalize(b); + d1 = a - b; + d2 = 2 * M_PI - fabs(d1); + if (d1 > 0) + d2 *= -1.0; + if (fabs(d1) < fabs(d2)) + return(d1); + else + return(d2); +} + +/* This function is only useful to have the whole code work + * with old rosbags that have trailing slashes for their frames + */ +inline +std::string stripSlash(const std::string& in) +{ + std::string out = in; + if ((!in.empty()) && (in[0] == '/')) + out.erase(0, 1); + return out; +} +namespace amcl +{ + class Amcl + { + public: + Amcl(); + Amcl(ros::NodeHandle nh); + virtual ~Amcl(); + + /** + * @brief Uses TF and LaserScan messages from bag file to drive AMCL instead + * @param in_bag_fn input bagfile + * @param trigger_global_localization whether to trigger global localization + * before starting to process the bagfile + */ + void runFromBag(const std::string& in_bag_fn, bool trigger_global_localization = false); + + int process(); + void savePoseToServer(geometry_msgs::PoseWithCovarianceStamped &amcl_pose); + bool getInitialized() {return initialized_;} + + private: + bool initialized_; + std::shared_ptr tfb_; + std::shared_ptr tfl_; + std::shared_ptr tf_; + + bool sent_first_transform_; + + tf2::Transform latest_tf_; + bool latest_tf_valid_; + + // Pose-generating function used to uniformly distribute particles over + // the map + static pf_vector_t uniformPoseGenerator(void* arg); +#if NEW_UNIFORM_SAMPLING + static std::vector > free_space_indices; +#endif + // Callbacks + bool globalLocalizationCallback(std_srvs::Empty::Request& req, + std_srvs::Empty::Response& res); + bool nomotionUpdateCallback(std_srvs::Empty::Request& req, + std_srvs::Empty::Response& res); + bool setMapCallback(nav_msgs::SetMap::Request& req, + nav_msgs::SetMap::Response& res); + + void initialize(ros::NodeHandle nh); + void laserReceived(const sensor_msgs::LaserScanConstPtr& laser_scan); + void initialPoseReceived(const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg); + void handleInitialPoseMessage(const geometry_msgs::PoseWithCovarianceStamped& msg); + void mapReceived(const nav_msgs::OccupancyGridConstPtr& msg); + + void handleMapMessage(const nav_msgs::OccupancyGrid& msg); + void freeMapDependentMemory(); + map_t* convertMap(const nav_msgs::OccupancyGrid& map_msg); + void updatePoseFromServer(); + void applyInitialPose(); + + //parameter for which odom to use + std::string odom_frame_id_; + + //paramater to store latest odom pose + geometry_msgs::PoseStamped latest_odom_pose_; + + //parameter for which base to use + std::string base_frame_id_; + std::string global_frame_id_; + + bool use_map_topic_; + bool first_map_only_; + + ros::Duration gui_publish_period; + ros::Time save_pose_last_time; + ros::Duration save_pose_period; + + geometry_msgs::PoseWithCovarianceStamped last_published_pose; + + map_t* map_; + char* mapdata; + int sx, sy; + double resolution; + + message_filters::Subscriber* laser_scan_sub_; + tf2_ros::MessageFilter* laser_scan_filter_; + ros::Subscriber initial_pose_sub_; + std::vector< amcl::AMCLLaser* > lasers_; + std::vector< bool > lasers_update_; + std::map< std::string, int > frame_to_laser_; + + // Particle filter + pf_t* pf_; + double pf_err_, pf_z_; + bool pf_init_; + pf_vector_t pf_odom_pose_; + double d_thresh_, a_thresh_; + int resample_interval_; + int resample_count_; + double laser_min_range_; + double laser_max_range_; + + //Nomotion update control + bool m_force_update; // used to temporarily let amcl update samples even when no motion occurs... + + amcl::AMCLOdom* odom_; + amcl::AMCLLaser* laser_; + + ros::Duration cloud_pub_interval; + ros::Time last_cloud_pub_time; + + // For slowing play-back when reading directly from a bag file + ros::WallDuration bag_scan_period_; + + void requestMap(); + + // Helper to get odometric pose from transform system + bool getOdomPose(geometry_msgs::PoseStamped& pose, + double& x, double& y, double& yaw, + const ros::Time& t, const std::string& f); + + //time for tolerance on the published transform, + //basically defines how long a map->odom transform is good for + ros::Duration transform_tolerance_; + + ros::NodeHandle nh_; + ros::NodeHandle private_nh_; + ros::Publisher pose_pub_; + ros::Publisher particlecloud_pub_; + ros::ServiceServer global_loc_srv_; + ros::ServiceServer nomotion_update_srv_; //to let amcl update samples without requiring motion + ros::ServiceServer set_map_srv_; + ros::Subscriber initial_pose_sub_old_; + ros::Subscriber map_sub_; + + diagnostic_updater::Updater diagnosic_updater_; + void standardDeviationDiagnostics(diagnostic_updater::DiagnosticStatusWrapper& diagnostic_status); + double std_warn_level_x_; + double std_warn_level_y_; + double std_warn_level_yaw_; + + std::string scan_topic_; + std::string map_topic_; + std::string map_service_; + + amcl_hyp_t* initial_pose_hyp_; + bool first_map_received_; + bool first_reconfigure_call_; + + boost::recursive_mutex configuration_mutex_; + dynamic_reconfigure::Server* dsrv_; + amcl::AMCLConfig default_config_; + ros::Timer check_laser_timer_; + + int max_beams_, min_particles_, max_particles_; + double alpha1_, alpha2_, alpha3_, alpha4_, alpha5_; + double alpha_slow_, alpha_fast_; + double z_hit_, z_short_, z_max_, z_rand_, sigma_hit_, lambda_short_; + //beam skip related params + bool do_beamskip_; + double beam_skip_distance_, beam_skip_threshold_, beam_skip_error_threshold_; + double laser_likelihood_max_dist_; + amcl::odom_model_t odom_model_type_; + double init_pose_[3]; + double init_cov_[3]; + amcl::laser_model_t laser_model_type_; + bool tf_broadcast_; + bool force_update_after_initialpose_; + bool force_update_after_set_map_; + bool selective_resampling_; + ros::Time last_tfb_time_; + + void reconfigureCB(amcl::AMCLConfig& config, uint32_t level); + + ros::Time last_laser_received_ts_; + ros::Duration laser_check_interval_; + void checkLaserReceived(const ros::TimerEvent& event); + }; +} // namespace amcl +#define USAGE "USAGE: amcl" + +#if NEW_UNIFORM_SAMPLING +std::vector > amcl::Amcl::free_space_indices; +#endif + +#endif // _AMCL_LIBRARY_H_INCLUDED_ \ No newline at end of file diff --git a/Localizations/Libraries/Ros/amcl/include/amcl/map/map.h b/Localizations/Libraries/Ros/amcl/include/amcl/map/map.h new file mode 100644 index 0000000..17aabf9 --- /dev/null +++ b/Localizations/Libraries/Ros/amcl/include/amcl/map/map.h @@ -0,0 +1,150 @@ +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2000 Brian Gerkey & Kasper Stoy + * gerkey@usc.edu kaspers@robotics.usc.edu + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ +/************************************************************************** + * Desc: Global map (grid-based) + * Author: Andrew Howard + * Date: 6 Feb 2003 + * CVS: $Id: map.h 1713 2003-08-23 04:03:43Z inspectorg $ + **************************************************************************/ + +#ifndef MAP_H +#define MAP_H + +#include + +#ifdef __cplusplus +extern "C" { +#endif + +// Forward declarations +struct _rtk_fig_t; + + +// Limits +#define MAP_WIFI_MAX_LEVELS 8 + + +// Description for a single map cell. +typedef struct +{ + // Occupancy state (-1 = free, 0 = unknown, +1 = occ) + int occ_state; + + // Distance to the nearest occupied cell + double occ_dist; + + // Wifi levels + //int wifi_levels[MAP_WIFI_MAX_LEVELS]; + +} map_cell_t; + + +// Description for a map +typedef struct +{ + // Map origin; the map is a viewport onto a conceptual larger map. + double origin_x, origin_y; + + // Map scale (m/cell) + double scale; + + // Map dimensions (number of cells) + int size_x, size_y; + + // The map data, stored as a grid + map_cell_t *cells; + + // Max distance at which we care about obstacles, for constructing + // likelihood field + double max_occ_dist; + +} map_t; + + + +/************************************************************************** + * Basic map functions + **************************************************************************/ + +// Create a new (empty) map +map_t *map_alloc(void); + +// Destroy a map +void map_free(map_t *map); + +// Get the cell at the given point +map_cell_t *map_get_cell(map_t *map, double ox, double oy, double oa); + +// Load an occupancy map +int map_load_occ(map_t *map, const char *filename, double scale, int negate); + +// Load a wifi signal strength map +//int map_load_wifi(map_t *map, const char *filename, int index); + +// Update the cspace distances +void map_update_cspace(map_t *map, double max_occ_dist); + + +/************************************************************************** + * Range functions + **************************************************************************/ + +// Extract a single range reading from the map +double map_calc_range(map_t *map, double ox, double oy, double oa, double max_range); + + +/************************************************************************** + * GUI/diagnostic functions + **************************************************************************/ + +// Draw the occupancy grid +void map_draw_occ(map_t *map, struct _rtk_fig_t *fig); + +// Draw the cspace map +void map_draw_cspace(map_t *map, struct _rtk_fig_t *fig); + +// Draw a wifi map +void map_draw_wifi(map_t *map, struct _rtk_fig_t *fig, int index); + + +/************************************************************************** + * Map manipulation macros + **************************************************************************/ + +// Convert from map index to world coords +#define MAP_WXGX(map, i) (map->origin_x + ((i) - map->size_x / 2) * map->scale) +#define MAP_WYGY(map, j) (map->origin_y + ((j) - map->size_y / 2) * map->scale) + +// Convert from world coords to map coords +#define MAP_GXWX(map, x) (floor((x - map->origin_x) / map->scale + 0.5) + map->size_x / 2) +#define MAP_GYWY(map, y) (floor((y - map->origin_y) / map->scale + 0.5) + map->size_y / 2) + +// Test to see if the given map coords lie within the absolute map bounds. +#define MAP_VALID(map, i, j) ((i >= 0) && (i < map->size_x) && (j >= 0) && (j < map->size_y)) + +// Compute the cell index for the given map coords. +#define MAP_INDEX(map, i, j) ((i) + (j) * map->size_x) + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/Localizations/Libraries/Ros/amcl/include/amcl/pf/eig3.h b/Localizations/Libraries/Ros/amcl/include/amcl/pf/eig3.h new file mode 100644 index 0000000..b708cb1 --- /dev/null +++ b/Localizations/Libraries/Ros/amcl/include/amcl/pf/eig3.h @@ -0,0 +1,11 @@ + +/* Eigen-decomposition for symmetric 3x3 real matrices. + Public domain, copied from the public domain Java library JAMA. */ + +#ifndef _eig_h + +/* Symmetric matrix A => eigenvectors in columns of V, corresponding + eigenvalues in d. */ +void eigen_decomposition(double A[3][3], double V[3][3], double d[3]); + +#endif diff --git a/Localizations/Libraries/Ros/amcl/include/amcl/pf/pf.h b/Localizations/Libraries/Ros/amcl/include/amcl/pf/pf.h new file mode 100644 index 0000000..ba778d1 --- /dev/null +++ b/Localizations/Libraries/Ros/amcl/include/amcl/pf/pf.h @@ -0,0 +1,210 @@ +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2000 Brian Gerkey & Kasper Stoy + * gerkey@usc.edu kaspers@robotics.usc.edu + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ +/************************************************************************** + * Desc: Simple particle filter for localization. + * Author: Andrew Howard + * Date: 10 Dec 2002 + * CVS: $Id: pf.h 3293 2005-11-19 08:37:45Z gerkey $ + *************************************************************************/ + +#ifndef PF_H +#define PF_H + +#include "pf_vector.h" +#include "pf_kdtree.h" + +#ifdef __cplusplus +extern "C" { +#endif + +// Forward declarations +struct _pf_t; +struct _rtk_fig_t; +struct _pf_sample_set_t; + +// Function prototype for the initialization model; generates a sample pose from +// an appropriate distribution. +typedef pf_vector_t (*pf_init_model_fn_t) (void *init_data); + +// Function prototype for the action model; generates a sample pose from +// an appropriate distribution +typedef void (*pf_action_model_fn_t) (void *action_data, + struct _pf_sample_set_t* set); + +// Function prototype for the sensor model; determines the probability +// for the given set of sample poses. +typedef double (*pf_sensor_model_fn_t) (void *sensor_data, + struct _pf_sample_set_t* set); + + +// Information for a single sample +typedef struct +{ + // Pose represented by this sample + pf_vector_t pose; + + // Weight for this pose + double weight; + +} pf_sample_t; + + +// Information for a cluster of samples +typedef struct +{ + // Number of samples + int count; + + // Total weight of samples in this cluster + double weight; + + // Cluster statistics + pf_vector_t mean; + pf_matrix_t cov; + + // Workspace + double m[4], c[2][2]; + +} pf_cluster_t; + + +// Information for a set of samples +typedef struct _pf_sample_set_t +{ + // The samples + int sample_count; + pf_sample_t *samples; + + // A kdtree encoding the histogram + pf_kdtree_t *kdtree; + + // Clusters + int cluster_count, cluster_max_count; + pf_cluster_t *clusters; + + // Filter statistics + pf_vector_t mean; + pf_matrix_t cov; + int converged; + double n_effective; +} pf_sample_set_t; + + +// Information for an entire filter +typedef struct _pf_t +{ + // This min and max number of samples + int min_samples, max_samples; + + // Population size parameters + double pop_err, pop_z; + + // Resample limit cache + int *limit_cache; + + // The sample sets. We keep two sets and use [current_set] + // to identify the active set. + int current_set; + pf_sample_set_t sets[2]; + + // Running averages, slow and fast, of likelihood + double w_slow, w_fast; + + // Decay rates for running averages + double alpha_slow, alpha_fast; + + // Function used to draw random pose samples + pf_init_model_fn_t random_pose_fn; + void *random_pose_data; + + double dist_threshold; //distance threshold in each axis over which the pf is considered to not be converged + int converged; + + // boolean parameter to enamble/diable selective resampling + int selective_resampling; +} pf_t; + + +// Create a new filter +pf_t *pf_alloc(int min_samples, int max_samples, + double alpha_slow, double alpha_fast, + pf_init_model_fn_t random_pose_fn, void *random_pose_data); + +// Free an existing filter +void pf_free(pf_t *pf); + +// Initialize the filter using a guassian +void pf_init(pf_t *pf, pf_vector_t mean, pf_matrix_t cov); + +// Initialize the filter using some model +void pf_init_model(pf_t *pf, pf_init_model_fn_t init_fn, void *init_data); + +// Update the filter with some new action +void pf_update_action(pf_t *pf, pf_action_model_fn_t action_fn, void *action_data); + +// Update the filter with some new sensor observation +void pf_update_sensor(pf_t *pf, pf_sensor_model_fn_t sensor_fn, void *sensor_data); + +// Resample the distribution +void pf_update_resample(pf_t *pf); + +// set selective resampling parameter +void pf_set_selective_resampling(pf_t *pf, int selective_resampling); + +// Compute the CEP statistics (mean and variance). +void pf_get_cep_stats(pf_t *pf, pf_vector_t *mean, double *var); + +// Compute the statistics for a particular cluster. Returns 0 if +// there is no such cluster. +int pf_get_cluster_stats(pf_t *pf, int cluster, double *weight, + pf_vector_t *mean, pf_matrix_t *cov); + +// Re-compute the cluster statistics for a sample set +void pf_cluster_stats(pf_t *pf, pf_sample_set_t *set); + + +// Display the sample set +void pf_draw_samples(pf_t *pf, struct _rtk_fig_t *fig, int max_samples); + +// Draw the histogram (kdtree) +void pf_draw_hist(pf_t *pf, struct _rtk_fig_t *fig); + +// Draw the CEP statistics +void pf_draw_cep_stats(pf_t *pf, struct _rtk_fig_t *fig); + +// Draw the cluster statistics +void pf_draw_cluster_stats(pf_t *pf, struct _rtk_fig_t *fig); + +//calculate if the particle filter has converged - +//and sets the converged flag in the current set and the pf +int pf_update_converged(pf_t *pf); + +//sets the current set and pf converged values to zero +void pf_init_converged(pf_t *pf); + +void pf_copy_set(pf_sample_set_t* set_a, pf_sample_set_t* set_b); + +#ifdef __cplusplus +} +#endif + + +#endif diff --git a/Localizations/Libraries/Ros/amcl/include/amcl/pf/pf_kdtree.h b/Localizations/Libraries/Ros/amcl/include/amcl/pf/pf_kdtree.h new file mode 100644 index 0000000..2368fa3 --- /dev/null +++ b/Localizations/Libraries/Ros/amcl/include/amcl/pf/pf_kdtree.h @@ -0,0 +1,109 @@ +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2000 Brian Gerkey & Kasper Stoy + * gerkey@usc.edu kaspers@robotics.usc.edu + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ +/************************************************************************** + * Desc: KD tree functions + * Author: Andrew Howard + * Date: 18 Dec 2002 + * CVS: $Id: pf_kdtree.h 6532 2008-06-11 02:45:56Z gbiggs $ + *************************************************************************/ + +#ifndef PF_KDTREE_H +#define PF_KDTREE_H + +#ifdef INCLUDE_RTKGUI +#include "rtk.h" +#endif + + +// Info for a node in the tree +typedef struct pf_kdtree_node +{ + // Depth in the tree + int leaf, depth; + + // Pivot dimension and value + int pivot_dim; + double pivot_value; + + // The key for this node + int key[3]; + + // The value for this node + double value; + + // The cluster label (leaf nodes) + int cluster; + + // Child nodes + struct pf_kdtree_node *children[2]; + +} pf_kdtree_node_t; + + +// A kd tree +typedef struct +{ + // Cell size + double size[3]; + + // The root node of the tree + pf_kdtree_node_t *root; + + // The number of nodes in the tree + int node_count, node_max_count; + pf_kdtree_node_t *nodes; + + // The number of leaf nodes in the tree + int leaf_count; + +} pf_kdtree_t; + + +// Create a tree +extern pf_kdtree_t *pf_kdtree_alloc(int max_size); + +// Destroy a tree +extern void pf_kdtree_free(pf_kdtree_t *self); + +// Clear all entries from the tree +extern void pf_kdtree_clear(pf_kdtree_t *self); + +// Insert a pose into the tree +extern void pf_kdtree_insert(pf_kdtree_t *self, pf_vector_t pose, double value); + +// Cluster the leaves in the tree +extern void pf_kdtree_cluster(pf_kdtree_t *self); + +// Determine the probability estimate for the given pose +extern double pf_kdtree_get_prob(pf_kdtree_t *self, pf_vector_t pose); + +// Determine the cluster label for the given pose +extern int pf_kdtree_get_cluster(pf_kdtree_t *self, pf_vector_t pose); + + +#ifdef INCLUDE_RTKGUI + +// Draw the tree +extern void pf_kdtree_draw(pf_kdtree_t *self, rtk_fig_t *fig); + +#endif + +#endif diff --git a/Localizations/Libraries/Ros/amcl/include/amcl/pf/pf_pdf.h b/Localizations/Libraries/Ros/amcl/include/amcl/pf/pf_pdf.h new file mode 100644 index 0000000..2f2ad87 --- /dev/null +++ b/Localizations/Libraries/Ros/amcl/include/amcl/pf/pf_pdf.h @@ -0,0 +1,85 @@ +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2000 Brian Gerkey & Kasper Stoy + * gerkey@usc.edu kaspers@robotics.usc.edu + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ +/************************************************************************** + * Desc: Useful pdf functions + * Author: Andrew Howard + * Date: 10 Dec 2002 + * CVS: $Id: pf_pdf.h 6345 2008-04-17 01:36:39Z gerkey $ + *************************************************************************/ + +#ifndef PF_PDF_H +#define PF_PDF_H + +#include "pf_vector.h" + +//#include +//#include + +#ifdef __cplusplus +extern "C" { +#endif + +/************************************************************************** + * Gaussian + *************************************************************************/ + +// Gaussian PDF info +typedef struct +{ + // Mean, covariance and inverse covariance + pf_vector_t x; + pf_matrix_t cx; + //pf_matrix_t cxi; + double cxdet; + + // Decomposed covariance matrix (rotation * diagonal) + pf_matrix_t cr; + pf_vector_t cd; + + // A random number generator + //gsl_rng *rng; + +} pf_pdf_gaussian_t; + + +// Create a gaussian pdf +pf_pdf_gaussian_t *pf_pdf_gaussian_alloc(pf_vector_t x, pf_matrix_t cx); + +// Destroy the pdf +void pf_pdf_gaussian_free(pf_pdf_gaussian_t *pdf); + +// Compute the value of the pdf at some point [z]. +//double pf_pdf_gaussian_value(pf_pdf_gaussian_t *pdf, pf_vector_t z); + +// Draw randomly from a zero-mean Gaussian distribution, with standard +// deviation sigma. +// We use the polar form of the Box-Muller transformation, explained here: +// http://www.taygeta.com/random/gaussian.html +double pf_ran_gaussian(double sigma); + +// Generate a sample from the pdf. +pf_vector_t pf_pdf_gaussian_sample(pf_pdf_gaussian_t *pdf); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/Localizations/Libraries/Ros/amcl/include/amcl/pf/pf_vector.h b/Localizations/Libraries/Ros/amcl/include/amcl/pf/pf_vector.h new file mode 100644 index 0000000..6162ba3 --- /dev/null +++ b/Localizations/Libraries/Ros/amcl/include/amcl/pf/pf_vector.h @@ -0,0 +1,94 @@ +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2000 Brian Gerkey & Kasper Stoy + * gerkey@usc.edu kaspers@robotics.usc.edu + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ +/************************************************************************** + * Desc: Vector functions + * Author: Andrew Howard + * Date: 10 Dec 2002 + * CVS: $Id: pf_vector.h 6345 2008-04-17 01:36:39Z gerkey $ + *************************************************************************/ + +#ifndef PF_VECTOR_H +#define PF_VECTOR_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include + +// The basic vector +typedef struct +{ + double v[3]; +} pf_vector_t; + + +// The basic matrix +typedef struct +{ + double m[3][3]; +} pf_matrix_t; + + +// Return a zero vector +pf_vector_t pf_vector_zero(); + +// Check for NAN or INF in any component +int pf_vector_finite(pf_vector_t a); + +// Print a vector +void pf_vector_fprintf(pf_vector_t s, FILE *file, const char *fmt); + +// Simple vector addition +pf_vector_t pf_vector_add(pf_vector_t a, pf_vector_t b); + +// Simple vector subtraction +pf_vector_t pf_vector_sub(pf_vector_t a, pf_vector_t b); + +// Transform from local to global coords (a + b) +pf_vector_t pf_vector_coord_add(pf_vector_t a, pf_vector_t b); + +// Transform from global to local coords (a - b) +pf_vector_t pf_vector_coord_sub(pf_vector_t a, pf_vector_t b); + + +// Return a zero matrix +pf_matrix_t pf_matrix_zero(); + +// Check for NAN or INF in any component +int pf_matrix_finite(pf_matrix_t a); + +// Print a matrix +void pf_matrix_fprintf(pf_matrix_t s, FILE *file, const char *fmt); + +// Compute the matrix inverse. Will also return the determinant, +// which should be checked for underflow (indicated singular matrix). +//pf_matrix_t pf_matrix_inverse(pf_matrix_t a, double *det); + +// Decompose a covariance matrix [a] into a rotation matrix [r] and a +// diagonal matrix [d] such that a = r * d * r^T. +void pf_matrix_unitary(pf_matrix_t *r, pf_matrix_t *d, pf_matrix_t a); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/Localizations/Libraries/Ros/amcl/include/amcl/portable_utils.hpp b/Localizations/Libraries/Ros/amcl/include/amcl/portable_utils.hpp new file mode 100644 index 0000000..f216cd5 --- /dev/null +++ b/Localizations/Libraries/Ros/amcl/include/amcl/portable_utils.hpp @@ -0,0 +1,35 @@ +#ifndef PORTABLE_UTILS_H +#define PORTABLE_UTILS_H + +#include + +#ifdef __cplusplus +extern "C" { +#endif + +#ifndef HAVE_DRAND48 +// Some system (e.g., Windows) doesn't come with drand48(), srand48(). +// Use rand, and srand for such system. + +// Remove extern declarations to avoid conflicts +// extern double drand48(void); // Change this to static if it should be static + +extern double drand48(void) +{ + return ((double)rand())/RAND_MAX; +} + +// Remove extern declarations to avoid conflicts +// extern double srand48(long int seedval); // Change this to static if it should be static + +extern void srand48(long int seedval) +{ + srand(seedval); +} +#endif + +#ifdef __cplusplus +} +#endif + +#endif \ No newline at end of file diff --git a/Localizations/Libraries/Ros/amcl/include/amcl/sensors/amcl_laser.h b/Localizations/Libraries/Ros/amcl/include/amcl/sensors/amcl_laser.h new file mode 100644 index 0000000..391c921 --- /dev/null +++ b/Localizations/Libraries/Ros/amcl/include/amcl/sensors/amcl_laser.h @@ -0,0 +1,156 @@ +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2000 Brian Gerkey et al. + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ +/////////////////////////////////////////////////////////////////////////// +// +// Desc: LASER sensor model for AMCL +// Author: Andrew Howard +// Date: 17 Aug 2003 +// CVS: $Id: amcl_laser.h 6443 2008-05-15 19:46:11Z gerkey $ +// +/////////////////////////////////////////////////////////////////////////// + +#ifndef AMCL_LASER_H +#define AMCL_LASER_H + +#include "amcl_sensor.h" +#include "../map/map.h" + +namespace amcl +{ + +typedef enum +{ + LASER_MODEL_BEAM, + LASER_MODEL_LIKELIHOOD_FIELD, + LASER_MODEL_LIKELIHOOD_FIELD_PROB +} laser_model_t; + +// Laser sensor data +class AMCLLaserData : public AMCLSensorData +{ + public: + AMCLLaserData () {ranges=NULL;}; + virtual ~AMCLLaserData() {delete [] ranges;}; + // Laser range data (range, bearing tuples) + public: int range_count; + public: double range_max; + public: double (*ranges)[2]; +}; + + +// Laseretric sensor model +class AMCLLaser : public AMCLSensor +{ + // Default constructor + public: AMCLLaser(size_t max_beams, map_t* map); + + public: virtual ~AMCLLaser(); + + public: void SetModelBeam(double z_hit, + double z_short, + double z_max, + double z_rand, + double sigma_hit, + double lambda_short, + double chi_outlier); + + public: void SetModelLikelihoodField(double z_hit, + double z_rand, + double sigma_hit, + double max_occ_dist); + + //a more probabilistically correct model - also with the option to do beam skipping + public: void SetModelLikelihoodFieldProb(double z_hit, + double z_rand, + double sigma_hit, + double max_occ_dist, + bool do_beamskip, + double beam_skip_distance, + double beam_skip_threshold, + double beam_skip_error_threshold); + + // Update the filter based on the sensor model. Returns true if the + // filter has been updated. + public: virtual bool UpdateSensor(pf_t *pf, AMCLSensorData *data); + + // Set the laser's pose after construction + public: void SetLaserPose(pf_vector_t& laser_pose) + {this->laser_pose = laser_pose;} + + // Determine the probability for the given pose + private: static double BeamModel(AMCLLaserData *data, + pf_sample_set_t* set); + // Determine the probability for the given pose + private: static double LikelihoodFieldModel(AMCLLaserData *data, + pf_sample_set_t* set); + + // Determine the probability for the given pose - more probablistic model + private: static double LikelihoodFieldModelProb(AMCLLaserData *data, + pf_sample_set_t* set); + + private: void reallocTempData(int max_samples, int max_obs); + + private: laser_model_t model_type; + + // Current data timestamp + private: double time; + + // The laser map + private: map_t *map; + + // Laser offset relative to robot + private: pf_vector_t laser_pose; + + // Max beams to consider + private: int max_beams; + + // Beam skipping parameters (used by LikelihoodFieldModelProb model) + private: bool do_beamskip; + private: double beam_skip_distance; + private: double beam_skip_threshold; + //threshold for the ratio of invalid beams - at which all beams are integrated to the likelihoods + //this would be an error condition + private: double beam_skip_error_threshold; + + //temp data that is kept before observations are integrated to each particle (requried for beam skipping) + private: int max_samples; + private: int max_obs; + private: double **temp_obs; + + // Laser model params + // + // Mixture params for the components of the model; must sum to 1 + private: double z_hit; + private: double z_short; + private: double z_max; + private: double z_rand; + // + // Stddev of Gaussian model for laser hits. + private: double sigma_hit; + // Decay rate of exponential model for short readings. + private: double lambda_short; + // Threshold for outlier rejection (unused) + private: double chi_outlier; +}; + + +} + +#endif diff --git a/Localizations/Libraries/Ros/amcl/include/amcl/sensors/amcl_laser.puml b/Localizations/Libraries/Ros/amcl/include/amcl/sensors/amcl_laser.puml new file mode 100644 index 0000000..3c161b9 --- /dev/null +++ b/Localizations/Libraries/Ros/amcl/include/amcl/sensors/amcl_laser.puml @@ -0,0 +1,54 @@ +@startuml + +class Player { + + Player() +} + +class AMCLLaserData { + + AMCLLaserData() + + ~AMCLLaserData() + # range_count: int + # range_max: double + # ranges: double[][] + +} + +class AMCLLaser { + - model_type: laser_model_t + - time: double + - map: map_t + - laser_pose: pf_vector_t + - max_beams: int + - do_beamskip: bool + - beam_skip_distance: double + - beam_skip_threshold: double + - beam_skip_error_threshold: double + - max_samples: int + - max_obs: int + - temp_obs: double[][] + - z_hit: double + - z_short: double + - z_max: double + - z_rand: double + - sigma_hit: double + - lambda_short: double + - chi_outlier: double + + + AMCLLaser(max_beams: size_t, map: map_t) + + ~AMCLLaser() + + SetModelBeam(z_hit: double, z_short: double, z_max: double, z_rand: double, sigma_hit: double, lambda_short: double, chi_outlier: double) + + SetModelLikelihoodField(z_hit: double, z_rand: double, sigma_hit: double, max_occ_dist: double) + + SetModelLikelihoodFieldProb(z_hit: double, z_rand: double, sigma_hit: double, max_occ_dist: double, do_beamskip: bool, beam_skip_distance: double, beam_skip_threshold: double, beam_skip_error_threshold: double) + + UpdateSensor(pf: pf_t, data: AMCLSensorData): bool + + SetLaserPose(laser_pose: pf_vector_t) + + BeamModel(data: AMCLLaserData, set: pf_sample_set_t): double + + LikelihoodFieldModel(data: AMCLLaserData, set: pf_sample_set_t): double + + LikelihoodFieldModelProb(data: AMCLLaserData, set: pf_sample_set_t): double + + reallocTempData(max_samples: int, max_obs: int) +} + +Player --|> AMCLSensor + +AMCLLaserData --|> AMCLSensorData + +@enduml \ No newline at end of file diff --git a/Localizations/Libraries/Ros/amcl/include/amcl/sensors/amcl_odom.h b/Localizations/Libraries/Ros/amcl/include/amcl/sensors/amcl_odom.h new file mode 100644 index 0000000..d50244a --- /dev/null +++ b/Localizations/Libraries/Ros/amcl/include/amcl/sensors/amcl_odom.h @@ -0,0 +1,98 @@ +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2000 Brian Gerkey et al. + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ +/////////////////////////////////////////////////////////////////////////// +// +// Desc: Odometry sensor model for AMCL +// Author: Andrew Howard +// Date: 17 Aug 2003 +// CVS: $Id: amcl_odom.h 4135 2007-08-23 19:58:48Z gerkey $ +// +/////////////////////////////////////////////////////////////////////////// + +#ifndef AMCL_ODOM_H +#define AMCL_ODOM_H + +#include "amcl_sensor.h" +#include "../pf/pf_pdf.h" + +namespace amcl +{ + +typedef enum +{ + ODOM_MODEL_DIFF, + ODOM_MODEL_OMNI, + ODOM_MODEL_DIFF_CORRECTED, + ODOM_MODEL_OMNI_CORRECTED +} odom_model_t; + +// Odometric sensor data +class AMCLOdomData : public AMCLSensorData +{ + // Odometric pose + public: pf_vector_t pose; + + // Change in odometric pose + public: pf_vector_t delta; +}; + + +// Odometric sensor model +class AMCLOdom : public AMCLSensor +{ + // Default constructor + public: AMCLOdom(); + + public: void SetModelDiff(double alpha1, + double alpha2, + double alpha3, + double alpha4); + + public: void SetModelOmni(double alpha1, + double alpha2, + double alpha3, + double alpha4, + double alpha5); + + public: void SetModel( odom_model_t type, + double alpha1, + double alpha2, + double alpha3, + double alpha4, + double alpha5 = 0 ); + + // Update the filter based on the action model. Returns true if the filter + // has been updated. + public: virtual bool UpdateAction(pf_t *pf, AMCLSensorData *data); + + // Current data timestamp + private: double time; + + // Model type + private: odom_model_t model_type; + + // Drift parameters + private: double alpha1, alpha2, alpha3, alpha4, alpha5; +}; + + +} + +#endif diff --git a/Localizations/Libraries/Ros/amcl/include/amcl/sensors/amcl_odom.puml b/Localizations/Libraries/Ros/amcl/include/amcl/sensors/amcl_odom.puml new file mode 100644 index 0000000..8991b0f --- /dev/null +++ b/Localizations/Libraries/Ros/amcl/include/amcl/sensors/amcl_odom.puml @@ -0,0 +1,54 @@ +@startuml + +class Player { + + Player() +} + +class AMCLSensor { + - is_action: bool + - pose: pf_vector_t + + + AMCLSensor() + + ~AMCLSensor() + + UpdateAction(pf: pf_t, data: AMCLSensorData): bool + + InitSensor(pf: pf_t, data: AMCLSensorData): bool + + UpdateSensor(pf: pf_t, data: AMCLSensorData): bool +} + +class AMCLSensorData { + - sensor: AMCLSensor + + + AMCLSensorData() + + ~AMCLSensorData() +} + +class AMCLOdomData { + - pose: pf_vector_t + - delta: pf_vector_t + + + AMCLOdomData() +} + +class AMCLOdom { + - time: double + - model_type: odom_model_t + - alpha1: double + - alpha2: double + - alpha3: double + - alpha4: double + - alpha5: double + + + AMCLOdom() + + SetModelDiff(alpha1: double, alpha2: double, alpha3: double, alpha4: double) + + SetModelOmni(alpha1: double, alpha2: double, alpha3: double, alpha4: double, alpha5: double) + + SetModel(type: odom_model_t, alpha1: double, alpha2: double, alpha3: double, alpha4: double, alpha5: double = 0) + + UpdateAction(pf: pf_t, data: AMCLSensorData): bool +} + +Player --|> AMCLSensor + +AMCLSensorData --|> AMCLSensor +AMCLOdomData --|> AMCLSensorData +AMCLOdom --|> AMCLSensor + +@enduml diff --git a/Localizations/Libraries/Ros/amcl/include/amcl/sensors/amcl_sensor.h b/Localizations/Libraries/Ros/amcl/include/amcl/sensors/amcl_sensor.h new file mode 100644 index 0000000..fe0875a --- /dev/null +++ b/Localizations/Libraries/Ros/amcl/include/amcl/sensors/amcl_sensor.h @@ -0,0 +1,97 @@ +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2000 Brian Gerkey et al. + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ +/////////////////////////////////////////////////////////////////////////// +// +// Desc: Adaptive Monte-Carlo localization +// Author: Andrew Howard +// Date: 6 Feb 2003 +// CVS: $Id: amcl_sensor.h 6443 2008-05-15 19:46:11Z gerkey $ +// +/////////////////////////////////////////////////////////////////////////// + +#ifndef AMCL_SENSOR_H +#define AMCL_SENSOR_H + +#include "../pf/pf.h" + +namespace amcl +{ + +// Forward declarations +class AMCLSensorData; + + +// Base class for all AMCL sensors +class AMCLSensor +{ + // Default constructor + public: AMCLSensor(); + + // Default destructor + public: virtual ~AMCLSensor(); + + // Update the filter based on the action model. Returns true if the filter + // has been updated. + public: virtual bool UpdateAction(pf_t *pf, AMCLSensorData *data); + + // Initialize the filter based on the sensor model. Returns true if the + // filter has been initialized. + public: virtual bool InitSensor(pf_t *pf, AMCLSensorData *data); + + // Update the filter based on the sensor model. Returns true if the + // filter has been updated. + public: virtual bool UpdateSensor(pf_t *pf, AMCLSensorData *data); + + // Flag is true if this is the action sensor + public: bool is_action; + + // Action pose (action sensors only) + public: pf_vector_t pose; + + // AMCL Base + //protected: AdaptiveMCL & AMCL; + +#ifdef INCLUDE_RTKGUI + // Setup the GUI + public: virtual void SetupGUI(rtk_canvas_t *canvas, rtk_fig_t *robot_fig); + + // Finalize the GUI + public: virtual void ShutdownGUI(rtk_canvas_t *canvas, rtk_fig_t *robot_fig); + + // Draw sensor data + public: virtual void UpdateGUI(rtk_canvas_t *canvas, rtk_fig_t *robot_fig, AMCLSensorData *data); +#endif +}; + + + +// Base class for all AMCL sensor measurements +class AMCLSensorData +{ + // Pointer to sensor that generated the data + public: AMCLSensor *sensor; + virtual ~AMCLSensorData() {} + + // Data timestamp + public: double time; +}; + +} + +#endif diff --git a/Localizations/Libraries/Ros/amcl/include/amcl/sensors/amcl_sensor.puml b/Localizations/Libraries/Ros/amcl/include/amcl/sensors/amcl_sensor.puml new file mode 100644 index 0000000..7197de0 --- /dev/null +++ b/Localizations/Libraries/Ros/amcl/include/amcl/sensors/amcl_sensor.puml @@ -0,0 +1,29 @@ +@startuml + +class Player { + + Player() +} + +class AMCLSensor { + - is_action: bool + - pose: pf_vector_t + + + AMCLSensor() + + ~AMCLSensor() + + UpdateAction(pf: pf_t, data: AMCLSensorData): bool + + InitSensor(pf: pf_t, data: AMCLSensorData): bool + + UpdateSensor(pf: pf_t, data: AMCLSensorData): bool +} + +class AMCLSensorData { + - sensor: AMCLSensor + + + AMCLSensorData() + + ~AMCLSensorData() +} + +Player --|> AMCLSensor + +AMCLSensorData --|> AMCLSensor + +@enduml \ No newline at end of file diff --git a/Localizations/Libraries/Ros/amcl/package.xml b/Localizations/Libraries/Ros/amcl/package.xml new file mode 100644 index 0000000..8bed780 --- /dev/null +++ b/Localizations/Libraries/Ros/amcl/package.xml @@ -0,0 +1,47 @@ + + + + amcl + 1.17.3 + +

+ amcl is a probabilistic localization system for a robot moving in + 2D. It implements the adaptive (or KLD-sampling) Monte Carlo + localization approach (as described by Dieter Fox), which uses a + particle filter to track the pose of a robot against a known map. +

+

+ This node is derived, with thanks, from Andrew Howard's excellent + 'amcl' Player driver. +

+ + http://wiki.ros.org/amcl + Brian P. Gerkey + contradict@gmail.com + David V. Lu!! + Michael Ferguson + Aaron Hoy + LGPL + + catkin + + message_filters + tf2_geometry_msgs + + diagnostic_updater + dynamic_reconfigure + geometry_msgs + nav_msgs + rosbag + roscpp + sensor_msgs + std_srvs + tf2 + tf2_msgs + tf2_ros + + map_server + rostest + python3-pykdl + tf2_py + diff --git a/Localizations/Libraries/Ros/amcl/src/amcl.cpp b/Localizations/Libraries/Ros/amcl/src/amcl.cpp new file mode 100644 index 0000000..4b0a277 --- /dev/null +++ b/Localizations/Libraries/Ros/amcl/src/amcl.cpp @@ -0,0 +1,1409 @@ +#include "amcl/amcl.h" + +amcl::Amcl::Amcl() : sent_first_transform_(false), + latest_tf_valid_(false), + map_(NULL), + pf_(NULL), + resample_count_(0), + odom_(NULL), + laser_(NULL), + private_nh_("~/Amcl"), + initial_pose_hyp_(NULL), + first_map_received_(false), + first_reconfigure_call_(true), + dsrv_(NULL), + laser_scan_filter_(NULL), + laser_scan_sub_(NULL), + tfb_(nullptr), + tf_(nullptr), + tfl_(nullptr) +{ + init_pose_[0] = 0.0; + init_pose_[1] = 0.0; + init_pose_[2] = 0.0; + init_cov_[0] = 0.5 * 0.5; + init_cov_[1] = 0.5 * 0.5; + init_cov_[2] = (M_PI / 12.0) * (M_PI / 12.0); + this->initialize(private_nh_); +} + +amcl::Amcl::Amcl(ros::NodeHandle nh) : sent_first_transform_(false), + latest_tf_valid_(false), + map_(NULL), + pf_(NULL), + resample_count_(0), + odom_(NULL), + laser_(NULL), + private_nh_("~/Amcl"), + initial_pose_hyp_(NULL), + first_map_received_(false), + first_reconfigure_call_(true), + initialized_(false), + dsrv_(NULL), + laser_scan_filter_(NULL), + laser_scan_sub_(NULL), + tfb_(nullptr), + tf_(nullptr), + tfl_(nullptr) +{ + init_pose_[0] = 0.0; + init_pose_[1] = 0.0; + init_pose_[2] = 0.0; + init_cov_[0] = 0.5 * 0.5; + init_cov_[1] = 0.5 * 0.5; + init_cov_[2] = (M_PI / 12.0) * (M_PI / 12.0); + this->initialize(nh); +} + +amcl::Amcl::~Amcl() +{ + if (dsrv_) + { + delete dsrv_; + dsrv_ = NULL; + } + + freeMapDependentMemory(); + if (laser_scan_filter_) + { + delete laser_scan_filter_; + laser_scan_filter_ = NULL; + } + + if (laser_scan_sub_) + { + delete laser_scan_sub_; + laser_scan_sub_ = NULL; + } + + if (tfl_) + tfl_.reset(); + if (tf_) + tf_.reset(); + if (tfb_) + tfb_.reset(); + // TODO: delete everything allocated in constructor +} + +void amcl::Amcl::initialize(ros::NodeHandle nh) +{ + if (!initialized_) + { + + // boost::recursive_mutex::scoped_lock l(configuration_mutex_); + // Grab params off the param server + nh.param("use_map_topic", use_map_topic_, false); + nh.param("first_map_only", first_map_only_, false); + + nh.param("scan_topic", scan_topic_, std::string("scan")); + nh.param("map_topic", map_topic_, std::string("map")); + nh.param("map_service", map_service_, std::string("static_map")); + + double tmp; + nh.param("gui_publish_rate", tmp, -1.0); + gui_publish_period = ros::Duration(1.0 / tmp); + nh.param("save_pose_rate", tmp, 0.5); + save_pose_period = ros::Duration(1.0 / tmp); + + nh.param("laser_min_range", laser_min_range_, -1.0); + nh.param("laser_max_range", laser_max_range_, -1.0); + nh.param("laser_max_beams", max_beams_, 30); + nh.param("min_particles", min_particles_, 100); + nh.param("max_particles", max_particles_, 5000); + nh.param("kld_err", pf_err_, 0.01); + nh.param("kld_z", pf_z_, 0.99); + nh.param("odom_alpha1", alpha1_, 0.2); + nh.param("odom_alpha2", alpha2_, 0.2); + nh.param("odom_alpha3", alpha3_, 0.2); + nh.param("odom_alpha4", alpha4_, 0.2); + nh.param("odom_alpha5", alpha5_, 0.2); + + nh.param("do_beamskip", do_beamskip_, false); + nh.param("beam_skip_distance", beam_skip_distance_, 0.5); + nh.param("beam_skip_threshold", beam_skip_threshold_, 0.3); + if (nh.hasParam("beam_skip_error_threshold_")) + { + nh.param("beam_skip_error_threshold_", beam_skip_error_threshold_); + } + else + { + nh.param("beam_skip_error_threshold", beam_skip_error_threshold_, 0.9); + } + + nh.param("laser_z_hit", z_hit_, 0.95); + nh.param("laser_z_short", z_short_, 0.1); + nh.param("laser_z_max", z_max_, 0.05); + nh.param("laser_z_rand", z_rand_, 0.05); + nh.param("laser_sigma_hit", sigma_hit_, 0.2); + nh.param("laser_lambda_short", lambda_short_, 0.1); + nh.param("laser_likelihood_max_dist", laser_likelihood_max_dist_, 2.0); + std::string tmp_model_type; + nh.param("laser_model_type", tmp_model_type, std::string("likelihood_field")); + if (tmp_model_type == "beam") + laser_model_type_ = amcl::LASER_MODEL_BEAM; + else if (tmp_model_type == "likelihood_field") + laser_model_type_ = amcl::LASER_MODEL_LIKELIHOOD_FIELD; + else if (tmp_model_type == "likelihood_field_prob") + { + laser_model_type_ = amcl::LASER_MODEL_LIKELIHOOD_FIELD_PROB; + } + else + { + ROS_WARN("Unknown laser model type \"%s\"; defaulting to likelihood_field model", + tmp_model_type.c_str()); + laser_model_type_ = amcl::LASER_MODEL_LIKELIHOOD_FIELD; + } + + nh.param("odom_model_type", tmp_model_type, std::string("diff")); + if (tmp_model_type == "diff") + odom_model_type_ = amcl::ODOM_MODEL_DIFF; + else if (tmp_model_type == "omni") + odom_model_type_ = amcl::ODOM_MODEL_OMNI; + else if (tmp_model_type == "diff-corrected") + odom_model_type_ = amcl::ODOM_MODEL_DIFF_CORRECTED; + else if (tmp_model_type == "omni-corrected") + odom_model_type_ = amcl::ODOM_MODEL_OMNI_CORRECTED; + else + { + ROS_WARN("Unknown odom model type \"%s\"; defaulting to diff model", + tmp_model_type.c_str()); + odom_model_type_ = amcl::ODOM_MODEL_DIFF; + } + + nh.param("update_min_d", d_thresh_, 0.2); + nh.param("update_min_a", a_thresh_, M_PI / 6.0); + nh.param("odom_frame_id", odom_frame_id_, std::string("odom")); + nh.param("base_frame_id", base_frame_id_, std::string("base_link")); + nh.param("global_frame_id", global_frame_id_, std::string("map")); + nh.param("resample_interval", resample_interval_, 2); + nh.param("selective_resampling", selective_resampling_, false); + double tmp_tol; + nh.param("transform_tolerance", tmp_tol, 0.1); + nh.param("recovery_alpha_slow", alpha_slow_, 0.001); + nh.param("recovery_alpha_fast", alpha_fast_, 0.1); + nh.param("tf_broadcast", tf_broadcast_, true); + nh.param("force_update_after_initialpose", force_update_after_initialpose_, false); + nh.param("force_update_after_set_map", force_update_after_set_map_, false); + + // For diagnostics + nh.param("std_warn_level_x", std_warn_level_x_, 0.2); + nh.param("std_warn_level_y", std_warn_level_y_, 0.2); + nh.param("std_warn_level_yaw", std_warn_level_yaw_, 0.1); + + transform_tolerance_.fromSec(tmp_tol); + { + double bag_scan_period; + nh.param("bag_scan_period", bag_scan_period, -1.0); + bag_scan_period_.fromSec(bag_scan_period); + } + + odom_frame_id_ = stripSlash(odom_frame_id_); + base_frame_id_ = stripSlash(base_frame_id_); + global_frame_id_ = stripSlash(global_frame_id_); + + updatePoseFromServer(); + + cloud_pub_interval.fromSec(1.0); + tfb_ = std::make_shared(); + tf_ = std::make_shared(); + tfl_ = std::make_shared(*tf_); + + pose_pub_ = nh_.advertise("amcl_pose", 2, true); + particlecloud_pub_ = nh_.advertise("particlecloud", 2, true); + global_loc_srv_ = nh_.advertiseService("global_localization", + &amcl::Amcl::globalLocalizationCallback, + this); + nomotion_update_srv_ = nh_.advertiseService("request_nomotion_update", &amcl::Amcl::nomotionUpdateCallback, this); + set_map_srv_ = nh_.advertiseService("set_map", &amcl::Amcl::setMapCallback, this); + + laser_scan_sub_ = new message_filters::Subscriber(nh_, scan_topic_, 100); + laser_scan_filter_ = + new tf2_ros::MessageFilter(*laser_scan_sub_, + *tf_, + odom_frame_id_, + 100, + nh_); + laser_scan_filter_->registerCallback(boost::bind(&amcl::Amcl::laserReceived, + this, _1)); + initial_pose_sub_ = nh_.subscribe("initialpose", 2, &amcl::Amcl::initialPoseReceived, this); + + if (use_map_topic_) + { + map_sub_ = nh_.subscribe(map_topic_, 1, &amcl::Amcl::mapReceived, this); + ROS_INFO("Subscribed to map topic."); + } + else + { + requestMap(); + } + m_force_update = false; + + dsrv_ = new dynamic_reconfigure::Server(private_nh_); + dynamic_reconfigure::Server::CallbackType cb = boost::bind(&amcl::Amcl::reconfigureCB, this, _1, _2); + dsrv_->setCallback(cb); + + // 15s timer to warn on lack of receipt of laser scans, #5209 + laser_check_interval_ = ros::Duration(15.0); + check_laser_timer_ = nh_.createTimer(laser_check_interval_, + boost::bind(&amcl::Amcl::checkLaserReceived, this, _1)); + + diagnosic_updater_.setHardwareID("None"); + diagnosic_updater_.add("Standard deviation", this, &amcl::Amcl::standardDeviationDiagnostics); + ros::Duration(0.5).sleep(); + initialized_ = true; + } +} + +void amcl::Amcl::reconfigureCB(amcl::AMCLConfig &config, uint32_t level) +{ + boost::recursive_mutex::scoped_lock cfl(configuration_mutex_); + + // we don't want to do anything on the first call + // which corresponds to startup + if (first_reconfigure_call_) + { + first_reconfigure_call_ = false; + default_config_ = config; + return; + } + + if (config.restore_defaults) + { + config = default_config_; + // avoid looping + config.restore_defaults = false; + } + + d_thresh_ = config.update_min_d; + a_thresh_ = config.update_min_a; + + resample_interval_ = config.resample_interval; + + laser_min_range_ = config.laser_min_range; + laser_max_range_ = config.laser_max_range; + + gui_publish_period = ros::Duration(1.0 / config.gui_publish_rate); + save_pose_period = ros::Duration(1.0 / config.save_pose_rate); + + transform_tolerance_.fromSec(config.transform_tolerance); + + max_beams_ = config.laser_max_beams; + alpha1_ = config.odom_alpha1; + alpha2_ = config.odom_alpha2; + alpha3_ = config.odom_alpha3; + alpha4_ = config.odom_alpha4; + alpha5_ = config.odom_alpha5; + + z_hit_ = config.laser_z_hit; + z_short_ = config.laser_z_short; + z_max_ = config.laser_z_max; + z_rand_ = config.laser_z_rand; + sigma_hit_ = config.laser_sigma_hit; + lambda_short_ = config.laser_lambda_short; + laser_likelihood_max_dist_ = config.laser_likelihood_max_dist; + + if (config.laser_model_type == "beam") + laser_model_type_ = amcl::LASER_MODEL_BEAM; + else if (config.laser_model_type == "likelihood_field") + laser_model_type_ = amcl::LASER_MODEL_LIKELIHOOD_FIELD; + else if (config.laser_model_type == "likelihood_field_prob") + laser_model_type_ = amcl::LASER_MODEL_LIKELIHOOD_FIELD_PROB; + + if (config.odom_model_type == "diff") + odom_model_type_ = amcl::ODOM_MODEL_DIFF; + else if (config.odom_model_type == "omni") + odom_model_type_ = amcl::ODOM_MODEL_OMNI; + else if (config.odom_model_type == "diff-corrected") + odom_model_type_ = amcl::ODOM_MODEL_DIFF_CORRECTED; + else if (config.odom_model_type == "omni-corrected") + odom_model_type_ = amcl::ODOM_MODEL_OMNI_CORRECTED; + + if (config.min_particles > config.max_particles) + { + ROS_WARN("You've set min_particles to be greater than max particles, this isn't allowed so they'll be set to be equal."); + config.max_particles = config.min_particles; + } + + min_particles_ = config.min_particles; + max_particles_ = config.max_particles; + alpha_slow_ = config.recovery_alpha_slow; + alpha_fast_ = config.recovery_alpha_fast; + tf_broadcast_ = config.tf_broadcast; + force_update_after_initialpose_ = config.force_update_after_initialpose; + force_update_after_set_map_ = config.force_update_after_set_map; + + do_beamskip_ = config.do_beamskip; + beam_skip_distance_ = config.beam_skip_distance; + beam_skip_threshold_ = config.beam_skip_threshold; + + // Clear queued laser objects so that their parameters get updated + lasers_.clear(); + lasers_update_.clear(); + frame_to_laser_.clear(); + + if (pf_ != NULL) + { + pf_free(pf_); + pf_ = NULL; + } + pf_ = pf_alloc(min_particles_, max_particles_, + alpha_slow_, alpha_fast_, + (pf_init_model_fn_t)amcl::Amcl::uniformPoseGenerator, + (void *)map_); + pf_set_selective_resampling(pf_, selective_resampling_); + pf_err_ = config.kld_err; + pf_z_ = config.kld_z; + pf_->pop_err = pf_err_; + pf_->pop_z = pf_z_; + + // Initialize the filter + pf_vector_t pf_init_pose_mean = pf_vector_zero(); + pf_init_pose_mean.v[0] = last_published_pose.pose.pose.position.x; + pf_init_pose_mean.v[1] = last_published_pose.pose.pose.position.y; + pf_init_pose_mean.v[2] = tf2::getYaw(last_published_pose.pose.pose.orientation); + pf_matrix_t pf_init_pose_cov = pf_matrix_zero(); + pf_init_pose_cov.m[0][0] = last_published_pose.pose.covariance[6 * 0 + 0]; + pf_init_pose_cov.m[1][1] = last_published_pose.pose.covariance[6 * 1 + 1]; + pf_init_pose_cov.m[2][2] = last_published_pose.pose.covariance[6 * 5 + 5]; + pf_init(pf_, pf_init_pose_mean, pf_init_pose_cov); + pf_init_ = false; + + // Instantiate the sensor objects + // Odometry + delete odom_; + odom_ = new amcl::AMCLOdom(); + ROS_ASSERT(odom_); + odom_->SetModel(odom_model_type_, alpha1_, alpha2_, alpha3_, alpha4_, alpha5_); + // Laser + delete laser_; + laser_ = new amcl::AMCLLaser(max_beams_, map_); + ROS_ASSERT(laser_); + if (laser_model_type_ == amcl::LASER_MODEL_BEAM) + laser_->SetModelBeam(z_hit_, z_short_, z_max_, z_rand_, + sigma_hit_, lambda_short_, 0.0); + else if (laser_model_type_ == amcl::LASER_MODEL_LIKELIHOOD_FIELD_PROB) + { + ROS_INFO("Initializing likelihood field model; this can take some time on large maps..."); + laser_->SetModelLikelihoodFieldProb(z_hit_, z_rand_, sigma_hit_, + laser_likelihood_max_dist_, + do_beamskip_, beam_skip_distance_, + beam_skip_threshold_, beam_skip_error_threshold_); + ROS_INFO("Done initializing likelihood field model with probabilities."); + } + else if (laser_model_type_ == amcl::LASER_MODEL_LIKELIHOOD_FIELD) + { + ROS_INFO("Initializing likelihood field model; this can take some time on large maps..."); + laser_->SetModelLikelihoodField(z_hit_, z_rand_, sigma_hit_, + laser_likelihood_max_dist_); + ROS_INFO("Done initializing likelihood field model."); + } + + odom_frame_id_ = stripSlash(config.odom_frame_id); + base_frame_id_ = stripSlash(config.base_frame_id); + global_frame_id_ = stripSlash(config.global_frame_id); + + delete laser_scan_filter_; + laser_scan_filter_ = + new tf2_ros::MessageFilter(*laser_scan_sub_, + *tf_, + odom_frame_id_, + 100, + nh_); + laser_scan_filter_->registerCallback(boost::bind(&amcl::Amcl::laserReceived, + this, _1)); + + initial_pose_sub_ = nh_.subscribe("initialpose", 2, &amcl::Amcl::initialPoseReceived, this); +} + +void amcl::Amcl::runFromBag(const std::string &in_bag_fn, bool trigger_global_localization) +{ + rosbag::Bag bag; + bag.open(in_bag_fn, rosbag::bagmode::Read); + std::vector topics; + topics.push_back(std::string("tf")); + std::string scan_topic_name = "base_scan"; // TODO determine what topic this actually is from ROS + topics.push_back(scan_topic_name); + rosbag::View view(bag, rosbag::TopicQuery(topics)); + + ros::Publisher laser_pub = nh_.advertise(scan_topic_name, 100); + ros::Publisher tf_pub = nh_.advertise("/tf", 100); + + // Sleep for a second to let all subscribers connect + ros::WallDuration(1.0).sleep(); + + ros::WallTime start(ros::WallTime::now()); + + // Wait for map + while (ros::ok()) + { + { + boost::recursive_mutex::scoped_lock cfl(configuration_mutex_); + if (map_) + { + ROS_INFO("Map is ready"); + break; + } + } + ROS_INFO("Waiting for the map..."); + ros::getGlobalCallbackQueue()->callAvailable(ros::WallDuration(1.0)); + } + + if (trigger_global_localization) + { + std_srvs::Empty empty_srv; + globalLocalizationCallback(empty_srv.request, empty_srv.response); + } + + BOOST_FOREACH (rosbag::MessageInstance const msg, view) + { + if (!ros::ok()) + { + break; + } + + // Process any ros messages or callbacks at this point + ros::getGlobalCallbackQueue()->callAvailable(ros::WallDuration()); + + tf2_msgs::TFMessage::ConstPtr tf_msg = msg.instantiate(); + if (tf_msg != NULL) + { + tf_pub.publish(msg); + for (size_t ii = 0; ii < tf_msg->transforms.size(); ++ii) + { + tf_->setTransform(tf_msg->transforms[ii], "rosbag_authority"); + } + continue; + } + + sensor_msgs::LaserScan::ConstPtr base_scan = msg.instantiate(); + if (base_scan != NULL) + { + laser_pub.publish(msg); + laser_scan_filter_->add(base_scan); + if (bag_scan_period_ > ros::WallDuration(0)) + { + bag_scan_period_.sleep(); + } + continue; + } + + ROS_WARN_STREAM("Unsupported message type" << msg.getTopic()); + } + + bag.close(); + + double runtime = (ros::WallTime::now() - start).toSec(); + ROS_INFO("Bag complete, took %.1f seconds to process, shutting down", runtime); + + const geometry_msgs::Quaternion &q(last_published_pose.pose.pose.orientation); + double yaw, pitch, roll; + tf2::Matrix3x3(tf2::Quaternion(q.x, q.y, q.z, q.w)).getEulerYPR(yaw, pitch, roll); + ROS_INFO("Final location %.3f, %.3f, %.3f with stamp=%f", + last_published_pose.pose.pose.position.x, + last_published_pose.pose.pose.position.y, + yaw, last_published_pose.header.stamp.toSec()); + + ros::shutdown(); +} + +void amcl::Amcl::savePoseToServer(geometry_msgs::PoseWithCovarianceStamped &amcl_pose) +{ + // We need to apply the last transform to the latest odom pose to get + // the latest map pose to store. We'll take the covariance from + // last_published_pose. + tf2::Transform odom_pose_tf2; + tf2::convert(latest_odom_pose_.pose, odom_pose_tf2); + tf2::Transform map_pose = latest_tf_.inverse() * odom_pose_tf2; + + double yaw = tf2::getYaw(map_pose.getRotation()); + + ROS_DEBUG("Saving pose to server. x: %.3f, y: %.3f, yaw: %.3f", map_pose.getOrigin().x(), map_pose.getOrigin().y(), yaw); + // ROS_INFO("Saving pose to server %s. x: %.3f, y: %.3f, yaw: %.3f", private_nh_.getNamespace().c_str(), map_pose.getOrigin().x(), map_pose.getOrigin().y(), yaw); + + private_nh_.setParam("initial_pose_x", map_pose.getOrigin().x()); + private_nh_.setParam("initial_pose_y", map_pose.getOrigin().y()); + private_nh_.setParam("initial_pose_a", yaw); + private_nh_.setParam("initial_cov_xx", + last_published_pose.pose.covariance[6 * 0 + 0]); + private_nh_.setParam("initial_cov_yy", + last_published_pose.pose.covariance[6 * 1 + 1]); + private_nh_.setParam("initial_cov_aa", + last_published_pose.pose.covariance[6 * 5 + 5]); + + // Set the pose in the input parameter object (amcl_pose) + amcl_pose.header.stamp = ros::Time::now(); // Set timestamp + amcl_pose.header.frame_id = "map"; // Set frame id to "map" + amcl_pose.pose.pose.position.x = map_pose.getOrigin().x(); + amcl_pose.pose.pose.position.y = map_pose.getOrigin().y(); + amcl_pose.pose.pose.orientation = tf2::toMsg(map_pose.getRotation()); + + // Set the covariance from last_published_pose + for (size_t i = 0; i < 36; ++i) + { + amcl_pose.pose.covariance[i] = last_published_pose.pose.covariance[i]; + } +} + +void amcl::Amcl::updatePoseFromServer() +{ + init_pose_[0] = 0.0; + init_pose_[1] = 0.0; + init_pose_[2] = 0.0; + init_cov_[0] = 0.5 * 0.5; + init_cov_[1] = 0.5 * 0.5; + init_cov_[2] = (M_PI / 12.0) * (M_PI / 12.0); + // Check for NAN on input from param server, #5239 + double tmp_pos; + private_nh_.param("initial_pose_x", tmp_pos, init_pose_[0]); + if (!std::isnan(tmp_pos)) + init_pose_[0] = tmp_pos; + else + ROS_WARN("ignoring NAN in initial pose X position"); + private_nh_.param("initial_pose_y", tmp_pos, init_pose_[1]); + if (!std::isnan(tmp_pos)) + init_pose_[1] = tmp_pos; + else + ROS_WARN("ignoring NAN in initial pose Y position"); + private_nh_.param("initial_pose_a", tmp_pos, init_pose_[2]); + if (!std::isnan(tmp_pos)) + init_pose_[2] = tmp_pos; + else + ROS_WARN("ignoring NAN in initial pose Yaw"); + private_nh_.param("initial_cov_xx", tmp_pos, init_cov_[0]); + if (!std::isnan(tmp_pos)) + init_cov_[0] = tmp_pos; + else + ROS_WARN("ignoring NAN in initial covariance XX"); + private_nh_.param("initial_cov_yy", tmp_pos, init_cov_[1]); + if (!std::isnan(tmp_pos)) + init_cov_[1] = tmp_pos; + else + ROS_WARN("ignoring NAN in initial covariance YY"); + private_nh_.param("initial_cov_aa", tmp_pos, init_cov_[2]); + if (!std::isnan(tmp_pos)) + init_cov_[2] = tmp_pos; + else + ROS_WARN("ignoring NAN in initial covariance AA"); + + // ROS_INFO("Updates pose %s [%f %f %f]", private_nh_.getNamespace().c_str(), init_pose_[0], init_pose_[1], init_pose_[2]); +} + +void amcl::Amcl::checkLaserReceived(const ros::TimerEvent &event) +{ + ros::Duration d = ros::Time::now() - last_laser_received_ts_; + if (d > laser_check_interval_) + { + ROS_WARN("No laser scan received (and thus no pose updates have been published) for %f seconds. Verify that data is being published on the %s topic.", + d.toSec(), + ros::names::resolve(scan_topic_).c_str()); + } +} + +void amcl::Amcl::requestMap() +{ + boost::recursive_mutex::scoped_lock ml(configuration_mutex_); + + // get map via RPC + nav_msgs::GetMap::Request req; + nav_msgs::GetMap::Response resp; + ROS_INFO("Requesting the map..."); + while (!ros::service::call(map_service_, req, resp)) + { + ROS_WARN("Request for map failed; trying again..."); + ros::Duration d(0.5); + d.sleep(); + } + handleMapMessage(resp.map); +} + +void amcl::Amcl::mapReceived(const nav_msgs::OccupancyGridConstPtr &msg) +{ + if (first_map_only_ && first_map_received_) + { + return; + } + + handleMapMessage(*msg); + + first_map_received_ = true; +} + +void amcl::Amcl::handleMapMessage(const nav_msgs::OccupancyGrid &msg) +{ + boost::recursive_mutex::scoped_lock cfl(configuration_mutex_); + + ROS_INFO("[Amcl] Received a %d X %d map @ %.3f m/pix\n", + msg.info.width, + msg.info.height, + msg.info.resolution); + + if (msg.header.frame_id != global_frame_id_) + ROS_WARN("Frame_id of map received:'%s' doesn't match global_frame_id:'%s'. This could cause issues with reading published topics", + msg.header.frame_id.c_str(), + global_frame_id_.c_str()); + + freeMapDependentMemory(); + + // Clear queued laser objects because they hold pointers to the existing + // map, #5202. + lasers_.clear(); + lasers_update_.clear(); + frame_to_laser_.clear(); + + map_ = convertMap(msg); + +#if NEW_UNIFORM_SAMPLING + // Index of free space + free_space_indices.resize(0); + for (int i = 0; i < map_->size_x; i++) + for (int j = 0; j < map_->size_y; j++) + if (map_->cells[MAP_INDEX(map_, i, j)].occ_state == -1) + free_space_indices.push_back(std::make_pair(i, j)); +#endif + // Create the particle filter + pf_ = pf_alloc(min_particles_, max_particles_, + alpha_slow_, alpha_fast_, + (pf_init_model_fn_t)amcl::Amcl::uniformPoseGenerator, + (void *)map_); + pf_set_selective_resampling(pf_, selective_resampling_); + pf_->pop_err = pf_err_; + pf_->pop_z = pf_z_; + + // Initialize the filter + updatePoseFromServer(); + pf_vector_t pf_init_pose_mean = pf_vector_zero(); + pf_init_pose_mean.v[0] = init_pose_[0]; + pf_init_pose_mean.v[1] = init_pose_[1]; + pf_init_pose_mean.v[2] = init_pose_[2]; + pf_matrix_t pf_init_pose_cov = pf_matrix_zero(); + pf_init_pose_cov.m[0][0] = init_cov_[0]; + pf_init_pose_cov.m[1][1] = init_cov_[1]; + pf_init_pose_cov.m[2][2] = init_cov_[2]; + pf_init(pf_, pf_init_pose_mean, pf_init_pose_cov); + pf_init_ = false; + + // Instantiate the sensor objects + // Odometry + delete odom_; + odom_ = new amcl::AMCLOdom(); + ROS_ASSERT(odom_); + odom_->SetModel(odom_model_type_, alpha1_, alpha2_, alpha3_, alpha4_, alpha5_); + // Laser + delete laser_; + laser_ = new amcl::AMCLLaser(max_beams_, map_); + ROS_ASSERT(laser_); + if (laser_model_type_ == amcl::LASER_MODEL_BEAM) + laser_->SetModelBeam(z_hit_, z_short_, z_max_, z_rand_, + sigma_hit_, lambda_short_, 0.0); + else if (laser_model_type_ == amcl::LASER_MODEL_LIKELIHOOD_FIELD_PROB) + { + ROS_INFO("Initializing likelihood field model; this can take some time on large maps..."); + laser_->SetModelLikelihoodFieldProb(z_hit_, z_rand_, sigma_hit_, + laser_likelihood_max_dist_, + do_beamskip_, beam_skip_distance_, + beam_skip_threshold_, beam_skip_error_threshold_); + ROS_INFO("Done initializing likelihood field model."); + } + else + { + ROS_INFO("Initializing likelihood field model; this can take some time on large maps..."); + laser_->SetModelLikelihoodField(z_hit_, z_rand_, sigma_hit_, + laser_likelihood_max_dist_); + ROS_INFO("Done initializing likelihood field model."); + } + + // In case the initial pose message arrived before the first map, + // try to apply the initial pose now that the map has arrived. + applyInitialPose(); +} + +void amcl::Amcl::freeMapDependentMemory() +{ + if (map_ != NULL) + { + map_free(map_); + map_ = NULL; + } + if (pf_ != NULL) + { + pf_free(pf_); + pf_ = NULL; + } + odom_ = NULL; + delete odom_; + laser_ = NULL; + delete laser_; +} + +/** + * Convert an OccupancyGrid map message into the internal + * representation. This allocates a map_t and returns it. + */ +map_t * +amcl::Amcl::convertMap(const nav_msgs::OccupancyGrid &map_msg) +{ + map_t *map = map_alloc(); + ROS_ASSERT(map); + + map->size_x = map_msg.info.width; + map->size_y = map_msg.info.height; + map->scale = map_msg.info.resolution; + map->origin_x = map_msg.info.origin.position.x + (map->size_x / 2) * map->scale; + map->origin_y = map_msg.info.origin.position.y + (map->size_y / 2) * map->scale; + // Convert to player format + map->cells = (map_cell_t *)malloc(sizeof(map_cell_t) * map->size_x * map->size_y); + ROS_ASSERT(map->cells); + for (int i = 0; i < map->size_x * map->size_y; i++) + { + if (map_msg.data[i] == 0) + map->cells[i].occ_state = -1; + else if (map_msg.data[i] == 100) + map->cells[i].occ_state = +1; + else + map->cells[i].occ_state = 0; + } + + return map; +} + +bool amcl::Amcl::getOdomPose(geometry_msgs::PoseStamped &odom_pose, + double &x, double &y, double &yaw, + const ros::Time &t, const std::string &f) +{ + // Get the robot's pose + geometry_msgs::PoseStamped ident; + ident.header.frame_id = stripSlash(f); + ident.header.stamp = t; + tf2::toMsg(tf2::Transform::getIdentity(), ident.pose); + try + { + this->tf_->transform(ident, odom_pose, odom_frame_id_); + } + catch (const tf2::TransformException &e) + { + ROS_WARN("Failed to compute odom pose, skipping scan (%s)", e.what()); + return false; + } + x = odom_pose.pose.position.x; + y = odom_pose.pose.position.y; + yaw = tf2::getYaw(odom_pose.pose.orientation); + + return true; +} + +pf_vector_t +amcl::Amcl::uniformPoseGenerator(void *arg) +{ + map_t *map = (map_t *)arg; +#if NEW_UNIFORM_SAMPLING + unsigned int rand_index = drand48() * free_space_indices.size(); + std::pair free_point = free_space_indices[rand_index]; + pf_vector_t p; + p.v[0] = MAP_WXGX(map, free_point.first); + p.v[1] = MAP_WYGY(map, free_point.second); + p.v[2] = drand48() * 2 * M_PI - M_PI; +#else + double min_x, max_x, min_y, max_y; + + min_x = (map->size_x * map->scale) / 2.0 - map->origin_x; + max_x = (map->size_x * map->scale) / 2.0 + map->origin_x; + min_y = (map->size_y * map->scale) / 2.0 - map->origin_y; + max_y = (map->size_y * map->scale) / 2.0 + map->origin_y; + + pf_vector_t p; + + ROS_DEBUG("Generating new uniform sample"); + for (;;) + { + p.v[0] = min_x + drand48() * (max_x - min_x); + p.v[1] = min_y + drand48() * (max_y - min_y); + p.v[2] = drand48() * 2 * M_PI - M_PI; + // Check that it's a free cell + int i, j; + i = MAP_GXWX(map, p.v[0]); + j = MAP_GYWY(map, p.v[1]); + if (MAP_VALID(map, i, j) && (map->cells[MAP_INDEX(map, i, j)].occ_state == -1)) + break; + } +#endif + return p; +} + +bool amcl::Amcl::globalLocalizationCallback(std_srvs::Empty::Request &req, + std_srvs::Empty::Response &res) +{ + if (map_ == NULL) + { + return true; + } + boost::recursive_mutex::scoped_lock gl(configuration_mutex_); + ROS_INFO("Initializing with uniform distribution"); + pf_init_model(pf_, (pf_init_model_fn_t)amcl::Amcl::uniformPoseGenerator, + (void *)map_); + ROS_INFO("Global initialisation done!"); + pf_init_ = false; + return true; +} + +// force nomotion updates (amcl updating without requiring motion) +bool amcl::Amcl::nomotionUpdateCallback(std_srvs::Empty::Request &req, + std_srvs::Empty::Response &res) +{ + m_force_update = true; + // ROS_INFO("Requesting no-motion update"); + return true; +} + +bool amcl::Amcl::setMapCallback(nav_msgs::SetMap::Request &req, + nav_msgs::SetMap::Response &res) +{ + handleMapMessage(req.map); + handleInitialPoseMessage(req.initial_pose); + if (force_update_after_set_map_) + { + m_force_update = true; + } + res.success = true; + return true; +} + +void amcl::Amcl::laserReceived(const sensor_msgs::LaserScanConstPtr &laser_scan) +{ + std::string laser_scan_frame_id = stripSlash(laser_scan->header.frame_id); + last_laser_received_ts_ = ros::Time::now(); + if (map_ == NULL) + { + return; + } + boost::recursive_mutex::scoped_lock lr(configuration_mutex_); + int laser_index = -1; + + // Do we have the base->base_laser Tx yet? + if (frame_to_laser_.find(laser_scan_frame_id) == frame_to_laser_.end()) + { + ROS_DEBUG("Setting up laser %d (frame_id=%s)\n", (int)frame_to_laser_.size(), laser_scan_frame_id.c_str()); + lasers_.push_back(new amcl::AMCLLaser(*laser_)); + lasers_update_.push_back(true); + laser_index = frame_to_laser_.size(); + + geometry_msgs::PoseStamped ident; + ident.header.frame_id = laser_scan_frame_id; + ident.header.stamp = ros::Time(); + tf2::toMsg(tf2::Transform::getIdentity(), ident.pose); + + geometry_msgs::PoseStamped laser_pose; + try + { + this->tf_->transform(ident, laser_pose, base_frame_id_); + } + catch (const tf2::TransformException &e) + { + ROS_ERROR("Couldn't transform from %s to %s, " + "even though the message notifier is in use", + laser_scan_frame_id.c_str(), + base_frame_id_.c_str()); + return; + } + + pf_vector_t laser_pose_v; + laser_pose_v.v[0] = laser_pose.pose.position.x; + laser_pose_v.v[1] = laser_pose.pose.position.y; + // laser mounting angle gets computed later -> set to 0 here! + laser_pose_v.v[2] = 0; + lasers_[laser_index]->SetLaserPose(laser_pose_v); + ROS_DEBUG("Received laser's pose wrt robot: %.3f %.3f %.3f", + laser_pose_v.v[0], + laser_pose_v.v[1], + laser_pose_v.v[2]); + + frame_to_laser_[laser_scan_frame_id] = laser_index; + } + else + { + // we have the laser pose, retrieve laser index + laser_index = frame_to_laser_[laser_scan_frame_id]; + } + + // Where was the robot when this scan was taken? + pf_vector_t pose; + if (!getOdomPose(latest_odom_pose_, pose.v[0], pose.v[1], pose.v[2], + laser_scan->header.stamp, base_frame_id_)) + { + ROS_ERROR("Couldn't determine robot's pose associated with laser scan"); + return; + } + + pf_vector_t delta = pf_vector_zero(); + + if (pf_init_) + { + // Compute change in pose + // delta = pf_vector_coord_sub(pose, pf_odom_pose_); + delta.v[0] = pose.v[0] - pf_odom_pose_.v[0]; + delta.v[1] = pose.v[1] - pf_odom_pose_.v[1]; + delta.v[2] = angle_diff(pose.v[2], pf_odom_pose_.v[2]); + + // See if we should update the filter + bool update = fabs(delta.v[0]) > d_thresh_ || + fabs(delta.v[1]) > d_thresh_ || + fabs(delta.v[2]) > a_thresh_; + update = update || m_force_update; + m_force_update = false; + + // Set the laser update flags + if (update) + for (unsigned int i = 0; i < lasers_update_.size(); i++) + lasers_update_[i] = true; + } + + bool force_publication = false; + if (!pf_init_) + { + // Pose at last filter update + pf_odom_pose_ = pose; + + // Filter is now initialized + pf_init_ = true; + + // Should update sensor data + for (unsigned int i = 0; i < lasers_update_.size(); i++) + lasers_update_[i] = true; + + force_publication = true; + + resample_count_ = 0; + } + // If the robot has moved, update the filter + else if (pf_init_ && lasers_update_[laser_index]) + { + // printf("pose\n"); + // pf_vector_fprintf(pose, stdout, "%.3f"); + + amcl::AMCLOdomData odata; + odata.pose = pose; + // HACK + // Modify the delta in the action data so the filter gets + // updated correctly + odata.delta = delta; + + // Use the action data to update the filter + odom_->UpdateAction(pf_, (amcl::AMCLSensorData *)&odata); + + // Pose at last filter update + // this->pf_odom_pose = pose; + } + + bool resampled = false; + // If the robot has moved, update the filter + if (lasers_update_[laser_index]) + { + amcl::AMCLLaserData ldata; + ldata.sensor = lasers_[laser_index]; + ldata.range_count = laser_scan->ranges.size(); + + // To account for lasers that are mounted upside-down, we determine the + // min, max, and increment angles of the laser in the base frame. + // + // Construct min and max angles of laser, in the base_link frame. + tf2::Quaternion q; + q.setRPY(0.0, 0.0, laser_scan->angle_min); + geometry_msgs::QuaternionStamped min_q, inc_q; + min_q.header.stamp = laser_scan->header.stamp; + min_q.header.frame_id = stripSlash(laser_scan->header.frame_id); + tf2::convert(q, min_q.quaternion); + + q.setRPY(0.0, 0.0, laser_scan->angle_min + laser_scan->angle_increment); + inc_q.header = min_q.header; + tf2::convert(q, inc_q.quaternion); + try + { + tf_->transform(min_q, min_q, base_frame_id_); + tf_->transform(inc_q, inc_q, base_frame_id_); + } + catch (const tf2::TransformException &e) + { + ROS_WARN("Unable to transform min/max laser angles into base frame: %s", + e.what()); + return; + } + + double angle_min = tf2::getYaw(min_q.quaternion); + double angle_increment = tf2::getYaw(inc_q.quaternion) - angle_min; + + // wrapping angle to [-pi .. pi] + angle_increment = fmod(angle_increment + 5 * M_PI, 2 * M_PI) - M_PI; + + ROS_DEBUG("Laser %d angles in base frame: min: %.3f inc: %.3f", laser_index, angle_min, angle_increment); + + // Apply range min/max thresholds, if the user supplied them + if (laser_max_range_ > 0.0) + ldata.range_max = std::min(laser_scan->range_max, (float)laser_max_range_); + else + ldata.range_max = laser_scan->range_max; + double range_min; + if (laser_min_range_ > 0.0) + range_min = std::max(laser_scan->range_min, (float)laser_min_range_); + else + range_min = laser_scan->range_min; + + if (ldata.range_max <= 0.0 || range_min < 0.0) + { + ROS_ERROR("range_max or range_min from laser is negative! ignore this message."); + return; // ignore this. + } + + // The AMCLLaserData destructor will free this memory + ldata.ranges = new double[ldata.range_count][2]; + ROS_ASSERT(ldata.ranges); + for (int i = 0; i < ldata.range_count; i++) + { + // amcl doesn't (yet) have a concept of min range. So we'll map short + // readings to max range. + if (laser_scan->ranges[i] <= range_min) + ldata.ranges[i][0] = ldata.range_max; + else if (laser_scan->ranges[i] > ldata.range_max) + ldata.ranges[i][0] = std::numeric_limits::max(); + else + ldata.ranges[i][0] = laser_scan->ranges[i]; + // Compute bearing + ldata.ranges[i][1] = angle_min + + (i * angle_increment); + } + + lasers_[laser_index]->UpdateSensor(pf_, (amcl::AMCLSensorData *)&ldata); + + lasers_update_[laser_index] = false; + + pf_odom_pose_ = pose; + + // Resample the particles + if (!(++resample_count_ % resample_interval_)) + { + pf_update_resample(pf_); + resampled = true; + } + + pf_sample_set_t *set = pf_->sets + pf_->current_set; + ROS_DEBUG("Num samples: %d\n", set->sample_count); + + // Publish the resulting cloud + // TODO: set maximum rate for publishing + if (!m_force_update) + { + geometry_msgs::PoseArray cloud_msg; + cloud_msg.header.stamp = ros::Time::now(); + cloud_msg.header.frame_id = global_frame_id_; + cloud_msg.poses.resize(set->sample_count); + for (int i = 0; i < set->sample_count; i++) + { + cloud_msg.poses[i].position.x = set->samples[i].pose.v[0]; + cloud_msg.poses[i].position.y = set->samples[i].pose.v[1]; + cloud_msg.poses[i].position.z = 0; + tf2::Quaternion q; + q.setRPY(0, 0, set->samples[i].pose.v[2]); + tf2::convert(q, cloud_msg.poses[i].orientation); + } + particlecloud_pub_.publish(cloud_msg); + } + } + + if (resampled || force_publication) + { + // Read out the current hypotheses + double max_weight = 0.0; + int max_weight_hyp = -1; + std::vector hyps; + hyps.resize(pf_->sets[pf_->current_set].cluster_count); + for (int hyp_count = 0; + hyp_count < pf_->sets[pf_->current_set].cluster_count; hyp_count++) + { + double weight; + pf_vector_t pose_mean; + pf_matrix_t pose_cov; + if (!pf_get_cluster_stats(pf_, hyp_count, &weight, &pose_mean, &pose_cov)) + { + ROS_ERROR("Couldn't get stats on cluster %d", hyp_count); + break; + } + + hyps[hyp_count].weight = weight; + hyps[hyp_count].pf_pose_mean = pose_mean; + hyps[hyp_count].pf_pose_cov = pose_cov; + + if (hyps[hyp_count].weight > max_weight) + { + max_weight = hyps[hyp_count].weight; + max_weight_hyp = hyp_count; + } + } + + if (max_weight > 0.0) + { + ROS_DEBUG("Max weight pose: %.3f %.3f %.3f", + hyps[max_weight_hyp].pf_pose_mean.v[0], + hyps[max_weight_hyp].pf_pose_mean.v[1], + hyps[max_weight_hyp].pf_pose_mean.v[2]); + + /* + puts(""); + pf_matrix_fprintf(hyps[max_weight_hyp].pf_pose_cov, stdout, "%6.3f"); + puts(""); + */ + + geometry_msgs::PoseWithCovarianceStamped p; + // Fill in the header + p.header.frame_id = global_frame_id_; + p.header.stamp = laser_scan->header.stamp; + // Copy in the pose + p.pose.pose.position.x = hyps[max_weight_hyp].pf_pose_mean.v[0]; + p.pose.pose.position.y = hyps[max_weight_hyp].pf_pose_mean.v[1]; + + tf2::Quaternion q; + q.setRPY(0, 0, hyps[max_weight_hyp].pf_pose_mean.v[2]); + tf2::convert(q, p.pose.pose.orientation); + // Copy in the covariance, converting from 3-D to 6-D + pf_sample_set_t *set = pf_->sets + pf_->current_set; + for (int i = 0; i < 2; i++) + { + for (int j = 0; j < 2; j++) + { + // Report the overall filter covariance, rather than the + // covariance for the highest-weight cluster + // p.covariance[6*i+j] = hyps[max_weight_hyp].pf_pose_cov.m[i][j]; + p.pose.covariance[6 * i + j] = set->cov.m[i][j]; + } + } + // Report the overall filter covariance, rather than the + // covariance for the highest-weight cluster + // p.covariance[6*5+5] = hyps[max_weight_hyp].pf_pose_cov.m[2][2]; + p.pose.covariance[6 * 5 + 5] = set->cov.m[2][2]; + + /* + printf("cov:\n"); + for(int i=0; i<6; i++) + { + for(int j=0; j<6; j++) + printf("%6.3f ", p.covariance[6*i+j]); + puts(""); + } + */ + + pose_pub_.publish(p); + last_published_pose = p; + + ROS_DEBUG("New pose: %6.3f %6.3f %6.3f", + hyps[max_weight_hyp].pf_pose_mean.v[0], + hyps[max_weight_hyp].pf_pose_mean.v[1], + hyps[max_weight_hyp].pf_pose_mean.v[2]); + + // subtracting base to odom from map to base and send map to odom instead + geometry_msgs::PoseStamped odom_to_map; + try + { + tf2::Quaternion q; + q.setRPY(0, 0, hyps[max_weight_hyp].pf_pose_mean.v[2]); + tf2::Transform tmp_tf(q, tf2::Vector3(hyps[max_weight_hyp].pf_pose_mean.v[0], + hyps[max_weight_hyp].pf_pose_mean.v[1], + 0.0)); + + geometry_msgs::PoseStamped tmp_tf_stamped; + tmp_tf_stamped.header.frame_id = base_frame_id_; + tmp_tf_stamped.header.stamp = laser_scan->header.stamp; + tf2::toMsg(tmp_tf.inverse(), tmp_tf_stamped.pose); + + this->tf_->transform(tmp_tf_stamped, odom_to_map, odom_frame_id_); + } + catch (const tf2::TransformException &) + { + ROS_DEBUG("Failed to subtract base to odom transform"); + return; + } + + tf2::convert(odom_to_map.pose, latest_tf_); + latest_tf_valid_ = true; + + if (tf_broadcast_ == true) + { + // We want to send a transform that is good up until a + // tolerance time so that odom can be used + ros::Time transform_expiration = (laser_scan->header.stamp + + transform_tolerance_); + geometry_msgs::TransformStamped tmp_tf_stamped; + tmp_tf_stamped.header.frame_id = global_frame_id_; + tmp_tf_stamped.header.stamp = transform_expiration; + tmp_tf_stamped.child_frame_id = odom_frame_id_; + tf2::convert(latest_tf_.inverse(), tmp_tf_stamped.transform); + if (transform_expiration - last_tfb_time_ > ros::Duration(0.02)) + { + // ROS_INFO("%s -> %s %.6f %.6f", global_frame_id_.c_str(), odom_frame_id_.c_str(), + // tmp_tf_stamped.header.stamp.toSec(), last_tfb_time_.toSec()); + this->tfb_->sendTransform(tmp_tf_stamped); + sent_first_transform_ = true; + last_tfb_time_ = transform_expiration; + } + } + } + else + { + ROS_ERROR("No pose!"); + } + } + else if (latest_tf_valid_) + { + if (tf_broadcast_ == true) + { + // Nothing changed, so we'll just republish the last transform, to keep + // everybody happy. + ros::Time transform_expiration = (laser_scan->header.stamp + + transform_tolerance_); + geometry_msgs::TransformStamped tmp_tf_stamped; + tmp_tf_stamped.header.frame_id = global_frame_id_; + tmp_tf_stamped.header.stamp = transform_expiration; + tmp_tf_stamped.child_frame_id = odom_frame_id_; + tf2::convert(latest_tf_.inverse(), tmp_tf_stamped.transform); + if (transform_expiration - last_tfb_time_ > ros::Duration(0.02)) + { + // ROS_INFO("%s -> %s %.6f %.6f", global_frame_id_.c_str(), odom_frame_id_.c_str(), + // tmp_tf_stamped.header.stamp.toSec(), last_tfb_time_.toSec()); + this->tfb_->sendTransform(tmp_tf_stamped); + last_tfb_time_ = transform_expiration; + } + } + + // Is it time to save our last pose to the param server + ros::Time now = ros::Time::now(); + if ((save_pose_period.toSec() > 0.0) && + (now - save_pose_last_time) >= save_pose_period) + { + geometry_msgs::PoseWithCovarianceStamped amcl_pose; + this->savePoseToServer(amcl_pose); + save_pose_last_time = now; + } + } + + diagnosic_updater_.update(); +} + +void amcl::Amcl::initialPoseReceived(const geometry_msgs::PoseWithCovarianceStampedConstPtr &msg) +{ + handleInitialPoseMessage(*msg); + if (force_update_after_initialpose_) + { + m_force_update = true; + } +} + +void amcl::Amcl::handleInitialPoseMessage(const geometry_msgs::PoseWithCovarianceStamped &msg) +{ + boost::recursive_mutex::scoped_lock prl(configuration_mutex_); + if (msg.header.frame_id == "") + { + // This should be removed at some point + ROS_WARN("Received initial pose with empty frame_id. You should always supply a frame_id."); + } + // We only accept initial pose estimates in the global frame, #5148. + else if (stripSlash(msg.header.frame_id) != global_frame_id_) + { + ROS_WARN("Ignoring initial pose in frame \"%s\"; initial poses must be in the global frame, \"%s\"", + stripSlash(msg.header.frame_id).c_str(), + global_frame_id_.c_str()); + return; + } + + // In case the client sent us a pose estimate in the past, integrate the + // intervening odometric change. + geometry_msgs::TransformStamped tx_odom; + try + { + // wait a little for the latest tf to become available + tx_odom = tf_->lookupTransform(base_frame_id_, msg.header.stamp, + base_frame_id_, ros::Time::now(), + odom_frame_id_, ros::Duration(0.5)); + } + catch (const tf2::TransformException &e) + { + // If we've never sent a transform, then this is normal, because the + // global_frame_id_ frame doesn't exist. We only care about in-time + // transformation for on-the-move pose-setting, so ignoring this + // startup condition doesn't really cost us anything. + if (sent_first_transform_) + ROS_WARN("Failed to transform initial pose in time (%s)", e.what()); + tf2::convert(tf2::Transform::getIdentity(), tx_odom.transform); + } + + tf2::Transform tx_odom_tf2; + tf2::convert(tx_odom.transform, tx_odom_tf2); + tf2::Transform pose_old, pose_new; + tf2::convert(msg.pose.pose, pose_old); + pose_new = pose_old * tx_odom_tf2; + + // Transform into the global frame + + ROS_INFO("Setting pose (%.6f): %.3f %.3f %.3f", + ros::Time::now().toSec(), + pose_new.getOrigin().x(), + pose_new.getOrigin().y(), + tf2::getYaw(pose_new.getRotation())); + // Re-initialize the filter + pf_vector_t pf_init_pose_mean = pf_vector_zero(); + pf_init_pose_mean.v[0] = pose_new.getOrigin().x(); + pf_init_pose_mean.v[1] = pose_new.getOrigin().y(); + pf_init_pose_mean.v[2] = tf2::getYaw(pose_new.getRotation()); + pf_matrix_t pf_init_pose_cov = pf_matrix_zero(); + // Copy in the covariance, converting from 6-D to 3-D + for (int i = 0; i < 2; i++) + { + for (int j = 0; j < 2; j++) + { + pf_init_pose_cov.m[i][j] = msg.pose.covariance[6 * i + j]; + } + } + pf_init_pose_cov.m[2][2] = msg.pose.covariance[6 * 5 + 5]; + + delete initial_pose_hyp_; + initial_pose_hyp_ = new amcl_hyp_t(); + initial_pose_hyp_->pf_pose_mean = pf_init_pose_mean; + initial_pose_hyp_->pf_pose_cov = pf_init_pose_cov; + applyInitialPose(); +} + +/** + * If initial_pose_hyp_ and map_ are both non-null, apply the initial + * pose to the particle filter state. initial_pose_hyp_ is deleted + * and set to NULL after it is used. + */ +void amcl::Amcl::applyInitialPose() +{ + boost::recursive_mutex::scoped_lock cfl(configuration_mutex_); + if (initial_pose_hyp_ != NULL && map_ != NULL) + { + pf_init(pf_, initial_pose_hyp_->pf_pose_mean, initial_pose_hyp_->pf_pose_cov); + pf_init_ = false; + + delete initial_pose_hyp_; + initial_pose_hyp_ = NULL; + } +} + +void amcl::Amcl::standardDeviationDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &diagnostic_status) +{ + double std_x = sqrt(last_published_pose.pose.covariance[6 * 0 + 0]); + double std_y = sqrt(last_published_pose.pose.covariance[6 * 1 + 1]); + double std_yaw = sqrt(last_published_pose.pose.covariance[6 * 5 + 5]); + + diagnostic_status.add("std_x", std_x); + diagnostic_status.add("std_y", std_y); + diagnostic_status.add("std_yaw", std_yaw); + diagnostic_status.add("std_warn_level_x", std_warn_level_x_); + diagnostic_status.add("std_warn_level_y", std_warn_level_y_); + diagnostic_status.add("std_warn_level_yaw", std_warn_level_yaw_); + + if (std_x > std_warn_level_x_ || std_y > std_warn_level_y_ || std_yaw > std_warn_level_yaw_) + { + diagnostic_status.summary(diagnostic_msgs::DiagnosticStatus::WARN, "Too large"); + } + else + { + diagnostic_status.summary(diagnostic_msgs::DiagnosticStatus::OK, "OK"); + } +} diff --git a/Localizations/Libraries/Ros/amcl/src/amcl/map/map.c b/Localizations/Libraries/Ros/amcl/src/amcl/map/map.c new file mode 100644 index 0000000..23c9662 --- /dev/null +++ b/Localizations/Libraries/Ros/amcl/src/amcl/map/map.c @@ -0,0 +1,84 @@ +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2000 Brian Gerkey & Kasper Stoy + * gerkey@usc.edu kaspers@robotics.usc.edu + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ +/************************************************************************** + * Desc: Global map (grid-based) + * Author: Andrew Howard + * Date: 6 Feb 2003 + * CVS: $Id: map.c 1713 2003-08-23 04:03:43Z inspectorg $ +**************************************************************************/ + +#include +#include +#include +#include +#include + +#include "amcl/map/map.h" + + +// Create a new map +map_t *map_alloc(void) +{ + map_t *map; + + map = (map_t*) malloc(sizeof(map_t)); + + // Assume we start at (0, 0) + map->origin_x = 0; + map->origin_y = 0; + + // Make the size odd + map->size_x = 0; + map->size_y = 0; + map->scale = 0; + + // Allocate storage for main map + map->cells = (map_cell_t*) NULL; + + return map; +} + + +// Destroy a map +void map_free(map_t *map) +{ + free(map->cells); + free(map); + return; +} + + +// Get the cell at the given point +map_cell_t *map_get_cell(map_t *map, double ox, double oy, double oa) +{ + int i, j; + map_cell_t *cell; + + i = MAP_GXWX(map, ox); + j = MAP_GYWY(map, oy); + + if (!MAP_VALID(map, i, j)) + return NULL; + + cell = map->cells + MAP_INDEX(map, i, j); + return cell; +} + diff --git a/Localizations/Libraries/Ros/amcl/src/amcl/map/map_cspace.cpp b/Localizations/Libraries/Ros/amcl/src/amcl/map/map_cspace.cpp new file mode 100644 index 0000000..94f93a9 --- /dev/null +++ b/Localizations/Libraries/Ros/amcl/src/amcl/map/map_cspace.cpp @@ -0,0 +1,177 @@ +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2000 Brian Gerkey & Kasper Stoy + * gerkey@usc.edu kaspers@robotics.usc.edu + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ + +#include +#include +#include +#include +#include "amcl/map/map.h" +#include + +class CellData +{ + public: + map_t* map_; + unsigned int i_, j_; + unsigned int src_i_, src_j_; +}; + +class CachedDistanceMap +{ + public: + CachedDistanceMap(double scale, double max_dist) : + distances_(NULL), scale_(scale), max_dist_(max_dist) + { + cell_radius_ = max_dist / scale; + distances_ = new double *[cell_radius_+2]; + for(int i=0; i<=cell_radius_+1; i++) + { + distances_[i] = new double[cell_radius_+2]; + for(int j=0; j<=cell_radius_+1; j++) + { + distances_[i][j] = sqrt(i*i + j*j); + } + } + } + ~CachedDistanceMap() + { + if(distances_) + { + for(int i=0; i<=cell_radius_+1; i++) + delete[] distances_[i]; + delete[] distances_; + } + } + double** distances_; + double scale_; + double max_dist_; + int cell_radius_; +}; + + +bool operator<(const CellData& a, const CellData& b) +{ + return a.map_->cells[MAP_INDEX(a.map_, a.i_, a.j_)].occ_dist > a.map_->cells[MAP_INDEX(b.map_, b.i_, b.j_)].occ_dist; +} + +CachedDistanceMap* +get_distance_map(double scale, double max_dist) +{ + static CachedDistanceMap* cdm = NULL; + + if(!cdm || (cdm->scale_ != scale) || (cdm->max_dist_ != max_dist)) + { + if(cdm) + delete cdm; + cdm = new CachedDistanceMap(scale, max_dist); + } + + return cdm; +} + +void enqueue(map_t* map, int i, int j, + int src_i, int src_j, + std::priority_queue& Q, + CachedDistanceMap* cdm, + unsigned char* marked) +{ + if(marked[MAP_INDEX(map, i, j)]) + return; + + int di = abs(i - src_i); + int dj = abs(j - src_j); + double distance = cdm->distances_[di][dj]; + + if(distance > cdm->cell_radius_) + return; + + map->cells[MAP_INDEX(map, i, j)].occ_dist = distance * map->scale; + + CellData cell; + cell.map_ = map; + cell.i_ = i; + cell.j_ = j; + cell.src_i_ = src_i; + cell.src_j_ = src_j; + + Q.push(cell); + + marked[MAP_INDEX(map, i, j)] = 1; +} + +// Update the cspace distance values +void map_update_cspace(map_t *map, double max_occ_dist) +{ + unsigned char* marked; + std::priority_queue Q; + + marked = new unsigned char[map->size_x*map->size_y]; + memset(marked, 0, sizeof(unsigned char) * map->size_x*map->size_y); + + map->max_occ_dist = max_occ_dist; + + CachedDistanceMap* cdm = get_distance_map(map->scale, map->max_occ_dist); + + // Enqueue all the obstacle cells + CellData cell; + cell.map_ = map; + for(int i=0; isize_x; i++) + { + cell.src_i_ = cell.i_ = i; + for(int j=0; jsize_y; j++) + { + if(map->cells[MAP_INDEX(map, i, j)].occ_state == +1) + { + map->cells[MAP_INDEX(map, i, j)].occ_dist = 0.0; + cell.src_j_ = cell.j_ = j; + marked[MAP_INDEX(map, i, j)] = 1; + Q.push(cell); + } + else + map->cells[MAP_INDEX(map, i, j)].occ_dist = max_occ_dist; + } + } + + while(!Q.empty()) + { + CellData current_cell = Q.top(); + if(current_cell.i_ > 0) + enqueue(map, current_cell.i_-1, current_cell.j_, + current_cell.src_i_, current_cell.src_j_, + Q, cdm, marked); + if(current_cell.j_ > 0) + enqueue(map, current_cell.i_, current_cell.j_-1, + current_cell.src_i_, current_cell.src_j_, + Q, cdm, marked); + if((int)current_cell.i_ < map->size_x - 1) + enqueue(map, current_cell.i_+1, current_cell.j_, + current_cell.src_i_, current_cell.src_j_, + Q, cdm, marked); + if((int)current_cell.j_ < map->size_y - 1) + enqueue(map, current_cell.i_, current_cell.j_+1, + current_cell.src_i_, current_cell.src_j_, + Q, cdm, marked); + + Q.pop(); + } + + delete[] marked; +} diff --git a/Localizations/Libraries/Ros/amcl/src/amcl/map/map_draw.c b/Localizations/Libraries/Ros/amcl/src/amcl/map/map_draw.c new file mode 100644 index 0000000..b1201fd --- /dev/null +++ b/Localizations/Libraries/Ros/amcl/src/amcl/map/map_draw.c @@ -0,0 +1,158 @@ +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2000 Brian Gerkey & Kasper Stoy + * gerkey@usc.edu kaspers@robotics.usc.edu + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ +/************************************************************************** + * Desc: Local map GUI functions + * Author: Andrew Howard + * Date: 18 Jan 2003 + * CVS: $Id: map_draw.c 7057 2008-10-02 00:44:06Z gbiggs $ +**************************************************************************/ + +#ifdef INCLUDE_RTKGUI + +#include +#include +#include +#include + +#include +#include "amcl/map/map.h" + + +//////////////////////////////////////////////////////////////////////////// +// Draw the occupancy map +void map_draw_occ(map_t *map, rtk_fig_t *fig) +{ + int i, j; + int col; + map_cell_t *cell; + uint16_t *image; + uint16_t *pixel; + + image = malloc(map->size_x * map->size_y * sizeof(image[0])); + + // Draw occupancy + for (j = 0; j < map->size_y; j++) + { + for (i = 0; i < map->size_x; i++) + { + cell = map->cells + MAP_INDEX(map, i, j); + pixel = image + (j * map->size_x + i); + + col = 127 - 127 * cell->occ_state; + *pixel = RTK_RGB16(col, col, col); + } + } + + // Draw the entire occupancy map as an image + rtk_fig_image(fig, map->origin_x, map->origin_y, 0, + map->scale, map->size_x, map->size_y, 16, image, NULL); + + free(image); + + return; +} + + +//////////////////////////////////////////////////////////////////////////// +// Draw the cspace map +void map_draw_cspace(map_t *map, rtk_fig_t *fig) +{ + int i, j; + int col; + map_cell_t *cell; + uint16_t *image; + uint16_t *pixel; + + image = malloc(map->size_x * map->size_y * sizeof(image[0])); + + // Draw occupancy + for (j = 0; j < map->size_y; j++) + { + for (i = 0; i < map->size_x; i++) + { + cell = map->cells + MAP_INDEX(map, i, j); + pixel = image + (j * map->size_x + i); + + col = 255 * cell->occ_dist / map->max_occ_dist; + + *pixel = RTK_RGB16(col, col, col); + } + } + + // Draw the entire occupancy map as an image + rtk_fig_image(fig, map->origin_x, map->origin_y, 0, + map->scale, map->size_x, map->size_y, 16, image, NULL); + + free(image); + + return; +} + + +//////////////////////////////////////////////////////////////////////////// +// Draw a wifi map +void map_draw_wifi(map_t *map, rtk_fig_t *fig, int index) +{ + int i, j; + int level, col; + map_cell_t *cell; + uint16_t *image, *mask; + uint16_t *ipix, *mpix; + + image = malloc(map->size_x * map->size_y * sizeof(image[0])); + mask = malloc(map->size_x * map->size_y * sizeof(mask[0])); + + // Draw wifi levels + for (j = 0; j < map->size_y; j++) + { + for (i = 0; i < map->size_x; i++) + { + cell = map->cells + MAP_INDEX(map, i, j); + ipix = image + (j * map->size_x + i); + mpix = mask + (j * map->size_x + i); + + level = cell->wifi_levels[index]; + + if (cell->occ_state == -1 && level != 0) + { + col = 255 * (100 + level) / 100; + *ipix = RTK_RGB16(col, col, col); + *mpix = 1; + } + else + { + *mpix = 0; + } + } + } + + // Draw the entire occupancy map as an image + rtk_fig_image(fig, map->origin_x, map->origin_y, 0, + map->scale, map->size_x, map->size_y, 16, image, mask); + + free(mask); + free(image); + + return; +} + + +#endif diff --git a/Localizations/Libraries/Ros/amcl/src/amcl/map/map_range.c b/Localizations/Libraries/Ros/amcl/src/amcl/map/map_range.c new file mode 100644 index 0000000..658619f --- /dev/null +++ b/Localizations/Libraries/Ros/amcl/src/amcl/map/map_range.c @@ -0,0 +1,120 @@ +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2000 Brian Gerkey & Kasper Stoy + * gerkey@usc.edu kaspers@robotics.usc.edu + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ +/************************************************************************** + * Desc: Range routines + * Author: Andrew Howard + * Date: 18 Jan 2003 + * CVS: $Id: map_range.c 1347 2003-05-05 06:24:33Z inspectorg $ +**************************************************************************/ + +#include +#include +#include +#include + +#include "amcl/map/map.h" + +// Extract a single range reading from the map. Unknown cells and/or +// out-of-bound cells are treated as occupied, which makes it easy to +// use Stage bitmap files. +double map_calc_range(map_t *map, double ox, double oy, double oa, double max_range) +{ + // Bresenham raytracing + int x0,x1,y0,y1; + int x,y; + int xstep, ystep; + char steep; + int tmp; + int deltax, deltay, error, deltaerr; + + x0 = MAP_GXWX(map,ox); + y0 = MAP_GYWY(map,oy); + + x1 = MAP_GXWX(map,ox + max_range * cos(oa)); + y1 = MAP_GYWY(map,oy + max_range * sin(oa)); + + if(abs(y1-y0) > abs(x1-x0)) + steep = 1; + else + steep = 0; + + if(steep) + { + tmp = x0; + x0 = y0; + y0 = tmp; + + tmp = x1; + x1 = y1; + y1 = tmp; + } + + deltax = abs(x1-x0); + deltay = abs(y1-y0); + error = 0; + deltaerr = deltay; + + x = x0; + y = y0; + + if(x0 < x1) + xstep = 1; + else + xstep = -1; + if(y0 < y1) + ystep = 1; + else + ystep = -1; + + if(steep) + { + if(!MAP_VALID(map,y,x) || map->cells[MAP_INDEX(map,y,x)].occ_state > -1) + return sqrt((x-x0)*(x-x0) + (y-y0)*(y-y0)) * map->scale; + } + else + { + if(!MAP_VALID(map,x,y) || map->cells[MAP_INDEX(map,x,y)].occ_state > -1) + return sqrt((x-x0)*(x-x0) + (y-y0)*(y-y0)) * map->scale; + } + + while(x != (x1 + xstep * 1)) + { + x += xstep; + error += deltaerr; + if(2*error >= deltax) + { + y += ystep; + error -= deltax; + } + + if(steep) + { + if(!MAP_VALID(map,y,x) || map->cells[MAP_INDEX(map,y,x)].occ_state > -1) + return sqrt((x-x0)*(x-x0) + (y-y0)*(y-y0)) * map->scale; + } + else + { + if(!MAP_VALID(map,x,y) || map->cells[MAP_INDEX(map,x,y)].occ_state > -1) + return sqrt((x-x0)*(x-x0) + (y-y0)*(y-y0)) * map->scale; + } + } + return max_range; +} diff --git a/Localizations/Libraries/Ros/amcl/src/amcl/map/map_store.c b/Localizations/Libraries/Ros/amcl/src/amcl/map/map_store.c new file mode 100644 index 0000000..b6d6ddb --- /dev/null +++ b/Localizations/Libraries/Ros/amcl/src/amcl/map/map_store.c @@ -0,0 +1,215 @@ +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2000 Brian Gerkey & Kasper Stoy + * gerkey@usc.edu kaspers@robotics.usc.edu + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ +/************************************************************************** + * Desc: Global map storage functions + * Author: Andrew Howard + * Date: 6 Feb 2003 + * CVS: $Id: map_store.c 2951 2005-08-19 00:48:20Z gerkey $ +**************************************************************************/ + +#include +#include +#include +#include +#include + +#include "amcl/map/map.h" + + +//////////////////////////////////////////////////////////////////////////// +// Load an occupancy grid +int map_load_occ(map_t *map, const char *filename, double scale, int negate) +{ + FILE *file; + char magic[3]; + int i, j; + int ch, occ; + int width, height, depth; + map_cell_t *cell; + + // Open file + file = fopen(filename, "r"); + if (file == NULL) + { + fprintf(stderr, "%s: %s\n", strerror(errno), filename); + return -1; + } + + // Read ppm header + + if ((fscanf(file, "%2s \n", magic) != 1) || (strcmp(magic, "P5") != 0)) + { + fprintf(stderr, "incorrect image format; must be PGM/binary"); + fclose(file); + return -1; + } + + // Ignore comments + while ((ch = fgetc(file)) == '#') + while (fgetc(file) != '\n'); + ungetc(ch, file); + + // Read image dimensions + if(fscanf(file, " %d %d \n %d \n", &width, &height, &depth) != 3) + { + fprintf(stderr, "Failed ot read image dimensions"); + return -1; + } + + // Allocate space in the map + if (map->cells == NULL) + { + map->scale = scale; + map->size_x = width; + map->size_y = height; + map->cells = calloc(width * height, sizeof(map->cells[0])); + } + else + { + if (width != map->size_x || height != map->size_y) + { + //PLAYER_ERROR("map dimensions are inconsistent with prior map dimensions"); + return -1; + } + } + + // Read in the image + for (j = height - 1; j >= 0; j--) + { + for (i = 0; i < width; i++) + { + ch = fgetc(file); + + // Black-on-white images + if (!negate) + { + if (ch < depth / 4) + occ = +1; + else if (ch > 3 * depth / 4) + occ = -1; + else + occ = 0; + } + + // White-on-black images + else + { + if (ch < depth / 4) + occ = -1; + else if (ch > 3 * depth / 4) + occ = +1; + else + occ = 0; + } + + if (!MAP_VALID(map, i, j)) + continue; + cell = map->cells + MAP_INDEX(map, i, j); + cell->occ_state = occ; + } + } + + fclose(file); + + return 0; +} + + +//////////////////////////////////////////////////////////////////////////// +// Load a wifi signal strength map +/* +int map_load_wifi(map_t *map, const char *filename, int index) +{ + FILE *file; + char magic[3]; + int i, j; + int ch, level; + int width, height, depth; + map_cell_t *cell; + + // Open file + file = fopen(filename, "r"); + if (file == NULL) + { + fprintf(stderr, "%s: %s\n", strerror(errno), filename); + return -1; + } + + // Read ppm header + fscanf(file, "%10s \n", magic); + if (strcmp(magic, "P5") != 0) + { + fprintf(stderr, "incorrect image format; must be PGM/binary"); + return -1; + } + + // Ignore comments + while ((ch = fgetc(file)) == '#') + while (fgetc(file) != '\n'); + ungetc(ch, file); + + // Read image dimensions + fscanf(file, " %d %d \n %d \n", &width, &height, &depth); + + // Allocate space in the map + if (map->cells == NULL) + { + map->size_x = width; + map->size_y = height; + map->cells = calloc(width * height, sizeof(map->cells[0])); + } + else + { + if (width != map->size_x || height != map->size_y) + { + //PLAYER_ERROR("map dimensions are inconsistent with prior map dimensions"); + return -1; + } + } + + // Read in the image + for (j = height - 1; j >= 0; j--) + { + for (i = 0; i < width; i++) + { + ch = fgetc(file); + + if (!MAP_VALID(map, i, j)) + continue; + + if (ch == 0) + level = 0; + else + level = ch * 100 / 255 - 100; + + cell = map->cells + MAP_INDEX(map, i, j); + cell->wifi_levels[index] = level; + } + } + + fclose(file); + + return 0; +} +*/ + + + diff --git a/Localizations/Libraries/Ros/amcl/src/amcl/pf/eig3.c b/Localizations/Libraries/Ros/amcl/src/amcl/pf/eig3.c new file mode 100644 index 0000000..3e8aee6 --- /dev/null +++ b/Localizations/Libraries/Ros/amcl/src/amcl/pf/eig3.c @@ -0,0 +1,274 @@ + +/* Eigen decomposition code for symmetric 3x3 matrices, copied from the public + domain Java Matrix library JAMA. */ + +#include + +#ifndef MAX +#define MAX(a, b) ((a)>(b)?(a):(b)) +#endif + +#ifdef _MSC_VER +#define n 3 +#else +static int n = 3; +#endif + +static double hypot2(double x, double y) { + return sqrt(x*x+y*y); +} + +// Symmetric Householder reduction to tridiagonal form. + +static void tred2(double V[n][n], double d[n], double e[n]) { + +// This is derived from the Algol procedures tred2 by +// Bowdler, Martin, Reinsch, and Wilkinson, Handbook for +// Auto. Comp., Vol.ii-Linear Algebra, and the corresponding +// Fortran subroutine in EISPACK. + + int i,j,k; + double f,g,h,hh; + for (j = 0; j < n; j++) { + d[j] = V[n-1][j]; + } + + // Householder reduction to tridiagonal form. + + for (i = n-1; i > 0; i--) { + + // Scale to avoid under/overflow. + + double scale = 0.0; + double h = 0.0; + for (k = 0; k < i; k++) { + scale = scale + fabs(d[k]); + } + if (scale == 0.0) { + e[i] = d[i-1]; + for (j = 0; j < i; j++) { + d[j] = V[i-1][j]; + V[i][j] = 0.0; + V[j][i] = 0.0; + } + } else { + + // Generate Householder vector. + + for (k = 0; k < i; k++) { + d[k] /= scale; + h += d[k] * d[k]; + } + f = d[i-1]; + g = sqrt(h); + if (f > 0) { + g = -g; + } + e[i] = scale * g; + h = h - f * g; + d[i-1] = f - g; + for (j = 0; j < i; j++) { + e[j] = 0.0; + } + + // Apply similarity transformation to remaining columns. + + for (j = 0; j < i; j++) { + f = d[j]; + V[j][i] = f; + g = e[j] + V[j][j] * f; + for (k = j+1; k <= i-1; k++) { + g += V[k][j] * d[k]; + e[k] += V[k][j] * f; + } + e[j] = g; + } + f = 0.0; + for (j = 0; j < i; j++) { + e[j] /= h; + f += e[j] * d[j]; + } + hh = f / (h + h); + for (j = 0; j < i; j++) { + e[j] -= hh * d[j]; + } + for (j = 0; j < i; j++) { + f = d[j]; + g = e[j]; + for (k = j; k <= i-1; k++) { + V[k][j] -= (f * e[k] + g * d[k]); + } + d[j] = V[i-1][j]; + V[i][j] = 0.0; + } + } + d[i] = h; + } + + // Accumulate transformations. + + for (i = 0; i < n-1; i++) { + V[n-1][i] = V[i][i]; + V[i][i] = 1.0; + h = d[i+1]; + if (h != 0.0) { + for (k = 0; k <= i; k++) { + d[k] = V[k][i+1] / h; + } + for (j = 0; j <= i; j++) { + g = 0.0; + for (k = 0; k <= i; k++) { + g += V[k][i+1] * V[k][j]; + } + for (k = 0; k <= i; k++) { + V[k][j] -= g * d[k]; + } + } + } + for (k = 0; k <= i; k++) { + V[k][i+1] = 0.0; + } + } + for (j = 0; j < n; j++) { + d[j] = V[n-1][j]; + V[n-1][j] = 0.0; + } + V[n-1][n-1] = 1.0; + e[0] = 0.0; +} + +// Symmetric tridiagonal QL algorithm. + +static void tql2(double V[n][n], double d[n], double e[n]) { + +// This is derived from the Algol procedures tql2, by +// Bowdler, Martin, Reinsch, and Wilkinson, Handbook for +// Auto. Comp., Vol.ii-Linear Algebra, and the corresponding +// Fortran subroutine in EISPACK. + + int i,j,m,l,k; + double g,p,r,dl1,h,f,tst1,eps; + double c,c2,c3,el1,s,s2; + + for (i = 1; i < n; i++) { + e[i-1] = e[i]; + } + e[n-1] = 0.0; + + f = 0.0; + tst1 = 0.0; + eps = pow(2.0,-52.0); + for (l = 0; l < n; l++) { + + // Find small subdiagonal element + + tst1 = MAX(tst1,fabs(d[l]) + fabs(e[l])); + m = l; + while (m < n) { + if (fabs(e[m]) <= eps*tst1) { + break; + } + m++; + } + + // If m == l, d[l] is an eigenvalue, + // otherwise, iterate. + + if (m > l) { + int iter = 0; + do { + iter = iter + 1; // (Could check iteration count here.) + + // Compute implicit shift + + g = d[l]; + p = (d[l+1] - g) / (2.0 * e[l]); + r = hypot2(p,1.0); + if (p < 0) { + r = -r; + } + d[l] = e[l] / (p + r); + d[l+1] = e[l] * (p + r); + dl1 = d[l+1]; + h = g - d[l]; + for (i = l+2; i < n; i++) { + d[i] -= h; + } + f = f + h; + + // Implicit QL transformation. + + p = d[m]; + c = 1.0; + c2 = c; + c3 = c; + el1 = e[l+1]; + s = 0.0; + s2 = 0.0; + for (i = m-1; i >= l; i--) { + c3 = c2; + c2 = c; + s2 = s; + g = c * e[i]; + h = c * p; + r = hypot2(p,e[i]); + e[i+1] = s * r; + s = e[i] / r; + c = p / r; + p = c * d[i] - s * g; + d[i+1] = h + s * (c * g + s * d[i]); + + // Accumulate transformation. + + for (k = 0; k < n; k++) { + h = V[k][i+1]; + V[k][i+1] = s * V[k][i] + c * h; + V[k][i] = c * V[k][i] - s * h; + } + } + p = -s * s2 * c3 * el1 * e[l] / dl1; + e[l] = s * p; + d[l] = c * p; + + // Check for convergence. + + } while (fabs(e[l]) > eps*tst1); + } + d[l] = d[l] + f; + e[l] = 0.0; + } + + // Sort eigenvalues and corresponding vectors. + + for (i = 0; i < n-1; i++) { + k = i; + p = d[i]; + for (j = i+1; j < n; j++) { + if (d[j] < p) { + k = j; + p = d[j]; + } + } + if (k != i) { + d[k] = d[i]; + d[i] = p; + for (j = 0; j < n; j++) { + p = V[j][i]; + V[j][i] = V[j][k]; + V[j][k] = p; + } + } + } +} + +void eigen_decomposition(double A[n][n], double V[n][n], double d[n]) { + int i,j; + double e[n]; + for (i = 0; i < n; i++) { + for (j = 0; j < n; j++) { + V[i][j] = A[i][j]; + } + } + tred2(V, d, e); + tql2(V, d, e); +} diff --git a/Localizations/Libraries/Ros/amcl/src/amcl/pf/pf.c b/Localizations/Libraries/Ros/amcl/src/amcl/pf/pf.c new file mode 100644 index 0000000..6063d41 --- /dev/null +++ b/Localizations/Libraries/Ros/amcl/src/amcl/pf/pf.c @@ -0,0 +1,763 @@ +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2000 Brian Gerkey & Kasper Stoy + * gerkey@usc.edu kaspers@robotics.usc.edu + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ +/************************************************************************** + * Desc: Simple particle filter for localization. + * Author: Andrew Howard + * Date: 10 Dec 2002 + * CVS: $Id: pf.c 6345 2008-04-17 01:36:39Z gerkey $ + *************************************************************************/ + +#include +#include +#include +#include + +#include "amcl/pf/pf.h" +#include "amcl/pf/pf_pdf.h" +#include "amcl/pf/pf_kdtree.h" +#include "amcl/portable_utils.hpp" + + +// Compute the required number of samples, given that there are k bins +// with samples in them. +static int pf_resample_limit(pf_t *pf, int k); + + + +// Create a new filter +pf_t *pf_alloc(int min_samples, int max_samples, + double alpha_slow, double alpha_fast, + pf_init_model_fn_t random_pose_fn, void *random_pose_data) +{ + int i, j; + pf_t *pf; + pf_sample_set_t *set; + pf_sample_t *sample; + + srand48(time(NULL)); + + pf = calloc(1, sizeof(pf_t)); + + pf->random_pose_fn = random_pose_fn; + pf->random_pose_data = random_pose_data; + + pf->min_samples = min_samples; + pf->max_samples = max_samples; + + // Control parameters for the population size calculation. [err] is + // the max error between the true distribution and the estimated + // distribution. [z] is the upper standard normal quantile for (1 - + // p), where p is the probability that the error on the estimated + // distrubition will be less than [err]. + pf->pop_err = 0.01; + pf->pop_z = 3; + pf->dist_threshold = 0.5; + + // Number of leaf nodes is never higher than the max number of samples + pf->limit_cache = calloc(max_samples, sizeof(int)); + + pf->current_set = 0; + for (j = 0; j < 2; j++) + { + set = pf->sets + j; + + set->sample_count = max_samples; + set->samples = calloc(max_samples, sizeof(pf_sample_t)); + + for (i = 0; i < set->sample_count; i++) + { + sample = set->samples + i; + sample->pose.v[0] = 0.0; + sample->pose.v[1] = 0.0; + sample->pose.v[2] = 0.0; + sample->weight = 1.0 / max_samples; + } + + // HACK: is 3 times max_samples enough? + set->kdtree = pf_kdtree_alloc(3 * max_samples); + + set->cluster_count = 0; + set->cluster_max_count = max_samples; + set->clusters = calloc(set->cluster_max_count, sizeof(pf_cluster_t)); + + set->mean = pf_vector_zero(); + set->cov = pf_matrix_zero(); + } + + pf->w_slow = 0.0; + pf->w_fast = 0.0; + + pf->alpha_slow = alpha_slow; + pf->alpha_fast = alpha_fast; + + //set converged to 0 + pf_init_converged(pf); + + return pf; +} + +// Free an existing filter +void pf_free(pf_t *pf) +{ + int i; + + free(pf->limit_cache); + + for (i = 0; i < 2; i++) + { + free(pf->sets[i].clusters); + pf_kdtree_free(pf->sets[i].kdtree); + free(pf->sets[i].samples); + } + free(pf); + + return; +} + +// Initialize the filter using a gaussian +void pf_init(pf_t *pf, pf_vector_t mean, pf_matrix_t cov) +{ + int i; + pf_sample_set_t *set; + pf_sample_t *sample; + pf_pdf_gaussian_t *pdf; + + set = pf->sets + pf->current_set; + + // Create the kd tree for adaptive sampling + pf_kdtree_clear(set->kdtree); + + set->sample_count = pf->max_samples; + + pdf = pf_pdf_gaussian_alloc(mean, cov); + + // Compute the new sample poses + for (i = 0; i < set->sample_count; i++) + { + sample = set->samples + i; + sample->weight = 1.0 / pf->max_samples; + sample->pose = pf_pdf_gaussian_sample(pdf); + + // Add sample to histogram + pf_kdtree_insert(set->kdtree, sample->pose, sample->weight); + } + + pf->w_slow = pf->w_fast = 0.0; + + pf_pdf_gaussian_free(pdf); + + // Re-compute cluster statistics + pf_cluster_stats(pf, set); + + //set converged to 0 + pf_init_converged(pf); + + return; +} + + +// Initialize the filter using some model +void pf_init_model(pf_t *pf, pf_init_model_fn_t init_fn, void *init_data) +{ + int i; + pf_sample_set_t *set; + pf_sample_t *sample; + + set = pf->sets + pf->current_set; + + // Create the kd tree for adaptive sampling + pf_kdtree_clear(set->kdtree); + + set->sample_count = pf->max_samples; + + // Compute the new sample poses + for (i = 0; i < set->sample_count; i++) + { + sample = set->samples + i; + sample->weight = 1.0 / pf->max_samples; + sample->pose = (*init_fn) (init_data); + + // Add sample to histogram + pf_kdtree_insert(set->kdtree, sample->pose, sample->weight); + } + + pf->w_slow = pf->w_fast = 0.0; + + // Re-compute cluster statistics + pf_cluster_stats(pf, set); + + //set converged to 0 + pf_init_converged(pf); + + return; +} + +void pf_init_converged(pf_t *pf){ + pf_sample_set_t *set; + set = pf->sets + pf->current_set; + set->converged = 0; + pf->converged = 0; +} + +int pf_update_converged(pf_t *pf) +{ + int i; + pf_sample_set_t *set; + pf_sample_t *sample; + double total; + + set = pf->sets + pf->current_set; + double mean_x = 0, mean_y = 0; + + for (i = 0; i < set->sample_count; i++){ + sample = set->samples + i; + + mean_x += sample->pose.v[0]; + mean_y += sample->pose.v[1]; + } + mean_x /= set->sample_count; + mean_y /= set->sample_count; + + for (i = 0; i < set->sample_count; i++){ + sample = set->samples + i; + if(fabs(sample->pose.v[0] - mean_x) > pf->dist_threshold || + fabs(sample->pose.v[1] - mean_y) > pf->dist_threshold){ + set->converged = 0; + pf->converged = 0; + return 0; + } + } + set->converged = 1; + pf->converged = 1; + return 1; +} + +// Update the filter with some new action +void pf_update_action(pf_t *pf, pf_action_model_fn_t action_fn, void *action_data) +{ + pf_sample_set_t *set; + + set = pf->sets + pf->current_set; + + (*action_fn) (action_data, set); + + return; +} + + +#include +// Update the filter with some new sensor observation +void pf_update_sensor(pf_t *pf, pf_sensor_model_fn_t sensor_fn, void *sensor_data) +{ + int i; + pf_sample_set_t *set; + pf_sample_t *sample; + double total; + + set = pf->sets + pf->current_set; + + // Compute the sample weights + total = (*sensor_fn) (sensor_data, set); + + set->n_effective = 0; + + if (total > 0.0) + { + // Normalize weights + double w_avg=0.0; + for (i = 0; i < set->sample_count; i++) + { + sample = set->samples + i; + w_avg += sample->weight; + sample->weight /= total; + set->n_effective += sample->weight*sample->weight; + } + // Update running averages of likelihood of samples (Prob Rob p258) + w_avg /= set->sample_count; + if(pf->w_slow == 0.0) + pf->w_slow = w_avg; + else + pf->w_slow += pf->alpha_slow * (w_avg - pf->w_slow); + if(pf->w_fast == 0.0) + pf->w_fast = w_avg; + else + pf->w_fast += pf->alpha_fast * (w_avg - pf->w_fast); + //printf("w_avg: %e slow: %e fast: %e\n", + //w_avg, pf->w_slow, pf->w_fast); + } + else + { + // Handle zero total + for (i = 0; i < set->sample_count; i++) + { + sample = set->samples + i; + sample->weight = 1.0 / set->sample_count; + } + } + + set->n_effective = 1.0/set->n_effective; + return; +} + +// copy set a to set b +void copy_set(pf_sample_set_t* set_a, pf_sample_set_t* set_b) +{ + int i; + double total; + pf_sample_t *sample_a, *sample_b; + + // Clean set b's kdtree + pf_kdtree_clear(set_b->kdtree); + + // Copy samples from set a to create set b + total = 0; + set_b->sample_count = 0; + + for(i = 0; i < set_a->sample_count; i++) + { + sample_b = set_b->samples + set_b->sample_count++; + + sample_a = set_a->samples + i; + + assert(sample_a->weight > 0); + + // Copy sample a to sample b + sample_b->pose = sample_a->pose; + sample_b->weight = sample_a->weight; + + total += sample_b->weight; + + // Add sample to histogram + pf_kdtree_insert(set_b->kdtree, sample_b->pose, sample_b->weight); + } + + // Normalize weights + for (i = 0; i < set_b->sample_count; i++) + { + sample_b = set_b->samples + i; + sample_b->weight /= total; + } + + set_b->converged = set_a->converged; +} + +// Resample the distribution +void pf_update_resample(pf_t *pf) +{ + int i; + double total; + pf_sample_set_t *set_a, *set_b; + pf_sample_t *sample_a, *sample_b; + + //double r,c,U; + //int m; + //double count_inv; + double* c; + + double w_diff; + + set_a = pf->sets + pf->current_set; + set_b = pf->sets + (pf->current_set + 1) % 2; + + if (pf->selective_resampling != 0) + { + if (set_a->n_effective > 0.5*(set_a->sample_count)) + { + // copy set a to b + copy_set(set_a,set_b); + + // Re-compute cluster statistics + pf_cluster_stats(pf, set_b); + + // Use the newly created sample set + pf->current_set = (pf->current_set + 1) % 2; + return; + } + } + + // Build up cumulative probability table for resampling. + // TODO: Replace this with a more efficient procedure + // (e.g., http://www.network-theory.co.uk/docs/gslref/GeneralDiscreteDistributions.html) + c = (double*)malloc(sizeof(double)*(set_a->sample_count+1)); + c[0] = 0.0; + for(i=0;isample_count;i++) + c[i+1] = c[i]+set_a->samples[i].weight; + + // Create the kd tree for adaptive sampling + pf_kdtree_clear(set_b->kdtree); + + // Draw samples from set a to create set b. + total = 0; + set_b->sample_count = 0; + + w_diff = 1.0 - pf->w_fast / pf->w_slow; + if(w_diff < 0.0) + w_diff = 0.0; + //printf("w_diff: %9.6f\n", w_diff); + + // Can't (easily) combine low-variance sampler with KLD adaptive + // sampling, so we'll take the more traditional route. + /* + // Low-variance resampler, taken from Probabilistic Robotics, p110 + count_inv = 1.0/set_a->sample_count; + r = drand48() * count_inv; + c = set_a->samples[0].weight; + i = 0; + m = 0; + */ + while(set_b->sample_count < pf->max_samples) + { + sample_b = set_b->samples + set_b->sample_count++; + + if(drand48() < w_diff) + sample_b->pose = (pf->random_pose_fn)(pf->random_pose_data); + else + { + // Can't (easily) combine low-variance sampler with KLD adaptive + // sampling, so we'll take the more traditional route. + /* + // Low-variance resampler, taken from Probabilistic Robotics, p110 + U = r + m * count_inv; + while(U>c) + { + i++; + // Handle wrap-around by resetting counters and picking a new random + // number + if(i >= set_a->sample_count) + { + r = drand48() * count_inv; + c = set_a->samples[0].weight; + i = 0; + m = 0; + U = r + m * count_inv; + continue; + } + c += set_a->samples[i].weight; + } + m++; + */ + + // Naive discrete event sampler + double r; + r = drand48(); + for(i=0;isample_count;i++) + { + if((c[i] <= r) && (r < c[i+1])) + break; + } + assert(isample_count); + + sample_a = set_a->samples + i; + + assert(sample_a->weight > 0); + + // Add sample to list + sample_b->pose = sample_a->pose; + } + + sample_b->weight = 1.0; + total += sample_b->weight; + + // Add sample to histogram + pf_kdtree_insert(set_b->kdtree, sample_b->pose, sample_b->weight); + + // See if we have enough samples yet + if (set_b->sample_count > pf_resample_limit(pf, set_b->kdtree->leaf_count)) + break; + } + + // Reset averages, to avoid spiraling off into complete randomness. + if(w_diff > 0.0) + pf->w_slow = pf->w_fast = 0.0; + + //fprintf(stderr, "\n\n"); + + // Normalize weights + for (i = 0; i < set_b->sample_count; i++) + { + sample_b = set_b->samples + i; + sample_b->weight /= total; + } + + // Re-compute cluster statistics + pf_cluster_stats(pf, set_b); + + // Use the newly created sample set + pf->current_set = (pf->current_set + 1) % 2; + + pf_update_converged(pf); + + free(c); + return; +} + + +// Compute the required number of samples, given that there are k bins +// with samples in them. This is taken directly from Fox et al. +int pf_resample_limit(pf_t *pf, int k) +{ + double a, b, c, x; + int n; + + // Return max_samples in case k is outside expected range, this shouldn't + // happen, but is added to prevent any runtime errors + if (k < 1 || k > pf->max_samples) + return pf->max_samples; + + // Return value if cache is valid, which means value is non-zero positive + if (pf->limit_cache[k-1] > 0) + return pf->limit_cache[k-1]; + + if (k == 1) + { + pf->limit_cache[k-1] = pf->max_samples; + return pf->max_samples; + } + + a = 1; + b = 2 / (9 * ((double) k - 1)); + c = sqrt(2 / (9 * ((double) k - 1))) * pf->pop_z; + x = a - b + c; + + n = (int) ceil((k - 1) / (2 * pf->pop_err) * x * x * x); + + if (n < pf->min_samples) + { + pf->limit_cache[k-1] = pf->min_samples; + return pf->min_samples; + } + if (n > pf->max_samples) + { + pf->limit_cache[k-1] = pf->max_samples; + return pf->max_samples; + } + + pf->limit_cache[k-1] = n; + return n; +} + + +// Re-compute the cluster statistics for a sample set +void pf_cluster_stats(pf_t *pf, pf_sample_set_t *set) +{ + int i, j, k, cidx; + pf_sample_t *sample; + pf_cluster_t *cluster; + + // Workspace + double m[4], c[2][2]; + size_t count; + double weight; + + // Cluster the samples + pf_kdtree_cluster(set->kdtree); + + // Initialize cluster stats + set->cluster_count = 0; + + for (i = 0; i < set->cluster_max_count; i++) + { + cluster = set->clusters + i; + cluster->count = 0; + cluster->weight = 0; + cluster->mean = pf_vector_zero(); + cluster->cov = pf_matrix_zero(); + + for (j = 0; j < 4; j++) + cluster->m[j] = 0.0; + for (j = 0; j < 2; j++) + for (k = 0; k < 2; k++) + cluster->c[j][k] = 0.0; + } + + // Initialize overall filter stats + count = 0; + weight = 0.0; + set->mean = pf_vector_zero(); + set->cov = pf_matrix_zero(); + for (j = 0; j < 4; j++) + m[j] = 0.0; + for (j = 0; j < 2; j++) + for (k = 0; k < 2; k++) + c[j][k] = 0.0; + + // Compute cluster stats + for (i = 0; i < set->sample_count; i++) + { + sample = set->samples + i; + + //printf("%d %f %f %f\n", i, sample->pose.v[0], sample->pose.v[1], sample->pose.v[2]); + + // Get the cluster label for this sample + cidx = pf_kdtree_get_cluster(set->kdtree, sample->pose); + assert(cidx >= 0); + if (cidx >= set->cluster_max_count) + continue; + if (cidx + 1 > set->cluster_count) + set->cluster_count = cidx + 1; + + cluster = set->clusters + cidx; + + cluster->count += 1; + cluster->weight += sample->weight; + + count += 1; + weight += sample->weight; + + // Compute mean + cluster->m[0] += sample->weight * sample->pose.v[0]; + cluster->m[1] += sample->weight * sample->pose.v[1]; + cluster->m[2] += sample->weight * cos(sample->pose.v[2]); + cluster->m[3] += sample->weight * sin(sample->pose.v[2]); + + m[0] += sample->weight * sample->pose.v[0]; + m[1] += sample->weight * sample->pose.v[1]; + m[2] += sample->weight * cos(sample->pose.v[2]); + m[3] += sample->weight * sin(sample->pose.v[2]); + + // Compute covariance in linear components + for (j = 0; j < 2; j++) + for (k = 0; k < 2; k++) + { + cluster->c[j][k] += sample->weight * sample->pose.v[j] * sample->pose.v[k]; + c[j][k] += sample->weight * sample->pose.v[j] * sample->pose.v[k]; + } + } + + // Normalize + for (i = 0; i < set->cluster_count; i++) + { + cluster = set->clusters + i; + + cluster->mean.v[0] = cluster->m[0] / cluster->weight; + cluster->mean.v[1] = cluster->m[1] / cluster->weight; + cluster->mean.v[2] = atan2(cluster->m[3], cluster->m[2]); + + cluster->cov = pf_matrix_zero(); + + // Covariance in linear components + for (j = 0; j < 2; j++) + for (k = 0; k < 2; k++) + cluster->cov.m[j][k] = cluster->c[j][k] / cluster->weight - + cluster->mean.v[j] * cluster->mean.v[k]; + + // Covariance in angular components; I think this is the correct + // formula for circular statistics. + cluster->cov.m[2][2] = -2 * log(sqrt(cluster->m[2] * cluster->m[2] + + cluster->m[3] * cluster->m[3]) / cluster->weight); + + //printf("cluster %d %d %f (%f %f %f)\n", i, cluster->count, cluster->weight, + //cluster->mean.v[0], cluster->mean.v[1], cluster->mean.v[2]); + //pf_matrix_fprintf(cluster->cov, stdout, "%e"); + } + + assert(fabs(weight) >= DBL_EPSILON); + if (fabs(weight) < DBL_EPSILON) + { + printf("ERROR : divide-by-zero exception : weight is zero\n"); + return; + } + // Compute overall filter stats + set->mean.v[0] = m[0] / weight; + set->mean.v[1] = m[1] / weight; + set->mean.v[2] = atan2(m[3], m[2]); + + // Covariance in linear components + for (j = 0; j < 2; j++) + for (k = 0; k < 2; k++) + set->cov.m[j][k] = c[j][k] / weight - set->mean.v[j] * set->mean.v[k]; + + // Covariance in angular components; I think this is the correct + // formula for circular statistics. + set->cov.m[2][2] = -2 * log(sqrt(m[2] * m[2] + m[3] * m[3])); + + return; +} + +void pf_set_selective_resampling(pf_t *pf, int selective_resampling) +{ + pf->selective_resampling = selective_resampling; +} + +// Compute the CEP statistics (mean and variance). +void pf_get_cep_stats(pf_t *pf, pf_vector_t *mean, double *var) +{ + int i; + double mn, mx, my, mrr; + pf_sample_set_t *set; + pf_sample_t *sample; + + set = pf->sets + pf->current_set; + + mn = 0.0; + mx = 0.0; + my = 0.0; + mrr = 0.0; + + for (i = 0; i < set->sample_count; i++) + { + sample = set->samples + i; + + mn += sample->weight; + mx += sample->weight * sample->pose.v[0]; + my += sample->weight * sample->pose.v[1]; + mrr += sample->weight * sample->pose.v[0] * sample->pose.v[0]; + mrr += sample->weight * sample->pose.v[1] * sample->pose.v[1]; + } + + assert(fabs(mn) >= DBL_EPSILON); + if (fabs(mn) < DBL_EPSILON) + { + printf("ERROR : divide-by-zero exception : mn is zero\n"); + return; + } + + mean->v[0] = mx / mn; + mean->v[1] = my / mn; + mean->v[2] = 0.0; + + *var = mrr / mn - (mx * mx / (mn * mn) + my * my / (mn * mn)); + + return; +} + + +// Get the statistics for a particular cluster. +int pf_get_cluster_stats(pf_t *pf, int clabel, double *weight, + pf_vector_t *mean, pf_matrix_t *cov) +{ + pf_sample_set_t *set; + pf_cluster_t *cluster; + + set = pf->sets + pf->current_set; + + if (clabel >= set->cluster_count) + return 0; + cluster = set->clusters + clabel; + + *weight = cluster->weight; + *mean = cluster->mean; + *cov = cluster->cov; + + return 1; +} + + diff --git a/Localizations/Libraries/Ros/amcl/src/amcl/pf/pf_draw.c b/Localizations/Libraries/Ros/amcl/src/amcl/pf/pf_draw.c new file mode 100644 index 0000000..0c445af --- /dev/null +++ b/Localizations/Libraries/Ros/amcl/src/amcl/pf/pf_draw.c @@ -0,0 +1,163 @@ +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2000 Brian Gerkey & Kasper Stoy + * gerkey@usc.edu kaspers@robotics.usc.edu + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ +/************************************************************************** + * Desc: Particle filter; drawing routines + * Author: Andrew Howard + * Date: 10 Dec 2002 + * CVS: $Id: pf_draw.c 7057 2008-10-02 00:44:06Z gbiggs $ + *************************************************************************/ + +#ifdef INCLUDE_RTKGUI + +#include +#include +#include + + +#include "rtk.h" + +#include "pf.h" +#include "pf_pdf.h" +#include "pf_kdtree.h" + + +// Draw the statistics +void pf_draw_statistics(pf_t *pf, rtk_fig_t *fig); + + +// Draw the sample set +void pf_draw_samples(pf_t *pf, rtk_fig_t *fig, int max_samples) +{ + int i; + double px, py, pa; + pf_sample_set_t *set; + pf_sample_t *sample; + + set = pf->sets + pf->current_set; + max_samples = MIN(max_samples, set->sample_count); + + for (i = 0; i < max_samples; i++) + { + sample = set->samples + i; + + px = sample->pose.v[0]; + py = sample->pose.v[1]; + pa = sample->pose.v[2]; + + //printf("%f %f\n", px, py); + + rtk_fig_point(fig, px, py); + rtk_fig_arrow(fig, px, py, pa, 0.1, 0.02); + //rtk_fig_rectangle(fig, px, py, 0, 0.1, 0.1, 0); + } + + return; +} + + +// Draw the hitogram (kd tree) +void pf_draw_hist(pf_t *pf, rtk_fig_t *fig) +{ + pf_sample_set_t *set; + + set = pf->sets + pf->current_set; + + rtk_fig_color(fig, 0.0, 0.0, 1.0); + pf_kdtree_draw(set->kdtree, fig); + + return; +} + + +// Draw the CEP statistics +void pf_draw_cep_stats(pf_t *pf, rtk_fig_t *fig) +{ + pf_vector_t mean; + double var; + + pf_get_cep_stats(pf, &mean, &var); + var = sqrt(var); + + rtk_fig_color(fig, 0, 0, 1); + rtk_fig_ellipse(fig, mean.v[0], mean.v[1], mean.v[2], 3 * var, 3 * var, 0); + + return; +} + + +// Draw the cluster statistics +void pf_draw_cluster_stats(pf_t *pf, rtk_fig_t *fig) +{ + int i; + pf_cluster_t *cluster; + pf_sample_set_t *set; + pf_vector_t mean; + pf_matrix_t cov; + pf_matrix_t r, d; + double weight, o, d1, d2; + + set = pf->sets + pf->current_set; + + for (i = 0; i < set->cluster_count; i++) + { + cluster = set->clusters + i; + + weight = cluster->weight; + mean = cluster->mean; + cov = cluster->cov; + + // Compute unitary representation S = R D R^T + pf_matrix_unitary(&r, &d, cov); + + /* Debugging + printf("mean = \n"); + pf_vector_fprintf(mean, stdout, "%e"); + printf("cov = \n"); + pf_matrix_fprintf(cov, stdout, "%e"); + printf("r = \n"); + pf_matrix_fprintf(r, stdout, "%e"); + printf("d = \n"); + pf_matrix_fprintf(d, stdout, "%e"); + */ + + // Compute the orientation of the error ellipse (first eigenvector) + o = atan2(r.m[1][0], r.m[0][0]); + d1 = 6 * sqrt(d.m[0][0]); + d2 = 6 * sqrt(d.m[1][1]); + + if (d1 > 1e-3 && d2 > 1e-3) + { + // Draw the error ellipse + rtk_fig_ellipse(fig, mean.v[0], mean.v[1], o, d1, d2, 0); + rtk_fig_line_ex(fig, mean.v[0], mean.v[1], o, d1); + rtk_fig_line_ex(fig, mean.v[0], mean.v[1], o + M_PI / 2, d2); + } + + // Draw a direction indicator + rtk_fig_arrow(fig, mean.v[0], mean.v[1], mean.v[2], 0.50, 0.10); + rtk_fig_arrow(fig, mean.v[0], mean.v[1], mean.v[2] + 3 * sqrt(cov.m[2][2]), 0.50, 0.10); + rtk_fig_arrow(fig, mean.v[0], mean.v[1], mean.v[2] - 3 * sqrt(cov.m[2][2]), 0.50, 0.10); + } + + return; +} + +#endif diff --git a/Localizations/Libraries/Ros/amcl/src/amcl/pf/pf_kdtree.c b/Localizations/Libraries/Ros/amcl/src/amcl/pf/pf_kdtree.c new file mode 100644 index 0000000..b625b83 --- /dev/null +++ b/Localizations/Libraries/Ros/amcl/src/amcl/pf/pf_kdtree.c @@ -0,0 +1,486 @@ +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2000 Brian Gerkey & Kasper Stoy + * gerkey@usc.edu kaspers@robotics.usc.edu + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ +/************************************************************************** + * Desc: kd-tree functions + * Author: Andrew Howard + * Date: 18 Dec 2002 + * CVS: $Id: pf_kdtree.c 7057 2008-10-02 00:44:06Z gbiggs $ + *************************************************************************/ + +#include +#include +#include +#include + + +#include "amcl/pf/pf_vector.h" +#include "amcl/pf/pf_kdtree.h" + + +// Compare keys to see if they are equal +static int pf_kdtree_equal(pf_kdtree_t *self, int key_a[], int key_b[]); + +// Insert a node into the tree +static pf_kdtree_node_t *pf_kdtree_insert_node(pf_kdtree_t *self, pf_kdtree_node_t *parent, + pf_kdtree_node_t *node, int key[], double value); + +// Recursive node search +static pf_kdtree_node_t *pf_kdtree_find_node(pf_kdtree_t *self, pf_kdtree_node_t *node, int key[]); + +// Recursively label nodes in this cluster +static void pf_kdtree_cluster_node(pf_kdtree_t *self, pf_kdtree_node_t *node, int depth); + +// Recursive node printing +//static void pf_kdtree_print_node(pf_kdtree_t *self, pf_kdtree_node_t *node); + + +#ifdef INCLUDE_RTKGUI + +// Recursively draw nodes +static void pf_kdtree_draw_node(pf_kdtree_t *self, pf_kdtree_node_t *node, rtk_fig_t *fig); + +#endif + + + +//////////////////////////////////////////////////////////////////////////////// +// Create a tree +pf_kdtree_t *pf_kdtree_alloc(int max_size) +{ + pf_kdtree_t *self; + + self = calloc(1, sizeof(pf_kdtree_t)); + + self->size[0] = 0.50; + self->size[1] = 0.50; + self->size[2] = (10 * M_PI / 180); + + self->root = NULL; + + self->node_count = 0; + self->node_max_count = max_size; + self->nodes = calloc(self->node_max_count, sizeof(pf_kdtree_node_t)); + + self->leaf_count = 0; + + return self; +} + + +//////////////////////////////////////////////////////////////////////////////// +// Destroy a tree +void pf_kdtree_free(pf_kdtree_t *self) +{ + free(self->nodes); + free(self); + return; +} + + +//////////////////////////////////////////////////////////////////////////////// +// Clear all entries from the tree +void pf_kdtree_clear(pf_kdtree_t *self) +{ + self->root = NULL; + self->leaf_count = 0; + self->node_count = 0; + + return; +} + + +//////////////////////////////////////////////////////////////////////////////// +// Insert a pose into the tree. +void pf_kdtree_insert(pf_kdtree_t *self, pf_vector_t pose, double value) +{ + int key[3]; + + key[0] = floor(pose.v[0] / self->size[0]); + key[1] = floor(pose.v[1] / self->size[1]); + key[2] = floor(pose.v[2] / self->size[2]); + + self->root = pf_kdtree_insert_node(self, NULL, self->root, key, value); + + // Test code + /* + printf("find %d %d %d\n", key[0], key[1], key[2]); + assert(pf_kdtree_find_node(self, self->root, key) != NULL); + + pf_kdtree_print_node(self, self->root); + + printf("\n"); + + for (i = 0; i < self->node_count; i++) + { + node = self->nodes + i; + if (node->leaf) + { + printf("find %d %d %d\n", node->key[0], node->key[1], node->key[2]); + assert(pf_kdtree_find_node(self, self->root, node->key) == node); + } + } + printf("\n\n"); + */ + + return; +} + + +//////////////////////////////////////////////////////////////////////////////// +// Determine the probability estimate for the given pose. TODO: this +// should do a kernel density estimate rather than a simple histogram. +double pf_kdtree_get_prob(pf_kdtree_t *self, pf_vector_t pose) +{ + int key[3]; + pf_kdtree_node_t *node; + + key[0] = floor(pose.v[0] / self->size[0]); + key[1] = floor(pose.v[1] / self->size[1]); + key[2] = floor(pose.v[2] / self->size[2]); + + node = pf_kdtree_find_node(self, self->root, key); + if (node == NULL) + return 0.0; + return node->value; +} + + +//////////////////////////////////////////////////////////////////////////////// +// Determine the cluster label for the given pose +int pf_kdtree_get_cluster(pf_kdtree_t *self, pf_vector_t pose) +{ + int key[3]; + pf_kdtree_node_t *node; + + key[0] = floor(pose.v[0] / self->size[0]); + key[1] = floor(pose.v[1] / self->size[1]); + key[2] = floor(pose.v[2] / self->size[2]); + + node = pf_kdtree_find_node(self, self->root, key); + if (node == NULL) + return -1; + return node->cluster; +} + + +//////////////////////////////////////////////////////////////////////////////// +// Compare keys to see if they are equal +int pf_kdtree_equal(pf_kdtree_t *self, int key_a[], int key_b[]) +{ + //double a, b; + + if (key_a[0] != key_b[0]) + return 0; + if (key_a[1] != key_b[1]) + return 0; + + if (key_a[2] != key_b[2]) + return 0; + + /* TODO: make this work (pivot selection needs fixing, too) + // Normalize angles + a = key_a[2] * self->size[2]; + a = atan2(sin(a), cos(a)) / self->size[2]; + b = key_b[2] * self->size[2]; + b = atan2(sin(b), cos(b)) / self->size[2]; + + if ((int) a != (int) b) + return 0; + */ + + return 1; +} + + +//////////////////////////////////////////////////////////////////////////////// +// Insert a node into the tree +pf_kdtree_node_t *pf_kdtree_insert_node(pf_kdtree_t *self, pf_kdtree_node_t *parent, + pf_kdtree_node_t *node, int key[], double value) +{ + int i; + int split, max_split; + + // If the node doesnt exist yet... + if (node == NULL) + { + assert(self->node_count < self->node_max_count); + node = self->nodes + self->node_count++; + memset(node, 0, sizeof(pf_kdtree_node_t)); + + node->leaf = 1; + + if (parent == NULL) + node->depth = 0; + else + node->depth = parent->depth + 1; + + for (i = 0; i < 3; i++) + node->key[i] = key[i]; + + node->value = value; + self->leaf_count += 1; + } + + // If the node exists, and it is a leaf node... + else if (node->leaf) + { + // If the keys are equal, increment the value + if (pf_kdtree_equal(self, key, node->key)) + { + node->value += value; + } + + // The keys are not equal, so split this node + else + { + // Find the dimension with the largest variance and do a mean + // split + max_split = 0; + node->pivot_dim = -1; + for (i = 0; i < 3; i++) + { + split = abs(key[i] - node->key[i]); + if (split > max_split) + { + max_split = split; + node->pivot_dim = i; + } + } + assert(node->pivot_dim >= 0); + + node->pivot_value = (key[node->pivot_dim] + node->key[node->pivot_dim]) / 2.0; + + if (key[node->pivot_dim] < node->pivot_value) + { + node->children[0] = pf_kdtree_insert_node(self, node, NULL, key, value); + node->children[1] = pf_kdtree_insert_node(self, node, NULL, node->key, node->value); + } + else + { + node->children[0] = pf_kdtree_insert_node(self, node, NULL, node->key, node->value); + node->children[1] = pf_kdtree_insert_node(self, node, NULL, key, value); + } + + node->leaf = 0; + self->leaf_count -= 1; + } + } + + // If the node exists, and it has children... + else + { + assert(node->children[0] != NULL); + assert(node->children[1] != NULL); + + if (key[node->pivot_dim] < node->pivot_value) + pf_kdtree_insert_node(self, node, node->children[0], key, value); + else + pf_kdtree_insert_node(self, node, node->children[1], key, value); + } + + return node; +} + + +//////////////////////////////////////////////////////////////////////////////// +// Recursive node search +pf_kdtree_node_t *pf_kdtree_find_node(pf_kdtree_t *self, pf_kdtree_node_t *node, int key[]) +{ + if (node->leaf) + { + //printf("find : leaf %p %d %d %d\n", node, node->key[0], node->key[1], node->key[2]); + + // If the keys are the same... + if (pf_kdtree_equal(self, key, node->key)) + return node; + else + return NULL; + } + else + { + //printf("find : brch %p %d %f\n", node, node->pivot_dim, node->pivot_value); + + assert(node->children[0] != NULL); + assert(node->children[1] != NULL); + + // If the keys are different... + if (key[node->pivot_dim] < node->pivot_value) + return pf_kdtree_find_node(self, node->children[0], key); + else + return pf_kdtree_find_node(self, node->children[1], key); + } + + return NULL; +} + + +//////////////////////////////////////////////////////////////////////////////// +// Recursive node printing +/* +void pf_kdtree_print_node(pf_kdtree_t *self, pf_kdtree_node_t *node) +{ + if (node->leaf) + { + printf("(%+02d %+02d %+02d)\n", node->key[0], node->key[1], node->key[2]); + printf("%*s", node->depth * 11, ""); + } + else + { + printf("(%+02d %+02d %+02d) ", node->key[0], node->key[1], node->key[2]); + pf_kdtree_print_node(self, node->children[0]); + pf_kdtree_print_node(self, node->children[1]); + } + return; +} +*/ + + +//////////////////////////////////////////////////////////////////////////////// +// Cluster the leaves in the tree +void pf_kdtree_cluster(pf_kdtree_t *self) +{ + int i; + int queue_count, cluster_count; + pf_kdtree_node_t **queue, *node; + + queue_count = 0; + queue = calloc(self->node_count, sizeof(queue[0])); + + // Put all the leaves in a queue + for (i = 0; i < self->node_count; i++) + { + node = self->nodes + i; + if (node->leaf) + { + node->cluster = -1; + assert(queue_count < self->node_count); + queue[queue_count++] = node; + + // TESTING; remove + assert(node == pf_kdtree_find_node(self, self->root, node->key)); + } + } + + cluster_count = 0; + + // Do connected components for each node + while (queue_count > 0) + { + node = queue[--queue_count]; + + // If this node has already been labelled, skip it + if (node->cluster >= 0) + continue; + + // Assign a label to this cluster + node->cluster = cluster_count++; + + // Recursively label nodes in this cluster + pf_kdtree_cluster_node(self, node, 0); + } + + free(queue); + return; +} + + +//////////////////////////////////////////////////////////////////////////////// +// Recursively label nodes in this cluster +void pf_kdtree_cluster_node(pf_kdtree_t *self, pf_kdtree_node_t *node, int depth) +{ + int i; + int nkey[3]; + pf_kdtree_node_t *nnode; + + for (i = 0; i < 3 * 3 * 3; i++) + { + nkey[0] = node->key[0] + (i / 9) - 1; + nkey[1] = node->key[1] + ((i % 9) / 3) - 1; + nkey[2] = node->key[2] + ((i % 9) % 3) - 1; + + nnode = pf_kdtree_find_node(self, self->root, nkey); + if (nnode == NULL) + continue; + + assert(nnode->leaf); + + // This node already has a label; skip it. The label should be + // consistent, however. + if (nnode->cluster >= 0) + { + assert(nnode->cluster == node->cluster); + continue; + } + + // Label this node and recurse + nnode->cluster = node->cluster; + + pf_kdtree_cluster_node(self, nnode, depth + 1); + } + return; +} + + + +#ifdef INCLUDE_RTKGUI + +//////////////////////////////////////////////////////////////////////////////// +// Draw the tree +void pf_kdtree_draw(pf_kdtree_t *self, rtk_fig_t *fig) +{ + if (self->root != NULL) + pf_kdtree_draw_node(self, self->root, fig); + return; +} + + +//////////////////////////////////////////////////////////////////////////////// +// Recursively draw nodes +void pf_kdtree_draw_node(pf_kdtree_t *self, pf_kdtree_node_t *node, rtk_fig_t *fig) +{ + double ox, oy; + char text[64]; + + if (node->leaf) + { + ox = (node->key[0] + 0.5) * self->size[0]; + oy = (node->key[1] + 0.5) * self->size[1]; + + rtk_fig_rectangle(fig, ox, oy, 0.0, self->size[0], self->size[1], 0); + + //snprintf(text, sizeof(text), "%0.3f", node->value); + //rtk_fig_text(fig, ox, oy, 0.0, text); + + snprintf(text, sizeof(text), "%d", node->cluster); + rtk_fig_text(fig, ox, oy, 0.0, text); + } + else + { + assert(node->children[0] != NULL); + assert(node->children[1] != NULL); + pf_kdtree_draw_node(self, node->children[0], fig); + pf_kdtree_draw_node(self, node->children[1], fig); + } + + return; +} + +#endif diff --git a/Localizations/Libraries/Ros/amcl/src/amcl/pf/pf_pdf.c b/Localizations/Libraries/Ros/amcl/src/amcl/pf/pf_pdf.c new file mode 100644 index 0000000..6286b6b --- /dev/null +++ b/Localizations/Libraries/Ros/amcl/src/amcl/pf/pf_pdf.c @@ -0,0 +1,147 @@ +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2000 Brian Gerkey & Kasper Stoy + * gerkey@usc.edu kaspers@robotics.usc.edu + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ +/************************************************************************** + * Desc: Useful pdf functions + * Author: Andrew Howard + * Date: 10 Dec 2002 + * CVS: $Id: pf_pdf.c 6348 2008-04-17 02:53:17Z gerkey $ + *************************************************************************/ + +#include +#include +#include +#include +//#include +//#include + +#include "amcl/pf/pf_pdf.h" +#include "amcl/portable_utils.hpp" + +// Random number generator seed value +static unsigned int pf_pdf_seed; + + +/************************************************************************** + * Gaussian + *************************************************************************/ + +// Create a gaussian pdf +pf_pdf_gaussian_t *pf_pdf_gaussian_alloc(pf_vector_t x, pf_matrix_t cx) +{ + pf_matrix_t cd; + pf_pdf_gaussian_t *pdf; + + pdf = calloc(1, sizeof(pf_pdf_gaussian_t)); + + pdf->x = x; + pdf->cx = cx; + //pdf->cxi = pf_matrix_inverse(cx, &pdf->cxdet); + + // Decompose the convariance matrix into a rotation + // matrix and a diagonal matrix. + pf_matrix_unitary(&pdf->cr, &cd, pdf->cx); + pdf->cd.v[0] = sqrt(cd.m[0][0]); + pdf->cd.v[1] = sqrt(cd.m[1][1]); + pdf->cd.v[2] = sqrt(cd.m[2][2]); + + // Initialize the random number generator + //pdf->rng = gsl_rng_alloc(gsl_rng_taus); + //gsl_rng_set(pdf->rng, ++pf_pdf_seed); + srand48(++pf_pdf_seed); + + return pdf; +} + + +// Destroy the pdf +void pf_pdf_gaussian_free(pf_pdf_gaussian_t *pdf) +{ + //gsl_rng_free(pdf->rng); + free(pdf); + return; +} + + +/* +// Compute the value of the pdf at some point [x]. +double pf_pdf_gaussian_value(pf_pdf_gaussian_t *pdf, pf_vector_t x) +{ + int i, j; + pf_vector_t z; + double zz, p; + + z = pf_vector_sub(x, pdf->x); + + zz = 0; + for (i = 0; i < 3; i++) + for (j = 0; j < 3; j++) + zz += z.v[i] * pdf->cxi.m[i][j] * z.v[j]; + + p = 1 / (2 * M_PI * pdf->cxdet) * exp(-zz / 2); + + return p; +} +*/ + + +// Generate a sample from the pdf. +pf_vector_t pf_pdf_gaussian_sample(pf_pdf_gaussian_t *pdf) +{ + int i, j; + pf_vector_t r; + pf_vector_t x; + + // Generate a random vector + for (i = 0; i < 3; i++) + { + //r.v[i] = gsl_ran_gaussian(pdf->rng, pdf->cd.v[i]); + r.v[i] = pf_ran_gaussian(pdf->cd.v[i]); + } + + for (i = 0; i < 3; i++) + { + x.v[i] = pdf->x.v[i]; + for (j = 0; j < 3; j++) + x.v[i] += pdf->cr.m[i][j] * r.v[j]; + } + + return x; +} + +// Draw randomly from a zero-mean Gaussian distribution, with standard +// deviation sigma. +// We use the polar form of the Box-Muller transformation, explained here: +// http://www.taygeta.com/random/gaussian.html +double pf_ran_gaussian(double sigma) +{ + double x1, x2, w, r; + + do + { + do { r = drand48(); } while (r==0.0); + x1 = 2.0 * r - 1.0; + do { r = drand48(); } while (r==0.0); + x2 = 2.0 * r - 1.0; + w = x1*x1 + x2*x2; + } while(w > 1.0 || w==0.0); + + return(sigma * x2 * sqrt(-2.0*log(w)/w)); +} diff --git a/Localizations/Libraries/Ros/amcl/src/amcl/pf/pf_vector.c b/Localizations/Libraries/Ros/amcl/src/amcl/pf/pf_vector.c new file mode 100644 index 0000000..23d7ccb --- /dev/null +++ b/Localizations/Libraries/Ros/amcl/src/amcl/pf/pf_vector.c @@ -0,0 +1,276 @@ +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2000 Brian Gerkey & Kasper Stoy + * gerkey@usc.edu kaspers@robotics.usc.edu + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ +/************************************************************************** + * Desc: Vector functions + * Author: Andrew Howard + * Date: 10 Dec 2002 + * CVS: $Id: pf_vector.c 6345 2008-04-17 01:36:39Z gerkey $ + *************************************************************************/ + +#include +//#include +//#include +//#include + +#include "amcl/pf/pf_vector.h" +#include "amcl/pf/eig3.h" + + +// Return a zero vector +pf_vector_t pf_vector_zero() +{ + pf_vector_t c; + + c.v[0] = 0.0; + c.v[1] = 0.0; + c.v[2] = 0.0; + + return c; +} + + +// Check for NAN or INF in any component +int pf_vector_finite(pf_vector_t a) +{ + int i; + + for (i = 0; i < 3; i++) + if (!isfinite(a.v[i])) + return 0; + + return 1; +} + + +// Print a vector +void pf_vector_fprintf(pf_vector_t a, FILE *file, const char *fmt) +{ + int i; + + for (i = 0; i < 3; i++) + { + fprintf(file, fmt, a.v[i]); + fprintf(file, " "); + } + fprintf(file, "\n"); + + return; +} + + +// Simple vector addition +pf_vector_t pf_vector_add(pf_vector_t a, pf_vector_t b) +{ + pf_vector_t c; + + c.v[0] = a.v[0] + b.v[0]; + c.v[1] = a.v[1] + b.v[1]; + c.v[2] = a.v[2] + b.v[2]; + + return c; +} + + +// Simple vector subtraction +pf_vector_t pf_vector_sub(pf_vector_t a, pf_vector_t b) +{ + pf_vector_t c; + + c.v[0] = a.v[0] - b.v[0]; + c.v[1] = a.v[1] - b.v[1]; + c.v[2] = a.v[2] - b.v[2]; + + return c; +} + + +// Transform from local to global coords (a + b) +pf_vector_t pf_vector_coord_add(pf_vector_t a, pf_vector_t b) +{ + pf_vector_t c; + + c.v[0] = b.v[0] + a.v[0] * cos(b.v[2]) - a.v[1] * sin(b.v[2]); + c.v[1] = b.v[1] + a.v[0] * sin(b.v[2]) + a.v[1] * cos(b.v[2]); + c.v[2] = b.v[2] + a.v[2]; + c.v[2] = atan2(sin(c.v[2]), cos(c.v[2])); + + return c; +} + + +// Transform from global to local coords (a - b) +pf_vector_t pf_vector_coord_sub(pf_vector_t a, pf_vector_t b) +{ + pf_vector_t c; + + c.v[0] = +(a.v[0] - b.v[0]) * cos(b.v[2]) + (a.v[1] - b.v[1]) * sin(b.v[2]); + c.v[1] = -(a.v[0] - b.v[0]) * sin(b.v[2]) + (a.v[1] - b.v[1]) * cos(b.v[2]); + c.v[2] = a.v[2] - b.v[2]; + c.v[2] = atan2(sin(c.v[2]), cos(c.v[2])); + + return c; +} + + +// Return a zero matrix +pf_matrix_t pf_matrix_zero() +{ + int i, j; + pf_matrix_t c; + + for (i = 0; i < 3; i++) + for (j = 0; j < 3; j++) + c.m[i][j] = 0.0; + + return c; +} + + +// Check for NAN or INF in any component +int pf_matrix_finite(pf_matrix_t a) +{ + int i, j; + + for (i = 0; i < 3; i++) + for (j = 0; j < 3; j++) + if (!isfinite(a.m[i][j])) + return 0; + + return 1; +} + + +// Print a matrix +void pf_matrix_fprintf(pf_matrix_t a, FILE *file, const char *fmt) +{ + int i, j; + + for (i = 0; i < 3; i++) + { + for (j = 0; j < 3; j++) + { + fprintf(file, fmt, a.m[i][j]); + fprintf(file, " "); + } + fprintf(file, "\n"); + } + return; +} + + +/* +// Compute the matrix inverse +pf_matrix_t pf_matrix_inverse(pf_matrix_t a, double *det) +{ + double lndet; + int signum; + gsl_permutation *p; + gsl_matrix_view A, Ai; + + pf_matrix_t ai; + + A = gsl_matrix_view_array((double*) a.m, 3, 3); + Ai = gsl_matrix_view_array((double*) ai.m, 3, 3); + + // Do LU decomposition + p = gsl_permutation_alloc(3); + gsl_linalg_LU_decomp(&A.matrix, p, &signum); + + // Check for underflow + lndet = gsl_linalg_LU_lndet(&A.matrix); + if (lndet < -1000) + { + //printf("underflow in matrix inverse lndet = %f", lndet); + gsl_matrix_set_zero(&Ai.matrix); + } + else + { + // Compute inverse + gsl_linalg_LU_invert(&A.matrix, p, &Ai.matrix); + } + + gsl_permutation_free(p); + + if (det) + *det = exp(lndet); + + return ai; +} +*/ + + +// Decompose a covariance matrix [a] into a rotation matrix [r] and a diagonal +// matrix [d] such that a = r d r^T. +void pf_matrix_unitary(pf_matrix_t *r, pf_matrix_t *d, pf_matrix_t a) +{ + int i, j; + /* + gsl_matrix *aa; + gsl_vector *eval; + gsl_matrix *evec; + gsl_eigen_symmv_workspace *w; + + aa = gsl_matrix_alloc(3, 3); + eval = gsl_vector_alloc(3); + evec = gsl_matrix_alloc(3, 3); + */ + + double aa[3][3]; + double eval[3]; + double evec[3][3]; + + for (i = 0; i < 3; i++) + { + for (j = 0; j < 3; j++) + { + //gsl_matrix_set(aa, i, j, a.m[i][j]); + aa[i][j] = a.m[i][j]; + } + } + + // Compute eigenvectors/values + /* + w = gsl_eigen_symmv_alloc(3); + gsl_eigen_symmv(aa, eval, evec, w); + gsl_eigen_symmv_free(w); + */ + + eigen_decomposition(aa,evec,eval); + + *d = pf_matrix_zero(); + for (i = 0; i < 3; i++) + { + //d->m[i][i] = gsl_vector_get(eval, i); + d->m[i][i] = eval[i]; + for (j = 0; j < 3; j++) + { + //r->m[i][j] = gsl_matrix_get(evec, i, j); + r->m[i][j] = evec[i][j]; + } + } + + //gsl_matrix_free(evec); + //gsl_vector_free(eval); + //gsl_matrix_free(aa); + + return; +} + diff --git a/Localizations/Libraries/Ros/amcl/src/amcl/sensors/amcl_laser.cpp b/Localizations/Libraries/Ros/amcl/src/amcl/sensors/amcl_laser.cpp new file mode 100644 index 0000000..f8aa35c --- /dev/null +++ b/Localizations/Libraries/Ros/amcl/src/amcl/sensors/amcl_laser.cpp @@ -0,0 +1,510 @@ +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2000 Brian Gerkey & Kasper Stoy + * gerkey@usc.edu kaspers@robotics.usc.edu + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ +/////////////////////////////////////////////////////////////////////////// +// +// Desc: AMCL laser routines +// Author: Andrew Howard +// Date: 6 Feb 2003 +// CVS: $Id: amcl_laser.cc 7057 2008-10-02 00:44:06Z gbiggs $ +// +/////////////////////////////////////////////////////////////////////////// + +#include // required by Darwin +#include +#include +#include +#ifdef HAVE_UNISTD_H +#include +#endif + +#include "amcl/sensors/amcl_laser.h" + +using namespace amcl; + +//////////////////////////////////////////////////////////////////////////////// +// Default constructor +AMCLLaser::AMCLLaser(size_t max_beams, map_t* map) : AMCLSensor(), + max_samples(0), max_obs(0), + temp_obs(NULL) +{ + this->time = 0.0; + + this->max_beams = max_beams; + this->map = map; + + return; +} + +AMCLLaser::~AMCLLaser() +{ + if(temp_obs){ + for(int k=0; k < max_samples; k++){ + delete [] temp_obs[k]; + } + delete []temp_obs; + } +} + +void +AMCLLaser::SetModelBeam(double z_hit, + double z_short, + double z_max, + double z_rand, + double sigma_hit, + double lambda_short, + double chi_outlier) +{ + this->model_type = LASER_MODEL_BEAM; + this->z_hit = z_hit; + this->z_short = z_short; + this->z_max = z_max; + this->z_rand = z_rand; + this->sigma_hit = sigma_hit; + this->lambda_short = lambda_short; + this->chi_outlier = chi_outlier; +} + +void +AMCLLaser::SetModelLikelihoodField(double z_hit, + double z_rand, + double sigma_hit, + double max_occ_dist) +{ + this->model_type = LASER_MODEL_LIKELIHOOD_FIELD; + this->z_hit = z_hit; + this->z_rand = z_rand; + this->sigma_hit = sigma_hit; + + map_update_cspace(this->map, max_occ_dist); +} + +void +AMCLLaser::SetModelLikelihoodFieldProb(double z_hit, + double z_rand, + double sigma_hit, + double max_occ_dist, + bool do_beamskip, + double beam_skip_distance, + double beam_skip_threshold, + double beam_skip_error_threshold) +{ + this->model_type = LASER_MODEL_LIKELIHOOD_FIELD_PROB; + this->z_hit = z_hit; + this->z_rand = z_rand; + this->sigma_hit = sigma_hit; + this->do_beamskip = do_beamskip; + this->beam_skip_distance = beam_skip_distance; + this->beam_skip_threshold = beam_skip_threshold; + this->beam_skip_error_threshold = beam_skip_error_threshold; + map_update_cspace(this->map, max_occ_dist); +} + + +//////////////////////////////////////////////////////////////////////////////// +// Apply the laser sensor model +bool AMCLLaser::UpdateSensor(pf_t *pf, AMCLSensorData *data) +{ + if (this->max_beams < 2) + return false; + + // Apply the laser sensor model + if(this->model_type == LASER_MODEL_BEAM) + pf_update_sensor(pf, (pf_sensor_model_fn_t) BeamModel, data); + else if(this->model_type == LASER_MODEL_LIKELIHOOD_FIELD) + pf_update_sensor(pf, (pf_sensor_model_fn_t) LikelihoodFieldModel, data); + else if(this->model_type == LASER_MODEL_LIKELIHOOD_FIELD_PROB) + pf_update_sensor(pf, (pf_sensor_model_fn_t) LikelihoodFieldModelProb, data); + else + pf_update_sensor(pf, (pf_sensor_model_fn_t) BeamModel, data); + + return true; +} + + +//////////////////////////////////////////////////////////////////////////////// +// Determine the probability for the given pose +double AMCLLaser::BeamModel(AMCLLaserData *data, pf_sample_set_t* set) +{ + AMCLLaser *self; + int i, j, step; + double z, pz; + double p; + double map_range; + double obs_range, obs_bearing; + double total_weight; + pf_sample_t *sample; + pf_vector_t pose; + + self = (AMCLLaser*) data->sensor; + + total_weight = 0.0; + + // Compute the sample weights + for (j = 0; j < set->sample_count; j++) + { + sample = set->samples + j; + pose = sample->pose; + + // Take account of the laser pose relative to the robot + pose = pf_vector_coord_add(self->laser_pose, pose); + + p = 1.0; + + step = (data->range_count - 1) / (self->max_beams - 1); + for (i = 0; i < data->range_count; i += step) + { + obs_range = data->ranges[i][0]; + obs_bearing = data->ranges[i][1]; + + // Compute the range according to the map + map_range = map_calc_range(self->map, pose.v[0], pose.v[1], + pose.v[2] + obs_bearing, data->range_max); + pz = 0.0; + + // Part 1: good, but noisy, hit + z = obs_range - map_range; + pz += self->z_hit * exp(-(z * z) / (2 * self->sigma_hit * self->sigma_hit)); + + // Part 2: short reading from unexpected obstacle (e.g., a person) + if(z < 0) + pz += self->z_short * self->lambda_short * exp(-self->lambda_short*obs_range); + + // Part 3: Failure to detect obstacle, reported as max-range + if(obs_range == data->range_max) + pz += self->z_max * 1.0; + + // Part 4: Random measurements + if(obs_range < data->range_max) + pz += self->z_rand * 1.0/data->range_max; + + // TODO: outlier rejection for short readings + + assert(pz <= 1.0); + assert(pz >= 0.0); + // p *= pz; + // here we have an ad-hoc weighting scheme for combining beam probs + // works well, though... + p += pz*pz*pz; + } + + sample->weight *= p; + total_weight += sample->weight; + } + + return(total_weight); +} + +double AMCLLaser::LikelihoodFieldModel(AMCLLaserData *data, pf_sample_set_t* set) +{ + AMCLLaser *self; + int i, j, step; + double z, pz; + double p; + double obs_range, obs_bearing; + double total_weight; + pf_sample_t *sample; + pf_vector_t pose; + pf_vector_t hit; + + self = (AMCLLaser*) data->sensor; + + total_weight = 0.0; + + // Compute the sample weights + for (j = 0; j < set->sample_count; j++) + { + sample = set->samples + j; + pose = sample->pose; + + // Take account of the laser pose relative to the robot + pose = pf_vector_coord_add(self->laser_pose, pose); + + p = 1.0; + + // Pre-compute a couple of things + double z_hit_denom = 2 * self->sigma_hit * self->sigma_hit; + double z_rand_mult = 1.0/data->range_max; + + step = (data->range_count - 1) / (self->max_beams - 1); + + // Step size must be at least 1 + if(step < 1) + step = 1; + + for (i = 0; i < data->range_count; i += step) + { + obs_range = data->ranges[i][0]; + obs_bearing = data->ranges[i][1]; + + // This model ignores max range readings + if(obs_range >= data->range_max) + continue; + + // Check for NaN + if(obs_range != obs_range) + continue; + + pz = 0.0; + + // Compute the endpoint of the beam + hit.v[0] = pose.v[0] + obs_range * cos(pose.v[2] + obs_bearing); + hit.v[1] = pose.v[1] + obs_range * sin(pose.v[2] + obs_bearing); + + // Convert to map grid coords. + int mi, mj; + mi = MAP_GXWX(self->map, hit.v[0]); + mj = MAP_GYWY(self->map, hit.v[1]); + + // Part 1: Get distance from the hit to closest obstacle. + // Off-map penalized as max distance + if(!MAP_VALID(self->map, mi, mj)) + z = self->map->max_occ_dist; + else + z = self->map->cells[MAP_INDEX(self->map,mi,mj)].occ_dist; + // Gaussian model + // NOTE: this should have a normalization of 1/(sqrt(2pi)*sigma) + pz += self->z_hit * exp(-(z * z) / z_hit_denom); + // Part 2: random measurements + pz += self->z_rand * z_rand_mult; + + // TODO: outlier rejection for short readings + + assert(pz <= 1.0); + assert(pz >= 0.0); + // p *= pz; + // here we have an ad-hoc weighting scheme for combining beam probs + // works well, though... + p += pz*pz*pz; + } + + sample->weight *= p; + total_weight += sample->weight; + } + + return(total_weight); +} + +double AMCLLaser::LikelihoodFieldModelProb(AMCLLaserData *data, pf_sample_set_t* set) +{ + AMCLLaser *self; + int i, j, step; + double z, pz; + double log_p; + double obs_range, obs_bearing; + double total_weight; + pf_sample_t *sample; + pf_vector_t pose; + pf_vector_t hit; + + self = (AMCLLaser*) data->sensor; + + total_weight = 0.0; + + step = ceil((data->range_count) / static_cast(self->max_beams)); + + // Step size must be at least 1 + if(step < 1) + step = 1; + + // Pre-compute a couple of things + double z_hit_denom = 2 * self->sigma_hit * self->sigma_hit; + double z_rand_mult = 1.0/data->range_max; + + double max_dist_prob = exp(-(self->map->max_occ_dist * self->map->max_occ_dist) / z_hit_denom); + + //Beam skipping - ignores beams for which a majority of particles do not agree with the map + //prevents correct particles from getting down weighted because of unexpected obstacles + //such as humans + + bool do_beamskip = self->do_beamskip; + double beam_skip_distance = self->beam_skip_distance; + double beam_skip_threshold = self->beam_skip_threshold; + + //we only do beam skipping if the filter has converged + if(do_beamskip && !set->converged){ + do_beamskip = false; + } + + //we need a count the no of particles for which the beam agreed with the map + int *obs_count = new int[self->max_beams](); + + //we also need a mask of which observations to integrate (to decide which beams to integrate to all particles) + bool *obs_mask = new bool[self->max_beams](); + + int beam_ind = 0; + + //realloc indicates if we need to reallocate the temp data structure needed to do beamskipping + bool realloc = false; + + if(do_beamskip){ + if(self->max_obs < self->max_beams){ + realloc = true; + } + + if(self->max_samples < set->sample_count){ + realloc = true; + } + + if(realloc){ + self->reallocTempData(set->sample_count, self->max_beams); + fprintf(stderr, "Reallocing temp weights %d - %d\n", self->max_samples, self->max_obs); + } + } + + // Compute the sample weights + for (j = 0; j < set->sample_count; j++) + { + sample = set->samples + j; + pose = sample->pose; + + // Take account of the laser pose relative to the robot + pose = pf_vector_coord_add(self->laser_pose, pose); + + log_p = 0; + + beam_ind = 0; + + for (i = 0; i < data->range_count; i += step, beam_ind++) + { + obs_range = data->ranges[i][0]; + obs_bearing = data->ranges[i][1]; + + // This model ignores max range readings + if(obs_range >= data->range_max){ + continue; + } + + // Check for NaN + if(obs_range != obs_range){ + continue; + } + + pz = 0.0; + + // Compute the endpoint of the beam + hit.v[0] = pose.v[0] + obs_range * cos(pose.v[2] + obs_bearing); + hit.v[1] = pose.v[1] + obs_range * sin(pose.v[2] + obs_bearing); + + // Convert to map grid coords. + int mi, mj; + mi = MAP_GXWX(self->map, hit.v[0]); + mj = MAP_GYWY(self->map, hit.v[1]); + + // Part 1: Get distance from the hit to closest obstacle. + // Off-map penalized as max distance + + if(!MAP_VALID(self->map, mi, mj)){ + pz += self->z_hit * max_dist_prob; + } + else{ + z = self->map->cells[MAP_INDEX(self->map,mi,mj)].occ_dist; + if(z < beam_skip_distance){ + obs_count[beam_ind] += 1; + } + pz += self->z_hit * exp(-(z * z) / z_hit_denom); + } + + // Gaussian model + // NOTE: this should have a normalization of 1/(sqrt(2pi)*sigma) + + // Part 2: random measurements + pz += self->z_rand * z_rand_mult; + + assert(pz <= 1.0); + assert(pz >= 0.0); + + // TODO: outlier rejection for short readings + + if(!do_beamskip){ + log_p += log(pz); + } + else{ + self->temp_obs[j][beam_ind] = pz; + } + } + if(!do_beamskip){ + sample->weight *= exp(log_p); + total_weight += sample->weight; + } + } + + if(do_beamskip){ + int skipped_beam_count = 0; + for (beam_ind = 0; beam_ind < self->max_beams; beam_ind++){ + if((obs_count[beam_ind] / static_cast(set->sample_count)) > beam_skip_threshold){ + obs_mask[beam_ind] = true; + } + else{ + obs_mask[beam_ind] = false; + skipped_beam_count++; + } + } + + //we check if there is at least a critical number of beams that agreed with the map + //otherwise it probably indicates that the filter converged to a wrong solution + //if that's the case we integrate all the beams and hope the filter might converge to + //the right solution + bool error = false; + + if(skipped_beam_count >= (beam_ind * self->beam_skip_error_threshold)){ + fprintf(stderr, "Over %f%% of the observations were not in the map - pf may have converged to wrong pose - integrating all observations\n", (100 * self->beam_skip_error_threshold)); + error = true; + } + + for (j = 0; j < set->sample_count; j++) + { + sample = set->samples + j; + pose = sample->pose; + + log_p = 0; + + for (beam_ind = 0; beam_ind < self->max_beams; beam_ind++){ + if(error || obs_mask[beam_ind]){ + log_p += log(self->temp_obs[j][beam_ind]); + } + } + + sample->weight *= exp(log_p); + + total_weight += sample->weight; + } + } + + delete [] obs_count; + delete [] obs_mask; + return(total_weight); +} + +void AMCLLaser::reallocTempData(int new_max_samples, int new_max_obs){ + if(temp_obs){ + for(int k=0; k < max_samples; k++){ + delete [] temp_obs[k]; + } + delete []temp_obs; + } + max_obs = new_max_obs; + max_samples = fmax(max_samples, new_max_samples); + + temp_obs = new double*[max_samples](); + for(int k=0; k < max_samples; k++){ + temp_obs[k] = new double[max_obs](); + } +} diff --git a/Localizations/Libraries/Ros/amcl/src/amcl/sensors/amcl_odom.cpp b/Localizations/Libraries/Ros/amcl/src/amcl/sensors/amcl_odom.cpp new file mode 100644 index 0000000..5aa846f --- /dev/null +++ b/Localizations/Libraries/Ros/amcl/src/amcl/sensors/amcl_odom.cpp @@ -0,0 +1,310 @@ +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2000 Brian Gerkey & Kasper Stoy + * gerkey@usc.edu kaspers@robotics.usc.edu + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ +/////////////////////////////////////////////////////////////////////////// +// +// Desc: AMCL odometry routines +// Author: Andrew Howard +// Date: 6 Feb 2003 +// CVS: $Id: amcl_odom.cc 7057 2008-10-02 00:44:06Z gbiggs $ +// +/////////////////////////////////////////////////////////////////////////// + +#include + +#include // required by Darwin +#include + +#include "amcl/sensors/amcl_odom.h" + +using namespace amcl; + +static double +normalize(double z) +{ + return atan2(sin(z),cos(z)); +} +static double +angle_diff(double a, double b) +{ + double d1, d2; + a = normalize(a); + b = normalize(b); + d1 = a-b; + d2 = 2*M_PI - fabs(d1); + if(d1 > 0) + d2 *= -1.0; + if(fabs(d1) < fabs(d2)) + return(d1); + else + return(d2); +} + +//////////////////////////////////////////////////////////////////////////////// +// Default constructor +AMCLOdom::AMCLOdom() : AMCLSensor() +{ + this->time = 0.0; +} + +void +AMCLOdom::SetModelDiff(double alpha1, + double alpha2, + double alpha3, + double alpha4) +{ + this->model_type = ODOM_MODEL_DIFF; + this->alpha1 = alpha1; + this->alpha2 = alpha2; + this->alpha3 = alpha3; + this->alpha4 = alpha4; +} + +void +AMCLOdom::SetModelOmni(double alpha1, + double alpha2, + double alpha3, + double alpha4, + double alpha5) +{ + this->model_type = ODOM_MODEL_OMNI; + this->alpha1 = alpha1; + this->alpha2 = alpha2; + this->alpha3 = alpha3; + this->alpha4 = alpha4; + this->alpha5 = alpha5; +} + +void +AMCLOdom::SetModel( odom_model_t type, + double alpha1, + double alpha2, + double alpha3, + double alpha4, + double alpha5 ) +{ + this->model_type = type; + this->alpha1 = alpha1; + this->alpha2 = alpha2; + this->alpha3 = alpha3; + this->alpha4 = alpha4; + this->alpha5 = alpha5; +} + +//////////////////////////////////////////////////////////////////////////////// +// Apply the action model +bool AMCLOdom::UpdateAction(pf_t *pf, AMCLSensorData *data) +{ + AMCLOdomData *ndata; + ndata = (AMCLOdomData*) data; + + // Compute the new sample poses + pf_sample_set_t *set; + + set = pf->sets + pf->current_set; + pf_vector_t old_pose = pf_vector_sub(ndata->pose, ndata->delta); + + switch( this->model_type ) + { + case ODOM_MODEL_OMNI: + { + double delta_trans, delta_rot, delta_bearing; + double delta_trans_hat, delta_rot_hat, delta_strafe_hat; + + delta_trans = sqrt(ndata->delta.v[0]*ndata->delta.v[0] + + ndata->delta.v[1]*ndata->delta.v[1]); + delta_rot = ndata->delta.v[2]; + + // Precompute a couple of things + double trans_hat_stddev = (alpha3 * (delta_trans*delta_trans) + + alpha1 * (delta_rot*delta_rot)); + double rot_hat_stddev = (alpha4 * (delta_rot*delta_rot) + + alpha2 * (delta_trans*delta_trans)); + double strafe_hat_stddev = (alpha1 * (delta_rot*delta_rot) + + alpha5 * (delta_trans*delta_trans)); + + for (int i = 0; i < set->sample_count; i++) + { + pf_sample_t* sample = set->samples + i; + + delta_bearing = angle_diff(atan2(ndata->delta.v[1], ndata->delta.v[0]), + old_pose.v[2]) + sample->pose.v[2]; + double cs_bearing = cos(delta_bearing); + double sn_bearing = sin(delta_bearing); + + // Sample pose differences + delta_trans_hat = delta_trans + pf_ran_gaussian(trans_hat_stddev); + delta_rot_hat = delta_rot + pf_ran_gaussian(rot_hat_stddev); + delta_strafe_hat = 0 + pf_ran_gaussian(strafe_hat_stddev); + // Apply sampled update to particle pose + sample->pose.v[0] += (delta_trans_hat * cs_bearing + + delta_strafe_hat * sn_bearing); + sample->pose.v[1] += (delta_trans_hat * sn_bearing - + delta_strafe_hat * cs_bearing); + sample->pose.v[2] += delta_rot_hat ; + } + } + break; + case ODOM_MODEL_DIFF: + { + // Implement sample_motion_odometry (Prob Rob p 136) + double delta_rot1, delta_trans, delta_rot2; + double delta_rot1_hat, delta_trans_hat, delta_rot2_hat; + double delta_rot1_noise, delta_rot2_noise; + + // Avoid computing a bearing from two poses that are extremely near each + // other (happens on in-place rotation). + if(sqrt(ndata->delta.v[1]*ndata->delta.v[1] + + ndata->delta.v[0]*ndata->delta.v[0]) < 0.01) + delta_rot1 = 0.0; + else + delta_rot1 = angle_diff(atan2(ndata->delta.v[1], ndata->delta.v[0]), + old_pose.v[2]); + delta_trans = sqrt(ndata->delta.v[0]*ndata->delta.v[0] + + ndata->delta.v[1]*ndata->delta.v[1]); + delta_rot2 = angle_diff(ndata->delta.v[2], delta_rot1); + + // We want to treat backward and forward motion symmetrically for the + // noise model to be applied below. The standard model seems to assume + // forward motion. + delta_rot1_noise = std::min(fabs(angle_diff(delta_rot1,0.0)), + fabs(angle_diff(delta_rot1,M_PI))); + delta_rot2_noise = std::min(fabs(angle_diff(delta_rot2,0.0)), + fabs(angle_diff(delta_rot2,M_PI))); + + for (int i = 0; i < set->sample_count; i++) + { + pf_sample_t* sample = set->samples + i; + + // Sample pose differences + delta_rot1_hat = angle_diff(delta_rot1, + pf_ran_gaussian(this->alpha1*delta_rot1_noise*delta_rot1_noise + + this->alpha2*delta_trans*delta_trans)); + delta_trans_hat = delta_trans - + pf_ran_gaussian(this->alpha3*delta_trans*delta_trans + + this->alpha4*delta_rot1_noise*delta_rot1_noise + + this->alpha4*delta_rot2_noise*delta_rot2_noise); + delta_rot2_hat = angle_diff(delta_rot2, + pf_ran_gaussian(this->alpha1*delta_rot2_noise*delta_rot2_noise + + this->alpha2*delta_trans*delta_trans)); + + // Apply sampled update to particle pose + sample->pose.v[0] += delta_trans_hat * + cos(sample->pose.v[2] + delta_rot1_hat); + sample->pose.v[1] += delta_trans_hat * + sin(sample->pose.v[2] + delta_rot1_hat); + sample->pose.v[2] += delta_rot1_hat + delta_rot2_hat; + } + } + break; + case ODOM_MODEL_OMNI_CORRECTED: + { + double delta_trans, delta_rot, delta_bearing; + double delta_trans_hat, delta_rot_hat, delta_strafe_hat; + + delta_trans = sqrt(ndata->delta.v[0]*ndata->delta.v[0] + + ndata->delta.v[1]*ndata->delta.v[1]); + delta_rot = ndata->delta.v[2]; + + // Precompute a couple of things + double trans_hat_stddev = sqrt( alpha3 * (delta_trans*delta_trans) + + alpha4 * (delta_rot*delta_rot) ); + double rot_hat_stddev = sqrt( alpha1 * (delta_rot*delta_rot) + + alpha2 * (delta_trans*delta_trans) ); + double strafe_hat_stddev = sqrt( alpha4 * (delta_rot*delta_rot) + + alpha5 * (delta_trans*delta_trans) ); + + for (int i = 0; i < set->sample_count; i++) + { + pf_sample_t* sample = set->samples + i; + + delta_bearing = angle_diff(atan2(ndata->delta.v[1], ndata->delta.v[0]), + old_pose.v[2]) + sample->pose.v[2]; + double cs_bearing = cos(delta_bearing); + double sn_bearing = sin(delta_bearing); + + // Sample pose differences + delta_trans_hat = delta_trans + pf_ran_gaussian(trans_hat_stddev); + delta_rot_hat = delta_rot + pf_ran_gaussian(rot_hat_stddev); + delta_strafe_hat = 0 + pf_ran_gaussian(strafe_hat_stddev); + // Apply sampled update to particle pose + sample->pose.v[0] += (delta_trans_hat * cs_bearing + + delta_strafe_hat * sn_bearing); + sample->pose.v[1] += (delta_trans_hat * sn_bearing - + delta_strafe_hat * cs_bearing); + sample->pose.v[2] += delta_rot_hat ; + } + } + break; + case ODOM_MODEL_DIFF_CORRECTED: + { + // Implement sample_motion_odometry (Prob Rob p 136) + double delta_rot1, delta_trans, delta_rot2; + double delta_rot1_hat, delta_trans_hat, delta_rot2_hat; + double delta_rot1_noise, delta_rot2_noise; + + // Avoid computing a bearing from two poses that are extremely near each + // other (happens on in-place rotation). + if(sqrt(ndata->delta.v[1]*ndata->delta.v[1] + + ndata->delta.v[0]*ndata->delta.v[0]) < 0.01) + delta_rot1 = 0.0; + else + delta_rot1 = angle_diff(atan2(ndata->delta.v[1], ndata->delta.v[0]), + old_pose.v[2]); + delta_trans = sqrt(ndata->delta.v[0]*ndata->delta.v[0] + + ndata->delta.v[1]*ndata->delta.v[1]); + delta_rot2 = angle_diff(ndata->delta.v[2], delta_rot1); + + // We want to treat backward and forward motion symmetrically for the + // noise model to be applied below. The standard model seems to assume + // forward motion. + delta_rot1_noise = std::min(fabs(angle_diff(delta_rot1,0.0)), + fabs(angle_diff(delta_rot1,M_PI))); + delta_rot2_noise = std::min(fabs(angle_diff(delta_rot2,0.0)), + fabs(angle_diff(delta_rot2,M_PI))); + + for (int i = 0; i < set->sample_count; i++) + { + pf_sample_t* sample = set->samples + i; + + // Sample pose differences + delta_rot1_hat = angle_diff(delta_rot1, + pf_ran_gaussian(sqrt(this->alpha1*delta_rot1_noise*delta_rot1_noise + + this->alpha2*delta_trans*delta_trans))); + delta_trans_hat = delta_trans - + pf_ran_gaussian(sqrt(this->alpha3*delta_trans*delta_trans + + this->alpha4*delta_rot1_noise*delta_rot1_noise + + this->alpha4*delta_rot2_noise*delta_rot2_noise)); + delta_rot2_hat = angle_diff(delta_rot2, + pf_ran_gaussian(sqrt(this->alpha1*delta_rot2_noise*delta_rot2_noise + + this->alpha2*delta_trans*delta_trans))); + + // Apply sampled update to particle pose + sample->pose.v[0] += delta_trans_hat * + cos(sample->pose.v[2] + delta_rot1_hat); + sample->pose.v[1] += delta_trans_hat * + sin(sample->pose.v[2] + delta_rot1_hat); + sample->pose.v[2] += delta_rot1_hat + delta_rot2_hat; + } + } + break; + } + return true; +} diff --git a/Localizations/Libraries/Ros/amcl/src/amcl/sensors/amcl_sensor.cpp b/Localizations/Libraries/Ros/amcl/src/amcl/sensors/amcl_sensor.cpp new file mode 100644 index 0000000..48e190d --- /dev/null +++ b/Localizations/Libraries/Ros/amcl/src/amcl/sensors/amcl_sensor.cpp @@ -0,0 +1,95 @@ +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2000 Brian Gerkey & Kasper Stoy + * gerkey@usc.edu kaspers@robotics.usc.edu + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ +/////////////////////////////////////////////////////////////////////////// +// +// Desc: AMCL sensor +// Author: Andrew Howard +// Date: 6 Feb 2003 +// CVS: $Id: amcl_sensor.cc 7057 2008-10-02 00:44:06Z gbiggs $ +// +/////////////////////////////////////////////////////////////////////////// + + +#include "amcl/sensors/amcl_sensor.h" + +using namespace amcl; + +//////////////////////////////////////////////////////////////////////////////// +// Default constructor +AMCLSensor::AMCLSensor() +{ + return; +} + +AMCLSensor::~AMCLSensor() +{ +} + +//////////////////////////////////////////////////////////////////////////////// +// Apply the action model +bool AMCLSensor::UpdateAction(pf_t *pf, AMCLSensorData *data) +{ + return false; +} + + +//////////////////////////////////////////////////////////////////////////////// +// Initialize the filter +bool AMCLSensor::InitSensor(pf_t *pf, AMCLSensorData *data) +{ + return false; +} + + +//////////////////////////////////////////////////////////////////////////////// +// Apply the sensor model +bool AMCLSensor::UpdateSensor(pf_t *pf, AMCLSensorData *data) +{ + return false; +} + + +#ifdef INCLUDE_RTKGUI + +//////////////////////////////////////////////////////////////////////////////// +// Setup the GUI +void AMCLSensor::SetupGUI(rtk_canvas_t *canvas, rtk_fig_t *robot_fig) +{ + return; +} + + +//////////////////////////////////////////////////////////////////////////////// +// Shutdown the GUI +void AMCLSensor::ShutdownGUI(rtk_canvas_t *canvas, rtk_fig_t *robot_fig) +{ + return; +} + + +//////////////////////////////////////////////////////////////////////////////// +// Draw sensor data +void AMCLSensor::UpdateGUI(rtk_canvas_t *canvas, rtk_fig_t *robot_fig, AMCLSensorData *data) +{ + return; +} + +#endif diff --git a/Localizations/Libraries/Ros/amcl/src/amcl_node.cpp b/Localizations/Libraries/Ros/amcl/src/amcl_node.cpp new file mode 100644 index 0000000..5883922 --- /dev/null +++ b/Localizations/Libraries/Ros/amcl/src/amcl_node.cpp @@ -0,0 +1,72 @@ +/* + * Copyright (c) 2008, Willow Garage, Inc. + * All rights reserved. + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ + +/* Author: Brian Gerkey */ + +// Signal handling +#include +#include + + +boost::shared_ptr amcl_node_ptr; + +void sigintHandler(int sig) +{ + // Save latest pose as we're shutting down. + geometry_msgs::PoseWithCovarianceStamped amcl_pose; + amcl_node_ptr->savePoseToServer(amcl_pose); + ros::shutdown(); +} + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "amcl"); + ros::NodeHandle nh; + + // Override default sigint handler + signal(SIGINT, sigintHandler); + + // Make our node available to sigintHandler + amcl_node_ptr.reset(new amcl::Amcl()); + + if (argc == 1) + { + // run using ROS input + ros::spin(); + } + else if ((argc >= 3) && (std::string(argv[1]) == "--run-from-bag")) + { + if (argc == 3) + { + amcl_node_ptr->runFromBag(argv[2]); + } + else if ((argc == 4) && (std::string(argv[3]) == "--global-localization")) + { + amcl_node_ptr->runFromBag(argv[2], true); + } + } + + // Without this, our boost locks are not shut down nicely + amcl_node_ptr.reset(); + + // To quote Morgan, Hooray! + return(0); +} + diff --git a/Localizations/Libraries/Ros/amcl/test/basic_localization.py b/Localizations/Libraries/Ros/amcl/test/basic_localization.py new file mode 100644 index 0000000..4f5fa4a --- /dev/null +++ b/Localizations/Libraries/Ros/amcl/test/basic_localization.py @@ -0,0 +1,101 @@ +#!/usr/bin/env python + +from __future__ import print_function + +import sys +import time +from math import fmod, pi + +import unittest +import rospy +import rostest + +import tf2_py as tf2 +import tf2_ros +import PyKDL +from std_srvs.srv import Empty + + +class TestBasicLocalization(unittest.TestCase): + def setUp(self): + self.tf = None + self.target_x = None + self.target_y = None + self.target_a = None + self.tfBuffer = None + + def print_pose_diff(self): + a_curr = self.compute_angle() + a_diff = self.wrap_angle(a_curr - self.target_a) + print('Curr:\t %16.6f %16.6f %16.6f' % (self.tf.translation.x, self.tf.translation.y, a_curr)) + print('Target:\t %16.6f %16.6f %16.6f' % (self.target_x, self.target_y, self.target_a)) + print('Diff:\t %16.6f %16.6f %16.6f' % ( + abs(self.tf.translation.x - self.target_x), abs(self.tf.translation.y - self.target_y), a_diff)) + + def get_pose(self): + try: + tf_stamped = self.tfBuffer.lookup_transform("map", "base_link", rospy.Time()) + self.tf = tf_stamped.transform + self.print_pose_diff() + except (tf2.LookupException, tf2.ExtrapolationException): + pass + + @staticmethod + def wrap_angle(angle): + """ + Wrap angle to [-pi, pi) + :param angle: Angle to be wrapped + :return: wrapped angle + """ + angle += pi + while angle < 0: + angle += 2*pi + return fmod(angle, 2*pi) - pi + + def compute_angle(self): + rot = self.tf.rotation + a_curr = PyKDL.Rotation.Quaternion(rot.x, rot.y, rot.z, rot.w).GetRPY()[2] + a_diff = self.wrap_angle(a_curr - self.target_a) + return self.target_a + a_diff + + def test_basic_localization(self): + global_localization = int(sys.argv[1]) + self.target_x = float(sys.argv[2]) + self.target_y = float(sys.argv[3]) + self.target_a = self.wrap_angle(float(sys.argv[4])) + tolerance_d = float(sys.argv[5]) + tolerance_a = float(sys.argv[6]) + target_time = float(sys.argv[7]) + + rospy.init_node('test', anonymous=True) + while rospy.rostime.get_time() == 0.0: + print('Waiting for initial time publication') + time.sleep(0.1) + + if global_localization == 1: + print('Waiting for service global_localization') + rospy.wait_for_service('global_localization') + global_localization = rospy.ServiceProxy('global_localization', Empty) + global_localization() + + start_time = rospy.rostime.get_time() + self.tfBuffer = tf2_ros.Buffer() + listener = tf2_ros.TransformListener(self.tfBuffer) + + while (rospy.rostime.get_time() - start_time) < target_time: + print('Waiting for end time %.6f (current: %.6f)' % (target_time, (rospy.rostime.get_time() - start_time))) + self.get_pose() + time.sleep(0.1) + + print("Final pose:") + self.get_pose() + a_curr = self.compute_angle() + + self.assertNotEqual(self.tf, None) + self.assertAlmostEqual(self.tf.translation.x, self.target_x, delta=tolerance_d) + self.assertAlmostEqual(self.tf.translation.y, self.target_y, delta=tolerance_d) + self.assertAlmostEqual(a_curr, self.target_a, delta=tolerance_a) + + +if __name__ == '__main__': + rostest.run('amcl', 'amcl_localization', TestBasicLocalization, sys.argv) \ No newline at end of file diff --git a/Localizations/Libraries/Ros/amcl/test/basic_localization_stage.xml b/Localizations/Libraries/Ros/amcl/test/basic_localization_stage.xml new file mode 100644 index 0000000..3184121 --- /dev/null +++ b/Localizations/Libraries/Ros/amcl/test/basic_localization_stage.xml @@ -0,0 +1,51 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Localizations/Libraries/Ros/amcl/test/global_localization_stage.xml b/Localizations/Libraries/Ros/amcl/test/global_localization_stage.xml new file mode 100644 index 0000000..8fca2ae --- /dev/null +++ b/Localizations/Libraries/Ros/amcl/test/global_localization_stage.xml @@ -0,0 +1,48 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Localizations/Libraries/Ros/amcl/test/rosie_multilaser.xml b/Localizations/Libraries/Ros/amcl/test/rosie_multilaser.xml new file mode 100644 index 0000000..ade665a --- /dev/null +++ b/Localizations/Libraries/Ros/amcl/test/rosie_multilaser.xml @@ -0,0 +1,51 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Localizations/Libraries/Ros/amcl/test/set_initial_pose.xml b/Localizations/Libraries/Ros/amcl/test/set_initial_pose.xml new file mode 100644 index 0000000..58d8432 --- /dev/null +++ b/Localizations/Libraries/Ros/amcl/test/set_initial_pose.xml @@ -0,0 +1,54 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Localizations/Libraries/Ros/amcl/test/set_initial_pose_delayed.xml b/Localizations/Libraries/Ros/amcl/test/set_initial_pose_delayed.xml new file mode 100644 index 0000000..cae2ec3 --- /dev/null +++ b/Localizations/Libraries/Ros/amcl/test/set_initial_pose_delayed.xml @@ -0,0 +1,58 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Localizations/Libraries/Ros/amcl/test/set_pose.py b/Localizations/Libraries/Ros/amcl/test/set_pose.py new file mode 100644 index 0000000..74592ed --- /dev/null +++ b/Localizations/Libraries/Ros/amcl/test/set_pose.py @@ -0,0 +1,46 @@ +#!/usr/bin/env python + +import rospy + +import math +import PyKDL +from geometry_msgs.msg import PoseWithCovarianceStamped + + +class PoseSetter(rospy.SubscribeListener): + def __init__(self, pose, stamp, publish_time): + self.pose = pose + self.stamp = stamp + self.publish_time = publish_time + + def peer_subscribe(self, topic_name, topic_publish, peer_publish): + p = PoseWithCovarianceStamped() + p.header.frame_id = "map" + p.header.stamp = self.stamp + p.pose.pose.position.x = self.pose[0] + p.pose.pose.position.y = self.pose[1] + (p.pose.pose.orientation.x, + p.pose.pose.orientation.y, + p.pose.pose.orientation.z, + p.pose.pose.orientation.w) = PyKDL.Rotation.RPY(0, 0, self.pose[2]).GetQuaternion() + p.pose.covariance[6*0+0] = 0.5 * 0.5 + p.pose.covariance[6*1+1] = 0.5 * 0.5 + p.pose.covariance[6*3+3] = math.pi/12.0 * math.pi/12.0 + # wait for the desired publish time + while rospy.get_rostime() < self.publish_time: + rospy.sleep(0.01) + peer_publish(p) + + +if __name__ == '__main__': + pose = list(map(float, rospy.myargv()[1:4])) + t_stamp = rospy.Time() + t_publish = rospy.Time() + if len(rospy.myargv()) > 4: + t_stamp = rospy.Time.from_sec(float(rospy.myargv()[4])) + if len(rospy.myargv()) > 5: + t_publish = rospy.Time.from_sec(float(rospy.myargv()[5])) + rospy.init_node('pose_setter', anonymous=True) + rospy.loginfo("Going to publish pose {} with stamp {} at {}".format(pose, t_stamp.to_sec(), t_publish.to_sec())) + pub = rospy.Publisher("initialpose", PoseWithCovarianceStamped, PoseSetter(pose, stamp=t_stamp, publish_time=t_publish), queue_size=1) + rospy.spin() diff --git a/Localizations/Libraries/Ros/amcl/test/small_loop_crazy_driving_prg.xml b/Localizations/Libraries/Ros/amcl/test/small_loop_crazy_driving_prg.xml new file mode 100644 index 0000000..7169c1c --- /dev/null +++ b/Localizations/Libraries/Ros/amcl/test/small_loop_crazy_driving_prg.xml @@ -0,0 +1,51 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Localizations/Libraries/Ros/amcl/test/small_loop_crazy_driving_prg_corrected.xml b/Localizations/Libraries/Ros/amcl/test/small_loop_crazy_driving_prg_corrected.xml new file mode 100644 index 0000000..ce4f2d4 --- /dev/null +++ b/Localizations/Libraries/Ros/amcl/test/small_loop_crazy_driving_prg_corrected.xml @@ -0,0 +1,51 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Localizations/Libraries/Ros/amcl/test/small_loop_prf.xml b/Localizations/Libraries/Ros/amcl/test/small_loop_prf.xml new file mode 100644 index 0000000..eadbd10 --- /dev/null +++ b/Localizations/Libraries/Ros/amcl/test/small_loop_prf.xml @@ -0,0 +1,48 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Localizations/Libraries/Ros/amcl/test/texas_greenroom_loop.xml b/Localizations/Libraries/Ros/amcl/test/texas_greenroom_loop.xml new file mode 100644 index 0000000..7fea530 --- /dev/null +++ b/Localizations/Libraries/Ros/amcl/test/texas_greenroom_loop.xml @@ -0,0 +1,41 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Localizations/Libraries/Ros/amcl/test/texas_greenroom_loop_corrected.xml b/Localizations/Libraries/Ros/amcl/test/texas_greenroom_loop_corrected.xml new file mode 100644 index 0000000..90ee389 --- /dev/null +++ b/Localizations/Libraries/Ros/amcl/test/texas_greenroom_loop_corrected.xml @@ -0,0 +1,41 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Localizations/Libraries/Ros/amcl/test/texas_willow_hallway_loop.xml b/Localizations/Libraries/Ros/amcl/test/texas_willow_hallway_loop.xml new file mode 100644 index 0000000..c223308 --- /dev/null +++ b/Localizations/Libraries/Ros/amcl/test/texas_willow_hallway_loop.xml @@ -0,0 +1,41 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Localizations/Libraries/Ros/amcl/test/texas_willow_hallway_loop_corrected.xml b/Localizations/Libraries/Ros/amcl/test/texas_willow_hallway_loop_corrected.xml new file mode 100644 index 0000000..bd01194 --- /dev/null +++ b/Localizations/Libraries/Ros/amcl/test/texas_willow_hallway_loop_corrected.xml @@ -0,0 +1,41 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Localizations/Libraries/Ros/hector_slam/.gitignore b/Localizations/Libraries/Ros/hector_slam/.gitignore new file mode 100644 index 0000000..248cbd0 --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/.gitignore @@ -0,0 +1,10 @@ +bin/ +build/ +lib/ +msg_gen/ +srv_gen/ +hector_mapping/src/hector_mapping/ +hector_nav_msgs/src/hector_nav_msgs/ +.idea/ +.vscode/ +cmake-build-*/ diff --git a/Localizations/Libraries/Ros/hector_slam/README.txt b/Localizations/Libraries/Ros/hector_slam/README.txt new file mode 100644 index 0000000..98927db --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/README.txt @@ -0,0 +1,3 @@ +# hector_slam + +See the ROS Wiki for documentation: http://wiki.ros.org/hector_slam diff --git a/Localizations/Libraries/Ros/hector_slam/hector_compressed_map_transport/CHANGELOG.rst b/Localizations/Libraries/Ros/hector_slam/hector_compressed_map_transport/CHANGELOG.rst new file mode 100644 index 0000000..90a9a79 --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_compressed_map_transport/CHANGELOG.rst @@ -0,0 +1,50 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package hector_compressed_map_transport +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.5.2 (2021-04-08) +------------------ +* Fix out of bounds access + Fixes `#52 `_ and `#70 `_ +* Contributors: Stefan Fabian + +0.5.1 (2021-01-15) +------------------ + +0.5.0 (2020-12-17) +------------------ +* Moved hector_geotiff launch files to separate package to solve cyclic dependency. + Clean up for noetic release. +* Bump CMake version to avoid CMP0048 warning +* Contributors: Marius Schnaubelt, Stefan Fabian + +0.4.1 (2020-05-15) +------------------ + +0.3.6 (2019-10-31) +------------------ + +0.3.5 (2016-06-24) +------------------ +* Use the FindEigen3.cmake module provided by Eigen +* Contributors: Johannes Meyer + +0.3.4 (2015-11-07) +------------------ + +0.3.3 (2014-06-15) +------------------ +* fixed cmake find for eigen in indigo +* fixed libopencv-dev rosdep key +* Contributors: Alexander Stumpf, Johannes Meyer + +0.3.2 (2014-03-30) +------------------ + +0.3.1 (2013-10-09) +------------------ +* added changelogs + +0.3.0 (2013-08-08) +------------------ +* catkinized hector_slam diff --git a/Localizations/Libraries/Ros/hector_slam/hector_compressed_map_transport/CMakeLists.txt b/Localizations/Libraries/Ros/hector_slam/hector_compressed_map_transport/CMakeLists.txt new file mode 100644 index 0000000..846ce5a --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_compressed_map_transport/CMakeLists.txt @@ -0,0 +1,40 @@ +cmake_minimum_required(VERSION 3.0.2) +project(hector_compressed_map_transport) + +find_package(catkin REQUIRED COMPONENTS cv_bridge geometry_msgs hector_map_tools image_transport nav_msgs sensor_msgs) + +find_package(Eigen3 REQUIRED) +find_package(OpenCV REQUIRED) + +catkin_package( + CATKIN_DEPENDS geometry_msgs nav_msgs sensor_msgs +) + +########### +## Build ## +########### + +include_directories( + ${catkin_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIRS} + ${OpenCV_INCLUDE_DIRS} +) + +add_executable(map_to_image_node src/map_to_image_node.cpp) +target_link_libraries(map_to_image_node + ${catkin_LIBRARIES} + ${OpenCV_LIBRARIES} +) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +install(TARGETS map_to_image_node + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) diff --git a/Localizations/Libraries/Ros/hector_slam/hector_compressed_map_transport/package.xml b/Localizations/Libraries/Ros/hector_slam/hector_compressed_map_transport/package.xml new file mode 100644 index 0000000..3a2b187 --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_compressed_map_transport/package.xml @@ -0,0 +1,65 @@ + + + hector_compressed_map_transport + 0.5.2 + + hector_compressed_map_transport provides means for transporting compressed map data through the use of image_transport. + + + + Johannes Meyer + + + + + + BSD + + + + + + http://ros.org/wiki/hector_compressed_map_transport + + + + + Stefan Kohlbrecher + + + + + + + + + + + + + + catkin + cv_bridge + geometry_msgs + hector_map_tools + image_transport + nav_msgs + sensor_msgs + eigen + cv_bridge + geometry_msgs + hector_map_tools + image_transport + nav_msgs + sensor_msgs + eigen + + + + + + + + + + diff --git a/Localizations/Libraries/Ros/hector_slam/hector_compressed_map_transport/src/map_to_image_node.cpp b/Localizations/Libraries/Ros/hector_slam/hector_compressed_map_transport/src/map_to_image_node.cpp new file mode 100644 index 0000000..167480d --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_compressed_map_transport/src/map_to_image_node.cpp @@ -0,0 +1,270 @@ +//================================================================================================= +// Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt +// All rights reserved. + +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of the Simulation, Systems Optimization and Robotics +// group, TU Darmstadt nor the names of its contributors may be used to +// endorse or promote products derived from this software without +// specific prior written permission. + +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +//================================================================================================= + +#include "ros/ros.h" + +#include +#include +#include +#include + +#include +#include +#include + +#include + +using namespace std; + +/** + * @brief This node provides occupancy grid maps as images via image_transport, so the transmission consumes less bandwidth. + * The provided code is a incomplete proof of concept. + */ +class MapAsImageProvider +{ +public: + MapAsImageProvider() + : pn_("~") + { + + image_transport_ = new image_transport::ImageTransport(n_); + image_transport_publisher_full_ = image_transport_->advertise("map_image/full", 1); + image_transport_publisher_tile_ = image_transport_->advertise("map_image/tile", 1); + + pose_sub_ = n_.subscribe("pose", 1, &MapAsImageProvider::poseCallback, this); + map_sub_ = n_.subscribe("map", 1, &MapAsImageProvider::mapCallback, this); + + //Which frame_id makes sense? + cv_img_full_.header.frame_id = "map_image"; + cv_img_full_.encoding = sensor_msgs::image_encodings::MONO8; + + cv_img_tile_.header.frame_id = "map_image"; + cv_img_tile_.encoding = sensor_msgs::image_encodings::MONO8; + + //Fixed cell width for tile based image, use dynamic_reconfigure for this later + p_size_tiled_map_image_x_ = 64; + p_size_tiled_map_image_y_ = 64; + + ROS_INFO("Map to Image node started."); + } + + ~MapAsImageProvider() + { + delete image_transport_; + } + + //We assume the robot position is available as a PoseStamped here (querying tf would be the more general option) + void poseCallback(const geometry_msgs::PoseStampedConstPtr& pose) + { + pose_ptr_ = pose; + } + + //The map->image conversion runs every time a new map is received at the moment + void mapCallback(const nav_msgs::OccupancyGridConstPtr& map) + { + int size_x = map->info.width; + int size_y = map->info.height; + + if ((size_x < 3) || (size_y < 3) ){ + ROS_INFO("Map size is only x: %d, y: %d . Not running map to image conversion", size_x, size_y); + return; + } + + // Only if someone is subscribed to it, do work and publish full map image + if (image_transport_publisher_full_.getNumSubscribers() > 0){ + cv::Mat* map_mat = &cv_img_full_.image; + + // resize cv image if it doesn't have the same dimensions as the map + if ( (map_mat->rows != size_y) && (map_mat->cols != size_x)){ + *map_mat = cv::Mat(size_y, size_x, CV_8U); + } + + const std::vector& map_data (map->data); + + unsigned char *map_mat_data_p=(unsigned char*) map_mat->data; + + //We have to flip around the y axis, y for image starts at the top and y for map at the bottom + int size_y_rev = size_y-1; + + for (int y = size_y_rev; y >= 0; --y){ + + int idx_map_y = size_x * (size_y_rev - y); + int idx_img_y = size_x * y; + + for (int x = 0; x < size_x; ++x){ + + int idx = idx_img_y + x; + + switch (map_data[idx_map_y + x]) + { + case -1: + map_mat_data_p[idx] = 127; + break; + + case 0: + map_mat_data_p[idx] = 255; + break; + + case 100: + map_mat_data_p[idx] = 0; + break; + } + } + } + image_transport_publisher_full_.publish(cv_img_full_.toImageMsg()); + } + + // Only if someone is subscribed to it, do work and publish tile-based map image Also check if pose_ptr_ is valid + if ((image_transport_publisher_tile_.getNumSubscribers() > 0) && (pose_ptr_)){ + + world_map_transformer_.setTransforms(*map); + + Eigen::Vector2f rob_position_world (pose_ptr_->pose.position.x, pose_ptr_->pose.position.y); + Eigen::Vector2f rob_position_map (world_map_transformer_.getC2Coords(rob_position_world)); + + Eigen::Vector2i rob_position_mapi (rob_position_map.cast()); + + Eigen::Vector2i tile_size_lower_halfi (p_size_tiled_map_image_x_ / 2, p_size_tiled_map_image_y_ / 2); + + Eigen::Vector2i min_coords_map (rob_position_mapi - tile_size_lower_halfi); + + //Clamp to lower map coords + if (min_coords_map[0] < 0){ + min_coords_map[0] = 0; + } + + if (min_coords_map[1] < 0){ + min_coords_map[1] = 0; + } + + Eigen::Vector2i max_coords_map (min_coords_map + Eigen::Vector2i(p_size_tiled_map_image_x_,p_size_tiled_map_image_y_)); + + //Clamp to upper map coords + if (max_coords_map[0] > size_x){ + + int diff = max_coords_map[0] - size_x; + min_coords_map[0] -= diff; + + max_coords_map[0] = size_x; + } + + if (max_coords_map[1] > size_y){ + + int diff = max_coords_map[1] - size_y; + min_coords_map[1] -= diff; + + max_coords_map[1] = size_y; + } + + //Clamp lower again (in case the map is smaller than the selected visualization window) + if (min_coords_map[0] < 0){ + min_coords_map[0] = 0; + } + + if (min_coords_map[1] < 0){ + min_coords_map[1] = 0; + } + + Eigen::Vector2i actual_map_dimensions(max_coords_map - min_coords_map); + + cv::Mat* map_mat = &cv_img_tile_.image; + + // resize cv image if it doesn't have the same dimensions as the selected visualization window + if ( (map_mat->rows != actual_map_dimensions[0]) || (map_mat->cols != actual_map_dimensions[1])){ + *map_mat = cv::Mat(actual_map_dimensions[0], actual_map_dimensions[1], CV_8U); + } + + const std::vector& map_data (map->data); + + unsigned char *map_mat_data_p=(unsigned char*) map_mat->data; + + //We have to flip around the y axis, y for image starts at the top and y for map at the bottom + int y_img = max_coords_map[1]-1; + + for (int y = min_coords_map[1]; y < max_coords_map[1];++y){ + + int idx_map_y = y_img-- * size_x; + int idx_img_y = (y-min_coords_map[1]) * actual_map_dimensions.x(); + + for (int x = min_coords_map[0]; x < max_coords_map[0];++x){ + + int img_index = idx_img_y + (x-min_coords_map[0]); + + switch (map_data[idx_map_y+x]) + { + case 0: + map_mat_data_p[img_index] = 255; + break; + + case -1: + map_mat_data_p[img_index] = 127; + break; + + case 100: + map_mat_data_p[img_index] = 0; + break; + } + } + } + image_transport_publisher_tile_.publish(cv_img_tile_.toImageMsg()); + } + } + + ros::Subscriber map_sub_; + ros::Subscriber pose_sub_; + + image_transport::Publisher image_transport_publisher_full_; + image_transport::Publisher image_transport_publisher_tile_; + + image_transport::ImageTransport* image_transport_; + + geometry_msgs::PoseStampedConstPtr pose_ptr_; + + cv_bridge::CvImage cv_img_full_; + cv_bridge::CvImage cv_img_tile_; + + ros::NodeHandle n_; + ros::NodeHandle pn_; + + int p_size_tiled_map_image_x_; + int p_size_tiled_map_image_y_; + + HectorMapTools::CoordinateTransformer world_map_transformer_; + +}; + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "map_to_image_node"); + + MapAsImageProvider map_image_provider; + + ros::spin(); + + return 0; +} diff --git a/Localizations/Libraries/Ros/hector_slam/hector_geotiff/CHANGELOG.rst b/Localizations/Libraries/Ros/hector_slam/hector_geotiff/CHANGELOG.rst new file mode 100644 index 0000000..250e356 --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_geotiff/CHANGELOG.rst @@ -0,0 +1,68 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package hector_geotiff +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.5.2 (2021-04-08) +------------------ +* Refactored hector_geotiff dependencies. +* Contributors: Stefan Fabian + +0.5.1 (2021-01-15) +------------------ +* Fixed "SEVERE WARNING" by pluginloader when killing geotiff node. + Some minor cleanup. +* Contributors: Stefan Fabian + +0.5.0 (2020-12-17) +------------------ +* Added missing dependency for Qt5 cmake. +* Moved hector_geotiff launch files to separate package to solve cyclic dependency. + Clean up for noetic release. +* Qt5 support for hector geotiff on headless systems. +* Updated platform args. Test on robot. +* Experiments with platform argument. +* Renamed depends for (hopefully soon) rosdep compatibility. +* Moved to Qt5. +* Contributors: Stefan Fabian + +0.4.1 (2020-05-15) +------------------ + +0.3.6 (2019-10-31) +------------------ +* Update geotiff draw interface to support different shapes +* Contributors: Stefan Kohlbrecher + +0.3.5 (2016-06-24) +------------------ +* Use the FindEigen3.cmake module provided by Eigen +* hector_geotiff/hector_geotiff_plugins: added possibility to specify Color of robot path in the geotiff file in order to allow multiple color robot paths +* Contributors: Dorothea Koert, Johannes Meyer + +0.3.4 (2015-11-07) +------------------ +* Removes trailing spaces and fixes indents +* Contributors: YuK_Ota + +0.3.3 (2014-06-15) +------------------ +* fixed cmake find for eigen in indigo +* added launchfile to restart geotiff node +* Contributors: Alexander Stumpf + +0.3.2 (2014-03-30) +------------------ +* Add TrajectoryMapWriter to geotiff_mapper.launch per default +* Add arguments to launch files for specifying geotiff file path +* Contributors: Stefan Kohlbrecher + +0.3.1 (2013-10-09) +------------------ +* added missing install rule for launch files +* moved header files from include/geotiff_writer to include/hector_geotiff +* fixed warnings for deprecated pluginlib method/macro calls +* added changelogs + +0.3.0 (2013-08-08) +------------------ +* catkinized hector_slam diff --git a/Localizations/Libraries/Ros/hector_slam/hector_geotiff/CMakeLists.txt b/Localizations/Libraries/Ros/hector_slam/hector_geotiff/CMakeLists.txt new file mode 100644 index 0000000..1fa1f99 --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_geotiff/CMakeLists.txt @@ -0,0 +1,63 @@ +cmake_minimum_required(VERSION 3.0.2) +project(hector_geotiff) +set(CMAKE_CXX_STANDARD 14) + +find_package(catkin REQUIRED COMPONENTS hector_map_tools hector_nav_msgs nav_msgs pluginlib roscpp std_msgs) + +find_package(Qt5 COMPONENTS Widgets REQUIRED) + +find_package(Eigen3 REQUIRED) + +catkin_package( + INCLUDE_DIRS include + LIBRARIES geotiff_writer + CATKIN_DEPENDS hector_map_tools hector_nav_msgs nav_msgs pluginlib roscpp std_msgs + DEPENDS EIGEN3 Qt5Widgets +) + +########### +## Build ## +########### + +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIRS} + ${Qt5Widgets_INCLUDE_DIRS} +) + +add_library(geotiff_writer src/geotiff_writer/geotiff_writer.cpp) +target_link_libraries(geotiff_writer ${catkin_LIBRARIES} Qt5::Widgets stdc++fs) +add_dependencies(geotiff_writer ${catkin_EXPORTED_TARGETS}) + +add_executable(geotiff_saver src/geotiff_saver.cpp) +target_link_libraries(geotiff_saver geotiff_writer) +add_dependencies(geotiff_saver ${catkin_EXPORTED_TARGETS}) + +add_executable(geotiff_node src/geotiff_node.cpp) +target_link_libraries(geotiff_node geotiff_writer) +add_dependencies(geotiff_node ${catkin_EXPORTED_TARGETS}) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +install(TARGETS geotiff_writer geotiff_saver geotiff_node + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE +) + +install(DIRECTORY fonts + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + diff --git a/Localizations/Libraries/Ros/hector_slam/hector_geotiff/fonts/LICENSE.txt b/Localizations/Libraries/Ros/hector_slam/hector_geotiff/fonts/LICENSE.txt new file mode 100644 index 0000000..6e50fa3 --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_geotiff/fonts/LICENSE.txt @@ -0,0 +1,203 @@ +License for Roboto-Regular.ttf: + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/Localizations/Libraries/Ros/hector_slam/hector_geotiff/fonts/Roboto-Regular.ttf b/Localizations/Libraries/Ros/hector_slam/hector_geotiff/fonts/Roboto-Regular.ttf new file mode 100644 index 0000000..2b6392f Binary files /dev/null and b/Localizations/Libraries/Ros/hector_slam/hector_geotiff/fonts/Roboto-Regular.ttf differ diff --git a/Localizations/Libraries/Ros/hector_slam/hector_geotiff/include/hector_geotiff/geotiff_writer.h b/Localizations/Libraries/Ros/hector_slam/hector_geotiff/include/hector_geotiff/geotiff_writer.h new file mode 100644 index 0000000..27bfa19 --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_geotiff/include/hector_geotiff/geotiff_writer.h @@ -0,0 +1,139 @@ +//================================================================================================= +// Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt +// All rights reserved. + +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of the Simulation, Systems Optimization and Robotics +// group, TU Darmstadt nor the names of its contributors may be used to +// endorse or promote products derived from this software without +// specific prior written permission. + +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +//================================================================================================= + +#ifndef _GEOTIFFWRITER_H__ +#define _GEOTIFFWRITER_H__ + +#include "map_writer_interface.h" + +#include + +#include +#include + +#include +#include +#include + +#include + +#if __cplusplus < 201703L + #include + namespace fs = std::experimental::filesystem; +#else + #include + namespace fs = std::filesystem; +#endif + + +namespace hector_geotiff{ + + +class GeotiffWriter : public MapWriterInterface +{ + public: + explicit GeotiffWriter(bool useCheckerboardCacheIn = false); + virtual ~GeotiffWriter(); + + //setUsePrecalcGrid(bool usePrecalc, const Eigen::Vector2f& size); + + void setMapFileName(const std::string& mapFileName); + void setMapFilePath(const std::string& mapFilePath); + void setUseUtcTimeSuffix(bool useSuffix); + + void setupImageSize(); + bool setupTransforms(const nav_msgs::OccupancyGrid& map); + void drawBackgroundCheckerboard(); + void drawMap(const nav_msgs::OccupancyGrid& map, bool draw_explored_space_grid = true); + void drawObjectOfInterest(const Eigen::Vector2f& coords, const std::string& txt, const Color& color, const Shape& shape); + inline virtual void drawPath(const Eigen::Vector3f& start, const std::vector& points){ + drawPath(start, points, 120,0,240); + } + void drawPath(const Eigen::Vector3f& start, const std::vector& points, int color_r, int color_g, int color_b); + void drawCoords(); + std::string getBasePathAndFileName() const; + void writeGeotiffImage(bool completed); + + +protected: + + void transformPainterToImgCoords(QPainter& painter); + void drawCross(QPainter& painter, const Eigen::Vector2f& coords); + void drawArrow(QPainter& painter); + void drawCoordSystem(QPainter& painter); + + float resolution = std::numeric_limits::quiet_NaN(); + Eigen::Vector2f origin; + + int resolutionFactor = 3; + float resolutionFactorf = std::numeric_limits::quiet_NaN(); + + bool useCheckerboardCache; + bool use_utc_time_suffix_; + + float pixelsPerMapMeter = std::numeric_limits::quiet_NaN(); + float pixelsPerGeoTiffMeter = std::numeric_limits::quiet_NaN(); + + Eigen::Vector2i minCoordsMap; + Eigen::Vector2i maxCoordsMap; + + Eigen::Vector2i sizeMap; + Eigen::Vector2f sizeMapf; + + Eigen::Vector2f rightBottomMarginMeters; + Eigen::Vector2f rightBottomMarginPixelsf; + Eigen::Vector2i rightBottomMarginPixels; + + Eigen::Vector2f leftTopMarginMeters; + + Eigen::Vector2f totalMeters; + + Eigen::Vector2i geoTiffSizePixels; + + Eigen::Vector2f mapOrigInGeotiff; + Eigen::Vector2f mapEndInGeotiff; + + std::string map_file_name_; + std::string map_file_path_; + + QImage image; + QImage checkerboard_cache; + QApplication* app; + QString font_family_; + QFont map_draw_font_; + + HectorMapTools::CoordinateTransformer world_map_transformer_; + HectorMapTools::CoordinateTransformer map_geo_transformer_; + HectorMapTools::CoordinateTransformer world_geo_transformer_; + + nav_msgs::MapMetaData cached_map_meta_data_; +}; + +} + +#endif diff --git a/Localizations/Libraries/Ros/hector_slam/hector_geotiff/include/hector_geotiff/map_writer_interface.h b/Localizations/Libraries/Ros/hector_slam/hector_geotiff/include/hector_geotiff/map_writer_interface.h new file mode 100644 index 0000000..53331d4 --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_geotiff/include/hector_geotiff/map_writer_interface.h @@ -0,0 +1,64 @@ +//================================================================================================= +// Copyright (c) 2012, Stefan Kohlbrecher, TU Darmstadt +// All rights reserved. + +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of the Simulation, Systems Optimization and Robotics +// group, TU Darmstadt nor the names of its contributors may be used to +// endorse or promote products derived from this software without +// specific prior written permission. + +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +//================================================================================================= + +#ifndef _MAPWRITERINTERFACE_H__ +#define _MAPWRITERINTERFACE_H__ + +#include +#include + +namespace hector_geotiff{ + +enum Shape { + SHAPE_CIRCLE, + SHAPE_DIAMOND +}; + +class MapWriterInterface{ +public: + struct Color { + Color(unsigned int r, unsigned int g, unsigned int b) : r(r), g(g), b(b) {} + unsigned int r,g,b; + }; + + bool completed_map_ = false; + + virtual std::string getBasePathAndFileName() const = 0; + virtual void drawObjectOfInterest(const Eigen::Vector2f& coords, const std::string& txt, const Color& color, const Shape& shape = SHAPE_CIRCLE) = 0; + //virtual void drawPath(const Eigen::Vector3f& start, const std::vector& points) = 0; + + inline virtual void drawPath(const Eigen::Vector3f& start, const std::vector& points){ + drawPath(start, points, 120,0,240); + } + virtual void drawPath(const Eigen::Vector3f& start, const std::vector& points, int color_r, int color_g, int color_b) = 0; + +}; + +} + +#endif diff --git a/Localizations/Libraries/Ros/hector_slam/hector_geotiff/include/hector_geotiff/map_writer_plugin_interface.h b/Localizations/Libraries/Ros/hector_slam/hector_geotiff/include/hector_geotiff/map_writer_plugin_interface.h new file mode 100644 index 0000000..ddb0347 --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_geotiff/include/hector_geotiff/map_writer_plugin_interface.h @@ -0,0 +1,47 @@ +//================================================================================================= +// Copyright (c) 2012, Stefan Kohlbrecher, TU Darmstadt +// All rights reserved. + +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of the Simulation, Systems Optimization and Robotics +// group, TU Darmstadt nor the names of its contributors may be used to +// endorse or promote products derived from this software without +// specific prior written permission. + +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +//================================================================================================= + +#ifndef _MAPWRITERPLUGININTERFACE_H__ +#define _MAPWRITERPLUGININTERFACE_H__ + +#include "map_writer_interface.h" + +namespace hector_geotiff{ + +class MapWriterPluginInterface{ + +public: + + virtual void initialize(const std::string& name) = 0; + virtual void draw(MapWriterInterface* map_writer_interface) = 0; + +}; + +} //namespace hector_geotiff + +#endif diff --git a/Localizations/Libraries/Ros/hector_slam/hector_geotiff/package.xml b/Localizations/Libraries/Ros/hector_slam/hector_geotiff/package.xml new file mode 100644 index 0000000..edc8b08 --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_geotiff/package.xml @@ -0,0 +1,50 @@ + + + hector_geotiff + 0.5.2 + + hector_geotiff provides a node that can be used to save occupancy grid map, robot trajectory and object of interest data to RoboCup Rescue compliant GeoTiff images. + + + + Johannes Meyer + + + + + + BSD + + + + + http://ros.org/wiki/hector_geotiff + + + + + Stefan Kohlbrecher + + + catkin + qtbase5-dev + qtbase5-dev + qt5-image-formats-plugins + hector_map_tools + hector_nav_msgs + libqt5-widgets + nav_msgs + pluginlib + roscpp + std_msgs + + + + + + + + + + + diff --git a/Localizations/Libraries/Ros/hector_slam/hector_geotiff/src/geotiff_node.cpp b/Localizations/Libraries/Ros/hector_slam/hector_geotiff/src/geotiff_node.cpp new file mode 100644 index 0000000..5e0ee5d --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_geotiff/src/geotiff_node.cpp @@ -0,0 +1,325 @@ +//================================================================================================= +// Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt +// All rights reserved. + +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of the Simulation, Systems Optimization and Robotics +// group, TU Darmstadt nor the names of its contributors may be used to +// endorse or promote products derived from this software without +// specific prior written permission. + +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +//================================================================================================= + +#include "hector_geotiff/geotiff_writer.h" +#include "hector_geotiff/map_writer_plugin_interface.h" + +#include +#include +#include + +#include + +#include +#include + +#include +#include +#include +#include + +#include + +using namespace std; + +namespace hector_geotiff{ + +/** + * @brief Map generation node. + */ +class MapGenerator +{ +public: + MapGenerator() + : geotiff_writer_(false) + , pn_("~") + , running_saved_map_num_(0) + { + pn_.param("map_file_path", p_map_file_path_, std::string(".")); + geotiff_writer_.setMapFilePath(p_map_file_path_); + geotiff_writer_.setUseUtcTimeSuffix(true); + + pn_.param("map_file_base_name", p_map_file_base_name_, std::string()); + + pn_.param("draw_background_checkerboard", p_draw_background_checkerboard_, true); + pn_.param("draw_free_space_grid", p_draw_free_space_grid_, true); + + sys_cmd_sub_ = n_.subscribe("syscommand", 1, &MapGenerator::sysCmdCallback, this); + pn_.param("use_map_topic", use_map_topic_,false); + + if(!use_map_topic_) map_service_client_ = n_.serviceClient("map"); + //object_service_client_ = n_.serviceClient("worldmodel/get_object_model"); + path_service_client_ = n_.serviceClient("trajectory"); + + + double p_geotiff_save_period = 0.0; + pn_.param("geotiff_save_period", p_geotiff_save_period, 0.0); + + if(p_geotiff_save_period > 0.0){ + //ros::Timer timer = pn_.createTimer(ros::Duration(p_geotiff_save_period), &MapGenerator::timerSaveGeotiffCallback, false); + //publish_trajectory_timer_ = private_nh.createTimer(ros::Duration(1.0 / p_trajectory_publish_rate_), &PathContainer::publishTrajectoryTimerCallback, this, false); + map_save_timer_ = pn_.createTimer(ros::Duration(p_geotiff_save_period), &MapGenerator::timerSaveGeotiffCallback, this, false ); + } + + + pn_.param("plugins", p_plugin_list_, std::string("")); + + std::vector plugin_list; + boost::algorithm::split(plugin_list, p_plugin_list_, boost::is_any_of("\t ")); + + //We always have at least one element containing "" in the string list + if ((plugin_list.size() > 0) && (plugin_list[0].length() > 0)){ + plugin_loader_ = std::make_unique>("hector_geotiff", "hector_geotiff::MapWriterPluginInterface"); + + for (size_t i = 0; i < plugin_list.size(); ++i){ + try + { + boost::shared_ptr tmp (plugin_loader_->createInstance(plugin_list[i])); + tmp->initialize(plugin_loader_->getName(plugin_list[i])); + plugin_vector_.push_back(tmp); + } + catch(pluginlib::PluginlibException& ex) + { + ROS_ERROR("The plugin failed to load for some reason. Error: %s", ex.what()); + } + } + }else{ + ROS_INFO("No plugins loaded for geotiff node"); + } + + ROS_INFO("Geotiff node started"); + } + + ~MapGenerator() = default; + + void writeGeotiff(bool completed) + { + ros::Time start_time (ros::Time::now()); + + std::stringstream ssStream; + + + bool received_map = false; + boost::shared_ptr map; + nav_msgs::GetMap srv_map; + if (use_map_topic_) + { + map = ros::topic::waitForMessage("map",ros::Duration(4)); + if (map != nullptr) received_map = true; + } + else + { + received_map = map_service_client_.call(srv_map); + map = boost::make_shared(srv_map.response.map); + } + if (received_map) + { + ROS_INFO("GeotiffNode: Map service called successfully"); + + std::string map_file_name = p_map_file_base_name_; + std::string competition_name; + std::string team_name; + std::string mission_name; + std::string postfix; + if (n_.getParamCached("/competition", competition_name) && !competition_name.empty()) map_file_name = map_file_name + "_" + competition_name; + if (n_.getParamCached("/team", team_name) && !team_name.empty()) map_file_name = map_file_name + "_" + team_name; + if (n_.getParamCached("/mission", mission_name) && !mission_name.empty()) map_file_name = map_file_name + "_" + mission_name; + if (pn_.getParamCached("map_file_postfix", postfix) && !postfix.empty()) map_file_name = map_file_name + "_" + postfix; + if (map_file_name.substr(0, 1) == "_") map_file_name = map_file_name.substr(1); + if (map_file_name.empty()) map_file_name = "GeoTiffMap"; + geotiff_writer_.setMapFileName(map_file_name); + bool transformSuccess = geotiff_writer_.setupTransforms(*map); + + if(!transformSuccess){ + ROS_INFO("Couldn't set map transform"); + return; + } + + geotiff_writer_.setupImageSize(); + + if (p_draw_background_checkerboard_){ + geotiff_writer_.drawBackgroundCheckerboard(); + } + + geotiff_writer_.drawMap(*map, p_draw_free_space_grid_); + geotiff_writer_.drawCoords(); + + geotiff_writer_.completed_map_ = completed; + + //ROS_INFO("Sum: %ld", (long int)srv.response.sum); + } + else + { + ROS_ERROR("Failed to call map service"); + return; + } + + ROS_INFO("Writing geotiff plugins"); + for (size_t i = 0; i < plugin_vector_.size(); ++i){ + plugin_vector_[i]->draw(&geotiff_writer_); + } + + ROS_INFO("Writing geotiff"); + + /** + * No Victims for now, first agree on a common standard for representation + */ + /* + if (req_object_model_){ + worldmodel_msgs::GetObjectModel srv_objects; + if (object_service_client_.call(srv_objects)) + { + ROS_INFO("GeotiffNode: Object service called successfully"); + + const worldmodel_msgs::ObjectModel& objects_model (srv_objects.response.model); + + size_t size = objects_model.objects.size(); + + + unsigned int victim_num = 1; + + for (size_t i = 0; i < size; ++i){ + const worldmodel_msgs::Object& object (objects_model.objects[i]); + + if (object.state.state == worldmodel_msgs::ObjectState::CONFIRMED){ + geotiff_writer_.drawVictim(Eigen::Vector2f(object.pose.pose.position.x,object.pose.pose.position.y),victim_num); + victim_num++; + } + } + } + else + { + ROS_ERROR("Failed to call objects service"); + } + } + */ + + /* + hector_nav_msgs::GetRobotTrajectory srv_path; + + if (path_service_client_.call(srv_path)) + { + ROS_INFO("GeotiffNode: Path service called successfully"); + + std::vector& traj_vector (srv_path.response.trajectory.poses); + + size_t size = traj_vector.size(); + + std::vector pointVec; + pointVec.resize(size); + + for (size_t i = 0; i < size; ++i){ + const geometry_msgs::PoseStamped& pose (traj_vector[i]); + + pointVec[i] = Eigen::Vector2f(pose.pose.position.x, pose.pose.position.y); + } + + if (size > 0){ + //Eigen::Vector3f startVec(pose_vector[0].x,pose_vector[0].y,pose_vector[0].z); + Eigen::Vector3f startVec(pointVec[0].x(),pointVec[0].y(),0.0f); + geotiff_writer_.drawPath(startVec, pointVec); + } + } + else + { + ROS_ERROR("Failed to call path service"); + } + */ + + + geotiff_writer_.writeGeotiffImage(completed); + running_saved_map_num_++; + + ros::Duration elapsed_time (ros::Time::now() - start_time); + + ROS_INFO("GeoTiff created in %f seconds", elapsed_time.toSec()); + } + + void timerSaveGeotiffCallback(const ros::TimerEvent& e) + { + this->writeGeotiff(false); + } + + void sysCmdCallback(const std_msgs::String& sys_cmd) + { + if (sys_cmd.data != "savegeotiff"){ + return; + } + + this->writeGeotiff(true); + } + + std::string p_map_file_path_; + std::string p_map_file_base_name_; + std::string p_plugin_list_; + bool p_draw_background_checkerboard_; + bool p_draw_free_space_grid_; + bool use_map_topic_; + + //double p_geotiff_save_period_; + + ros::NodeHandle n_; + ros::NodeHandle pn_; + + ros::ServiceClient map_service_client_;// = n.serviceClient("add_two_ints"); + ros::ServiceClient object_service_client_; + ros::ServiceClient path_service_client_; + + ros::Subscriber sys_cmd_sub_; + + std::unique_ptr> plugin_loader_; + std::vector > plugin_vector_; + + + GeotiffWriter geotiff_writer_; + + ros::Timer map_save_timer_; + + unsigned int running_saved_map_num_; + + std::string start_dir_; +}; + +} + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "geotiff_node"); + + hector_geotiff::MapGenerator mg; + + //ros::NodeHandle pn_; + //double p_geotiff_save_period = 60.0f; + //pn_.param("geotiff_save_period", p_geotiff_save_period, 60.0); + //ros::Timer timer = pn_.createTimer(ros::Duration(p_geotiff_save_period), &MapGenerator::timerSaveGeotiffCallback, &mg, false); + + ros::spin(); + + return 0; +} + diff --git a/Localizations/Libraries/Ros/hector_slam/hector_geotiff/src/geotiff_saver.cpp b/Localizations/Libraries/Ros/hector_slam/hector_geotiff/src/geotiff_saver.cpp new file mode 100644 index 0000000..352b78a --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_geotiff/src/geotiff_saver.cpp @@ -0,0 +1,121 @@ +//================================================================================================= +// Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt +// All rights reserved. + +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of the Simulation, Systems Optimization and Robotics +// group, TU Darmstadt nor the names of its contributors may be used to +// endorse or promote products derived from this software without +// specific prior written permission. + +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +//================================================================================================= + +#include "hector_geotiff/geotiff_writer.h" + +#include +#include +#include + +#include + +#include + +using namespace std; + +namespace hector_geotiff{ + +/** + * @brief Map generation node. + */ +class MapGenerator +{ + public: + MapGenerator(const std::string& mapname) : mapname_(mapname) + { + ros::NodeHandle n; + ROS_INFO("Waiting for the map"); + map_sub_ = n.subscribe("map", 1, &MapGenerator::mapCallback, this); + } + + void mapCallback(const nav_msgs::OccupancyGridConstPtr& map) + { + ros::Time start_time (ros::Time::now()); + + geotiff_writer.setMapFileName(mapname_); + geotiff_writer.setupTransforms(*map); + geotiff_writer.drawBackgroundCheckerboard(); + geotiff_writer.drawMap(*map); + geotiff_writer.drawCoords(); + + geotiff_writer.writeGeotiffImage(true); + + ros::Duration elapsed_time (ros::Time::now() - start_time); + ROS_INFO("GeoTiff created in %f seconds", elapsed_time.toSec()); + } + + GeotiffWriter geotiff_writer; + + std::string mapname_; + ros::Subscriber map_sub_; +}; + +} + +#define USAGE "Usage: \n" \ + " geotiff_saver -h\n"\ + " geotiff_saver [-f ] [ROS remapping args]" + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "map_saver"); + std::string mapname = "map"; + + for(int i=1; i + +#include +#include +#include +//#include +#include +#include +#include + +#include + +#if __cplusplus < 201703L + #include + namespace fs = std::experimental::filesystem; +#else + #include + namespace fs = std::filesystem; +#endif + +namespace hector_geotiff +{ + + +GeotiffWriter::GeotiffWriter( bool useCheckerboardCacheIn ) + : useCheckerboardCache( useCheckerboardCacheIn ) + , use_utc_time_suffix_( true ) +{ + cached_map_meta_data_.height = -1; + cached_map_meta_data_.width = -1; + cached_map_meta_data_.resolution = -1.0f; + + int fake_argc = 3; + char *fake_argv[3] = { new char[15], new char[10], new char[10] }; + strcpy( fake_argv[0], "geotiff_writer" ); + strcpy( fake_argv[1], "-platform" ); + strcpy( fake_argv[2], "offscreen" ); // Set the env QT_DEBUG_PLUGINS to 1 to see available platforms + + ROS_INFO("Creating application with offscreen platform."); + //Create a QApplication cause otherwise drawing text will crash + app = new QApplication( fake_argc, fake_argv ); + delete[] fake_argv[0]; + delete[] fake_argv[1]; + delete[] fake_argv[2]; + ROS_INFO("Created application"); + + std::string font_path = ros::package::getPath( "hector_geotiff" ) + "/fonts/Roboto-Regular.ttf"; + int id = QFontDatabase::addApplicationFont( QString::fromStdString( font_path )); + font_family_ = QFontDatabase::applicationFontFamilies( id ).at( 0 ); + + map_file_name_ = ""; + map_file_path_ = ""; +} + +GeotiffWriter::~GeotiffWriter() +{ + delete app; +} + +void GeotiffWriter::setMapFileName( const std::string &mapFileName ) +{ + map_file_name_ = mapFileName; + + if ( use_utc_time_suffix_ ) + { + //QDateTime now (QDateTime::currentDateTimeUtc()); + //std::string current_time_string = now.toString(Qt::ISODate).toStdString(); + QTime now( QTime::currentTime()); + std::string current_time_string = now.toString( Qt::ISODate ).toStdString(); + + map_file_name_ += "_" + current_time_string; + } +} + +void GeotiffWriter::setMapFilePath( const std::string &mapFilePath ) +{ + map_file_path_ = mapFilePath; +} + +void GeotiffWriter::setUseUtcTimeSuffix( bool useSuffix ) +{ + use_utc_time_suffix_ = useSuffix; +} + + +bool GeotiffWriter::setupTransforms( const nav_msgs::OccupancyGrid &map ) +{ + resolution = static_cast(map.info.resolution); + origin = Eigen::Vector2f( map.info.origin.position.x, map.info.origin.position.y ); + + resolutionFactor = 3; + resolutionFactorf = static_cast(resolutionFactor); + + pixelsPerMapMeter = 1.0f / map.info.resolution; + pixelsPerGeoTiffMeter = pixelsPerMapMeter * static_cast(resolutionFactor); + + minCoordsMap = Eigen::Vector2i::Zero(); + maxCoordsMap = Eigen::Vector2i( map.info.width, map.info.height ); + + if ( !HectorMapTools::getMapExtends( map, minCoordsMap, maxCoordsMap )) + { + ROS_INFO( "Cannot determine map extends!" ); + return false; + } + + sizeMap = Eigen::Vector2i( maxCoordsMap - minCoordsMap ); + sizeMapf = ((maxCoordsMap - minCoordsMap).cast()); + + + rightBottomMarginMeters = Eigen::Vector2f( 1.0f, 1.0f ); + rightBottomMarginPixelsf = Eigen::Vector2f( rightBottomMarginMeters.array() * pixelsPerGeoTiffMeter ); + rightBottomMarginPixels = ((rightBottomMarginPixelsf.array() + 0.5f).cast()); + + leftTopMarginMeters = Eigen::Vector2f( 3.0f, 3.0f ); + + totalMeters = (rightBottomMarginMeters + sizeMapf * map.info.resolution + leftTopMarginMeters); + //std::cout << "\n" << totalMeters; + + totalMeters.x() = ceil( totalMeters.x()); + totalMeters.y() = ceil( totalMeters.y()); + //std::cout << "\n" << totalMeters; + + geoTiffSizePixels = ((totalMeters.array() * pixelsPerGeoTiffMeter).cast()); + + + mapOrigInGeotiff = (rightBottomMarginPixelsf); + mapEndInGeotiff = (rightBottomMarginPixelsf + sizeMapf * resolutionFactorf); + //std::cout << "\n mapOrig\n" << mapOrigInGeotiff; + //std::cout << "\n mapOrig\n" << mapEndInGeotiff; + + world_map_transformer_.setTransforms( map ); + + map_geo_transformer_.setTransformsBetweenCoordSystems( mapOrigInGeotiff, mapEndInGeotiff, minCoordsMap.cast(), + maxCoordsMap.cast()); + + /* + Eigen::Vector2f temp_zero_map_g (map_geo_transformer_.getC2Coords(Eigen::Vector2f::Zero())); + + Eigen::Vector2f temp_zero_map_g_floor (floor(temp_zero_map_g.x()), floor(temp_zero_map_g.x())); + + Eigen::Vector2f diff (temp_zero_map_g - temp_zero_map_g_floor); + + map*/ + + + Eigen::Vector2f p1_w( Eigen::Vector2f::Zero()); + Eigen::Vector2f p2_w( Eigen::Vector2f( 100.0f, 100.0f )); + + Eigen::Vector2f p1_m( world_map_transformer_.getC2Coords( p1_w )); + Eigen::Vector2f p2_m( world_map_transformer_.getC2Coords( p2_w )); + + Eigen::Vector2f p1_g( map_geo_transformer_.getC2Coords( p1_m )); + Eigen::Vector2f p2_g( map_geo_transformer_.getC2Coords( p2_m )); + + world_geo_transformer_.setTransformsBetweenCoordSystems( p1_g, p2_g, p1_w, p2_w ); + + map_draw_font_ = QFont( font_family_ ); + map_draw_font_.setPixelSize( 6 * resolutionFactor ); + + if ( useCheckerboardCache ) + { + + if ((cached_map_meta_data_.height != map.info.height) || + (cached_map_meta_data_.width != map.info.width) || + (cached_map_meta_data_.resolution = map.info.resolution)) + { + + cached_map_meta_data_ = map.info; + + Eigen::Vector2f img_size( Eigen::Vector2f( map.info.width, map.info.height ) * resolutionFactorf + + (rightBottomMarginMeters + leftTopMarginMeters) * pixelsPerGeoTiffMeter ); + checkerboard_cache = QImage( img_size.y(), img_size.x(), QImage::Format_RGB32 ); + + QPainter qPainter( &image ); + transformPainterToImgCoords( qPainter ); + + QBrush c1 = QBrush( QColor( 226, 226, 227 )); + QBrush c2 = QBrush( QColor( 237, 237, 238 )); + QRectF background_grid_tile( 0.0f, 0.0f, pixelsPerGeoTiffMeter, pixelsPerGeoTiffMeter ); + + + int xMaxGeo = geoTiffSizePixels[0]; + int yMaxGeo = geoTiffSizePixels[1]; + + for ( int y = 0; y < yMaxGeo; ++y ) + { + for ( int x = 0; x < xMaxGeo; ++x ) + { + //std::cout << "\n" << x << " " << y; + + if ((x + y) % 2 == 0 ) + { + //qPainter.fillRect(background_grid_tile, c1); + qPainter.fillRect( static_cast(x) * pixelsPerGeoTiffMeter, + static_cast(y) * pixelsPerGeoTiffMeter, pixelsPerGeoTiffMeter, + pixelsPerGeoTiffMeter, c1 ); + } + else + { + //qPainter.fillRect(background_grid_tile, c2); + qPainter.fillRect( static_cast(x) * pixelsPerGeoTiffMeter, + static_cast(y) * pixelsPerGeoTiffMeter, pixelsPerGeoTiffMeter, + pixelsPerGeoTiffMeter, c2 ); + } + //background_grid_tile.moveTo(QPointF(static_cast(x)*pixelsPerGeoTiffMeter,static_cast(y)*pixelsPerGeoTiffMeter)); + } + } + } + } + + return true; +} + +void GeotiffWriter::setupImageSize() +{ + bool painter_rotate = true; + + int xMaxGeo = geoTiffSizePixels[0]; + int yMaxGeo = geoTiffSizePixels[1]; + + if ( !useCheckerboardCache ) + { + if ( painter_rotate ) + { + image = QImage( yMaxGeo, xMaxGeo, QImage::Format_RGB32 ); + } + else + { + image = QImage( xMaxGeo, yMaxGeo, QImage::Format_RGB32 ); + } + + QPainter qPainter( &image ); + + QBrush grey = QBrush( QColor( 128, 128, 128 )); + + qPainter.fillRect( image.rect(), grey ); + } +} + +void GeotiffWriter::drawBackgroundCheckerboard() +{ + int xMaxGeo = geoTiffSizePixels[0]; + int yMaxGeo = geoTiffSizePixels[1]; + + bool painter_rotate = true; + + if ( !useCheckerboardCache ) + { + + QPainter qPainter( &image ); + + if ( painter_rotate ) + { + transformPainterToImgCoords( qPainter ); + } + + //*********************** Background checkerboard pattern ********************** + QBrush c1 = QBrush( QColor( 226, 226, 227 )); + QBrush c2 = QBrush( QColor( 237, 237, 238 )); + QRectF background_grid_tile( 0.0f, 0.0f, pixelsPerGeoTiffMeter, pixelsPerGeoTiffMeter ); + + + for ( int y = 0; y < yMaxGeo; ++y ) + { + for ( int x = 0; x < xMaxGeo; ++x ) + { + //std::cout << "\n" << x << " " << y; + + if ((x + y) % 2 == 0 ) + { + //qPainter.fillRect(background_grid_tile, c1); + qPainter.fillRect( static_cast(x) * pixelsPerGeoTiffMeter, + static_cast(y) * pixelsPerGeoTiffMeter, pixelsPerGeoTiffMeter, + pixelsPerGeoTiffMeter, c1 ); + } + else + { + //qPainter.fillRect(background_grid_tile, c2); + qPainter.fillRect( static_cast(x) * pixelsPerGeoTiffMeter, + static_cast(y) * pixelsPerGeoTiffMeter, pixelsPerGeoTiffMeter, + pixelsPerGeoTiffMeter, c2 ); + } + //background_grid_tile.moveTo(QPointF(static_cast(x)*pixelsPerGeoTiffMeter,static_cast(y)*pixelsPerGeoTiffMeter)); + } + } + } + else + { + image = checkerboard_cache.copy( 0, 0, geoTiffSizePixels[0], geoTiffSizePixels[1] ); + } +} + +void GeotiffWriter::drawMap( const nav_msgs::OccupancyGrid &map, bool draw_explored_space_grid ) +{ + QPainter qPainter( &image ); + + transformPainterToImgCoords( qPainter ); + + //this->drawCoordSystem(qPainter); + + QRectF map_cell_grid_tile( 0.0f, 0.0f, resolutionFactor, resolutionFactor ); + + QBrush occupied_brush( QColor( 0, 40, 120 )); + QBrush free_brush( QColor( 255, 255, 255 )); + QBrush explored_space_grid_brush( QColor( 190, 190, 191 )); + + int width = map.info.width; + + float explored_space_grid_resolution_pixels = pixelsPerGeoTiffMeter * 0.5f; + + float yGeo = 0.0f; + float currYLimit = 0.0f; + + bool drawY = false; + + for ( int y = minCoordsMap[1]; y < maxCoordsMap[1]; ++y ) + { + + float xGeo = 0.0f; + + if ( yGeo >= currYLimit ) + { + drawY = true; + } + + float currXLimit = 0.0f; + bool drawX = false; + + for ( int x = minCoordsMap[0]; x < maxCoordsMap[0]; ++x ) + { + + unsigned int i = y * width + x; + + int8_t data = map.data[i]; + + if ( xGeo >= currXLimit ) + { + drawX = true; + } + + if ( data == 0 ) + { + + Eigen::Vector2f coords( mapOrigInGeotiff + (Eigen::Vector2f( xGeo, yGeo ))); + qPainter.fillRect( coords[0], coords[1], resolutionFactorf, resolutionFactorf, free_brush ); + + + if ( draw_explored_space_grid ) + { + if ( drawY ) + { + qPainter.fillRect( coords[0], mapOrigInGeotiff.y() + currYLimit, resolutionFactorf, 1.0f, + explored_space_grid_brush ); + } + + if ( drawX ) + { + qPainter.fillRect( mapOrigInGeotiff.x() + currXLimit, coords[1], 1.0f, resolutionFactorf, + explored_space_grid_brush ); + } + } + } + else if ( data == 100 ) + { + qPainter.fillRect( mapOrigInGeotiff.x() + xGeo, mapOrigInGeotiff.y() + yGeo, resolutionFactorf, + resolutionFactorf, occupied_brush ); + } + + if ( drawX ) + { + currXLimit += explored_space_grid_resolution_pixels; + drawX = false; + } + + xGeo += resolutionFactorf; + } + + if ( drawY ) + { + drawY = false; + currYLimit += explored_space_grid_resolution_pixels; + } + + yGeo += resolutionFactorf; + } +} + +void GeotiffWriter::drawObjectOfInterest( const Eigen::Vector2f &coords, const std::string &txt, const Color &color, + const Shape &shape ) +{ + QPainter qPainter( &image ); + + transformPainterToImgCoords( qPainter ); + + + //qPainter.setPen(ellipse_pen_); + + Eigen::Vector2f map_coords( world_map_transformer_.getC2Coords( coords )); + + Eigen::Vector2f coords_g( world_geo_transformer_.getC2Coords( coords )); + + qPainter.translate( coords_g[0], coords_g[1] ); + + qPainter.rotate( 90 ); + + qPainter.setRenderHint( QPainter::Antialiasing, true ); + + float radius = pixelsPerGeoTiffMeter * 0.175f; + + QRectF shape_rect( -radius, -radius, radius * 2.0f, radius * 2.0f ); + qPainter.save(); + + QBrush tmpBrush( QColor( color.r, color.g, color.b )); + QPen tmpPen( Qt::NoPen ); + qPainter.setBrush( tmpBrush ); + qPainter.setPen( tmpPen ); + + if ( shape == SHAPE_CIRCLE ) + { + qPainter.drawEllipse( shape_rect ); + } + else if ( shape == SHAPE_DIAMOND ) + { + qPainter.rotate( 45 ); + qPainter.drawRect( shape_rect ); + } + + qPainter.restore(); + + + QString tmp( txt.c_str()); + //tmp.setNum(number); + + if ( tmp.length() < 2 ) + { + qPainter.setFont( map_draw_font_ ); + } + else + { + QFont tmp_font( font_family_ ); + tmp_font.setPixelSize( 3 * resolutionFactor ); + qPainter.setFont( tmp_font ); + } + + + qPainter.setPen( Qt::white ); + qPainter.scale( -1.0, 1.0 ); + + qPainter.drawText( shape_rect, Qt::AlignCenter, tmp ); +} + +void GeotiffWriter::drawPath( const Eigen::Vector3f &start, const std::vector &points, int color_r, + int color_g, int color_b ) +{ + QPainter qPainter( &image ); + + transformPainterToImgCoords( qPainter ); + + Eigen::Vector2f start_geo( world_geo_transformer_.getC2Coords( start.head<2>())); + + + size_t size = points.size(); + + QPolygonF polygon; + polygon.reserve( size ); + + polygon.push_back( QPointF( start_geo.x(), start_geo.y())); + + for ( size_t i = 0; i < size; ++i ) + { + const Eigen::Vector2f vec( world_geo_transformer_.getC2Coords( points[i] )); + polygon.push_back( QPointF( vec.x(), vec.y())); + } + + QPen pen( qPainter.pen()); + pen.setColor( QColor( color_r, color_g, color_b )); + pen.setWidth( 3 ); + + qPainter.setPen( pen ); + + //qPainter.setPen(QColor(120,0,240)); + + + qPainter.drawPolyline( polygon ); + + qPainter.save(); + qPainter.translate( start_geo.x(), start_geo.y()); + qPainter.rotate( start.z()); + qPainter.setRenderHint( QPainter::Antialiasing, true ); + drawArrow( qPainter ); + //drawCoordSystem(qPainter); + qPainter.restore(); +} + +std::string GeotiffWriter::getBasePathAndFileName() const +{ + return std::string( map_file_path_ + "/" + map_file_name_ ); +} + +void GeotiffWriter::writeGeotiffImage(bool completed) +{ + //Only works with recent Qt versions + //QDateTime now (QDateTime::currentDateTimeUtc()); + //std::string current_time_string = now.toString(Qt::ISODate).toStdString(); + std::string complete_file_string; + if (completed) { + complete_file_string = map_file_path_ + "/" + map_file_name_; + } + else { + auto t = std::time(nullptr); + auto tm = *std::localtime(&t); + std::stringstream start_ss; + start_ss << std::put_time(&tm, "%Y-%m-%d"); + + complete_file_string = map_file_path_ + "/autosave"; + std::error_code error; + if(!fs::exists(complete_file_string.c_str())) { + fs::create_directory(complete_file_string.c_str(), error); + if (error) { + ROS_ERROR("Can't create autosave folder"); + return; + } + } + + complete_file_string += "/" + start_ss.str(); + if(!fs::exists(complete_file_string.c_str())) { + fs::create_directory(complete_file_string.c_str(), error); + if (error) { + ROS_ERROR("Can't create folder in autosave"); + return; + } + } + + complete_file_string += "/" + map_file_name_; + } + + QImageWriter imageWriter( QString::fromStdString( complete_file_string + ".tif" )); + imageWriter.setCompression( 1 ); + + bool success = imageWriter.write( image ); + + std::string tfw_file_name( complete_file_string + ".tfw" ); + QFile tfwFile( QString::fromStdString( tfw_file_name )); + + tfwFile.open( QIODevice::WriteOnly ); + + QTextStream out( &tfwFile ); + + float resolution_geo = resolution / resolutionFactorf; + + QString resolution_string; + resolution_string.setNum( resolution_geo, 'f', 10 ); + + //positive x resolution + out << resolution_string << "\n"; + + QString zero_string; + zero_string.setNum( 0.0f, 'f', 10 ); + + //rotation, translation + out << zero_string << "\n" << zero_string << "\n"; + + //negative y resolution + out << "-" << resolution_string << "\n"; + + QString top_left_string_x; + QString top_left_string_y; + + //Eigen::Vector2f zero_map_w = world_map_transformer_.getC1Coords(Eigen::Vector2f::Zero()); + Eigen::Vector2f zero_geo_w( world_geo_transformer_.getC1Coords((geoTiffSizePixels.array() + 1).cast())); + + + top_left_string_x.setNum( -zero_geo_w.y(), 'f', 10 ); + top_left_string_y.setNum( zero_geo_w.x(), 'f', 10 ); + + out << top_left_string_x << "\n" << top_left_string_y << "\n"; + + tfwFile.close(); + + if ( !success ) + { + ROS_INFO( "Writing image with file %s failed with error %s", complete_file_string.c_str(), + imageWriter.errorString().toStdString().c_str()); + } + else + { + ROS_INFO( "Successfully wrote geotiff to %s", complete_file_string.c_str()); + } +} + +void GeotiffWriter::transformPainterToImgCoords( QPainter &painter ) +{ + painter.rotate( -90 ); + painter.translate( -geoTiffSizePixels.x(), geoTiffSizePixels.y()); + painter.scale( 1.0, -1.0 ); +} + +void GeotiffWriter::drawCoords() +{ + QPainter qPainter( &image ); + qPainter.setFont( map_draw_font_ ); + + float arrowOffset = pixelsPerGeoTiffMeter * 0.15f; + + // MAP ORIENTATION + qPainter.setPen( QColor( 0, 50, 140 )); + qPainter.drawLine( pixelsPerGeoTiffMeter / 2, pixelsPerGeoTiffMeter, pixelsPerGeoTiffMeter / 2, + 2.0f * pixelsPerGeoTiffMeter ); + qPainter.drawLine( pixelsPerGeoTiffMeter * 2 / 5, pixelsPerGeoTiffMeter - 1, pixelsPerGeoTiffMeter * 3 / 5, + pixelsPerGeoTiffMeter - 1 ); + qPainter.drawLine( pixelsPerGeoTiffMeter * 2 / 5, 2 * pixelsPerGeoTiffMeter, pixelsPerGeoTiffMeter * 3 / 5, + 2 * pixelsPerGeoTiffMeter ); + + + qPainter.drawLine( pixelsPerGeoTiffMeter, 2 * pixelsPerGeoTiffMeter, 2 * pixelsPerGeoTiffMeter, + 2 * pixelsPerGeoTiffMeter ); + qPainter.drawLine( pixelsPerGeoTiffMeter, 2 * pixelsPerGeoTiffMeter, pixelsPerGeoTiffMeter + arrowOffset, + 2 * pixelsPerGeoTiffMeter - arrowOffset ); + qPainter.drawLine( pixelsPerGeoTiffMeter, 2 * pixelsPerGeoTiffMeter, pixelsPerGeoTiffMeter + arrowOffset, + 2 * pixelsPerGeoTiffMeter + arrowOffset ); + + qPainter.drawLine( 2 * pixelsPerGeoTiffMeter, pixelsPerGeoTiffMeter, 2 * pixelsPerGeoTiffMeter, + 2 * pixelsPerGeoTiffMeter ); + qPainter.drawLine( 2 * pixelsPerGeoTiffMeter, pixelsPerGeoTiffMeter, 2 * pixelsPerGeoTiffMeter + arrowOffset, + pixelsPerGeoTiffMeter + arrowOffset ); + qPainter.drawLine( 2 * pixelsPerGeoTiffMeter, pixelsPerGeoTiffMeter, 2 * pixelsPerGeoTiffMeter - arrowOffset, + pixelsPerGeoTiffMeter + arrowOffset ); + + qPainter.drawText( 0.6 * pixelsPerGeoTiffMeter, 1.6 * pixelsPerGeoTiffMeter, QString( "1m" )); + + qPainter.drawText( 2.2 * pixelsPerGeoTiffMeter, 1.1 * pixelsPerGeoTiffMeter, QString( "x" )); + qPainter.drawText( 1.2 * pixelsPerGeoTiffMeter, 1.8 * pixelsPerGeoTiffMeter, QString( "y" )); + + qPainter.drawText( 0.5f * pixelsPerGeoTiffMeter, 0.75f * pixelsPerGeoTiffMeter, + QString((map_file_name_ + ".tif").c_str())); +} + +void GeotiffWriter::drawCross( QPainter &painter, const Eigen::Vector2f &coords ) +{ + painter.drawLine( QPointF( coords[0] - 1.0f, coords[1] ), QPointF( coords[0] + 1.0f, coords[1] )); + painter.drawLine( QPointF( coords[0], coords[1] - 1.0f ), QPointF( coords[0], coords[1] + 1.0f )); +} + +void GeotiffWriter::drawArrow( QPainter &painter ) +{ + float tip_distance = pixelsPerGeoTiffMeter * 0.3f; + + QPolygonF polygon; + + polygon << QPointF( tip_distance, 0.0f ) << QPointF( -tip_distance * 0.5f, -tip_distance * 0.5f ) + << QPointF( 0.0f, 0.0f ) << QPointF( -tip_distance * 0.5f, tip_distance * 0.5f ); + + painter.save(); + + QBrush tmpBrush( QColor( 255, 200, 0 )); + QPen tmpPen( Qt::NoPen ); + painter.setBrush( tmpBrush ); + painter.setPen( tmpPen ); + + painter.drawPolygon( polygon ); + + painter.restore(); +} + +void GeotiffWriter::drawCoordSystem( QPainter &painter ) +{ + painter.save(); + QPointF zero_point( 0.0f, 0.0f ); + QPointF x_point( pixelsPerGeoTiffMeter, 0.0f ); + QPointF y_point( 0.0f, pixelsPerGeoTiffMeter ); + + QPen tmp = painter.pen(); + tmp.setWidth( 5 ); + tmp.setColor( QColor( 255.0, 0.0, 0.0 )); + //painter.setPen(QPen::setWidth(5)); + painter.setPen( tmp ); + painter.drawLine( zero_point, x_point ); + + tmp.setColor( QColor( 0, 255, 0 )); + painter.setPen( tmp ); + painter.drawLine( zero_point, y_point ); + + painter.restore(); +} +} diff --git a/Localizations/Libraries/Ros/hector_slam/hector_geotiff_launch/CHANGELOG.rst b/Localizations/Libraries/Ros/hector_slam/hector_geotiff_launch/CHANGELOG.rst new file mode 100644 index 0000000..86c361b --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_geotiff_launch/CHANGELOG.rst @@ -0,0 +1,15 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package hector_geotiff_launch +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.5.2 (2021-04-08) +------------------ + +0.5.1 (2021-01-15) +------------------ + +0.5.0 (2020-12-17) +------------------ +* Moved hector_geotiff launch files to separate package to solve cyclic dependency. + Clean up for noetic release. +* Contributors: Stefan Fabian diff --git a/Localizations/Libraries/Ros/hector_slam/hector_geotiff_launch/CMakeLists.txt b/Localizations/Libraries/Ros/hector_slam/hector_geotiff_launch/CMakeLists.txt new file mode 100644 index 0000000..008f08d --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_geotiff_launch/CMakeLists.txt @@ -0,0 +1,15 @@ +cmake_minimum_required(VERSION 3.0.2) +project(hector_geotiff_launch) + +find_package(catkin REQUIRED) + +catkin_package() + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) diff --git a/Localizations/Libraries/Ros/hector_slam/hector_geotiff_launch/launch/geotiff_mapper.launch b/Localizations/Libraries/Ros/hector_slam/hector_geotiff_launch/launch/geotiff_mapper.launch new file mode 100644 index 0000000..ed96888 --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_geotiff_launch/launch/geotiff_mapper.launch @@ -0,0 +1,28 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Localizations/Libraries/Ros/hector_slam/hector_geotiff_launch/launch/geotiff_mapper_only.launch b/Localizations/Libraries/Ros/hector_slam/hector_geotiff_launch/launch/geotiff_mapper_only.launch new file mode 100644 index 0000000..5606de5 --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_geotiff_launch/launch/geotiff_mapper_only.launch @@ -0,0 +1,19 @@ + + + + + + + + + + + + + + + + + + + diff --git a/Localizations/Libraries/Ros/hector_slam/hector_geotiff_launch/package.xml b/Localizations/Libraries/Ros/hector_slam/hector_geotiff_launch/package.xml new file mode 100644 index 0000000..8b274c3 --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_geotiff_launch/package.xml @@ -0,0 +1,19 @@ + + + hector_geotiff_launch + 0.5.2 + Contains launch files for the hector_geotiff mapper. + + Stefan Fabian + + BSD + + http://ros.org/wiki/hector_geotiff + + Stefan Kohlbrecher + + catkin + hector_geotiff + hector_geotiff_plugins + hector_trajectory_server + diff --git a/Localizations/Libraries/Ros/hector_slam/hector_geotiff_plugins/CHANGELOG.rst b/Localizations/Libraries/Ros/hector_slam/hector_geotiff_plugins/CHANGELOG.rst new file mode 100644 index 0000000..19a3978 --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_geotiff_plugins/CHANGELOG.rst @@ -0,0 +1,51 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package hector_geotiff_plugins +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.5.2 (2021-04-08) +------------------ + +0.5.1 (2021-01-15) +------------------ +* Fixed "SEVERE WARNING" by pluginloader when killing geotiff node. + Some minor cleanup. +* Contributors: Stefan Fabian + +0.5.0 (2020-12-17) +------------------ +* Moved hector_geotiff launch files to separate package to solve cyclic dependency. + Clean up for noetic release. +* Bump CMake version to avoid CMP0048 warning +* Contributors: Marius Schnaubelt, Stefan Fabian + +0.4.1 (2020-05-15) +------------------ + +0.3.6 (2019-10-31) +------------------ + +0.3.5 (2016-06-24) +------------------ +* hector_geotiff/hector_geotiff_plugins: added possibility to specify Color of robot path in the geotiff file in order to allow multiple color robot paths +* Contributors: Dorothea Koert + +0.3.4 (2015-11-07) +------------------ + +0.3.3 (2014-06-15) +------------------ + +0.3.2 (2014-03-30) +------------------ + +0.3.1 (2013-10-09) +------------------ +* readded PLUGINLIB_DECLARE_CLASS macro for fuerte compatibility +* use hector_geotiff_plugins/TrajectoryMapWriter as legacy lookup name for the trajectory geotiff plugin to not break old launch files +* fixed warnings for deprecated pluginlib method/macro calls +* added changelogs +* added cmake dependencies to catkin exported targets to force message targets to be built before any cpp target using them + +0.3.0 (2013-08-08) +------------------ +* catkinized hector_slam diff --git a/Localizations/Libraries/Ros/hector_slam/hector_geotiff_plugins/CMakeLists.txt b/Localizations/Libraries/Ros/hector_slam/hector_geotiff_plugins/CMakeLists.txt new file mode 100644 index 0000000..7af83de --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_geotiff_plugins/CMakeLists.txt @@ -0,0 +1,44 @@ +cmake_minimum_required(VERSION 3.0.2) +project(hector_geotiff_plugins) + +find_package(catkin REQUIRED COMPONENTS hector_geotiff hector_nav_msgs) + + +################################### +## catkin specific configuration ## +################################### +catkin_package( + CATKIN_DEPENDS hector_geotiff hector_nav_msgs +) + +########### +## Build ## +########### + +include_directories( + ${catkin_INCLUDE_DIRS} +) + +add_library(hector_geotiff_plugins src/trajectory_geotiff_plugin.cpp) +add_dependencies(hector_geotiff_plugins ${catkin_EXPORTED_TARGETS}) +target_link_libraries(hector_geotiff_plugins + ${catkin_LIBRARIES} +) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +install(TARGETS hector_geotiff_plugins + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(FILES + hector_geotiff_plugins.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) diff --git a/Localizations/Libraries/Ros/hector_slam/hector_geotiff_plugins/hector_geotiff_plugins.xml b/Localizations/Libraries/Ros/hector_slam/hector_geotiff_plugins/hector_geotiff_plugins.xml new file mode 100644 index 0000000..987c6f8 --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_geotiff_plugins/hector_geotiff_plugins.xml @@ -0,0 +1,7 @@ + + + + This is a MapWriter plugin that draws the robot's trajectory in the map. + + + diff --git a/Localizations/Libraries/Ros/hector_slam/hector_geotiff_plugins/package.xml b/Localizations/Libraries/Ros/hector_slam/hector_geotiff_plugins/package.xml new file mode 100644 index 0000000..57df97d --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_geotiff_plugins/package.xml @@ -0,0 +1,56 @@ + + + hector_geotiff_plugins + 0.5.2 + + hector_geotiff_plugins contains plugins that extend geotiff maps generated by hector_geotiff. + + + + Johannes Meyer + + + + + + BSD + + + + + http://ros.org/wiki/hector_geotiff_plugins + + + + + Stefan Kohlbrecher + Gregor Gebhardt + + + + + + + + + + + + + catkin + hector_geotiff + hector_nav_msgs + hector_geotiff + hector_nav_msgs + + + + + + + + + + + + diff --git a/Localizations/Libraries/Ros/hector_slam/hector_geotiff_plugins/src/trajectory_geotiff_plugin.cpp b/Localizations/Libraries/Ros/hector_slam/hector_geotiff_plugins/src/trajectory_geotiff_plugin.cpp new file mode 100644 index 0000000..f98d1f8 --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_geotiff_plugins/src/trajectory_geotiff_plugin.cpp @@ -0,0 +1,123 @@ +//================================================================================================= +// Copyright (c) 2012, Gregor Gebhardt, TU Darmstadt +// All rights reserved. + +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of the Flight Systems and Automatic Control group, +// TU Darmstadt, nor the names of its contributors may be used to +// endorse or promote products derived from this software without +// specific prior written permission. + +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +//================================================================================================= + +#include +#include + +#include +#include + +#include + +namespace hector_geotiff_plugins +{ + +using namespace hector_geotiff; + +class TrajectoryMapWriter : public MapWriterPluginInterface +{ +public: + TrajectoryMapWriter(); + virtual ~TrajectoryMapWriter(); + + virtual void initialize(const std::string& name); + virtual void draw(MapWriterInterface *interface); + +protected: + ros::NodeHandle nh_; + ros::ServiceClient service_client_; + + bool initialized_; + std::string name_; + bool draw_all_objects_; + std::string class_id_; + int path_color_r_; + int path_color_g_; + int path_color_b_; +}; + +TrajectoryMapWriter::TrajectoryMapWriter() + : initialized_(false) +{} + +TrajectoryMapWriter::~TrajectoryMapWriter() +{} + +void TrajectoryMapWriter::initialize(const std::string& name) +{ + ros::NodeHandle plugin_nh("~/" + name); + std::string service_name_; + + + plugin_nh.param("service_name", service_name_, std::string("trajectory")); + plugin_nh.param("path_color_r", path_color_r_, 120); + plugin_nh.param("path_color_g", path_color_g_, 0); + plugin_nh.param("path_color_b", path_color_b_, 240); + + service_client_ = nh_.serviceClient(service_name_); + + initialized_ = true; + this->name_ = name; + ROS_INFO_NAMED(name_, "Successfully initialized hector_geotiff MapWriter plugin %s.", name_.c_str()); +} + +void TrajectoryMapWriter::draw(MapWriterInterface *interface) +{ + if(!initialized_) return; + + hector_nav_msgs::GetRobotTrajectory srv_path; + if (!service_client_.call(srv_path)) { + ROS_ERROR_NAMED(name_, "Cannot draw trajectory, service %s failed", service_client_.getService().c_str()); + return; + } + + std::vector& traj_vector (srv_path.response.trajectory.poses); + + size_t size = traj_vector.size(); + + std::vector pointVec; + pointVec.resize(size); + + for (size_t i = 0; i < size; ++i){ + const geometry_msgs::PoseStamped& pose (traj_vector[i]); + + pointVec[i] = Eigen::Vector2f(pose.pose.position.x, pose.pose.position.y); + } + + if (size > 0){ + //Eigen::Vector3f startVec(pose_vector[0].x,pose_vector[0].y,pose_vector[0].z); + Eigen::Vector3f startVec(pointVec[0].x(),pointVec[0].y(),0.0f); + interface->drawPath(startVec, pointVec, path_color_r_, path_color_g_, path_color_b_); + } +} + +} // namespace + +//register this planner as a MapWriterPluginInterface plugin +#include +PLUGINLIB_EXPORT_CLASS(hector_geotiff_plugins::TrajectoryMapWriter, hector_geotiff::MapWriterPluginInterface) diff --git a/Localizations/Libraries/Ros/hector_slam/hector_imu_attitude_to_tf/CHANGELOG.rst b/Localizations/Libraries/Ros/hector_slam/hector_imu_attitude_to_tf/CHANGELOG.rst new file mode 100644 index 0000000..4e28d38 --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_imu_attitude_to_tf/CHANGELOG.rst @@ -0,0 +1,45 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package hector_imu_attitude_to_tf +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.5.2 (2021-04-08) +------------------ + +0.5.1 (2021-01-15) +------------------ + +0.5.0 (2020-12-17) +------------------ +* Moved hector_geotiff launch files to separate package to solve cyclic dependency. + Clean up for noetic release. +* Bump CMake version to avoid CMP0048 warning +* Contributors: Marius Schnaubelt, Stefan Fabian + +0.4.1 (2020-05-15) +------------------ + +0.3.6 (2019-10-31) +------------------ + +0.3.5 (2016-06-24) +------------------ + +0.3.4 (2015-11-07) +------------------ +* hector_imu_attitude_to_tf: fixed default values of the base_frame and base_stabilized_frame parameters (fix #20) +* Contributors: Johannes Meyer + +0.3.3 (2014-06-15) +------------------ + +0.3.2 (2014-03-30) +------------------ + +0.3.1 (2013-10-09) +------------------ +* added missing install rule for launch files +* added changelogs + +0.3.0 (2013-08-08) +------------------ +* catkinized hector_slam diff --git a/Localizations/Libraries/Ros/hector_slam/hector_imu_attitude_to_tf/CMakeLists.txt b/Localizations/Libraries/Ros/hector_slam/hector_imu_attitude_to_tf/CMakeLists.txt new file mode 100644 index 0000000..612d630 --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_imu_attitude_to_tf/CMakeLists.txt @@ -0,0 +1,32 @@ +cmake_minimum_required(VERSION 3.0.2) +project(hector_imu_attitude_to_tf) + +find_package(catkin REQUIRED COMPONENTS roscpp sensor_msgs tf) + +catkin_package( + CATKIN_DEPENDS roscpp sensor_msgs tf +) + +include_directories( + ${catkin_INCLUDE_DIRS} +) + +add_executable(imu_attitude_to_tf_node src/imu_attitude_to_tf_node.cpp) +target_link_libraries(imu_attitude_to_tf_node ${catkin_LIBRARIES}) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +install(TARGETS imu_attitude_to_tf_node + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(DIRECTORY launch + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) diff --git a/Localizations/Libraries/Ros/hector_slam/hector_imu_attitude_to_tf/launch/example.launch b/Localizations/Libraries/Ros/hector_slam/hector_imu_attitude_to_tf/launch/example.launch new file mode 100644 index 0000000..f770b22 --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_imu_attitude_to_tf/launch/example.launch @@ -0,0 +1,7 @@ + + + + + + + diff --git a/Localizations/Libraries/Ros/hector_slam/hector_imu_attitude_to_tf/package.xml b/Localizations/Libraries/Ros/hector_slam/hector_imu_attitude_to_tf/package.xml new file mode 100644 index 0000000..13851f0 --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_imu_attitude_to_tf/package.xml @@ -0,0 +1,55 @@ + + + hector_imu_attitude_to_tf + 0.5.2 + + hector_imu_attitude_to_tf is a lightweight node that can be used to publish the roll/pitch attitude angles reported via a imu message to tf. + + + + Johannes Meyer + + + + + + BSD + + + + + + http://ros.org/wiki/hector_imu_attitude_to_tf + + + + + Stefan Kohlbrecher + + + + + + + + + + + + + + catkin + roscpp + sensor_msgs + tf + + + + + + + + + + + diff --git a/Localizations/Libraries/Ros/hector_slam/hector_imu_attitude_to_tf/src/imu_attitude_to_tf_node.cpp b/Localizations/Libraries/Ros/hector_slam/hector_imu_attitude_to_tf/src/imu_attitude_to_tf_node.cpp new file mode 100644 index 0000000..fc4dbcd --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_imu_attitude_to_tf/src/imu_attitude_to_tf_node.cpp @@ -0,0 +1,84 @@ +//================================================================================================= +// Copyright (c) 2012, Stefan Kohlbrecher, TU Darmstadt +// All rights reserved. + +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of the Simulation, Systems Optimization and Robotics +// group, TU Darmstadt nor the names of its contributors may be used to +// endorse or promote products derived from this software without +// specific prior written permission. + +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +//================================================================================================= + + +#include "ros/ros.h" +#include "tf/transform_broadcaster.h" +#include "sensor_msgs/Imu.h" + +std::string p_base_stabilized_frame_; +std::string p_base_frame_; +tf::TransformBroadcaster* tfB_; +tf::StampedTransform transform_; +tf::Quaternion tmp_; + +#ifndef TF_MATRIX3x3_H + typedef btScalar tfScalar; + namespace tf { typedef btMatrix3x3 Matrix3x3; } +#endif + +void imuMsgCallback(const sensor_msgs::Imu& imu_msg) +{ + tf::quaternionMsgToTF(imu_msg.orientation, tmp_); + + tfScalar yaw, pitch, roll; + tf::Matrix3x3(tmp_).getRPY(roll, pitch, yaw); + + tmp_.setRPY(roll, pitch, 0.0); + + transform_.setRotation(tmp_); + + transform_.stamp_ = imu_msg.header.stamp; + + tfB_->sendTransform(transform_); +} + +int main(int argc, char **argv) { + ros::init(argc, argv, ROS_PACKAGE_NAME); + + ros::NodeHandle n; + ros::NodeHandle pn("~"); + + pn.param("base_stabilized_frame", p_base_stabilized_frame_, std::string("base_stabilized")); + pn.param("base_frame", p_base_frame_, std::string("base_link")); + + tfB_ = new tf::TransformBroadcaster(); + transform_.getOrigin().setX(0.0); + transform_.getOrigin().setY(0.0); + transform_.getOrigin().setZ(0.0); + transform_.frame_id_ = p_base_stabilized_frame_; + transform_.child_frame_id_ = p_base_frame_; + + ros::Subscriber imu_subscriber = n.subscribe("imu_topic", 10, imuMsgCallback); + + ros::spin(); + + delete tfB_; + + return 0; +} diff --git a/Localizations/Libraries/Ros/hector_slam/hector_imu_tools/CHANGELOG.rst b/Localizations/Libraries/Ros/hector_slam/hector_imu_tools/CHANGELOG.rst new file mode 100644 index 0000000..fcb7e50 --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_imu_tools/CHANGELOG.rst @@ -0,0 +1,41 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package hector_imu_tools +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.5.2 (2021-04-08) +------------------ + +0.5.1 (2021-01-15) +------------------ + +0.5.0 (2020-12-17) +------------------ +* Moved hector_geotiff launch files to separate package to solve cyclic dependency. + Clean up for noetic release. +* Bump CMake version to avoid CMP0048 warning +* Contributors: Marius Schnaubelt, Stefan Fabian + +0.4.1 (2020-05-15) +------------------ + +0.3.6 (2019-10-31) +------------------ + +0.3.5 (2016-06-24) +------------------ + +0.3.4 (2015-11-07) +------------------ +* Fix sim setup +* remap for bertl setup +* Contributors: Florian Kunz, kohlbrecher + +0.3.3 (2014-06-15) +------------------ +* hector_imu_tools: Basics of simple height etimation +* hector_imu_tools: Add tf publishers in hector_imu_tools +* hector_imu_tools: Also write out fake odometry +* hector_imu_tools: Fix typo +* hector_imu_tools: Prevent race conditions in slam, formatting +* hector_imu_tools: Small executable for generating a IMU message out of a (2d) pose and roll/pitch imu message +* Contributors: Stefan Kohlbrecher diff --git a/Localizations/Libraries/Ros/hector_slam/hector_imu_tools/CMakeLists.txt b/Localizations/Libraries/Ros/hector_slam/hector_imu_tools/CMakeLists.txt new file mode 100644 index 0000000..cd916b7 --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_imu_tools/CMakeLists.txt @@ -0,0 +1,39 @@ +cmake_minimum_required(VERSION 3.0.2) +project(hector_imu_tools) + +find_package(catkin REQUIRED COMPONENTS roscpp sensor_msgs geometry_msgs nav_msgs tf) + +catkin_package( + CATKIN_DEPENDS geometry_msgs nav_msgs roscpp sensor_msgs tf +) + +########### +## Build ## +########### + +include_directories( + ${catkin_INCLUDE_DIRS} +) + +add_executable(pose_and_orientation_to_imu_node src/pose_and_orientation_to_imu_node.cpp) + +target_link_libraries(pose_and_orientation_to_imu_node + ${catkin_LIBRARIES} +) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +install(TARGETS pose_and_orientation_to_imu_node + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(DIRECTORY launch/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch/ +) diff --git a/Localizations/Libraries/Ros/hector_slam/hector_imu_tools/launch/bertl_robot.launch b/Localizations/Libraries/Ros/hector_slam/hector_imu_tools/launch/bertl_robot.launch new file mode 100644 index 0000000..cce56ad --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_imu_tools/launch/bertl_robot.launch @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/Localizations/Libraries/Ros/hector_slam/hector_imu_tools/launch/jasmine_robot.launch b/Localizations/Libraries/Ros/hector_slam/hector_imu_tools/launch/jasmine_robot.launch new file mode 100644 index 0000000..c5d6904 --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_imu_tools/launch/jasmine_robot.launch @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/Localizations/Libraries/Ros/hector_slam/hector_imu_tools/package.xml b/Localizations/Libraries/Ros/hector_slam/hector_imu_tools/package.xml new file mode 100644 index 0000000..d253828 --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_imu_tools/package.xml @@ -0,0 +1,62 @@ + + + hector_imu_tools + 0.5.2 + + hector_imu_tools provides some tools for processing IMU messages + + + + Johannes Meyer + + + + + + BSD + + + + + + http://ros.org/wiki/hector_imu_attitude_to_tf + + + + + Stefan Kohlbrecher + + + + + + + + + + + + + + catkin + roscpp + tf + geometry_msgs + sensor_msgs + nav_msgs + + roscpp + tf + geometry_msgs + sensor_msgs + nav_msgs + + + + + + + + + + diff --git a/Localizations/Libraries/Ros/hector_slam/hector_imu_tools/src/pose_and_orientation_to_imu_node.cpp b/Localizations/Libraries/Ros/hector_slam/hector_imu_tools/src/pose_and_orientation_to_imu_node.cpp new file mode 100644 index 0000000..a17acfb --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_imu_tools/src/pose_and_orientation_to_imu_node.cpp @@ -0,0 +1,190 @@ +//================================================================================================= +// Copyright (c) 2012, Stefan Kohlbrecher, TU Darmstadt +// All rights reserved. + +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of the Simulation, Systems Optimization and Robotics +// group, TU Darmstadt nor the names of its contributors may be used to +// endorse or promote products derived from this software without +// specific prior written permission. + +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +//================================================================================================= + + +#include "ros/ros.h" +#include "tf/transform_broadcaster.h" +#include +#include +#include + +std::string p_map_frame_; +std::string p_base_footprint_frame_; +std::string p_base_stabilized_frame_; +std::string p_base_frame_; +tf::TransformBroadcaster* tfB_; +tf::StampedTransform transform_; + +tf::Quaternion robot_pose_quaternion_; +tf::Point robot_pose_position_; +tf::Transform robot_pose_transform_; + +tf::Quaternion tmp_; +tf::Quaternion orientation_quaternion_; + +sensor_msgs::ImuConstPtr last_imu_msg_; +sensor_msgs::Imu fused_imu_msg_; +nav_msgs::Odometry odom_msg_; +geometry_msgs::PoseStampedConstPtr last_pose_msg_; + +ros::Publisher fused_imu_publisher_; +ros::Publisher odometry_publisher_; + +size_t callback_count_; + +#ifndef TF_MATRIX3x3_H +typedef btScalar tfScalar; +namespace tf { typedef btMatrix3x3 Matrix3x3; } +#endif + +void imuMsgCallback(const sensor_msgs::Imu::ConstPtr& imu_msg) +{ + callback_count_++; + + tf::quaternionMsgToTF(imu_msg->orientation, tmp_); + + tfScalar imu_yaw, imu_pitch, imu_roll; + tf::Matrix3x3(tmp_).getRPY(imu_roll, imu_pitch, imu_yaw); + + tf::Transform transform; + transform.setIdentity(); + tf::Quaternion quat; + + quat.setRPY(imu_roll, imu_pitch, 0.0); + + if (true){ + transform.setRotation(quat); + tfB_->sendTransform(tf::StampedTransform(transform, imu_msg->header.stamp, p_base_stabilized_frame_, p_base_frame_)); + } + + tfScalar pose_yaw, pose_pitch, pose_roll; + + if (last_pose_msg_ != 0){ + tf::quaternionMsgToTF(last_pose_msg_->pose.orientation, tmp_); + + tf::Matrix3x3(tmp_).getRPY(pose_roll, pose_pitch, pose_yaw); + }else{ + pose_yaw = 0.0; + } + + orientation_quaternion_.setRPY(imu_roll, imu_pitch, pose_yaw); + + fused_imu_msg_.header.stamp = imu_msg->header.stamp; + + fused_imu_msg_.orientation.x = orientation_quaternion_.getX(); + fused_imu_msg_.orientation.y = orientation_quaternion_.getY(); + fused_imu_msg_.orientation.z = orientation_quaternion_.getZ(); + fused_imu_msg_.orientation.w = orientation_quaternion_.getW(); + + fused_imu_publisher_.publish(fused_imu_msg_); + + //If no pose message received, yaw is set to 0. + //@TODO: Check for timestamp of pose and disable sending if too old + if (last_pose_msg_ != 0){ + if ( (callback_count_ % 5) == 0){ + odom_msg_.header.stamp = imu_msg->header.stamp; + odom_msg_.pose.pose.orientation = fused_imu_msg_.orientation; + odom_msg_.pose.pose.position = last_pose_msg_->pose.position; + + odometry_publisher_.publish(odom_msg_); + } + } + +} + +void poseMsgCallback(const geometry_msgs::PoseStamped::ConstPtr& pose_msg) +{ + + std::vector transforms; + transforms.resize(2); + + tf::quaternionMsgToTF(pose_msg->pose.orientation, robot_pose_quaternion_); + tf::pointMsgToTF(pose_msg->pose.position, robot_pose_position_); + + robot_pose_transform_.setRotation(robot_pose_quaternion_); + robot_pose_transform_.setOrigin(robot_pose_position_); + + tf::Transform height_transform; + height_transform.setIdentity(); + height_transform.setOrigin(tf::Vector3(0.0, 0.0, 0.0)); + + transforms[0] = tf::StampedTransform(robot_pose_transform_, pose_msg->header.stamp, p_map_frame_, p_base_footprint_frame_); + transforms[1] = tf::StampedTransform(height_transform, pose_msg->header.stamp, p_base_footprint_frame_, p_base_stabilized_frame_); + + tfB_->sendTransform(transforms); + + // Perform simple estimation of vehicle altitude based on orientation + if (last_pose_msg_){ + tf::Vector3 plane_normal = tf::Matrix3x3(orientation_quaternion_) * tf::Vector3(0.0, 0.0, 1.0); + + tf::Vector3 last_position; + tf::pointMsgToTF(last_pose_msg_->pose.position, last_position); + + double height_difference = + (-plane_normal.getX() * (robot_pose_position_.getX() - last_position.getX()) + -plane_normal.getY() * (robot_pose_position_.getY() - last_position.getY()) + +plane_normal.getZ() * last_position.getZ()) / last_position.getZ(); + + } + + + + last_pose_msg_ = pose_msg; + +} + +int main(int argc, char **argv) { + ros::init(argc, argv, ROS_PACKAGE_NAME); + + ros::NodeHandle n; + ros::NodeHandle pn("~"); + + pn.param("map_frame", p_map_frame_, std::string("map")); + pn.param("base_footprint_frame", p_base_footprint_frame_, std::string("base_footprint")); + pn.param("base_stabilized_frame", p_base_stabilized_frame_, std::string("base_stabilized")); + pn.param("base_frame", p_base_frame_, std::string("base_link")); + + fused_imu_msg_.header.frame_id = p_base_stabilized_frame_; + odom_msg_.header.frame_id = "map"; + + tfB_ = new tf::TransformBroadcaster(); + + fused_imu_publisher_ = n.advertise("/fused_imu",1,false); + odometry_publisher_ = n.advertise("/state", 1, false); + + ros::Subscriber imu_subscriber = n.subscribe("/imu", 10, imuMsgCallback); + ros::Subscriber pose_subscriber = n.subscribe("/pose", 10, poseMsgCallback); + + callback_count_ = 0; + + ros::spin(); + + delete tfB_; + + return 0; +} diff --git a/Localizations/Libraries/Ros/hector_slam/hector_map_server/CHANGELOG.rst b/Localizations/Libraries/Ros/hector_slam/hector_map_server/CHANGELOG.rst new file mode 100644 index 0000000..973895a --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_map_server/CHANGELOG.rst @@ -0,0 +1,43 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package hector_map_server +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.5.2 (2021-04-08) +------------------ + +0.5.1 (2021-01-15) +------------------ + +0.5.0 (2020-12-17) +------------------ +* Moved hector_geotiff launch files to separate package to solve cyclic dependency. + Clean up for noetic release. +* Bump CMake version to avoid CMP0048 warning +* Contributors: Marius Schnaubelt, Stefan Fabian + +0.4.1 (2020-05-15) +------------------ + +0.3.6 (2019-10-31) +------------------ + +0.3.5 (2016-06-24) +------------------ + +0.3.4 (2015-11-07) +------------------ + +0.3.3 (2014-06-15) +------------------ + +0.3.2 (2014-03-30) +------------------ + +0.3.1 (2013-10-09) +------------------ +* added changelogs +* added cmake dependencies to catkin exported targets to force message targets to be built before any cpp target using them + +0.3.0 (2013-08-08) +------------------ +* catkinized hector_slam diff --git a/Localizations/Libraries/Ros/hector_slam/hector_map_server/CMakeLists.txt b/Localizations/Libraries/Ros/hector_slam/hector_map_server/CMakeLists.txt new file mode 100644 index 0000000..2a1e682 --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_map_server/CMakeLists.txt @@ -0,0 +1,35 @@ +cmake_minimum_required(VERSION 3.0.2) +project(hector_map_server) + +find_package(catkin REQUIRED COMPONENTS roscpp hector_map_tools hector_marker_drawing hector_nav_msgs nav_msgs tf) + +catkin_package( + CATKIN_DEPENDS roscpp hector_map_tools hector_marker_drawing hector_nav_msgs nav_msgs tf +) + +########### +## Build ## +########### + +include_directories( + ${catkin_INCLUDE_DIRS} +) + +add_executable(hector_map_server src/hector_map_server.cpp) +add_dependencies(hector_map_server ${catkin_EXPORTED_TARGETS}) +target_link_libraries(hector_map_server + ${catkin_LIBRARIES} +) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +install(TARGETS hector_map_server + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) diff --git a/Localizations/Libraries/Ros/hector_slam/hector_map_server/package.xml b/Localizations/Libraries/Ros/hector_slam/hector_map_server/package.xml new file mode 100644 index 0000000..e540631 --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_map_server/package.xml @@ -0,0 +1,63 @@ + + + hector_map_server + 0.5.2 + + hector_map_server provides a service for retrieving the map, as well as for raycasting based obstacle queries (finds next obstacle in the map, given start and endpoint + in any tf coordinate frame). + + + + Johannes Meyer + + + + + + BSD + + + + + http://ros.org/wiki/hector_map_server + + + + + Stefan Kohlbrecher + + + + + + + + + + + + + + catkin + roscpp + hector_map_tools + hector_marker_drawing + hector_nav_msgs + nav_msgs + tf + roscpp + hector_map_tools + hector_marker_drawing + hector_nav_msgs + nav_msgs + tf + + + + + + + + + + diff --git a/Localizations/Libraries/Ros/hector_slam/hector_map_server/src/hector_map_server.cpp b/Localizations/Libraries/Ros/hector_slam/hector_map_server/src/hector_map_server.cpp new file mode 100644 index 0000000..54bba39 --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_map_server/src/hector_map_server.cpp @@ -0,0 +1,331 @@ +//================================================================================================= +// Copyright (c) 2011, Stefan Kohlbrecher, Johannes Meyer TU Darmstadt +// All rights reserved. + +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of the Simulation, Systems Optimization and Robotics +// group, TU Darmstadt nor the names of its contributors may be used to +// endorse or promote products derived from this software without +// specific prior written permission. + +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +//================================================================================================= + +#include +#include +//#include +//#include + +#include "ros/ros.h" +#include "ros/console.h" +#include "nav_msgs/MapMetaData.h" +#include "nav_msgs/OccupancyGrid.h" + +#include + +#include + +#include "nav_msgs/GetMap.h" + +#include "hector_marker_drawing/HectorDrawings.h" +#include "hector_map_tools/HectorMapTools.h" + +#include "hector_nav_msgs/GetDistanceToObstacle.h" +#include "hector_nav_msgs/GetSearchPosition.h" + + +class OccupancyGridContainer +{ +public: + OccupancyGridContainer(std::string sub_topic, std::string prefix, ros::NodeHandle& nh, HectorDrawings* drawing_provider, tf::TransformListener* tf_) + : drawing_provider_(drawing_provider) + , tf_(tf_) + { + + std::string service_name = "map"; + map_service_ = nh.advertiseService(service_name, &OccupancyGridContainer::mapServiceCallback, this); + + ros::NodeHandle pnh("~"); + std::string lookup_service_name = "get_distance_to_obstacle"; + dist_lookup_service_ = pnh.advertiseService(lookup_service_name, &OccupancyGridContainer::lookupServiceCallback, this); + + std::string get_search_pos_service_name = "get_search_position"; + get_search_pos_service_ = pnh.advertiseService(get_search_pos_service_name, &OccupancyGridContainer::getSearchPosServiceCallback, this); + + map_sub_ = nh.subscribe("map", 1, &OccupancyGridContainer::mapCallback, this); + } + + ~OccupancyGridContainer() + {} + + bool mapServiceCallback(nav_msgs::GetMap::Request &req, + nav_msgs::GetMap::Response &res ) + { + ROS_INFO("hector_map_server map service called"); + + if (!map_ptr_){ + ROS_INFO("map_server has no map yet, no map service available"); + return false; + } + + res.map = *map_ptr_; + + return true; + } + + bool lookupServiceCallback(hector_nav_msgs::GetDistanceToObstacle::Request &req, + hector_nav_msgs::GetDistanceToObstacle::Response &res ) + { + //ROS_INFO("hector_map_server lookup service called"); + + + if (!map_ptr_){ + ROS_INFO("map_server has no map yet, no lookup service available"); + return false; + } + + tf::StampedTransform stamped_pose; + + + + try{ + tf_->waitForTransform(map_ptr_->header.frame_id,req.point.header.frame_id, req.point.header.stamp, ros::Duration(1.0)); + tf_->lookupTransform(map_ptr_->header.frame_id, req.point.header.frame_id, req.point.header.stamp, stamped_pose); + + tf::Point v2_tf; + tf::pointMsgToTF(req.point.point,v2_tf); + + tf::Vector3 v1 = stamped_pose * tf::Vector3(0.0, 0.0, 0.0); + tf::Vector3 v2 = stamped_pose * v2_tf; + tf::Vector3 diff = v2 - v1; + v2 = v1 + diff / tf::Vector3(diff.x(), diff.y(), 0.0).length() * 5.0f; + + Eigen::Vector2f start(v1.x(),v1.y()); + Eigen::Vector2f end(v2.x(),v2.y()); + + Eigen::Vector2f hit_world; + //float dist = dist_meas_.getDist(start,end, &hit_world); + float dist = dist_meas_.getDist(start,end,&hit_world); + + if (dist >=0.0f){ + tf::Vector3 diff (v2-v1); + + float angle = diff.angle(tf::Vector3(diff.x(),diff.y(),0.0f)); + + res.distance = dist/cos(angle); + + }else{ + res.distance = -1.0f; + } + + //debug drawing + if (false){ + + float cube_scale = map_ptr_->info.resolution; + drawing_provider_->setColor(1.0, 0.0, 0.0); + drawing_provider_->setScale(static_cast(cube_scale)); + + drawing_provider_->drawPoint(start); + + drawing_provider_->setColor(0.0, 1.0, 0.0); + drawing_provider_->drawPoint(end); + + if (dist >= 0.0f){ + drawing_provider_->setColor(0.0, 0.0, 1.0); + drawing_provider_->drawPoint(hit_world); + } + + drawing_provider_->sendAndResetData(); + } + + return true; + + } + catch(tf::TransformException e) + { + ROS_ERROR("Transform failed in lookup distance service call: %s",e.what()); + } + + return false; + } + + bool getSearchPosServiceCallback(hector_nav_msgs::GetSearchPosition::Request &req, + hector_nav_msgs::GetSearchPosition::Response &res ) + { + if (!map_ptr_){ + ROS_INFO("map_server has no map yet, no get best search pos service available"); + return false; + } + + try{ + + tf::Stamped ooi_pose, transformed_pose, search_position; + tf::poseStampedMsgToTF(req.ooi_pose, ooi_pose); + + tf_->waitForTransform(map_ptr_->header.frame_id, req.ooi_pose.header.frame_id, req.ooi_pose.header.stamp, ros::Duration(1.0)); + tf_->transformPose(map_ptr_->header.frame_id, ooi_pose, transformed_pose); + + tf::Vector3 direction(-req.distance, 0.0, 0.0); + search_position = transformed_pose; + search_position.setOrigin(transformed_pose.getOrigin() + transformed_pose.getBasis() * direction); + + tf::poseStampedTFToMsg(search_position, res.search_pose); + + return true; + +// //tf::Point v2_tf; +// //tf::pointMsgToTF(req.point.point,v2_tf); + +// tf::Vector3 v1 = stamped_pose * tf::Vector3(0.0, 0.0, 0.0); + +// //warning: 3D! +// tf::Vector3 v2 = stamped_pose * tf::Vector3(-1.0, 0.0, 0.0); + +// tf::Vector3 dir = v2-v1; + +// Eigen::Vector2f dir_2d (dir.x(), dir.y()); + +// dir_2d.normalize(); + +// Eigen::Vector2f searchPos (Eigen::Vector2f(v1.x(),v1.y()) + (dir_2d*0.5f)); + +// //copy original pose message but set translation +// res.search_pose.pose = ooi_pose.pose; + +// res.search_pose.pose.position.x = searchPos.x(); +// res.search_pose.pose.position.y = searchPos.y(); + + //return true; + + + //Eigen::Vector2f ooi_pos(v1.x(),v1.y()); + //Eigen::Vector2f sample_point_pos(v2.x(),v2.y()); + + + //float dist_from_target = dist_meas_.getDist(ooi_pos,sample_point_pos); + //float dist_from_sample_point = dist_meas_.getDist(sample_point_pos, ooi_pos); + + + +/* + if (dist >=0.0f){ + tf::Vector3 diff (v2-v1); + + float angle = diff.angle(tf::Vector3(diff.x(),diff.y(),0.0f)); + + res.distance = dist/cos(angle); + + + //debug drawing + if (true){ + + float cube_scale = map_ptr_->info.resolution; + drawing_provider_->setColor(1.0, 0.0, 0.0); + drawing_provider_->setScale(static_cast(cube_scale)); + + drawing_provider_->drawPoint(start); + drawing_provider_->drawPoint(end); + + if (dist >= 0.0f){ + drawing_provider_->setColor(0.0, 0.0, 1.0); + drawing_provider_->drawPoint(hit_world); + } + + drawing_provider_->sendAndResetData(); + } + + } + */ + //return true; + + } catch(tf::TransformException e){ + ROS_ERROR("Transform failed in getSearchPosition service call: %s",e.what()); + } + + return false; + } + + void mapCallback(const nav_msgs::OccupancyGridConstPtr& map) + { + map_ptr_ = map; + + dist_meas_.setMap(map_ptr_); + } + + //Services + ros::ServiceServer map_service_; + ros::ServiceServer dist_lookup_service_; + ros::ServiceServer get_search_pos_service_; + + //Subscriber + ros::Subscriber map_sub_; + + HectorMapTools::DistanceMeasurementProvider dist_meas_; + + HectorDrawings* drawing_provider_; + tf::TransformListener* tf_; + + //nav_msgs::MapMetaData meta_data_message_; + nav_msgs::GetMap::Response map_resp_; + + nav_msgs::OccupancyGridConstPtr map_ptr_; +}; + +class HectorMapServer +{ + public: + /** Trivial constructor */ + HectorMapServer(ros::NodeHandle& private_nh) + { + std::string frame_id; + + hector_drawings_ = new HectorDrawings(); + hector_drawings_->setNamespace("map_server"); + + mapContainer = new OccupancyGridContainer("map", "" ,private_nh, hector_drawings_,&tf_); + } + + ~HectorMapServer() + { + delete mapContainer; + + + delete hector_drawings_; + } + + +public: + OccupancyGridContainer* mapContainer; +private: + + HectorDrawings* hector_drawings_; + tf::TransformListener tf_; +}; + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "hector_map_server"); + ros::NodeHandle nh; + + HectorMapServer ms(nh); + + ros::spin(); + + return 0; +} + diff --git a/Localizations/Libraries/Ros/hector_slam/hector_map_tools/CHANGELOG.rst b/Localizations/Libraries/Ros/hector_slam/hector_map_tools/CHANGELOG.rst new file mode 100644 index 0000000..2dc1f7c --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_map_tools/CHANGELOG.rst @@ -0,0 +1,51 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package hector_map_tools +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.5.2 (2021-04-08) +------------------ + +0.5.1 (2021-01-15) +------------------ + +0.5.0 (2020-12-17) +------------------ +* Moved hector_geotiff launch files to separate package to solve cyclic dependency. + Clean up for noetic release. +* Bump CMake version to avoid CMP0048 warning +* Contributors: Marius Schnaubelt, Stefan Fabian + +0.4.1 (2020-05-15) +------------------ + +0.3.6 (2019-10-31) +------------------ +* hector_map_tools: Use the FindEigen3.cmake module provided by Eigen + This patch applies the recommendation from http://wiki.ros.org/jade/Migration and removes the + dependency from package cmake_modules (unless your installation of Eigen3 does not provide a + cmake config). + Same as 1251d9dc20854f48da116eed25780c103a5bd003, but package hector_map_tools was not updated + back then. +* Contributors: Johannes Meyer + +0.3.5 (2016-06-24) +------------------ + +0.3.4 (2015-11-07) +------------------ +* -Fix severe bug when not using square size maps (would results in completely wrong obstacle distances and coordinates) +* Contributors: Stefan Kohlbrecher + +0.3.3 (2014-06-15) +------------------ + +0.3.2 (2014-03-30) +------------------ + +0.3.1 (2013-10-09) +------------------ +* added changelogs + +0.3.0 (2013-08-08) +------------------ +* catkinized hector_slam diff --git a/Localizations/Libraries/Ros/hector_slam/hector_map_tools/CMakeLists.txt b/Localizations/Libraries/Ros/hector_slam/hector_map_tools/CMakeLists.txt new file mode 100644 index 0000000..9e62135 --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_map_tools/CMakeLists.txt @@ -0,0 +1,25 @@ +cmake_minimum_required(VERSION 3.0.2) +project(hector_map_tools) + +find_package(catkin REQUIRED COMPONENTS nav_msgs) + +find_package(Eigen3 REQUIRED) + +catkin_package( + INCLUDE_DIRS include + CATKIN_DEPENDS nav_msgs + DEPENDS EIGEN3 +) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE +) diff --git a/Localizations/Libraries/Ros/hector_slam/hector_map_tools/include/hector_map_tools/HectorMapTools.h b/Localizations/Libraries/Ros/hector_slam/hector_map_tools/include/hector_map_tools/HectorMapTools.h new file mode 100644 index 0000000..fef2326 --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_map_tools/include/hector_map_tools/HectorMapTools.h @@ -0,0 +1,293 @@ +//================================================================================================= +// Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt +// All rights reserved. + +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of the Simulation, Systems Optimization and Robotics +// group, TU Darmstadt nor the names of its contributors may be used to +// endorse or promote products derived from this software without +// specific prior written permission. + +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +//================================================================================================= + +#ifndef __HectorMapTools_h_ +#define __HectorMapTools_h_ + +#include +#include + +#include + +class HectorMapTools{ +public: + + template + class CoordinateTransformer{ + public: + + CoordinateTransformer() + { + + } + + CoordinateTransformer(const nav_msgs::OccupancyGridConstPtr map) + { + this->setTransforms(*map); + } + + + void setTransforms(const nav_msgs::OccupancyGrid& map) + { + this->setTransforms(map.info); + } + + void setTransforms(const nav_msgs::MapMetaData& meta) + { + origo_ = (Eigen::Matrix(static_cast(meta.origin.position.x),static_cast(meta.origin.position.y))); + scale_ = (static_cast(meta.resolution)); + inv_scale_ = (static_cast(1.0f / meta.resolution)); + } + + void setTransformsBetweenCoordSystems(const Eigen::Matrix& origoCS1, const Eigen::Matrix& endCS1, + const Eigen::Matrix& origoCS2, const Eigen::Matrix& endCS2) + { + Eigen::Matrix map_t_geotiff_x_factors = getLinearTransform(Eigen::Vector2f(origoCS1[0], endCS1[0]), Eigen::Vector2f(origoCS2[0], endCS2[0])); + Eigen::Matrix map_t_geotiff_y_factors = getLinearTransform(Eigen::Vector2f(origoCS1[1], endCS1[1]), Eigen::Vector2f(origoCS2[1], endCS2[1])); + + //std::cout << "\n geo_x: \n" << map_t_geotiff_x_factors << "\n geo_y: \n" << map_t_geotiff_y_factors << "\n"; + + origo_.x() = map_t_geotiff_x_factors[1]; + origo_.y() = map_t_geotiff_y_factors[1]; + + scale_ = map_t_geotiff_x_factors[0]; + inv_scale_ = 1.0 / scale_; + + //std::cout << "\nscale " << scale_ << "\n"; + } + + Eigen::Matrix getC1Coords(const Eigen::Matrix& mapCoords) const + { + return origo_ + (mapCoords * scale_); + } + + Eigen::Matrix getC2Coords(const Eigen::Matrix& worldCoords) const + { + return ((worldCoords - origo_) * inv_scale_); + } + + ConcreteScalar getC1Scale(float c2_scale) const + { + return scale_ * c2_scale; + } + + ConcreteScalar getC2Scale(float c1_scale) const + { + return inv_scale_ * c1_scale; + } + + protected: + + Eigen::Matrix getLinearTransform(const Eigen::Matrix& coordSystem1, const Eigen::Matrix& coordSystem2) + { + ConcreteScalar scaling = (coordSystem2[0] - coordSystem2[1]) / (coordSystem1[0] - coordSystem1[1]); + ConcreteScalar translation = coordSystem2[0] - coordSystem1[0] * scaling; + return Eigen::Vector2f (scaling, translation); + } + + Eigen::Matrix origo_; + ConcreteScalar scale_; + ConcreteScalar inv_scale_; + }; + + class DistanceMeasurementProvider + { + public: + DistanceMeasurementProvider() + { + + } + + void setMap(const nav_msgs::OccupancyGridConstPtr map) + { + map_ptr_ = map; + + world_map_transformer_.setTransforms(*map_ptr_); + } + + float getDist(const Eigen::Vector2f& begin_world, const Eigen::Vector2f& end_world, Eigen::Vector2f* hitCoords = 0) + { + Eigen::Vector2i end_point_map; + + Eigen::Vector2i begin_map (world_map_transformer_.getC2Coords(begin_world).cast()); + Eigen::Vector2i end_map (world_map_transformer_.getC2Coords(end_world).cast()); + float dist = checkOccupancyBresenhami(begin_map, end_map, &end_point_map); + + if (hitCoords != 0){ + *hitCoords = world_map_transformer_.getC1Coords(end_point_map.cast()); + } + + return world_map_transformer_.getC1Scale(dist); + } + + inline float checkOccupancyBresenhami( const Eigen::Vector2i& beginMap, const Eigen::Vector2i& endMap, Eigen::Vector2i* hitCoords = 0, unsigned int max_length = UINT_MAX){ + + int x0 = beginMap[0]; + int y0 = beginMap[1]; + + int sizeX = map_ptr_->info.width; + int sizeY = map_ptr_->info.height; + + //check if beam start point is inside map, cancel update if this is not the case + if ((x0 < 0) || (x0 >= sizeX) || (y0 < 0) || (y0 >= sizeY)) { + return -1.0f; + } + + int x1 = endMap[0]; + int y1 = endMap[1]; + + //std::cout << " x: "<< x1 << " y: " << y1 << " length: " << length << " "; + + //check if beam end point is inside map, cancel update if this is not the case + if ((x1 < 0) || (x1 >= sizeX) || (y1 < 0) || (y1 >= sizeY)) { + return -1.0f; + } + + int dx = x1 - x0; + int dy = y1 - y0; + + unsigned int abs_dx = abs(dx); + unsigned int abs_dy = abs(dy); + + int offset_dx = dx > 0 ? 1 : -1; + int offset_dy = (dy > 0 ? 1 : -1) * sizeX; + + unsigned int startOffset = beginMap.y() * sizeX + beginMap.x(); + + int end_offset; + + //if x is dominant + if(abs_dx >= abs_dy){ + int error_y = abs_dx / 2; + end_offset = bresenham2D(abs_dx, abs_dy, error_y, offset_dx, offset_dy, startOffset,5000); + }else{ + //otherwise y is dominant + int error_x = abs_dy / 2; + end_offset = bresenham2D(abs_dy, abs_dx, error_x, offset_dy, offset_dx, startOffset,5000); + } + + if (end_offset != -1){ + Eigen::Vector2i end_coords(end_offset % sizeX, end_offset / sizeX); + + int distMap = ((beginMap - end_coords).cast()).norm(); + + if (hitCoords != 0){ + *hitCoords = end_coords; + } + + return distMap; + } + + return -1.0f; + + //unsigned int endOffset = endMap.y() * sizeX + endMap.x(); + //this->bresenhamCellOcc(endOffset); + } + + inline int bresenham2D(unsigned int abs_da, unsigned int abs_db, int error_b, int offset_a, int offset_b, + unsigned int offset, unsigned int max_length){ + unsigned int end = std::min(max_length, abs_da); + + const std::vector& data = map_ptr_->data; + + for(unsigned int i = 0; i < end; ++i){ + if (data[offset] == 100){ + return static_cast(offset); + } + + offset += offset_a; + error_b += abs_db; + if((unsigned int)error_b >= abs_da){ + offset += offset_b; + error_b -= abs_da; + } + } + return -1; + //at(offset); + } + + protected: + CoordinateTransformer world_map_transformer_; + nav_msgs::OccupancyGridConstPtr map_ptr_; + + + }; + + static bool getMapExtends(const nav_msgs::OccupancyGrid& map, Eigen::Vector2i& topLeft, Eigen::Vector2i& bottomRight) + { + int lowerStart = -1; + int upperStart = 10000000; + + int xMaxTemp = lowerStart; + int yMaxTemp = lowerStart; + int xMinTemp = upperStart; + int yMinTemp = upperStart; + + int width = map.info.width; + int height = map.info.height; + + for (int y = 0; y < height; ++y){ + for (int x = 0; x < width; ++x){ + + if ( map.data[x+y*width] != -1){ + + if (x > xMaxTemp) { + xMaxTemp = x; + } + + if (x < xMinTemp) { + xMinTemp = x; + } + + if (y > yMaxTemp) { + yMaxTemp = y; + } + + if (y < yMinTemp) { + yMinTemp = y; + } + } + } + } + + if ((xMaxTemp != lowerStart) && + (yMaxTemp != lowerStart) && + (xMinTemp != upperStart) && + (yMinTemp != upperStart)) { + + topLeft = Eigen::Vector2i(xMinTemp,yMinTemp); + bottomRight = Eigen::Vector2i(xMaxTemp+1, yMaxTemp+1); + + return true; + } else { + return false; + } + }; +}; + +#endif diff --git a/Localizations/Libraries/Ros/hector_slam/hector_map_tools/package.xml b/Localizations/Libraries/Ros/hector_slam/hector_map_tools/package.xml new file mode 100644 index 0000000..77a293b --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_map_tools/package.xml @@ -0,0 +1,55 @@ + + + hector_map_tools + 0.5.2 + + hector_map_tools contains some functions related to accessing information from OccupancyGridMap maps. + Currently consists of a single header. + + + + Johannes Meyer + + + + + + BSD + + + + + http://ros.org/wiki/hector_map_tools + + + + + Stefan Kohlbrecher + + + + + + + + + + + + + + catkin + nav_msgs + eigen + nav_msgs + eigen + + + + + + + + + + diff --git a/Localizations/Libraries/Ros/hector_slam/hector_mapping/CHANGELOG.rst b/Localizations/Libraries/Ros/hector_slam/hector_mapping/CHANGELOG.rst new file mode 100644 index 0000000..c43454c --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_mapping/CHANGELOG.rst @@ -0,0 +1,64 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package hector_mapping +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.5.2 (2021-04-08) +------------------ +* Remove tf_conversions as a dependency (`#93 `_) +* Add reset mapping (and pose) service (`#87 `_) + * add service to reset the mapping with a new initial pose +* Reorganize scanCallback and add comments (`#89 `_) + * Refactor, reformat and comment scanCallback + * make rosPointCloudToDataContainer void + * make rosLaserScanToDataContainer void +* Add pause and reset services to hector_mapping (`#86 `_) + * Add pause and reset services to Hector +* Contributors: Marcelino Almeida + +0.5.1 (2021-01-15) +------------------ + +0.5.0 (2020-12-17) +------------------ +* Moved hector_geotiff launch files to separate package to solve cyclic dependency. + Clean up for noetic release. +* Bump CMake version to avoid CMP0048 warning +* fixed compilation under noetic +* Contributors: Marius Schnaubelt, Stefan Fabian + +0.4.1 (2020-05-15) +------------------ +* Remove unnecessary boost signals find_package + With Boost >1.69 hector_mapping won't build. Furthermore, hector_mapping doesn't use signals anywhere. +* Contributors: Sam Pfeiffer + +0.3.6 (2019-10-31) +------------------ +* Merge pull request `#49 `_ from davidbsp/catkin + populate child_frame_id in odometry msg +* Added child_frame_id in hector mapping's odometry msg +* Contributors: David Portugal, Johannes Meyer + +0.3.5 (2016-06-24) +------------------ +* Use the FindEigen3.cmake module provided by Eigen +* Contributors: Johannes Meyer + +0.3.4 (2015-11-07) +------------------ + +0.3.3 (2014-06-15) +------------------ + +0.3.2 (2014-03-30) +------------------ + +0.3.1 (2013-10-09) +------------------ +* respect ``p_map_frame_`` +* added changelogs + +0.3.0 (2013-08-08) +------------------ +* catkinized hector_slam +* hector_mapping: fixed multi-resolution map scan matching index bug in MapRepMultiMap.h diff --git a/Localizations/Libraries/Ros/hector_slam/hector_mapping/CMakeLists.txt b/Localizations/Libraries/Ros/hector_slam/hector_mapping/CMakeLists.txt new file mode 100644 index 0000000..20f014f --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_mapping/CMakeLists.txt @@ -0,0 +1,112 @@ +cmake_minimum_required(VERSION 3.0.2) +project(hector_mapping) + +find_package(catkin REQUIRED COMPONENTS + roscpp + nav_msgs + visualization_msgs + tf + message_filters + laser_geometry + message_generation + std_srvs) + +find_package(Boost REQUIRED COMPONENTS thread) + +find_package(Eigen3 REQUIRED) + +####################################### +## Declare ROS messages and services ## +####################################### + +## Generate messages in the 'msg' folder +add_message_files( + FILES + HectorDebugInfo.msg + HectorIterData.msg +) + +## Generate services in the 'srv' folder +add_service_files( + FILES + ResetMapping.srv +) + +generate_messages( + DEPENDENCIES + geometry_msgs +) + +################################### +## catkin specific configuration ## +################################### +catkin_package( + INCLUDE_DIRS include + LIBRARIES hector_mapping_lib + CATKIN_DEPENDS roscpp nav_msgs visualization_msgs tf message_filters laser_geometry message_runtime + #DEPENDS EIGEN3 +) + +########### +## Build ## +########### + +include_directories( + include + ${Boost_INCLUDE_DIRS} + ${catkin_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIRS} +) + +add_library( ${PROJECT_NAME}_lib + src/PoseInfoContainer.cpp + src/HectorMappingRos.cpp +) + +add_dependencies(${PROJECT_NAME}_lib ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +target_link_libraries(${PROJECT_NAME}_lib + ${catkin_LIBRARIES} + ${Boost_LIBRARIES} + ${EIGEN3_LIBRARIES} +) + +add_executable(${PROJECT_NAME} src/main.cpp) +add_dependencies(${PROJECT_NAME} + ${${PROJECT_NAME}_EXPORTED_TARGETS} + ${catkin_EXPORTED_TARGETS} +) + +target_link_libraries(${PROJECT_NAME} + ${PROJECT_NAME}_lib + ${catkin_LIBRARIES} + ${Boost_LIBRARIES} +) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +install(TARGETS ${PROJECT_NAME}_lib + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(TARGETS hector_mapping + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +## Don't ask why it's hector_mapping, this was Stefan Kohlbrecher's first ROS package and a wrapper of a pre ROS header only library +install(DIRECTORY include + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE +) + +install(DIRECTORY launch + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch/ +) diff --git a/Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/HectorDebugInfoProvider.h b/Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/HectorDebugInfoProvider.h new file mode 100644 index 0000000..0b2ba66 --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/HectorDebugInfoProvider.h @@ -0,0 +1,94 @@ +//================================================================================================= +// Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt +// All rights reserved. + +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of the Simulation, Systems Optimization and Robotics +// group, TU Darmstadt nor the names of its contributors may be used to +// endorse or promote products derived from this software without +// specific prior written permission. + +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +//================================================================================================= + +#ifndef HECTOR_DEBUG_INFO_PROVIDER_H__ +#define HECTOR_DEBUG_INFO_PROVIDER_H__ + +#include "util/HectorDebugInfoInterface.h" +#include "util/UtilFunctions.h" + +#include "ros/ros.h" + +#include "hector_mapping/HectorDebugInfo.h" + + +class HectorDebugInfoProvider : public HectorDebugInfoInterface +{ +public: + + HectorDebugInfoProvider() + { + ros::NodeHandle nh_; + + debugInfoPublisher_ = nh_.advertise("hector_debug_info", 50, true); + }; + + virtual void sendAndResetData() + { + debugInfoPublisher_.publish(debugInfo); + debugInfo.iterData.clear(); + } + + + virtual void addHessianMatrix(const Eigen::Matrix3f& hessian) + { + hector_mapping::HectorIterData iterData; + + for (int i=0; i < 9; ++i){ + iterData.hessian[i] = static_cast(hessian.data()[i]); + iterData.determinant = hessian.determinant(); + + Eigen::SelfAdjointEigenSolver eig(hessian); + + const Eigen::Vector3f& eigValues (eig.eigenvalues()); + iterData.conditionNum = eigValues[2] / eigValues[0]; + + + iterData.determinant2d = hessian.block<2,2>(0,0).determinant(); + Eigen::SelfAdjointEigenSolver eig2d(hessian.block<2,2>(0,0)); + + const Eigen::Vector2f& eigValues2d (eig2d.eigenvalues()); + iterData.conditionNum2d = eigValues2d[1] / eigValues2d[0]; + } + + debugInfo.iterData.push_back(iterData); + } + + virtual void addPoseLikelihood(float lh) + { + + } + + + hector_mapping::HectorDebugInfo debugInfo; + + ros::Publisher debugInfoPublisher_; + +}; + +#endif diff --git a/Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/HectorDrawings.h b/Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/HectorDrawings.h new file mode 100644 index 0000000..e3694da --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/HectorDrawings.h @@ -0,0 +1,171 @@ +//================================================================================================= +// Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt +// All rights reserved. + +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of the Simulation, Systems Optimization and Robotics +// group, TU Darmstadt nor the names of its contributors may be used to +// endorse or promote products derived from this software without +// specific prior written permission. + +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +//================================================================================================= + +#ifndef HECTOR_DRAWINGS_H__ +#define HECTOR_DRAWINGS_H__ + +#include "util/DrawInterface.h" +#include "util/UtilFunctions.h" + +#include "ros/ros.h" + +#include + +#include + + +class HectorDrawings : public DrawInterface +{ +public: + + HectorDrawings() + { + idCounter = 0; + + ros::NodeHandle nh_; + + markerPublisher_ = nh_.advertise("visualization_marker", 1, true); + markerArrayPublisher_ = nh_.advertise("visualization_marker_array", 1, true); + + tempMarker.header.frame_id = "map"; + tempMarker.ns = "slam"; + + this->setScale(1.0); + this->setColor(1.0, 1.0, 1.0); + + tempMarker.action = visualization_msgs::Marker::ADD; + }; + + virtual void drawPoint(const Eigen::Vector2f& pointWorldFrame) + { + tempMarker.id = idCounter++; + + tempMarker.pose.position.x = pointWorldFrame.x(); + tempMarker.pose.position.y = pointWorldFrame.y(); + + tempMarker.pose.orientation.w = 0.0; + tempMarker.pose.orientation.z = 0.0; + tempMarker.type = visualization_msgs::Marker::CUBE; + + //markerPublisher_.publish(tempMarker); + + markerArray.markers.push_back(tempMarker); + } + + virtual void drawArrow(const Eigen::Vector3f& poseWorld) + { + tempMarker.id = idCounter++; + + tempMarker.pose.position.x = poseWorld.x(); + tempMarker.pose.position.y = poseWorld.y(); + + tempMarker.pose.orientation.w = cos(poseWorld.z()*0.5f); + tempMarker.pose.orientation.z = sin(poseWorld.z()*0.5f); + + tempMarker.type = visualization_msgs::Marker::ARROW; + + //markerPublisher_.publish(tempMarker); + + markerArray.markers.push_back(tempMarker); + + } + + virtual void drawCovariance(const Eigen::Vector2f& mean, const Eigen::Matrix2f& covMatrix) + { + tempMarker.pose.position.x = mean[0]; + tempMarker.pose.position.y = mean[1]; + + Eigen::SelfAdjointEigenSolver eig(covMatrix); + + const Eigen::Vector2f& eigValues (eig.eigenvalues()); + const Eigen::Matrix2f& eigVectors (eig.eigenvectors()); + + float angle = (atan2(eigVectors(1, 0), eigVectors(0, 0))); + + tempMarker.type = visualization_msgs::Marker::CYLINDER; + + double lengthMajor = sqrt(eigValues[0]); + double lengthMinor = sqrt(eigValues[1]); + + tempMarker.scale.x = lengthMajor; + tempMarker.scale.y = lengthMinor; + tempMarker.scale.z = 0.001; + + + tempMarker.pose.orientation.w = cos(angle*0.5); + tempMarker.pose.orientation.z = sin(angle*0.5); + + tempMarker.id = idCounter++; + markerArray.markers.push_back(tempMarker); + + //drawLine(Eigen::Vector3f(0,0,0), Eigen::Vector3f(lengthMajor ,0,0)); + //drawLine(Eigen::Vector3f(0,0,0), Eigen::Vector3f(0,lengthMinor,0)); + + //glScalef(lengthMajor, lengthMinor, 0); + //glCallList(dlCircle); + //this->popCS(); + } + + virtual void setScale(double scale) + { + tempMarker.scale.x = scale; + tempMarker.scale.y = scale; + tempMarker.scale.z = scale; + } + + virtual void setColor(double r, double g, double b, double a = 1.0) + { + tempMarker.color.r = r; + tempMarker.color.g = g; + tempMarker.color.b = b; + tempMarker.color.a = a; + } + + virtual void sendAndResetData() + { + markerArrayPublisher_.publish(markerArray); + markerArray.markers.clear(); + idCounter = 0; + } + + void setTime(const ros::Time& time) + { + tempMarker.header.stamp = time; + } + + + ros::Publisher markerPublisher_; + ros::Publisher markerArrayPublisher_; + + visualization_msgs::Marker tempMarker; + visualization_msgs::MarkerArray markerArray; + + int idCounter; +}; + +#endif diff --git a/Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/HectorMapMutex.h b/Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/HectorMapMutex.h new file mode 100644 index 0000000..f2826a3 --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/HectorMapMutex.h @@ -0,0 +1,24 @@ +#ifndef hectormapmutex_h__ +#define hectormapmutex_h__ + +#include "util/MapLockerInterface.h" + +#include + +class HectorMapMutex : public MapLockerInterface +{ +public: + virtual void lockMap() + { + mapModifyMutex_.lock(); + } + + virtual void unlockMap() + { + mapModifyMutex_.unlock(); + } + + boost::mutex mapModifyMutex_; +}; + +#endif diff --git a/Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/HectorMappingRos.h b/Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/HectorMappingRos.h new file mode 100644 index 0000000..7edcf8b --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/HectorMappingRos.h @@ -0,0 +1,208 @@ +//================================================================================================= +// Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt +// All rights reserved. + +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of the Simulation, Systems Optimization and Robotics +// group, TU Darmstadt nor the names of its contributors may be used to +// endorse or promote products derived from this software without +// specific prior written permission. + +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +//================================================================================================= + +#ifndef HECTOR_MAPPING_ROS_H__ +#define HECTOR_MAPPING_ROS_H__ + +#include "ros/ros.h" + +#include "tf/transform_listener.h" +#include "tf/transform_broadcaster.h" +#include "tf/message_filter.h" +#include "message_filters/subscriber.h" + +#include "sensor_msgs/LaserScan.h" +#include + +#include +#include +#include + +#include "laser_geometry/laser_geometry.h" +#include "nav_msgs/GetMap.h" + +#include "slam_main/HectorSlamProcessor.h" + +#include "scan/DataPointContainer.h" +#include "util/MapLockerInterface.h" + +#include + +#include "PoseInfoContainer.h" + + +class HectorDrawings; +class HectorDebugInfoProvider; + +class MapPublisherContainer +{ +public: + ros::Publisher mapPublisher_; + ros::Publisher mapMetadataPublisher_; + nav_msgs::GetMap::Response map_; + ros::ServiceServer dynamicMapServiceServer_; +}; + +class HectorMappingRos +{ +public: + HectorMappingRos(); + ~HectorMappingRos(); + + + void scanCallback(const sensor_msgs::LaserScan& scan); + void sysMsgCallback(const std_msgs::String& string); + + bool mapCallback(nav_msgs::GetMap::Request &req, nav_msgs::GetMap::Response &res); + bool resetMapCallback(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res); + bool restartHectorCallback(hector_mapping::ResetMapping::Request &req, hector_mapping::ResetMapping::Response &res); + bool pauseMapCallback(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res); + + void publishMap(MapPublisherContainer& map_, const hectorslam::GridMap& gridMap, ros::Time timestamp, MapLockerInterface* mapMutex = 0); + + void rosLaserScanToDataContainer(const sensor_msgs::LaserScan& scan, hectorslam::DataContainer& dataContainer, float scaleToMap); + void rosPointCloudToDataContainer(const sensor_msgs::PointCloud& pointCloud, const tf::StampedTransform& laserTransform, hectorslam::DataContainer& dataContainer, float scaleToMap); + + void setServiceGetMapData(nav_msgs::GetMap::Response& map_, const hectorslam::GridMap& gridMap); + + void publishTransformLoop(double p_transform_pub_period_); + void publishMapLoop(double p_map_pub_period_); + void publishTransform(); + + void staticMapCallback(const nav_msgs::OccupancyGrid& map); + void initialPoseCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg); + + // Internal mapping management functions + void toggleMappingPause(bool pause); + void resetPose(const geometry_msgs::Pose &pose); + + /* + void setStaticMapData(const nav_msgs::OccupancyGrid& map); + */ +protected: + + HectorDebugInfoProvider* debugInfoProvider; + HectorDrawings* hectorDrawings; + + int lastGetMapUpdateIndex; + + ros::NodeHandle node_; + + ros::Subscriber scanSubscriber_; + ros::Subscriber sysMsgSubscriber_; + + ros::Subscriber mapSubscriber_; + message_filters::Subscriber* initial_pose_sub_; + tf::MessageFilter* initial_pose_filter_; + + ros::Publisher posePublisher_; + ros::Publisher poseUpdatePublisher_; + ros::Publisher twistUpdatePublisher_; + ros::Publisher odometryPublisher_; + ros::Publisher scan_point_cloud_publisher_; + + ros::ServiceServer reset_map_service_; + ros::ServiceServer restart_hector_service_; + ros::ServiceServer toggle_scan_processing_service_; + + std::vector mapPubContainer; + + tf::TransformListener tf_; + tf::TransformBroadcaster* tfB_; + + laser_geometry::LaserProjection projector_; + + tf::Transform map_to_odom_; + + boost::thread* map__publish_thread_; + + hectorslam::HectorSlamProcessor* slamProcessor; + hectorslam::DataContainer laserScanContainer; + + PoseInfoContainer poseInfoContainer_; + + sensor_msgs::PointCloud laser_point_cloud_; + + ros::Time lastMapPublishTime; + ros::Time lastScanTime; + Eigen::Vector3f lastSlamPose; + + bool initial_pose_set_; + Eigen::Vector3f initial_pose_; + + bool pause_scan_processing_; + + //----------------------------------------------------------- + // Parameters + + std::string p_base_frame_; + std::string p_map_frame_; + std::string p_odom_frame_; + + //Parameters related to publishing the scanmatcher pose directly via tf + bool p_pub_map_scanmatch_transform_; + std::string p_tf_map_scanmatch_transform_frame_name_; + + std::string p_scan_topic_; + std::string p_sys_msg_topic_; + + std::string p_pose_update_topic_; + std::string p_twist_update_topic_; + + bool p_pub_drawings; + bool p_pub_debug_output_; + bool p_pub_map_odom_transform_; + bool p_pub_odometry_; + bool p_advertise_map_service_; + int p_scan_subscriber_queue_size_; + + double p_update_factor_free_; + double p_update_factor_occupied_; + double p_map_update_distance_threshold_; + double p_map_update_angle_threshold_; + + double p_map_resolution_; + int p_map_size_; + double p_map_start_x_; + double p_map_start_y_; + int p_map_multi_res_levels_; + + double p_map_pub_period_; + + bool p_use_tf_scan_transformation_; + bool p_use_tf_pose_start_estimate_; + bool p_map_with_known_poses_; + bool p_timing_output_; + + float p_sqr_laser_min_dist_; + float p_sqr_laser_max_dist_; + float p_laser_z_min_value_; + float p_laser_z_max_value_; +}; + +#endif diff --git a/Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/PoseInfoContainer.h b/Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/PoseInfoContainer.h new file mode 100644 index 0000000..2632a8c --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/PoseInfoContainer.h @@ -0,0 +1,55 @@ +//================================================================================================= +// Copyright (c) 2012, Stefan Kohlbrecher, TU Darmstadt +// All rights reserved. + +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of the Simulation, Systems Optimization and Robotics +// group, TU Darmstadt nor the names of its contributors may be used to +// endorse or promote products derived from this software without +// specific prior written permission. + +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +//================================================================================================= + +#ifndef POSE_INFO_CONTAINER_H__ +#define POSE_INFO_CONTAINER_H__ + +#include +#include +#include + +#include + +class PoseInfoContainer{ +public: + + void update(const Eigen::Vector3f& slamPose, const Eigen::Matrix3f& slamCov, const ros::Time& stamp, const std::string& frame_id); + + const geometry_msgs::PoseStamped& getPoseStamped() { return stampedPose_; }; + const geometry_msgs::PoseWithCovarianceStamped& getPoseWithCovarianceStamped() { return covPose_; }; + const tf::Transform& getTfTransform() { return poseTransform_; }; + +protected: + geometry_msgs::PoseStamped stampedPose_; + geometry_msgs::PoseWithCovarianceStamped covPose_; + tf::Transform poseTransform_; + +}; + +#endif + diff --git a/Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/map/GridMap.h b/Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/map/GridMap.h new file mode 100644 index 0000000..ce5aa26 --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/map/GridMap.h @@ -0,0 +1,45 @@ +//================================================================================================= +// Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt +// All rights reserved. + +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of the Simulation, Systems Optimization and Robotics +// group, TU Darmstadt nor the names of its contributors may be used to +// endorse or promote products derived from this software without +// specific prior written permission. + +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +//================================================================================================= + +#ifndef __GridMap_h_ +#define __GridMap_h_ + +#include "OccGridMapBase.h" +#include "GridMapLogOdds.h" +#include "GridMapReflectanceCount.h" +#include "GridMapSimpleCount.h" + +namespace hectorslam { + +typedef OccGridMapBase GridMap; +//typedef OccGridMapBase GridMap; +//typedef OccGridMapBase GridMap; + +} + +#endif diff --git a/Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/map/GridMapBase.h b/Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/map/GridMapBase.h new file mode 100644 index 0000000..b027a88 --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/map/GridMapBase.h @@ -0,0 +1,404 @@ +//================================================================================================= +// Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt +// All rights reserved. + +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of the Simulation, Systems Optimization and Robotics +// group, TU Darmstadt nor the names of its contributors may be used to +// endorse or promote products derived from this software without +// specific prior written permission. + +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +//================================================================================================= + +#ifndef __GridMapBase_h_ +#define __GridMapBase_h_ + +#include +#include + +#include "MapDimensionProperties.h" + +namespace hectorslam { + +/** + * GridMapBase provides basic grid map functionality (creates grid , provides transformation from/to world coordinates). + * It serves as the base class for different map representations that may extend it's functionality. + */ +template +class GridMapBase +{ + +public: + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + /** + * Indicates if given x and y are within map bounds + * @return True if coordinates are within map bounds + */ + bool hasGridValue(int x, int y) const + { + return (x >= 0) && (y >= 0) && (x < this->getSizeX()) && (y < this->getSizeY()); + } + + const Eigen::Vector2i& getMapDimensions() const { return mapDimensionProperties.getMapDimensions(); }; + int getSizeX() const { return mapDimensionProperties.getSizeX(); }; + int getSizeY() const { return mapDimensionProperties.getSizeY(); }; + + bool pointOutOfMapBounds(const Eigen::Vector2f& pointMapCoords) const + { + return mapDimensionProperties.pointOutOfMapBounds(pointMapCoords); + } + + virtual void reset() + { + this->clear(); + } + + /** + * Resets the grid cell values by using the resetGridCell() function. + */ + void clear() + { + int size = this->getSizeX() * this->getSizeY(); + + for (int i = 0; i < size; ++i) { + this->mapArray[i].resetGridCell(); + } + + //this->mapArray[0].set(1.0f); + //this->mapArray[size-1].set(1.0f); + } + + + const MapDimensionProperties& getMapDimProperties() const { return mapDimensionProperties; }; + + /** + * Constructor, creates grid representation and transformations. + */ + GridMapBase(float mapResolution, const Eigen::Vector2i& size, const Eigen::Vector2f& offset) + : mapArray(0) + , lastUpdateIndex(-1) + { + Eigen::Vector2i newMapDimensions (size); + + this->setMapGridSize(newMapDimensions); + sizeX = size[0]; + + setMapTransformation(offset, mapResolution); + + this->clear(); + } + + /** + * Destructor + */ + virtual ~GridMapBase() + { + deleteArray(); + } + + /** + * Allocates memory for the two dimensional pointer array for map representation. + */ + void allocateArray(const Eigen::Vector2i& newMapDims) + { + int sizeX = newMapDims.x(); + int sizeY = newMapDims.y(); + + mapArray = new ConcreteCellType [sizeX*sizeY]; + + mapDimensionProperties.setMapCellDims(newMapDims); + } + + void deleteArray() + { + if (mapArray != 0){ + + delete[] mapArray; + + mapArray = 0; + mapDimensionProperties.setMapCellDims(Eigen::Vector2i(-1,-1)); + } + } + + ConcreteCellType& getCell(int x, int y) + { + return mapArray[y * sizeX + x]; + } + + const ConcreteCellType& getCell(int x, int y) const + { + return mapArray[y * sizeX + x]; + } + + ConcreteCellType& getCell(int index) + { + return mapArray[index]; + } + + const ConcreteCellType& getCell(int index) const + { + return mapArray[index]; + } + + void setMapGridSize(const Eigen::Vector2i& newMapDims) + { + if (newMapDims != mapDimensionProperties.getMapDimensions() ){ + deleteArray(); + allocateArray(newMapDims); + this->reset(); + } + } + + /** + * Copy Constructor, only needed if pointer members are present. + */ + GridMapBase(const GridMapBase& other) + { + allocateArray(other.getMapDimensions()); + *this = other; + } + + /** + * Assignment operator, only needed if pointer members are present. + */ + GridMapBase& operator=(const GridMapBase& other) + { + if ( !(this->mapDimensionProperties == other.mapDimensionProperties)){ + this->setMapGridSize(other.mapDimensionProperties.getMapDimensions()); + } + + this->mapDimensionProperties = other.mapDimensionProperties; + + this->worldTmap = other.worldTmap; + this->mapTworld = other.mapTworld; + this->worldTmap3D = other.worldTmap3D; + + this->scaleToMap = other.scaleToMap; + + //@todo potential resize + int sizeX = this->getSizeX(); + int sizeY = this->getSizeY(); + + size_t concreteCellSize = sizeof(ConcreteCellType); + + memcpy(this->mapArray, other.mapArray, sizeX*sizeY*concreteCellSize); + + return *this; + } + + /** + * Returns the world coordinates for the given map coords. + */ + inline Eigen::Vector2f getWorldCoords(const Eigen::Vector2f& mapCoords) const + { + return worldTmap * mapCoords; + } + + /** + * Returns the map coordinates for the given world coords. + */ + inline Eigen::Vector2f getMapCoords(const Eigen::Vector2f& worldCoords) const + { + return mapTworld * worldCoords; + } + + /** + * Returns the world pose for the given map pose. + */ + inline Eigen::Vector3f getWorldCoordsPose(const Eigen::Vector3f& mapPose) const + { + Eigen::Vector2f worldCoords (worldTmap * mapPose.head<2>()); + return Eigen::Vector3f(worldCoords[0], worldCoords[1], mapPose[2]); + } + + /** + * Returns the map pose for the given world pose. + */ + inline Eigen::Vector3f getMapCoordsPose(const Eigen::Vector3f& worldPose) const + { + Eigen::Vector2f mapCoords (mapTworld * worldPose.head<2>()); + return Eigen::Vector3f(mapCoords[0], mapCoords[1], worldPose[2]); + } + + void setDimensionProperties(const Eigen::Vector2f& topLeftOffsetIn, const Eigen::Vector2i& mapDimensionsIn, float cellLengthIn) + { + setDimensionProperties(MapDimensionProperties(topLeftOffsetIn,mapDimensionsIn,cellLengthIn)); + } + + void setDimensionProperties(const MapDimensionProperties& newMapDimProps) + { + //Grid map cell number has changed + if (!newMapDimProps.hasEqualDimensionProperties(this->mapDimensionProperties)){ + this->setMapGridSize(newMapDimProps.getMapDimensions()); + } + + //Grid map transformation/cell size has changed + if(!newMapDimProps.hasEqualTransformationProperties(this->mapDimensionProperties)){ + this->setMapTransformation(newMapDimProps.getTopLeftOffset(), newMapDimProps.getCellLength()); + } + } + + /** + * Set the map transformations + * @param xWorld The origin of the map coordinate system on the x axis in world coordinates + * @param yWorld The origin of the map coordinate system on the y axis in world coordinates + * @param The cell length of the grid map + */ + void setMapTransformation(const Eigen::Vector2f& topLeftOffset, float cellLength) + { + mapDimensionProperties.setCellLength(cellLength); + mapDimensionProperties.setTopLeftOffset(topLeftOffset); + + scaleToMap = 1.0f / cellLength; + + mapTworld = Eigen::AlignedScaling2f(scaleToMap, scaleToMap) * Eigen::Translation2f(topLeftOffset[0], topLeftOffset[1]); + + worldTmap3D = Eigen::AlignedScaling3f(scaleToMap, scaleToMap, 1.0f) * Eigen::Translation3f(topLeftOffset[0], topLeftOffset[1], 0); + + //std::cout << worldTmap3D.matrix() << std::endl; + worldTmap3D = worldTmap3D.inverse(); + + worldTmap = mapTworld.inverse(); + } + + + /** + * Returns the scale factor for one unit in world coords to one unit in map coords. + * @return The scale factor + */ + float getScaleToMap() const + { + return scaleToMap; + } + + /** + * Returns the cell edge length of grid cells in millimeters. + * @return the cell edge length in millimeters. + */ + float getCellLength() const + { + return mapDimensionProperties.getCellLength(); + } + + /** + * Returns a reference to the homogenous 2D transform from map to world coordinates. + * @return The homogenous 2D transform. + */ + const Eigen::Affine2f& getWorldTmap() const + { + return worldTmap; + } + + /** + * Returns a reference to the homogenous 3D transform from map to world coordinates. + * @return The homogenous 3D transform. + */ + const Eigen::Affine3f& getWorldTmap3D() const + { + return worldTmap3D; + } + + /** + * Returns a reference to the homogenous 2D transform from world to map coordinates. + * @return The homogenous 2D transform. + */ + const Eigen::Affine2f& getMapTworld() const + { + return mapTworld; + } + + void setUpdated() { lastUpdateIndex++; }; + int getUpdateIndex() const { return lastUpdateIndex; }; + + /** + * Returns the rectangle ([xMin,yMin],[xMax,xMax]) containing non-default cell values + */ + bool getMapExtends(int& xMax, int& yMax, int& xMin, int& yMin) const + { + int lowerStart = -1; + int upperStart = 10000; + + int xMaxTemp = lowerStart; + int yMaxTemp = lowerStart; + int xMinTemp = upperStart; + int yMinTemp = upperStart; + + int sizeX = this->getSizeX(); + int sizeY = this->getSizeY(); + + for (int x = 0; x < sizeX; ++x) { + for (int y = 0; y < sizeY; ++y) { + if (this->mapArray[x][y].getValue() != 0.0f) { + + if (x > xMaxTemp) { + xMaxTemp = x; + } + + if (x < xMinTemp) { + xMinTemp = x; + } + + if (y > yMaxTemp) { + yMaxTemp = y; + } + + if (y < yMinTemp) { + yMinTemp = y; + } + } + } + } + + if ((xMaxTemp != lowerStart) && + (yMaxTemp != lowerStart) && + (xMinTemp != upperStart) && + (yMinTemp != upperStart)) { + + xMax = xMaxTemp; + yMax = yMaxTemp; + xMin = xMinTemp; + yMin = yMinTemp; + return true; + } else { + return false; + } + } + +protected: + + ConcreteCellType *mapArray; ///< Map representation used with plain pointer array. + + float scaleToMap; ///< Scaling factor from world to map. + + Eigen::Affine2f worldTmap; ///< Homogenous 2D transform from map to world coordinates. + Eigen::Affine3f worldTmap3D; ///< Homogenous 3D transform from map to world coordinates. + Eigen::Affine2f mapTworld; ///< Homogenous 2D transform from world to map coordinates. + + MapDimensionProperties mapDimensionProperties; + int sizeX; + +private: + int lastUpdateIndex; +}; + +} + +#endif diff --git a/Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/map/GridMapCacheArray.h b/Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/map/GridMapCacheArray.h new file mode 100644 index 0000000..d4b3ea6 --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/map/GridMapCacheArray.h @@ -0,0 +1,167 @@ +//================================================================================================= +// Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt +// All rights reserved. + +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of the Simulation, Systems Optimization and Robotics +// group, TU Darmstadt nor the names of its contributors may be used to +// endorse or promote products derived from this software without +// specific prior written permission. + +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +//================================================================================================= + +#ifndef __GridMapCacheArray_h_ +#define __GridMapCacheArray_h_ + +#include + +class CachedMapElement +{ +public: + float val; + int index; +}; + +/** + * Caches filtered grid map accesses in a two dimensional array of the same size as the map. + */ +class GridMapCacheArray +{ +public: + + /** + * Constructor + */ + GridMapCacheArray() + : cacheArray(0) + , arrayDimensions(-1,-1) + { + currCacheIndex = 0; + } + + /** + * Destructor + */ + ~GridMapCacheArray() + { + deleteCacheArray(); + } + + /** + * Resets/deletes the cached data + */ + void resetCache() + { + currCacheIndex++; + } + + /** + * Checks wether cached data for coords is available. If this is the case, writes data into val. + * @param coords The coordinates + * @param val Reference to a float the data is written to if available + * @return Indicates if cached data is available + */ + bool containsCachedData(int index, float& val) + { + const CachedMapElement& elem (cacheArray[index]); + + if (elem.index == currCacheIndex) { + val = elem.val; + return true; + } else { + return false; + } + } + + /** + * Caches float value val for coordinates coords. + * @param coords The coordinates + * @param val The value to be cached for coordinates. + */ + void cacheData(int index, float val) + { + CachedMapElement& elem (cacheArray[index]); + elem.index = currCacheIndex; + elem.val = val; + } + + /** + * Sets the map size and resizes the cache array accordingly + * @param sizeIn The map size. + */ + void setMapSize(const Eigen::Vector2i& newDimensions) + { + setArraySize(newDimensions); + } + +protected: + + /** + * Creates a cache array of size sizeIn. + * @param sizeIn The size of the array + */ + void createCacheArray(const Eigen::Vector2i& newDimensions) + { + arrayDimensions = newDimensions; + + int sizeX = arrayDimensions[0]; + int sizeY = arrayDimensions[1]; + + int size = sizeX * sizeY; + + cacheArray = new CachedMapElement [size]; + + for (int x = 0; x < size; ++x) { + cacheArray[x].index = -1; + } + } + + /** + * Deletes the existing cache array. + */ + void deleteCacheArray() + { + delete[] cacheArray; + } + + /** + * Sets a new cache array size + */ + void setArraySize(const Eigen::Vector2i& newDimensions) + { + if (this->arrayDimensions != newDimensions) { + if (cacheArray != 0) { + deleteCacheArray(); + cacheArray = 0; + } + createCacheArray(newDimensions); + } + } + +protected: + + CachedMapElement* cacheArray; ///< Array used for caching data. + int currCacheIndex; ///< The cache iteration index value + + Eigen::Vector2i arrayDimensions; ///< The size of the array + +}; + + +#endif diff --git a/Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/map/GridMapLogOdds.h b/Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/map/GridMapLogOdds.h new file mode 100644 index 0000000..cfbbf7a --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/map/GridMapLogOdds.h @@ -0,0 +1,212 @@ +//================================================================================================= +// Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt +// All rights reserved. + +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of the Simulation, Systems Optimization and Robotics +// group, TU Darmstadt nor the names of its contributors may be used to +// endorse or promote products derived from this software without +// specific prior written permission. + +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +//================================================================================================= + +#ifndef __GridMapLogOdds_h_ +#define __GridMapLogOdds_h_ + +#include + +/** + * Provides a log odds of occupancy probability representation for cells in a occupancy grid map. + */ +class LogOddsCell +{ +public: + + /* + void setOccupied() + { + logOddsVal += 0.7f; + }; + + void setFree() + { + logOddsVal -= 0.4f; + }; + */ + + + /** + * Sets the cell value to val. + * @param val The log odds value. + */ + void set(float val) + { + logOddsVal = val; + } + + /** + * Returns the value of the cell. + * @return The log odds value. + */ + float getValue() const + { + return logOddsVal; + } + + /** + * Returns wether the cell is occupied. + * @return Cell is occupied + */ + bool isOccupied() const + { + return logOddsVal > 0.0f; + } + + bool isFree() const + { + return logOddsVal < 0.0f; + } + + /** + * Reset Cell to prior probability. + */ + void resetGridCell() + { + logOddsVal = 0.0f; + updateIndex = -1; + } + + //protected: + +public: + + float logOddsVal; ///< The log odds representation of occupancy probability. + int updateIndex; + + +}; + +/** + * Provides functions related to a log odds of occupancy probability respresentation for cells in a occupancy grid map. + */ +class GridMapLogOddsFunctions +{ +public: + + /** + * Constructor, sets parameters like free and occupied log odds ratios. + */ + GridMapLogOddsFunctions() + { + this->setUpdateFreeFactor(0.4f); + this->setUpdateOccupiedFactor(0.6f); + /* + //float probOccupied = 0.6f; + float probOccupied = 0.9f; + float oddsOccupied = probOccupied / (1.0f - probOccupied); + logOddsOccupied = log(oddsOccupied); + + float probFree = 0.4f; + float oddsFree = probFree / (1.0f - probFree); + logOddsFree = log(oddsFree); + */ + } + + /** + * Update cell as occupied + * @param cell The cell. + */ + void updateSetOccupied(LogOddsCell& cell) const + { + if (cell.logOddsVal < 50.0f){ + cell.logOddsVal += logOddsOccupied; + } + } + + /** + * Update cell as free + * @param cell The cell. + */ + void updateSetFree(LogOddsCell& cell) const + { + + cell.logOddsVal += logOddsFree; + + } + + void updateUnsetFree(LogOddsCell& cell) const + { + cell.logOddsVal -= logOddsFree; + } + + /** + * Get the probability value represented by the grid cell. + * @param cell The cell. + * @return The probability + */ + float getGridProbability(const LogOddsCell& cell) const + { + float odds = exp(cell.logOddsVal); + return odds / (odds + 1.0f); + + /* + float val = cell.logOddsVal; + + //prevent #IND when doing exp(large number). + if (val > 50.0f) { + return 1.0f; + } else { + float odds = exp(val); + return odds / (odds + 1.0f); + } + */ + //return 0.5f; + } + + //void getGridValueMap( const LogOddsCell& cell) const{}; + //void isOccupied(LogOddsCell& cell) {}; + + //void resetGridCell() {}; + + void setUpdateFreeFactor(float factor) + { + logOddsFree = probToLogOdds(factor); + } + + void setUpdateOccupiedFactor(float factor) + { + logOddsOccupied = probToLogOdds(factor); + } + +protected: + + float probToLogOdds(float prob) + { + float odds = prob / (1.0f - prob); + return log(odds); + } + + float logOddsOccupied; /// < The log odds representation of probability used for updating cells as occupied + float logOddsFree; /// < The log odds representation of probability used for updating cells as free + + +}; + + +#endif diff --git a/Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/map/GridMapReflectanceCount.h b/Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/map/GridMapReflectanceCount.h new file mode 100644 index 0000000..d83ab30 --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/map/GridMapReflectanceCount.h @@ -0,0 +1,111 @@ +//================================================================================================= +// Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt +// All rights reserved. + +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of the Simulation, Systems Optimization and Robotics +// group, TU Darmstadt nor the names of its contributors may be used to +// endorse or promote products derived from this software without +// specific prior written permission. + +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +//================================================================================================= + +#ifndef __GridMapReflectanceCount_h_ +#define __GridMapReflectanceCount_h_ + +/** + * Provides a reflectance count representation for cells in a occupancy grid map. + */ +class ReflectanceCell +{ +public: + + void set(float val) + { + probOccupied = val; + } + + float getValue() const + { + return probOccupied; + } + + bool isOccupied() const + { + return probOccupied > 0.5f; + } + + bool isFree() const{ + return probOccupied < 0.5f; + } + + void resetGridCell() + { + probOccupied = 0.5f; + visitedCount = 0.0f; + reflectedCount = 0.0f; + updateIndex = -1; + } + +//protected: + + float visitedCount; + float reflectedCount; + float probOccupied; + int updateIndex; +}; + + +class GridMapReflectanceFunctions +{ +public: + + GridMapReflectanceFunctions() + {} + + void updateSetOccupied(ReflectanceCell& cell) const + { + ++cell.reflectedCount; + ++cell.visitedCount; + cell.probOccupied = cell.reflectedCount / cell.visitedCount; + } + + void updateSetFree(ReflectanceCell& cell) const + { + ++cell.visitedCount; + cell.probOccupied = cell.reflectedCount / cell.visitedCount; + } + + void updateUnsetFree(ReflectanceCell& cell) const + { + --cell.visitedCount; + cell.probOccupied = cell.reflectedCount / cell.visitedCount; + } + + float getGridProbability(const ReflectanceCell& cell) const + { + return cell.probOccupied; + } + +protected: + +}; + + +#endif diff --git a/Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/map/GridMapSimpleCount.h b/Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/map/GridMapSimpleCount.h new file mode 100644 index 0000000..887df08 --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/map/GridMapSimpleCount.h @@ -0,0 +1,159 @@ +//================================================================================================= +// Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt +// All rights reserved. + +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of the Simulation, Systems Optimization and Robotics +// group, TU Darmstadt nor the names of its contributors may be used to +// endorse or promote products derived from this software without +// specific prior written permission. + +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +//================================================================================================= + +#ifndef __GridMapSimpleCount_h_ +#define __GridMapSimpleCount_h_ + + +/** + * Provides a (very) simple count based representation of occupancy + */ +class SimpleCountCell +{ +public: + + /** + * Sets the cell value to val. + * @param val The log odds value. + */ + void set(float val) + { + simpleOccVal = val; + } + + /** + * Returns the value of the cell. + * @return The log odds value. + */ + float getValue() const + { + return simpleOccVal; + } + + /** + * Returns wether the cell is occupied. + * @return Cell is occupied + */ + bool isOccupied() const + { + return (simpleOccVal > 0.5f); + } + + bool isFree() const + { + return (simpleOccVal < 0.5f); + } + + /** + * Reset Cell to prior probability. + */ + void resetGridCell() + { + simpleOccVal = 0.5f; + updateIndex = -1; + } + +//protected: + +public: + + float simpleOccVal; ///< The log odds representation of occupancy probability. + int updateIndex; + + +}; + +/** + * Provides functions related to a log odds of occupancy probability respresentation for cells in a occupancy grid map. + */ +class GridMapSimpleCountFunctions +{ +public: + + /** + * Constructor, sets parameters like free and occupied log odds ratios. + */ + GridMapSimpleCountFunctions() + { + updateFreeVal = -0.10f; + updateOccVal = 0.15f; + + updateFreeLimit = -updateFreeVal + updateFreeVal/100.0f; + updateOccLimit = 1.0f - (updateOccVal + updateOccVal/100.0f); + } + + /** + * Update cell as occupied + * @param cell The cell. + */ + void updateSetOccupied(SimpleCountCell& cell) const + { + if (cell.simpleOccVal < updateOccLimit){ + cell.simpleOccVal += updateOccVal; + } + } + + /** + * Update cell as free + * @param cell The cell. + */ + void updateSetFree(SimpleCountCell& cell) const + { + if (cell.simpleOccVal > updateFreeLimit){ + cell.simpleOccVal += updateFreeVal; + } + } + + void updateUnsetFree(SimpleCountCell& cell) const + { + cell.simpleOccVal -= updateFreeVal; + } + + /** + * Get the probability value represented by the grid cell. + * @param cell The cell. + * @return The probability + */ + float getGridProbability(const SimpleCountCell& cell) const + { + return cell.simpleOccVal; + } + +protected: + + float updateFreeVal; + float updateOccVal; + + float updateFreeLimit; + float updateOccLimit; + + +}; + + +#endif diff --git a/Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/map/MapDimensionProperties.h b/Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/map/MapDimensionProperties.h new file mode 100644 index 0000000..fd3915b --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/map/MapDimensionProperties.h @@ -0,0 +1,100 @@ +//================================================================================================= +// Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt +// All rights reserved. + +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of the Simulation, Systems Optimization and Robotics +// group, TU Darmstadt nor the names of its contributors may be used to +// endorse or promote products derived from this software without +// specific prior written permission. + +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +//================================================================================================= + +#ifndef __MapDimensionProperties_h_ +#define __MapDimensionProperties_h_ + +class MapDimensionProperties +{ +public: + MapDimensionProperties() + : topLeftOffset(-1.0f,-1.0f) + , mapDimensions(-1,-1) + , cellLength(-1.0f) + {} + + + MapDimensionProperties(const Eigen::Vector2f& topLeftOffsetIn, const Eigen::Vector2i& mapDimensionsIn, float cellLengthIn) + : topLeftOffset(topLeftOffsetIn) + , mapDimensions(mapDimensionsIn) + , cellLength(cellLengthIn) + { + mapLimitsf = (mapDimensionsIn.cast()).array() - 1.0f; + } + + bool operator==(const MapDimensionProperties& other) const + { + return (topLeftOffset == other.topLeftOffset) && (mapDimensions == other.mapDimensions) && (cellLength == other.cellLength); + } + + bool hasEqualDimensionProperties(const MapDimensionProperties& other) const + { + return (mapDimensions == other.mapDimensions); + } + + bool hasEqualTransformationProperties(const MapDimensionProperties& other) const + { + return (topLeftOffset == other.topLeftOffset) && (cellLength == other.cellLength); + } + + bool pointOutOfMapBounds(const Eigen::Vector2f& coords) const + { + return ((coords[0] < 0.0f) || (coords[0] > mapLimitsf[0]) || (coords[1] < 0.0f) || (coords[1] > mapLimitsf[1])); + } + + void setMapCellDims(const Eigen::Vector2i& newDims) + { + mapDimensions = newDims; + mapLimitsf = (newDims.cast()).array() - 2.0f; + } + + void setTopLeftOffset(const Eigen::Vector2f& topLeftOffsetIn) + { + topLeftOffset = topLeftOffsetIn; + } + + void setSizeX(int sX) { mapDimensions[0] = sX; }; + void setSizeY(int sY) { mapDimensions[1] = sY; }; + void setCellLength(float cl) { cellLength = cl; }; + + const Eigen::Vector2f& getTopLeftOffset() const { return topLeftOffset; }; + const Eigen::Vector2i& getMapDimensions() const { return mapDimensions; }; + int getSizeX() const { return mapDimensions[0]; }; + int getSizeY() const { return mapDimensions[1]; }; + float getCellLength() const { return cellLength; }; + +protected: + Eigen::Vector2f topLeftOffset; + Eigen::Vector2i mapDimensions; + Eigen::Vector2f mapLimitsf; + float cellLength; +}; + +#endif + + diff --git a/Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/map/OccGridMapBase.h b/Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/map/OccGridMapBase.h new file mode 100644 index 0000000..e3b439a --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/map/OccGridMapBase.h @@ -0,0 +1,273 @@ +//================================================================================================= +// Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt +// All rights reserved. + +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of the Simulation, Systems Optimization and Robotics +// group, TU Darmstadt nor the names of its contributors may be used to +// endorse or promote products derived from this software without +// specific prior written permission. + +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +//================================================================================================= + +#ifndef __OccGridMapBase_h_ +#define __OccGridMapBase_h_ + +#include "GridMapBase.h" + +#include "../scan/DataPointContainer.h" +#include "../util/UtilFunctions.h" + +#include + +namespace hectorslam { + +template +class OccGridMapBase + : public GridMapBase +{ + +public: + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + OccGridMapBase(float mapResolution, const Eigen::Vector2i& size, const Eigen::Vector2f& offset) + : GridMapBase(mapResolution, size, offset) + , currUpdateIndex(0) + , currMarkOccIndex(-1) + , currMarkFreeIndex(-1) + {} + + virtual ~OccGridMapBase() {} + + void updateSetOccupied(int index) + { + concreteGridFunctions.updateSetOccupied(this->getCell(index)); + } + + void updateSetFree(int index) + { + concreteGridFunctions.updateSetFree(this->getCell(index)); + } + + void updateUnsetFree(int index) + { + concreteGridFunctions.updateUnsetFree(this->getCell(index)); + } + + float getGridProbabilityMap(int index) const + { + return concreteGridFunctions.getGridProbability(this->getCell(index)); + } + + bool isOccupied(int xMap, int yMap) const + { + return (this->getCell(xMap,yMap).isOccupied()); + } + + bool isFree(int xMap, int yMap) const + { + return (this->getCell(xMap,yMap).isFree()); + } + + bool isOccupied(int index) const + { + return (this->getCell(index).isOccupied()); + } + + bool isFree(int index) const + { + return (this->getCell(index).isFree()); + } + + float getObstacleThreshold() const + { + ConcreteCellType temp; + temp.resetGridCell(); + return concreteGridFunctions.getGridProbability(temp); + } + + void setUpdateFreeFactor(float factor) + { + concreteGridFunctions.setUpdateFreeFactor(factor); + } + + void setUpdateOccupiedFactor(float factor) + { + concreteGridFunctions.setUpdateOccupiedFactor(factor); + } + + /** + * Updates the map using the given scan data and robot pose + * @param dataContainer Contains the laser scan data + * @param robotPoseWorld The 2D robot pose in world coordinates + */ + void updateByScan(const DataContainer& dataContainer, const Eigen::Vector3f& robotPoseWorld) + { + currMarkFreeIndex = currUpdateIndex + 1; + currMarkOccIndex = currUpdateIndex + 2; + + //Get pose in map coordinates from pose in world coordinates + Eigen::Vector3f mapPose(this->getMapCoordsPose(robotPoseWorld)); + + //Get a 2D homogenous transform that can be left-multiplied to a robot coordinates vector to get world coordinates of that vector + Eigen::Affine2f poseTransform((Eigen::Translation2f( + mapPose[0], mapPose[1]) * Eigen::Rotation2Df(mapPose[2]))); + + //Get start point of all laser beams in map coordinates (same for alle beams, stored in robot coords in dataContainer) + Eigen::Vector2f scanBeginMapf(poseTransform * dataContainer.getOrigo()); + + //Get integer vector of laser beams start point + Eigen::Vector2i scanBeginMapi(scanBeginMapf[0] + 0.5f, scanBeginMapf[1] + 0.5f); + + //Get number of valid beams in current scan + int numValidElems = dataContainer.getSize(); + + //std::cout << "\n maxD: " << maxDist << " num: " << numValidElems << "\n"; + + //Iterate over all valid laser beams + for (int i = 0; i < numValidElems; ++i) { + + //Get map coordinates of current beam endpoint + Eigen::Vector2f scanEndMapf(poseTransform * (dataContainer.getVecEntry(i))); + //std::cout << "\ns\n" << scanEndMapf << "\n"; + + //add 0.5 to beam endpoint vector for following integer cast (to round, not truncate) + scanEndMapf.array() += (0.5f); + + //Get integer map coordinates of current beam endpoint + Eigen::Vector2i scanEndMapi(scanEndMapf.cast()); + + //Update map using a bresenham variant for drawing a line from beam start to beam endpoint in map coordinates + if (scanBeginMapi != scanEndMapi){ + updateLineBresenhami(scanBeginMapi, scanEndMapi); + } + } + + //Tell the map that it has been updated + this->setUpdated(); + + //Increase update index (used for updating grid cells only once per incoming scan) + currUpdateIndex += 3; + } + + inline void updateLineBresenhami( const Eigen::Vector2i& beginMap, const Eigen::Vector2i& endMap, unsigned int max_length = UINT_MAX){ + + int x0 = beginMap[0]; + int y0 = beginMap[1]; + + //check if beam start point is inside map, cancel update if this is not the case + if ((x0 < 0) || (x0 >= this->getSizeX()) || (y0 < 0) || (y0 >= this->getSizeY())) { + return; + } + + int x1 = endMap[0]; + int y1 = endMap[1]; + + //std::cout << " x: "<< x1 << " y: " << y1 << " length: " << length << " "; + + //check if beam end point is inside map, cancel update if this is not the case + if ((x1 < 0) || (x1 >= this->getSizeX()) || (y1 < 0) || (y1 >= this->getSizeY())) { + return; + } + + int dx = x1 - x0; + int dy = y1 - y0; + + unsigned int abs_dx = abs(dx); + unsigned int abs_dy = abs(dy); + + int offset_dx = util::sign(dx); + int offset_dy = util::sign(dy) * this->sizeX; + + unsigned int startOffset = beginMap.y() * this->sizeX + beginMap.x(); + + //if x is dominant + if(abs_dx >= abs_dy){ + int error_y = abs_dx / 2; + bresenham2D(abs_dx, abs_dy, error_y, offset_dx, offset_dy, startOffset); + }else{ + //otherwise y is dominant + int error_x = abs_dy / 2; + bresenham2D(abs_dy, abs_dx, error_x, offset_dy, offset_dx, startOffset); + } + + unsigned int endOffset = endMap.y() * this->sizeX + endMap.x(); + this->bresenhamCellOcc(endOffset); + + } + + inline void bresenhamCellFree(unsigned int offset) + { + ConcreteCellType& cell (this->getCell(offset)); + + if (cell.updateIndex < currMarkFreeIndex) { + concreteGridFunctions.updateSetFree(cell); + cell.updateIndex = currMarkFreeIndex; + } + } + + inline void bresenhamCellOcc(unsigned int offset) + { + ConcreteCellType& cell (this->getCell(offset)); + + if (cell.updateIndex < currMarkOccIndex) { + + //if this cell has been updated as free in the current iteration, revert this + if (cell.updateIndex == currMarkFreeIndex) { + concreteGridFunctions.updateUnsetFree(cell); + } + + concreteGridFunctions.updateSetOccupied(cell); + //std::cout << " setOcc " << "\n"; + cell.updateIndex = currMarkOccIndex; + } + } + + inline void bresenham2D( unsigned int abs_da, unsigned int abs_db, int error_b, int offset_a, int offset_b, unsigned int offset){ + + this->bresenhamCellFree(offset); + + unsigned int end = abs_da-1; + + for(unsigned int i = 0; i < end; ++i){ + offset += offset_a; + error_b += abs_db; + + if((unsigned int)error_b >= abs_da){ + offset += offset_b; + error_b -= abs_da; + } + + this->bresenhamCellFree(offset); + } + } + +protected: + + ConcreteGridFunctions concreteGridFunctions; + int currUpdateIndex; + int currMarkOccIndex; + int currMarkFreeIndex; +}; + + +} + +#endif diff --git a/Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/map/OccGridMapUtil.h b/Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/map/OccGridMapUtil.h new file mode 100644 index 0000000..d166391 --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/map/OccGridMapUtil.h @@ -0,0 +1,392 @@ +//================================================================================================= +// Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt +// All rights reserved. + +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of the Simulation, Systems Optimization and Robotics +// group, TU Darmstadt nor the names of its contributors may be used to +// endorse or promote products derived from this software without +// specific prior written permission. + +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +//================================================================================================= + +#ifndef __OccGridMapUtil_h_ +#define __OccGridMapUtil_h_ + +#include + +#include "../scan/DataPointContainer.h" +#include "../util/UtilFunctions.h" + +namespace hectorslam { + +template +class OccGridMapUtil +{ +public: + + OccGridMapUtil(const ConcreteOccGridMap* gridMap) + : concreteGridMap(gridMap) + , size(0) + { + mapObstacleThreshold = gridMap->getObstacleThreshold(); + cacheMethod.setMapSize(gridMap->getMapDimensions()); + } + + ~OccGridMapUtil() + {} + +public: + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + inline Eigen::Vector3f getWorldCoordsPose(const Eigen::Vector3f& mapPose) const { return concreteGridMap->getWorldCoordsPose(mapPose); }; + inline Eigen::Vector3f getMapCoordsPose(const Eigen::Vector3f& worldPose) const { return concreteGridMap->getMapCoordsPose(worldPose); }; + + inline Eigen::Vector2f getWorldCoordsPoint(const Eigen::Vector2f& mapPoint) const { return concreteGridMap->getWorldCoords(mapPoint); }; + + void getCompleteHessianDerivs(const Eigen::Vector3f& pose, const DataContainer& dataPoints, Eigen::Matrix3f& H, Eigen::Vector3f& dTr) + { + int size = dataPoints.getSize(); + + Eigen::Affine2f transform(getTransformForState(pose)); + + float sinRot = sin(pose[2]); + float cosRot = cos(pose[2]); + + H = Eigen::Matrix3f::Zero(); + dTr = Eigen::Vector3f::Zero(); + + for (int i = 0; i < size; ++i) { + + const Eigen::Vector2f& currPoint (dataPoints.getVecEntry(i)); + + Eigen::Vector3f transformedPointData(interpMapValueWithDerivatives(transform * currPoint)); + + float funVal = 1.0f - transformedPointData[0]; + + dTr[0] += transformedPointData[1] * funVal; + dTr[1] += transformedPointData[2] * funVal; + + float rotDeriv = ((-sinRot * currPoint.x() - cosRot * currPoint.y()) * transformedPointData[1] + (cosRot * currPoint.x() - sinRot * currPoint.y()) * transformedPointData[2]); + + dTr[2] += rotDeriv * funVal; + + H(0, 0) += util::sqr(transformedPointData[1]); + H(1, 1) += util::sqr(transformedPointData[2]); + H(2, 2) += util::sqr(rotDeriv); + + H(0, 1) += transformedPointData[1] * transformedPointData[2]; + H(0, 2) += transformedPointData[1] * rotDeriv; + H(1, 2) += transformedPointData[2] * rotDeriv; + } + + H(1, 0) = H(0, 1); + H(2, 0) = H(0, 2); + H(2, 1) = H(1, 2); + + } + + Eigen::Matrix3f getCovarianceForPose(const Eigen::Vector3f& mapPose, const DataContainer& dataPoints) + { + + float deltaTransX = 1.5f; + float deltaTransY = 1.5f; + float deltaAng = 0.05f; + + float x = mapPose[0]; + float y = mapPose[1]; + float ang = mapPose[2]; + + Eigen::Matrix sigmaPoints; + + sigmaPoints.block<3, 1>(0, 0) = Eigen::Vector3f(x + deltaTransX, y, ang); + sigmaPoints.block<3, 1>(0, 1) = Eigen::Vector3f(x - deltaTransX, y, ang); + sigmaPoints.block<3, 1>(0, 2) = Eigen::Vector3f(x, y + deltaTransY, ang); + sigmaPoints.block<3, 1>(0, 3) = Eigen::Vector3f(x, y - deltaTransY, ang); + sigmaPoints.block<3, 1>(0, 4) = Eigen::Vector3f(x, y, ang + deltaAng); + sigmaPoints.block<3, 1>(0, 5) = Eigen::Vector3f(x, y, ang - deltaAng); + sigmaPoints.block<3, 1>(0, 6) = mapPose; + + Eigen::Matrix likelihoods; + + likelihoods[0] = getLikelihoodForState(Eigen::Vector3f(x + deltaTransX, y, ang), dataPoints); + likelihoods[1] = getLikelihoodForState(Eigen::Vector3f(x - deltaTransX, y, ang), dataPoints); + likelihoods[2] = getLikelihoodForState(Eigen::Vector3f(x, y + deltaTransY, ang), dataPoints); + likelihoods[3] = getLikelihoodForState(Eigen::Vector3f(x, y - deltaTransY, ang), dataPoints); + likelihoods[4] = getLikelihoodForState(Eigen::Vector3f(x, y, ang + deltaAng), dataPoints); + likelihoods[5] = getLikelihoodForState(Eigen::Vector3f(x, y, ang - deltaAng), dataPoints); + likelihoods[6] = getLikelihoodForState(Eigen::Vector3f(x, y, ang), dataPoints); + + float invLhNormalizer = 1 / likelihoods.sum(); + + std::cout << "\n lhs:\n" << likelihoods; + + Eigen::Vector3f mean(Eigen::Vector3f::Zero()); + + for (int i = 0; i < 7; ++i) { + mean += (sigmaPoints.block<3, 1>(0, i) * likelihoods[i]); + } + + mean *= invLhNormalizer; + + Eigen::Matrix3f covMatrixMap(Eigen::Matrix3f::Zero()); + + for (int i = 0; i < 7; ++i) { + Eigen::Vector3f sigPointMinusMean(sigmaPoints.block<3, 1>(0, i) - mean); + covMatrixMap += (likelihoods[i] * invLhNormalizer) * (sigPointMinusMean * (sigPointMinusMean.transpose())); + } + + return covMatrixMap; + + //covMatrix.cwise() * invLhNormalizer; + //transform = getTransformForState(Eigen::Vector3f(x-deltaTrans, y, ang); + } + + Eigen::Matrix3f getCovMatrixWorldCoords(const Eigen::Matrix3f& covMatMap) + { + + //std::cout << "\nCovMap:\n" << covMatMap; + + Eigen::Matrix3f covMatWorld; + + float scaleTrans = concreteGridMap->getCellLength(); + float scaleTransSq = util::sqr(scaleTrans); + + covMatWorld(0, 0) = covMatMap(0, 0) * scaleTransSq; + covMatWorld(1, 1) = covMatMap(1, 1) * scaleTransSq; + + covMatWorld(1, 0) = covMatMap(1, 0) * scaleTransSq; + covMatWorld(0, 1) = covMatWorld(1, 0); + + covMatWorld(2, 0) = covMatMap(2, 0) * scaleTrans; + covMatWorld(0, 2) = covMatWorld(2, 0); + + covMatWorld(2, 1) = covMatMap(2, 1) * scaleTrans; + covMatWorld(1, 2) = covMatWorld(2, 1); + + covMatWorld(2, 2) = covMatMap(2, 2); + + return covMatWorld; + } + + float getLikelihoodForState(const Eigen::Vector3f& state, const DataContainer& dataPoints) + { + float resid = getResidualForState(state, dataPoints); + + return getLikelihoodForResidual(resid, dataPoints.getSize()); + } + + float getLikelihoodForResidual(float residual, int numDataPoints) + { + float numDataPointsA = static_cast(numDataPoints); + float sizef = static_cast(numDataPointsA); + + return 1 - (residual / sizef); + } + + float getResidualForState(const Eigen::Vector3f& state, const DataContainer& dataPoints) + { + int size = dataPoints.getSize(); + + int stepSize = 1; + float residual = 0.0f; + + + Eigen::Affine2f transform(getTransformForState(state)); + + for (int i = 0; i < size; i += stepSize) { + + float funval = 1.0f - interpMapValue(transform * dataPoints.getVecEntry(i)); + residual += funval; + } + + return residual; + } + + float getUnfilteredGridPoint(Eigen::Vector2i& gridCoords) const + { + return (concreteGridMap->getGridProbabilityMap(gridCoords.x(), gridCoords.y())); + } + + float getUnfilteredGridPoint(int index) const + { + return (concreteGridMap->getGridProbabilityMap(index)); + } + + float interpMapValue(const Eigen::Vector2f& coords) + { + //check if coords are within map limits. + if (concreteGridMap->pointOutOfMapBounds(coords)){ + return 0.0f; + } + + //map coords are alway positive, floor them by casting to int + Eigen::Vector2i indMin(coords.cast()); + + //get factors for bilinear interpolation + Eigen::Vector2f factors(coords - indMin.cast()); + + int sizeX = concreteGridMap->getSizeX(); + + int index = indMin[1] * sizeX + indMin[0]; + + // get grid values for the 4 grid points surrounding the current coords. Check cached data first, if not contained + // filter gridPoint with gaussian and store in cache. + if (!cacheMethod.containsCachedData(index, intensities[0])) { + intensities[0] = getUnfilteredGridPoint(index); + cacheMethod.cacheData(index, intensities[0]); + } + + ++index; + + if (!cacheMethod.containsCachedData(index, intensities[1])) { + intensities[1] = getUnfilteredGridPoint(index); + cacheMethod.cacheData(index, intensities[1]); + } + + index += sizeX-1; + + if (!cacheMethod.containsCachedData(index, intensities[2])) { + intensities[2] = getUnfilteredGridPoint(index); + cacheMethod.cacheData(index, intensities[2]); + } + + ++index; + + if (!cacheMethod.containsCachedData(index, intensities[3])) { + intensities[3] = getUnfilteredGridPoint(index); + cacheMethod.cacheData(index, intensities[3]); + } + + float xFacInv = (1.0f - factors[0]); + float yFacInv = (1.0f - factors[1]); + + return + ((intensities[0] * xFacInv + intensities[1] * factors[0]) * (yFacInv)) + + ((intensities[2] * xFacInv + intensities[3] * factors[0]) * (factors[1])); + + } + + Eigen::Vector3f interpMapValueWithDerivatives(const Eigen::Vector2f& coords) + { + //check if coords are within map limits. + if (concreteGridMap->pointOutOfMapBounds(coords)){ + return Eigen::Vector3f(0.0f, 0.0f, 0.0f); + } + + //map coords are always positive, floor them by casting to int + Eigen::Vector2i indMin(coords.cast()); + + //get factors for bilinear interpolation + Eigen::Vector2f factors(coords - indMin.cast()); + + int sizeX = concreteGridMap->getSizeX(); + + int index = indMin[1] * sizeX + indMin[0]; + + // get grid values for the 4 grid points surrounding the current coords. Check cached data first, if not contained + // filter gridPoint with gaussian and store in cache. + if (!cacheMethod.containsCachedData(index, intensities[0])) { + intensities[0] = getUnfilteredGridPoint(index); + cacheMethod.cacheData(index, intensities[0]); + } + + ++index; + + if (!cacheMethod.containsCachedData(index, intensities[1])) { + intensities[1] = getUnfilteredGridPoint(index); + cacheMethod.cacheData(index, intensities[1]); + } + + index += sizeX-1; + + if (!cacheMethod.containsCachedData(index, intensities[2])) { + intensities[2] = getUnfilteredGridPoint(index); + cacheMethod.cacheData(index, intensities[2]); + } + + ++index; + + if (!cacheMethod.containsCachedData(index, intensities[3])) { + intensities[3] = getUnfilteredGridPoint(index); + cacheMethod.cacheData(index, intensities[3]); + } + + float dx1 = intensities[0] - intensities[1]; + float dx2 = intensities[2] - intensities[3]; + + float dy1 = intensities[0] - intensities[2]; + float dy2 = intensities[1] - intensities[3]; + + float xFacInv = (1.0f - factors[0]); + float yFacInv = (1.0f - factors[1]); + + return Eigen::Vector3f( + ((intensities[0] * xFacInv + intensities[1] * factors[0]) * (yFacInv)) + + ((intensities[2] * xFacInv + intensities[3] * factors[0]) * (factors[1])), + -((dx1 * xFacInv) + (dx2 * factors[0])), + -((dy1 * yFacInv) + (dy2 * factors[1])) + ); + } + + Eigen::Affine2f getTransformForState(const Eigen::Vector3f& transVector) const + { + return Eigen::Translation2f(transVector[0], transVector[1]) * Eigen::Rotation2Df(transVector[2]); + } + + Eigen::Translation2f getTranslationForState(const Eigen::Vector3f& transVector) const + { + return Eigen::Translation2f(transVector[0], transVector[1]); + } + + void resetCachedData() + { + cacheMethod.resetCache(); + } + + void resetSamplePoints() + { + samplePoints.clear(); + } + + const std::vector& getSamplePoints() const + { + return samplePoints; + } + +protected: + + Eigen::Vector4f intensities; + + ConcreteCacheMethod cacheMethod; + + const ConcreteOccGridMap* concreteGridMap; + + std::vector samplePoints; + + int size; + + float mapObstacleThreshold; +}; + +} + + +#endif diff --git a/Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/map/OccGridMapUtilConfig.h b/Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/map/OccGridMapUtilConfig.h new file mode 100644 index 0000000..f37f979 --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/map/OccGridMapUtilConfig.h @@ -0,0 +1,58 @@ +//================================================================================================= +// Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt +// All rights reserved. + +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of the Simulation, Systems Optimization and Robotics +// group, TU Darmstadt nor the names of its contributors may be used to +// endorse or promote products derived from this software without +// specific prior written permission. + +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +//================================================================================================= + +#ifndef __OccGridMapUtilConfig_h_ +#define __OccGridMapUtilConfig_h_ + +#include "OccGridMapUtil.h" + +//#define SLAM_USE_HASH_CACHING +#ifdef SLAM_USE_HASH_CACHING +#include "GridMapCacheHash.h" +typedef GridMapCacheHash GridMapCacheMethod; +#else +#include "GridMapCacheArray.h" +typedef GridMapCacheArray GridMapCacheMethod; +#endif + +namespace hectorslam { + +template +class OccGridMapUtilConfig + : public OccGridMapUtil +{ +public: + + OccGridMapUtilConfig(ConcreteOccGridMap* gridMap = 0) + : OccGridMapUtil(gridMap) + {} +}; + +} + +#endif diff --git a/Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/matcher/ScanMatcher.h b/Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/matcher/ScanMatcher.h new file mode 100644 index 0000000..ad777e9 --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/matcher/ScanMatcher.h @@ -0,0 +1,252 @@ +//================================================================================================= +// Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt +// All rights reserved. + +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of the Simulation, Systems Optimization and Robotics +// group, TU Darmstadt nor the names of its contributors may be used to +// endorse or promote products derived from this software without +// specific prior written permission. + +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +//================================================================================================= + +#ifndef _scanmatcher_h__ +#define _scanmatcher_h__ + +#include +#include "../scan/DataPointContainer.h" +#include "../util/UtilFunctions.h" + +#include "../util/DrawInterface.h" +#include "../util/HectorDebugInfoInterface.h" + +namespace hectorslam{ + +template +class ScanMatcher +{ +public: + + ScanMatcher(DrawInterface* drawInterfaceIn = 0, HectorDebugInfoInterface* debugInterfaceIn = 0) + : drawInterface(drawInterfaceIn) + , debugInterface(debugInterfaceIn) + {} + + ~ScanMatcher() + {} + + Eigen::Vector3f matchData(const Eigen::Vector3f& beginEstimateWorld, ConcreteOccGridMapUtil& gridMapUtil, const DataContainer& dataContainer, Eigen::Matrix3f& covMatrix, int maxIterations) + { + if (drawInterface){ + drawInterface->setScale(0.05f); + drawInterface->setColor(0.0f,1.0f, 0.0f); + drawInterface->drawArrow(beginEstimateWorld); + + Eigen::Vector3f beginEstimateMap(gridMapUtil.getMapCoordsPose(beginEstimateWorld)); + + drawScan(beginEstimateMap, gridMapUtil, dataContainer); + + drawInterface->setColor(1.0,0.0,0.0); + } + + if (dataContainer.getSize() != 0) { + + Eigen::Vector3f beginEstimateMap(gridMapUtil.getMapCoordsPose(beginEstimateWorld)); + + Eigen::Vector3f estimate(beginEstimateMap); + + estimateTransformationLogLh(estimate, gridMapUtil, dataContainer); + //bool notConverged = estimateTransformationLogLh(estimate, gridMapUtil, dataContainer); + + /* + const Eigen::Matrix2f& hessian (H.block<2,2>(0,0)); + + + Eigen::SelfAdjointEigenSolver eig(hessian); + + const Eigen::Vector2f& eigValues (eig.eigenvalues()); + + float cond = eigValues[1] / eigValues[0]; + float determinant = (hessian.determinant()); + */ + //std::cout << "\n cond: " << cond << " det: " << determinant << "\n"; + + + int numIter = maxIterations; + + + for (int i = 0; i < numIter; ++i) { + //std::cout << "\nest:\n" << estimate; + + estimateTransformationLogLh(estimate, gridMapUtil, dataContainer); + //notConverged = estimateTransformationLogLh(estimate, gridMapUtil, dataContainer); + + if(drawInterface){ + float invNumIterf = 1.0f/static_cast (numIter); + drawInterface->setColor(static_cast(i)*invNumIterf,0.0f, 0.0f); + drawInterface->drawArrow(gridMapUtil.getWorldCoordsPose(estimate)); + //drawInterface->drawArrow(Eigen::Vector3f(0.0f, static_cast(i)*0.05, 0.0f)); + } + + if(debugInterface){ + debugInterface->addHessianMatrix(H); + } + } + + if (drawInterface){ + drawInterface->setColor(0.0,0.0,1.0); + drawScan(estimate, gridMapUtil, dataContainer); + } + /* + Eigen::Matrix2f testMat(Eigen::Matrix2f::Identity()); + testMat(0,0) = 2.0f; + + float angleWorldCoords = util::toRad(30.0f); + float sinAngle = sin(angleWorldCoords); + float cosAngle = cos(angleWorldCoords); + + Eigen::Matrix2f rotMat; + rotMat << cosAngle, -sinAngle, sinAngle, cosAngle; + Eigen::Matrix2f covarianceRotated (rotMat * testMat * rotMat.transpose()); + + drawInterface->setColor(0.0,0.0,1.0,0.5); + drawInterface->drawCovariance(gridMapUtil.getWorldCoordsPoint(estimate.start<2>()), covarianceRotated); + */ + + + + /* + Eigen::Matrix3f covMatMap (gridMapUtil.getCovarianceForPose(estimate, dataContainer)); + std::cout << "\nestim:" << estimate; + std::cout << "\ncovMap\n" << covMatMap; + drawInterface->setColor(0.0,0.0,1.0,0.5); + + + Eigen::Matrix3f covMatWorld(gridMapUtil.getCovMatrixWorldCoords(covMatMap)); + std::cout << "\ncovWorld\n" << covMatWorld; + + drawInterface->drawCovariance(gridMapUtil.getWorldCoordsPoint(estimate.start<2>()), covMatMap.block<2,2>(0,0)); + + drawInterface->setColor(1.0,0.0,0.0,0.5); + drawInterface->drawCovariance(gridMapUtil.getWorldCoordsPoint(estimate.start<2>()), covMatWorld.block<2,2>(0,0)); + + std::cout << "\nH:\n" << H; + + float determinant = H.determinant(); + std::cout << "\nH_det: " << determinant; + */ + + /* + Eigen::Matrix2f covFromHessian(H.block<2,2>(0,0) * 1.0f); + //std::cout << "\nCovFromHess:\n" << covFromHessian; + + drawInterface->setColor(0.0, 1.0, 0.0, 0.5); + drawInterface->drawCovariance(gridMapUtil.getWorldCoordsPoint(estimate.start<2>()),covFromHessian.inverse()); + + Eigen::Matrix3f covFromHessian3d(H * 1.0f); + //std::cout << "\nCovFromHess:\n" << covFromHessian; + + drawInterface->setColor(1.0, 0.0, 0.0, 0.8); + drawInterface->drawCovariance(gridMapUtil.getWorldCoordsPoint(estimate.start<2>()),(covFromHessian3d.inverse()).block<2,2>(0,0)); + */ + + + estimate[2] = util::normalize_angle(estimate[2]); + + covMatrix = Eigen::Matrix3f::Zero(); + //covMatrix.block<2,2>(0,0) = (H.block<2,2>(0,0).inverse()); + //covMatrix.block<2,2>(0,0) = (H.block<2,2>(0,0)); + + + /* + covMatrix(0,0) = 1.0/(0.1*0.1); + covMatrix(1,1) = 1.0/(0.1*0.1); + covMatrix(2,2) = 1.0/((M_PI / 18.0f) * (M_PI / 18.0f)); + */ + + + covMatrix = H; + + return gridMapUtil.getWorldCoordsPose(estimate); + } + + return beginEstimateWorld; + } + +protected: + + bool estimateTransformationLogLh(Eigen::Vector3f& estimate, ConcreteOccGridMapUtil& gridMapUtil, const DataContainer& dataPoints) + { + gridMapUtil.getCompleteHessianDerivs(estimate, dataPoints, H, dTr); + //std::cout << "\nH\n" << H << "\n"; + //std::cout << "\ndTr\n" << dTr << "\n"; + + + if ((H(0, 0) != 0.0f) && (H(1, 1) != 0.0f)) { + + + //H += Eigen::Matrix3f::Identity() * 1.0f; + Eigen::Vector3f searchDir (H.inverse() * dTr); + + //std::cout << "\nsearchdir\n" << searchDir << "\n"; + + if (searchDir[2] > 0.2f) { + searchDir[2] = 0.2f; + std::cout << "SearchDir angle change too large\n"; + } else if (searchDir[2] < -0.2f) { + searchDir[2] = -0.2f; + std::cout << "SearchDir angle change too large\n"; + } + + updateEstimatedPose(estimate, searchDir); + return true; + } + return false; + } + + void updateEstimatedPose(Eigen::Vector3f& estimate, const Eigen::Vector3f& change) + { + estimate += change; + } + + void drawScan(const Eigen::Vector3f& pose, const ConcreteOccGridMapUtil& gridMapUtil, const DataContainer& dataContainer) + { + drawInterface->setScale(0.02); + + Eigen::Affine2f transform(gridMapUtil.getTransformForState(pose)); + + int size = dataContainer.getSize(); + for (int i = 0; i < size; ++i) { + const Eigen::Vector2f& currPoint (dataContainer.getVecEntry(i)); + drawInterface->drawPoint(gridMapUtil.getWorldCoordsPoint(transform * currPoint)); + } + } + +protected: + Eigen::Vector3f dTr; + Eigen::Matrix3f H; + + DrawInterface* drawInterface; + HectorDebugInfoInterface* debugInterface; +}; + +} + + +#endif diff --git a/Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/scan/DataPointContainer.h b/Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/scan/DataPointContainer.h new file mode 100644 index 0000000..409d2c4 --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/scan/DataPointContainer.h @@ -0,0 +1,100 @@ +//================================================================================================= +// Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt +// All rights reserved. + +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of the Simulation, Systems Optimization and Robotics +// group, TU Darmstadt nor the names of its contributors may be used to +// endorse or promote products derived from this software without +// specific prior written permission. + +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +//================================================================================================= + +#ifndef __DataPointContainer_h_ +#define __DataPointContainer_h_ + +#include + +namespace hectorslam { + +template +class DataPointContainer +{ +public: + + DataPointContainer(int size = 1000) + { + dataPoints.reserve(size); + } + + void setFrom(const DataPointContainer& other, float factor) + { + origo = other.getOrigo()*factor; + + dataPoints = other.dataPoints; + + unsigned int size = dataPoints.size(); + + for (unsigned int i = 0; i < size; ++i){ + dataPoints[i] *= factor; + } + + } + + void add(const DataPointType& dataPoint) + { + dataPoints.push_back(dataPoint); + } + + void clear() + { + dataPoints.clear(); + } + + int getSize() const + { + return dataPoints.size(); + } + + const DataPointType& getVecEntry(int index) const + { + return dataPoints[index]; + } + + DataPointType getOrigo() const + { + return origo; + } + + void setOrigo(const DataPointType& origoIn) + { + origo = origoIn; + } + +protected: + + std::vector dataPoints; + DataPointType origo; +}; + +typedef DataPointContainer DataContainer; + +} + +#endif diff --git a/Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/slam_main/HectorSlamProcessor.h b/Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/slam_main/HectorSlamProcessor.h new file mode 100644 index 0000000..0b79fc1 --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/slam_main/HectorSlamProcessor.h @@ -0,0 +1,158 @@ +//================================================================================================= +// Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt +// All rights reserved. + +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of the Simulation, Systems Optimization and Robotics +// group, TU Darmstadt nor the names of its contributors may be used to +// endorse or promote products derived from this software without +// specific prior written permission. + +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +//================================================================================================= + +#ifndef _hectorslamprocessor_h__ +#define _hectorslamprocessor_h__ + +#include "../map/GridMap.h" +#include "../map/OccGridMapUtilConfig.h" +#include "../matcher/ScanMatcher.h" +#include "../scan/DataPointContainer.h" + +#include "../util/UtilFunctions.h" +#include "../util/DrawInterface.h" +#include "../util/HectorDebugInfoInterface.h" +#include "../util/MapLockerInterface.h" + +#include "MapRepresentationInterface.h" +#include "MapRepMultiMap.h" + + +#include + +namespace hectorslam{ + +class HectorSlamProcessor +{ +public: + + HectorSlamProcessor(float mapResolution, int mapSizeX, int mapSizeY , const Eigen::Vector2f& startCoords, int multi_res_size, DrawInterface* drawInterfaceIn = 0, HectorDebugInfoInterface* debugInterfaceIn = 0) + : drawInterface(drawInterfaceIn) + , debugInterface(debugInterfaceIn) + { + mapRep = new MapRepMultiMap(mapResolution, mapSizeX, mapSizeY, multi_res_size, startCoords, drawInterfaceIn, debugInterfaceIn); + + this->reset(); + + this->setMapUpdateMinDistDiff(0.4f *1.0f); + this->setMapUpdateMinAngleDiff(0.13f * 1.0f); + } + + ~HectorSlamProcessor() + { + delete mapRep; + } + + void update(const DataContainer& dataContainer, const Eigen::Vector3f& poseHintWorld, bool map_without_matching = false) + { + //std::cout << "\nph:\n" << poseHintWorld << "\n"; + + Eigen::Vector3f newPoseEstimateWorld; + + if (!map_without_matching){ + newPoseEstimateWorld = (mapRep->matchData(poseHintWorld, dataContainer, lastScanMatchCov)); + }else{ + newPoseEstimateWorld = poseHintWorld; + } + + lastScanMatchPose = newPoseEstimateWorld; + + //std::cout << "\nt1:\n" << newPoseEstimateWorld << "\n"; + + //std::cout << "\n1"; + //std::cout << "\n" << lastScanMatchPose << "\n"; + if(util::poseDifferenceLargerThan(newPoseEstimateWorld, lastMapUpdatePose, paramMinDistanceDiffForMapUpdate, paramMinAngleDiffForMapUpdate) || map_without_matching){ + + mapRep->updateByScan(dataContainer, newPoseEstimateWorld); + + mapRep->onMapUpdated(); + lastMapUpdatePose = newPoseEstimateWorld; + } + + if(drawInterface){ + const GridMap& gridMapRef (mapRep->getGridMap()); + drawInterface->setColor(1.0, 0.0, 0.0); + drawInterface->setScale(0.15); + + drawInterface->drawPoint(gridMapRef.getWorldCoords(Eigen::Vector2f::Zero())); + drawInterface->drawPoint(gridMapRef.getWorldCoords((gridMapRef.getMapDimensions().array()-1).cast())); + drawInterface->drawPoint(Eigen::Vector2f(1.0f, 1.0f)); + + drawInterface->sendAndResetData(); + } + + if (debugInterface) + { + debugInterface->sendAndResetData(); + } + } + + void reset() + { + lastMapUpdatePose = Eigen::Vector3f(FLT_MAX, FLT_MAX, FLT_MAX); + lastScanMatchPose = Eigen::Vector3f::Zero(); + //lastScanMatchPose.x() = -10.0f; + //lastScanMatchPose.y() = -15.0f; + //lastScanMatchPose.z() = M_PI*0.15f; + + mapRep->reset(); + } + + const Eigen::Vector3f& getLastScanMatchPose() const { return lastScanMatchPose; }; + const Eigen::Matrix3f& getLastScanMatchCovariance() const { return lastScanMatchCov; }; + float getScaleToMap() const { return mapRep->getScaleToMap(); }; + + int getMapLevels() const { return mapRep->getMapLevels(); }; + const GridMap& getGridMap(int mapLevel = 0) const { return mapRep->getGridMap(mapLevel); }; + + void addMapMutex(int i, MapLockerInterface* mapMutex) { mapRep->addMapMutex(i, mapMutex); }; + MapLockerInterface* getMapMutex(int i) { return mapRep->getMapMutex(i); }; + + void setUpdateFactorFree(float free_factor) { mapRep->setUpdateFactorFree(free_factor); }; + void setUpdateFactorOccupied(float occupied_factor) { mapRep->setUpdateFactorOccupied(occupied_factor); }; + void setMapUpdateMinDistDiff(float minDist) { paramMinDistanceDiffForMapUpdate = minDist; }; + void setMapUpdateMinAngleDiff(float angleChange) { paramMinAngleDiffForMapUpdate = angleChange; }; + +protected: + + MapRepresentationInterface* mapRep; + + Eigen::Vector3f lastMapUpdatePose; + Eigen::Vector3f lastScanMatchPose; + Eigen::Matrix3f lastScanMatchCov; + + float paramMinDistanceDiffForMapUpdate; + float paramMinAngleDiffForMapUpdate; + + DrawInterface* drawInterface; + HectorDebugInfoInterface* debugInterface; +}; + +} + +#endif diff --git a/Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/slam_main/MapProcContainer.h b/Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/slam_main/MapProcContainer.h new file mode 100644 index 0000000..46cfe13 --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/slam_main/MapProcContainer.h @@ -0,0 +1,126 @@ +//================================================================================================= +// Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt +// All rights reserved. + +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of the Simulation, Systems Optimization and Robotics +// group, TU Darmstadt nor the names of its contributors may be used to +// endorse or promote products derived from this software without +// specific prior written permission. + +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +//================================================================================================= + +#ifndef _hectormapproccontainer_h__ +#define _hectormapproccontainer_h__ + +#include "../map/GridMap.h" +#include "../map/OccGridMapUtilConfig.h" +#include "../matcher/ScanMatcher.h" +#include "../util/MapLockerInterface.h" + +class GridMap; +class ConcreteOccGridMapUtil; +class DataContainer; + +namespace hectorslam{ + +class MapProcContainer +{ +public: + MapProcContainer(GridMap* gridMapIn, OccGridMapUtilConfig* gridMapUtilIn, ScanMatcher >* scanMatcherIn) + : gridMap(gridMapIn) + , gridMapUtil(gridMapUtilIn) + , scanMatcher(scanMatcherIn) + , mapMutex(0) + {} + + virtual ~MapProcContainer() + {} + + void cleanup() + { + delete gridMap; + delete gridMapUtil; + delete scanMatcher; + + if (mapMutex){ + delete mapMutex; + } + } + + void reset() + { + gridMap->reset(); + gridMapUtil->resetCachedData(); + } + + void resetCachedData() + { + gridMapUtil->resetCachedData(); + } + + float getScaleToMap() const { return gridMap->getScaleToMap(); }; + + const GridMap& getGridMap() const { return *gridMap; }; + GridMap& getGridMap() { return *gridMap; }; + + void addMapMutex(MapLockerInterface* mapMutexIn) + { + if (mapMutex) + { + delete mapMutex; + } + + mapMutex = mapMutexIn; + } + + MapLockerInterface* getMapMutex() + { + return mapMutex; + } + + Eigen::Vector3f matchData(const Eigen::Vector3f& beginEstimateWorld, const DataContainer& dataContainer, Eigen::Matrix3f& covMatrix, int maxIterations) + { + return scanMatcher->matchData(beginEstimateWorld, *gridMapUtil, dataContainer, covMatrix, maxIterations); + } + + void updateByScan(const DataContainer& dataContainer, const Eigen::Vector3f& robotPoseWorld) + { + if (mapMutex) + { + mapMutex->lockMap(); + } + + gridMap->updateByScan(dataContainer, robotPoseWorld); + + if (mapMutex) + { + mapMutex->unlockMap(); + } + } + + GridMap* gridMap; + OccGridMapUtilConfig* gridMapUtil; + ScanMatcher >* scanMatcher; + MapLockerInterface* mapMutex; +}; + +} + +#endif diff --git a/Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/slam_main/MapRepMultiMap.h b/Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/slam_main/MapRepMultiMap.h new file mode 100644 index 0000000..635d9e1 --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/slam_main/MapRepMultiMap.h @@ -0,0 +1,176 @@ +//================================================================================================= +// Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt +// All rights reserved. + +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of the Simulation, Systems Optimization and Robotics +// group, TU Darmstadt nor the names of its contributors may be used to +// endorse or promote products derived from this software without +// specific prior written permission. + +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +//================================================================================================= + +#ifndef _hectormaprepmultimap_h__ +#define _hectormaprepmultimap_h__ + +#include "MapRepresentationInterface.h" +#include "MapProcContainer.h" + +#include "../map/GridMap.h" +#include "../map/OccGridMapUtilConfig.h" +#include "../matcher/ScanMatcher.h" + +#include "../util/DrawInterface.h" +#include "../util/HectorDebugInfoInterface.h" + +namespace hectorslam{ + +class MapRepMultiMap : public MapRepresentationInterface +{ + +public: + MapRepMultiMap(float mapResolution, int mapSizeX, int mapSizeY, unsigned int numDepth, const Eigen::Vector2f& startCoords, DrawInterface* drawInterfaceIn, HectorDebugInfoInterface* debugInterfaceIn) + { + //unsigned int numDepth = 3; + Eigen::Vector2i resolution(mapSizeX, mapSizeY); + + float totalMapSizeX = mapResolution * static_cast(mapSizeX); + float mid_offset_x = totalMapSizeX * startCoords.x(); + + float totalMapSizeY = mapResolution * static_cast(mapSizeY); + float mid_offset_y = totalMapSizeY * startCoords.y(); + + for (unsigned int i = 0; i < numDepth; ++i){ + std::cout << "HectorSM map lvl " << i << ": cellLength: " << mapResolution << " res x:" << resolution.x() << " res y: " << resolution.y() << "\n"; + GridMap* gridMap = new hectorslam::GridMap(mapResolution,resolution, Eigen::Vector2f(mid_offset_x, mid_offset_y)); + OccGridMapUtilConfig* gridMapUtil = new OccGridMapUtilConfig(gridMap); + ScanMatcher >* scanMatcher = new hectorslam::ScanMatcher >(drawInterfaceIn, debugInterfaceIn); + + mapContainer.push_back(MapProcContainer(gridMap, gridMapUtil, scanMatcher)); + + resolution /= 2; + mapResolution*=2.0f; + } + + dataContainers.resize(numDepth-1); + } + + virtual ~MapRepMultiMap() + { + unsigned int size = mapContainer.size(); + + for (unsigned int i = 0; i < size; ++i){ + mapContainer[i].cleanup(); + } + } + + virtual void reset() + { + unsigned int size = mapContainer.size(); + + for (unsigned int i = 0; i < size; ++i){ + mapContainer[i].reset(); + } + } + + virtual float getScaleToMap() const { return mapContainer[0].getScaleToMap(); }; + + virtual int getMapLevels() const { return mapContainer.size(); }; + virtual const GridMap& getGridMap(int mapLevel) const { return mapContainer[mapLevel].getGridMap(); }; + + virtual void addMapMutex(int i, MapLockerInterface* mapMutex) + { + mapContainer[i].addMapMutex(mapMutex); + } + + MapLockerInterface* getMapMutex(int i) + { + return mapContainer[i].getMapMutex(); + } + + virtual void onMapUpdated() + { + unsigned int size = mapContainer.size(); + + for (unsigned int i = 0; i < size; ++i){ + mapContainer[i].resetCachedData(); + } + } + + virtual Eigen::Vector3f matchData(const Eigen::Vector3f& beginEstimateWorld, const DataContainer& dataContainer, Eigen::Matrix3f& covMatrix) + { + size_t size = mapContainer.size(); + + Eigen::Vector3f tmp(beginEstimateWorld); + + for (int index = size - 1; index >= 0; --index){ + //std::cout << " m " << i; + if (index == 0){ + tmp = (mapContainer[index].matchData(tmp, dataContainer, covMatrix, 5)); + }else{ + dataContainers[index-1].setFrom(dataContainer, static_cast(1.0 / pow(2.0, static_cast(index)))); + tmp = (mapContainer[index].matchData(tmp, dataContainers[index-1], covMatrix, 3)); + } + } + return tmp; + } + + virtual void updateByScan(const DataContainer& dataContainer, const Eigen::Vector3f& robotPoseWorld) + { + unsigned int size = mapContainer.size(); + + for (unsigned int i = 0; i < size; ++i){ + //std::cout << " u " << i; + if (i==0){ + mapContainer[i].updateByScan(dataContainer, robotPoseWorld); + }else{ + mapContainer[i].updateByScan(dataContainers[i-1], robotPoseWorld); + } + } + //std::cout << "\n"; + } + + virtual void setUpdateFactorFree(float free_factor) + { + size_t size = mapContainer.size(); + + for (unsigned int i = 0; i < size; ++i){ + GridMap& map = mapContainer[i].getGridMap(); + map.setUpdateFreeFactor(free_factor); + } + } + + virtual void setUpdateFactorOccupied(float occupied_factor) + { + size_t size = mapContainer.size(); + + for (unsigned int i = 0; i < size; ++i){ + GridMap& map = mapContainer[i].getGridMap(); + map.setUpdateOccupiedFactor(occupied_factor); + } + } + +protected: + std::vector mapContainer; + std::vector dataContainers; +}; + +} + +#endif diff --git a/Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/slam_main/MapRepSingleMap.h b/Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/slam_main/MapRepSingleMap.h new file mode 100644 index 0000000..2bdfab5 --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/slam_main/MapRepSingleMap.h @@ -0,0 +1,97 @@ +//================================================================================================= +// Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt +// All rights reserved. + +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of the Simulation, Systems Optimization and Robotics +// group, TU Darmstadt nor the names of its contributors may be used to +// endorse or promote products derived from this software without +// specific prior written permission. + +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +//================================================================================================= + +#ifndef _hectormaprepsinglemap_h__ +#define _hectormaprepsinglemap_h__ + +#include "MapRepresentationInterface.h" + +#include "../map/GridMap.h" +#include "../map/OccGridMapUtilConfig.h" +#include "../matcher/ScanMatcher.h" + +#include "../util/DrawInterface.h" +#include "../util/HectorDebugInfoInterface.h" + +namespace hectorslam{ + +class MapRepSingleMap : public MapRepresentationInterface +{ + +public: + MapRepSingleMap(float mapResolution, DrawInterface* drawInterfaceIn, HectorDebugInfoInterface* debugInterfaceIn) + { + gridMap = new hectorslam::GridMap(mapResolution,Eigen::Vector2i(1024,1024), Eigen::Vector2f(20.0f, 20.0f)); + gridMapUtil = new OccGridMapUtilConfig(gridMap); + scanMatcher = new hectorslam::ScanMatcher >(drawInterfaceIn, debugInterfaceIn); + } + + virtual ~MapRepSingleMap() + { + delete gridMap; + delete gridMapUtil; + delete scanMatcher; + } + + virtual void reset() + { + gridMap->reset(); + gridMapUtil->resetCachedData(); + } + + virtual float getScaleToMap() const { return gridMap->getScaleToMap(); }; + + virtual int getMapLevels() const { return 1; }; + virtual const GridMap& getGridMap(int mapLevel) const { return *gridMap; }; + + virtual void onMapUpdated() + { + gridMapUtil->resetCachedData(); + } + + virtual Eigen::Vector3f matchData(const Eigen::Vector3f& beginEstimateWorld, const DataContainer& dataContainer, Eigen::Matrix3f& covMatrix) + { + return scanMatcher->matchData(beginEstimateWorld, *gridMapUtil, dataContainer, covMatrix, 20); + } + + virtual void updateByScan(const DataContainer& dataContainer, const Eigen::Vector3f& robotPoseWorld) + { + gridMap->updateByScan(dataContainer, robotPoseWorld); + } + +protected: + GridMap* gridMap; + OccGridMapUtilConfig* gridMapUtil; + ScanMatcher >* scanMatcher; +}; + +} + +#endif + + diff --git a/Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/slam_main/MapRepresentationInterface.h b/Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/slam_main/MapRepresentationInterface.h new file mode 100644 index 0000000..2cb5a22 --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/slam_main/MapRepresentationInterface.h @@ -0,0 +1,66 @@ +//================================================================================================= +// Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt +// All rights reserved. + +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of the Simulation, Systems Optimization and Robotics +// group, TU Darmstadt nor the names of its contributors may be used to +// endorse or promote products derived from this software without +// specific prior written permission. + +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +//================================================================================================= + +#ifndef _hectormaprepresentationinterface_h__ +#define _hectormaprepresentationinterface_h__ + +class GridMap; +class ConcreteOccGridMapUtil; +class DataContainer; + +namespace hectorslam{ + +class MapRepresentationInterface +{ +public: + + virtual ~MapRepresentationInterface() {}; + + virtual void reset() = 0; + + virtual float getScaleToMap() const = 0; + + virtual int getMapLevels() const = 0; + virtual const GridMap& getGridMap(int mapLevel = 0) const = 0; + + virtual void addMapMutex(int i, MapLockerInterface* mapMutex) = 0; + virtual MapLockerInterface* getMapMutex(int i) = 0; + + virtual void onMapUpdated() = 0; + + virtual Eigen::Vector3f matchData(const Eigen::Vector3f& beginEstimateWorld, const DataContainer& dataContainer, Eigen::Matrix3f& covMatrix) = 0; + + virtual void updateByScan(const DataContainer& dataContainer, const Eigen::Vector3f& robotPoseWorld) = 0; + + virtual void setUpdateFactorFree(float free_factor) = 0; + virtual void setUpdateFactorOccupied(float occupied_factor) = 0; +}; + +} + +#endif diff --git a/Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/util/DrawInterface.h b/Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/util/DrawInterface.h new file mode 100644 index 0000000..b98932c --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/util/DrawInterface.h @@ -0,0 +1,44 @@ +//================================================================================================= +// Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt +// All rights reserved. + +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of the Simulation, Systems Optimization and Robotics +// group, TU Darmstadt nor the names of its contributors may be used to +// endorse or promote products derived from this software without +// specific prior written permission. + +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +//================================================================================================= + +#ifndef drawinterface_h__ +#define drawinterface_h__ + +class DrawInterface{ +public: + virtual void drawPoint(const Eigen::Vector2f& pointWorldFrame) = 0; + virtual void drawArrow(const Eigen::Vector3f& poseWorld) = 0; + virtual void drawCovariance(const Eigen::Vector2f& mean, const Eigen::Matrix2f& cov) = 0; + + virtual void setScale(double scale) = 0; + virtual void setColor(double r, double g, double b, double a = 1.0) = 0; + + virtual void sendAndResetData() = 0; +}; + +#endif diff --git a/Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/util/HectorDebugInfoInterface.h b/Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/util/HectorDebugInfoInterface.h new file mode 100644 index 0000000..435494e --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/util/HectorDebugInfoInterface.h @@ -0,0 +1,40 @@ +//================================================================================================= +// Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt +// All rights reserved. + +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of the Simulation, Systems Optimization and Robotics +// group, TU Darmstadt nor the names of its contributors may be used to +// endorse or promote products derived from this software without +// specific prior written permission. + +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +//================================================================================================= + +#ifndef hectordebuginfointerface_h__ +#define hectordebuginfointerface_h__ + +class HectorDebugInfoInterface{ +public: + + virtual void sendAndResetData() = 0; + virtual void addHessianMatrix(const Eigen::Matrix3f& hessian) = 0; + virtual void addPoseLikelihood(float lh) = 0; +}; + +#endif diff --git a/Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/util/MapLockerInterface.h b/Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/util/MapLockerInterface.h new file mode 100644 index 0000000..3db5db7 --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/util/MapLockerInterface.h @@ -0,0 +1,39 @@ +//================================================================================================= +// Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt +// All rights reserved. + +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of the Simulation, Systems Optimization and Robotics +// group, TU Darmstadt nor the names of its contributors may be used to +// endorse or promote products derived from this software without +// specific prior written permission. + +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +//================================================================================================= + +#ifndef maplockerinterface_h__ +#define maplockerinterface_h__ + +class MapLockerInterface +{ +public: + virtual void lockMap() = 0; + virtual void unlockMap() = 0; +}; + +#endif diff --git a/Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/util/UtilFunctions.h b/Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/util/UtilFunctions.h new file mode 100644 index 0000000..fe7c9c6 --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_mapping/include/hector_mapping/util/UtilFunctions.h @@ -0,0 +1,101 @@ +//================================================================================================= +// Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt +// All rights reserved. + +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of the Simulation, Systems Optimization and Robotics +// group, TU Darmstadt nor the names of its contributors may be used to +// endorse or promote products derived from this software without +// specific prior written permission. + +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +//================================================================================================= + +#ifndef utilfunctions_h__ +#define utilfunctions_h__ + +#include +#include + +namespace util{ + +static inline float normalize_angle_pos(float angle) +{ + return fmod(fmod(angle, 2.0f*M_PI) + 2.0f*M_PI, 2.0f*M_PI); +} + +static inline float normalize_angle(float angle) +{ + float a = normalize_angle_pos(angle); + if (a > M_PI){ + a -= 2.0f*M_PI; + } + return a; +} + +static inline float sqr(float val) +{ + return val*val; +} + +static inline int sign(int x) +{ + return x > 0 ? 1 : -1; +} + +template +static T toDeg(const T radVal) +{ + return radVal * static_cast(180.0 / M_PI); +} + +template +static T toRad(const T degVal) +{ + return degVal * static_cast(M_PI / 180.0); +} + +static bool poseDifferenceLargerThan(const Eigen::Vector3f& pose1, const Eigen::Vector3f& pose2, float distanceDiffThresh, float angleDiffThresh) +{ + //check distance + if ( ( (pose1.head<2>() - pose2.head<2>()).norm() ) > distanceDiffThresh){ + return true; + } + + float angleDiff = (pose1.z() - pose2.z()); + + if (angleDiff > M_PI) { + angleDiff -= M_PI * 2.0f; + } else if (angleDiff < -M_PI) { + angleDiff += M_PI * 2.0f; + } + + if (abs(angleDiff) > angleDiffThresh){ + return true; + } + return false; +} + +static double getYawFromQuat(const geometry_msgs::Quaternion &quat) +{ + return tf::getYaw(tf::Quaternion(quat.x, quat.y, quat.z, quat.w)); +} + +} + +#endif diff --git a/Localizations/Libraries/Ros/hector_slam/hector_mapping/launch/mapping_default.launch b/Localizations/Libraries/Ros/hector_slam/hector_mapping/launch/mapping_default.launch new file mode 100644 index 0000000..9af61bf --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_mapping/launch/mapping_default.launch @@ -0,0 +1,57 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Localizations/Libraries/Ros/hector_slam/hector_mapping/msg/HectorDebugInfo.msg b/Localizations/Libraries/Ros/hector_slam/hector_mapping/msg/HectorDebugInfo.msg new file mode 100644 index 0000000..a6b8b2a --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_mapping/msg/HectorDebugInfo.msg @@ -0,0 +1 @@ +HectorIterData[] iterData \ No newline at end of file diff --git a/Localizations/Libraries/Ros/hector_slam/hector_mapping/msg/HectorIterData.msg b/Localizations/Libraries/Ros/hector_slam/hector_mapping/msg/HectorIterData.msg new file mode 100644 index 0000000..a0792ba --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_mapping/msg/HectorIterData.msg @@ -0,0 +1,5 @@ +float64[9] hessian +float64 conditionNum +float64 determinant +float64 conditionNum2d +float64 determinant2d diff --git a/Localizations/Libraries/Ros/hector_slam/hector_mapping/package.xml b/Localizations/Libraries/Ros/hector_slam/hector_mapping/package.xml new file mode 100644 index 0000000..afa082f --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_mapping/package.xml @@ -0,0 +1,71 @@ + + + hector_mapping + 0.5.2 + + hector_mapping is a SLAM approach that can be used without odometry as well as on platforms that exhibit roll/pitch motion (of the sensor, the platform or both). + It leverages the high update rate of modern LIDAR systems like the Hokuyo UTM-30LX and provides 2D pose estimates at scan rate of the sensors (40Hz for the UTM-30LX). + While the system does not provide explicit loop closing ability, it is sufficiently accurate for many real world scenarios. The system has successfully been used on + Unmanned Ground Robots, Unmanned Surface Vehicles, Handheld Mapping Devices and logged data from quadrotor UAVs. + + + + Johannes Meyer + + + + + + BSD + + + + + http://ros.org/wiki/hector_mapping + + + + + Stefan Kohlbrecher + + + + + + + + + + + + + + catkin + roscpp + nav_msgs + visualization_msgs + tf + message_filters + laser_geometry + eigen + boost + message_generation + roscpp + nav_msgs + visualization_msgs + tf + message_filters + laser_geometry + eigen + boost + message_runtime + + + + + + + + + + diff --git a/Localizations/Libraries/Ros/hector_slam/hector_mapping/src/HectorMappingRos.cpp b/Localizations/Libraries/Ros/hector_slam/hector_mapping/src/HectorMappingRos.cpp new file mode 100644 index 0000000..3d49459 --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_mapping/src/HectorMappingRos.cpp @@ -0,0 +1,627 @@ +//================================================================================================= +// Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt +// All rights reserved. + +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of the Simulation, Systems Optimization and Robotics +// group, TU Darmstadt nor the names of its contributors may be used to +// endorse or promote products derived from this software without +// specific prior written permission. + +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +//================================================================================================= + +#include "hector_mapping/HectorMappingRos.h" + +#include "hector_mapping/map/GridMap.h" + +#include +#include + +#include "sensor_msgs/PointCloud2.h" + +#include "hector_mapping/HectorDrawings.h" +#include "hector_mapping/HectorDebugInfoProvider.h" +#include "hector_mapping/HectorMapMutex.h" + +#ifndef TF_SCALAR_H + typedef btScalar tfScalar; +#endif + +HectorMappingRos::HectorMappingRos() + : debugInfoProvider(0) + , hectorDrawings(0) + , lastGetMapUpdateIndex(-100) + , tfB_(0) + , map__publish_thread_(0) + , initial_pose_set_(false) + , pause_scan_processing_(false) +{ + ros::NodeHandle private_nh_("~"); + + std::string mapTopic_ = "map"; + + private_nh_.param("pub_drawings", p_pub_drawings, false); + private_nh_.param("pub_debug_output", p_pub_debug_output_, false); + private_nh_.param("pub_map_odom_transform", p_pub_map_odom_transform_,true); + private_nh_.param("pub_odometry", p_pub_odometry_,false); + private_nh_.param("advertise_map_service", p_advertise_map_service_,true); + private_nh_.param("scan_subscriber_queue_size", p_scan_subscriber_queue_size_, 5); + + private_nh_.param("map_resolution", p_map_resolution_, 0.025); + private_nh_.param("map_size", p_map_size_, 1024); + private_nh_.param("map_start_x", p_map_start_x_, 0.5); + private_nh_.param("map_start_y", p_map_start_y_, 0.5); + private_nh_.param("map_multi_res_levels", p_map_multi_res_levels_, 3); + + private_nh_.param("update_factor_free", p_update_factor_free_, 0.4); + private_nh_.param("update_factor_occupied", p_update_factor_occupied_, 0.9); + + private_nh_.param("map_update_distance_thresh", p_map_update_distance_threshold_, 0.4); + private_nh_.param("map_update_angle_thresh", p_map_update_angle_threshold_, 0.9); + + private_nh_.param("scan_topic", p_scan_topic_, std::string("scan")); + private_nh_.param("sys_msg_topic", p_sys_msg_topic_, std::string("syscommand")); + private_nh_.param("pose_update_topic", p_pose_update_topic_, std::string("poseupdate")); + + private_nh_.param("use_tf_scan_transformation", p_use_tf_scan_transformation_,true); + private_nh_.param("use_tf_pose_start_estimate", p_use_tf_pose_start_estimate_,false); + private_nh_.param("map_with_known_poses", p_map_with_known_poses_, false); + + private_nh_.param("base_frame", p_base_frame_, std::string("base_link")); + private_nh_.param("map_frame", p_map_frame_, std::string("map")); + private_nh_.param("odom_frame", p_odom_frame_, std::string("odom")); + + private_nh_.param("pub_map_scanmatch_transform", p_pub_map_scanmatch_transform_,true); + private_nh_.param("tf_map_scanmatch_transform_frame_name", p_tf_map_scanmatch_transform_frame_name_, std::string("scanmatcher_frame")); + + private_nh_.param("output_timing", p_timing_output_,false); + + private_nh_.param("map_pub_period", p_map_pub_period_, 2.0); + + double tmp = 0.0; + private_nh_.param("laser_min_dist", tmp, 0.4); + p_sqr_laser_min_dist_ = static_cast(tmp*tmp); + + private_nh_.param("laser_max_dist", tmp, 30.0); + p_sqr_laser_max_dist_ = static_cast(tmp*tmp); + + private_nh_.param("laser_z_min_value", tmp, -1.0); + p_laser_z_min_value_ = static_cast(tmp); + + private_nh_.param("laser_z_max_value", tmp, 1.0); + p_laser_z_max_value_ = static_cast(tmp); + + if (p_pub_drawings) + { + ROS_INFO("HectorSM publishing debug drawings"); + hectorDrawings = new HectorDrawings(); + } + + if(p_pub_debug_output_) + { + ROS_INFO("HectorSM publishing debug info"); + debugInfoProvider = new HectorDebugInfoProvider(); + } + + if(p_pub_odometry_) + { + odometryPublisher_ = node_.advertise("scanmatch_odom", 50); + } + + slamProcessor = new hectorslam::HectorSlamProcessor(static_cast(p_map_resolution_), p_map_size_, p_map_size_, Eigen::Vector2f(p_map_start_x_, p_map_start_y_), p_map_multi_res_levels_, hectorDrawings, debugInfoProvider); + slamProcessor->setUpdateFactorFree(p_update_factor_free_); + slamProcessor->setUpdateFactorOccupied(p_update_factor_occupied_); + slamProcessor->setMapUpdateMinDistDiff(p_map_update_distance_threshold_); + slamProcessor->setMapUpdateMinAngleDiff(p_map_update_angle_threshold_); + + int mapLevels = slamProcessor->getMapLevels(); + mapLevels = 1; + + for (int i = 0; i < mapLevels; ++i) + { + mapPubContainer.push_back(MapPublisherContainer()); + slamProcessor->addMapMutex(i, new HectorMapMutex()); + + std::string mapTopicStr(mapTopic_); + + if (i != 0) + { + mapTopicStr.append("_" + boost::lexical_cast(i)); + } + + std::string mapMetaTopicStr(mapTopicStr); + mapMetaTopicStr.append("_metadata"); + + MapPublisherContainer& tmp = mapPubContainer[i]; + tmp.mapPublisher_ = node_.advertise(mapTopicStr, 1, true); + tmp.mapMetadataPublisher_ = node_.advertise(mapMetaTopicStr, 1, true); + + if ( (i == 0) && p_advertise_map_service_) + { + tmp.dynamicMapServiceServer_ = node_.advertiseService("dynamic_map", &HectorMappingRos::mapCallback, this); + } + + setServiceGetMapData(tmp.map_, slamProcessor->getGridMap(i)); + + if ( i== 0){ + mapPubContainer[i].mapMetadataPublisher_.publish(mapPubContainer[i].map_.map.info); + } + } + + // Initialize services + reset_map_service_ = node_.advertiseService("reset_map", &HectorMappingRos::resetMapCallback, this); + restart_hector_service_ = node_.advertiseService("restart_mapping_with_new_pose", &HectorMappingRos::restartHectorCallback, this); + toggle_scan_processing_service_ = node_.advertiseService("pause_mapping", &HectorMappingRos::pauseMapCallback, this); + + ROS_INFO("HectorSM p_base_frame_: %s", p_base_frame_.c_str()); + ROS_INFO("HectorSM p_map_frame_: %s", p_map_frame_.c_str()); + ROS_INFO("HectorSM p_odom_frame_: %s", p_odom_frame_.c_str()); + ROS_INFO("HectorSM p_scan_topic_: %s", p_scan_topic_.c_str()); + ROS_INFO("HectorSM p_use_tf_scan_transformation_: %s", p_use_tf_scan_transformation_ ? ("true") : ("false")); + ROS_INFO("HectorSM p_pub_map_odom_transform_: %s", p_pub_map_odom_transform_ ? ("true") : ("false")); + ROS_INFO("HectorSM p_scan_subscriber_queue_size_: %d", p_scan_subscriber_queue_size_); + ROS_INFO("HectorSM p_map_pub_period_: %f", p_map_pub_period_); + ROS_INFO("HectorSM p_update_factor_free_: %f", p_update_factor_free_); + ROS_INFO("HectorSM p_update_factor_occupied_: %f", p_update_factor_occupied_); + ROS_INFO("HectorSM p_map_update_distance_threshold_: %f ", p_map_update_distance_threshold_); + ROS_INFO("HectorSM p_map_update_angle_threshold_: %f", p_map_update_angle_threshold_); + ROS_INFO("HectorSM p_laser_z_min_value_: %f", p_laser_z_min_value_); + ROS_INFO("HectorSM p_laser_z_max_value_: %f", p_laser_z_max_value_); + + scanSubscriber_ = node_.subscribe(p_scan_topic_, p_scan_subscriber_queue_size_, &HectorMappingRos::scanCallback, this); + sysMsgSubscriber_ = node_.subscribe(p_sys_msg_topic_, 2, &HectorMappingRos::sysMsgCallback, this); + + poseUpdatePublisher_ = node_.advertise(p_pose_update_topic_, 1, false); + posePublisher_ = node_.advertise("slam_out_pose", 1, false); + + scan_point_cloud_publisher_ = node_.advertise("slam_cloud",1,false); + + tfB_ = new tf::TransformBroadcaster(); + ROS_ASSERT(tfB_); + + /* + bool p_use_static_map_ = false; + + if (p_use_static_map_){ + mapSubscriber_ = node_.subscribe(mapTopic_, 1, &HectorMappingRos::staticMapCallback, this); + } + */ + + initial_pose_sub_ = new message_filters::Subscriber(node_, "initialpose", 2); + initial_pose_filter_ = new tf::MessageFilter(*initial_pose_sub_, tf_, p_map_frame_, 2); + initial_pose_filter_->registerCallback(boost::bind(&HectorMappingRos::initialPoseCallback, this, _1)); + + + map__publish_thread_ = new boost::thread(boost::bind(&HectorMappingRos::publishMapLoop, this, p_map_pub_period_)); + + map_to_odom_.setIdentity(); + + lastMapPublishTime = ros::Time(0,0); +} + +HectorMappingRos::~HectorMappingRos() +{ + delete slamProcessor; + + if (hectorDrawings) + delete hectorDrawings; + + if (debugInfoProvider) + delete debugInfoProvider; + + if (tfB_) + delete tfB_; + + if(map__publish_thread_) + delete map__publish_thread_; +} + +void HectorMappingRos::scanCallback(const sensor_msgs::LaserScan& scan) +{ + if (pause_scan_processing_) + { + return; + } + + if (hectorDrawings) + { + hectorDrawings->setTime(scan.header.stamp); + } + + ros::WallTime start_time = ros::WallTime::now(); + if (!p_use_tf_scan_transformation_) + { + // If we are not using the tf tree to find the transform between the base frame and laser frame, + // then just convert the laser scan to our data container and process the update based on our last + // pose estimate + this->rosLaserScanToDataContainer(scan, laserScanContainer, slamProcessor->getScaleToMap()); + slamProcessor->update(laserScanContainer, slamProcessor->getLastScanMatchPose()); + } + else + { + // If we are using the tf tree to find the transform between the base frame and laser frame, + // let's get that transform + const ros::Duration dur(0.5); + tf::StampedTransform laser_transform; + if (tf_.waitForTransform(p_base_frame_, scan.header.frame_id, scan.header.stamp, dur)) + { + tf_.lookupTransform(p_base_frame_, scan.header.frame_id, scan.header.stamp, laser_transform); + } + else + { + ROS_INFO("lookupTransform %s to %s timed out. Could not transform laser scan into base_frame.", p_base_frame_.c_str(), scan.header.frame_id.c_str()); + return; + } + + // Convert the laser scan to point cloud + projector_.projectLaser(scan, laser_point_cloud_, 30.0); + + // Publish the point cloud if there are any subscribers + if (scan_point_cloud_publisher_.getNumSubscribers() > 0) + { + scan_point_cloud_publisher_.publish(laser_point_cloud_); + } + + // Convert the point cloud to our data container + this->rosPointCloudToDataContainer(laser_point_cloud_, laser_transform, laserScanContainer, slamProcessor->getScaleToMap()); + + // Now let's choose the initial pose estimate for our slam process update + Eigen::Vector3f start_estimate(Eigen::Vector3f::Zero()); + if (initial_pose_set_) + { + // User has requested a pose reset + initial_pose_set_ = false; + start_estimate = initial_pose_; + } + else if (p_use_tf_pose_start_estimate_) + { + // Initial pose estimate comes from the tf tree + try + { + tf::StampedTransform stamped_pose; + + tf_.waitForTransform(p_map_frame_, p_base_frame_, scan.header.stamp, ros::Duration(0.5)); + tf_.lookupTransform(p_map_frame_, p_base_frame_, scan.header.stamp, stamped_pose); + + const double yaw = tf::getYaw(stamped_pose.getRotation()); + start_estimate = Eigen::Vector3f(stamped_pose.getOrigin().getX(), stamped_pose.getOrigin().getY(), yaw); + } + catch(tf::TransformException e) + { + ROS_ERROR("Transform from %s to %s failed\n", p_map_frame_.c_str(), p_base_frame_.c_str()); + start_estimate = slamProcessor->getLastScanMatchPose(); + } + } + else + { + // If none of the above, the initial pose is simply the last estimated pose + start_estimate = slamProcessor->getLastScanMatchPose(); + } + + // If "p_map_with_known_poses_" is enabled, we assume that start_estimate is precise and doesn't need to be refined + if (p_map_with_known_poses_) + { + slamProcessor->update(laserScanContainer, start_estimate, true); + } + else + { + slamProcessor->update(laserScanContainer, start_estimate); + } + } + + // If the debug flag "p_timing_output_" is enabled, print how long this last iteration took + if (p_timing_output_) + { + ros::WallDuration duration = ros::WallTime::now() - start_time; + ROS_INFO("HectorSLAM Iter took: %f milliseconds", duration.toSec()*1000.0f ); + } + + // If we're just building a map with known poses, we're finished now. Code below this point publishes the localization results. + if (p_map_with_known_poses_) + { + return; + } + + poseInfoContainer_.update(slamProcessor->getLastScanMatchPose(), slamProcessor->getLastScanMatchCovariance(), scan.header.stamp, p_map_frame_); + + // Publish pose with and without covariances + poseUpdatePublisher_.publish(poseInfoContainer_.getPoseWithCovarianceStamped()); + posePublisher_.publish(poseInfoContainer_.getPoseStamped()); + + // Publish odometry if enabled + if(p_pub_odometry_) + { + nav_msgs::Odometry tmp; + tmp.pose = poseInfoContainer_.getPoseWithCovarianceStamped().pose; + + tmp.header = poseInfoContainer_.getPoseWithCovarianceStamped().header; + tmp.child_frame_id = p_base_frame_; + odometryPublisher_.publish(tmp); + } + + // Publish the map->odom transform if enabled + if (p_pub_map_odom_transform_) + { + tf::StampedTransform odom_to_base; + try + { + tf_.waitForTransform(p_odom_frame_, p_base_frame_, scan.header.stamp, ros::Duration(0.5)); + tf_.lookupTransform(p_odom_frame_, p_base_frame_, scan.header.stamp, odom_to_base); + } + catch(tf::TransformException e) + { + ROS_ERROR("Transform failed during publishing of map_odom transform: %s",e.what()); + odom_to_base.setIdentity(); + } + map_to_odom_ = tf::Transform(poseInfoContainer_.getTfTransform() * odom_to_base.inverse()); + tfB_->sendTransform( tf::StampedTransform (map_to_odom_, scan.header.stamp, p_map_frame_, p_odom_frame_)); + } + + // Publish the transform from map to estimated pose (if enabled) + if (p_pub_map_scanmatch_transform_) + { + tfB_->sendTransform( tf::StampedTransform(poseInfoContainer_.getTfTransform(), scan.header.stamp, p_map_frame_, p_tf_map_scanmatch_transform_frame_name_)); + } +} + +void HectorMappingRos::sysMsgCallback(const std_msgs::String& string) +{ + ROS_INFO("HectorSM sysMsgCallback, msg contents: %s", string.data.c_str()); + + if (string.data == "reset") + { + ROS_INFO("HectorSM reset"); + slamProcessor->reset(); + } +} + +bool HectorMappingRos::mapCallback(nav_msgs::GetMap::Request &req, + nav_msgs::GetMap::Response &res) +{ + ROS_INFO("HectorSM Map service called"); + res = mapPubContainer[0].map_; + return true; +} + +bool HectorMappingRos::resetMapCallback(std_srvs::Trigger::Request &req, + std_srvs::Trigger::Response &res) +{ + ROS_INFO("HectorSM Reset map service called"); + slamProcessor->reset(); + return true; +} + +bool HectorMappingRos::restartHectorCallback(hector_mapping::ResetMapping::Request &req, + hector_mapping::ResetMapping::Response &res) +{ + // Reset map + ROS_INFO("HectorSM Reset map"); + slamProcessor->reset(); + + // Reset pose + this->resetPose(req.initial_pose); + + // Unpause node (in case it is paused) + this->toggleMappingPause(false); + + // Return success + return true; +} + +bool HectorMappingRos::pauseMapCallback(std_srvs::SetBool::Request &req, + std_srvs::SetBool::Response &res) +{ + this->toggleMappingPause(req.data); + res.success = true; + return true; +} + +void HectorMappingRos::publishMap(MapPublisherContainer& mapPublisher, const hectorslam::GridMap& gridMap, ros::Time timestamp, MapLockerInterface* mapMutex) +{ + nav_msgs::GetMap::Response& map_ (mapPublisher.map_); + + //only update map if it changed + if (lastGetMapUpdateIndex != gridMap.getUpdateIndex()) + { + + int sizeX = gridMap.getSizeX(); + int sizeY = gridMap.getSizeY(); + + int size = sizeX * sizeY; + + std::vector& data = map_.map.data; + + //std::vector contents are guaranteed to be contiguous, use memset to set all to unknown to save time in loop + memset(&data[0], -1, sizeof(int8_t) * size); + + if (mapMutex) + { + mapMutex->lockMap(); + } + + for(int i=0; i < size; ++i) + { + if(gridMap.isFree(i)) + { + data[i] = 0; + } + else if (gridMap.isOccupied(i)) + { + data[i] = 100; + } + } + + lastGetMapUpdateIndex = gridMap.getUpdateIndex(); + + if (mapMutex) + { + mapMutex->unlockMap(); + } + } + + map_.map.header.stamp = timestamp; + + mapPublisher.mapPublisher_.publish(map_.map); +} + +void HectorMappingRos::rosLaserScanToDataContainer(const sensor_msgs::LaserScan& scan, hectorslam::DataContainer& dataContainer, float scaleToMap) +{ + size_t size = scan.ranges.size(); + + float angle = scan.angle_min; + + dataContainer.clear(); + + dataContainer.setOrigo(Eigen::Vector2f::Zero()); + + float maxRangeForContainer = scan.range_max - 0.1f; + + for (size_t i = 0; i < size; ++i) + { + float dist = scan.ranges[i]; + + if ( (dist > scan.range_min) && (dist < maxRangeForContainer)) + { + dist *= scaleToMap; + dataContainer.add(Eigen::Vector2f(cos(angle) * dist, sin(angle) * dist)); + } + + angle += scan.angle_increment; + } +} + +void HectorMappingRos::rosPointCloudToDataContainer(const sensor_msgs::PointCloud& pointCloud, const tf::StampedTransform& laserTransform, hectorslam::DataContainer& dataContainer, float scaleToMap) +{ + size_t size = pointCloud.points.size(); + //ROS_INFO("size: %d", size); + + dataContainer.clear(); + + tf::Vector3 laserPos (laserTransform.getOrigin()); + dataContainer.setOrigo(Eigen::Vector2f(laserPos.x(), laserPos.y())*scaleToMap); + + for (size_t i = 0; i < size; ++i) + { + + const geometry_msgs::Point32& currPoint(pointCloud.points[i]); + + float dist_sqr = currPoint.x*currPoint.x + currPoint.y* currPoint.y; + + if ( (dist_sqr > p_sqr_laser_min_dist_) && (dist_sqr < p_sqr_laser_max_dist_) ){ + + if ( (currPoint.x < 0.0f) && (dist_sqr < 0.50f)){ + continue; + } + + tf::Vector3 pointPosBaseFrame(laserTransform * tf::Vector3(currPoint.x, currPoint.y, currPoint.z)); + + float pointPosLaserFrameZ = pointPosBaseFrame.z() - laserPos.z(); + + if (pointPosLaserFrameZ > p_laser_z_min_value_ && pointPosLaserFrameZ < p_laser_z_max_value_) + { + dataContainer.add(Eigen::Vector2f(pointPosBaseFrame.x(),pointPosBaseFrame.y())*scaleToMap); + } + } + } +} + +void HectorMappingRos::setServiceGetMapData(nav_msgs::GetMap::Response& map_, const hectorslam::GridMap& gridMap) +{ + Eigen::Vector2f mapOrigin (gridMap.getWorldCoords(Eigen::Vector2f::Zero())); + mapOrigin.array() -= gridMap.getCellLength()/1000*0.5f; + + map_.map.info.origin.position.x = mapOrigin.x(); + map_.map.info.origin.position.y = mapOrigin.y(); + map_.map.info.origin.orientation.w = 1.0; + + map_.map.info.resolution = gridMap.getCellLength(); + + map_.map.info.width = gridMap.getSizeX(); + map_.map.info.height = gridMap.getSizeY(); + + map_.map.header.frame_id = p_map_frame_; + map_.map.data.resize(map_.map.info.width * map_.map.info.height); +} + +/* +void HectorMappingRos::setStaticMapData(const nav_msgs::OccupancyGrid& map) +{ + float cell_length = map.info.resolution; + Eigen::Vector2f mapOrigin (map.info.origin.position.x + cell_length*0.5f, + map.info.origin.position.y + cell_length*0.5f); + + int map_size_x = map.info.width; + int map_size_y = map.info.height; + + slamProcessor = new hectorslam::HectorSlamProcessor(cell_length, map_size_x, map_size_y, Eigen::Vector2f(0.0f, 0.0f), 1, hectorDrawings, debugInfoProvider); +} +*/ + + +void HectorMappingRos::publishMapLoop(double map_pub_period) +{ + ros::Rate r(1.0 / map_pub_period); + while(ros::ok()) + { + //ros::WallTime t1 = ros::WallTime::now(); + ros::Time mapTime (ros::Time::now()); + //publishMap(mapPubContainer[2],slamProcessor->getGridMap(2), mapTime); + //publishMap(mapPubContainer[1],slamProcessor->getGridMap(1), mapTime); + publishMap(mapPubContainer[0],slamProcessor->getGridMap(0), mapTime, slamProcessor->getMapMutex(0)); + + //ros::WallDuration t2 = ros::WallTime::now() - t1; + + //std::cout << "time s: " << t2.toSec(); + //ROS_INFO("HectorSM ms: %4.2f", t2.toSec()*1000.0f); + + r.sleep(); + } +} + +void HectorMappingRos::staticMapCallback(const nav_msgs::OccupancyGrid& map) +{ + +} + +void HectorMappingRos::initialPoseCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg) +{ + this->resetPose(msg->pose.pose); +} + +void HectorMappingRos::toggleMappingPause(bool pause) +{ + // Pause/unpause + if (pause && !pause_scan_processing_) + { + ROS_INFO("[HectorSM]: Mapping paused"); + } + else if (!pause && pause_scan_processing_) + { + ROS_INFO("[HectorSM]: Mapping no longer paused"); + } + pause_scan_processing_ = pause; +} + +void HectorMappingRos::resetPose(const geometry_msgs::Pose &pose) +{ + initial_pose_set_ = true; + initial_pose_ = Eigen::Vector3f(pose.position.x, pose.position.y, util::getYawFromQuat(pose.orientation)); + ROS_INFO("[HectorSM]: Setting initial pose with world coords x: %f y: %f yaw: %f", + initial_pose_[0], initial_pose_[1], initial_pose_[2]); +} diff --git a/Localizations/Libraries/Ros/hector_slam/hector_mapping/src/PoseInfoContainer.cpp b/Localizations/Libraries/Ros/hector_slam/hector_mapping/src/PoseInfoContainer.cpp new file mode 100644 index 0000000..df1ff13 --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_mapping/src/PoseInfoContainer.cpp @@ -0,0 +1,70 @@ +//================================================================================================= +// Copyright (c) 2012, Stefan Kohlbrecher, TU Darmstadt +// All rights reserved. + +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of the Simulation, Systems Optimization and Robotics +// group, TU Darmstadt nor the names of its contributors may be used to +// endorse or promote products derived from this software without +// specific prior written permission. + +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +//================================================================================================= + +#include "hector_mapping/PoseInfoContainer.h" + +void PoseInfoContainer::update(const Eigen::Vector3f& slamPose, const Eigen::Matrix3f& slamCov, const ros::Time& stamp, const std::string& frame_id) +{ + //Fill stampedPose + std_msgs::Header& header = stampedPose_.header; + header.stamp = stamp; + header.frame_id = frame_id; + + geometry_msgs::Pose& pose = stampedPose_.pose; + pose.position.x = slamPose.x(); + pose.position.y = slamPose.y(); + + pose.orientation.w = cos(slamPose.z()*0.5f); + pose.orientation.z = sin(slamPose.z()*0.5f); + + //Fill covPose + //geometry_msgs::PoseWithCovarianceStamped covPose; + covPose_.header = header; + covPose_.pose.pose = pose; + + boost::array& cov(covPose_.pose.covariance); + + cov[0] = static_cast(slamCov(0,0)); + cov[7] = static_cast(slamCov(1,1)); + cov[35] = static_cast(slamCov(2,2)); + + double xyC = static_cast(slamCov(0,1)); + cov[1] = xyC; + cov[6] = xyC; + + double xaC = static_cast(slamCov(0,2)); + cov[5] = xaC; + cov[30] = xaC; + + double yaC = static_cast(slamCov(1,2)); + cov[11] = yaC; + cov[31] = yaC; + + //Fill tf tansform + tf::poseMsgToTF(pose, poseTransform_); +} diff --git a/Localizations/Libraries/Ros/hector_slam/hector_mapping/src/main.cpp b/Localizations/Libraries/Ros/hector_slam/hector_mapping/src/main.cpp new file mode 100644 index 0000000..918d415 --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_mapping/src/main.cpp @@ -0,0 +1,44 @@ +//================================================================================================= +// Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt +// All rights reserved. + +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of the Simulation, Systems Optimization and Robotics +// group, TU Darmstadt nor the names of its contributors may be used to +// endorse or promote products derived from this software without +// specific prior written permission. + +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +//================================================================================================= + + +#include + +#include "hector_mapping/HectorMappingRos.h" + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "hector_slam"); + + HectorMappingRos sm; + + ros::spin(); + + return(0); +} + diff --git a/Localizations/Libraries/Ros/hector_slam/hector_mapping/src/main_mapper.cpp b/Localizations/Libraries/Ros/hector_slam/hector_mapping/src/main_mapper.cpp new file mode 100644 index 0000000..e76264a --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_mapping/src/main_mapper.cpp @@ -0,0 +1,44 @@ +//================================================================================================= +// Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt +// All rights reserved. + +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of the Simulation, Systems Optimization and Robotics +// group, TU Darmstadt nor the names of its contributors may be used to +// endorse or promote products derived from this software without +// specific prior written permission. + +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +//================================================================================================= + + +#include + +#include "HectorMapperRos.h" + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "hector_slam"); + + HectorMapperRos sm; + + ros::spin(); + + return(0); +} + diff --git a/Localizations/Libraries/Ros/hector_slam/hector_mapping/srv/ResetMapping.srv b/Localizations/Libraries/Ros/hector_slam/hector_mapping/srv/ResetMapping.srv new file mode 100644 index 0000000..8905dad --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_mapping/srv/ResetMapping.srv @@ -0,0 +1,2 @@ +geometry_msgs/Pose initial_pose +--- diff --git a/Localizations/Libraries/Ros/hector_slam/hector_marker_drawing/CHANGELOG.rst b/Localizations/Libraries/Ros/hector_slam/hector_marker_drawing/CHANGELOG.rst new file mode 100644 index 0000000..86f6079 --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_marker_drawing/CHANGELOG.rst @@ -0,0 +1,46 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package hector_marker_drawing +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.5.2 (2021-04-08) +------------------ + +0.5.1 (2021-01-15) +------------------ + +0.5.0 (2020-12-17) +------------------ +* Moved hector_geotiff launch files to separate package to solve cyclic dependency. + Clean up for noetic release. +* Bump CMake version to avoid CMP0048 warning +* Contributors: Marius Schnaubelt, Stefan Fabian + +0.4.1 (2020-05-15) +------------------ + +0.3.6 (2019-10-31) +------------------ + +0.3.5 (2016-06-24) +------------------ +* Use the FindEigen3.cmake module provided by Eigen +* Contributors: Johannes Meyer + +0.3.4 (2015-11-07) +------------------ + +0.3.3 (2014-06-15) +------------------ +* fixed cmake find for eigen in indigo +* Contributors: Alexander Stumpf + +0.3.2 (2014-03-30) +------------------ + +0.3.1 (2013-10-09) +------------------ +* added changelogs + +0.3.0 (2013-08-08) +------------------ +* catkinized hector_slam diff --git a/Localizations/Libraries/Ros/hector_slam/hector_marker_drawing/CMakeLists.txt b/Localizations/Libraries/Ros/hector_slam/hector_marker_drawing/CMakeLists.txt new file mode 100644 index 0000000..dfe7333 --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_marker_drawing/CMakeLists.txt @@ -0,0 +1,25 @@ +cmake_minimum_required(VERSION 3.0.2) +project(hector_marker_drawing) + +find_package(catkin REQUIRED COMPONENTS roscpp visualization_msgs) + +find_package(Eigen3 REQUIRED) + +catkin_package( + INCLUDE_DIRS include + CATKIN_DEPENDS roscpp visualization_msgs + DEPENDS EIGEN3 +) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE +) diff --git a/Localizations/Libraries/Ros/hector_slam/hector_marker_drawing/include/hector_marker_drawing/DrawInterface.h b/Localizations/Libraries/Ros/hector_slam/hector_marker_drawing/include/hector_marker_drawing/DrawInterface.h new file mode 100644 index 0000000..7fea1e3 --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_marker_drawing/include/hector_marker_drawing/DrawInterface.h @@ -0,0 +1,50 @@ +//================================================================================================= +// Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt +// All rights reserved. + +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of the Simulation, Systems Optimization and Robotics +// group, TU Darmstadt nor the names of its contributors may be used to +// endorse or promote products derived from this software without +// specific prior written permission. + +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +//================================================================================================= + +#ifndef drawinterface_h__ +#define drawinterface_h__ + +#include +#include + +class DrawInterface{ +public: + virtual void setNamespace(const std::string& ns) = 0; + + virtual void drawPoint(const Eigen::Vector2f& pointWorldFrame) = 0; + virtual void drawArrow(const Eigen::Vector3f& poseWorld) = 0; + virtual void drawCovariance(const Eigen::Vector2f& mean, const Eigen::Matrix2f& cov) = 0; + virtual void drawCovariance(const Eigen::Vector3f& mean, const Eigen::Matrix3f& covMatrix) = 0; + + virtual void setScale(double scale) = 0; + virtual void setColor(double r, double g, double b, double a = 1.0) = 0; + + virtual void sendAndResetData() = 0; +}; + +#endif diff --git a/Localizations/Libraries/Ros/hector_slam/hector_marker_drawing/include/hector_marker_drawing/HectorDrawings.h b/Localizations/Libraries/Ros/hector_slam/hector_marker_drawing/include/hector_marker_drawing/HectorDrawings.h new file mode 100644 index 0000000..b04d77c --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_marker_drawing/include/hector_marker_drawing/HectorDrawings.h @@ -0,0 +1,242 @@ +//================================================================================================= +// Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt +// All rights reserved. + +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of the Simulation, Systems Optimization and Robotics +// group, TU Darmstadt nor the names of its contributors may be used to +// endorse or promote products derived from this software without +// specific prior written permission. + +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +//================================================================================================= + +#include "DrawInterface.h" +//#include "util/UtilFunctions.h" + +#include "ros/ros.h" + +#include +#include + + +#include + +class HectorDrawings : public DrawInterface +{ +public: + + HectorDrawings() + { + idCounter = 0; + maxId = 0; + + ros::NodeHandle nh_; + + markerPublisher_ = nh_.advertise("visualization_marker", 1, true); + markerArrayPublisher_ = nh_.advertise("visualization_marker_array", 1, true); + + tempMarker.header.frame_id = "map"; + tempMarker.ns = "marker"; + + this->setScale(1.0); + this->setColor(1.0, 1.0, 1.0); + + tempMarker.action = visualization_msgs::Marker::ADD; + }; + + virtual void setNamespace(const std::string& ns) + { + tempMarker.ns = ns; + } + + virtual void drawPoint(const Eigen::Vector2f& pointWorldFrame) + { + tempMarker.id = idCounter++; + + tempMarker.pose.position.x = pointWorldFrame.x(); + tempMarker.pose.position.y = pointWorldFrame.y(); + + tempMarker.pose.orientation.w = 0.0; + tempMarker.pose.orientation.z = 0.0; + tempMarker.type = visualization_msgs::Marker::CUBE; + + //markerPublisher_.publish(tempMarker); + + markerArray.markers.push_back(tempMarker); + } + + virtual void drawArrow(const Eigen::Vector3f& poseWorld) + { + tempMarker.id = idCounter++; + + tempMarker.pose.position.x = poseWorld.x(); + tempMarker.pose.position.y = poseWorld.y(); + + tempMarker.pose.orientation.w = cos(poseWorld.z()*0.5f); + tempMarker.pose.orientation.z = sin(poseWorld.z()*0.5f); + + tempMarker.type = visualization_msgs::Marker::ARROW; + + //markerPublisher_.publish(tempMarker); + + markerArray.markers.push_back(tempMarker); + + } + + virtual void drawCovariance(const Eigen::Vector2f& mean, const Eigen::Matrix2f& covMatrix) + { + + tempMarker.pose.position.x = mean[0]; + tempMarker.pose.position.y = mean[1]; + + Eigen::SelfAdjointEigenSolver eig(covMatrix); + + const Eigen::Vector2f& eigValues (eig.eigenvalues()); + const Eigen::Matrix2f& eigVectors (eig.eigenvectors()); + + float angle = (atan2(eigVectors(1, 0), eigVectors(0, 0))); + + tempMarker.type = visualization_msgs::Marker::CYLINDER; + + double lengthMajor = sqrt(eigValues[0]); + double lengthMinor = sqrt(eigValues[1]); + + tempMarker.scale.x = lengthMajor; + tempMarker.scale.y = lengthMinor; + tempMarker.scale.z = 0.001; + + + tempMarker.pose.orientation.w = cos(angle*0.5); + tempMarker.pose.orientation.z = sin(angle*0.5); + + tempMarker.id = idCounter++; + markerArray.markers.push_back(tempMarker); + } + + virtual void drawCovariance(const Eigen::Vector3f& mean, const Eigen::Matrix3f& covMatrix) + { + tempMarker.type = visualization_msgs::Marker::SPHERE; + + tempMarker.color.r = 0.0; + tempMarker.color.a = 0.5; + + tempMarker.pose.position.x = mean[0]; + tempMarker.pose.position.y = mean[1]; + tempMarker.pose.position.z = mean[2]; + + Eigen::SelfAdjointEigenSolver eig(covMatrix); + + const Eigen::Vector3f& eigValues (eig.eigenvalues()); + const Eigen::Matrix3f& eigVectors (eig.eigenvectors()); + + + Eigen::Matrix3f eigVectorsFlipped; + eigVectorsFlipped.col(0) = eigVectors.col(2); + eigVectorsFlipped.col(1) = eigVectors.col(1); + eigVectorsFlipped.col(2) = eigVectors.col(0); + + if (eigVectorsFlipped.determinant() < 0){ + eigVectorsFlipped.col(2) = -eigVectorsFlipped.col(2); + } + + Eigen::Quaternionf quaternion (eigVectorsFlipped); + + //std::cout << "\neigVec:\n" << eigVectors << "\n"; + //std::cout << "\neigVecFlipped:\n" << eigVectorsFlipped << "\n"; + + //std::cout << "\now:" << quaternion.w() << " x:" << quaternion.x() << " y:" << quaternion.y() << " z:" << quaternion.z() << "\n"; + + + tempMarker.pose.orientation.w = quaternion.w(); + tempMarker.pose.orientation.x = quaternion.x(); + tempMarker.pose.orientation.y = quaternion.y(); + tempMarker.pose.orientation.z = quaternion.z(); + + tempMarker.scale.x = sqrt(eigValues[2]); + tempMarker.scale.y = sqrt(eigValues[1]); + tempMarker.scale.z = sqrt(eigValues[0]); + + tempMarker.id = idCounter++; + markerArray.markers.push_back(tempMarker); + + + } + + virtual void setScale(double scale) + { + tempMarker.scale.x = scale; + tempMarker.scale.y = scale; + tempMarker.scale.z = scale; + } + + virtual void setColor(double r, double g, double b, double a = 1.0) + { + tempMarker.color.r = r; + tempMarker.color.g = g; + tempMarker.color.b = b; + tempMarker.color.a = a; + } + + virtual void addMarker(visualization_msgs::Marker marker) { + if (marker.id == 0) marker.id = idCounter++; + if (marker.ns.empty()) marker.ns = tempMarker.ns; + markerArray.markers.push_back(marker); + } + + virtual void addMarkers(visualization_msgs::MarkerArray markers) { + for(visualization_msgs::MarkerArray::_markers_type::iterator it = markers.markers.begin(); it != markers.markers.end(); ++it) { + visualization_msgs::Marker &marker = *it; + addMarker(marker); + } + } + + virtual void sendAndResetData() + { + allMarkers.markers.insert(allMarkers.markers.end(), markerArray.markers.begin(), markerArray.markers.end()); + markerArrayPublisher_.publish(markerArray); + markerArray.markers.clear(); + if (idCounter > maxId) maxId = idCounter; + idCounter = 0; + } + + void setTime(const ros::Time& time) + { + tempMarker.header.stamp = time; + } + + void reset() + { + for(visualization_msgs::MarkerArray::_markers_type::iterator it = allMarkers.markers.begin(); it != allMarkers.markers.end(); ++it) + { + it->action = visualization_msgs::Marker::DELETE; + } + markerArrayPublisher_.publish(allMarkers); + allMarkers.markers.clear(); + } + + ros::Publisher markerPublisher_; + ros::Publisher markerArrayPublisher_; + + visualization_msgs::Marker tempMarker; + visualization_msgs::MarkerArray markerArray; + visualization_msgs::MarkerArray allMarkers; + + int idCounter; + int maxId; +}; diff --git a/Localizations/Libraries/Ros/hector_slam/hector_marker_drawing/package.xml b/Localizations/Libraries/Ros/hector_slam/hector_marker_drawing/package.xml new file mode 100644 index 0000000..bea879b --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_marker_drawing/package.xml @@ -0,0 +1,56 @@ + + + hector_marker_drawing + 0.5.2 + + hector_marker_drawing provides convenience functions for easier publishing of visualization markers. + + + + Johannes Meyer + + + + + + BSD + + + + + http://ros.org/wiki/hector_marker_drawing + + + + + Stefan Kohlbrecher + + + + + + + + + + + + + + catkin + roscpp + visualization_msgs + eigen + roscpp + visualization_msgs + eigen + + + + + + + + + + diff --git a/Localizations/Libraries/Ros/hector_slam/hector_nav_msgs/CHANGELOG.rst b/Localizations/Libraries/Ros/hector_slam/hector_nav_msgs/CHANGELOG.rst new file mode 100644 index 0000000..0beed9f --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_nav_msgs/CHANGELOG.rst @@ -0,0 +1,47 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package hector_nav_msgs +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.5.2 (2021-04-08) +------------------ + +0.5.1 (2021-01-15) +------------------ + +0.5.0 (2020-12-17) +------------------ +* Moved hector_geotiff launch files to separate package to solve cyclic dependency. + Clean up for noetic release. +* Bump CMake version to avoid CMP0048 warning +* Contributors: Marius Schnaubelt, Stefan Fabian + +0.4.1 (2020-05-15) +------------------ + +0.3.6 (2019-10-31) +------------------ + +0.3.5 (2016-06-24) +------------------ + +0.3.4 (2015-11-07) +------------------ +* hector_nav_msgs: removed yaw member from GetNormal response + yaw is implicitly given by the normal vector +* Contributors: Dorothea Koert + +0.3.3 (2014-06-15) +------------------ +* added GetNormal service, that returns normal and orientation of an occupied cell +* Contributors: Dorothea Koert + +0.3.2 (2014-03-30) +------------------ + +0.3.1 (2013-10-09) +------------------ +* added changelogs + +0.3.0 (2013-08-08) +------------------ +* catkinized hector_slam diff --git a/Localizations/Libraries/Ros/hector_slam/hector_nav_msgs/CMakeLists.txt b/Localizations/Libraries/Ros/hector_slam/hector_nav_msgs/CMakeLists.txt new file mode 100644 index 0000000..0731a82 --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_nav_msgs/CMakeLists.txt @@ -0,0 +1,28 @@ +cmake_minimum_required(VERSION 3.0.2) +project(hector_nav_msgs) + +find_package(catkin REQUIRED COMPONENTS nav_msgs geometry_msgs message_generation std_msgs) + + +####################################### +## Declare ROS messages and services ## +####################################### + +## Generate services in the 'srv' folder +add_service_files( + FILES + GetDistanceToObstacle.srv + GetRecoveryInfo.srv + GetRobotTrajectory.srv + GetSearchPosition.srv + GetNormal.srv +) + +generate_messages( + DEPENDENCIES + geometry_msgs nav_msgs std_msgs +) + +catkin_package( + CATKIN_DEPENDS nav_msgs geometry_msgs message_runtime std_msgs +) diff --git a/Localizations/Libraries/Ros/hector_slam/hector_nav_msgs/package.xml b/Localizations/Libraries/Ros/hector_slam/hector_nav_msgs/package.xml new file mode 100644 index 0000000..9ce858f --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_nav_msgs/package.xml @@ -0,0 +1,44 @@ + + + hector_nav_msgs + 0.5.2 + + hector_nav_msgs contains messages and services used in the hector_slam stack. + + + + Johannes Meyer + + + + + + BSD + + + + + http://ros.org/wiki/hector_nav_msgs + + + + + Stefan Kohlbrecher + + + catkin + nav_msgs + geometry_msgs + std_msgs + message_generation + message_runtime + + + + + + + + + + diff --git a/Localizations/Libraries/Ros/hector_slam/hector_nav_msgs/srv/GetDistanceToObstacle.srv b/Localizations/Libraries/Ros/hector_slam/hector_nav_msgs/srv/GetDistanceToObstacle.srv new file mode 100644 index 0000000..c4af099 --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_nav_msgs/srv/GetDistanceToObstacle.srv @@ -0,0 +1,11 @@ +# Returns the distance to the next obstacle from the origin of frame point.header.frame_id +# in the direction of the point +# +# All units are meters. + +geometry_msgs/PointStamped point +--- +float32 distance +geometry_msgs/PointStamped end_point + + diff --git a/Localizations/Libraries/Ros/hector_slam/hector_nav_msgs/srv/GetNormal.srv b/Localizations/Libraries/Ros/hector_slam/hector_nav_msgs/srv/GetNormal.srv new file mode 100644 index 0000000..08d7206 --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_nav_msgs/srv/GetNormal.srv @@ -0,0 +1,3 @@ +geometry_msgs/PointStamped point +--- +geometry_msgs/Vector3 normal diff --git a/Localizations/Libraries/Ros/hector_slam/hector_nav_msgs/srv/GetRecoveryInfo.srv b/Localizations/Libraries/Ros/hector_slam/hector_nav_msgs/srv/GetRecoveryInfo.srv new file mode 100644 index 0000000..c826939 --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_nav_msgs/srv/GetRecoveryInfo.srv @@ -0,0 +1,11 @@ +# Returns the path travelled to get to req_pose (pose determined by request_time) +# up to request_radius away from req_pose. +# + +time request_time +float64 request_radius +--- +nav_msgs/Path trajectory_radius_entry_pose_to_req_pose +geometry_msgs/PoseStamped radius_entry_pose +geometry_msgs/PoseStamped req_pose + diff --git a/Localizations/Libraries/Ros/hector_slam/hector_nav_msgs/srv/GetRobotTrajectory.srv b/Localizations/Libraries/Ros/hector_slam/hector_nav_msgs/srv/GetRobotTrajectory.srv new file mode 100644 index 0000000..0d1bfa0 --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_nav_msgs/srv/GetRobotTrajectory.srv @@ -0,0 +1,8 @@ +# Returns the distance to the next obstacle from the origin of frame point.header.frame_id +# in the direction of the point +# +# All units are meters. + +--- +nav_msgs/Path trajectory + diff --git a/Localizations/Libraries/Ros/hector_slam/hector_nav_msgs/srv/GetSearchPosition.srv b/Localizations/Libraries/Ros/hector_slam/hector_nav_msgs/srv/GetSearchPosition.srv new file mode 100644 index 0000000..427bc29 --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_nav_msgs/srv/GetSearchPosition.srv @@ -0,0 +1,7 @@ +#Returns a suggested search/observation position for an object of interest located at ooi_pose + +geometry_msgs/PoseStamped ooi_pose +float32 distance +--- +geometry_msgs/PoseStamped search_pose + diff --git a/Localizations/Libraries/Ros/hector_slam/hector_slam/CHANGELOG.rst b/Localizations/Libraries/Ros/hector_slam/hector_slam/CHANGELOG.rst new file mode 100644 index 0000000..6a519de --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_slam/CHANGELOG.rst @@ -0,0 +1,42 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package hector_slam +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.5.2 (2021-04-08) +------------------ + +0.5.1 (2021-01-15) +------------------ +* Added hector_geotiff_launch to hector_slam metapackge. +* Contributors: Stefan Fabian + +0.5.0 (2020-12-17) +------------------ +* Bump CMake version to avoid CMP0048 warning +* Contributors: Marius Schnaubelt, Stefan Fabian + +0.4.1 (2020-05-15) +------------------ + +0.3.6 (2019-10-31) +------------------ + +0.3.5 (2016-06-24) +------------------ + +0.3.4 (2015-11-07) +------------------ + +0.3.3 (2014-06-15) +------------------ + +0.3.2 (2014-03-30) +------------------ + +0.3.1 (2013-10-09) +------------------ +* added changelogs + +0.3.0 (2013-08-08) +------------------ +* catkinized hector_slam diff --git a/Localizations/Libraries/Ros/hector_slam/hector_slam/CMakeLists.txt b/Localizations/Libraries/Ros/hector_slam/hector_slam/CMakeLists.txt new file mode 100644 index 0000000..bd204fb --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_slam/CMakeLists.txt @@ -0,0 +1,4 @@ +cmake_minimum_required(VERSION 3.0.2) +project(hector_slam) +find_package(catkin REQUIRED) +catkin_metapackage() diff --git a/Localizations/Libraries/Ros/hector_slam/hector_slam/package.xml b/Localizations/Libraries/Ros/hector_slam/hector_slam/package.xml new file mode 100644 index 0000000..bffffca --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_slam/package.xml @@ -0,0 +1,64 @@ + + + hector_slam + 0.5.2 + + The hector_slam metapackage that installs hector_mapping and related packages. + + + + Johannes Meyer + + + + + + BSD + + + + + http://ros.org/wiki/hector_slam + + + + + Stefan Kohlbrecher + Johannes Meyer + + + + + + + + + + + + + + catkin + hector_compressed_map_transport + hector_geotiff + hector_geotiff_launch + hector_geotiff_plugins + hector_imu_attitude_to_tf + hector_map_server + hector_map_tools + hector_mapping + hector_marker_drawing + hector_nav_msgs + hector_slam_launch + hector_trajectory_server + + + + + + + + + + + diff --git a/Localizations/Libraries/Ros/hector_slam/hector_slam_launch/CHANGELOG.rst b/Localizations/Libraries/Ros/hector_slam/hector_slam_launch/CHANGELOG.rst new file mode 100644 index 0000000..0981cb8 --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_slam_launch/CHANGELOG.rst @@ -0,0 +1,52 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package hector_slam_launch +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.5.2 (2021-04-08) +------------------ + +0.5.1 (2021-01-15) +------------------ +* Updated paths to launch files. +* Contributors: Stefan Fabian + +0.5.0 (2020-12-17) +------------------ +* Moved hector_geotiff launch files to separate package to solve cyclic dependency. + Clean up for noetic release. +* Bump CMake version to avoid CMP0048 warning +* Contributors: Marius Schnaubelt, Stefan Fabian + +0.4.1 (2020-05-15) +------------------ + +0.3.6 (2019-10-31) +------------------ + +0.3.5 (2016-06-24) +------------------ + +0.3.4 (2015-11-07) +------------------ +* Removes trailing spaces and fixes indents +* "rviz_cfg" directory in the repository version. +* Contributors: YuK_Ota, dani-carbonell + +0.3.3 (2014-06-15) +------------------ + +0.3.2 (2014-03-30) +------------------ +* Add arguments to launch files for specifying geotiff file path +* Update rviz config to have proper default displays +* deleted quadrotor_uav.launch (moved to hector_quadrotor) +* Contributors: Johannes Meyer, Stefan Kohlbrecher + +0.3.1 (2013-10-09) +------------------ +* updated rviz config to the new .rviz format +* added changelogs + +0.3.0 (2013-08-08) +------------------ +* catkinized hector_slam diff --git a/Localizations/Libraries/Ros/hector_slam/hector_slam_launch/CMakeLists.txt b/Localizations/Libraries/Ros/hector_slam/hector_slam_launch/CMakeLists.txt new file mode 100644 index 0000000..34638d3 --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_slam_launch/CMakeLists.txt @@ -0,0 +1,18 @@ +cmake_minimum_required(VERSION 3.0.2) +project(hector_slam_launch) + +find_package(catkin REQUIRED) + +catkin_package() + + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +install(DIRECTORY launch rviz_cfg + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) diff --git a/Localizations/Libraries/Ros/hector_slam/hector_slam_launch/launch/cityflyer_logfile_processing.launch b/Localizations/Libraries/Ros/hector_slam/hector_slam_launch/launch/cityflyer_logfile_processing.launch new file mode 100644 index 0000000..744a4e8 --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_slam_launch/launch/cityflyer_logfile_processing.launch @@ -0,0 +1,62 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Localizations/Libraries/Ros/hector_slam/hector_slam_launch/launch/hector_ugv.launch b/Localizations/Libraries/Ros/hector_slam/hector_slam_launch/launch/hector_ugv.launch new file mode 100644 index 0000000..dcec9a1 --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_slam_launch/launch/hector_ugv.launch @@ -0,0 +1,33 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Localizations/Libraries/Ros/hector_slam/hector_slam_launch/launch/height_mapping.launch b/Localizations/Libraries/Ros/hector_slam/hector_slam_launch/launch/height_mapping.launch new file mode 100644 index 0000000..4411581 --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_slam_launch/launch/height_mapping.launch @@ -0,0 +1,44 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Localizations/Libraries/Ros/hector_slam/hector_slam_launch/launch/mapping_box.launch b/Localizations/Libraries/Ros/hector_slam/hector_slam_launch/launch/mapping_box.launch new file mode 100644 index 0000000..033b711 --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_slam_launch/launch/mapping_box.launch @@ -0,0 +1,54 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Localizations/Libraries/Ros/hector_slam/hector_slam_launch/launch/mpo700_mapping.launch b/Localizations/Libraries/Ros/hector_slam/hector_slam_launch/launch/mpo700_mapping.launch new file mode 100644 index 0000000..889d6f7 --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_slam_launch/launch/mpo700_mapping.launch @@ -0,0 +1,32 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Localizations/Libraries/Ros/hector_slam/hector_slam_launch/launch/postproc_data.launch b/Localizations/Libraries/Ros/hector_slam/hector_slam_launch/launch/postproc_data.launch new file mode 100644 index 0000000..206d90a --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_slam_launch/launch/postproc_data.launch @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/Localizations/Libraries/Ros/hector_slam/hector_slam_launch/launch/postproc_qut_logs.launch b/Localizations/Libraries/Ros/hector_slam/hector_slam_launch/launch/postproc_qut_logs.launch new file mode 100644 index 0000000..9293513 --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_slam_launch/launch/postproc_qut_logs.launch @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/Localizations/Libraries/Ros/hector_slam/hector_slam_launch/launch/pr2os.launch b/Localizations/Libraries/Ros/hector_slam/hector_slam_launch/launch/pr2os.launch new file mode 100644 index 0000000..231537e --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_slam_launch/launch/pr2os.launch @@ -0,0 +1,57 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Localizations/Libraries/Ros/hector_slam/hector_slam_launch/launch/tutorial.launch b/Localizations/Libraries/Ros/hector_slam/hector_slam_launch/launch/tutorial.launch new file mode 100644 index 0000000..5210c6e --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_slam_launch/launch/tutorial.launch @@ -0,0 +1,19 @@ + + + + + + + + + + + + + + + + + + diff --git a/Localizations/Libraries/Ros/hector_slam/hector_slam_launch/package.xml b/Localizations/Libraries/Ros/hector_slam/hector_slam_launch/package.xml new file mode 100644 index 0000000..ead3c40 --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_slam_launch/package.xml @@ -0,0 +1,47 @@ + + + hector_slam_launch + 0.5.2 + + hector_slam_launch contains launch files for launching hector_slam with different robot systems/setups/postprocessing scenarios. + + + + Johannes Meyer + + + + + + BSD + + + + + http://ros.org/wiki/hector_slam_launch + + + + + Stefan Kohlbrecher + + + catkin + hector_mapping + hector_map_server + hector_trajectory_server + hector_geotiff + hector_geotiff_plugins + rviz + tf + topic_tools + + + + + + + + + + diff --git a/Localizations/Libraries/Ros/hector_slam/hector_slam_launch/rviz_cfg/mapping_demo.rviz b/Localizations/Libraries/Ros/hector_slam/hector_slam_launch/rviz_cfg/mapping_demo.rviz new file mode 100644 index 0000000..1d998ac --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_slam_launch/rviz_cfg/mapping_demo.rviz @@ -0,0 +1,137 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /Map1 + - /Path1 + - /Pose1 + Splitter Ratio: 0.5 + Tree Height: 540 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.588679 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.03 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 0.7 + Class: rviz/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: Map + Topic: /map + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 25; 255; 0 + Enabled: true + Name: Path + Topic: /trajectory + Value: true + - Alpha: 1 + Axes Length: 1 + Axes Radius: 0.1 + Class: rviz/Pose + Color: 255; 25; 0 + Enabled: true + Head Length: 0.3 + Head Radius: 0.1 + Name: Pose + Shaft Length: 1 + Shaft Radius: 0.05 + Shape: Axes + Topic: /slam_out_pose + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Topic: /initialpose + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/XYOrbit + Distance: 44.388 + Focal Point: + X: 9.50434 + Y: -0.685607 + Z: 0 + Name: Current View + Near Clip Distance: 0.01 + Pitch: 1.56976 + Target Frame: + Value: XYOrbit (rviz) + Yaw: -0.7846 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 846 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd00000004000000000000013c000002abfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000041000002ab000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002abfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000041000002ab000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000002f600fffffffb0000000800540069006d0065010000000000000450000000000000000000000259000002ab00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1200 + X: 53 + Y: 60 diff --git a/Localizations/Libraries/Ros/hector_slam/hector_trajectory_server/CHANGELOG.rst b/Localizations/Libraries/Ros/hector_slam/hector_trajectory_server/CHANGELOG.rst new file mode 100644 index 0000000..0fdcd6d --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_trajectory_server/CHANGELOG.rst @@ -0,0 +1,51 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package hector_trajectory_server +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.5.2 (2021-04-08) +------------------ + +0.5.1 (2021-01-15) +------------------ + +0.5.0 (2020-12-17) +------------------ +* Moved hector_geotiff launch files to separate package to solve cyclic dependency. + Clean up for noetic release. +* Bump CMake version to avoid CMP0048 warning +* Contributors: Marius Schnaubelt, Stefan Fabian + +0.4.1 (2020-05-15) +------------------ + +0.3.6 (2019-10-31) +------------------ + +0.3.5 (2016-06-24) +------------------ +* Changed from ros::WallTime to ros::Time in trajectory server +* hector_trajectory_server: removed bug leading to potential infinite loop +* Contributors: Andreas Lindahl Flåten, Paul Manns + +0.3.4 (2015-11-07) +------------------ + +0.3.3 (2014-06-15) +------------------ + +0.3.2 (2014-03-30) +------------------ +* wait based on WallTime and check ros::ok() in waitForTf() +* Print out tf availability warning only once. +* Wait for tf become available to suppress warnings on startup +* Create a warning instead of an error for TransformExceptions +* Contributors: Johannes Meyer, Stefan Kohlbrecher, wachaja + +0.3.1 (2013-10-09) +------------------ +* added changelogs +* added cmake dependencies to catkin exported targets to force message targets to be built before any cpp target using them + +0.3.0 (2013-08-08) +------------------ +* catkinized hector_slam diff --git a/Localizations/Libraries/Ros/hector_slam/hector_trajectory_server/CMakeLists.txt b/Localizations/Libraries/Ros/hector_slam/hector_trajectory_server/CMakeLists.txt new file mode 100644 index 0000000..578b279 --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_trajectory_server/CMakeLists.txt @@ -0,0 +1,35 @@ +cmake_minimum_required(VERSION 3.0.2) +project(hector_trajectory_server) + +find_package(catkin REQUIRED COMPONENTS roscpp hector_nav_msgs nav_msgs hector_map_tools tf) + +catkin_package( + CATKIN_DEPENDS roscpp hector_nav_msgs nav_msgs hector_map_tools tf +) + +########### +## Build ## +########### + +include_directories( + ${catkin_INCLUDE_DIRS} +) + +add_executable(hector_trajectory_server src/hector_trajectory_server.cpp) +add_dependencies(hector_trajectory_server ${catkin_EXPORTED_TARGETS}) +target_link_libraries(hector_trajectory_server + ${catkin_LIBRARIES} +) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +install(TARGETS hector_trajectory_server + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) diff --git a/Localizations/Libraries/Ros/hector_slam/hector_trajectory_server/package.xml b/Localizations/Libraries/Ros/hector_slam/hector_trajectory_server/package.xml new file mode 100644 index 0000000..bf3e1dd --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_trajectory_server/package.xml @@ -0,0 +1,60 @@ + + + hector_trajectory_server + 0.5.2 + + hector_trajectory_server keeps track of tf trajectories extracted from tf data and makes this data accessible via a service and topic. + + + + Johannes Meyer + + + + + + BSD + + + + + http://ros.org/wiki/hector_trajectory_server + + + + + Stefan Kohlbrecher + + + + + + + + + + + + + + catkin + roscpp + hector_nav_msgs + nav_msgs + hector_map_tools + tf + roscpp + hector_nav_msgs + nav_msgs + hector_map_tools + tf + + + + + + + + + + diff --git a/Localizations/Libraries/Ros/hector_slam/hector_trajectory_server/src/hector_trajectory_server.cpp b/Localizations/Libraries/Ros/hector_slam/hector_trajectory_server/src/hector_trajectory_server.cpp new file mode 100644 index 0000000..f3e9785 --- /dev/null +++ b/Localizations/Libraries/Ros/hector_slam/hector_trajectory_server/src/hector_trajectory_server.cpp @@ -0,0 +1,277 @@ +//================================================================================================= +// Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt +// All rights reserved. + +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of the Simulation, Systems Optimization and Robotics +// group, TU Darmstadt nor the names of its contributors may be used to +// endorse or promote products derived from this software without +// specific prior written permission. + +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +//================================================================================================= + +#include +#include "ros/ros.h" +#include "ros/console.h" + +#include "nav_msgs/Path.h" +#include "std_msgs/String.h" + +#include +#include + +#include "tf/transform_listener.h" + +#include +#include + +#include + +#include + +using namespace std; + +bool comparePoseStampedStamps (const geometry_msgs::PoseStamped& t1, const geometry_msgs::PoseStamped& t2) { return (t1.header.stamp < t2.header.stamp); } + + +/** + * @brief Map generation node. + */ +class PathContainer +{ +public: + PathContainer() + { + ros::NodeHandle private_nh("~"); + + private_nh.param("target_frame_name", p_target_frame_name_, std::string("map")); + private_nh.param("source_frame_name", p_source_frame_name_, std::string("base_link")); + private_nh.param("trajectory_update_rate", p_trajectory_update_rate_, 4.0); + private_nh.param("trajectory_publish_rate", p_trajectory_publish_rate_, 0.25); + + waitForTf(); + + ros::NodeHandle nh; + sys_cmd_sub_ = nh.subscribe("syscommand", 1, &PathContainer::sysCmdCallback, this); + trajectory_pub_ = nh.advertise("trajectory",1, true); + + trajectory_provider_service_ = nh.advertiseService("trajectory", &PathContainer::trajectoryProviderCallBack, this); + recovery_info_provider_service_ = nh.advertiseService("trajectory_recovery_info", &PathContainer::recoveryInfoProviderCallBack, this); + + last_reset_time_ = ros::Time::now(); + + update_trajectory_timer_ = private_nh.createTimer(ros::Duration(1.0 / p_trajectory_update_rate_), &PathContainer::trajectoryUpdateTimerCallback, this, false); + publish_trajectory_timer_ = private_nh.createTimer(ros::Duration(1.0 / p_trajectory_publish_rate_), &PathContainer::publishTrajectoryTimerCallback, this, false); + + pose_source_.pose.orientation.w = 1.0; + pose_source_.header.frame_id = p_source_frame_name_; + + trajectory_.trajectory.header.frame_id = p_target_frame_name_; + } + + void waitForTf() + { + ros::Time start = ros::Time::now(); + ROS_INFO("Waiting for tf transform data between frames %s and %s to become available", p_target_frame_name_.c_str(), p_source_frame_name_.c_str() ); + + bool transform_successful = false; + + while (!transform_successful){ + transform_successful = tf_.canTransform(p_target_frame_name_, p_source_frame_name_, ros::Time()); + if (transform_successful) break; + + ros::Time now = ros::Time::now(); + + if ((now-start).toSec() > 20.0){ + ROS_WARN_ONCE("No transform between frames %s and %s available after %f seconds of waiting. This warning only prints once.", p_target_frame_name_.c_str(), p_source_frame_name_.c_str(), (now-start).toSec()); + } + + if (!ros::ok()) return; + ros::WallDuration(1.0).sleep(); + } + + ros::Time end = ros::Time::now(); + ROS_INFO("Finished waiting for tf, waited %f seconds", (end-start).toSec()); + } + + + void sysCmdCallback(const std_msgs::String& sys_cmd) + { + if (sys_cmd.data == "reset") + { + last_reset_time_ = ros::Time::now(); + trajectory_.trajectory.poses.clear(); + trajectory_.trajectory.header.stamp = ros::Time::now(); + } + } + + void addCurrentTfPoseToTrajectory() + { + pose_source_.header.stamp = ros::Time(0); + + geometry_msgs::PoseStamped pose_out; + + tf_.transformPose(p_target_frame_name_, pose_source_, pose_out); + + if (trajectory_.trajectory.poses.size() != 0){ + //Only add pose to trajectory if it's not already stored + if (pose_out.header.stamp != trajectory_.trajectory.poses.back().header.stamp){ + trajectory_.trajectory.poses.push_back(pose_out); + } + }else{ + trajectory_.trajectory.poses.push_back(pose_out); + } + + trajectory_.trajectory.header.stamp = pose_out.header.stamp; + } + + void trajectoryUpdateTimerCallback(const ros::TimerEvent& event) + { + + try{ + addCurrentTfPoseToTrajectory(); + }catch(tf::TransformException e) + { + ROS_WARN("Trajectory Server: Transform from %s to %s failed: %s \n", p_target_frame_name_.c_str(), pose_source_.header.frame_id.c_str(), e.what() ); + } + } + + void publishTrajectoryTimerCallback(const ros::TimerEvent& event) + { + trajectory_pub_.publish(trajectory_.trajectory); + } + + bool trajectoryProviderCallBack(hector_nav_msgs::GetRobotTrajectory::Request &req, + hector_nav_msgs::GetRobotTrajectory::Response &res ) + { + res = getTrajectory(); + return true; + } + + inline const hector_nav_msgs::GetRobotTrajectoryResponse getTrajectory() const + { + return trajectory_; + } + + bool recoveryInfoProviderCallBack(hector_nav_msgs::GetRecoveryInfo::Request &req, + hector_nav_msgs::GetRecoveryInfo::Response &res ) + { + const ros::Time req_time = req.request_time; + + geometry_msgs::PoseStamped tmp; + tmp.header.stamp = req_time; + + std::vector const & poses = trajectory_.trajectory.poses; + + if(poses.size() == 0) + { + ROS_WARN("Failed to find trajectory leading out of radius %f" + " because no poses, i.e. no inverse trajectory, exists.", req.request_radius); + return false; + } + + //Find the robot pose in the saved trajectory + std::vector::const_iterator it + = std::lower_bound(poses.begin(), poses.end(), tmp, comparePoseStampedStamps); + + //If we didn't find the robot pose for the desired time, add the current robot pose to trajectory + if (it == poses.end()){ + addCurrentTfPoseToTrajectory(); + it = poses.end(); + --it; + } + + std::vector::const_iterator it_start = it; + + const geometry_msgs::Point& req_coords ((*it).pose.position); + + double dist_sqr_threshold = req.request_radius * req.request_radius; + + double dist_sqr = 0.0; + + //Iterate backwards till the start of the trajectory is reached or we find a pose that's outside the specified radius + while (it != poses.begin() && dist_sqr < dist_sqr_threshold){ + const geometry_msgs::Point& curr_coords ((*it).pose.position); + + dist_sqr = (req_coords.x - curr_coords.x) * (req_coords.x - curr_coords.x) + + (req_coords.y - curr_coords.y) * (req_coords.y - curr_coords.y); + + --it; + } + + if (dist_sqr < dist_sqr_threshold){ + ROS_INFO("Failed to find trajectory leading out of radius %f", req.request_radius); + return false; + } + + std::vector::const_iterator it_end = it; + + res.req_pose = *it_start; + res.radius_entry_pose = *it_end; + + std::vector& traj_out_poses = res.trajectory_radius_entry_pose_to_req_pose.poses; + + res.trajectory_radius_entry_pose_to_req_pose.poses.clear(); + res.trajectory_radius_entry_pose_to_req_pose.header = res.req_pose.header; + + for (std::vector::const_iterator it_tmp = it_start; it_tmp != it_end; --it_tmp){ + traj_out_poses.push_back(*it_tmp); + } + + return true; + } + + //parameters + std::string p_target_frame_name_; + std::string p_source_frame_name_; + double p_trajectory_update_rate_; + double p_trajectory_publish_rate_; + + // Zero pose used for transformation to target_frame. + geometry_msgs::PoseStamped pose_source_; + + ros::ServiceServer trajectory_provider_service_; + ros::ServiceServer recovery_info_provider_service_; + + ros::Timer update_trajectory_timer_; + ros::Timer publish_trajectory_timer_; + + + //ros::Subscriber pose_update_sub_; + ros::Subscriber sys_cmd_sub_; + ros::Publisher trajectory_pub_; + + hector_nav_msgs::GetRobotTrajectory::Response trajectory_; + + tf::TransformListener tf_; + + ros::Time last_reset_time_; + ros::Time last_pose_save_time_; +}; + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "hector_trajectory_server"); + + PathContainer pc; + + ros::spin(); + + return 0; +} diff --git a/Localizations/Libraries/Ros/map_server/CHANGELOG.rst b/Localizations/Libraries/Ros/map_server/CHANGELOG.rst new file mode 100644 index 0000000..7c16d45 --- /dev/null +++ b/Localizations/Libraries/Ros/map_server/CHANGELOG.rst @@ -0,0 +1,212 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package map_server +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +1.17.3 (2023-01-10) +------------------- +* [ROS-O] various patches (`#1225 `_) + * do not specify obsolete c++11 standard + this breaks with current versions of log4cxx. + * update pluginlib include paths + the non-hpp headers have been deprecated since kinetic + * use lambdas in favor of boost::bind + Using boost's _1 as a global system is deprecated since C++11. + The ROS packages in Debian removed the implicit support for the global symbols, + so this code fails to compile there without the patch. +* Contributors: Michael Görner + +1.17.2 (2022-06-20) +------------------- +* Change_map service to map_server [Rebase/Noetic] (`#1029 `_) + * Refactored map loading from constructor to three methods + * Added change_map service using LoadMap.srv +* map_server: Initialise a NodeHandle in main. (`#1122 `_) +* Add debug output regarding waiting for time (`#1078 `_) + Added debug messages suggested in https://github.com/ros-planning/navigation/issues/1074#issuecomment-751557177. Makes it easier to discover if use_sim_time is true but no clock server is running +* crop_map: Fix extra pixel origin shift up every cropping (`#1064 `_) (`#1067 `_) + Co-authored-by: Pavlo Kolomiiets +* (map_server) add rtest dependency to tests (`#1061 `_) +* [noetic] MapServer variable cleanup: Precursor for `#1029 `_ (`#1043 `_) +* Contributors: Christian Fritz, David V. Lu!!, Matthijs van der Burgh, Nikos Koukis, Pavlo Kolomiiets + +1.17.1 (2020-08-27) +------------------- +* Initial map_server map_mode tests (`#1006 `_) +* [noetic] Adding CMake way to find yaml-cpp (`#998 `_) +* Contributors: David V. Lu!!, Sean Yen + +1.17.0 (2020-04-02) +------------------- +* Merge pull request `#982 `_ from ros-planning/noetic_prep + Noetic Migration +* increase required cmake version +* Contributors: Michael Ferguson + +1.16.6 (2020-03-18) +------------------- + +1.16.5 (2020-03-15) +------------------- +* [melodic] updated install for better portability. (`#973 `_) +* Contributors: Sean Yen + +1.16.4 (2020-03-04) +------------------- + +1.16.3 (2019-11-15) +------------------- +* Merge branch 'melodic-devel' into layer_clear_area-melodic +* Merge pull request `#850 `_ from seanyen/map_server_windows_fix + [Windows][melodic] map_server Windows build bring up +* map_server Windows build bring up + * Fix install location for Windows build. (On Windows build, shared library uses RUNTIME location, but not LIBRARY) + * Use boost::filesystem::path to handle path logic and remove the libgen.h dependency for better cross platform. + * Fix gtest hard-coded and add YAML library dir in CMakeList.txt. +* Contributors: Michael Ferguson, Sean Yen, Steven Macenski + +1.16.2 (2018-07-31) +------------------- + +1.16.1 (2018-07-28) +------------------- + +1.16.0 (2018-07-25) +------------------- +* Merge pull request `#735 `_ from ros-planning/melodic_708 + Allow specification of free/occupied thresholds for map_saver (`#708 `_) +* Allow specification of free/occupied thresholds for map_saver (`#708 `_) + * add occupied threshold command line parameter to map_saver (--occ) + * add free threshold command line parameter to map_saver (--free) +* Merge pull request `#704 `_ from DLu/fix573_lunar + Map server wait for a valid time fix [lunar] +* Map server wait for a valid time, fix `#573 `_ (`#700 `_) + When launching the map_server with Gazebo, the current time is picked + before the simulation is started and then has a value of 0. + Later when querying the stamp of the map, a value of has a special + signification on tf transform for example. +* Contributors: Michael Ferguson, Romain Reignier, ipa-fez + +1.15.2 (2018-03-22) +------------------- +* Merge pull request `#673 `_ from ros-planning/email_update_lunar + update maintainer email (lunar) +* Merge pull request `#649 `_ from aaronhoy/lunar_add_ahoy + Add myself as a maintainer. +* Rebase PRs from Indigo/Kinetic (`#637 `_) + * Respect planner_frequency intended behavior (`#622 `_) + * Only do a getRobotPose when no start pose is given (`#628 `_) + Omit the unnecessary call to getRobotPose when the start pose was + already given, so that move_base can also generate a path in + situations where getRobotPose would fail. + This is actually to work around an issue of getRobotPose randomly + failing. + * Update gradient_path.cpp (`#576 `_) + * Update gradient_path.cpp + * Update navfn.cpp + * update to use non deprecated pluginlib macro (`#630 `_) + * update to use non deprecated pluginlib macro + * multiline version as well + * Print SDL error on IMG_Load failure in server_map (`#631 `_) +* Use occupancy values when saving a map (`#613 `_) +* Closes `#625 `_ (`#627 `_) +* Contributors: Aaron Hoy, David V. Lu!!, Hunter Allen, Michael Ferguson + +1.15.1 (2017-08-14) +------------------- +* remove offending library export (fixes `#612 `_) +* Contributors: Michael Ferguson + +1.15.0 (2017-08-07) +------------------- +* Fix compiler warning for GCC 8. +* convert packages to format2 +* Merge pull request `#596 `_ from ros-planning/lunar_548 +* refactor to not use tf version 1 (`#561 `_) +* Fix CMakeLists + package.xmls (`#548 `_) +* Merge pull request `#560 `_ from wjwwood/map_server_fixup_cmake +* update to support Python 2 and 3 (`#559 `_) +* remove duplicate and unreferenced file (`#558 `_) +* remove trailing whitespace from map_server package (`#557 `_) +* fix cmake use of yaml-cpp and sdl / sdl-image +* Fix CMake warnings +* Contributors: Hunter L. Allen, Martin Günther, Michael Ferguson, Mikael Arguedas, Vincent Rabaud, William Woodall + +1.14.0 (2016-05-20) +------------------- +* Corrections to alpha channel detection and usage. + Changing to actually detect whether the image has an alpha channel instead of + inferring based on the number of channels. + Also reverting to legacy behavior of trinary mode overriding alpha removal. + This will cause the alpha channel to be averaged in with the others in trinary + mode, which is the current behavior before this PR. +* Removing some trailing whitespace. +* Use enum to control map interpretation +* Contributors: Aaron Hoy, David Lu + +1.13.1 (2015-10-29) +------------------- + +1.13.0 (2015-03-17) +------------------- +* rename image_loader library, fixes `#208 `_ +* Contributors: Michael Ferguson + +1.12.0 (2015-02-04) +------------------- +* update maintainer email +* Contributors: Michael Ferguson + +1.11.15 (2015-02-03) +-------------------- + +1.11.14 (2014-12-05) +-------------------- +* prevent inf loop +* Contributors: Jeremie Deray + +1.11.13 (2014-10-02) +-------------------- + +1.11.12 (2014-10-01) +-------------------- +* map_server: [style] alphabetize dependencies +* map_server: remove vestigial export line + the removed line does not do anything in catkin +* Contributors: William Woodall + +1.11.11 (2014-07-23) +-------------------- + +1.11.10 (2014-06-25) +-------------------- + +1.11.9 (2014-06-10) +------------------- + +1.11.8 (2014-05-21) +------------------- +* fix build, was broken by `#175 `_ +* Contributors: Michael Ferguson + +1.11.7 (2014-05-21) +------------------- +* make rostest in CMakeLists optional +* Contributors: Lukas Bulwahn + +1.11.5 (2014-01-30) +------------------- +* install crop map +* removing .py from executable script +* Map Server can serve maps with non-lethal values +* Added support for YAML-CPP 0.5+. + The new yaml-cpp API removes the "node >> outputvar;" operator, and + it has a new way of loading documents. There's no version hint in the + library's headers, so I'm getting the version number from pkg-config. +* check for CATKIN_ENABLE_TESTING +* Change maintainer from Hersh to Lu + +1.11.4 (2013-09-27) +------------------- +* prefix utest target to not collide with other targets +* Package URL Updates +* unique target names to avoid conflicts (e.g. with map-store) diff --git a/Localizations/Libraries/Ros/map_server/CMakeLists.txt b/Localizations/Libraries/Ros/map_server/CMakeLists.txt new file mode 100644 index 0000000..f5c7ceb --- /dev/null +++ b/Localizations/Libraries/Ros/map_server/CMakeLists.txt @@ -0,0 +1,153 @@ +cmake_minimum_required(VERSION 3.0.2) +project(map_server) + +find_package(catkin REQUIRED + COMPONENTS + roscpp + nav_msgs + tf2 + ) + +find_package(Bullet REQUIRED) +find_package(SDL REQUIRED) +find_package(SDL_image REQUIRED) +find_package(Boost REQUIRED COMPONENTS filesystem) + +find_package(PkgConfig REQUIRED) +pkg_check_modules(YAMLCPP yaml-cpp QUIET) +if(NOT YAMLCPP_FOUND) + find_package(yaml-cpp 0.6 REQUIRED) + set(YAMLCPP_INCLUDE_DIRS ${YAML_CPP_INCLUDE_DIR}) + set(YAMLCPP_LIBRARIES ${YAML_CPP_LIBRARIES}) + add_definitions(-DHAVE_YAMLCPP_GT_0_5_0) +else() + if(YAMLCPP_VERSION VERSION_GREATER "0.5.0") + add_definitions(-DHAVE_YAMLCPP_GT_0_5_0) + endif() + link_directories(${YAMLCPP_LIBRARY_DIRS}) +endif() + +catkin_package( + INCLUDE_DIRS include + LIBRARIES map_server_lib map_server_image_loader map_generator_lib + CATKIN_DEPENDS roscpp nav_msgs tf2 +) + +include_directories( + include + ${BULLET_INCLUDE_DIRS} + ${catkin_INCLUDE_DIRS} + ${SDL_INCLUDE_DIR} + ${SDL_IMAGE_INCLUDE_DIRS} + ${YAMLCPP_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} +) + +add_library(map_server_image_loader + src/image_loader.cpp + src/image_16bit_loader.cpp + src/image_32bit_loader.cpp + ) +add_dependencies(map_server_image_loader ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(map_server_image_loader + ${BULLET_LIBRARIES} + ${catkin_LIBRARIES} + ${SDL_LIBRARY} + ${SDL_IMAGE_LIBRARIES} + ${YAMLCPP_LIBRARIES} +) + +add_library(map_server_lib src/map_server.cpp) +add_dependencies(map_server_lib map_server_image_loader ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(map_server_lib + map_server_image_loader + ${BULLET_LIBRARIES} + ${catkin_LIBRARIES} + ${SDL_LIBRARY} + ${SDL_IMAGE_LIBRARIES} + ${YAMLCPP_LIBRARIES} +) + +add_library (map_generator_lib src/map_generator.cpp) +add_dependencies(map_generator_lib ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(map_generator_lib + ${BULLET_LIBRARIES} + ${catkin_LIBRARIES} + ${SDL_LIBRARY} + ${SDL_IMAGE_LIBRARIES} + ${YAMLCPP_LIBRARIES} +) + +add_executable(map_server src/main.cpp) +add_dependencies(map_server ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(map_server + map_server_lib + ${YAMLCPP_LIBRARIES} + ${catkin_LIBRARIES} +) + +add_executable(map_server-map_saver src/map_saver.cpp) +add_dependencies(map_server-map_saver map_generator_lib ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +set_target_properties(map_server-map_saver PROPERTIES OUTPUT_NAME map_saver) +target_link_libraries(map_server-map_saver + map_generator_lib + ${catkin_LIBRARIES} +) + +# copy test data to same place as tests are run +function(copy_test_data) + cmake_parse_arguments(PROJECT "" "" "FILES" ${ARGN}) + foreach(datafile ${PROJECT_FILES}) + file(COPY ${datafile} DESTINATION ${PROJECT_BINARY_DIR}/test) + endforeach() +endfunction() + +## Tests +if(CATKIN_ENABLE_TESTING) + copy_test_data( FILES + test/testmap.bmp + test/testmap.png + test/spectrum.png ) + catkin_add_gtest(${PROJECT_NAME}_utest test/utest.cpp test/test_constants.cpp) + target_link_libraries(${PROJECT_NAME}_utest + map_server_lib + ${SDL_LIBRARY} + ${SDL_IMAGE_LIBRARIES} + ) + + find_package(roslib REQUIRED) + include_directories(${roslib_INCLUDE_DIRS}) + add_executable(rtest test/rtest.cpp test/test_constants.cpp) + add_dependencies(rtest ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + add_dependencies(tests rtest) + target_link_libraries( rtest + ${GTEST_LIBRARIES} + ${catkin_LIBRARIES} + ${roslib_LIBRARIES} + ) + + # This has to be done after we've already built targets, or catkin variables get borked + find_package(rostest REQUIRED) + add_rostest(test/rtest.xml) +endif() + +## Install executables and/or libraries +install(TARGETS map_server-map_saver map_server + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) + +install(TARGETS map_server_image_loader map_server_lib map_generator_lib + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) + +## Install project namespaced headers +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE) + +## Install excutable python script +install( + PROGRAMS + scripts/crop_map + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) diff --git a/Localizations/Libraries/Ros/map_server/include/map_server/image_loader.h b/Localizations/Libraries/Ros/map_server/include/map_server/image_loader.h new file mode 100644 index 0000000..ea226d3 --- /dev/null +++ b/Localizations/Libraries/Ros/map_server/include/map_server/image_loader.h @@ -0,0 +1,115 @@ +/* + * Copyright (c) 2008, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#ifndef MAP_SERVER_MAP_SERVER_H +#define MAP_SERVER_MAP_SERVER_H + +/* + * Author: Brian Gerkey + */ + +#include "nav_msgs/GetMap.h" + +/** Map mode + * Default: TRINARY - + * value >= occ_th - Occupied (100) + * value <= free_th - Free (0) + * otherwise - Unknown + * SCALE - + * alpha < 1.0 - Unknown + * value >= occ_th - Occupied (100) + * value <= free_th - Free (0) + * otherwise - f( (free_th, occ_th) ) = (0, 100) + * (linearly map in between values to (0,100) + * RAW - + * value = value + */ +enum MapMode {TRINARY, SCALE, RAW}; + +namespace map_server +{ + +/** Read the image from file and fill out the resp object, for later + * use when our services are requested. + * + * @param resp The map wil be written into here + * @param fname The image file to read from + * @param res The resolution of the map (gets stored in resp) + * @param negate If true, then whiter pixels are occupied, and blacker + * pixels are free + * @param occ_th Threshold above which pixels are occupied + * @param free_th Threshold below which pixels are free + * @param origin Triple specifying 2-D pose of lower-left corner of image + * @param mode Map mode + * @throws std::runtime_error If the image file can't be loaded + * */ +void loadMapFromFile(nav_msgs::GetMap::Response* resp, + const char* fname, double res, bool negate, + double occ_th, double free_th, double* origin, + MapMode mode=TRINARY); + +/** Read the image from file and fill out the resp object, for later + * use when our services are requested. + * + * @param resp The map wil be written into here + * @param fname The image file to read from + * @param res The resolution of the map (gets stored in resp) + * @param negate If true, then whiter pixels are occupied, and blacker + * pixels are free + * @param occ_th Threshold above which pixels are occupied + * @param free_th Threshold below which pixels are free + * @param origin Triple specifying 2-D pose of lower-left corner of image + * @param mode Map mode + * @throws std::runtime_error If the image file can't be loaded + * */ +void loadMapFromFile16Bit(nav_msgs::GetMap::Response* resp, + const char* fname, double res, bool negate, + double occ_th, double free_th, double* origin, + MapMode mode=TRINARY); + +/** Read the image from file and fill out the resp object, for later + * use when our services are requested. + * + * @param resp The map wil be written into here + * @param fname The image file to read from + * @param res The resolution of the map (gets stored in resp) + * @param negate If true, then whiter pixels are occupied, and blacker + * pixels are free + * @param occ_th Threshold above which pixels are occupied + * @param free_th Threshold below which pixels are free + * @param origin Triple specifying 2-D pose of lower-left corner of image + * @param mode Map mode + * @throws std::runtime_error If the image file can't be loaded + * */ +void loadMapFromFile32Bit(nav_msgs::GetMap::Response* resp, + const char* fname, double res, bool negate, + double occ_th, double free_th, double* origin, + MapMode mode=TRINARY); +} + +#endif diff --git a/Localizations/Libraries/Ros/map_server/include/map_server/map_generator.h b/Localizations/Libraries/Ros/map_server/include/map_server/map_generator.h new file mode 100644 index 0000000..befd4b8 --- /dev/null +++ b/Localizations/Libraries/Ros/map_server/include/map_server/map_generator.h @@ -0,0 +1,43 @@ +#ifndef MAP_GENERATOR_H_INCLUDED_ +#define MAP_GENERATOR_H_INCLUDED_ + +#include +#include "ros/ros.h" +#include "ros/console.h" +#include "nav_msgs/GetMap.h" +#include "tf2/LinearMath/Matrix3x3.h" +#include "geometry_msgs/Quaternion.h" + + +namespace map_server +{ + /** + * @brief Map generation node. + */ + class MapGenerator + { + public: + MapGenerator(ros::NodeHandle nh, const std::string& mapname, const std::string map_topic, + int threshold_occupied = 65, int threshold_free = 25); + + MapGenerator(const std::string& mapname, const std::string map_topic, + int threshold_occupied = 65, int threshold_free = 25); + virtual ~MapGenerator() {} + virtual bool waitingGenerator(double tolerance = 2.0); + bool isMapSaved() {return saved_map_;} + + private: + + void mapCallback(const nav_msgs::OccupancyGridConstPtr& map); + + std::string mapname_; + ros::Subscriber map_sub_; + bool saved_map_; + int threshold_occupied_; + int threshold_free_; + }; + + +} // namespace map_server + +#endif // MAP_GENERATOR_H_INCLUDED_ \ No newline at end of file diff --git a/Localizations/Libraries/Ros/map_server/include/map_server/map_server.h b/Localizations/Libraries/Ros/map_server/include/map_server/map_server.h new file mode 100644 index 0000000..796f4fd --- /dev/null +++ b/Localizations/Libraries/Ros/map_server/include/map_server/map_server.h @@ -0,0 +1,123 @@ + +/* + * Copyright (c) 2008, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + + /* Author: Brian Gerkey */ + +#ifndef __MAP_SERVER_H_INCLUDED_ +#define __MAP_SERVER_H_INCLUDED_ + +#include +#include +#include +#include + +#include "ros/ros.h" +#include "ros/console.h" +#include "map_server/image_loader.h" +#include "nav_msgs/MapMetaData.h" +#include "nav_msgs/LoadMap.h" +#include "yaml-cpp/yaml.h" + +#ifdef HAVE_YAMLCPP_GT_0_5_0 +// The >> operator disappeared in yaml-cpp 0.5, so this function is +// added to provide support for code written under the yaml-cpp 0.3 API. +template +void operator>>(const YAML::Node& node, T& i) +{ + i = node.as(); +} +#endif +#define MAP_IDX(sx, i, j) ((sx) * (j) + (i)) + +namespace map_server +{ + + class MapServer + { + public: + /** + * @brief Trivial constructor + */ + MapServer(const std::string& fname, double res); + MapServer(ros::NodeHandle nh, std::string name_sp, const std::string& fname, double res); + + /** + * @brief Load a map given a path to a yaml file + * @param path_to_yaml The path to file yaml + * @return True/False If load map succesed or not done + */ + bool loadMapFromYaml(std::string path_to_yaml); + + private: + ros::NodeHandle nh_; + ros::Publisher map_pub_; + ros::Publisher map_convert_; + ros::Publisher metadata_pub_; + ros::ServiceServer get_map_service_; + ros::ServiceServer change_map_srv_; + bool deprecated_; + std::string frame_id_; + int type_; + + void initialize(ros::NodeHandle nh, const std::string& fname, double res); + + /** + * @brief Callback invoked when someone requests our service + */ + bool mapCallback(nav_msgs::GetMap::Request& req, nav_msgs::GetMap::Response& res); + + /** + * @brief Callback invoked when someone requests to change the map + */ + bool changeMapCallback(nav_msgs::LoadMap::Request& request, nav_msgs::LoadMap::Response& response); + + /** + * @brief Load a map given all the values needed to understand it + */ + bool loadMapFromValues(std::string map_file_name, double resolution, + int negate, double occ_th, double free_th, double origin[3], MapMode mode); + + + /** + * @brief Load a map using the deprecated method + */ + bool loadMapFromParams(std::string map_file_name, double resolution); + + /** + * @brief The map data is cached here, to be sent out to service callers + */ + nav_msgs::MapMetaData meta_data_message_; + nav_msgs::GetMap::Response map_resp_; + + }; // class MapServer + +} // namespace map_sever + +#endif // __MAP_SERVER_H_INCLUDED_ \ No newline at end of file diff --git a/Localizations/Libraries/Ros/map_server/launch/test.launch b/Localizations/Libraries/Ros/map_server/launch/test.launch new file mode 100644 index 0000000..0d1c2dc --- /dev/null +++ b/Localizations/Libraries/Ros/map_server/launch/test.launch @@ -0,0 +1,4 @@ + + + + \ No newline at end of file diff --git a/Localizations/Libraries/Ros/map_server/maps/Direction_Zones_0.png b/Localizations/Libraries/Ros/map_server/maps/Direction_Zones_0.png new file mode 100644 index 0000000..5a98027 Binary files /dev/null and b/Localizations/Libraries/Ros/map_server/maps/Direction_Zones_0.png differ diff --git a/Localizations/Libraries/Ros/map_server/maps/Direction_Zones_180.png b/Localizations/Libraries/Ros/map_server/maps/Direction_Zones_180.png new file mode 100644 index 0000000..354d4c8 Binary files /dev/null and b/Localizations/Libraries/Ros/map_server/maps/Direction_Zones_180.png differ diff --git a/Localizations/Libraries/Ros/map_server/maps/Direction_Zones_270.png b/Localizations/Libraries/Ros/map_server/maps/Direction_Zones_270.png new file mode 100644 index 0000000..27a4eac Binary files /dev/null and b/Localizations/Libraries/Ros/map_server/maps/Direction_Zones_270.png differ diff --git a/Localizations/Libraries/Ros/map_server/maps/Direction_Zones_90.png b/Localizations/Libraries/Ros/map_server/maps/Direction_Zones_90.png new file mode 100644 index 0000000..a4337d5 Binary files /dev/null and b/Localizations/Libraries/Ros/map_server/maps/Direction_Zones_90.png differ diff --git a/Localizations/Libraries/Ros/map_server/maps/maze.png b/Localizations/Libraries/Ros/map_server/maps/maze.png new file mode 100644 index 0000000..41d6fa1 Binary files /dev/null and b/Localizations/Libraries/Ros/map_server/maps/maze.png differ diff --git a/Localizations/Libraries/Ros/map_server/maps/maze.yaml b/Localizations/Libraries/Ros/map_server/maps/maze.yaml new file mode 100644 index 0000000..79220e7 --- /dev/null +++ b/Localizations/Libraries/Ros/map_server/maps/maze.yaml @@ -0,0 +1,6 @@ +image: maze.png +resolution: 0.05 +origin: [0.0, 0.0, 0.0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 diff --git a/Localizations/Libraries/Ros/map_server/maps/maze_virtual_walls.png b/Localizations/Libraries/Ros/map_server/maps/maze_virtual_walls.png new file mode 100644 index 0000000..e5dd8bb Binary files /dev/null and b/Localizations/Libraries/Ros/map_server/maps/maze_virtual_walls.png differ diff --git a/Localizations/Libraries/Ros/map_server/maps/maze_virtual_walls.yaml b/Localizations/Libraries/Ros/map_server/maps/maze_virtual_walls.yaml new file mode 100644 index 0000000..89320ac --- /dev/null +++ b/Localizations/Libraries/Ros/map_server/maps/maze_virtual_walls.yaml @@ -0,0 +1,6 @@ +image: maze_virtual_walls.png +resolution: 0.05 +origin: [0.0, 0.0, 0.0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 diff --git a/Localizations/Libraries/Ros/map_server/package.xml b/Localizations/Libraries/Ros/map_server/package.xml new file mode 100644 index 0000000..640cc76 --- /dev/null +++ b/Localizations/Libraries/Ros/map_server/package.xml @@ -0,0 +1,33 @@ + + + + map_server + 1.17.3 + + + map_server provides the map_server ROS Node, which offers map data as a ROS Service. It also provides the map_saver command-line utility, which allows dynamically generated maps to be saved to file. + + + Brian Gerkey, Tony Pratkanis + contradict@gmail.com + David V. Lu!! + Michael Ferguson + Aaron Hoy + http://wiki.ros.org/map_server + BSD + + catkin + + bullet + nav_msgs + roscpp + sdl + sdl-image + tf2 + yaml-cpp + + roslib + rospy + rostest + rosunit + diff --git a/Localizations/Libraries/Ros/map_server/scripts/crop_map b/Localizations/Libraries/Ros/map_server/scripts/crop_map new file mode 100644 index 0000000..78b2c14 --- /dev/null +++ b/Localizations/Libraries/Ros/map_server/scripts/crop_map @@ -0,0 +1,109 @@ +#!/usr/bin/env python +# Software License Agreement (BSD License) +# +# Copyright (c) 2008, Willow Garage, Inc. +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of the Willow Garage nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# + +from __future__ import print_function + +import sys +import yaml +from PIL import Image +import math + +def find_bounds(map_image): + x_min = map_image.size[0] + x_end = 0 + y_min = map_image.size[1] + y_end = 0 + pix = map_image.load() + for x in range(map_image.size[0]): + for y in range(map_image.size[1]): + val = pix[x, y] + if val != 205: # not unknown + x_min = min(x, x_min) + x_end = max(x, x_end) + y_min = min(y, y_min) + y_end = max(y, y_end) + return x_min, x_end, y_min, y_end + +def computed_cropped_origin(map_image, bounds, resolution, origin): + """ Compute the image for the cropped map when map_image is cropped by bounds and had origin before. """ + ox = origin[0] + oy = origin[1] + oth = origin[2] + + # First figure out the delta we have to translate from the lower left corner (which is the origin) + # in the image system + dx = bounds[0] * resolution + dy = (map_image.size[1] - bounds[3] - 1) * resolution + + # Next rotate this by the theta and add to the old origin + + new_ox = ox + dx * math.cos(oth) - dy * math.sin(oth) + new_oy = oy + dx * math.sin(oth) + dy * math.cos(oth) + + return [new_ox, new_oy, oth] + +if __name__ == "__main__": + if len(sys.argv) < 2: + print("Usage: %s map.yaml [cropped.yaml]" % sys.argv[0], file=sys.stderr) + sys.exit(1) + + with open(sys.argv[1]) as f: + map_data = yaml.safe_load(f) + + if len(sys.argv) > 2: + crop_name = sys.argv[2] + if crop_name.endswith(".yaml"): + crop_name = crop_name[:-5] + crop_yaml = crop_name + ".yaml" + crop_image = crop_name + ".pgm" + else: + crop_yaml = "cropped.yaml" + crop_image = "cropped.pgm" + + map_image_file = map_data["image"] + resolution = map_data["resolution"] + origin = map_data["origin"] + map_image = Image.open(map_image_file) + + bounds = find_bounds(map_image) + + # left, upper, right, lower + cropped_image = map_image.crop((bounds[0], bounds[2], bounds[1] + 1, bounds[3] + 1)) + + cropped_image.save(crop_image) + map_data["image"] = crop_image + map_data["origin"] = computed_cropped_origin(map_image, bounds, resolution, origin) + with open(crop_yaml, "w") as f: + yaml.dump(map_data, f) + diff --git a/Localizations/Libraries/Ros/map_server/src/image_16bit_loader.cpp b/Localizations/Libraries/Ros/map_server/src/image_16bit_loader.cpp new file mode 100644 index 0000000..8d531ac --- /dev/null +++ b/Localizations/Libraries/Ros/map_server/src/image_16bit_loader.cpp @@ -0,0 +1,133 @@ +/* + * Copyright (c) 2008, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/* + * This file contains helper functions for loading images as maps. + * + * Author: Brian Gerkey + */ + +#include +#include + +#include +#include + +// We use SDL_image to load the image from disk +#include + +// Use Bullet's Quaternion object to create one from Euler angles +#include + +#include "map_server/image_loader.h" +#include + +// compute linear index for given map coords +#define MAP_IDX(sx, i, j) ((sx) * (j) + (i)) + +namespace map_server +{ + +void loadMapFromFile16Bit(nav_msgs::GetMap::Response* resp, + const char* fname, double res, bool negate, + double occ_th, double free_th, double* origin, + MapMode mode) +{ + SDL_Surface* img; + + unsigned char* pixels; + unsigned char* p; + int rowstride, n_channels, avg_channels; + unsigned int i,j; + int k; + int alpha; + uint16_t color_avg; + + // Load the image using SDL. If we get NULL back, the image load failed. + if(!(img = IMG_Load(fname))) + { + std::string errmsg = std::string("failed to open image file \"") + + std::string(fname) + std::string("\": ") + IMG_GetError(); + throw std::runtime_error(errmsg); + } + + // Copy the image data into the map structure + resp->map.info.width = img->w*2; + resp->map.info.height = img->h; + resp->map.info.resolution = res; + resp->map.info.origin.position.x = *(origin); + resp->map.info.origin.position.y = *(origin+1); + resp->map.info.origin.position.z = 0.0; + btQuaternion q; + // setEulerZYX(yaw, pitch, roll) + q.setEulerZYX(*(origin+2), 0, 0); + resp->map.info.origin.orientation.x = q.x(); + resp->map.info.origin.orientation.y = q.y(); + resp->map.info.origin.orientation.z = q.z(); + resp->map.info.origin.orientation.w = q.w(); + + // Allocate space to hold the data + resp->map.data.resize(resp->map.info.width * resp->map.info.height); + + // Get values that we'll need to iterate through the pixels + rowstride = img->pitch; + n_channels = img->format->BytesPerPixel; + + // NOTE: Trinary mode still overrides here to preserve existing behavior. + // Alpha will be averaged in with color channels when using trinary mode. + if (mode==TRINARY || !img->format->Amask) + avg_channels = n_channels; + else + avg_channels = n_channels - 1; + // Copy pixel data into the map structure + pixels = (unsigned char*)(img->pixels); + for(j = 0; j < resp->map.info.height; j++) + { + for (i = 0; i < resp->map.info.width; i+=2) + { + // Compute mean of RGB for this pixel + p = pixels + j*rowstride + i/2*n_channels; + color_avg = (*(p) << 16) + (*(p+1) << 8) + (*(p+2)); + if (n_channels == 1) + alpha = 1; + else + alpha = *(p+n_channels-1); + + if(negate) + color_avg = 65535 - color_avg; + + resp->map.data[MAP_IDX(resp->map.info.width,i,resp->map.info.height - j - 1)] = (unsigned char)(color_avg & 0x00FF); + resp->map.data[MAP_IDX(resp->map.info.width,i+1,resp->map.info.height - j - 1)] = (unsigned char)((color_avg & 0xFF00) >> 8); + } + } + + SDL_FreeSurface(img); +} + +} diff --git a/Localizations/Libraries/Ros/map_server/src/image_32bit_loader.cpp b/Localizations/Libraries/Ros/map_server/src/image_32bit_loader.cpp new file mode 100644 index 0000000..1fccdf2 --- /dev/null +++ b/Localizations/Libraries/Ros/map_server/src/image_32bit_loader.cpp @@ -0,0 +1,134 @@ +/* + * Copyright (c) 2008, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/* + * This file contains helper functions for loading images as maps. + * + * Author: Brian Gerkey + */ + +#include +#include + +#include +#include + +// We use SDL_image to load the image from disk +#include + +// Use Bullet's Quaternion object to create one from Euler angles +#include + +#include "map_server/image_loader.h" +#include + +// compute linear index for given map coords +#define MAP_IDX(sx, i, j) ((sx) * (j) + (i)) + +namespace map_server +{ + void loadMapFromFile32Bit(nav_msgs::GetMap::Response *resp, + const char *fname, double res, bool negate, + double occ_th, double free_th, double *origin, + MapMode mode) + { + SDL_Surface *img; + + unsigned char *pixels; + unsigned char *p; + int rowstride, n_channels, avg_channels; + unsigned int i, j; + int k; + int alpha; + uint32_t color_avg; + + // Load the image using SDL. If we get NULL back, the image load failed. + if (!(img = IMG_Load(fname))) + { + std::string errmsg = std::string("failed to open image file \"") + + std::string(fname) + std::string("\": ") + IMG_GetError(); + throw std::runtime_error(errmsg); + } + + // Copy the image data into the map structure + resp->map.info.width = img->w * 2; + resp->map.info.height = img->h * 2; + resp->map.info.resolution = res; + resp->map.info.origin.position.x = *(origin); + resp->map.info.origin.position.y = *(origin + 1); + resp->map.info.origin.position.z = 0.0; + btQuaternion q; + // setEulerZYX(yaw, pitch, roll) + q.setEulerZYX(*(origin + 2), 0, 0); + resp->map.info.origin.orientation.x = q.x(); + resp->map.info.origin.orientation.y = q.y(); + resp->map.info.origin.orientation.z = q.z(); + resp->map.info.origin.orientation.w = q.w(); + + // Allocate space to hold the data + resp->map.data.resize(resp->map.info.width * resp->map.info.height); + + // Get values that we'll need to iterate through the pixels + rowstride = img->pitch; + n_channels = img->format->BytesPerPixel; + + // NOTE: Trinary mode still overrides here to preserve existing behavior. + // Alpha will be averaged in with color channels when using trinary mode. + if (mode == TRINARY || !img->format->Amask) + avg_channels = n_channels; + else + avg_channels = n_channels - 1; + // Copy pixel data into the map structure + pixels = (unsigned char *)(img->pixels); + for (j = 0; j < resp->map.info.height; j += 2) + { + for (i = 0; i < resp->map.info.width; i += 2) + { + // Compute mean of RGB for this pixel + p = pixels + j / 2 * rowstride + i / 2 * n_channels; + color_avg = (*(p) << 16) + (*(p + 1) << 8) + (*(p + 2)); + if (n_channels == 1) + alpha = 1; + else + alpha = *(p + n_channels - 1); + + if (negate) + color_avg = 16777215 - color_avg; + + resp->map.data[MAP_IDX(resp->map.info.width, i, resp->map.info.height - j - 1)] = (color_avg >> 16u) & 0xFF; + resp->map.data[MAP_IDX(resp->map.info.width, i + 1, resp->map.info.height - j - 1)] = (color_avg >> 24u) & 0xFF; + resp->map.data[MAP_IDX(resp->map.info.width, i, resp->map.info.height - j - 2)] = (color_avg >> 0u) & 0xFF; + resp->map.data[MAP_IDX(resp->map.info.width, i + 1, resp->map.info.height - j - 2)] = (color_avg >> 8u) & 0xFF; + } + } + + SDL_FreeSurface(img); + } + +} diff --git a/Localizations/Libraries/Ros/map_server/src/image_loader.cpp b/Localizations/Libraries/Ros/map_server/src/image_loader.cpp new file mode 100644 index 0000000..02419bf --- /dev/null +++ b/Localizations/Libraries/Ros/map_server/src/image_loader.cpp @@ -0,0 +1,167 @@ +/* + * Copyright (c) 2008, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/* + * This file contains helper functions for loading images as maps. + * + * Author: Brian Gerkey + */ + +#include +#include + +#include +#include + +// We use SDL_image to load the image from disk +#include + +// Use Bullet's Quaternion object to create one from Euler angles +#include + +#include "map_server/image_loader.h" +#include + +// compute linear index for given map coords +#define MAP_IDX(sx, i, j) ((sx) * (j) + (i)) + +namespace map_server +{ + +void +loadMapFromFile(nav_msgs::GetMap::Response* resp, + const char* fname, double res, bool negate, + double occ_th, double free_th, double* origin, + MapMode mode) +{ + SDL_Surface* img; + + unsigned char* pixels; + unsigned char* p; + unsigned char value; + int rowstride, n_channels, avg_channels; + unsigned int i,j; + int k; + double occ; + int alpha; + int color_sum; + double color_avg; + + // Load the image using SDL. If we get NULL back, the image load failed. + if(!(img = IMG_Load(fname))) + { + std::string errmsg = std::string("failed to open image file \"") + + std::string(fname) + std::string("\": ") + IMG_GetError(); + throw std::runtime_error(errmsg); + } + + // Copy the image data into the map structure + resp->map.info.width = img->w; + resp->map.info.height = img->h; + resp->map.info.resolution = res; + resp->map.info.origin.position.x = *(origin); + resp->map.info.origin.position.y = *(origin+1); + resp->map.info.origin.position.z = 0.0; + btQuaternion q; + // setEulerZYX(yaw, pitch, roll) + q.setEulerZYX(*(origin+2), 0, 0); + resp->map.info.origin.orientation.x = q.x(); + resp->map.info.origin.orientation.y = q.y(); + resp->map.info.origin.orientation.z = q.z(); + resp->map.info.origin.orientation.w = q.w(); + + // Allocate space to hold the data + resp->map.data.resize(resp->map.info.width * resp->map.info.height); + + // Get values that we'll need to iterate through the pixels + rowstride = img->pitch; + n_channels = img->format->BytesPerPixel; + + // NOTE: Trinary mode still overrides here to preserve existing behavior. + // Alpha will be averaged in with color channels when using trinary mode. + if (mode==TRINARY || !img->format->Amask) + avg_channels = n_channels; + else + avg_channels = n_channels - 1; + // Copy pixel data into the map structure + pixels = (unsigned char*)(img->pixels); + for(j = 0; j < resp->map.info.height; j++) + { + for (i = 0; i < resp->map.info.width; i++) + { + // Compute mean of RGB for this pixel + p = pixels + j*rowstride + i*n_channels; + color_sum = 0; + int color = 0; + for(k=0;kmap.data[MAP_IDX(resp->map.info.width,i,resp->map.info.height - j - 1)] = value; + continue; + } + + + // If negate is true, we consider blacker pixels free, and whiter + // pixels occupied. Otherwise, it's vice versa. + occ = (255 - color_avg) / 255.0; + + // Apply thresholds to RGB means to determine occupancy values for + // map. Note that we invert the graphics-ordering of the pixels to + // produce a map with cell (0,0) in the lower-left corner. + if(occ > occ_th) + value = +100; + else if(occ < free_th) + value = 0; + else if(mode==TRINARY || alpha < 1.0) + value = -1; + else { + double ratio = (occ - free_th) / (occ_th - free_th); + value = 1 + 98 * ratio; + } + + resp->map.data[MAP_IDX(resp->map.info.width,i,resp->map.info.height - j - 1)] = value; + } + } + + SDL_FreeSurface(img); +} + +} diff --git a/Localizations/Libraries/Ros/map_server/src/main.cpp b/Localizations/Libraries/Ros/map_server/src/main.cpp new file mode 100644 index 0000000..0ecabe7 --- /dev/null +++ b/Localizations/Libraries/Ros/map_server/src/main.cpp @@ -0,0 +1,40 @@ + +#include "ros/ros.h" +#include "ros/console.h" +#include "map_server/map_server.h" + +#define USAGE "\nUSAGE: map_server \n" \ + " map.yaml: map description file\n" \ + "DEPRECATED USAGE: map_server \n" \ + " map: image file to load\n" \ + " resolution: map resolution [meters/pixel]" + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "map_server", ros::init_options::AnonymousName); + // ros::init(argc, argv, "map_server"); + ros::NodeHandle nh("~"); + if (argc != 3 && argc != 2) + { + ROS_ERROR("%s", USAGE); + exit(-1); + } + if (argc != 2) + { + ROS_WARN("Using deprecated map server interface. Please switch to new interface."); + } + std::string fname(argv[1]); + double res = (argc == 2) ? 0.0 : atof(argv[2]); + try + { + map_server::MapServer* map_sever_ptr = new map_server::MapServer(fname, res); + ros::spin(); + } + catch (std::runtime_error& e) + { + ROS_ERROR("map_server exception: %s", e.what()); + return -1; + } + + return 0; +} diff --git a/Localizations/Libraries/Ros/map_server/src/map_generator.cpp b/Localizations/Libraries/Ros/map_server/src/map_generator.cpp new file mode 100644 index 0000000..e57da27 --- /dev/null +++ b/Localizations/Libraries/Ros/map_server/src/map_generator.cpp @@ -0,0 +1,104 @@ +#include "map_server/map_generator.h" + +map_server::MapGenerator::MapGenerator(ros::NodeHandle nh, const std::string& mapname, const std::string map_topic, int threshold_occupied, int threshold_free) + : mapname_(mapname), saved_map_(false), threshold_occupied_(threshold_occupied), threshold_free_(threshold_free) +{ + ROS_INFO("Waiting for the map by topic %s", map_topic.c_str()); + map_sub_ = nh.subscribe(map_topic, 1, &MapGenerator::mapCallback, this); +} + +map_server::MapGenerator::MapGenerator(const std::string& mapname, const std::string map_topic, int threshold_occupied, int threshold_free) + : mapname_(mapname), saved_map_(false), threshold_occupied_(threshold_occupied), threshold_free_(threshold_free) +{ + ros::NodeHandle nh; + ROS_INFO("Waiting for the map"); + map_sub_ = nh.subscribe(map_topic, 1, &MapGenerator::mapCallback, this); +} + +void map_server::MapGenerator::mapCallback(const nav_msgs::OccupancyGridConstPtr& map) +{ + ROS_INFO("[MapGenerator] Received a %d X %d map @ %.3f m/pix", + map->info.width, + map->info.height, + map->info.resolution); + + + std::string mapdatafile = mapname_ + ".pgm"; + ROS_INFO("Writing map occupancy data to %s", mapdatafile.c_str()); + FILE* out = fopen(mapdatafile.c_str(), "w"); + if (!out) + { + ROS_ERROR("Couldn't save map file to %s", mapdatafile.c_str()); + return; + } + + fprintf(out, "P5\n# CREATOR: map_saver.cpp %.3f m/pix\n%d %d\n255\n", + map->info.resolution, map->info.width, map->info.height); + for (unsigned int y = 0; y < map->info.height; y++) { + for (unsigned int x = 0; x < map->info.width; x++) { + unsigned int i = x + (map->info.height - y - 1) * map->info.width; + if (map->data[i] >= 0 && map->data[i] <= threshold_free_) { // [0,free) + fputc(254, out); + } + else if (map->data[i] >= threshold_occupied_) { // (occ,255] + fputc(000, out); + } + else { //occ [0.25,0.65] + fputc(205, out); + } + } + } + + fclose(out); + + + std::string mapmetadatafile = mapname_ + ".yaml"; + ROS_INFO("Writing map occupancy data to %s", mapmetadatafile.c_str()); + FILE* yaml = fopen(mapmetadatafile.c_str(), "w"); + + + /* +resolution: 0.100000 +origin: [0.000000, 0.000000, 0.000000] +# +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 + + */ + + geometry_msgs::Quaternion orientation = map->info.origin.orientation; + tf2::Matrix3x3 mat(tf2::Quaternion( + orientation.x, + orientation.y, + orientation.z, + orientation.w + )); + double yaw, pitch, roll; + mat.getEulerYPR(yaw, pitch, roll); + + fprintf(yaml, "image: %s\nresolution: %f\norigin: [%f, %f, %f]\nnegate: 0\noccupied_thresh: 0.65\nfree_thresh: 0.196\n\n", + mapdatafile.c_str(), map->info.resolution, map->info.origin.position.x, map->info.origin.position.y, yaw); + + fclose(yaml); + + ROS_INFO("Done\n"); + saved_map_ = true; +} + +bool map_server::MapGenerator::waitingGenerator(double tolerance) +{ + bool result = true; + ros::Time start = ros::Time::now(); + while(!this->isMapSaved() && ros::ok()) + { + if((ros::Time::now() - start) > ros::Duration(tolerance)) + { + result = false; + break; + } + ros::spinOnce(); + } + if(this->isMapSaved()) result = true; + return result; +} \ No newline at end of file diff --git a/Localizations/Libraries/Ros/map_server/src/map_saver.cpp b/Localizations/Libraries/Ros/map_server/src/map_saver.cpp new file mode 100644 index 0000000..b20333f --- /dev/null +++ b/Localizations/Libraries/Ros/map_server/src/map_saver.cpp @@ -0,0 +1,119 @@ +/* + * map_saver + * Copyright (c) 2008, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include "map_server/map_generator.h" + +#define USAGE "Usage: \n" \ + " map_saver -h\n"\ + " map_saver [--occ ] [--free ] [-f ] [ROS remapping args]" + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "map_saver"); + std::string mapname = "map"; + int threshold_occupied = 65; + int threshold_free = 25; + + for(int i=1; i 100) + { + ROS_ERROR("threshold_occupied must be between 1 and 100"); + return 1; + } + + } + else + { + puts(USAGE); + return 1; + } + } + else if (!strcmp(argv[i], "--free")) + { + if (++i < argc) + { + threshold_free = std::atoi(argv[i]); + if (threshold_free < 0 || threshold_free > 100) + { + ROS_ERROR("threshold_free must be between 0 and 100"); + return 1; + } + + } + else + { + puts(USAGE); + return 1; + } + } + else + { + puts(USAGE); + return 1; + } + } + + if (threshold_occupied <= threshold_free) + { + ROS_ERROR("threshold_free must be smaller than threshold_occupied"); + return 1; + } + + map_server::MapGenerator mg(mapname, "map", threshold_occupied, threshold_free); + + while(!mg.isMapSaved() && ros::ok()) + ros::spinOnce(); + + return 0; +} + + diff --git a/Localizations/Libraries/Ros/map_server/src/map_server.cpp b/Localizations/Libraries/Ros/map_server/src/map_server.cpp new file mode 100644 index 0000000..b03d356 --- /dev/null +++ b/Localizations/Libraries/Ros/map_server/src/map_server.cpp @@ -0,0 +1,308 @@ + +#include "map_server/map_server.h" + + +map_server::MapServer::MapServer(const std::string& fname, double res) +{ + ros::NodeHandle private_nh("~"); + this->initialize(private_nh, fname, res); +} + +map_server::MapServer::MapServer(ros::NodeHandle nh, std::string name_sp, const std::string& fname, double res) +{ + nh_ = ros::NodeHandle(name_sp); + this->initialize(nh, fname, res); +} + +void map_server::MapServer::initialize(ros::NodeHandle nh, const std::string& fname, double res) +{ + std::string mapfname = ""; + double origin[3]; + int negate; + double occ_th, free_th; + MapMode mode = TRINARY; + nh.param("frame_id", frame_id_, std::string("map")); + nh.param("type", type_, 8); + + // When called this service returns a copy of the current map + get_map_service_ = nh_.advertiseService("static_map", &map_server::MapServer::mapCallback, this); + + // Change the currently published map + change_map_srv_ = nh_.advertiseService("change_map", &map_server::MapServer::changeMapCallback, this); + + // Latched publisher for metadata + metadata_pub_ = nh_.advertise("map_metadata", 1, true); + + // Latched publisher for data + map_pub_ = nh_.advertise("map", 1, true); + if (type_ != 8) map_convert_ = nh_.advertise("map_convert", 1, true); + + deprecated_ = (res != 0); + if (!deprecated_) + { + if (!loadMapFromYaml(fname)) + { + exit(-1); + } + } + else + { + if (!loadMapFromParams(fname, res)) + { + exit(-1); + } + } +} + +bool map_server::MapServer::mapCallback(nav_msgs::GetMap::Request& req, nav_msgs::GetMap::Response& res) +{ + // request is empty; we ignore it + + // = operator is overloaded to make deep copy (tricky!) + res = map_resp_; + ROS_INFO("Sending map"); + + return true; +} + +bool map_server::MapServer::changeMapCallback(nav_msgs::LoadMap::Request& request, + nav_msgs::LoadMap::Response& response) +{ + if (loadMapFromYaml(request.map_url)) + { + response.result = response.RESULT_SUCCESS; + ROS_INFO("Changed map to %s", request.map_url.c_str()); + } + else + { + response.result = response.RESULT_UNDEFINED_FAILURE; + } + return true; +} + + +bool map_server::MapServer::loadMapFromValues(std::string map_file_name, double resolution, + int negate, double occ_th, double free_th, + double origin[3], MapMode mode) +{ + ROS_INFO("Loading map from images \"%s\"", map_file_name.c_str()); + try + { + if (type_ == 32) + map_server::loadMapFromFile32Bit(&map_resp_, map_file_name.c_str(), + resolution, negate, occ_th, free_th, + origin, mode); + else if (type_ == 16) + map_server::loadMapFromFile16Bit(&map_resp_, map_file_name.c_str(), + resolution, negate, occ_th, free_th, + origin, mode); + else + map_server::loadMapFromFile(&map_resp_, map_file_name.c_str(), + resolution, negate, occ_th, free_th, + origin, mode); + } + catch (std::runtime_error& e) + { + ROS_ERROR("%s", e.what()); + return false; + } + + // To make sure get a consistent time in simulation + ros::Time::waitForValid(); + map_resp_.map.info.map_load_time = ros::Time::now(); + map_resp_.map.header.frame_id = frame_id_; + map_resp_.map.header.stamp = ros::Time::now(); + ROS_INFO("Read a %d X %d map @ %.3lf m/cell", + map_resp_.map.info.width, + map_resp_.map.info.height, + map_resp_.map.info.resolution); + meta_data_message_ = map_resp_.map.info; + + // Publish latched topics + metadata_pub_.publish(meta_data_message_); + map_pub_.publish(map_resp_.map); + + if (type_ == 16) + { + double occ_th = 0.65; + double free_th = 0.196; + nav_msgs::OccupancyGrid convert; + convert.header = map_resp_.map.header; + convert.info = map_resp_.map.info; + convert.info.width = map_resp_.map.info.width / 2; + convert.data.resize(convert.info.width * convert.info.height); + unsigned int color_sum, color; + double color_avg; + unsigned char value; + int size_y = map_resp_.map.info.height; + int size_x = map_resp_.map.info.width; + + for (int j = 0; j < map_resp_.map.info.height; j++) + { + for (int i = 0; i < map_resp_.map.info.width; i += 2) + { + unsigned char low = map_resp_.map.data[MAP_IDX(size_x, i, size_y - j - 1)]; + unsigned char high = map_resp_.map.data[MAP_IDX(size_x, i + 1, size_y - j - 1)]; + color = (int)((low & 0x00FF) | (high << 8)); + unsigned char red = (unsigned char)(color & 0x00FF0000) >> 16; + unsigned char green = (unsigned char)(color & 0x0000FF00) >> 8; + unsigned char blue = (unsigned char)(color & 0x000000FF); + color_sum = red + green + blue; + + color_avg = color_sum / 2.0; + + double occ = (255 - color_avg) / 255.0; + if (occ > occ_th) + value = +100; + else if (occ < free_th) + value = 0; + else + { + double ratio = (occ - free_th) / (occ_th - free_th); + value = 1 + 98 * ratio; + } + convert.data[MAP_IDX(size_x / 2, i / 2, size_y - j - 1)] = value; + } + } + map_convert_.publish(convert); + } + return true; +} + + +bool map_server::MapServer::loadMapFromParams(std::string map_file_name, double resolution) +{ + ros::NodeHandle private_nh("~"); + int negate; + double occ_th; + double free_th; + double origin[3]; + private_nh.param("negate", negate, 0); + private_nh.param("occupied_thresh", occ_th, 0.65); + private_nh.param("free_thresh", free_th, 0.196); + origin[0] = origin[1] = origin[2] = 0.0; + return loadMapFromValues(map_file_name, resolution, negate, occ_th, free_th, origin, TRINARY); +} + + +bool map_server::MapServer::loadMapFromYaml(std::string path_to_yaml) +{ + std::string mapfname; + MapMode mode; + double res; + int negate; + double occ_th; + double free_th; + double origin[3]; + std::ifstream fin(path_to_yaml.c_str()); + if (fin.fail()) + { + ROS_ERROR("Map_server could not open %s.", path_to_yaml.c_str()); + return false; + } +#ifdef HAVE_YAMLCPP_GT_0_5_0 + // The document loading process changed in yaml-cpp 0.5. + YAML::Node doc = YAML::Load(fin); +#else + YAML::Parser parser(fin); + YAML::Node doc; + parser.GetNextDocument(doc); +#endif + try + { + doc["resolution"] >> res; + } + catch (YAML::InvalidScalar&) + { + ROS_ERROR("The map does not contain a resolution tag or it is invalid."); + return false; + } + try + { + doc["negate"] >> negate; + } + catch (YAML::InvalidScalar&) + { + ROS_ERROR("The map does not contain a negate tag or it is invalid."); + return false; + } + try + { + doc["occupied_thresh"] >> occ_th; + } + catch (YAML::InvalidScalar&) + { + ROS_ERROR("The map does not contain an occupied_thresh tag or it is invalid."); + return false; + } + try + { + doc["free_thresh"] >> free_th; + } + catch (YAML::InvalidScalar&) + { + ROS_ERROR("The map does not contain a free_thresh tag or it is invalid."); + return false; + } + try + { + std::string modeS = ""; + doc["mode"] >> modeS; + + if (modeS == "trinary") + mode = TRINARY; + else if (modeS == "scale") + mode = SCALE; + else if (modeS == "raw") + mode = RAW; + else + { + ROS_ERROR("Invalid mode tag \"%s\".", modeS.c_str()); + return false; + } + } + catch (YAML::Exception&) + { + ROS_DEBUG("The map does not contain a mode tag or it is invalid... assuming Trinary"); + mode = TRINARY; + } + try + { + doc["origin"][0] >> origin[0]; + doc["origin"][1] >> origin[1]; + doc["origin"][2] >> origin[2]; + } + catch (YAML::InvalidScalar&) + { + ROS_ERROR("The map does not contain an origin tag or it is invalid."); + return false; + } + try + { + doc["image"] >> mapfname; + // TODO: make this path-handling more robust + if (mapfname.size() == 0) + { + ROS_ERROR("The image tag cannot be an empty string."); + return false; + } + + boost::filesystem::path mapfpath(mapfname); + if (!mapfpath.is_absolute()) + { + boost::filesystem::path dir(path_to_yaml); + dir = dir.parent_path(); + mapfpath = dir / mapfpath; + mapfname = mapfpath.string(); + } + } + catch (YAML::InvalidScalar&) + { + ROS_ERROR("The map does not contain an image tag or it is invalid."); + return false; + } + return loadMapFromValues(mapfname, res, negate, occ_th, free_th, origin, mode); +} + + + diff --git a/Localizations/Libraries/Ros/map_server/src/map_server.dox b/Localizations/Libraries/Ros/map_server/src/map_server.dox new file mode 100644 index 0000000..1e5b237 --- /dev/null +++ b/Localizations/Libraries/Ros/map_server/src/map_server.dox @@ -0,0 +1,9 @@ +/** + +@mainpage + +@htmlinclude manifest.html + +Command-line usage is in the wiki. +*/ + diff --git a/Localizations/Libraries/Ros/map_server/test/consumer.py b/Localizations/Libraries/Ros/map_server/test/consumer.py new file mode 100644 index 0000000..6ff7e78 --- /dev/null +++ b/Localizations/Libraries/Ros/map_server/test/consumer.py @@ -0,0 +1,71 @@ +#!/usr/bin/env python +# Software License Agreement (BSD License) +# +# Copyright (c) 2008, Willow Garage, Inc. +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of the Willow Garage nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# + +from __future__ import print_function + +PKG = 'static_map_server' +NAME = 'consumer' + +import sys +import unittest +import time + +import rospy +import rostest +from nav_msgs.srv import GetMap + + +class TestConsumer(unittest.TestCase): + def __init__(self, *args): + super(TestConsumer, self).__init__(*args) + self.success = False + + def callback(self, data): + print(rospy.get_caller_id(), "I heard %s" % data.data) + self.success = data.data and data.data.startswith('hello world') + rospy.signal_shutdown('test done') + + def test_consumer(self): + rospy.wait_for_service('static_map') + mapsrv = rospy.ServiceProxy('static_map', GetMap) + resp = mapsrv() + self.success = True + print(resp) + while not rospy.is_shutdown() and not self.success: # and time.time() < timeout_t: <== timeout_t doesn't exists?? + time.sleep(0.1) + self.assert_(self.success) + rospy.signal_shutdown('test done') + +if __name__ == '__main__': + rostest.rosrun(PKG, NAME, TestConsumer, sys.argv) diff --git a/Localizations/Libraries/Ros/map_server/test/rtest.cpp b/Localizations/Libraries/Ros/map_server/test/rtest.cpp new file mode 100644 index 0000000..27f607f --- /dev/null +++ b/Localizations/Libraries/Ros/map_server/test/rtest.cpp @@ -0,0 +1,198 @@ +/* + * Copyright (c) 2008, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/* Author: Brian Gerkey */ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "test_constants.h" + +int g_argc; +char** g_argv; + +class MapClientTest : public testing::Test +{ + public: + // A node is needed to make a service call + ros::NodeHandle* n_; + + MapClientTest() + { + ros::init(g_argc, g_argv, "map_client_test"); + n_ = new ros::NodeHandle(); + got_map_ = false; + got_map_metadata_ = false; + } + + ~MapClientTest() + { + delete n_; + } + + bool got_map_; + boost::shared_ptr map_; + void mapCallback(const boost::shared_ptr& map) + { + map_ = map; + got_map_ = true; + } + + bool got_map_metadata_; + boost::shared_ptr map_metadata_; + void mapMetaDataCallback(const boost::shared_ptr& map_metadata) + { + map_metadata_ = map_metadata; + got_map_metadata_ = true; + } +}; + +/* Try to retrieve the map via service, and compare to ground truth */ +TEST_F(MapClientTest, call_service) +{ + nav_msgs::GetMap::Request req; + nav_msgs::GetMap::Response resp; + ASSERT_TRUE(ros::service::waitForService("static_map", 5000)); + ASSERT_TRUE(ros::service::call("static_map", req, resp)); + ASSERT_FLOAT_EQ(resp.map.info.resolution, g_valid_image_res); + ASSERT_EQ(resp.map.info.width, g_valid_image_width); + ASSERT_EQ(resp.map.info.height, g_valid_image_height); + ASSERT_STREQ(resp.map.header.frame_id.c_str(), "map"); + for(unsigned int i=0; i < resp.map.info.width * resp.map.info.height; i++) + ASSERT_EQ(g_valid_image_content[i], resp.map.data[i]); +} + +/* Try to retrieve the map via topic, and compare to ground truth */ +TEST_F(MapClientTest, subscribe_topic) +{ + ros::Subscriber sub = n_->subscribe("map", 1, [this](auto& map){ mapCallback(map); }); + + // Try a few times, because the server may not be up yet. + int i=20; + while(!got_map_ && i > 0) + { + ros::spinOnce(); + ros::Duration d = ros::Duration().fromSec(0.25); + d.sleep(); + i--; + } + ASSERT_TRUE(got_map_); + ASSERT_FLOAT_EQ(map_->info.resolution, g_valid_image_res); + ASSERT_EQ(map_->info.width, g_valid_image_width); + ASSERT_EQ(map_->info.height, g_valid_image_height); + ASSERT_STREQ(map_->header.frame_id.c_str(), "map"); + for(unsigned int i=0; i < map_->info.width * map_->info.height; i++) + ASSERT_EQ(g_valid_image_content[i], map_->data[i]); +} + +/* Try to retrieve the metadata via topic, and compare to ground truth */ +TEST_F(MapClientTest, subscribe_topic_metadata) +{ + ros::Subscriber sub = n_->subscribe("map_metadata", 1, [this](auto& map_metadata){ mapMetaDataCallback(map_metadata); }); + + // Try a few times, because the server may not be up yet. + int i=20; + while(!got_map_metadata_ && i > 0) + { + ros::spinOnce(); + ros::Duration d = ros::Duration().fromSec(0.25); + d.sleep(); + i--; + } + ASSERT_TRUE(got_map_metadata_); + ASSERT_FLOAT_EQ(map_metadata_->resolution, g_valid_image_res); + ASSERT_EQ(map_metadata_->width, g_valid_image_width); + ASSERT_EQ(map_metadata_->height, g_valid_image_height); +} + +/* Change the map, retrieve the map via topic, and compare to ground truth */ +TEST_F(MapClientTest, change_map) +{ + ros::Subscriber sub = n_->subscribe("map", 1, [this](auto& map){ mapCallback(map); }); + + // Try a few times, because the server may not be up yet. + for (int i = 20; i > 0 && !got_map_; i--) + { + ros::spinOnce(); + ros::Duration d = ros::Duration().fromSec(0.25); + d.sleep(); + } + ASSERT_TRUE(got_map_); + + // Now change the map + got_map_ = false; + nav_msgs::LoadMap::Request req; + nav_msgs::LoadMap::Response resp; + req.map_url = ros::package::getPath("map_server") + "/test/testmap2.yaml"; + ASSERT_TRUE(ros::service::waitForService("change_map", 5000)); + ASSERT_TRUE(ros::service::call("change_map", req, resp)); + ASSERT_EQ(0u, resp.result); + for (int i = 20; i > 0 && !got_map_; i--) + { + ros::spinOnce(); + ros::Duration d = ros::Duration().fromSec(0.25); + d.sleep(); + } + + ASSERT_FLOAT_EQ(tmap2::g_valid_image_res, map_->info.resolution); + ASSERT_EQ(tmap2::g_valid_image_width, map_->info.width); + ASSERT_EQ(tmap2::g_valid_image_height, map_->info.height); + ASSERT_STREQ("map", map_->header.frame_id.c_str()); + for(unsigned int i=0; i < map_->info.width * map_->info.height; i++) + ASSERT_EQ(tmap2::g_valid_image_content[i], map_->data[i]) << "idx:" << i; + + //Put the old map back so the next test isn't broken + got_map_ = false; + req.map_url = ros::package::getPath("map_server") + "/test/testmap.yaml"; + ASSERT_TRUE(ros::service::call("change_map", req, resp)); + ASSERT_EQ(0u, resp.result); + for (int i = 20; i > 0 && !got_map_; i--) + { + ros::spinOnce(); + ros::Duration d = ros::Duration().fromSec(0.25); + d.sleep(); + } + ASSERT_TRUE(got_map_); +} + +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + + g_argc = argc; + g_argv = argv; + + return RUN_ALL_TESTS(); +} diff --git a/Localizations/Libraries/Ros/map_server/test/rtest.xml b/Localizations/Libraries/Ros/map_server/test/rtest.xml new file mode 100644 index 0000000..e7afee9 --- /dev/null +++ b/Localizations/Libraries/Ros/map_server/test/rtest.xml @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + diff --git a/Localizations/Libraries/Ros/map_server/test/spectrum.png b/Localizations/Libraries/Ros/map_server/test/spectrum.png new file mode 100644 index 0000000..5e144eb Binary files /dev/null and b/Localizations/Libraries/Ros/map_server/test/spectrum.png differ diff --git a/Localizations/Libraries/Ros/map_server/test/test_constants.cpp b/Localizations/Libraries/Ros/map_server/test/test_constants.cpp new file mode 100644 index 0000000..4521521 --- /dev/null +++ b/Localizations/Libraries/Ros/map_server/test/test_constants.cpp @@ -0,0 +1,84 @@ +/* + * Copyright (c) 2008, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/* Author: Brian Gerkey */ + +/* This file contains global constants shared among tests */ + +/* Note that these must be changed if the test image changes */ + +#include "test_constants.h" + +const unsigned int g_valid_image_width = 10; +const unsigned int g_valid_image_height = 10; +// Note that the image content is given in row-major order, with the +// lower-left pixel first. This is different from a graphics coordinate +// system, which starts with the upper-left pixel. The loadMapFromFile +// call converts from the latter to the former when it loads the image, and +// we want to compare against the result of that conversion. +const char g_valid_image_content[] = { +0,0,0,0,0,0,0,0,0,0, +100,100,100,100,0,0,100,100,100,0, +100,100,100,100,0,0,100,100,100,0, +100,0,0,0,0,0,0,0,0,0, +100,0,0,0,0,0,0,0,0,0, +100,0,0,0,0,0,100,100,0,0, +100,0,0,0,0,0,100,100,0,0, +100,0,0,0,0,0,100,100,0,0, +100,0,0,0,0,0,100,100,0,0, +100,0,0,0,0,0,0,0,0,0, +}; + +const char* g_valid_png_file = "test/testmap.png"; +const char* g_valid_bmp_file = "test/testmap.bmp"; +const char* g_spectrum_png_file = "test/spectrum.png"; + +const float g_valid_image_res = 0.1; + +// Constants for testmap2 +namespace tmap2 +{ + const char g_valid_image_content[] = { + 100,100,100,100,100,100,100,100,100,100,100,100, + 100,0,0,0,0,0,0,0,0,0,0,100, + 100,0,100,100,100,100,100,100,100,100,0,100, + 100,0,100,0,0,0,0,0,0,100,0,100, + 100,0,0,0,0,0,0,0,0,0,0,100, + 100,0,0,0,0,0,0,0,0,0,0,100, + 100,0,0,0,0,0,0,0,0,0,0,100, + 100,0,0,0,0,0,0,0,0,0,0,100, + 100,0,0,0,0,0,0,0,0,0,0,100, + 100,0,100,0,0,0,0,0,0,100,0,100, + 100,0,0,0,0,0,0,0,0,0,0,100, + 100,100,100,100,100,100,100,100,100,100,100,100, + }; + const float g_valid_image_res = 0.1; + const unsigned int g_valid_image_width = 12; + const unsigned int g_valid_image_height = 12; +} diff --git a/Localizations/Libraries/Ros/map_server/test/test_constants.h b/Localizations/Libraries/Ros/map_server/test/test_constants.h new file mode 100644 index 0000000..c23a578 --- /dev/null +++ b/Localizations/Libraries/Ros/map_server/test/test_constants.h @@ -0,0 +1,51 @@ +/* + * Copyright (c) 2008, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#ifndef MAP_SERVER_TEST_CONSTANTS_H +#define MAP_SERVER_TEST_CONSTANTS_H + +/* Author: Brian Gerkey */ + +/* This file externs global constants shared among tests */ + +extern const unsigned int g_valid_image_width; +extern const unsigned int g_valid_image_height; +extern const char g_valid_image_content[]; +extern const char* g_valid_png_file; +extern const char* g_valid_bmp_file; +extern const float g_valid_image_res; +extern const char* g_spectrum_png_file; + +namespace tmap2 { + extern const char g_valid_image_content[]; + extern const float g_valid_image_res; + extern const unsigned int g_valid_image_width; + extern const unsigned int g_valid_image_height; +} + +#endif diff --git a/Localizations/Libraries/Ros/map_server/test/testmap.bmp b/Localizations/Libraries/Ros/map_server/test/testmap.bmp new file mode 100644 index 0000000..dc8b114 Binary files /dev/null and b/Localizations/Libraries/Ros/map_server/test/testmap.bmp differ diff --git a/Localizations/Libraries/Ros/map_server/test/testmap.png b/Localizations/Libraries/Ros/map_server/test/testmap.png new file mode 100644 index 0000000..910c5c3 Binary files /dev/null and b/Localizations/Libraries/Ros/map_server/test/testmap.png differ diff --git a/Localizations/Libraries/Ros/map_server/test/testmap.yaml b/Localizations/Libraries/Ros/map_server/test/testmap.yaml new file mode 100644 index 0000000..8cf0d73 --- /dev/null +++ b/Localizations/Libraries/Ros/map_server/test/testmap.yaml @@ -0,0 +1,6 @@ +image: testmap.png +resolution: 0.1 +origin: [2.0, 3.0, 1.0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 diff --git a/Localizations/Libraries/Ros/map_server/test/testmap2.png b/Localizations/Libraries/Ros/map_server/test/testmap2.png new file mode 100644 index 0000000..8ca9106 Binary files /dev/null and b/Localizations/Libraries/Ros/map_server/test/testmap2.png differ diff --git a/Localizations/Libraries/Ros/map_server/test/testmap2.yaml b/Localizations/Libraries/Ros/map_server/test/testmap2.yaml new file mode 100644 index 0000000..150a04a --- /dev/null +++ b/Localizations/Libraries/Ros/map_server/test/testmap2.yaml @@ -0,0 +1,6 @@ +image: testmap2.png +resolution: 0.1 +origin: [-2.0, -3.0, -1.0] +negate: 1 +occupied_thresh: 0.5 +free_thresh: 0.2 diff --git a/Localizations/Libraries/Ros/map_server/test/utest.cpp b/Localizations/Libraries/Ros/map_server/test/utest.cpp new file mode 100644 index 0000000..62c4b68 --- /dev/null +++ b/Localizations/Libraries/Ros/map_server/test/utest.cpp @@ -0,0 +1,149 @@ +/* + * Copyright (c) 2008, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/* Author: Brian Gerkey */ + +#include // for std::runtime_error +#include +#include "map_server/image_loader.h" +#include "test_constants.h" + +/* Try to load a valid PNG file. Succeeds if no exception is thrown, and if + * the loaded image matches the known dimensions and content of the file. + * + * This test can fail on OS X, due to an apparent limitation of the + * underlying SDL_Image library. */ +TEST(MapServer, loadValidPNG) +{ + try + { + nav_msgs::GetMap::Response map_resp; + double origin[3] = { 0.0, 0.0, 0.0 }; + map_server::loadMapFromFile(&map_resp, g_valid_png_file, g_valid_image_res, false, 0.65, 0.1, origin); + EXPECT_FLOAT_EQ(map_resp.map.info.resolution, g_valid_image_res); + EXPECT_EQ(map_resp.map.info.width, g_valid_image_width); + EXPECT_EQ(map_resp.map.info.height, g_valid_image_height); + for(unsigned int i=0; i < map_resp.map.info.width * map_resp.map.info.height; i++) + EXPECT_EQ(g_valid_image_content[i], map_resp.map.data[i]); + } + catch(...) + { + ADD_FAILURE() << "Uncaught exception : " << "This is OK on OS X"; + } +} + +/* Try to load a valid BMP file. Succeeds if no exception is thrown, and if + * the loaded image matches the known dimensions and content of the file. */ +TEST(MapServer, loadValidBMP) +{ + try + { + nav_msgs::GetMap::Response map_resp; + double origin[3] = { 0.0, 0.0, 0.0 }; + map_server::loadMapFromFile(&map_resp, g_valid_bmp_file, g_valid_image_res, false, 0.65, 0.1, origin); + EXPECT_FLOAT_EQ(map_resp.map.info.resolution, g_valid_image_res); + EXPECT_EQ(map_resp.map.info.width, g_valid_image_width); + EXPECT_EQ(map_resp.map.info.height, g_valid_image_height); + for(unsigned int i=0; i < map_resp.map.info.width * map_resp.map.info.height; i++) + EXPECT_EQ(g_valid_image_content[i], map_resp.map.data[i]); + } + catch(...) + { + ADD_FAILURE() << "Uncaught exception"; + } +} + +/* Try to load an invalid file. Succeeds if a std::runtime_error exception + * is thrown. */ +TEST(MapServer, loadInvalidFile) +{ + try + { + nav_msgs::GetMap::Response map_resp; + double origin[3] = { 0.0, 0.0, 0.0 }; + map_server::loadMapFromFile(&map_resp, "foo", 0.1, false, 0.65, 0.1, origin); + } + catch(std::runtime_error &e) + { + SUCCEED(); + return; + } + catch(...) + { + FAIL() << "Uncaught exception"; + } + ADD_FAILURE() << "Didn't throw exception as expected"; +} + +std::vector countValues(const nav_msgs::GetMap::Response& map_resp) +{ + std::vector counts(256, 0); + for (unsigned int i = 0; i < map_resp.map.data.size(); i++) + { + unsigned char value = static_cast(map_resp.map.data[i]); + counts[value]++; + } + return counts; +} + +TEST(MapServer, testMapMode) +{ + nav_msgs::GetMap::Response map_resp; + double origin[3] = { 0.0, 0.0, 0.0 }; + + map_server::loadMapFromFile(&map_resp, g_spectrum_png_file, 0.1, false, 0.65, 0.1, origin, TRINARY); + std::vector trinary_counts = countValues(map_resp); + EXPECT_EQ(90u, trinary_counts[100]); + EXPECT_EQ(26u, trinary_counts[0]); + EXPECT_EQ(140u, trinary_counts[255]); + + map_server::loadMapFromFile(&map_resp, g_spectrum_png_file, 0.1, false, 0.65, 0.1, origin, SCALE); + std::vector scale_counts = countValues(map_resp); + EXPECT_EQ(90u, scale_counts[100]); + EXPECT_EQ(26u, scale_counts[0]); + unsigned int scaled_values = 0; + for (unsigned int i = 1; i < 100; i++) + { + scaled_values += scale_counts[i]; + } + EXPECT_EQ(140u, scaled_values); + + map_server::loadMapFromFile(&map_resp, g_spectrum_png_file, 0.1, false, 0.65, 0.1, origin, RAW); + std::vector raw_counts = countValues(map_resp); + for (unsigned int i = 0; i < raw_counts.size(); i++) + { + EXPECT_EQ(1u, raw_counts[i]) << i; + } +} + +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/Localizations/Packages/loc_base/CMakeLists.txt b/Localizations/Packages/loc_base/CMakeLists.txt new file mode 100644 index 0000000..11ebdf1 --- /dev/null +++ b/Localizations/Packages/loc_base/CMakeLists.txt @@ -0,0 +1,235 @@ +cmake_minimum_required(VERSION 3.0.2) +project(loc_base) + +## Compile as C++11, supported in ROS Kinetic and newer +add_compile_options(-std=c++17) + + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + dynamic_reconfigure + geometry_msgs + loc_core + pluginlib + roscpp + map_server + amcl + slam_toolbox + slam_toolbox_msgs + slam_toolbox_rviz + std_msgs +) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) +find_package(yaml-cpp REQUIRED) + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a exec_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# geometry_msgs +# ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( + INCLUDE_DIRS include + LIBRARIES loc_base + CATKIN_DEPENDS + dynamic_reconfigure + geometry_msgs + loc_core + amcl + map_server + slam_toolbox + slam_toolbox_msgs + slam_toolbox_rviz + pluginlib + roscpp +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${YAML_INCLUDE_DIRS} +) + +## Declare a C++ library + add_library(${PROJECT_NAME} + src/loc_base.cpp + src/loc_base_util.cpp +) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(${PROJECT_NAME} + ${catkin_LIBRARIES} + yaml-cpp +) +## Declare a C++ executable +## With catkin_make all packages are built within a single CMake context +## The recommended prefix ensures that target names across packages don't collide +add_executable(${PROJECT_NAME}_node src/loc_base_node.cpp) + +## Rename C++ executable without prefix +## The above recommended prefix causes long target names, the following renames the +## target back to the shorter version for ease of user use +## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" +# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") + +## Add cmake target dependencies of the executable +## same as for the library above +add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +target_link_libraries(${PROJECT_NAME}_node + ${catkin_LIBRARIES} +) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# catkin_install_python(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html +install(TARGETS ${PROJECT_NAME}_node + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +## Mark libraries for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) + +## Mark cpp header files for installation +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE +) + +## Mark other files for installation (e.g. launch and bag files, etc.) +install(FILES + plugins.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +install(DIRECTORY + config + launch + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_loc_base.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/Localizations/Packages/loc_base/config/localization.yaml b/Localizations/Packages/loc_base/config/localization.yaml new file mode 100644 index 0000000..8cc080c --- /dev/null +++ b/Localizations/Packages/loc_base/config/localization.yaml @@ -0,0 +1,34 @@ +Amcl: + use_map_topic: true + odom_model_type: "diff-corrected" + gui_publish_rate: 10.0 + save_pose_rate: 0.5 + laser_max_beams: 200 + laser_min_range: -1.0 + laser_max_range: -1.0 + min_particles: 500 + max_particles: 5000 + kld_err: 0.09 + kld_z: 0.99 + odom_alpha1: 0.02 + odom_alpha2: 0.01 + odom_alpha3: 0.01 + odom_alpha4: 0.02 + laser_z_hit: 0.5 + laser_z_short: 0.05 + laser_z_max: 0.05 + laser_z_rand: 0.5 + laser_sigma_hit: 0.2 + laser_lambda_short: 0.1 + laser_model_type: "likelihood_field" + laser_likelihood_max_dist: 2.0 + update_min_d: 0.2 + update_min_a: 0.2 + odom_frame_id: odom + base_frame_id: base_footprint + global_frame_id: map + resample_interval: 1 + transform_tolerance: 0.2 + recovery_alpha_slow: 0.0 + recovery_alpha_fast: 0.0 + diff --git a/Localizations/Packages/loc_base/config/map_managers.yaml b/Localizations/Packages/loc_base/config/map_managers.yaml new file mode 100644 index 0000000..7967f22 --- /dev/null +++ b/Localizations/Packages/loc_base/config/map_managers.yaml @@ -0,0 +1,15 @@ +plugins: + - {name: navigation_map, type: "costmap_2d::StaticLayer" } + - {name: virtual_walls_map, type: "costmap_2d::StaticLayer" } + +navigation_map: + map_topic: map + map_pkg: managerments + map_file: maps/maze/maze.yaml + +virtual_walls_map: + map_topic: /virtual_walls/map + namespace: /virtual_walls + map_pkg: managerments + map_file: maps/maze/maze.yaml + diff --git a/Localizations/Packages/loc_base/include/loc_base/loc_base.h b/Localizations/Packages/loc_base/include/loc_base/loc_base.h new file mode 100644 index 0000000..2acf08a --- /dev/null +++ b/Localizations/Packages/loc_base/include/loc_base/loc_base.h @@ -0,0 +1,168 @@ +#ifndef __LOC_BASE_H_INCLUDED_ +#define __LOC_BASE_H_INCLUDED_ +#include +#include +#include +#include "loc_core/localization.h" +#include "amcl/amcl.h" +#include "map_server/map_server.h" +// #include "hector_mapping/HectorMappingRos.h" +#include "slam_toolbox/slam_toolbox_async.hpp" + +namespace loc_base +{ + class LocBase : public loc_core::BaseLocalization + { + public: + /** + * @brief Constructor + */ + LocBase(); + + /** + * @brief Destructor + */ + virtual ~LocBase(); + + /** + * @brief Initialization function for loc base + * @param nh A Node handle + * @param tf A poiter tranform listen + */ + void initialize(ros::NodeHandle nh, TFListenerPtr tf) override; + + /** + * @brief Loading a Activate Map File name + * @param activated_map_filename The Map File name + * @return True if activated_map_filename is exsits, else Fasle + */ + bool loadActivateMapFileName(std::string& activated_map_filename) override; + + /** + * @brief Saving a Activate Map File name + * @param activated_map_filename The Map File name + * @return True if activated_map_filename is exsits, else Fasle + */ + bool saveActivateMapFileName(const std::string& activated_map_filename) override; + + /** + * @brief Create any files to a folder of the path + * @param path The path where is be saved any files (png, yaml ...) + * @param map_name The name folder and files + */ + bool createStaticMap(const std::string map_name) override; + + /** + * @brief Load a map given a path to a yaml file + * @param filename The file name + * @return True/False If load map succesed or not done + */ + bool changeStaticMap(const std::string filename) override; + + /** + * @brief Load a virtual walls map given a path to a yaml file + * @param filename The file name + * @return True/False If load map succesed or not done + */ + bool changeVirtualWallsMap(const std::string filename) override; + + /** + * @brief Get the list of map folder in the workingdirection + * @param directories The list map name + */ + void listMapFiles(std::stringstream &directories) override; + + /** + * @brief Start the mapping process. + * When the state variable 'SlamState' is 'Ready', this function can be called. + * Upon successful invocation, the state variable 'SlamState' will change to 'Mapping'. + * @return True/False Call fucntion is successed/failed. + */ + bool startMapping() override; + + /** + * @brief Turn off the Localization mode. + * At this point, the robot's coordinate information will not be updated. + * If called successfully, the 'SlamState' variable will change to 'Ready'. + * @return True/False Call fucntion is successed/failed + */ + bool stopMapping() override; + + /** + * @brief Set the initial position of the robot on the map. + * The condition to call the function is that the 'SlamState' is 'Localization'. + * When the function is called successfully, the 'SlamState' will change to 'Calibrations'. + * When the 'Calibrations' process ends, the 'SlamState' will change back to 'Localization'. + * @return True/False Call fucntion is successed/failed + */ + bool startLocalization() override; + + /** + * @brief Turn off the Localization mode. + * At this point, the robot's coordinate information will not be updated. + * If called successfully, the 'SlamState' variable will change to 'Ready'. + * @return True/False Call fucntion is successed/failed + */ + bool stopLocalization() override; + + /** + * @brief Get State of Loc Base + */ + loc_core::slam_state_en getState() override; + + // static std::string activated_map_filename_; + // std::string map_topic_; + + protected: + + virtual bool loadParameters(); + + virtual void initalPose(geometry_msgs::PoseWithCovariance init_pose); + + static bool getPose(std::string base_frame_id, std::string map_frame, + geometry_msgs::PoseStamped& global_pose, double transform_tolerance = 2.0); + + static void savePose(); + + virtual bool init_map_server(); + + virtual bool close_map_server(); + + virtual bool init_localization(); + + virtual bool close_localization(); + + virtual bool init_mapping(); + + virtual bool close_mapping(); + + static void signalHandler(int sign); + + virtual void savePoseCallback(const ros::WallTimerEvent& event); + + virtual void cleanup(); + + + bool initialized_; + ros::NodeHandle nh_; + ros::NodeHandle private_nh_; + ros::Publisher init_pub_; + static TFListenerPtr tf_; + + XmlRpc::XmlRpcValue plugins_; + // std::map map_file_vt_; + std::map> static_map_vt_; + + static std::string base_frame_id_, global_frame_id_; + static std::shared_ptr localization_althm_; + std::mutex localization_mutex_; + std::shared_ptr localization_timer_ptr_; + static std::shared_ptr mapping_althm_; + + loc_core::slam_state_en slam_state_{ loc_core::Ready }; + static std::string *working_dir_ptr_; + }; + +} // namespace loc_base + +#endif // __LOC_BASE_H_INCLUDED_ \ No newline at end of file diff --git a/Localizations/Packages/loc_base/include/loc_base/loc_base_util.h b/Localizations/Packages/loc_base/include/loc_base/loc_base_util.h new file mode 100644 index 0000000..41c9e3c --- /dev/null +++ b/Localizations/Packages/loc_base/include/loc_base/loc_base_util.h @@ -0,0 +1,39 @@ +#ifndef __LocBaseUtil_H_INCLUDED_ +#define __LocBaseUtil_H_INCLUDED_ +#include + +namespace loc_base +{ + class LocBaseUtil + { + public: + /* + * @brief returns a full qualified filepath from package name and file name. + * + * @param package package name + * + * @return full qualified filepath (or filename if package is empty or couldn't be found). + */ + static std::string filepathFromPackage(const std::string & package, const std::string & filename); + + static bool loadActivateMapFileName(const std::string& filename, std::string &activated_map_filename); + + static bool saveActivateMapFileName(const std::string& filename, const std::string &activated_map_filename); + + static bool convertPgmToPng(const std::string& pgmFileName, const std::string& pngFileName); + + static bool writeGridMap(const std::string& filename); + + static bool checkIfMapFilesExist(const std::string& filename); + + static bool replaceMapDirectory(const std::string& mapDirectory); + + /** + * @brief Get the list of map folder in the workingdirection + * @param directories The list map name + */ + static std::stringstream listMapFiles(const std::string &path); + }; +} + +#endif diff --git a/Localizations/Packages/loc_base/launch/loc_base.launch b/Localizations/Packages/loc_base/launch/loc_base.launch new file mode 100644 index 0000000..3347f3b --- /dev/null +++ b/Localizations/Packages/loc_base/launch/loc_base.launch @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/Localizations/Packages/loc_base/package.xml b/Localizations/Packages/loc_base/package.xml new file mode 100644 index 0000000..7a50bb2 --- /dev/null +++ b/Localizations/Packages/loc_base/package.xml @@ -0,0 +1,98 @@ + + + loc_base + 0.0.0 + The loc_base package + + + + + robotics + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + dynamic_reconfigure + geometry_msgs + loc_core + pluginlib + roscpp + loc_core + amcl + map_server + slam_toolbox + slam_toolbox_msgs + slam_toolbox_rviz + std_msgs + yaml-cpp + + dynamic_reconfigure + geometry_msgs + loc_core + pluginlib + roscpp + loc_core + amcl + map_server + slam_toolbox + slam_toolbox_msgs + slam_toolbox_rviz + std_msgs + yaml-cpp + + dynamic_reconfigure + geometry_msgs + loc_core + pluginlib + roscpp + loc_core + amcl + map_server + slam_toolbox + slam_toolbox_msgs + slam_toolbox_rviz + std_msgs + yaml-cpp + + + + + + diff --git a/Localizations/Packages/loc_base/plugins.xml b/Localizations/Packages/loc_base/plugins.xml new file mode 100644 index 0000000..cc1fe10 --- /dev/null +++ b/Localizations/Packages/loc_base/plugins.xml @@ -0,0 +1,7 @@ + + + + + + + \ No newline at end of file diff --git a/Localizations/Packages/loc_base/src/loc_base.cpp b/Localizations/Packages/loc_base/src/loc_base.cpp new file mode 100644 index 0000000..2fb483f --- /dev/null +++ b/Localizations/Packages/loc_base/src/loc_base.cpp @@ -0,0 +1,699 @@ +#include +#include "loc_base/loc_base.h" +#include "loc_base/loc_base_util.h" +#include "map_server/map_generator.h" +#include +#include +#include + +std::shared_ptr loc_base::LocBase::localization_althm_ = nullptr; +std::shared_ptr loc_base::LocBase::mapping_althm_ = nullptr; +TFListenerPtr loc_base::LocBase::tf_; +std::string loc_base::LocBase::base_frame_id_; +std::string loc_base::LocBase::global_frame_id_; +std::string* loc_base::LocBase::working_dir_ptr_ = NULL; + +loc_base::LocBase::LocBase() + : initialized_(false) +{ + ROS_INFO_STREAM("Boost version: " << BOOST_LIB_VERSION); +} + +loc_base::LocBase::~LocBase() { + cleanup(); +} + +void loc_base::LocBase::cleanup() { + // Clean up map servers + for (auto& map_pair : static_map_vt_) { + map_pair.second.reset(); + } + static_map_vt_.clear(); + + // Clean up localization + if (loc_base::LocBase::localization_althm_) + loc_base::LocBase::localization_althm_.reset(); + // Clean up mapping + if (loc_base::LocBase::mapping_althm_) + loc_base::LocBase::mapping_althm_.reset(); +} + +void loc_base::LocBase::initialize(ros::NodeHandle nh, TFListenerPtr tf) +{ + if (!initialized_) + { + nh_ = nh; + tf_ = tf; + private_nh_ = ros::NodeHandle("~"); + this->loadParameters(); + std::string activated_map_filename; + if (this->loadActivateMapFileName(activated_map_filename)) + { + loc_base::LocBase::activated_map_filename_ = activated_map_filename; + ROS_INFO("Active map name %s", loc_base::LocBase::activated_map_filename_.c_str()); + } + else ROS_WARN("Can not get active map name"); + + ros::NodeHandle node; + init_pub_ = node.advertise("/initialpose", 1); + + switch (slam_state_) + { + case loc_core::Ready: + this->startLocalization(); + break; + case loc_core::Localization: + this->startLocalization(); + break; + default: + break; + } + initialized_ = true; + } +} + +bool loc_base::LocBase::loadActivateMapFileName(std::string& activated_map_filename) +{ + if (this->working_dir_.empty()) return false; + std::string filename = std::string(this->working_dir_ + "/activated_map.txt"); + bool result = loc_base::LocBaseUtil::loadActivateMapFileName(filename, activated_map_filename); + return result; +} + +bool loc_base::LocBase::saveActivateMapFileName(const std::string& activated_map_filename) +{ + if (activated_map_filename.empty()) return false; + if (this->working_dir_.empty()) return false; + std::string filename = std::string(this->working_dir_ + "/activated_map.txt"); + + bool result = this->createStaticMap(activated_map_filename); + if (!result) return false; + + if (slam_state_ != loc_core::Mapping) + { + std::string static_map_folder = std::string(this->working_dir_ + "/" + activated_map_filename); + if (!boost::filesystem::exists(static_map_folder)) + { + ROS_ERROR("%s file is not existed", static_map_folder.c_str()); + return false; + } + } + result = loc_base::LocBaseUtil::saveActivateMapFileName(filename, activated_map_filename); + if (!result) return false;; + activated_map_filename_ = activated_map_filename; + return result; +} + +bool loc_base::LocBase::createStaticMap(const std::string map_name) +{ + if (slam_state_ == loc_core::Mapping) + { + if (this->working_dir_.empty()) + { + ROS_WARN("Working directory is not seting"); + return false; + } + std::string static_map_folder = std::string(this->working_dir_ + "/" + map_name); + if (!boost::filesystem::exists(static_map_folder)) + boost::filesystem::create_directories(static_map_folder); + else + { + ROS_WARN("Map file is existed"); + return false; + } + std::string static_map_name = std::string(static_map_folder + "/" + map_name); + std::shared_ptr map_generator_ptr = + std::make_shared(static_map_name, this->map_topic_); + return map_generator_ptr->waitingGenerator(5.0); + } + return true; +} + +bool loc_base::LocBase::changeStaticMap(const std::string filename) +{ + if (filename.empty()) return false; + ROS_INFO("filename %s", filename.c_str()); + if (!loc_base::LocBase::activated_map_filename_.empty() && activated_map_filename_ == filename) return true; + try + { + for (int32_t i = 0; i < plugins_.size(); ++i) + { + std::string pname = static_cast(plugins_[i]["name"]); + std::string type = static_cast(plugins_[i]["type"]); + if (type == std::string("costmap_2d::StaticLayer")) + { + if (!nh_.hasParam(pname)) + continue; + else + { + std::string map_topic, ns; + nh_.param(std::string(pname + "/namespace"), ns, std::string("")); + nh_.param(std::string(pname + "/map_topic"), map_topic, std::string("")); + + if (ns == std::string("") && (map_topic == std::string("map") || map_topic == std::string("/map"))) + { + if (std::map>::iterator ms = static_map_vt_.find(pname); ms != static_map_vt_.end()) + { + std::string path_to_yaml = std::string(this->working_dir_ + "/" + filename + "/" + filename + ".yaml"); + ROS_INFO("path_to_yaml %s", path_to_yaml.c_str()); + if (!boost::filesystem::exists(path_to_yaml)) + { + ROS_WARN("%s is not exist", path_to_yaml.c_str()); + return false; + } + ROS_INFO("loadMapFromYaml %s", path_to_yaml.c_str()); + bool result = ms->second->loadMapFromYaml(path_to_yaml); + if (result) this->saveActivateMapFileName(filename); + return result; + } + else + { + ROS_WARN("Not found"); + return false; + } + } + } + } + } + } + catch (XmlRpc::XmlRpcException& e) + { + ROS_ERROR("Change map is error: %s", e.getMessage().c_str()); + return false; + } + return true; +} + +bool loc_base::LocBase::changeVirtualWallsMap(const std::string filename) +{ + if (filename.empty()) return false; + try + { + for (int32_t i = 0; i < plugins_.size(); ++i) + { + std::string pname = static_cast(plugins_[i]["name"]); + std::string type = static_cast(plugins_[i]["type"]); + if (type == std::string("costmap_2d::StaticLayer")) + { + if (!nh_.hasParam(pname)) + continue; + else + { + std::string map_topic, ns; + nh_.param(std::string(pname + "/namespace"), ns, std::string("")); + nh_.param(std::string(pname + "/map_topic"), map_topic, std::string("")); + } + } + } + } + catch (XmlRpc::XmlRpcException& e) + { + ROS_ERROR("Change virtual map is error: %s", e.getMessage().c_str()); + return false; + } + return true; +} + +void loc_base::LocBase::listMapFiles(std::stringstream& directories) +{ + directories = loc_base::LocBaseUtil::listMapFiles(*loc_base::LocBase::working_dir_ptr_); +} + +bool loc_base::LocBase::startMapping() +{ + if (slam_state_ != loc_core::Mapping) + { + this->stopLocalization(); + this->init_mapping(); + slam_state_ = loc_core::Mapping; + } + return true; +} + +bool loc_base::LocBase::stopMapping() +{ + if (slam_state_ == loc_core::Mapping) + { + this->close_mapping(); + slam_state_ = loc_core::Ready; + return true; + } + else return false; +} + +bool loc_base::LocBase::startLocalization() +{ + if (slam_state_ != loc_core::Localization) + { + this->stopMapping(); + bool resule_map_server = this->init_map_server(); + bool resule_localization = this->init_localization(); + + if (!resule_map_server || !resule_localization) + { + this->stopLocalization(); + slam_state_ = loc_core::Error; + return false; + } + slam_state_ = loc_core::Localization; + return resule_map_server && resule_localization; + } + return true; +} + +bool loc_base::LocBase::stopLocalization() +{ + if (slam_state_ == loc_core::Localization) + { + this->close_localization(); + this->close_map_server(); + slam_state_ = loc_core::Ready; + } + return true; +} + +loc_core::slam_state_en loc_base::LocBase::getState() +{ + return slam_state_; +} + +bool loc_base::LocBase::loadParameters() +{ + std::string package; + std::string map_topic; + std::string ns; + plugins_.clear(); + if (!nh_.getParam("plugins", plugins_)) + { + ROS_WARN("Can not get plugins"); + return false; + } + else + { + try + { + for (int32_t i = 0; i < plugins_.size(); ++i) + { + std::string pname = static_cast(plugins_[i]["name"]); + std::string type = static_cast(plugins_[i]["type"]); + if (pname != std::string("navigation_map")) continue; + if (type == std::string("costmap_2d::StaticLayer")) + { + if (!nh_.hasParam(pname)) + continue; + else + { + nh_.param(std::string(pname + "/map_pkg"), package, std::string("")); + nh_.param(std::string(pname + "/namespace"), ns, std::string("")); + nh_.param(std::string(pname + "/map_topic"), map_topic, std::string("")); + if (package.empty()) + { + ROS_FATAL("'/map_pkg' is empty"); + exit(1); + } + if (map_topic.empty()) + { + ROS_WARN("'/map_topic' is empty"); + exit(1); + } + break; + } + } + } + } + catch (XmlRpc::XmlRpcException& e) + { + ROS_ERROR("Load map is error: %s", e.getMessage().c_str()); + return false; + } + } + + try + { + std::string working_dir = loc_base::LocBaseUtil::filepathFromPackage(package, ""); + std::string static_map_folder = std::string(working_dir + "/maps"); + if (!boost::filesystem::exists(static_map_folder)) + boost::filesystem::create_directories(static_map_folder); + ROS_INFO("static_map_folder : %s", static_map_folder.c_str()); + loc_base::LocBase::working_dir_ptr_ = &this->working_dir_; + this->working_dir_ = static_map_folder; + this->map_topic_ = map_topic; + } + catch (std::exception& e) + { + ROS_ERROR("Error to check working directory: %s", e.what()); + return false; + } + return true; +} + +bool loc_base::LocBase::init_map_server() +{ + ROS_INFO("Init Map Server"); + ROS_INFO("Getting %s/plugins .....", nh_.getNamespace().c_str()); + try + { + for (int32_t i = 0; i < plugins_.size(); ++i) + { + std::string pname = static_cast(plugins_[i]["name"]); + std::string type = static_cast(plugins_[i]["type"]); + if (type == std::string("costmap_2d::StaticLayer")) + { + if (!nh_.hasParam(pname)) + ROS_WARN("Can not get plugins/%s", pname.c_str()); + else + { + try + { + std::string map_pkg, map_file, ns; + nh_.param(std::string(pname + "/map_pkg"), map_pkg, std::string("")); + nh_.param(std::string(pname + "/map_file"), map_file, std::string("")); + nh_.param(std::string(pname + "/namespace"), ns, std::string("")); + std::string path_map_file; + if (loc_base::LocBase::activated_map_filename_.empty()) + { + path_map_file = loc_base::LocBaseUtil::filepathFromPackage(map_pkg, std::string("/maps/" + map_file + "/" + map_file + ".yaml")); + if (pname == std::string("navigation_map")) + { + if (!boost::filesystem::exists(std::string(this->working_dir_ + "/activated_map.txt"))) + { + loc_base::LocBase::activated_map_filename_ = map_file; + this->saveActivateMapFileName(loc_base::LocBase::activated_map_filename_); + } + } + } + else + path_map_file = std::string(this->working_dir_ + "/" + loc_base::LocBase::activated_map_filename_ + "/" + loc_base::LocBase::activated_map_filename_ + ".yaml"); + + ROS_INFO(" Using plugin \"%s\" with type %s, map_file %s", pname.c_str(), type.c_str(), path_map_file.c_str()); + static_map_vt_[pname] = std::make_shared(nh_, ns, path_map_file, 0.0); + } + catch (std::runtime_error& e) + { + ROS_ERROR("map_server exception: %s", e.what()); + return false; + } + } + } + } + } + catch (XmlRpc::XmlRpcException& e) + { + ROS_ERROR("Init map is error: %s", e.getMessage().c_str()); + return false; + } + return true; + +} + +bool loc_base::LocBase::close_map_server() +{ + // plugins_.clear(); + // map_file_vt_.clear(); + ROS_INFO("Waiting MapServer be closed"); + static_map_vt_.clear(); + ROS_INFO("Closed MapServer"); + return true; +} + +bool loc_base::LocBase::init_localization() +{ + ROS_INFO("Init localization"); + signal(SIGINT, &loc_base::LocBase::signalHandler); + ros::NodeHandle nh = ros::NodeHandle(private_nh_, "Amcl"); + nh.param("base_frame_id", loc_base::LocBase::base_frame_id_, std::string("base_link")); + nh.param("global_frame_id", loc_base::LocBase::global_frame_id_, std::string("map")); + + geometry_msgs::PoseWithCovariance init_pose; + init_pose.pose.position.x = init_pose.pose.position.y = 0.0; + init_pose.covariance[6 * 0 + 0] = 0.5 * 0.5; + init_pose.covariance[6 * 1 + 1] = 0.5 * 0.5; + init_pose.covariance[6 * 5 + 5] = M_PI / 12.0 * M_PI / 12.0; + + std::string path_file = std::string(this->working_dir_ + "/initial_pose.txt"); + if (boost::filesystem::exists(path_file)) + { + try + { + std::ifstream file(path_file, std::ios::in); + if (!file.is_open()) + { + ROS_WARN_THROTTLE(2.0, "There was a problem opening the input file!\n"); + } + else + { + double x, y, theta; + float init_cov[3]; + + file >> x >> y >> theta >> init_cov[0] >> init_cov[1] >> init_cov[2]; + // Check if the read operation was successful + if (file.fail()) + { + ROS_ERROR("Failed to read data from the file: %s", path_file.c_str()); + } + ROS_INFO_STREAM(x << " " << y << " " << theta << " " << init_cov[0] << " " << init_cov[1] << " " << init_cov[2]); + + init_pose.pose.position.x = std::isnan(x) || fabs(x) < 1e-5 ? 0.0 : x; + init_pose.pose.position.y = std::isnan(y) || fabs(y) < 1e-5 ? 0.0 : y; + theta = std::isnan(theta) || fabs(theta) < 1e-5 ? 0.0 : theta; + tf::Quaternion quat; + quat.setRPY(0.0, 0.0, theta); + tf::quaternionTFToMsg(quat, init_pose.pose.orientation); + + init_pose.covariance[6 * 0 + 0] = std::isnan(init_cov[0]) || fabs(init_cov[0]) < 1e-4 ? 0.5 * 0.5 : init_cov[0]; + init_pose.covariance[6 * 1 + 1] = std::isnan(init_cov[1]) || fabs(init_cov[1]) < 1e-4 ? 0.5 * 0.5 : init_cov[1]; + init_pose.covariance[6 * 5 + 5] = std::isnan(init_cov[2]) || fabs(init_cov[2]) < 1e-4 ? M_PI / 12.0 * M_PI / 12.0 : init_cov[2]; + file.close(); // Optionally close the file explicitly, though it's closed when it goes out of scope + } + } + catch (const std::exception& e) + { + ROS_ERROR("Caught an exception: %s", e.what()); + // Handle the exception appropriately, maybe return or do additional logging + } + } + else + { + ROS_INFO("Creating file: %s", path_file.c_str()); + double zero = 0.0; + double initial_cov[3]; + initial_cov[0] = 0.5 * 0.5; + initial_cov[1] = 0.5 * 0.5; + initial_cov[2] = M_PI / 12.0 * M_PI / 12.0; + std::ofstream file(path_file); + file << zero << " " << zero << " " << zero << " " << initial_cov[0] << " " << initial_cov[1] << " " << initial_cov[3]; + file.close(); + } + + loc_base::LocBase::localization_althm_ = std::make_shared(nh); + while (loc_base::LocBase::localization_althm_ && + !loc_base::LocBase::localization_althm_->getInitialized()) + { + ros::spinOnce(); + } + + this->initalPose(init_pose); + localization_timer_ptr_ = std::make_shared( + private_nh_.createWallTimer(ros::WallDuration(1.0), &loc_base::LocBase::savePoseCallback, this)); + return true; +} + +bool loc_base::LocBase::close_localization() +{ + localization_timer_ptr_->stop(); + if (localization_timer_ptr_) localization_timer_ptr_.reset(); + + if (loc_base::LocBase::localization_althm_) + loc_base::LocBase::localization_althm_.reset(); + ROS_INFO("Waiting Localization be closed"); + while (ros::ok() && loc_base::LocBase::localization_althm_) + { + ros::spinOnce(); + } + ROS_INFO("Closed Localization"); + return true; +} + +bool loc_base::LocBase::init_mapping() +{ + int stack_size; + ros::NodeHandle nh = ros::NodeHandle(private_nh_, "SlamToolBox"); + if (nh.getParam("stack_size_to_use", stack_size)) + { + ROS_INFO("Node using stack size %i", (int)stack_size); + const rlim_t max_stack_size = stack_size; + struct rlimit stack_limit; + getrlimit(RLIMIT_STACK, &stack_limit); + if (stack_limit.rlim_cur < stack_size) + { + stack_limit.rlim_cur = stack_size; + } + setrlimit(RLIMIT_STACK, &stack_limit); + } + ROS_INFO("Waiting for init mapping"); + loc_base::LocBase::mapping_althm_ = std::make_shared(nh_); + while (ros::ok() && !loc_base::LocBase::mapping_althm_->getInitialized()) + { + ros::Duration(0.1).sleep(); + ros::spinOnce(); + } + + ROS_INFO("Initialized Mapping"); + return true; +} + +bool loc_base::LocBase::close_mapping() +{ + if (loc_base::LocBase::mapping_althm_) + loc_base::LocBase::mapping_althm_.reset(); + + ROS_INFO("Waiting for close mapping"); + while (ros::ok() && loc_base::LocBase::mapping_althm_) + { + ros::spinOnce(); + } + ROS_INFO("Closed Mapping"); + return true; +} + +void loc_base::LocBase::savePose() +{ + geometry_msgs::PoseStamped global_pose; + geometry_msgs::Pose2D pose2d; + float initial_cov[3]; + bool updating; + if (loc_base::LocBase::localization_althm_) + { + if (loc_base::LocBase::localization_althm_->getInitialized()) + { + geometry_msgs::PoseWithCovarianceStamped amcl_pose; + localization_althm_->savePoseToServer(amcl_pose); + pose2d.x = amcl_pose.pose.pose.position.x; + pose2d.y = amcl_pose.pose.pose.position.y; + pose2d.theta = tf2::getYaw(amcl_pose.pose.pose.orientation); + initial_cov[0] = amcl_pose.pose.covariance[6 * 0 + 0]; + initial_cov[1] = amcl_pose.pose.covariance[6 * 1 + 1]; + initial_cov[2] = amcl_pose.pose.covariance[6 * 5 + 5]; + updating = true; + } + } + else + { + if (loc_base::LocBase::getPose(loc_base::LocBase::base_frame_id_, loc_base::LocBase::global_frame_id_, global_pose)) + { + pose2d.x = global_pose.pose.position.x; + pose2d.y = global_pose.pose.position.y; + pose2d.theta = tf2::getYaw(global_pose.pose.orientation); + initial_cov[0] = 0.5 * 0.5; + initial_cov[1] = 0.5 * 0.5; + initial_cov[2] = M_PI / 12.0 * M_PI / 12.0; + updating = true; + } + } + std::string path_file; + if (loc_base::LocBase::working_dir_ptr_) + path_file = std::string((*loc_base::LocBase::working_dir_ptr_) + "/initial_pose.txt"); + else return; + + if (updating) + { + try + { + std::ofstream file(path_file, std::ios::out); + //check to see that the file was opened correctly: + if (!file.is_open()) { + ROS_WARN_THROTTLE(2.0, "There was a problem opening the input file!\n"); + } + else + { + // ROS_INFO_STREAM(pose2d.x << " " << pose2d.y << " " << pose2d.theta << " " << initial_cov[0] << " " << initial_cov[1] << " " << initial_cov[2]); + pose2d.x = std::isnan(pose2d.x) || fabs(pose2d.x) < 1e-4 ? 0.0 : pose2d.x; + pose2d.y = std::isnan(pose2d.y) || fabs(pose2d.y) < 1e-4 ? 0.0 : pose2d.y; + pose2d.theta = std::isnan(pose2d.theta) || fabs(pose2d.theta) < 1e-3 ? 0.0 : pose2d.theta; + + initial_cov[0] = std::isnan(initial_cov[0]) || fabs(initial_cov[0]) < 1e-4 ? 0.5 * 0.5 : initial_cov[0]; + initial_cov[1] = std::isnan(initial_cov[1]) || fabs(initial_cov[1]) < 1e-4 ? 0.5 * 0.5 : initial_cov[1]; + initial_cov[2] = std::isnan(initial_cov[2]) || fabs(initial_cov[2]) < 1e-4 ? M_PI / 12.0 * M_PI / 12.0 : initial_cov[2]; + // ROS_INFO_STREAM(pose2d.x << " " << pose2d.y << " " << pose2d.theta << " " << initial_cov[0] << " " << initial_cov[1] << " " << initial_cov[2]); + file << pose2d.x << " " << pose2d.y << " " << pose2d.theta << " " + << initial_cov[0] << " " << initial_cov[1] << " " << initial_cov[2]; + // file << pose2d.x << " " << pose2d.y << " " << pose2d.theta; + file.close(); + } + + } + catch (const std::exception& e) + { + return; + } + } +} + +void loc_base::LocBase::signalHandler(int sign) +{ + loc_base::LocBase::savePose(); + ros::shutdown(); +} + +void loc_base::LocBase::savePoseCallback(const ros::WallTimerEvent& event) +{ + loc_base::LocBase::savePose(); +} + +bool loc_base::LocBase::getPose(std::string base_frame_id, std::string map_frame, + geometry_msgs::PoseStamped& global_pose, double transform_tolerance) +{ + geometry_msgs::PoseStamped global_pose_stamped; + tf2::toMsg(tf2::Transform::getIdentity(), global_pose_stamped.pose); + geometry_msgs::PoseStamped robot_pose; + tf2::toMsg(tf2::Transform::getIdentity(), robot_pose.pose); + robot_pose.header.frame_id = base_frame_id; + robot_pose.header.stamp = ros::Time(); + ros::Time current_time = ros::Time::now(); // save time for checking tf delay later + + // get the global pose of the robot + try + { + // use current time if possible (makes sure it's not in the future) + if (loc_base::LocBase::tf_->canTransform(map_frame, base_frame_id, current_time)) + { + geometry_msgs::TransformStamped transform = loc_base::LocBase::tf_->lookupTransform(map_frame, base_frame_id, current_time); + tf2::doTransform(robot_pose, global_pose_stamped, transform); + } + // use the latest otherwise + else + { + loc_base::LocBase::tf_->transform(robot_pose, global_pose_stamped, map_frame); + } + global_pose = global_pose_stamped; + } + catch (tf2::LookupException& ex) + { + return false; + } + catch (tf2::ConnectivityException& ex) + { + return false; + } + catch (tf2::ExtrapolationException& ex) + { + return false; + } + if (!global_pose_stamped.header.stamp.isZero() && current_time.toSec() - global_pose_stamped.header.stamp.toSec() > transform_tolerance) + { + return false; + } + return true; +} + +void loc_base::LocBase::initalPose(geometry_msgs::PoseWithCovariance init_pose) +{ + geometry_msgs::PoseWithCovarianceStamped pose; + pose.header.frame_id = loc_base::LocBase::global_frame_id_; + pose.header.stamp = ros::Time::now(); + pose.pose = init_pose; + this->init_pub_.publish(pose); +} + +// register this planner as a LocalPlanner plugin +PLUGINLIB_EXPORT_CLASS(loc_base::LocBase, loc_core::BaseLocalization) \ No newline at end of file diff --git a/Localizations/Packages/loc_base/src/loc_base_node.cpp b/Localizations/Packages/loc_base/src/loc_base_node.cpp new file mode 100644 index 0000000..34f8326 --- /dev/null +++ b/Localizations/Packages/loc_base/src/loc_base_node.cpp @@ -0,0 +1,95 @@ +#include +#include +#include +#include + +// std::shared_ptr loc_base_ptr = nullptr; +std::shared_ptr loc_base_ptr = nullptr; +static loc_core::BaseLocalization* loc_base_; + +void callBack(const std_msgs::UInt16& msg) +{ + std::stringstream directories; + if (loc_base_) + { + switch (msg.data) + { + case 1: + loc_base_->startMapping(); + /* code */ + break; + case 2: + loc_base_->stopMapping(); + /* code */ + break; + case 3: + loc_base_->listMapFiles(directories); + ROS_INFO_STREAM(directories.str()); + break; + default: + break; + } + } +} + +void createMapCallBack(const std_msgs::String& msg) +{ + if (loc_base_) + loc_base_->saveActivateMapFileName(msg.data); +} + +void changeMapCallBack(const std_msgs::String& msg) +{ + if (loc_base_) + loc_base_->changeStaticMap(msg.data); +} + +int main(int argc, char** argv) { + ros::init(argc, argv, "loc_base_node"); + ros::start(); + ros::NodeHandle nh(ros::this_node::getName()); + + std::shared_ptr tf = std::make_shared(); + tf2_ros::TransformListener tf2(*tf); + // loc_base_ptr = std::make_shared(); + pluginlib::ClassLoader loc_base_loader_("loc_core", "loc_core::BaseLocalization"); + + std::string obj_name("loc_base::LocBase"); + if (tf == nullptr) + throw std::runtime_error("tf2_ros::Buffer object is null"); + try + { + loc_base_ptr = loc_base_loader_.createUniqueInstance(obj_name); + loc_base_ptr->initialize(nh, tf); + loc_base_ = loc_base_ptr.get(); + } + catch (pluginlib::PluginlibException& ex) + { + ROS_FATAL("Failed to create the %s, are you sure it is properly registered and that the containing library is built? Exception: %s", obj_name.c_str(), ex.what()); + exit(1); + } + catch (std::exception& e) + { + ROS_FATAL("Failed to create the %s, are you sure it is properly registered and that the containing library is built? Exception: %s", obj_name.c_str(), e.what()); + exit(1); + } + + ros::Subscriber sub = nh.subscribe("mode", 1000, callBack); + ros::Subscriber save_map_sub = nh.subscribe("save_map", 1000, createMapCallBack); + ros::Subscriber change_map_sub = nh.subscribe("change_map", 1000, changeMapCallBack); + ros::Publisher state = nh.advertise("state", 1); + ros::Rate rate(3); + + while (ros::ok()) + { + std_msgs::UInt16 state_msg; + state_msg.data = loc_base_->getState(); + state.publish(state_msg); + rate.sleep(); + ros::spinOnce(); + } + + ros::spin(); + loc_base_ptr.reset(); + return(0); +} diff --git a/Localizations/Packages/loc_base/src/loc_base_util.cpp b/Localizations/Packages/loc_base/src/loc_base_util.cpp new file mode 100644 index 0000000..9620708 --- /dev/null +++ b/Localizations/Packages/loc_base/src/loc_base_util.cpp @@ -0,0 +1,169 @@ +#include +#include +#include +#include +#include +#include "loc_base/loc_base_util.h" + +std::string loc_base::LocBaseUtil::filepathFromPackage(const std::string& package, const std::string& filename) +{ + std::string filepath = filename; + + if (!package.empty()) + { + // Get the package path using ROS package utility + std::string pkg_path = ros::package::getPath(package); + + // Log package path (info level, could be changed to ROS_DEBUG for less verbosity) + // ROS_INFO_STREAM("Package path: " << pkg_path); + + if (!pkg_path.empty()) + { + // Construct the full file path using Boost Filesystem (or std::filesystem) + filepath = (boost::filesystem::path(pkg_path) / filename).make_preferred().native(); + // For C++17 and later, you can use the following line instead: + // filepath = (std::filesystem::path(pkg_path) / filename).string(); + + // Check if the file exists + if (!boost::filesystem::exists(filepath)) // Or std::filesystem::exists(filepath) for C++17+ + { + // If file does not exist, throw an exception + std::string error_message = "File does not exist at: " + filepath; + // ROS_ERROR_STREAM(error_message); + throw std::runtime_error(error_message); + } + } + else + { + // If package path is empty, throw an exception + std::string error_message = "Failed to get path for package '" + package + "'."; + // ROS_ERROR_STREAM(error_message); + throw std::runtime_error(error_message); + } + } + + return filepath; +} + + + +bool loc_base::LocBaseUtil::loadActivateMapFileName(const std::string& filename, std::string& activated_map_filename) +{ + // Check if saved path file is existed + std::ifstream inputFile; + inputFile.open(filename); + std::stringstream buffer; + if (inputFile) + { + buffer << inputFile.rdbuf(); + inputFile.close(); + if (buffer.str().empty()) + return false; + } + else return false; + activated_map_filename = buffer.str(); + + return true; +} + +bool loc_base::LocBaseUtil::saveActivateMapFileName(const std::string& filename, const std::string& activated_map_filename) +{ + bool result = false; + // Check if saved path file is existed + std::ofstream outputFile(filename, std::ios::out); + if (outputFile.is_open()) { + outputFile << activated_map_filename; + outputFile.close(); + result = true; + } + else + result = false; + return result; +} + +bool loc_base::LocBaseUtil::convertPgmToPng(const std::string& pgmFileName, const std::string& pngFileName) +{ + std::string cmd; + int retVal; + cmd = "convert " + pgmFileName + " " + pngFileName; + retVal = system(cmd.c_str()); + if (retVal != 0) + return false; + + return true; +} + +bool loc_base::LocBaseUtil::writeGridMap(const std::string& filename) +{ + std::string pgmFileName = filename + ".pgm"; + std::string pngFileName = filename + ".png"; + std::string yamlFileName = filename + ".yaml"; + // bool is_SaveStateAsFiles = SaveStateAsFiles(filename); // cái này tham khảo https://github.com/ros-planning/navigation/blob/ac41d2480c4cf1602daf39a6e9629142731d92b0/map_server/src/map_saver.cpp#L114 + // if(is_SaveStateAsFiles) + // { + // if(convertPgmToPng(pgmFileName, pngFileName)) + // return true; + // } + return false; +} + +bool loc_base::LocBaseUtil::checkIfMapFilesExist(const std::string& filename) +{ + + std::string SavedGridMapFileName = filename + ".png"; + std::string SavedMapInfoFileName = filename + ".yaml"; + + std::ifstream SavedGridMapFile(SavedGridMapFileName); + std::ifstream SavedMapInfoFile(SavedMapInfoFileName); + if (SavedGridMapFile.good() && SavedMapInfoFile.good()) + { + ROS_INFO("map file: %s is found", filename.c_str()); + return true; + } + else ROS_WARN("map file: %s is not found", filename.c_str()); + return false; +} + +bool loc_base::LocBaseUtil::replaceMapDirectory(const std::string& mapDirectory) +{ + int retVal_1 = -1; + int retVal_2 = -1; + std::string removeCmd = "rm -rf " + mapDirectory; + retVal_1 = system(removeCmd.c_str()); + if (retVal_1 == 0) + { + std::string createCmd = "mkdir " + mapDirectory; + retVal_2 = system(createCmd.c_str()); + } + if (retVal_2 == 0) + { + return true; + } + else + { + return false; + } + return false; +} + +std::stringstream loc_base::LocBaseUtil::listMapFiles(const std::string& path) +{ + std::stringstream directories; + try + { + for (const auto& entry : boost::filesystem::directory_iterator(path)) { + if(entry.path().extension() == ".txt") + continue; + directories << entry.path().filename() << std::endl; + } + } + catch (const boost::system::system_error& e) { + // Catching boost::system::system_error (which inherits from boost::exception) + ROS_ERROR_STREAM("Error reading directory: " << e.what()); + } + catch (const boost::exception& e) { + // Catching general boost exceptions + ROS_ERROR_STREAM("Boost exception: List Files failed"); + } + return directories; +} \ No newline at end of file diff --git a/Localizations/Packages/robot_localization/.github/ISSUE_TEMPLATE/bug_report.md b/Localizations/Packages/robot_localization/.github/ISSUE_TEMPLATE/bug_report.md new file mode 100644 index 0000000..85d1156 --- /dev/null +++ b/Localizations/Packages/robot_localization/.github/ISSUE_TEMPLATE/bug_report.md @@ -0,0 +1,30 @@ +--- +name: Bug report +about: For filing confirmed bugs +title: '' +labels: bug +assignees: ayrton04 + +--- + + + +## Bug report + +**Required Info:** + +- Operating System: + - +- Installation type: + - +- Version or commit hash: + - + +#### Steps to reproduce issue + + +#### Expected behavior + +#### Actual behavior + +#### Additional information diff --git a/Localizations/Packages/robot_localization/.github/ISSUE_TEMPLATE/feature_request.md b/Localizations/Packages/robot_localization/.github/ISSUE_TEMPLATE/feature_request.md new file mode 100644 index 0000000..00937ff --- /dev/null +++ b/Localizations/Packages/robot_localization/.github/ISSUE_TEMPLATE/feature_request.md @@ -0,0 +1,16 @@ +--- +name: Feature request +about: For requesting new features +title: '' +labels: feature request +assignees: ayrton04 + +--- + +## Feature request + +#### Feature description + + +#### Implementation considerations + diff --git a/Localizations/Packages/robot_localization/.gitignore b/Localizations/Packages/robot_localization/.gitignore new file mode 100644 index 0000000..581b015 --- /dev/null +++ b/Localizations/Packages/robot_localization/.gitignore @@ -0,0 +1,4 @@ +doc/html +*.cproject +*.project +*.settings diff --git a/Localizations/Packages/robot_localization/CHANGELOG.rst b/Localizations/Packages/robot_localization/CHANGELOG.rst new file mode 100644 index 0000000..e028973 --- /dev/null +++ b/Localizations/Packages/robot_localization/CHANGELOG.rst @@ -0,0 +1,311 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package robot_localization +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +2.7.4 (2022-07-27) +------------------ +* Fix odometry and acceleration processing pipeline (`#753 `_) +* Use default CXX, need to specify boost::placeholders::_1 instead of just _1 (`#750 `_) +* Fix odometry msgs with child frame other than baseLink (`#728 `_) +* update documentation (`#723 `_) +* Fix unused-parameter warning (`#721 `_) +* Fix tf lookup timestamp during map->odom publication (`#719 `_) +* UKF cleanup (`#671 `_) +* Added geographiclib to catkin exported depends (`#709 `_) +* Make the navsat tf frame name parametric (`#699 `_) +* Propagate the suppression of tf warnings (`#705 `_) +* Fix typo in base_link_frame_output name. (`#701 `_) +* Contributors: AR Dabbour, Carlos Agüero, Haoguang Yang, J.P.S, Joshua Owen, Leonardo Hemerly, Lucas Walter, Marcus Scheunemann, Stephen Williams, Tom Moore + +2.7.3 (2021-07-23) +------------------ +* Prevent node from crashing on invalid UTM zone, but throw ROS_ERROR to notify user (`#682 `_) +* changed geographiclib to tag (`#684 `_) +* Small formatting and content change (`#677 `_) +* Fixed state transition for sigma points. The state transition function for each sigma point now uses its sigma point's posteriori state instead of always the posteriori state. (`#628 `_) + Co-authored-by: jola6897 +* Contributors: John Stechschulte, Jonas, MCFurry, Vivek Mhatre + +2.7.2 (2021-06-03) +------------------ +* Also test for gamma conversion (`#647 `_) +* fix: Transform gravitation vector to IMU frame before removing acceleration (`#639 `_) +* Fixed a typo in validate filter output error message. (`#646 `_) +* Stick to the global utm_zone\_ when transforming gps to UTM (`#627 `_) + * Stick to the global utm_zone\_ when transforming gps to UTM +* UTM conversions using geographiclib (`#626 `_) + * Use GeographicLib for UTMtoLL conversions +* SHARED linking for Geographiclib (`#624 `_) + * remove GeographicLib specific linking option +* Fixing lat-long to UTM conversion (`#620 `_) + Thanks again for the report. I'll get this into `noetic-devel` and the relevant ROS2 branches. +* Removing xmlrpcpp dependency +* yaml-cpp using find_package as backup (`#618 `_) +* Add ${GeographicLib_LIBRARIES} to navsat_transform (`#617 `_) +* Contributors: Achmad Fathoni, Leonardo Hemerly, Paul Verhoeckx, Tim Clephas, Tobias Fischer, Tom Moore + +2.7.0 (2020-12-17) +------------------ +* Making repeated state publication optional (`#595 `_) +* Fix sign error in dFY_dP part of transferFunctionJacobian\_ (`#592 `_) +* Fix typo in navsat_transform_node.rst (`#588 `_) +* Fix issue caused by starting on uneven terrain (`#582 `_) +* Local Cartesian Option (`#575 `_) +* Fix frame id of imu in differential mode, closes `#482 `_. (`#522 `_) +* navsat_transform diagram to address `#550 `_ (`#570 `_) +* Increasing the minimum CMake version (`#573 `_) +* Contributors: Aleksander Bojda, David Jensen, James Baxter, Jeffrey Kane Johnson, Mabel Zhang, Ronald Ensing, Tom Moore + +2.6.8 (2020-06-03) +------------------ +* Adding conditional build dependencies (`#572 `_) +* Contributors: Tom Moore + +2.6.7 (2020-06-01) +------------------ +* Parameterizing transform failure warnings +* [melodic] Fix Windows build break. (`#557 `_) +* Contributors: Sean Yen, Tom Moore, florianspy + +2.6.5 (2019-08-08) +------------------ +* fix: wall time used when `use_sim_time` is true +* Created service for converting to / from lat long +* Fix bug with tf_prefix +* Adding new contribution to doc +* Add missing undocumented params +* Update wiki location +* Contributors: Andrew Grindstaff, Axel Mousset, Charles Brian Quinn, Oswin So, Tom Moore + +2.6.4 (2019-02-15) +------------------ +* Meridian convergence adjustment added to navsat_transform. +* Documentation changes +* Add broadcast_utm_transform_as_parent_frame +* Enable build optimisations if no build type configured. +* Contributors: G.A. vd. Hoorn, Pavlo Kolomiiets, diasdm + +2.6.3 (2019-01-14) +------------------ +* Rename odomBaseLinkTrans to baseLinkOdomTrans + Adhere to the naming convention Trans used for worldBaseLinkTrans and mapOdomTrans. +* Add const& to catch values to prevent the error: catching polymorphic type ‘class tf2::TransformException’ by value + And ZoneNumber by 0x3fU to prevent error: directive output may be truncated writing between 1 and 11 bytes into a region of size 4 +* Enabling the user to override the output child_frame_id +* Fixing Euler body-to-world transformations +* Whitespace +* fixing no datum service in melodic +* Contributors: Alexis schad, Matthew Jones, Tom Moore, thallerod + +2.6.2 (2018-10-25) +------------------ +* Fixing tests +* Contributors: Tom Moore + +2.6.1 (2018-10-25) +------------------ +* Adding more output for measurement history failures +* Adding filter processing toggle service +* Waiting for valid ROS time before starting navsat_transform_node +* Contributors: Tom Moore, stevemacenski + +2.6.0 (2018-07-27) +------------------ +* Moving to C++14, adding error flags, and fixing all warnings +* Contributors: Tom Moore + +2.5.2 (2018-04-11) +------------------ +* Add published accel topic to documentation +* adding log statements for nans in the invertable matrix +* Fixing issue with potential seg fault +* Contributors: Oleg Kalachev, Tom Moore, stevemacenski + +2.5.1 (2018-01-03) +------------------ +* Fixing CMakeLists +* Contributors: Tom Moore + +2.5.0 (2017-12-15) +------------------ +* Fixing datum precision +* Fixing timing variable +* Fixing state history reversion +* Fixing critical bug with dynamic process noise covariance +* Fix typo in reading Mahalanobis thresholds. +* Zero out rotation in GPS to base_link transform +* Update xmlrpcpp includes for Indigo support +* Removing lastUpdateTime +* Fixing timestamps in map->odom transform +* Simplify enabledAtStartup logic +* Add std_srvs dependency +* Add enabling service +* Ensure all raw sensor input orientations are normalized even if messages are not +* Install params directory. +* Add robot localization estimator +* Adding nodelet support +* Contributors: Jacob Perron, Jacob Seibert, Jiri Hubacek, Mike Purvis, Miquel Massot, Pavlo Kolomiiets, Rein Appeldoorn, Rokus Ottervanger, Simon Gene Gottlieb, Tom Moore, stevemacenski + +2.4.0 (2017-06-12) +------------------ +* Updated documentation +* Added reset_on_time_jump option +* Added feature to optionally publish utm frame as parent in navsat_transform_node +* Moved global callback queue reset +* Added initial_state parameter and documentation +* Fixed ac/deceleration gains default logic +* Added gravity parameter +* Added delay and throttle if tf lookup fails +* Fixed UKF IMUTwistBasicIO test +* Added transform_timeout parameter +* Set gps_odom timestamp before tf2 lookuptransform +* Removed non-portable sincos calls +* Simplified logic to account for correlated error +* Added dynamic process noise covariance calculation +* Fixed catkin_package Eigen warning +* Added optional publication of acceleration state +* Contributors: Brian Gerkey, Enrique Fernandez, Jochen Sprickerhof, Rein Appeldoorn, Simon Gene Gottlieb, Tom Moore + +2.3.1 (2016-10-27) +------------------ +* Adding gitignore +* Adding remaining wiki pages +* Adding config and prep pages +* Adding navsat_transform_node documentation +* use_odometry_yaw fix for n_t_n +* Fixing issue with manual pose reset when history is not empty +* Getting inverse transform when looking up robot's pose. +* Sphinx documentation +* Removing forward slashes from navsat_transform input topics for template launch file +* Adding example launch and parameter files for a two-level EKF setup with navsat_transform_node +* Adding yaml file for navsat_transform_node, and moving parameter documentation to it. +* Updating EKF and UKF parameter templates with usage comments +* Contributors: Tom Moore, asimay + +2.3.0 (2016-07-28) +------------------ +* Fixed issues with datum usage and frame_ids +* Fixed comment for wait_for_datum +* Fixing issue with non-zero navsat sensor orientation offsets +* Fixing issue with base_link->gps transform wrecking the 'true' UTM position computation +* Using correct covariance for filtered GPS +* Fixed unitialized odometry covariance bug +* Added filter history and measurement queue behavior +* Changing output timestamp to more accurately use the time stamp of the most recently-processed measurement +* Added TcpNoDelay() +* Added parameter to make transform publishing optional +* Fixed differential handling for pose data so that it doesn't care about the message's frame_id +* Updated UKF config and launch +* Added a test case for the timestamp diagnostics +* Added reporting of bad timestamps via diagnostics +* Updated tests to match new method signatures +* Added control term +* Added smoothing capability for delayed measurements +* Making variables in navsat_transform conform to ROS coding standards +* Contributors: Adel Fakih, Ivor Wanders, Marc Essinger, Tobias Tueylue, Tom Moore + +2.2.3 (2016-04-24) +------------------ +* Cleaning up callback data structure and callbacks and updating doxygen comments in headers +* Removing MessageFilters +* Removing deprecated parameters +* Adding the ability to handle GPS offsets from the vehicle's origin +* Cleaning up navsat_transform.h +* Making variables in navsat_transform conform to ROS coding standards + +2.2.2 (2016-02-04) +------------------ +* Updating trig functions to use sincos for efficiency +* Updating licensing information and adding Eigen MPL-only flag +* Added state to imu frame transformation +* Using state orientation if imu orientation is missing +* Manually adding second spin for odometry and IMU data that is passed to message filters +* Reducing delay between measurement reception and filter output +* Zero altitute in intital transform too, when zero altitude param is set +* Fixing regression with conversion back to GPS coordinates +* Switched cropping of orientation data in inovationSubset with mahalanobis check to prevent excluding measurements with orientations bigger/smaller than ± PI +* Fix Jacobian for EKF. +* Removing warning about orientation variables when only their velocities are measured +* Checking for -1 in IMU covariances and ignoring relevant message data +* roslint and catkin_lint applied +* Adding base_link to datum specification, and fixing bug with order of measurement handling when a datum is specified. Also added check to make sure IMU data is transformable before using it. +* Contributors: Adnan Ademovic, Jit Ray Chowdhury, Philipp Tscholl, Tom Moore, ayrton04, kphil + +2.2.1 (2015-05-27) +------------------ +* Fixed handling of IMU data w.r.t. differential mode and relative mode + +2.2.0 (2015-05-22) +------------------ +* Added tf2-friendly tf_prefix appending +* Corrected for IMU orientation in navsat_transform +* Fixed issue with out-of-order measurements and pose resets +* Nodes now assume ENU standard for yaw data +* Removed gps_common dependency +* Adding option to navsat_transform_node that enables the use of the heading from the odometry message instead of an IMU. +* Changed frame_id used in setPoseCallback to be the world_frame +* Optimized Eigen arithmetic for signficiant performance boost +* Migrated to tf2 +* Code refactoring and reorganization +* Removed roll and pitch from navsat_transform calculations +* Fixed transform for IMU data to better support mounting IMUs in non-standard orientations +* Added feature to navsat_transform_node whereby filtered odometry data can be coverted back into navsat data +* Added a parameter to allow future dating the world_frame->base_link_frame transform. +* Removed deprecated differential setting handler +* Added relative mode +* Updated and improved tests +* Fixing source frame_id in pose data handling +* Added initial covariance parameter +* Fixed bug in covariance copyinh +* Added parameters for topic queue sizes +* Improved motion model's handling of angular velocities when robot has non-zero roll and pitch +* Changed the way differential measurements are handled +* Added diagnostics + +2.1.7 (2015-01-05) +------------------ +* Added some checks to eliminate unnecessary callbacks +* Updated launch file templates +* Added measurement outlier rejection +* Added failure callbacks for tf message filters +* Added optional broadcast of world_frame->utm transform for navsat_transform_node +* Bug fixes for differential mode and handling of Z acceleration in 2D mode + +2.1.6 (2014-11-06) +------------------ +* Added unscented Kalman filter (UKF) localization node +* Fixed map->odom tf calculation +* Acceleration data from IMUs is now used in computing the state estimate +* Added 2D mode + +2.1.5 (2014-10-07) +------------------ +* Changed initial estimate error covariance to be much smaller +* Fixed some debug output +* Added test suite +* Better compliance with REP-105 +* Fixed differential measurement handling +* Implemented message filters +* Added navsat_transform_node + +2.1.4 (2014-08-22) +------------------ +* Adding utm_transform_node to install targets + +2.1.3 (2014-06-22) +------------------ +* Some changes to ease GPS integration +* Addition of differential integration of pose data +* Some documentation cleanup +* Added UTM transform node and launch file +* Bug fixes + +2.1.2 (2014-04-11) +------------------ +* Updated covariance correction formulation to "Joseph form" to improve filter stability. +* Implemented new versioning scheme. + +2.1.1 (2014-04-11) +------------------ +* Added cmake_modules dependency for Eigen support, and added include to silence boost::signals warning from tf include + diff --git a/Localizations/Packages/robot_localization/CMakeLists.txt b/Localizations/Packages/robot_localization/CMakeLists.txt new file mode 100644 index 0000000..cc7f334 --- /dev/null +++ b/Localizations/Packages/robot_localization/CMakeLists.txt @@ -0,0 +1,354 @@ +cmake_minimum_required(VERSION 3.0.2) +project(robot_localization) + +if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE) + message("${PROJECT_NAME}: You did not request a specific build type: selecting 'RelWithDebInfo'.") + set(CMAKE_BUILD_TYPE RelWithDebInfo) +endif() + +find_package(catkin REQUIRED COMPONENTS + angles + diagnostic_msgs + diagnostic_updater + eigen_conversions + geographic_msgs + geometry_msgs + message_filters + message_generation + nav_msgs + nodelet + roscpp + roslint + sensor_msgs + std_msgs + std_srvs + tf2 + tf2_geometry_msgs + tf2_ros) + +find_package(PkgConfig REQUIRED) +pkg_check_modules(YAML_CPP yaml-cpp) +if(NOT YAML_CPP_FOUND) + find_package(yaml-cpp REQUIRED) +endif() + + +# Geographiclib installs FindGeographicLib.cmake to this non-standard location +set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "/usr/share/cmake/geographiclib/") +find_package(GeographicLib REQUIRED) + +# Attempt to find Eigen using its own CMake module. +# If that fails, fall back to cmake_modules package. +find_package(Eigen3) +set(EIGEN_PACKAGE EIGEN3) +if(NOT EIGEN3_FOUND) + find_package(cmake_modules REQUIRED) + find_package(Eigen REQUIRED) + set(EIGEN3_INCLUDE_DIRS ${EIGEN_INCLUDE_DIRS}) + set(EIGEN3_LIBRARIES ${EIGEN_LIBRARIES}) + set(EIGEN_PACKAGE Eigen) +endif() + +if(NOT MSVC) + set_directory_properties(PROPERTIES COMPILE_OPTIONS "-Wall;-Werror") +endif() +add_definitions(-DEIGEN_NO_DEBUG -DEIGEN_MPL2_ONLY) + +set(ROSLINT_CPP_OPTS "--filter=-build/c++11,-runtime/references") +roslint_cpp() + +################################### +## catkin specific configuration ## +################################### +add_service_files( + FILES + GetState.srv + SetDatum.srv + SetPose.srv + SetUTMZone.srv + ToggleFilterProcessing.srv + FromLL.srv + ToLL.srv +) + +generate_messages( + DEPENDENCIES + geographic_msgs + geometry_msgs + std_msgs +) + +catkin_package( + INCLUDE_DIRS + include + LIBRARIES + ekf + ekf_localization_nodelet + filter_base + filter_utilities + navsat_transform + navsat_transform_nodelet + ros_filter + ros_filter_utilities + robot_localization_estimator + ros_robot_localization_listener + ukf + ukf_localization_nodelet + CATKIN_DEPENDS + angles + cmake_modules + diagnostic_msgs + diagnostic_updater + eigen_conversions + geographic_msgs + geometry_msgs + message_filters + message_runtime + nav_msgs + roscpp + sensor_msgs + std_msgs + std_srvs + tf2 + tf2_geometry_msgs + tf2_ros + DEPENDS + ${EIGEN_PACKAGE} + GeographicLib + YAML_CPP +) + +########### +## Build ## +########### + +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIRS} + ${YAML_CPP_INCLUDE_DIRS}) + +link_directories(${YAML_CPP_LIBRARY_DIRS}) + +# Library definitions +add_library(filter_utilities src/filter_utilities.cpp) +add_library(filter_base src/filter_base.cpp) +add_library(ekf src/ekf.cpp) +add_library(ukf src/ukf.cpp) +add_library(robot_localization_estimator src/robot_localization_estimator.cpp) +add_library(ros_robot_localization_listener src/ros_robot_localization_listener.cpp) +add_library(ros_filter_utilities src/ros_filter_utilities.cpp) +add_library(ros_filter src/ros_filter.cpp) +add_library(navsat_transform src/navsat_transform.cpp) +add_library(ekf_localization_nodelet src/ekf_localization_nodelet.cpp) +add_library(ukf_localization_nodelet src/ukf_localization_nodelet.cpp) +add_library(navsat_transform_nodelet src/navsat_transform_nodelet.cpp) + +# Executables +add_executable(ekf_localization_node src/ekf_localization_node.cpp) +add_executable(ukf_localization_node src/ukf_localization_node.cpp) +add_executable(navsat_transform_node src/navsat_transform_node.cpp) +add_executable(robot_localization_listener_node src/robot_localization_listener_node.cpp) + +# Dependencies +add_dependencies(filter_base ${PROJECT_NAME}_gencpp) +add_dependencies(navsat_transform ${PROJECT_NAME}_gencpp) +add_dependencies(robot_localization_listener_node ${PROJECT_NAME}_gencpp) + +# Linking +target_link_libraries(ros_filter_utilities ${catkin_LIBRARIES} ${EIGEN3_LIBRARIES}) +target_link_libraries(filter_utilities ${catkin_LIBRARIES} ${EIGEN3_LIBRARIES}) +target_link_libraries(filter_base filter_utilities ${catkin_LIBRARIES} ${EIGEN3_LIBRARIES}) +target_link_libraries(ekf filter_base ${catkin_LIBRARIES} ${EIGEN3_LIBRARIES}) +target_link_libraries(ukf filter_base ${catkin_LIBRARIES} ${EIGEN3_LIBRARIES}) +target_link_libraries(ros_filter ekf ukf ros_filter_utilities ${catkin_LIBRARIES} ${EIGEN3_LIBRARIES}) +target_link_libraries(robot_localization_estimator filter_utilities filter_base ekf ukf ${EIGEN3_LIBRARIES}) +target_link_libraries(ros_robot_localization_listener robot_localization_estimator ros_filter_utilities + ${catkin_LIBRARIES} ${EIGEN3_LIBRARIES} ${YAML_CPP_LIBRARIES}) +target_link_libraries(robot_localization_listener_node ros_robot_localization_listener ${catkin_LIBRARIES}) +target_link_libraries(ekf_localization_node ros_filter ${catkin_LIBRARIES}) +target_link_libraries(ekf_localization_nodelet ros_filter ${catkin_LIBRARIES}) +target_link_libraries(ukf_localization_node ros_filter ${catkin_LIBRARIES}) +target_link_libraries(ukf_localization_nodelet ros_filter ${catkin_LIBRARIES}) +target_link_libraries(navsat_transform filter_utilities ros_filter_utilities ${catkin_LIBRARIES} ${EIGEN3_LIBRARIES} ${GeographicLib_LIBRARIES}) +target_link_libraries(navsat_transform_node navsat_transform ${catkin_LIBRARIES} ${GeographicLib_LIBRARIES}) +target_link_libraries(navsat_transform_nodelet navsat_transform ${catkin_LIBRARIES} ${GeographicLib_LIBRARIES}) + +############# +## Install ## +############# + +## Mark executables and/or libraries for installation +install(TARGETS + ekf + ekf_localization_nodelet + filter_base + filter_utilities + navsat_transform + navsat_transform_nodelet + ros_filter + ros_filter_utilities + robot_localization_estimator + ros_robot_localization_listener + ukf + ukf_localization_nodelet + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) + +install(TARGETS + ekf_localization_node + navsat_transform_node + robot_localization_listener_node + ukf_localization_node + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) + +## Mark cpp header files for installation +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE) + +install(DIRECTORY launch/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch + FILES_MATCHING PATTERN "*.launch") + +install(DIRECTORY params/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/params) + +install(FILES + LICENSE + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) +install(FILES nodelet_plugins.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +############# +## Testing ## +############# + +if (CATKIN_ENABLE_TESTING) + + roslint_add_test() + + # Not really necessary, but it will cause the build to fail if it's + # missing, rather than failing once the tests are being executed + find_package(rosbag REQUIRED) + find_package(rostest REQUIRED) + + #### FILTER BASE TESTS #### + catkin_add_gtest(filter_base-test test/test_filter_base.cpp) + target_link_libraries(filter_base-test filter_base ${catkin_LIBRARIES}) + + # This test uses ekf_localization node for convenience. + add_rostest_gtest(test_filter_base_diagnostics_timestamps + test/test_filter_base_diagnostics_timestamps.test + test/test_filter_base_diagnostics_timestamps.cpp) + target_link_libraries(test_filter_base_diagnostics_timestamps ${catkin_LIBRARIES} ${rostest_LIBRARIES}) + add_dependencies(test_filter_base_diagnostics_timestamps ekf_localization_node) + + #### EKF TESTS ##### + add_rostest_gtest(test_ekf + test/test_ekf.test + test/test_ekf.cpp) + target_link_libraries(test_ekf ros_filter ekf ${catkin_LIBRARIES} ${rostest_LIBRARIES}) + + add_rostest_gtest(test_ekf_localization_node_interfaces + test/test_ekf_localization_node_interfaces.test + test/test_ekf_localization_node_interfaces.cpp) + target_link_libraries(test_ekf_localization_node_interfaces ros_filter ekf ${catkin_LIBRARIES} ${rostest_LIBRARIES}) + add_dependencies(test_ekf_localization_node_interfaces ekf_localization_node) + + add_rostest_gtest(test_ekf_localization_node_bag1 + test/test_ekf_localization_node_bag1.test + test/test_localization_node_bag_pose_tester.cpp) + target_link_libraries(test_ekf_localization_node_bag1 ${catkin_LIBRARIES} ${rostest_LIBRARIES}) + add_dependencies(test_ekf_localization_node_bag1 ekf_localization_node) + + add_rostest_gtest(test_ekf_localization_node_bag2 + test/test_ekf_localization_node_bag2.test + test/test_localization_node_bag_pose_tester.cpp) + target_link_libraries(test_ekf_localization_node_bag2 ${catkin_LIBRARIES} ${rostest_LIBRARIES}) + add_dependencies(test_ekf_localization_node_bag2 ekf_localization_node) + + add_rostest_gtest(test_ekf_localization_node_bag3 + test/test_ekf_localization_node_bag3.test + test/test_localization_node_bag_pose_tester.cpp) + target_link_libraries(test_ekf_localization_node_bag3 ${catkin_LIBRARIES} ${rostest_LIBRARIES}) + add_dependencies(test_ekf_localization_node_bag3 ekf_localization_node) + + add_rostest_gtest(test_ekf_localization_nodelet_bag1 + test/test_ekf_localization_nodelet_bag1.test + test/test_localization_node_bag_pose_tester.cpp) + target_link_libraries(test_ekf_localization_nodelet_bag1 ${catkin_LIBRARIES} ${rostest_LIBRARIES}) + add_dependencies(test_ekf_localization_nodelet_bag1 ekf_localization_nodelet) + + #### UKF TESTS ##### + add_rostest_gtest(test_ukf + test/test_ukf.test + test/test_ukf.cpp) + target_link_libraries(test_ukf ros_filter ukf ${catkin_LIBRARIES} ${rostest_LIBRARIES}) + + add_rostest_gtest(test_ukf_localization_node_interfaces + test/test_ukf_localization_node_interfaces.test + test/test_ukf_localization_node_interfaces.cpp) + target_link_libraries(test_ukf_localization_node_interfaces ${catkin_LIBRARIES} ${rostest_LIBRARIES}) + add_dependencies(test_ukf_localization_node_interfaces ukf_localization_node) + + add_rostest_gtest(test_ukf_localization_node_bag1 + test/test_ukf_localization_node_bag1.test + test/test_localization_node_bag_pose_tester.cpp) + target_link_libraries(test_ukf_localization_node_bag1 ${catkin_LIBRARIES} ${rostest_LIBRARIES}) + add_dependencies(test_ukf_localization_node_bag1 ukf_localization_node) + + add_rostest_gtest(test_ukf_localization_node_bag2 + test/test_ukf_localization_node_bag2.test + test/test_localization_node_bag_pose_tester.cpp) + target_link_libraries(test_ukf_localization_node_bag2 ${catkin_LIBRARIES} ${rostest_LIBRARIES}) + add_dependencies(test_ukf_localization_node_bag2 ukf_localization_node) + + add_rostest_gtest(test_ukf_localization_node_bag3 + test/test_ukf_localization_node_bag3.test + test/test_localization_node_bag_pose_tester.cpp) + target_link_libraries(test_ukf_localization_node_bag3 ${catkin_LIBRARIES} ${rostest_LIBRARIES}) + add_dependencies(test_ukf_localization_node_bag3 ukf_localization_node) + + add_rostest_gtest(test_ukf_localization_nodelet_bag1 + test/test_ukf_localization_nodelet_bag1.test + test/test_localization_node_bag_pose_tester.cpp) + target_link_libraries(test_ukf_localization_nodelet_bag1 ${catkin_LIBRARIES} ${rostest_LIBRARIES}) + add_dependencies(test_ukf_localization_nodelet_bag1 ukf_localization_nodelet) + + #### RLE/RLL TESTS ##### + add_rostest_gtest(test_robot_localization_estimator + test/test_robot_localization_estimator.test + test/test_robot_localization_estimator.cpp) + target_link_libraries(test_robot_localization_estimator + robot_localization_estimator + ${catkin_LIBRARIES} + ${rostest_LIBRARIES}) + + add_executable(test_ros_robot_localization_listener_publisher test/test_ros_robot_localization_listener_publisher.cpp) + target_link_libraries(test_ros_robot_localization_listener_publisher + ${catkin_LIBRARIES}) + + add_rostest_gtest(test_ros_robot_localization_listener + test/test_ros_robot_localization_listener.test + test/test_ros_robot_localization_listener.cpp) + target_link_libraries(test_ros_robot_localization_listener + ros_robot_localization_listener + ${catkin_LIBRARIES} + ${rostest_LIBRARIES}) + + #### NAVSAT CONVERSION TESTS #### + catkin_add_gtest(navsat_conversions-test test/test_navsat_conversions.cpp) + target_link_libraries(navsat_conversions-test navsat_transform ${catkin_LIBRARIES}) + + #### NAVSAT TRANSFORM TESTS #### + add_rostest_gtest(test_navsat_transform + test/test_navsat_transform.test + test/test_navsat_transform.cpp) + target_link_libraries(test_navsat_transform ${catkin_LIBRARIES} ${rostest_LIBRARIES}) + +endif() diff --git a/Localizations/Packages/robot_localization/LICENSE b/Localizations/Packages/robot_localization/LICENSE new file mode 100644 index 0000000..45521a4 --- /dev/null +++ b/Localizations/Packages/robot_localization/LICENSE @@ -0,0 +1,68 @@ +All code within the robot_localization package that was developed by Charles +River Analytics is released under the BSD license: + +Copyright (c) 2014, 2015, 2016, Charles River Analytics, Inc. +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the {organization} nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +========================================================================== + +The robot_localization package also makes use of code written by The University +of Texas at Austin. This code is released under a modified BSD license: + +Copyright (C) 2010 Austin Robot Technology, and others +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions +are met: + +* Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above + copyright notice, this list of conditions and the following + disclaimer in the documentation and/or other materials provided + with the distribution. + +* Neither the names of the University of Texas at Austin, nor + Austin Robot Technology, nor the names of other contributors may + be used to endorse or promote products derived from this + software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +POSSIBILITY OF SUCH DAMAGE. diff --git a/Localizations/Packages/robot_localization/README.md b/Localizations/Packages/robot_localization/README.md new file mode 100644 index 0000000..2220d2f --- /dev/null +++ b/Localizations/Packages/robot_localization/README.md @@ -0,0 +1,6 @@ +robot_localization +================== + +robot_localization is a package of nonlinear state estimation nodes. The package was developed by Charles River Analytics, Inc. + +Please see documentation here: http://docs.ros.org/noetic/api/robot_localization/html/index.html diff --git a/Localizations/Packages/robot_localization/doc/.templates/full_globaltoc.html b/Localizations/Packages/robot_localization/doc/.templates/full_globaltoc.html new file mode 100644 index 0000000..ba7226e --- /dev/null +++ b/Localizations/Packages/robot_localization/doc/.templates/full_globaltoc.html @@ -0,0 +1,10 @@ +

{{ _('Table Of Contents') }}

+{{ toctree(includehidden=True) }} + +

{{ _('Further Documentation') }}

+ + + diff --git a/Localizations/Packages/robot_localization/doc/CHANGELOG.rst b/Localizations/Packages/robot_localization/doc/CHANGELOG.rst new file mode 100644 index 0000000..e028973 --- /dev/null +++ b/Localizations/Packages/robot_localization/doc/CHANGELOG.rst @@ -0,0 +1,311 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package robot_localization +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +2.7.4 (2022-07-27) +------------------ +* Fix odometry and acceleration processing pipeline (`#753 `_) +* Use default CXX, need to specify boost::placeholders::_1 instead of just _1 (`#750 `_) +* Fix odometry msgs with child frame other than baseLink (`#728 `_) +* update documentation (`#723 `_) +* Fix unused-parameter warning (`#721 `_) +* Fix tf lookup timestamp during map->odom publication (`#719 `_) +* UKF cleanup (`#671 `_) +* Added geographiclib to catkin exported depends (`#709 `_) +* Make the navsat tf frame name parametric (`#699 `_) +* Propagate the suppression of tf warnings (`#705 `_) +* Fix typo in base_link_frame_output name. (`#701 `_) +* Contributors: AR Dabbour, Carlos Agüero, Haoguang Yang, J.P.S, Joshua Owen, Leonardo Hemerly, Lucas Walter, Marcus Scheunemann, Stephen Williams, Tom Moore + +2.7.3 (2021-07-23) +------------------ +* Prevent node from crashing on invalid UTM zone, but throw ROS_ERROR to notify user (`#682 `_) +* changed geographiclib to tag (`#684 `_) +* Small formatting and content change (`#677 `_) +* Fixed state transition for sigma points. The state transition function for each sigma point now uses its sigma point's posteriori state instead of always the posteriori state. (`#628 `_) + Co-authored-by: jola6897 +* Contributors: John Stechschulte, Jonas, MCFurry, Vivek Mhatre + +2.7.2 (2021-06-03) +------------------ +* Also test for gamma conversion (`#647 `_) +* fix: Transform gravitation vector to IMU frame before removing acceleration (`#639 `_) +* Fixed a typo in validate filter output error message. (`#646 `_) +* Stick to the global utm_zone\_ when transforming gps to UTM (`#627 `_) + * Stick to the global utm_zone\_ when transforming gps to UTM +* UTM conversions using geographiclib (`#626 `_) + * Use GeographicLib for UTMtoLL conversions +* SHARED linking for Geographiclib (`#624 `_) + * remove GeographicLib specific linking option +* Fixing lat-long to UTM conversion (`#620 `_) + Thanks again for the report. I'll get this into `noetic-devel` and the relevant ROS2 branches. +* Removing xmlrpcpp dependency +* yaml-cpp using find_package as backup (`#618 `_) +* Add ${GeographicLib_LIBRARIES} to navsat_transform (`#617 `_) +* Contributors: Achmad Fathoni, Leonardo Hemerly, Paul Verhoeckx, Tim Clephas, Tobias Fischer, Tom Moore + +2.7.0 (2020-12-17) +------------------ +* Making repeated state publication optional (`#595 `_) +* Fix sign error in dFY_dP part of transferFunctionJacobian\_ (`#592 `_) +* Fix typo in navsat_transform_node.rst (`#588 `_) +* Fix issue caused by starting on uneven terrain (`#582 `_) +* Local Cartesian Option (`#575 `_) +* Fix frame id of imu in differential mode, closes `#482 `_. (`#522 `_) +* navsat_transform diagram to address `#550 `_ (`#570 `_) +* Increasing the minimum CMake version (`#573 `_) +* Contributors: Aleksander Bojda, David Jensen, James Baxter, Jeffrey Kane Johnson, Mabel Zhang, Ronald Ensing, Tom Moore + +2.6.8 (2020-06-03) +------------------ +* Adding conditional build dependencies (`#572 `_) +* Contributors: Tom Moore + +2.6.7 (2020-06-01) +------------------ +* Parameterizing transform failure warnings +* [melodic] Fix Windows build break. (`#557 `_) +* Contributors: Sean Yen, Tom Moore, florianspy + +2.6.5 (2019-08-08) +------------------ +* fix: wall time used when `use_sim_time` is true +* Created service for converting to / from lat long +* Fix bug with tf_prefix +* Adding new contribution to doc +* Add missing undocumented params +* Update wiki location +* Contributors: Andrew Grindstaff, Axel Mousset, Charles Brian Quinn, Oswin So, Tom Moore + +2.6.4 (2019-02-15) +------------------ +* Meridian convergence adjustment added to navsat_transform. +* Documentation changes +* Add broadcast_utm_transform_as_parent_frame +* Enable build optimisations if no build type configured. +* Contributors: G.A. vd. Hoorn, Pavlo Kolomiiets, diasdm + +2.6.3 (2019-01-14) +------------------ +* Rename odomBaseLinkTrans to baseLinkOdomTrans + Adhere to the naming convention Trans used for worldBaseLinkTrans and mapOdomTrans. +* Add const& to catch values to prevent the error: catching polymorphic type ‘class tf2::TransformException’ by value + And ZoneNumber by 0x3fU to prevent error: directive output may be truncated writing between 1 and 11 bytes into a region of size 4 +* Enabling the user to override the output child_frame_id +* Fixing Euler body-to-world transformations +* Whitespace +* fixing no datum service in melodic +* Contributors: Alexis schad, Matthew Jones, Tom Moore, thallerod + +2.6.2 (2018-10-25) +------------------ +* Fixing tests +* Contributors: Tom Moore + +2.6.1 (2018-10-25) +------------------ +* Adding more output for measurement history failures +* Adding filter processing toggle service +* Waiting for valid ROS time before starting navsat_transform_node +* Contributors: Tom Moore, stevemacenski + +2.6.0 (2018-07-27) +------------------ +* Moving to C++14, adding error flags, and fixing all warnings +* Contributors: Tom Moore + +2.5.2 (2018-04-11) +------------------ +* Add published accel topic to documentation +* adding log statements for nans in the invertable matrix +* Fixing issue with potential seg fault +* Contributors: Oleg Kalachev, Tom Moore, stevemacenski + +2.5.1 (2018-01-03) +------------------ +* Fixing CMakeLists +* Contributors: Tom Moore + +2.5.0 (2017-12-15) +------------------ +* Fixing datum precision +* Fixing timing variable +* Fixing state history reversion +* Fixing critical bug with dynamic process noise covariance +* Fix typo in reading Mahalanobis thresholds. +* Zero out rotation in GPS to base_link transform +* Update xmlrpcpp includes for Indigo support +* Removing lastUpdateTime +* Fixing timestamps in map->odom transform +* Simplify enabledAtStartup logic +* Add std_srvs dependency +* Add enabling service +* Ensure all raw sensor input orientations are normalized even if messages are not +* Install params directory. +* Add robot localization estimator +* Adding nodelet support +* Contributors: Jacob Perron, Jacob Seibert, Jiri Hubacek, Mike Purvis, Miquel Massot, Pavlo Kolomiiets, Rein Appeldoorn, Rokus Ottervanger, Simon Gene Gottlieb, Tom Moore, stevemacenski + +2.4.0 (2017-06-12) +------------------ +* Updated documentation +* Added reset_on_time_jump option +* Added feature to optionally publish utm frame as parent in navsat_transform_node +* Moved global callback queue reset +* Added initial_state parameter and documentation +* Fixed ac/deceleration gains default logic +* Added gravity parameter +* Added delay and throttle if tf lookup fails +* Fixed UKF IMUTwistBasicIO test +* Added transform_timeout parameter +* Set gps_odom timestamp before tf2 lookuptransform +* Removed non-portable sincos calls +* Simplified logic to account for correlated error +* Added dynamic process noise covariance calculation +* Fixed catkin_package Eigen warning +* Added optional publication of acceleration state +* Contributors: Brian Gerkey, Enrique Fernandez, Jochen Sprickerhof, Rein Appeldoorn, Simon Gene Gottlieb, Tom Moore + +2.3.1 (2016-10-27) +------------------ +* Adding gitignore +* Adding remaining wiki pages +* Adding config and prep pages +* Adding navsat_transform_node documentation +* use_odometry_yaw fix for n_t_n +* Fixing issue with manual pose reset when history is not empty +* Getting inverse transform when looking up robot's pose. +* Sphinx documentation +* Removing forward slashes from navsat_transform input topics for template launch file +* Adding example launch and parameter files for a two-level EKF setup with navsat_transform_node +* Adding yaml file for navsat_transform_node, and moving parameter documentation to it. +* Updating EKF and UKF parameter templates with usage comments +* Contributors: Tom Moore, asimay + +2.3.0 (2016-07-28) +------------------ +* Fixed issues with datum usage and frame_ids +* Fixed comment for wait_for_datum +* Fixing issue with non-zero navsat sensor orientation offsets +* Fixing issue with base_link->gps transform wrecking the 'true' UTM position computation +* Using correct covariance for filtered GPS +* Fixed unitialized odometry covariance bug +* Added filter history and measurement queue behavior +* Changing output timestamp to more accurately use the time stamp of the most recently-processed measurement +* Added TcpNoDelay() +* Added parameter to make transform publishing optional +* Fixed differential handling for pose data so that it doesn't care about the message's frame_id +* Updated UKF config and launch +* Added a test case for the timestamp diagnostics +* Added reporting of bad timestamps via diagnostics +* Updated tests to match new method signatures +* Added control term +* Added smoothing capability for delayed measurements +* Making variables in navsat_transform conform to ROS coding standards +* Contributors: Adel Fakih, Ivor Wanders, Marc Essinger, Tobias Tueylue, Tom Moore + +2.2.3 (2016-04-24) +------------------ +* Cleaning up callback data structure and callbacks and updating doxygen comments in headers +* Removing MessageFilters +* Removing deprecated parameters +* Adding the ability to handle GPS offsets from the vehicle's origin +* Cleaning up navsat_transform.h +* Making variables in navsat_transform conform to ROS coding standards + +2.2.2 (2016-02-04) +------------------ +* Updating trig functions to use sincos for efficiency +* Updating licensing information and adding Eigen MPL-only flag +* Added state to imu frame transformation +* Using state orientation if imu orientation is missing +* Manually adding second spin for odometry and IMU data that is passed to message filters +* Reducing delay between measurement reception and filter output +* Zero altitute in intital transform too, when zero altitude param is set +* Fixing regression with conversion back to GPS coordinates +* Switched cropping of orientation data in inovationSubset with mahalanobis check to prevent excluding measurements with orientations bigger/smaller than ± PI +* Fix Jacobian for EKF. +* Removing warning about orientation variables when only their velocities are measured +* Checking for -1 in IMU covariances and ignoring relevant message data +* roslint and catkin_lint applied +* Adding base_link to datum specification, and fixing bug with order of measurement handling when a datum is specified. Also added check to make sure IMU data is transformable before using it. +* Contributors: Adnan Ademovic, Jit Ray Chowdhury, Philipp Tscholl, Tom Moore, ayrton04, kphil + +2.2.1 (2015-05-27) +------------------ +* Fixed handling of IMU data w.r.t. differential mode and relative mode + +2.2.0 (2015-05-22) +------------------ +* Added tf2-friendly tf_prefix appending +* Corrected for IMU orientation in navsat_transform +* Fixed issue with out-of-order measurements and pose resets +* Nodes now assume ENU standard for yaw data +* Removed gps_common dependency +* Adding option to navsat_transform_node that enables the use of the heading from the odometry message instead of an IMU. +* Changed frame_id used in setPoseCallback to be the world_frame +* Optimized Eigen arithmetic for signficiant performance boost +* Migrated to tf2 +* Code refactoring and reorganization +* Removed roll and pitch from navsat_transform calculations +* Fixed transform for IMU data to better support mounting IMUs in non-standard orientations +* Added feature to navsat_transform_node whereby filtered odometry data can be coverted back into navsat data +* Added a parameter to allow future dating the world_frame->base_link_frame transform. +* Removed deprecated differential setting handler +* Added relative mode +* Updated and improved tests +* Fixing source frame_id in pose data handling +* Added initial covariance parameter +* Fixed bug in covariance copyinh +* Added parameters for topic queue sizes +* Improved motion model's handling of angular velocities when robot has non-zero roll and pitch +* Changed the way differential measurements are handled +* Added diagnostics + +2.1.7 (2015-01-05) +------------------ +* Added some checks to eliminate unnecessary callbacks +* Updated launch file templates +* Added measurement outlier rejection +* Added failure callbacks for tf message filters +* Added optional broadcast of world_frame->utm transform for navsat_transform_node +* Bug fixes for differential mode and handling of Z acceleration in 2D mode + +2.1.6 (2014-11-06) +------------------ +* Added unscented Kalman filter (UKF) localization node +* Fixed map->odom tf calculation +* Acceleration data from IMUs is now used in computing the state estimate +* Added 2D mode + +2.1.5 (2014-10-07) +------------------ +* Changed initial estimate error covariance to be much smaller +* Fixed some debug output +* Added test suite +* Better compliance with REP-105 +* Fixed differential measurement handling +* Implemented message filters +* Added navsat_transform_node + +2.1.4 (2014-08-22) +------------------ +* Adding utm_transform_node to install targets + +2.1.3 (2014-06-22) +------------------ +* Some changes to ease GPS integration +* Addition of differential integration of pose data +* Some documentation cleanup +* Added UTM transform node and launch file +* Bug fixes + +2.1.2 (2014-04-11) +------------------ +* Updated covariance correction formulation to "Joseph form" to improve filter stability. +* Implemented new versioning scheme. + +2.1.1 (2014-04-11) +------------------ +* Added cmake_modules dependency for Eigen support, and added include to silence boost::signals warning from tf include + diff --git a/Localizations/Packages/robot_localization/doc/Makefile b/Localizations/Packages/robot_localization/doc/Makefile new file mode 100644 index 0000000..642efcf --- /dev/null +++ b/Localizations/Packages/robot_localization/doc/Makefile @@ -0,0 +1,216 @@ +# Makefile for Sphinx documentation +# + +# You can set these variables from the command line. +SPHINXOPTS = +SPHINXBUILD = sphinx-build +PAPER = +BUILDDIR = .build + +# User-friendly check for sphinx-build +ifeq ($(shell which $(SPHINXBUILD) >/dev/null 2>&1; echo $$?), 1) +$(error The '$(SPHINXBUILD)' command was not found. Make sure you have Sphinx installed, then set the SPHINXBUILD environment variable to point to the full path of the '$(SPHINXBUILD)' executable. Alternatively you can add the directory with the executable to your PATH. If you don't have Sphinx installed, grab it from http://sphinx-doc.org/) +endif + +# Internal variables. +PAPEROPT_a4 = -D latex_paper_size=a4 +PAPEROPT_letter = -D latex_paper_size=letter +ALLSPHINXOPTS = -d $(BUILDDIR)/doctrees $(PAPEROPT_$(PAPER)) $(SPHINXOPTS) . +# the i18n builder cannot share the environment and doctrees with the others +I18NSPHINXOPTS = $(PAPEROPT_$(PAPER)) $(SPHINXOPTS) . + +.PHONY: help +help: + @echo "Please use \`make ' where is one of" + @echo " html to make standalone HTML files" + @echo " dirhtml to make HTML files named index.html in directories" + @echo " singlehtml to make a single large HTML file" + @echo " pickle to make pickle files" + @echo " json to make JSON files" + @echo " htmlhelp to make HTML files and a HTML help project" + @echo " qthelp to make HTML files and a qthelp project" + @echo " applehelp to make an Apple Help Book" + @echo " devhelp to make HTML files and a Devhelp project" + @echo " epub to make an epub" + @echo " latex to make LaTeX files, you can set PAPER=a4 or PAPER=letter" + @echo " latexpdf to make LaTeX files and run them through pdflatex" + @echo " latexpdfja to make LaTeX files and run them through platex/dvipdfmx" + @echo " text to make text files" + @echo " man to make manual pages" + @echo " texinfo to make Texinfo files" + @echo " info to make Texinfo files and run them through makeinfo" + @echo " gettext to make PO message catalogs" + @echo " changes to make an overview of all changed/added/deprecated items" + @echo " xml to make Docutils-native XML files" + @echo " pseudoxml to make pseudoxml-XML files for display purposes" + @echo " linkcheck to check all external links for integrity" + @echo " doctest to run all doctests embedded in the documentation (if enabled)" + @echo " coverage to run coverage check of the documentation (if enabled)" + +.PHONY: clean +clean: + rm -rf $(BUILDDIR)/* + +.PHONY: html +html: + $(SPHINXBUILD) -b html $(ALLSPHINXOPTS) $(BUILDDIR)/html + @echo + @echo "Build finished. The HTML pages are in $(BUILDDIR)/html." + +.PHONY: dirhtml +dirhtml: + $(SPHINXBUILD) -b dirhtml $(ALLSPHINXOPTS) $(BUILDDIR)/dirhtml + @echo + @echo "Build finished. The HTML pages are in $(BUILDDIR)/dirhtml." + +.PHONY: singlehtml +singlehtml: + $(SPHINXBUILD) -b singlehtml $(ALLSPHINXOPTS) $(BUILDDIR)/singlehtml + @echo + @echo "Build finished. The HTML page is in $(BUILDDIR)/singlehtml." + +.PHONY: pickle +pickle: + $(SPHINXBUILD) -b pickle $(ALLSPHINXOPTS) $(BUILDDIR)/pickle + @echo + @echo "Build finished; now you can process the pickle files." + +.PHONY: json +json: + $(SPHINXBUILD) -b json $(ALLSPHINXOPTS) $(BUILDDIR)/json + @echo + @echo "Build finished; now you can process the JSON files." + +.PHONY: htmlhelp +htmlhelp: + $(SPHINXBUILD) -b htmlhelp $(ALLSPHINXOPTS) $(BUILDDIR)/htmlhelp + @echo + @echo "Build finished; now you can run HTML Help Workshop with the" \ + ".hhp project file in $(BUILDDIR)/htmlhelp." + +.PHONY: qthelp +qthelp: + $(SPHINXBUILD) -b qthelp $(ALLSPHINXOPTS) $(BUILDDIR)/qthelp + @echo + @echo "Build finished; now you can run "qcollectiongenerator" with the" \ + ".qhcp project file in $(BUILDDIR)/qthelp, like this:" + @echo "# qcollectiongenerator $(BUILDDIR)/qthelp/robot_localization.qhcp" + @echo "To view the help file:" + @echo "# assistant -collectionFile $(BUILDDIR)/qthelp/robot_localization.qhc" + +.PHONY: applehelp +applehelp: + $(SPHINXBUILD) -b applehelp $(ALLSPHINXOPTS) $(BUILDDIR)/applehelp + @echo + @echo "Build finished. The help book is in $(BUILDDIR)/applehelp." + @echo "N.B. You won't be able to view it unless you put it in" \ + "~/Library/Documentation/Help or install it in your application" \ + "bundle." + +.PHONY: devhelp +devhelp: + $(SPHINXBUILD) -b devhelp $(ALLSPHINXOPTS) $(BUILDDIR)/devhelp + @echo + @echo "Build finished." + @echo "To view the help file:" + @echo "# mkdir -p $$HOME/.local/share/devhelp/robot_localization" + @echo "# ln -s $(BUILDDIR)/devhelp $$HOME/.local/share/devhelp/robot_localization" + @echo "# devhelp" + +.PHONY: epub +epub: + $(SPHINXBUILD) -b epub $(ALLSPHINXOPTS) $(BUILDDIR)/epub + @echo + @echo "Build finished. The epub file is in $(BUILDDIR)/epub." + +.PHONY: latex +latex: + $(SPHINXBUILD) -b latex $(ALLSPHINXOPTS) $(BUILDDIR)/latex + @echo + @echo "Build finished; the LaTeX files are in $(BUILDDIR)/latex." + @echo "Run \`make' in that directory to run these through (pdf)latex" \ + "(use \`make latexpdf' here to do that automatically)." + +.PHONY: latexpdf +latexpdf: + $(SPHINXBUILD) -b latex $(ALLSPHINXOPTS) $(BUILDDIR)/latex + @echo "Running LaTeX files through pdflatex..." + $(MAKE) -C $(BUILDDIR)/latex all-pdf + @echo "pdflatex finished; the PDF files are in $(BUILDDIR)/latex." + +.PHONY: latexpdfja +latexpdfja: + $(SPHINXBUILD) -b latex $(ALLSPHINXOPTS) $(BUILDDIR)/latex + @echo "Running LaTeX files through platex and dvipdfmx..." + $(MAKE) -C $(BUILDDIR)/latex all-pdf-ja + @echo "pdflatex finished; the PDF files are in $(BUILDDIR)/latex." + +.PHONY: text +text: + $(SPHINXBUILD) -b text $(ALLSPHINXOPTS) $(BUILDDIR)/text + @echo + @echo "Build finished. The text files are in $(BUILDDIR)/text." + +.PHONY: man +man: + $(SPHINXBUILD) -b man $(ALLSPHINXOPTS) $(BUILDDIR)/man + @echo + @echo "Build finished. The manual pages are in $(BUILDDIR)/man." + +.PHONY: texinfo +texinfo: + $(SPHINXBUILD) -b texinfo $(ALLSPHINXOPTS) $(BUILDDIR)/texinfo + @echo + @echo "Build finished. The Texinfo files are in $(BUILDDIR)/texinfo." + @echo "Run \`make' in that directory to run these through makeinfo" \ + "(use \`make info' here to do that automatically)." + +.PHONY: info +info: + $(SPHINXBUILD) -b texinfo $(ALLSPHINXOPTS) $(BUILDDIR)/texinfo + @echo "Running Texinfo files through makeinfo..." + make -C $(BUILDDIR)/texinfo info + @echo "makeinfo finished; the Info files are in $(BUILDDIR)/texinfo." + +.PHONY: gettext +gettext: + $(SPHINXBUILD) -b gettext $(I18NSPHINXOPTS) $(BUILDDIR)/locale + @echo + @echo "Build finished. The message catalogs are in $(BUILDDIR)/locale." + +.PHONY: changes +changes: + $(SPHINXBUILD) -b changes $(ALLSPHINXOPTS) $(BUILDDIR)/changes + @echo + @echo "The overview file is in $(BUILDDIR)/changes." + +.PHONY: linkcheck +linkcheck: + $(SPHINXBUILD) -b linkcheck $(ALLSPHINXOPTS) $(BUILDDIR)/linkcheck + @echo + @echo "Link check complete; look for any errors in the above output " \ + "or in $(BUILDDIR)/linkcheck/output.txt." + +.PHONY: doctest +doctest: + $(SPHINXBUILD) -b doctest $(ALLSPHINXOPTS) $(BUILDDIR)/doctest + @echo "Testing of doctests in the sources finished, look at the " \ + "results in $(BUILDDIR)/doctest/output.txt." + +.PHONY: coverage +coverage: + $(SPHINXBUILD) -b coverage $(ALLSPHINXOPTS) $(BUILDDIR)/coverage + @echo "Testing of coverage in the sources finished, look at the " \ + "results in $(BUILDDIR)/coverage/python.txt." + +.PHONY: xml +xml: + $(SPHINXBUILD) -b xml $(ALLSPHINXOPTS) $(BUILDDIR)/xml + @echo + @echo "Build finished. The XML files are in $(BUILDDIR)/xml." + +.PHONY: pseudoxml +pseudoxml: + $(SPHINXBUILD) -b pseudoxml $(ALLSPHINXOPTS) $(BUILDDIR)/pseudoxml + @echo + @echo "Build finished. The pseudo-XML files are in $(BUILDDIR)/pseudoxml." diff --git a/Localizations/Packages/robot_localization/doc/conf.py b/Localizations/Packages/robot_localization/doc/conf.py new file mode 100644 index 0000000..b8aad58 --- /dev/null +++ b/Localizations/Packages/robot_localization/doc/conf.py @@ -0,0 +1,197 @@ +import sys +import os + +import catkin_pkg.package +catkin_dir = os.path.dirname(os.path.dirname(os.path.abspath(__file__))) +catkin_package = catkin_pkg.package.parse_package(os.path.join(catkin_dir, catkin_pkg.package.PACKAGE_MANIFEST_FILENAME)) + +extensions = [ + 'sphinx.ext.autodoc', + 'sphinx.ext.doctest', + 'sphinx.ext.intersphinx', + 'sphinx.ext.todo', + 'sphinx.ext.coverage', + 'sphinx.ext.mathjax', +] + +templates_path = ['.templates'] +source_suffix = '.rst' +master_doc = 'index' + +project = u'robot_localization' +copyright = u'2016, Tom Moore' +author = u'Tom Moore' +version = catkin_package.version +release = catkin_package.version + +language = None +exclude_patterns = ['.build'] +pygments_style = 'sphinx' +todo_include_todos = True + +html_theme = 'nature' + +html_theme_options = { + "sidebarwidth": "350" +} + +# The name of an image file (relative to this directory) to place at the top +# of the sidebar. +html_logo = 'images/rl_small.png' + +# The name of an image file (relative to this directory) to use as a favicon of +# the docs. This file should be a Windows icon file (.ico) being 16x16 or 32x32 +# pixels large. +#html_favicon = None + +# Add any paths that contain custom static files (such as style sheets) here, +# relative to this directory. They are copied after the builtin static files, +# so a file named "default.css" will overwrite the builtin "default.css". +#html_static_path = ['.static'] + +# Add any extra paths that contain custom files (such as robots.txt or +# .htaccess) here, relative to this directory. These files are copied +# directly to the root of the documentation. +#html_extra_path = [] + +# If not '', a 'Last updated on:' timestamp is inserted at every page bottom, +# using the given strftime format. +#html_last_updated_fmt = '%b %d, %Y' + +# If true, SmartyPants will be used to convert quotes and dashes to +# typographically correct entities. +#html_use_smartypants = True + +# Custom sidebar templates, maps document names to template names. +html_sidebars = { '**': ['full_globaltoc.html', 'sourcelink.html', 'searchbox.html'], } + +# Additional templates that should be rendered to pages, maps page names to +# template names. +#html_additional_pages = {} + +# If false, no module index is generated. +#html_domain_indices = True + +# If false, no index is generated. +#html_use_index = True + +# If true, the index is split into individual pages for each letter. +#html_split_index = False + +# If true, links to the reST sources are added to the pages. +#html_show_sourcelink = True + +# If true, "Created using Sphinx" is shown in the HTML footer. Default is True. +#html_show_sphinx = True + +# If true, "(C) Copyright ..." is shown in the HTML footer. Default is True. +#html_show_copyright = True + +# If true, an OpenSearch description file will be output, and all pages will +# contain a tag referring to it. The value of this option must be the +# base URL from which the finished HTML is served. +#html_use_opensearch = '' + +# This is the file name suffix for HTML files (e.g. ".xhtml"). +#html_file_suffix = None + +# Language to be used for generating the HTML full-text search index. +# Sphinx supports the following languages: +# 'da', 'de', 'en', 'es', 'fi', 'fr', 'hu', 'it', 'ja' +# 'nl', 'no', 'pt', 'ro', 'ru', 'sv', 'tr' +#html_search_language = 'en' + +# A dictionary with options for the search language support, empty by default. +# Now only 'ja' uses this config value +#html_search_options = {'type': 'default'} + +# The name of a javascript file (relative to the configuration directory) that +# implements a search results scorer. If empty, the default will be used. +#html_search_scorer = 'scorer.js' + +# Output file base name for HTML help builder. +htmlhelp_basename = 'robot_localizationdoc' + +# -- Options for LaTeX output --------------------------------------------- + +latex_elements = { +# The paper size ('letterpaper' or 'a4paper'). +#'papersize': 'letterpaper', + +# The font size ('10pt', '11pt' or '12pt'). +#'pointsize': '10pt', + +# Additional stuff for the LaTeX preamble. +#'preamble': '', + +# Latex figure (float) alignment +#'figure_align': 'htbp', +} + +# Grouping the document tree into LaTeX files. List of tuples +# (source start file, target name, title, +# author, documentclass [howto, manual, or own class]). +latex_documents = [ + (master_doc, 'robot_localization.tex', u'robot\\_localization Documentation', + u'Tom Moore', 'manual'), +] + +# The name of an image file (relative to this directory) to place at the top of +# the title page. +#latex_logo = None + +# For "manual" documents, if this is true, then toplevel headings are parts, +# not chapters. +#latex_use_parts = False + +# If true, show page references after internal links. +#latex_show_pagerefs = False + +# If true, show URL addresses after external links. +#latex_show_urls = False + +# Documents to append as an appendix to all manuals. +#latex_appendices = [] + +# If false, no module index is generated. +#latex_domain_indices = True + + +# -- Options for manual page output --------------------------------------- + +# One entry per manual page. List of tuples +# (source start file, name, description, authors, manual section). +man_pages = [ + (master_doc, 'robot_localization', u'robot_localization Documentation', + [author], 1) +] + +# If true, show URL addresses after external links. +#man_show_urls = False + + +# -- Options for Texinfo output ------------------------------------------- + +# Grouping the document tree into Texinfo files. List of tuples +# (source start file, target name, title, author, +# dir menu entry, description, category) +texinfo_documents = [ + (master_doc, 'robot_localization', u'robot_localization Documentation', + author, 'robot_localization', 'One line description of project.', + 'Miscellaneous'), +] + +# Documents to append as an appendix to all manuals. +#texinfo_appendices = [] + +# If false, no module index is generated. +#texinfo_domain_indices = True + +# How to display URL addresses: 'footnote', 'no', or 'inline'. +#texinfo_show_urls = 'footnote' + +# If true, do not generate a @detailmenu in the "Top" node's menu. +#texinfo_no_detailmenu = False + +# Example configuration for intersphinx: refer to the Python standard library. +intersphinx_mapping = {'https://docs.python.org/': None} diff --git a/Localizations/Packages/robot_localization/doc/configuring_robot_localization.rst b/Localizations/Packages/robot_localization/doc/configuring_robot_localization.rst new file mode 100644 index 0000000..5db5385 --- /dev/null +++ b/Localizations/Packages/robot_localization/doc/configuring_robot_localization.rst @@ -0,0 +1,117 @@ +.. _configuring_robot_localization: + +Configuring robot_localization +############################## + + +When incorporating sensor data into the position estimate of any of ``robot_localization``'s state estimation nodes, it is important to extract as much information as possible. This tutorial details the best practices for sensor integration. + +For additional information, users are encouraged to watch this `presentation `_ from ROSCon 2015. + +Sensor Configuration +******************** + +The configuration vector format is the same for all sensors, even if the message type in question doesn't contain some of the variables in the configuration vector (e.g., a <> lacks any pose data, but the configuration vector still has values for pose variables). Unused variables are simply ignored. + +Note that the configuration vector is given in the ``frame_id`` of the input message. For example, consider a velocity sensor that produces a `geometry_msgs/TwistWithCovarianceStamped `_ message with a ``frame_id`` of *velocity_sensor_frame*. In this example, we'll assume that a transform exists from *velocity_sensor_frame* to your robot's ``base_link_frame`` (e.g., *base_link*), and that the transform would convert :math:`X` velocity in the *velocity_sensor_frame* to :math:`Z` velocity in the ``base_link_frame``. To include the :math:`X` velocity data from the sensor into the filter, the configuration vector should set the :math:`X` velocity value to *true*, and **not** the :math:`\dot{Z}` velocity value: + +.. code-block:: xml + + [false, false, false, + false, false, false, + true, false, false, + false, false, false, + false, false, false] + +.. note:: The order of the boolean values are :math:`(X, Y, Z, roll, pitch, yaw, \dot{X}, \dot{Y}, \dot{Z}, \dot{roll}, \dot{pitch}, \dot{yaw}, \ddot{X}, \ddot{Y}, \ddot{Z})`. + +Operating in 2D? +**************** + +The first decision to make when configuring your sensors is whether or not your robot is operating in a planar environment, and you're comfortable with ignoring subtle effects of variations in the ground plane as might be reported from an IMU. If so, please set the ``two_d_mode`` parameter to *true*. This effectively zeros out the 3D pose variables in every measurement and forces them to be fused in the state estimate. + +Fusing Unmeasured Variables +*************************** + +Let's start with an example. Let's say you have a wheeled, nonholonomic robot that works in a planar environment. Your robot has some wheel encoders that are used to estimate instantaneous X velocity as well as absolute pose information. This information is reported in an `nav_msgs/Odometry `_ message. Additionally, your robot has an IMU that measures rotational velocity, vehicle attitude, and linear acceleration. Its data is reported in a `sensor_msgs/Imu `_ message. As we are operating in a planar environment, we set the ``two_d_mode`` parameter to *true*. This will automatically zero out all 3D variables, such as :math:`Z`, :math:`roll`, :math:`pitch`, their respective velocities, and :math:`Z` acceleration. We start with this configuration: + +.. code-block:: xml + + [true, true, false, + false, false, true, + true, false, false, + false, false, true, + false, false, false] + + [false, false, false, + false, false, true, + false, false, false, + false, false, true, + true, false, false] + +As a first pass, this makes sense, as a planar robot only needs to concern itself with :math:`X`, :math:`Y`, :math:`\dot{X}`, :math:`\dot{Y}`, :math:`\ddot{X}`, :math:`\ddot{Y}`, :math:`yaw`, and :math:`\dot{yaw}`. However, there are a few things to note here. + +1. For ``odom0``, we are including :math:`X` and :math:`Y` (reported in a world coordinate frame), :math:`yaw`, :math:`\dot{X}` (reported in the body frame), and :math:`\dot{yaw}`. However, unless your robot is internally using an IMU, it is most likely simply using wheel encoder data to generate the values in its measurements. Therefore, its velocity, heading, and position data are all generated from the same source. In that instance, we don't want to use all the values, as you're feeding duplicate information into the filter. Instead, it's best to just use the velocities: + +.. code-block:: xml + + [false, false, false, + false, false, false, + true, false, false, + false, false, true, + false, false, false] + + [false, false, false, + false, false, true, + false, false, false, + false, false, true, + true, false, false] + +2. Next, we note that we are not fusing :math:`\dot{Y}`. At first glance, this is the right choice, as our robot cannot move instantaneously sideways. However, if the `nav_msgs/Odometry `_ message reports a :math:`0` value for :math:`\dot{Y}` (and the :math:`\dot{Y}` covariance is NOT inflated to a large value), it's best to feed that value to the filter. As a :math:`0` measurement in this case indicates that the robot cannot ever move in that direction, it serves as a perfectly valid measurement: + +.. code-block:: xml + + [false, false, false, + false, false, false, + true, true, false, + false, false, true, + false, false, false] + + [false, false, false, + false, false, true, + false, false, false, + false, false, true, + true, false, false] + +You may wonder why did we not fuse :math:`\dot{Z}` velocity for the same reason. The answer is that we did when we set ``two_d_mode`` to *false*. If we hadn't, we could, in fact, fuse the :math:`0` measurement for :math:`\dot{Z}` velocity into the filter. + +3. Last, we come to the IMU. You may notice that we have set the :math:`\ddot{Y}` to *false*. This is due to the fact that many systems, including the hypothetical one we are discussing here, will not undergo instantaneous :math:`Y` acceleration. However, the IMU will likely report non-zero, noisy values for Y acceleration, which can cause your estimate to drift rapidly. + +The *differential* Parameters +***************************** + +The state estimation nodes in ''robot_localization'' allow users to fuse as many sensors as they like. This allows users to measure certain state vector variables - in particular, pose variables - using more than one source. For example, your robot may obtain absolute orientation information from multiple IMUs, or it may have multiple data sources providing an estimate its absolute position. In this case, users have two options: + +1. Fuse all the absolute position/orientation data as-is, e.g., + +.. code-block:: xml + + [false, false, false, + true, true, true, + false, false, false, + false, false, false, + false, false, false] + + [false, false, false, + true, true, true, + false, false, false, + false, false, false, + false, false, false] + +In this case, users should be **very** careful and ensure that the covariances on each measured orientation variable are set correctly. If each IMU advertises a yaw variance of, for example, :math:`0.1`, yet the delta between the IMUs' yaw measurements is :math:`> 0.1`, then the output of the filter will oscillate back and forth between the values provided by each sensor. Users should make sure that the noise distributions around each measurement overlap. + +2. Alternatively, users can make use of the ``_differential`` parameter. By setting this to *true* for a given sensor, all pose (position and orientation) data is converted to a velocity by calculating the change in the measurement value between two consecutive time steps. The data is then fused as a velocity. Again, though, users should take care: when measurements are fused absolutely (especially IMUs), if the measurement has a static or non-increasing variance for a given variable, then the variance in the estimate's covariance matrix will be bounded. If that information is converted to a velocity, then at each time step, the estimate will gain some small amount of error, and the variance for the variable in question will grow without bound. For position :math:`(X, Y, Z)` information, this isn't an issue, but for orientation data, it is a problem. For example, it is acceptable for a robot to move around its environment and accumulate :math:`1.5` meters of error in the :math:`X` direction after some time. If that same robot moves around and accumulates :math:`1.5` radians of error in yaw, then when the robot next drives forward, its position error will explode. + +The general rule of thumb for the ``_differential`` parameter is that if a give robot has only one source of orientation data, then the differential parameter should be set to *false*. If there are :math:`N` sources, users can set the ``_differential`` parameter to *true* for :math:`N-1` of them, or simply ensure that the covariance values are large enough to eliminate oscillations. + + diff --git a/Localizations/Packages/robot_localization/doc/images/figure2.png b/Localizations/Packages/robot_localization/doc/images/figure2.png new file mode 100644 index 0000000..0224d29 Binary files /dev/null and b/Localizations/Packages/robot_localization/doc/images/figure2.png differ diff --git a/Localizations/Packages/robot_localization/doc/images/navsat_transform_workflow.png b/Localizations/Packages/robot_localization/doc/images/navsat_transform_workflow.png new file mode 100644 index 0000000..450bcf2 Binary files /dev/null and b/Localizations/Packages/robot_localization/doc/images/navsat_transform_workflow.png differ diff --git a/Localizations/Packages/robot_localization/doc/images/navsat_transform_workflow.tex b/Localizations/Packages/robot_localization/doc/images/navsat_transform_workflow.tex new file mode 100644 index 0000000..2dd0ab0 --- /dev/null +++ b/Localizations/Packages/robot_localization/doc/images/navsat_transform_workflow.tex @@ -0,0 +1,49 @@ +\documentclass{standalone} +\usepackage{tikz} +\usetikzlibrary{positioning} + +\begin{document} + +\sffamily + +\begin{tikzpicture}[>=stealth, + node distance = 3cm, + box/.style={shape=rectangle,draw,rounded corners},] + % Nodes + \node (wheel) [box] {Wheel Odometry}; + \node (imu) [box, right =of wheel]{IMU}; + \node (ekfLocal) [box, below =of wheel]{EKF Local}; + \node (ekfGlobal) [box, below =of imu]{EKF Global}; + \node (navsat) [box, right =7cm of ekfGlobal]{navsat\_transform}; + \node (gps) [box, above =of navsat]{GPS}; + % Coordinates for edges pointing to empty space + \node (gpsF) [right =of navsat]{}; + \node (tfLocal) [below =of ekfLocal]{}; + \node (odomLocal) [left =of ekfLocal]{}; + \node (tfGlobal) [below =of ekfGlobal]{}; + % Edges + \draw[->] (wheel.270) -- (ekfLocal.90); + \draw[->] (wheel.315) -- (ekfGlobal.135) + node [fill=white, pos=0.2, align=center] {\ttfamily"/wheel/odometry"\\nav\_msgs/Odometry}; + \draw[->] (imu.225) -- (ekfLocal.45); + \draw[->] (imu.315) -- (navsat.135); + \draw[->] (imu.270) -- (ekfGlobal.90) + node [fill=white, pos=0.2, align=center] {\ttfamily"/imu/data"\\sensor\_msgs/Imu}; + \draw[->] (gps.270) -- (navsat.90) + node [fill=white, pos=0.2, align=center] {\ttfamily"/gps/fix"\\sensor\_msgs/NavSatFix}; + \draw[->, transform canvas={yshift=1mm}] (ekfGlobal) -- (navsat) + node [fill=white, above=1mm, pos=0.5, align=center] {\ttfamily"/odometry/filtered/global"\\nav\_msgs/Odometry}; + \draw[->, transform canvas={yshift=-1mm}] (navsat) -- (ekfGlobal) + node [fill=white, below=1mm, pos=0.5, align=center] {\ttfamily"/odometry/gps"\\nav\_msgs/Odometry}; + % Outputs not cycled back into the graph + \draw[->, dashed] (navsat) -- (gpsF) + node [fill=white, above=1mm, pos=1.0, align=center] {\ttfamily"/gps/filtered"\\sensor\_msgs/NavSatFix}; + \draw[->, dashed] (ekfLocal) -- (tfLocal) + node [fill=white, pos=0.7, align=center] {\ttfamily"/tf" odom -> base\\tf2\_msgs/TFMessage}; + \draw[->, dashed] (ekfLocal) -- (odomLocal) + node [fill=white, above=1mm, pos=1.0, align=center] {\ttfamily"/odometry/filtered/local"\\nav\_msgs/Odometry}; + \draw[->, dashed] (ekfGlobal) -- (tfGlobal) + node [fill=white, pos=0.7, align=center] {\ttfamily"/tf" map -> odom\\tf2\_msgs/TFMessage}; +\end{tikzpicture} + +\end{document} diff --git a/Localizations/Packages/robot_localization/doc/images/rl_small.png b/Localizations/Packages/robot_localization/doc/images/rl_small.png new file mode 100644 index 0000000..33a4d59 Binary files /dev/null and b/Localizations/Packages/robot_localization/doc/images/rl_small.png differ diff --git a/Localizations/Packages/robot_localization/doc/index.rst b/Localizations/Packages/robot_localization/doc/index.rst new file mode 100644 index 0000000..b6c6aee --- /dev/null +++ b/Localizations/Packages/robot_localization/doc/index.rst @@ -0,0 +1,55 @@ +.. _index: + +robot_localization wiki +*********************** + +``robot_localization`` is a collection of state estimation nodes, each of which is an implementation of a nonlinear state estimator for robots moving in 3D space. It contains two state estimation nodes, ``ekf_localization_node`` and ``ukf_localization_node``. In addition, ``robot_localization`` provides ``navsat_transform_node``, which aids in the integration of GPS data. + +.. toctree:: + :hidden: + + state_estimation_nodes + navsat_transform_node + preparing_sensor_data + configuring_robot_localization + migrating_from_robot_pose_ekf + integrating_gps + CHANGELOG + user_contributed_tutorials + +Features +======== + +All the state estimation nodes in ``robot_localization`` share common features, namely: + +* Fusion of an arbitrary number of sensors. The nodes do not restrict the number of input sources. If, for example, your robot has multiple IMUs or multiple sources of odometry information, the state estimation nodes within ``robot_localization`` can support all of them. +* Support for multiple ROS message types. All state estimation nodes in ``robot_localization`` can take in `nav_msgs/Odometry `_, `sensor_msgs/Imu `_, `geometry_msgs/PoseWithCovarianceStamped `_, or `geometry_msgs/TwistWithCovarianceStamped `_ messages. +* Per-sensor input customization. If a given sensor message contains data that you don't want to include in your state estimate, the state estimation nodes in ``robot_localization`` allow you to exclude that data on a per-sensor basis. +* Continuous estimation. Each state estimation node in ``robot_localization`` begins estimating the vehicle's state as soon as it receives a single measurement. If there is a holiday in the sensor data (i.e., a long period in which no data is received), the filter will continue to estimate the robot's state via an internal motion model. + +All state estimation nodes track the 15-dimensional state of the vehicle: :math:`(X, Y, Z, roll, pitch, yaw, \dot{X}, \dot{Y}, \dot{Z}, \dot{roll}, \dot{pitch}, \dot{yaw}, \ddot{X}, \ddot{Y}, \ddot{Z})`. + +Other Resources +=============== + +If you're new to ``robot_localization``, check out the `2015 ROSCon talk `_ for some pointers on getting started. + +Further details can be found in :download:`this paper `: + +.. code-block:: none + + @inproceedings{MooreStouchKeneralizedEkf2014, + author = {T. Moore and D. Stouch}, + title = {A Generalized Extended Kalman Filter Implementation for the Robot Operating System}, + year = {2014}, + month = {July}, + booktitle = {Proceedings of the 13th International Conference on Intelligent Autonomous Systems (IAS-13)}, + publisher = {Springer} + } + +Indices and tables +================== + +* :ref:`genindex` +* :ref:`search` + diff --git a/Localizations/Packages/robot_localization/doc/integrating_gps.rst b/Localizations/Packages/robot_localization/doc/integrating_gps.rst new file mode 100644 index 0000000..65a7850 --- /dev/null +++ b/Localizations/Packages/robot_localization/doc/integrating_gps.rst @@ -0,0 +1,165 @@ +Integrating GPS Data +#################### + +Integration of GPS data is a common request from users. ``robot_localization`` contains a node, ``navsat_transform_node``, that transforms GPS data into a frame that is consistent with your robot's starting pose (position and orientation) in its world frame. This greatly simplifies fusion of GPS data. This tutorial explains how to use ``navsat_transform_node``, and delves into some of the math behind it. + +For additional information, users are encouraged to watch this `presentation `_ from ROSCon 2015. + +Notes on Fusing GPS Data +************************ + +Before beginning this tutorial, users should be sure to familiarize themselves with `REP-105 `_. It is important for users to realize that using a position estimate that includes GPS data will likely be unfit for use by navigation modules, owing to the fact that GPS data is subject to discrete discontinuities ("jumps"). If you want to fuse data from a GPS into your position estimate, one potential solution is to do the following: + +1. Run one instance of a ``robot_localization`` state estimation node that fuses only continuous data, such as odometry and IMU data. Set the ``world_frame`` parameter for this instance to the same value as the ``odom_frame`` parameter. Execute local path plans and motions in this frame. +2. Run another instance of a ``robot_localization`` state estimation node that fuses all sources of data, including the GPS. Set the ``world_frame`` parameter for this instance to the same value as the ``map_frame`` parameter. + +This is just a suggestion, however, and users are free to fuse the GPS data into a single instance of a ``robot_localization`` state estimation node. + +Using navsat_transform_node +*************************** + +The diagram below illustrates an example setup: + +.. image:: images/navsat_transform_workflow.png + :width: 800px + :align: center + :alt: navsat_transform workflow + + +Required Inputs +=============== + +``navsat_transform_node`` requires three sources of information: the robot's current pose estimate in its world frame, an earth-referenced heading, and a geographic coordinate expressed as a latitude/longitude pair (with optional altitude). + +These data can be obtained in three different ways: + +1. (Default behavior) The data can come entirely from the robot's sensors and pose estimation software. To enable this mode, make sure the ``wait_for_datum`` parameter is set to *false* (its default value). The required messages are: + + * A `sensor_msgs/NavSatFix `_ message with raw GPS coordinates in it. + * A `sensor_msgs/Imu `_ message with an absolute (earth-referenced) heading. + * A `nav_msgs/Odometry `_ message that contains the robot's current position estimate in the frame specified by its start location (typically the output of a ``robot_localization`` state estimation node). + +2. The datum (global frame origin) can be specified via the ``datum`` parameter. + + .. note:: In order to use this mode, the ``wait_for_datum`` parameter must be set to *true*. + + The ``datum`` parameter takes this form: + + .. code-block:: xml + + [55.944904, -3.186693, 0.0, map, base_link] + + The parameter order is ``latitude`` in decimal degrees, ``longitude`` in decimal degrees, ``heading`` in radians) the ``frame_id`` of your robot's world frame (i.e., the value of the ``world_frame`` parameter in a ``robot_localization`` state estimation node), and the ``frame_id`` of your robot's body frame (i.e., the value of the ``base_link_frame`` parameter in a ``robot_localization`` state estimation node). When this mode is used, the robot assumes that your robot's world frame origin is at the specified latitude and longitude and with a heading of :math:`0` (east). + +3. The datum can be set manually via the ``set_datum`` service and using the `robot_localization/SetDatum `_ service message. + + +GPS Data +^^^^^^^^ + +Please note that all development of ``navsat_transform_node`` was done using a Garmin 18x GPS unit, so there may be intricacies of the data generated by other units that need to be handled. + +The excellent `nmea_navsat_driver `_ package provides the required `sensor_msgs/NavSatFix `_ message. Here is the ``nmea_navsat_driver`` launch file we'll use for this tutorial: + + .. code-block:: xml + + + + + + +This information is only relevant if the user is not manually specifying the origin via the ``datum`` parameter or the ``set_datum`` service. + +IMU Data +^^^^^^^^ + +.. note:: Since version 2.2.1, ``navsat_transform_node`` has moved to a standard wherein all heading data is assumed to start with its zero point facing east. If your IMU does not conform to this standard and instead reports zero when facing north, you can still use the ``yaw_offset`` parameter to correct this. In this case, the value for ``yaw_offset`` would be :math:`\pi / 2` (approximately :math:`1.5707963`). + +Users should make sure their IMUs conform to `REP-105 `_. In particular, check that the signs of your orientation angles increase in the right direction. In addition, users should look up the `magnetic declination `_ for their robot's operating area, convert it to radians, and then use that value for the ``magnetic_declination_radians`` parameter. + +This information is only relevant if the user is not manually specifying the origin via the ``datum`` parameter or the ``set_datum`` service. + +Odometry Data +^^^^^^^^^^^^^ + +This should just be the output of whichever ``robot_localization`` state estimation node instance you are using to fuse GPS data. + +Configuring navsat_transform_node +================================= + +Below is the ``navsat_transform_node`` launch file we'll use for this tutorial: + +.. code-block:: xml + + + + + + + + + + + + + + + + + +These parameters are discussed on the :ref:`main page `. + +Configuring robot_localization +============================== + +Integration with ``robot_localization`` is straightforward at this point. Simply add this block to your state estimation node launch file: + +.. code-block:: xml + + + + [true, true, false, + false, false, false, + false, false, false, + false, false, false, + false, false, false] + + +Make sure to change ``odomN`` to whatever your odometry input values is (e.g., *odom1*, *odom2*, etc.). Also, if you wish to include altitude data, set ``odomN_config``'s third value to ``true``. + +.. note:: If you are operating in 2D don't have any sensor measuring Z or Z velocity, you can either: + + * Set ``navsat_transform_node's`` ``zero_altitude`` parameter to *true*, and then set ``odomN_config``'s third value to *true* + * Set ``two_d_mode`` to *true* in your ``robot_localization`` state estimation node + +You should have no need to modify the ``_differential`` setting within the state estimation node. The GPS is an absolute position sensor, and enabling differential integration defeats the purpose of using it. + +Details +======= + +We'll start with a picture. Consider a robot that starts at some latitude and longitude and with some heading. We assume in this tutorial that the heading comes from an IMU that reads 0 when facing east, and increases according to the ROS spec (i.e., counter-clockwise). The remainder of this tutorial will refer to Figure 2: + +.. image:: images/figure2.png + :width: 800px + :align: center + :alt: Figure 2 + + +`REP-105 `_ suggests four coordinate frames: *base_link*, *odom*, *map*, and *earth*. *base_link* is the coordinate frame that is rigidly attached to the vehicle. The *odom* and *map* frames are world-fixed frames and generally have their origins at the vehicle's start position and orientation. The *earth* frame is used as a common reference frame for multiple map frames, and is not yet supported by ``navsat_transform_node``. Note that in Figure 2, the robot has just started (``t = 0``), and so its *base_link*, *odom*, and *map* frames are aligned. We can also define a coordinate frame for the UTM grid, which we will call *utm*. For the purposes of this tutorial, we will refer to the UTM grid coordinate frame as *utm*. Therefore, what we want to do is create a *utm*->*map* transform. + +Referring to Figure 2, these ideas are (hopefully) made clear. The UTM origin is the :math:`(0_{UTM}, 0_{UTM})` point of the UTM zone that is associated with the robot's GPS location. The robot begins somewhere within the UTM zone at location :math:`(x_{UTM}, y_{UTM})`. The robot's initial orientation is some angle :math:`\theta` above the UTM grid's :math:`X`-axis. Our transform will therefore require that we know :math:`x_{UTM}, y_{UTM}` and :math:`\theta`. + +We now need to convert our latitude and longitude to UTM coordinates. The UTM grid assumes that the :math:`X`-axis faces east, the :math:`Y`-axis faces (true) north, and the :math:`Z`-axis points up out of the ground. This complies with the right-handed coordinate frame as dictated by `REP-105 `_. The REP also states that a yaw angle of :math:`0` means that we are facing straight down the :math:`X`-axis, and that the yaw increases counter-clockwise. ``navsat_transform_node`` assumes your heading data conforms to this standard. However, there are two factors that need to be considered: + +1. The IMU driver may not allow the user to apply the magnetic declination correction factor +2. The IMU driver may incorrectly report :math:`0` when facing north, and not when facing east (even though its headings increase and decrease correctly). Fortunately, ``navsat_transform_node`` exposes two parameters to adddress these possible shortcomings in IMU data: ``magnetic_declination_radians`` and ``yaw_offset``. Referring to Figure 2, for an IMU that is currently measuring a yaw value of ``imu_yaw``, + + :math:`yaw_{imu} = -\omega - offset_{yaw} + \theta` + + :math:`\theta = yaw_{imu} + \omega + offset_{yaw}` + +We now have a translation :math:`(x_{UTM}, y_{UTM})` and rotation :math:`\theta`, which we can use to create the required *utm* -> *map* transform. We use the transform to convert all future GPS positions into the robot's local coordinate frame. ``navsat_transform_node`` will also broadcast this transform if the ``broadcast_utm_transform`` parameter is set to *true*. + +If you have any questions about this tutorial, please feel free to ask questions on `answers.ros.org `_. + + diff --git a/Localizations/Packages/robot_localization/doc/manifest.yaml b/Localizations/Packages/robot_localization/doc/manifest.yaml new file mode 100644 index 0000000..a0dc0c4 --- /dev/null +++ b/Localizations/Packages/robot_localization/doc/manifest.yaml @@ -0,0 +1,38 @@ +actions: [] +authors: Tom Moore +brief: '' +bugtracker: '' +depends: +- catkin +- eigen +- diagnostic_updater +- cmake_modules +- tf2 +- nav_msgs +- roscpp +- rostest +- tf2_ros +- message_generation +- message_filters +- tf2_geometry_msgs +- sensor_msgs +- message_runtime +- std_msgs +- roslint +- rosunit +- diagnostic_msgs +- geographic_msgs +- python-catkin-pkg +- geometry_msgs +- rosbag +description: The robot_localization package provides nonlinear state estimation through + sensor fusion of an abritrary number of sensors. +license: BSD +maintainers: Tom Moore +msgs: [] +package_type: package +repo_url: '' +srvs: +- SetPose +- SetDatum +url: http://ros.org/wiki/robot_localization diff --git a/Localizations/Packages/robot_localization/doc/migrating_from_robot_pose_ekf.rst b/Localizations/Packages/robot_localization/doc/migrating_from_robot_pose_ekf.rst new file mode 100644 index 0000000..8ffba57 --- /dev/null +++ b/Localizations/Packages/robot_localization/doc/migrating_from_robot_pose_ekf.rst @@ -0,0 +1,29 @@ +.. _migrating_from_robot_pose_ekf: + +Migrating from robot_pose_ekf +############################# + +Migration from ``robot_pose_ekf`` is fairly straightforward. This page is meant to highlight relevant differences between the packages to facilitate rapid transitions. + +Covariances in Source Messages +============================== + +For ``robot_pose_ekf``, a common means of getting the filter to ignore measurements is to give it a massively inflated covariance, often on the order of 10^3. However, the state estimation nodes in ``robot_localization`` allow users to specify *which* variables from the measurement should be fused with the current state. If your sensor reports zero for a given variable and you don't want to fuse that value with your filter, or if the sensor is known to produce poor data for that field, then simply set its ``xxxx_config`` parameter value to false for the variable in question (see the main page for a description of this parameter). + +However, users should take care: sometimes platform constraints create implicit :math:`0` measurements of variables. For example, a differential drive robot that cannot move instantaneously in the :math:`Y` direction can safely fuse a :math:`0` measurement for :math:`\dot{Y}` with a small covariance value. + +The ''differential'' Parameter +============================== + +By default, ``robot_pose_ekf`` will take a pose measurement at time :math:`t`, determine the difference between it and the measurement at time :math:`t-1`, transform that difference into the current frame, and then integrate that measurement. This cleverly aids in cases where two sensors are measuring the same pose variable: as time progresses, the values reported by each sensor will start to diverge. If the covariances on at least one of these measurements do not grow appopriately, the filter will eventually start to oscillate between the measured values. By carrying out differential integration, this situation is avoided and measurements are always consistent with the current state. + +This situation can be avoided using three different methods for the ``robot_localization`` state estimation nodes: + +1. If fusing two different sources for the same pose data (e.g., two different sensors measuring :math:`Z` position), ensure that those sources accurately report their covariances. If the two sources begin to diverge, then their covariances should refect the growing error that must be occurring in at least one of them. + +2. If available, fuse velocity data instead of pose data. If you have two separate data sources measuring the same variable, fuse the most accurate one as pose data and the other as velocity. + +3. As an alternative to (2), if velocity data is unavailable for a given pose measurement, enable the ``_differential`` parameter for one of the sensors. This will cause it to be differentiated and fused as a velocity. + + + diff --git a/Localizations/Packages/robot_localization/doc/navsat_transform_node.rst b/Localizations/Packages/robot_localization/doc/navsat_transform_node.rst new file mode 100644 index 0000000..28f0359 --- /dev/null +++ b/Localizations/Packages/robot_localization/doc/navsat_transform_node.rst @@ -0,0 +1,75 @@ +navsat_transform_node +********************* + +``navsat_transform_node`` takes as input a `nav_msgs/Odometry `_ message (usually the output of ``ekf_localization_node`` or ``ukf_localization_node``), a `sensor_msgs/Imu `_ containing an accurate estimate of your robot's heading, and a `sensor_msgs/NavSatFix `_ message containing GPS data. It produces an odometry message in coordinates that are consistent with your robot's world frame. This value can be directly fused into your state estimate. + +.. note:: If you fuse the output of this node with any of the state estimation nodes in ``robot_localization``, you should make sure that the ``odomN_differential`` setting is *false* for that input. + +Parameters +========== + +~frequency +^^^^^^^^^^ +The real-valued frequency, in Hz, at which ``navsat_transform_node`` checks for new `sensor_msgs/NavSatFix `_ messages, and publishes filtered `sensor_msgs/NavSatFix `_ when ``publish_filtered_gps`` is set to *true*. + +~delay +^^^^^^ +The time, in seconds, to wait before calculating the transform from GPS coordinates to your robot's world frame. + +~magnetic_declination_radians +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Enter the magnetic declination for your location. If you don't know it, see `http://www.ngdc.noaa.gov/geomag-web `_ (make sure to convert the value to radians). This parameter is needed if your IMU provides its orientation with respect to the magnetic north. + +~yaw_offset +^^^^^^^^^^^ +Your IMU should read 0 for yaw when facing east. If it doesn't, enter the offset here (desired_value = offset + sensor_raw_value). For example, if your IMU reports 0 when facing north, as most of them do, this parameter would be ``pi/2`` (~1.5707963). This parameter changed in version ``2.2.1``. Previously, ``navsat_transform_node`` assumed that IMUs read 0 when facing north, so yaw_offset was used acordingly. + +~zero_altitude +^^^^^^^^^^^^^^ +If this is *true*, the `nav_msgs/Odometry `_ message produced by this node has its pose Z value set to 0. + +~publish_filtered_gps +^^^^^^^^^^^^^^^^^^^^^ +If *true*, ``navsat_transform_node`` will also transform your robot's world frame (e.g., *map*) position back to GPS coordinates, and publish a `sensor_msgs/NavSatFix `_ message on the ``/gps/filtered`` topic. + +~broadcast_utm_transform +^^^^^^^^^^^^^^^^^^^^^^^^ +If this is *true*, ``navsat_transform_node`` will broadcast the transform between the UTM grid and the frame of the input odometry data. See Published Transforms below for more information. + +~use_odometry_yaw +^^^^^^^^^^^^^^^^^ +If *true*, ``navsat_transform_node`` will not get its heading from the IMU data, but from the input odometry message. Users should take care to only set this to true if your odometry message has orientation data specified in an earth-referenced frame, e.g., as produced by a magnetometer. Additionally, if the odometry source is one of the state estimation nodes in ``robot_localization``, the user should have at least one source of absolute orientation data being fed into the node, with the ``_differential`` and ``_relative`` parameters set to *false*. + +~wait_for_datum +^^^^^^^^^^^^^^^ +If *true*, ``navsat_transform_node`` will wait to get a datum from either: + +* The ``datum`` parameter +* The ``set_datum`` service + +~broadcast_utm_transform_as_parent_frame +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +If *true*, ``navsat_transform_node`` will publish the utm->world_frame transform instead of the world_frame->utm transform. +Note that for the transform to be published ``broadcast_utm_transform`` also has to be set to *true*. + +~transform_timeout +^^^^^^^^^^^^^^^^^^ +This parameter specifies how long we would like to wait if a transformation is not available yet. Defaults to 0 if not set. The value 0 means we just get us the latest available (see ``tf2`` implementation) transform. + +Subscribed Topics +================= +* ``imu/data`` A `sensor_msgs/Imu `_ message with orientation data + +* ``odometry/filtered`` A `nav_msgs/Odometry `_ message of your robot's current position. This is needed in the event that your first GPS reading comes after your robot has attained some non-zero pose. + +* ``gps/fix`` A `sensor_msgs/NavSatFix `_ message containing your robot's GPS coordinates + +Published Topics +================ +* ``odometry/gps`` A `nav_msgs/Odometry `_ message containing the GPS coordinates of your robot, transformed into its world coordinate frame. This message can be directly fused into ``robot_localization``'s state estimation nodes. + +* ``gps/filtered`` (optional) A `sensor_msgs/NavSatFix `_ message containing your robot's world frame position, transformed into GPS coordinates + +Published Transforms +==================== +* ``world_frame->utm`` (optional) - If the ``broadcast_utm_transform`` parameter is set to *true*, ``navsat_transform_node`` calculates a transform from the *utm* frame to the ``frame_id`` of the input odometry data. By default, the *utm* frame is published as a child of the odometry frame by using the inverse transform. With use of the ``broadcast_utm_transform_as_parent_frame`` parameter, the *utm* frame will be published as a parent of the odometry frame. This is useful if you have multiple robots within one TF tree. diff --git a/Localizations/Packages/robot_localization/doc/preparing_sensor_data.rst b/Localizations/Packages/robot_localization/doc/preparing_sensor_data.rst new file mode 100644 index 0000000..2d67e6c --- /dev/null +++ b/Localizations/Packages/robot_localization/doc/preparing_sensor_data.rst @@ -0,0 +1,104 @@ +Preparing Your Data for Use with robot_localization +################################################### + +Before getting started with the state estimation nodes in ``robot_localization``, it is important that users ensure that their sensor data well-formed. There are various considerations for each class of sensor data, and users are encouraged to read this tutorial in its entirety before attempting to use ``robot_localization``. + +For additional information, users are encouraged to watch this `presentation `_ from ROSCon 2015. + +Adherence to ROS Standards +************************** + +The two most important ROS REPs to consider are: + +* `REP-103 `_ (Standard Units of Measure and Coordinate Conventions) +* `REP-105 `_ (Coordinate Frame Conventions). + +Users who are new to ROS or state estimation are encouraged to read over both REPs, as it will almost certainly aid you in preparing your sensor data. ``robot_localization`` attempts to adhere to these standards as much as possible. + +Also, it will likely benefit users to look over the specifications for each of the supported ROS message types: + +* `nav_msgs/Odometry `_ + +* `geometry_msgs/PoseWithCovarianceStamped `_ + +* `geometry_msgs/TwistWithCovarianceStamped `_ + +* `sensor_msgs/Imu `_ + +Coordinate Frames and Transforming Sensor Data +********************************************** + +`REP-105 `_ specifies four principal coordinate frames: *base_link*, *odom*, *map*, and *earth*. The *base_link* frame is rigidly affixed to the robot. The *map* and *odom* frames are world-fixed frames whose origins are typically aligned with the robot's start position. The *earth* frame is used to provide a common reference frame for multiple *map* frames (e.g., for robots distributed over a large area). The *earth* frame is not relevant to this tutorial. + +The state estimation nodes of ``robot_localization`` produce a state estimate whose pose is given in the *map* or *odom* frame and whose velocity is given in the *base_link* frame. All incoming data is transformed into one of these coordinate frames before being fused with the state. The data in each message type is transformed as follows: + +* `nav_msgs/Odometry `_ - All pose data (position and orientation) is transformed from the message header's ``frame_id`` into the coordinate frame specified by the ``world_frame`` parameter (typically *map* or *odom*). In the message itself, this specifically refers to everything contained within the ``pose`` property. All twist data (linear and angular velocity) is transformed from the ``child_frame_id`` of the message into the coordinate frame specified by the ``base_link_frame`` parameter (typically *base_link*). +* `geometry_msgs/PoseWithCovarianceStamped `_ - Handled in the same fashion as the pose data in the Odometry message. +* `geometry_msgs/TwistWithCovarianceStamped `_ - Handled in the same fashion as the twist data in the Odometry message. +* `sensor_msgs/Imu `_ - The IMU message is currently subject to some ambiguity, though this is being addressed by the ROS community. Most IMUs natively report orientation data in a world-fixed frame whose :math:`X` and :math:`Z` axes are defined by the vectors pointing to magnetic north and the center of the earth, respectively, with the Y axis facing east (90 degrees offset from the magnetic north vector). This frame is often referred to as NED (North, East, Down). However, `REP-103 `_ specifies an ENU (East, North, Up) coordinate frame for outdoor navigation. As of this writing, ``robot_localization`` assumes an ENU frame for all IMU data, and does not work with NED frame data. This may change in the future, but for now, users should ensure that data is transformed to the ENU frame before using it with any node in ``robot_localization``. + +The IMU may also be oriented on the robot in a position other than its "neutral" position. For example, the user may mount the IMU on its side, or rotate it so that it faces a direction other than the front of the robot. This offset is typically specified by a static transform from the ``base_link_frame`` parameter to the IMU message's ``frame_id``. The state estimation nodes in ``robot_localization`` will automatically correct for the orientation of the sensor so that its data aligns with the frame specified by the ``base_link_frame`` parameter. + +Handling tf_prefix +****************** + +With the migration to `tf2 `_ as of ROS Indigo, ``robot_localization`` still allows for the use of the ``tf_prefix`` parameter, but, in accordance with `tf2 `_, all ``frame_id`` values will have any leading '/' stripped. + +Considerations for Each Sensor Message Type +******************************************* + +Odometry +======== + +Many robot platforms come equipped with wheel encoders that provide instantaneous translational and rotational velocity. Many also internally integrate these velocities to generate a position estimate. If you are responsible for this data, or can edit it, keep the following in mind: + +1. **Velocities/Poses:** `robot_localization` can integrate velocities or absolute pose information. In general, the best practice is: + + * If the odometry provides both position and linear velocity, fuse the linear velocity. + * If the odometry provides both orientation and angular velocity, fuse the orientation. + + .. note:: If you have two sources of orientation data, then you'll want to be careful. If both produce orientations with accurate covariance matrices, it's safe to fuse the orientations. If, however, one or both under-reports its covariance, then you should only fuse the orientation data from the more accurate sensor. For the other sensor, use the angular velocity (if it's provided), or continue to fuse the absolute orientation data, but turn `_differential` mode on for that sensor. + +2. **frame_id:** See the section on coordinate frames and transforms above. + +3. **Covariance:** Covariance values **matter** to ``robot_localization``. `robot_pose_ekf `_ attempts to fuse all pose variables in an odometry message. Some robots' drivers have been written to accommodate its requirements. This means that if a given sensor does not produce a certain variable (e.g., a robot that doesn't report :math:`Z` position), then the only way to get ``robot_pose_ekf`` to ignore it is to inflate its variance to a very large value (on the order of :math:`1e3`) so that the variable in question is effectively ignored. This practice is both unnecessary and even detrimental to the performance of ``robot_localization``. The exception is the case where you have a second input source measuring the variable in question, in which case inflated covariances will work. + + .. note:: See :ref:`configuring_robot_localization` and :ref:`migrating_from_robot_pose_ekf` for more information. + +4. **Signs:** Adherence to `REP-103 `_ means that you need to ensure that the **signs** of your data are correct. For example, if you have a ground robot and turn it counter-clockwise, then its yaw angle should *increase*, and its yaw velocity should be *positive*. If you drive it *forward*, its X-position should *increase* and its X-velocity should be *positive*. + +5. **Transforms:** Broadcast of the *odom*->*base_link* transform. When the ``world_frame`` parameter is set to the value of the ``odom_frame`` parameter in the configuration file, ``robot_localization``'s state estimation nodes output both a position estimate in a `nav_msgs/Odometry `_ message and a transform from the frame specified by its ``odom_frame`` parameter to its ``base_link_frame`` parameter. However, some robot drivers also broadcast this transform along with their odometry message. If users want ``robot_localization`` to be responsible for this transform, then they need to disable the broadcast of that transform by their robot's driver. This is often exposed as a parameter. + +IMU +=== + +In addition to the following, be sure to read the above section regarding coordinate frames and transforms for IMU data. + +1. **Adherence to specifications:** As with odometry, be sure your data adheres to `REP-103 `_ and the `sensor_msgs/Imu `_ specification. Double-check the signs of your data, and make sure the ``frame_id`` values are correct. + +2. **Covariance:** Echoing the advice for odometry, make sure your covariances make sense. Do not use large values to get the filter to ignore a given variable. Set the configuration for the variable you'd like to ignore to *false*. + +3. **Acceleration:** Be careful with acceleration data. The state estimation nodes in ``robot_localization`` assume that an IMU that is placed in its neutral *right-side-up* position on a flat surface will: + + * Measure **+**:math:`9.81` meters per second squared for the :math:`Z` axis. + * If the sensor is rolled **+**:math:`90` degrees (left side up), the acceleration should be **+**:math:`9.81` meters per second squared for the :math:`Y` axis. + * If the sensor is pitched **+**:math:`90` degrees (front side down), it should read **-**:math:`9.81` meters per second squared for the :math:`X` axis. + +PoseWithCovarianceStamped +========================= + +See the section on odometry. + +TwistWithCovarianceStamped +========================== + +See the section on odometry. + +Common errors +************* + +* Input data doesn't adhere to `REP-103 `_. Make sure that all values (especially orientation angles) increase and decrease in the correct directions. +* Incorrect ``frame_id`` values. Velocity data should be reported in the frame given by the ``base_link_frame`` parameter, or a transform should exist between the ``frame_id`` of the velocity data and the ``base_link_frame``. +* Inflated covariances. The preferred method for ignoring variables in measurements is through the ``odomN_config`` parameter. +* Missing covariances. If you have configured a given sensor to fuse a given variable into the state estimation node, then the variance for that value (i.e., the covariance matrix value at position :math:`(i, i)`, where :math:`i` is the index of that variable) should **not** be :math:`0`. If a :math:`0` variance value is encountered for a variable that is being fused, the state estimation nodes will add a small epsilon value (:math:`1e^{-6}`) to that value. A better solution is for users to set covariances appropriately. + diff --git a/Localizations/Packages/robot_localization/doc/robot_localization_ias13_revised.pdf b/Localizations/Packages/robot_localization/doc/robot_localization_ias13_revised.pdf new file mode 100644 index 0000000..4387137 Binary files /dev/null and b/Localizations/Packages/robot_localization/doc/robot_localization_ias13_revised.pdf differ diff --git a/Localizations/Packages/robot_localization/doc/state_estimation_nodes.rst b/Localizations/Packages/robot_localization/doc/state_estimation_nodes.rst new file mode 100644 index 0000000..a913803 --- /dev/null +++ b/Localizations/Packages/robot_localization/doc/state_estimation_nodes.rst @@ -0,0 +1,336 @@ +State Estimation Nodes +###################### + +ekf_localization_node +********************* +``ekf_localization_node`` is an implementation of an `extended Kalman filter `_. It uses an omnidirectional motion model to project the state forward in time, and corrects that projected estimate using perceived sensor data. + +ukf_localization_node +********************* +``ukf_localization_node`` is an implementation of an `unscented Kalman filter `_. It uses a set of carefully selected sigma points to project the state through the same motion model that is used in the EKF, and then uses those projected sigma points to recover the state estimate and covariance. This eliminates the use of Jacobian matrices and makes the filter more stable. However, it is also more computationally taxing than ``ekf_localization_node``. + +Parameters +********** + +``ekf_localization_node`` and ``ukf_localization_node`` share the vast majority of their parameters, as most of the parameters control how data is treated before being fused with the core filters. + +The relatively large number of parameters available to the state estimation nodes make launch and configuration files the preferred method for starting any of its nodes. The package contains template launch and configuration files to help get users started. + +Parameters Common to *ekf_localization_node* and *ukf_localization_node* +======================================================================== + +Standard Parameters +------------------- + +~frequency +^^^^^^^^^^ +The real-valued frequency, in Hz, at which the filter produces a state estimate. + +.. note:: The filter will not begin computation until it receives at least one message from one of the inputs. + +~sensor_timeout +^^^^^^^^^^^^^^^ +The real-valued period, in seconds, after which we consider any sensor to have timed out. In this event, we carry out a predict cycle on the EKF without correcting it. This parameter can be thought of as the inverse of the minimum frequency at which the filter will generate *new* output. + +~two_d_mode +^^^^^^^^^^^ +If your robot is operating in a planar environment and you're comfortable with ignoring the subtle variations in the ground (as reported by an IMU), then set this to true. It will fuse 0 values for all 3D variables (Z, roll, pitch, and their respective velocities and accelerations). This keeps the covariances for those values from exploding while ensuring that your robot's state estimate remains affixed to the X-Y plane. + +~[frame] +^^^^^^^^^ +Specific parameters: + +* ``~map_frame`` +* ``~odom_frame`` +* ``~base_link_frame`` +* ``~base_link_frame_output`` +* ``~world_frame`` + +These parameters define the operating "mode" for ``robot_localization``. `REP-105 `_ specifies three principal coordinate frames: *map*, *odom*, and *base_link*. *base_link* is the coordinate frame that is affixed to the robot. The robot's position in the *odom* frame will drift over time, but is accurate in the short term and should be continuous. The *map* frame, like the *odom* frame, is a world-fixed coordinate frame, and while it contains the most globally accurate position estimate for your robot, it is subject to discrete jumps, e.g., due to the fusion of GPS data. Here is how to use these parameters: + +1. Set the ``map_frame``, ``odom_frame``, and ``base_link_frame`` parameters to the appropriate frame names for your system. + + .. note:: If your system does not have a ``map_frame``, just remove it, and make sure ``world_frame`` is set to the value of ``odom_frame``. + .. note:: If you are running multiple EKF instances and would like to "override" the output transform and message to have this frame for its ``child_frame_id``, you may set this. The ``base_link_frame_output`` is optional and will default to the ``base_link_frame``. This helps to enable disconnected TF trees when multiple EKF instances are being run. When the final state is computed, we "override" the output transform and message to have this frame for its ``child_frame_id``. + +2. If you are only fusing continuous position data such as wheel encoder odometry, visual odometry, or IMU data, set ``world_frame`` to your ``odom_frame`` value. This is the default behavior for the state estimation nodes in ``robot_localization``, and the most common use for it. +3. If you are fusing global absolute position data that is subject to discrete jumps (e.g., GPS or position updates from landmark observations) then: + + i. Set your ``world_frame`` to your ``map_frame`` value + ii. **Make sure** something else is generating the *odom->base_link* transform. This can even be another instance of a ``robot_localization`` state estimation node. However, that instance should *not* fuse the global data. + +The default values for ``map_frame``, ``odom_frame``, and ``base_link_frame`` are *map*, *odom,* and *base_link,* respectively. The ``base_link_frame_output`` parameter defaults to the value of ``base_link_frame``. The ``world_frame`` parameter defaults to the value of ``odom_frame``. + +~transform_time_offset +^^^^^^^^^^^^^^^^^^^^^^ +Some packages require that your transforms are future-dated by a small time offset. The value of this parameter will be added to the timestamp of *map->odom* or *odom->base_link* transform being generated by the state estimation nodes in ``robot_localization``. + +~transform_timeout +^^^^^^^^^^^^^^^^^^ +The ``robot_localization`` package uses ``tf2``'s ``lookupTransform`` method to request transformations. This parameter specifies how long we would like to wait if a transformation is not available yet. Defaults to 0 if not set. The value 0 means we just get us the latest available (see ``tf2`` implementation) transform so we are not blocking the filter. Specifying a non-zero `transform_timeout` affects the filter's timing since it waits for a maximum of the `transform_timeout` for a transform to become available. This directly implies that mostly the specified desired output rate is not met since the filter has to wait for transforms when updating. + +~[sensor] +^^^^^^^^^ +For each sensor, users need to define this parameter based on the message type. For example, if we define one source of Imu messages and two sources of Odometry messages, the configuration would look like this: + +.. code-block:: xml + + + + + +The index for each parameter name is 0-based (e.g., ``odom0``, ``odom1``, etc.) and must be defined sequentially (e.g., do *not* use ``pose0`` and ``pose2`` if you have not defined ``pose1``). The values for each parameter are the topic name for that sensor. + +~[sensor]_config +^^^^^^^^^^^^^^^^ + +Specific parameters: + +* ``~odomN_config`` +* ``~twistN_config`` +* ``~imuN_config`` +* ``~poseN_config`` + +For each of the sensor messages defined above, users must specify what variables of those messages should be fused into the final state estimate. An example odometry configuration might look like this: + +.. code-block:: xml + + [true, true, false, + false, false, true, + true, false, false, + false, false, true, + false, false, false] + + +The order of the boolean values are :math:`X, Y, Z, roll, pitch, yaw, \dot{X}, \dot{Y}, \dot{Z}, \dot{roll}, \dot{pitch}, \dot{yaw}, \ddot{X}, \ddot{Y}, \ddot{Z}`. In this example, we are fusing :math:`X` and :math:`Y` position, :math:`yaw`, :math:`\dot{X}`, and :math:`\dot{yaw}`. + +.. note:: The specification is done in the ``frame_id`` of the **sensor**, *not* in the ``world_frame`` or ``base_link_frame``. Please see the :doc:`coniguration tutorial ` for more information. + +~[sensor]_queue_size +^^^^^^^^^^^^^^^^^^^^ + +Specific parameters: + +* ``~odomN_queue_size`` +* ``~twistN_queue_size`` +* ``~imuN_queue_size`` +* ``~poseN_queue_size`` + +Users can use these parameters to adjust the callback queue sizes for each sensor. This is useful if your ``frequency`` parameter value is much lower than your sensor's frequency, as it allows the filter to incorporate all measurements that arrived in between update cycles. + +~[sensor]_differential +^^^^^^^^^^^^^^^^^^^^^^ + +Specific parameters: + +* ``~odomN_differential`` +* ``~imuN_differential`` +* ``~poseN_differential`` + +For each of the sensor messages defined above *that contain pose information*, users can specify whether the pose variables should be integrated differentially. If a given value is set to *true*, then for a measurement at time :math:`t` from the sensor in question, we first subtract the measurement at time :math:`t-1`, and convert the resulting value to a velocity. This setting is especially useful if your robot has two sources of absolute pose information, e.g., yaw measurements from odometry and an IMU. In that case, if the variances on the input sources are not configured correctly, these measurements may get out of sync with one another and cause oscillations in the filter, but by integrating one or both of them differentially, we avoid this scenario. + +Users should take care when using this parameter for orientation data, as the conversion to velocity means that the covariance for orientation state variables will grow without bound (unless another source of absolute orientation data is being fused). If you simply want all of your pose variables to start at :math:`0`, then please use the ``_relative`` parameter. + +.. note:: If you are fusing GPS information via ``navsat_transform_node`` or ``utm_transform_node``, you should make sure that the ``_differential`` setting is *false.* + +~[sensor]_relative +^^^^^^^^^^^^^^^^^^ + +Specific parameters: + +* ``~odomN_relative`` +* ``~imuN_relative`` +* ``~poseN_relative`` + +If this parameter is set to ``true``, then any measurements from this sensor will be fused relative to the first measurement received from that sensor. This is useful if, for example, you want your state estimate to always start at :math:`(0, 0, 0)` and with :math:`roll, pitch,` and :math:`yaw` values of :math:`(0, 0, 0)`. It is similar to the ``_differential`` parameter, but instead of removing the measurement at time :math:`t-1`, we always remove the measurement at time :math:`0`, and the measurement is not converted to a velocity. + +~imuN_remove_gravitational_acceleration +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +If fusing accelerometer data from IMUs, this parameter determines whether or not acceleration due to gravity is removed from the acceleration measurement before fusing it. + +.. note:: This assumes that the IMU that is providing the acceleration data is also producing an absolute orientation. The orientation data is required to correctly remove gravitational acceleration. + +~gravitational_acceleration +^^^^^^^^^^^^^^^^^^^^^^^^^^^ +If ``imuN_remove_gravitational_acceleration`` is set to ``true``, then this parameter determines the acceleration in Z due to gravity that will be removed from the IMU's linear acceleration data. Default is 9.80665 (m/s^2). + +~initial_state +^^^^^^^^^^^^^^ +Starts the filter with the specified state. The state is given as a 15-D vector of doubles, in the same order as the sensor configurations. For example, to start your robot at a position of :math:`(5.0, 4.0, 3.0)`, a :math:`yaw` of :math:`1.57`, and a linear velocity of :math:`(0.1, 0.2, 0.3)`, you would use: + +.. code-block:: xml + + [5.0, 4.0, 3.0, + 0.0, 0.0, 1.57, + 0.1, 0.2, 0.3, + 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0] + +~publish_tf +^^^^^^^^^^^ +If *true*, the state estimation node will publish the transform from the frame specified by the ``world_frame`` parameter to the frame specified by the ``base_link_frame`` parameter. Defaults to *true*. + +~publish_acceleration +^^^^^^^^^^^^^^^^^^^^^ +If *true*, the state estimation node will publish the linear acceleration state. Defaults to *false*. + +~permit_corrected_publication +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +When the state estimation nodes publish the state at time `t`, but then receive a measurement with a timestamp < `t`, they re-publish the corrected state, with the same time stamp as the previous publication. Setting this parameter to *false* disables that behavior. Defaults to *false*. + +~print_diagnostics +^^^^^^^^^^^^^^^^^^ +If true, the state estimation node will publish diagnostic messages to the ``/diagnostics`` topic. This is useful for debugging your configuration and sensor data. + +Advanced Parameters +------------------- + +~use_control +^^^^^^^^^^^^ +If *true*, the state estimation node will listen to the `cmd_vel` topic for a `geometry_msgs/Twist `_ message, and use that to generate an acceleration term. This term is then used in the robot's state prediction. This is especially useful in situations where even small amounts of lag in convergence for a given state variable cause problems in your application (e.g., LIDAR shifting during rotations). Defaults to *false*. + +.. note:: The presence and inclusion of linear acceleration data from an IMU will currently "override" the predicted linear acceleration value. + +~stamped_control +^^^^^^^^^^^^^^^^ +If *true* and ``use_control`` is also *true*, looks for a `geometry_msgs/TwistStamped `_ message instead of a `geometry_msgs/Twist `_ message. + +~control_timeout +^^^^^^^^^^^^^^^^ +If ``use_control`` is set to *true* and no control command is received in this amount of time, given in seconds, the control-based acceleration term ceases to be applied. + +~control_config +^^^^^^^^^^^^^^^ +Controls which variables in the ``cmd_vel`` message are used in state prediction. The order of the values is :math:`\dot{X}, \dot{Y}, \dot{Z}, \dot{roll}, \dot{pitch}, \dot{yaw}`. Only used if ``use_control`` is set to *true*. + +.. code-block:: xml + + [true, false, false, + false, false, true] + +~acceleration_limits +^^^^^^^^^^^^^^^^^^^^ +How rapidly your robot can accelerate for each dimension. Matches the parameter order in ``control_config``. Only used if ``use_control`` is set to *true*. + +.. code-block:: xml + + [1.3, 0.0, 0.0, + 0.0, 0.0, 3.2] + +~deceleration_limits +^^^^^^^^^^^^^^^^^^^^ +How rapidly your robot can decelerate for each dimension. Matches the parameter order in ``control_config``. Only used if ``use_control`` is set to *true*. + +~acceleration_gains +^^^^^^^^^^^^^^^^^^^ +If your robot cannot instantaneously reach its acceleration limit, the permitted change can be controlled with these gains. Only used if ``use_control`` is set to *true*. + +.. code-block:: xml + + [0.8, 0.0, 0.0, + 0.0, 0.0, 0.9] + +~deceleration_gains +^^^^^^^^^^^^^^^^^^^ +If your robot cannot instantaneously reach its deceleration limit, the permitted change can be controlled with these gains. Only used if ``use_control`` is set to *true*. + +~smooth_lagged_data +^^^^^^^^^^^^^^^^^^^ +If any of your sensors produce data with timestamps that are older than the most recent filter update (more plainly, if you have a source of lagged sensor data), setting this parameter to *true* will enable the filter, upon reception of lagged data, to revert to the last state prior to the lagged measurement, then process all measurements until the current time. This is especially useful for measurements that come from nodes that require heavy CPU usage to generate pose estimates (e.g., laser scan matchers), as they are frequently lagged behind the current time. + +~history_length +^^^^^^^^^^^^^^^ +If ``smooth_lagged_data`` is set to *true*, this parameter specifies the number of seconds for which the filter will retain its state and measurement history. This value should be at least as large as the time delta between your lagged measurements and the current time. + +~[sensor]_nodelay +^^^^^^^^^^^^^^^^^ + +Specific parameters: + +* ``~odomN_nodelay`` +* ``~twistN_nodelay`` +* ``~imuN_nodelay`` +* ``~poseN_nodelay`` + +If *true*, sets the `tcpNoDelay` `transport hint `_. There is some evidence that Nagle's algorithm intereferes with the timely reception of large message types, such as the `nav_msgs/Odometry `_ message. Setting this to *true* for an input disables Nagle's algorithm for that subscriber. Defaults to *false*. + +~[sensor]_threshold +^^^^^^^^^^^^^^^^^^^ +Specific parameters: + +* ``~odomN_pose_rejection_threshold`` +* ``odomN_twist_rejection_threshold`` +* ``poseN_rejection_threshold`` +* ``twistN_rejection_threshold`` +* ``imuN_pose_rejection_threshold`` +* ``imuN_angular_velocity_rejection_threshold`` +* ``imuN_linear_acceleration_rejection_threshold`` + +If your data is subject to outliers, use these threshold settings, expressed as `Mahalanobis distances `_, to control how far away from the current vehicle state a sensor measurement is permitted to be. Each defaults to ``numeric_limits::max()`` if unspecified. + +~debug +^^^^^^ +Boolean flag that specifies whether or not to run in debug mode. WARNING: setting this to true will generate a massive amount of data. The data is written to the value of the ``debug_out_file`` parameter. Defaults to *false*. + +~debug_out_file +^^^^^^^^^^^^^^^^ +If ``debug`` is *true*, the file to which debug output is written. + +~process_noise_covariance +^^^^^^^^^^^^^^^^^^^^^^^^^ +The process noise covariance, commonly denoted *Q*, is used to model uncertainty in the prediction stage of the filtering algorithms. It can be difficult to tune, and has been exposed as a parameter for easier customization. This parameter can be left alone, but you will achieve superior results by tuning it. In general, the larger the value for *Q* relative to the variance for a given variable in an input message, the faster the filter will converge to the value in the measurement. + +~dynamic_process_noise_covariance +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +If *true*, will dynamically scale the ``process_noise_covariance`` based on the robot's velocity. This is useful, e.g., when you want your robot's estimate error covariance to stop growing when the robot is stationary. Defaults to *false*. + +~initial_estimate_covariance +^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +The estimate covariance, commonly denoted *P*, defines the error in the current state estimate. The parameter allows users to set the initial value for the matrix, which will affect how quickly the filter converges. For example, if users set the value at position :math:`[0, 0]` to a very small value, e.g., `1e-12`, and then attempt to fuse measurements of X position with a high variance value for :math:`X`, then the filter will be very slow to "trust" those measurements, and the time required for convergence will increase. Again, users should take care with this parameter. When only fusing velocity data (e.g., no absolute pose information), users will likely *not* want to set the initial covariance values for the absolute pose variables to large numbers. This is because those errors are going to grow without bound (owing to the lack of absolute pose measurements to reduce the error), and starting them with large values will not benefit the state estimate. + +~reset_on_time_jump +^^^^^^^^^^^^^^^^^^^ +If set to *true* and ``ros::Time::isSimTime()`` is *true*, the filter will reset to its uninitialized state when a jump back in time is detected on a topic. This is useful when working with bag data, in that the bag can be restarted without restarting the node. + +~predict_to_current_time +^^^^^^^^^^^^^^^^^^^^^^^^ +If set to *true*, the filter predicts and corrects up to the time of the latest measurement (by default) but will now also predict up to the current time step. + +~disabled_at_startup +^^^^^^^^^^^^^^^^^^^^ +If set to *true* will not run the filter on start. + +Node-specific Parameters +------------------------ +The standard and advanced parameters are common to all state estimation nodes in ``robot_localization``. This section details parameters that are unique to their respective state estimation nodes. + +ukf_localization_node +^^^^^^^^^^^^^^^^^^^^^ + +The parameters for ``ukf_localization_node`` follow the nomenclature of the `original paper `_ and `wiki article `_. + +* **~alpha** - Controls the spread of sigma points. Unless you are familiar with unscented Kalman filters, it's probably best for this setting to remain at its default value (0.001). + +* **~kappa** - Also control the spread of sigma points. Unless you are familiar with unscented Kalman filters, it's probably best for this setting to remain at its default value (0). + +* **~beta** - Relates to the distribution of the state vector. The default value of 2 implies that the distribution is Gaussian. Like the other parameters, this should remain unchanged unless the user is familiar with unscented Kalman filters. + +Published Topics +================ + +* ``odometry/filtered`` (`nav_msgs/Odometry `_) +* ``accel/filtered`` (`geometry_msgs/AccelWithCovarianceStamped `_) (if enabled) + +Published Transforms +==================== + +* If the user's ``world_frame`` parameter is set to the value of ``odom_frame``, a transform is published from the frame given by the ``odom_frame`` parameter to the frame given by the ``base_link_frame`` parameter. + +* If the user's ``world_frame`` parameter is set to the value of ``map_frame``, a transform is published from the frame given by the ``map_frame`` parameter to the frame given by the ``odom_frame`` parameter. + + .. note:: This mode assumes that another node is broadcasting the transform from the frame given by the ``odom_frame`` parameter to the frame given by the ``base_link_frame`` parameter. This can be another instance of a ``robot_localization`` state estimation node. + +Services +======== + +* ``set_pose`` - By issuing a `geometry_msgs/PoseWithCovarianceStamped `_ message to the ``set_pose`` topic, users can manually set the state of the filter. This is useful for resetting the filter during testing, and allows for interaction with ``rviz``. Alternatively, the state estimation nodes advertise a ``SetPose`` service, whose type is `robot_localization/SetPose `_. diff --git a/Localizations/Packages/robot_localization/doc/user_contributed_tutorials.rst b/Localizations/Packages/robot_localization/doc/user_contributed_tutorials.rst new file mode 100644 index 0000000..0cbe609 --- /dev/null +++ b/Localizations/Packages/robot_localization/doc/user_contributed_tutorials.rst @@ -0,0 +1,13 @@ +User-Contributed Tutorials +########################## + +Here's a list of user-contributed tutorials for robot_localization! + +Tutorials +========= + +* `ros-sensor-fusion-tutorial `_ + A comprehensive end-to-end tutorial for setting up robot_localization for sensor fusion, as well as running through the necessary concepts. (Includes a practical example of setting it up with ultrasonic beacons!) + +* `Kapernikov: The ROS robot_localization package `_ + The documentation of the robot_localization package is quite clear once you know how it works. However, it lacks a hands-on tutorial to help you with your first steps. There are some great examples on how to set up the robot_localization package, but they require good working hardware. This tutorial tries to bridge the gap, using the turtlesim package as a virtual robot. diff --git a/Localizations/Packages/robot_localization/include/robot_localization/ekf.h b/Localizations/Packages/robot_localization/include/robot_localization/ekf.h new file mode 100644 index 0000000..80218e9 --- /dev/null +++ b/Localizations/Packages/robot_localization/include/robot_localization/ekf.h @@ -0,0 +1,87 @@ +/* + * Copyright (c) 2014, 2015, 2016, Charles River Analytics, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef ROBOT_LOCALIZATION_EKF_H +#define ROBOT_LOCALIZATION_EKF_H + +#include "robot_localization/filter_base.h" + +#include +#include +#include +#include + +namespace RobotLocalization +{ + +//! @brief Extended Kalman filter class +//! +//! Implementation of an extended Kalman filter (EKF). This +//! class derives from FilterBase and overrides the predict() +//! and correct() methods in keeping with the discrete time +//! EKF algorithm. +//! +class Ekf: public FilterBase +{ + public: + //! @brief Constructor for the Ekf class + //! + //! @param[in] args - Generic argument container (not used here, but + //! needed so that the ROS filters can pass arbitrary arguments to + //! templated filter types). + //! + explicit Ekf(std::vector args = std::vector()); + + //! @brief Destructor for the Ekf class + //! + ~Ekf(); + + //! @brief Carries out the correct step in the predict/update cycle. + //! + //! @param[in] measurement - The measurement to fuse with our estimate + //! + void correct(const Measurement &measurement); + + //! @brief Carries out the predict step in the predict/update cycle. + //! + //! Projects the state and error matrices forward using a model of + //! the vehicle's motion. + //! + //! @param[in] referenceTime - The time at which the prediction is being made + //! @param[in] delta - The time step over which to predict. + //! + void predict(const double referenceTime, const double delta); +}; + +} // namespace RobotLocalization + +#endif // ROBOT_LOCALIZATION_EKF_H diff --git a/Localizations/Packages/robot_localization/include/robot_localization/filter_base.h b/Localizations/Packages/robot_localization/include/robot_localization/filter_base.h new file mode 100644 index 0000000..825b14c --- /dev/null +++ b/Localizations/Packages/robot_localization/include/robot_localization/filter_base.h @@ -0,0 +1,534 @@ +/* + * Copyright (c) 2014, 2015, 2016, Charles River Analytics, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef ROBOT_LOCALIZATION_FILTER_BASE_H +#define ROBOT_LOCALIZATION_FILTER_BASE_H + +#include "robot_localization/filter_utilities.h" +#include "robot_localization/filter_common.h" + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace RobotLocalization +{ + +//! @brief Structure used for storing and comparing measurements +//! (for priority queues) +//! +//! Measurement units are assumed to be in meters and radians. +//! Times are real-valued and measured in seconds. +//! +struct Measurement +{ + // The time stamp of the most recent control term (needed for lagged data) + double latestControlTime_; + + // The Mahalanobis distance threshold in number of sigmas + double mahalanobisThresh_; + + // The real-valued time, in seconds, since some epoch + // (presumably the start of execution, but any will do) + double time_; + + // The topic name for this measurement. Needed + // for capturing previous state values for new + // measurements. + std::string topicName_; + + // This defines which variables within this measurement + // actually get passed into the filter. std::vector + // is generally frowned upon, so we use ints. + std::vector updateVector_; + + // The most recent control vector (needed for lagged data) + Eigen::VectorXd latestControl_; + + // The measurement and its associated covariance + Eigen::VectorXd measurement_; + Eigen::MatrixXd covariance_; + + // We want earlier times to have greater priority + bool operator()(const boost::shared_ptr &a, const boost::shared_ptr &b) + { + return (*this)(*(a.get()), *(b.get())); + } + + bool operator()(const Measurement &a, const Measurement &b) + { + return a.time_ > b.time_; + } + + Measurement() : + latestControlTime_(0.0), + mahalanobisThresh_(std::numeric_limits::max()), + time_(0.0), + topicName_("") + { + } +}; +typedef boost::shared_ptr MeasurementPtr; + +//! @brief Structure used for storing and comparing filter states +//! +//! This structure is useful when higher-level classes need to remember filter history. +//! Measurement units are assumed to be in meters and radians. +//! Times are real-valued and measured in seconds. +//! +struct FilterState +{ + // The time stamp of the most recent measurement for the filter + double lastMeasurementTime_; + + // The time stamp of the most recent control term + double latestControlTime_; + + // The most recent control vector + Eigen::VectorXd latestControl_; + + // The filter state vector + Eigen::VectorXd state_; + + // The filter error covariance matrix + Eigen::MatrixXd estimateErrorCovariance_; + + // We want the queue to be sorted from latest to earliest timestamps. + bool operator()(const FilterState &a, const FilterState &b) + { + return a.lastMeasurementTime_ < b.lastMeasurementTime_; + } + + FilterState() : + lastMeasurementTime_(0.0), + latestControlTime_(0.0) + {} +}; +typedef boost::shared_ptr FilterStatePtr; + +class FilterBase +{ + public: + //! @brief Constructor for the FilterBase class + //! + FilterBase(); + + //! @brief Destructor for the FilterBase class + //! + virtual ~FilterBase(); + + //! @brief Resets filter to its unintialized state + //! + void reset(); + + //! @brief Computes a dynamic process noise covariance matrix using the parameterized state + //! + //! This allows us to, e.g., not increase the pose covariance values when the vehicle is not moving + //! + //! @param[in] state - The STATE_SIZE state vector that is used to generate the dynamic process noise covariance + //! + void computeDynamicProcessNoiseCovariance(const Eigen::VectorXd &state, const double delta); + + //! @brief Carries out the correct step in the predict/update cycle. This method + //! must be implemented by subclasses. + //! + //! @param[in] measurement - The measurement to fuse with the state estimate + //! + virtual void correct(const Measurement &measurement) = 0; + + //! @brief Returns the control vector currently being used + //! + //! @return The control vector + //! + const Eigen::VectorXd& getControl(); + + //! @brief Returns the time at which the control term was issued + //! + //! @return The time the control vector was issued + //! + double getControlTime(); + + //! @brief Gets the value of the debug_ variable. + //! + //! @return True if in debug mode, false otherwise + //! + bool getDebug(); + + //! @brief Gets the estimate error covariance + //! + //! @return A copy of the estimate error covariance matrix + //! + const Eigen::MatrixXd& getEstimateErrorCovariance(); + + //! @brief Gets the filter's initialized status + //! + //! @return True if we've received our first measurement, false otherwise + //! + bool getInitializedStatus(); + + //! @brief Gets the most recent measurement time + //! + //! @return The time at which we last received a measurement + //! + double getLastMeasurementTime(); + + //! @brief Gets the filter's predicted state, i.e., the + //! state estimate before correct() is called. + //! + //! @return A constant reference to the predicted state + //! + const Eigen::VectorXd& getPredictedState(); + + //! @brief Gets the filter's process noise covariance + //! + //! @return A constant reference to the process noise covariance + //! + const Eigen::MatrixXd& getProcessNoiseCovariance(); + + //! @brief Gets the sensor timeout value (in seconds) + //! + //! @return The sensor timeout value + //! + double getSensorTimeout(); + + //! @brief Gets the filter state + //! + //! @return A constant reference to the current state + //! + const Eigen::VectorXd& getState(); + + //! @brief Carries out the predict step in the predict/update cycle. + //! Projects the state and error matrices forward using a model of + //! the vehicle's motion. This method must be implemented by subclasses. + //! + //! @param[in] referenceTime - The time at which the prediction is being made + //! @param[in] delta - The time step over which to predict. + //! + virtual void predict(const double referenceTime, const double delta) = 0; + + //! @brief Does some final preprocessing, carries out the predict/update cycle + //! + //! @param[in] measurement - The measurement object to fuse into the filter + //! + virtual void processMeasurement(const Measurement &measurement); + + //! @brief Sets the most recent control term + //! + //! @param[in] control - The control term to be applied + //! @param[in] controlTime - The time at which the control in question was received + //! + void setControl(const Eigen::VectorXd &control, const double controlTime); + + //! @brief Sets the control update vector and acceleration limits + //! + //! @param[in] updateVector - The values the control term affects + //! @param[in] controlTimeout - Timeout value, in seconds, after which a control is considered stale + //! @param[in] accelerationLimits - The acceleration limits for the control variables + //! @param[in] accelerationGains - Gains applied to the control term-derived acceleration + //! @param[in] decelerationLimits - The deceleration limits for the control variables + //! @param[in] decelerationGains - Gains applied to the control term-derived deceleration + //! + void setControlParams(const std::vector &updateVector, const double controlTimeout, + const std::vector &accelerationLimits, const std::vector &accelerationGains, + const std::vector &decelerationLimits, const std::vector &decelerationGains); + + //! @brief Sets the filter into debug mode + //! + //! NOTE: this will generates a lot of debug output to the provided stream. + //! The value must be a pointer to a valid ostream object. + //! + //! @param[in] debug - Whether or not to place the filter in debug mode + //! @param[in] outStream - If debug is true, then this must have a valid pointer. + //! If the pointer is invalid, the filter will not enter debug mode. If debug is + //! false, outStream is ignored. + //! + void setDebug(const bool debug, std::ostream *outStream = NULL); + + //! @brief Enables dynamic process noise covariance calculation + //! + //! @param[in] dynamicProcessNoiseCovariance - Whether or not to compute dynamic process noise covariance matrices + //! + void setUseDynamicProcessNoiseCovariance(const bool dynamicProcessNoiseCovariance); + + //! @brief Manually sets the filter's estimate error covariance + //! + //! @param[in] estimateErrorCovariance - The state to set as the filter's current state + //! + void setEstimateErrorCovariance(const Eigen::MatrixXd &estimateErrorCovariance); + + //! @brief Sets the filter's last measurement time. + //! + //! @param[in] lastMeasurementTime - The last measurement time of the filter + //! + void setLastMeasurementTime(const double lastMeasurementTime); + + //! @brief Sets the process noise covariance for the filter. + //! + //! This enables external initialization, which is important, as this + //! matrix can be difficult to tune for a given implementation. + //! + //! @param[in] processNoiseCovariance - The STATE_SIZExSTATE_SIZE process noise covariance matrix + //! to use for the filter + //! + void setProcessNoiseCovariance(const Eigen::MatrixXd &processNoiseCovariance); + + //! @brief Sets the sensor timeout + //! + //! @param[in] sensorTimeout - The time, in seconds, for a sensor to be + //! considered having timed out + //! + void setSensorTimeout(const double sensorTimeout); + + //! @brief Manually sets the filter's state + //! + //! @param[in] state - The state to set as the filter's current state + //! + void setState(const Eigen::VectorXd &state); + + //! @brief Ensures a given time delta is valid (helps with bag file playback issues) + //! + //! @param[in] delta - The time delta, in seconds, to validate + //! + void validateDelta(double &delta); + + protected: + //! @brief Method for settings bounds on acceleration values derived from controls + //! @param[in] state - The current state variable (e.g., linear X velocity) + //! @param[in] control - The current control commanded velocity corresponding to the state variable + //! @param[in] accelerationLimit - Limit for acceleration (regardless of driving direction) + //! @param[in] accelerationGain - Gain applied to acceleration control error + //! @param[in] decelerationLimit - Limit for deceleration (moving towards zero, regardless of driving direction) + //! @param[in] decelerationGain - Gain applied to deceleration control error + //! @return a usable acceleration estimate for the control vector + //! + inline double computeControlAcceleration(const double state, const double control, const double accelerationLimit, + const double accelerationGain, const double decelerationLimit, const double decelerationGain) + { + FB_DEBUG("---------- FilterBase::computeControlAcceleration ----------\n"); + + const double error = control - state; + const bool sameSign = (::fabs(error) <= ::fabs(control) + 0.01); + const double setPoint = (sameSign ? control : 0.0); + const bool decelerating = ::fabs(setPoint) < ::fabs(state); + double limit = accelerationLimit; + double gain = accelerationGain; + + if (decelerating) + { + limit = decelerationLimit; + gain = decelerationGain; + } + + const double finalAccel = std::min(std::max(gain * error, -limit), limit); + + FB_DEBUG("Control value: " << control << "\n" << + "State value: " << state << "\n" << + "Error: " << error << "\n" << + "Same sign: " << (sameSign ? "true" : "false") << "\n" << + "Set point: " << setPoint << "\n" << + "Decelerating: " << (decelerating ? "true" : "false") << "\n" << + "Limit: " << limit << "\n" << + "Gain: " << gain << "\n" << + "Final is " << finalAccel << "\n"); + + return finalAccel; + } + + //! @brief Keeps the state Euler angles in the range [-pi, pi] + //! + virtual void wrapStateAngles(); + + //! @brief Tests if innovation is within N-sigmas of covariance. Returns true if passed the test. + //! @param[in] innovation - The difference between the measurement and the state + //! @param[in] invCovariance - The innovation error + //! @param[in] nsigmas - Number of standard deviations that are considered acceptable + //! + virtual bool checkMahalanobisThreshold(const Eigen::VectorXd &innovation, + const Eigen::MatrixXd &invCovariance, + const double nsigmas); + + //! @brief Converts the control term to an acceleration to be applied in the prediction step + //! @param[in] referenceTime - The time of the update (measurement used in the prediction step) + //! @param[in] predictionDelta - The amount of time over which we are carrying out our prediction + //! + void prepareControl(const double referenceTime, const double predictionDelta); + + //! @brief Whether or not we've received any measurements + //! + bool initialized_; + + //! @brief Whether or not we apply the control term + //! + bool useControl_; + + //! @brief If true, uses the robot's vehicle state and the static process noise covariance matrix to generate a + //! dynamic process noise covariance matrix + //! + bool useDynamicProcessNoiseCovariance_; + + //! @brief Tracks the time the filter was last updated using a measurement. + //! + //! This value is used to monitor sensor readings with respect to the sensorTimeout_. + //! We also use it to compute the time delta values for our prediction step. + //! + double lastMeasurementTime_; + + //! @brief The time of reception of the most recent control term + //! + double latestControlTime_; + + //! @brief Timeout value, in seconds, after which a control is considered stale + //! + double controlTimeout_; + + //! @brief The updates to the filter - both predict and correct - are driven + //! by measurements. If we get a gap in measurements for some reason, we want + //! the filter to continue estimating. When this gap occurs, as specified by + //! this timeout, we will continue to call predict() at the filter's frequency. + //! + double sensorTimeout_; + + //! @brief Which control variables are being used (e.g., not every vehicle is controllable in Y or Z) + //! + std::vector controlUpdateVector_; + + //! @brief Gains applied to acceleration derived from control term + //! + std::vector accelerationGains_; + + //! @brief Caps the acceleration we apply from control input + //! + std::vector accelerationLimits_; + + //! @brief Gains applied to deceleration derived from control term + //! + std::vector decelerationGains_; + + //! @brief Caps the deceleration we apply from control input + //! + std::vector decelerationLimits_; + + //! @brief Variable that gets updated every time we process a measurement and we have a valid control + //! + Eigen::VectorXd controlAcceleration_; + + //! @brief Latest control term + //! + Eigen::VectorXd latestControl_; + + //! @brief Holds the last predicted state of the filter + //! + Eigen::VectorXd predictedState_; + + //! @brief This is the robot's state vector, which is what we are trying to + //! filter. The values in this vector are what get reported by the node. + //! + Eigen::VectorXd state_; + + //! @brief Covariance matrices can be incredibly unstable. We can + //! add a small value to it at each iteration to help maintain its + //! positive-definite property. + //! + Eigen::MatrixXd covarianceEpsilon_; + + //! @brief Gets updated when useDynamicProcessNoise_ is true + //! + Eigen::MatrixXd dynamicProcessNoiseCovariance_; + + //! @brief This matrix stores the total error in our position + //! estimate (the state_ variable). + //! + Eigen::MatrixXd estimateErrorCovariance_; + + //! @brief We need the identity for a few operations. Better to store it. + //! + Eigen::MatrixXd identity_; + + //! @brief As we move through the world, we follow a predict/update + //! cycle. If one were to imagine a scenario where all we did was make + //! predictions without correcting, the error in our position estimate + //! would grow without bound. This error is stored in the + //! stateEstimateCovariance_ matrix. However, this matrix doesn't answer + //! the question of *how much* our error should grow for each time step. + //! That's where the processNoiseCovariance matrix comes in. When we + //! make a prediction using the transfer function, we add this matrix + //! (times deltaT) to the state estimate covariance matrix. + //! + Eigen::MatrixXd processNoiseCovariance_; + + //! @brief The Kalman filter transfer function + //! + //! Kalman filters and extended Kalman filters project the current + //! state forward in time. This is the "predict" part of the predict/correct + //! cycle. A Kalman filter has a (typically constant) matrix A that defines + //! how to turn the current state, x, into the predicted next state. For an + //! EKF, this matrix becomes a function f(x). However, this function can still + //! be expressed as a matrix to make the math a little cleaner, which is what + //! we do here. Technically, each row in the matrix is actually a function. + //! Some rows will contain many trigonometric functions, which are of course + //! non-linear. In any case, you can think of this as the 'A' matrix in the + //! Kalman filter formulation. + //! + Eigen::MatrixXd transferFunction_; + + //! @brief The Kalman filter transfer function Jacobian + //! + //! The transfer function is allowed to be non-linear in an EKF, but + //! for propagating (predicting) the covariance matrix, we need to linearize + //! it about the current mean (i.e., state). This is done via a Jacobian, + //! which calculates partial derivatives of each row of the transfer function + //! matrix with respect to each state variable. + //! + Eigen::MatrixXd transferFunctionJacobian_; + + //! @brief Used for outputting debug messages + //! + std::ostream *debugStream_; + + private: + //! @brief Whether or not the filter is in debug mode + //! + bool debug_; +}; + +} // namespace RobotLocalization + +#endif // ROBOT_LOCALIZATION_FILTER_BASE_H diff --git a/Localizations/Packages/robot_localization/include/robot_localization/filter_common.h b/Localizations/Packages/robot_localization/include/robot_localization/filter_common.h new file mode 100644 index 0000000..30d1c54 --- /dev/null +++ b/Localizations/Packages/robot_localization/include/robot_localization/filter_common.h @@ -0,0 +1,97 @@ +/* + * Copyright (c) 2014, 2015, 2016, Charles River Analytics, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef ROBOT_LOCALIZATION_FILTER_COMMON_H +#define ROBOT_LOCALIZATION_FILTER_COMMON_H + +namespace RobotLocalization +{ + +//! @brief Enumeration that defines the state vector +//! +enum StateMembers +{ + StateMemberX = 0, + StateMemberY, + StateMemberZ, + StateMemberRoll, + StateMemberPitch, + StateMemberYaw, + StateMemberVx, + StateMemberVy, + StateMemberVz, + StateMemberVroll, + StateMemberVpitch, + StateMemberVyaw, + StateMemberAx, + StateMemberAy, + StateMemberAz +}; + +//! @brief Enumeration that defines the control vector +//! +enum ControlMembers +{ + ControlMemberVx, + ControlMemberVy, + ControlMemberVz, + ControlMemberVroll, + ControlMemberVpitch, + ControlMemberVyaw +}; + +//! @brief Global constants that define our state +//! vector size and offsets to groups of values +//! within that state. +const int STATE_SIZE = 15; +const int POSITION_OFFSET = StateMemberX; +const int ORIENTATION_OFFSET = StateMemberRoll; +const int POSITION_V_OFFSET = StateMemberVx; +const int ORIENTATION_V_OFFSET = StateMemberVroll; +const int POSITION_A_OFFSET = StateMemberAx; + +//! @brief Pose and twist messages each +//! contain six variables +const int POSE_SIZE = 6; +const int TWIST_SIZE = 6; +const int POSITION_SIZE = 3; +const int ORIENTATION_SIZE = 3; +const int LINEAR_VELOCITY_SIZE = 3; +const int ACCELERATION_SIZE = 3; + +//! @brief Common variables +const double PI = 3.141592653589793; +const double TAU = 6.283185307179587; + +} // namespace RobotLocalization + +#endif // ROBOT_LOCALIZATION_FILTER_COMMON_H diff --git a/Localizations/Packages/robot_localization/include/robot_localization/filter_utilities.h b/Localizations/Packages/robot_localization/include/robot_localization/filter_utilities.h new file mode 100644 index 0000000..61e93c9 --- /dev/null +++ b/Localizations/Packages/robot_localization/include/robot_localization/filter_utilities.h @@ -0,0 +1,71 @@ +/* + * Copyright (c) 2014, 2015, 2016, Charles River Analytics, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef ROBOT_LOCALIZATION_FILTER_UTILITIES_H +#define ROBOT_LOCALIZATION_FILTER_UTILITIES_H + +#include + +#include +#include +#include +#include + +#define FB_DEBUG(msg) if (getDebug()) { *debugStream_ << msg; } + +// Handy methods for debug output +std::ostream& operator<<(std::ostream& os, const Eigen::MatrixXd &mat); +std::ostream& operator<<(std::ostream& os, const Eigen::VectorXd &vec); +std::ostream& operator<<(std::ostream& os, const std::vector &vec); +std::ostream& operator<<(std::ostream& os, const std::vector &vec); + +namespace RobotLocalization +{ +namespace FilterUtilities +{ + + //! @brief Utility method keeping RPY angles in the range [-pi, pi] + //! @param[in] rotation - The rotation to bind + //! @return the bounded value + //! + double clampRotation(double rotation); + + //! @brief Utility method for appending tf2 prefixes cleanly + //! @param[in] tfPrefix - the tf2 prefix to append + //! @param[in, out] frameId - the resulting frame_id value + //! + void appendPrefix(std::string tfPrefix, std::string &frameId); + +} // namespace FilterUtilities +} // namespace RobotLocalization + +#endif // ROBOT_LOCALIZATION_FILTER_UTILITIES_H diff --git a/Localizations/Packages/robot_localization/include/robot_localization/navsat_conversions.h b/Localizations/Packages/robot_localization/include/robot_localization/navsat_conversions.h new file mode 100644 index 0000000..6c7ad59 --- /dev/null +++ b/Localizations/Packages/robot_localization/include/robot_localization/navsat_conversions.h @@ -0,0 +1,216 @@ +/* +* Copyright (C) 2010 Austin Robot Technology, and others +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* 1. Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* 2. Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* 3. Neither the names of the University of Texas at Austin, nor +* Austin Robot Technology, nor the names of other contributors may +* be used to endorse or promote products derived from this +* software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +* +* This file contains code from multiple files in the original +* source. The originals can be found here: +* +* https://github.com/austin-robot/utexas-art-ros-pkg/blob/afd147a1eb944fc3dbd138574c39699813f797bf/stacks/art_vehicle/art_common/include/art/UTM.h +* https://github.com/austin-robot/utexas-art-ros-pkg/blob/afd147a1eb944fc3dbd138574c39699813f797bf/stacks/art_vehicle/art_common/include/art/conversions.h +*/ + +#ifndef ROBOT_LOCALIZATION_NAVSAT_CONVERSIONS_H +#define ROBOT_LOCALIZATION_NAVSAT_CONVERSIONS_H + +/** @file + + @brief Universal Transverse Mercator transforms. + Functions to convert (spherical) latitude and longitude to and + from (Euclidean) UTM coordinates. + @author Chuck Gantz- chuck.gantz@globalstar.com + */ + +#include +#include + +#include +#include + +#include +#include + +namespace RobotLocalization +{ +namespace NavsatConversions +{ + +const double RADIANS_PER_DEGREE = M_PI/180.0; +const double DEGREES_PER_RADIAN = 180.0/M_PI; + +// Grid granularity for rounding UTM coordinates to generate MapXY. +const double grid_size = 100000.0; // 100 km grid + +// WGS84 Parameters +#define WGS84_A 6378137.0 // major axis +#define WGS84_B 6356752.31424518 // minor axis +#define WGS84_F 0.0033528107 // ellipsoid flattening +#define WGS84_E 0.0818191908 // first eccentricity +#define WGS84_EP 0.0820944379 // second eccentricity + +// UTM Parameters +#define UTM_K0 0.9996 // scale factor +#define UTM_FE 500000.0 // false easting +#define UTM_FN_N 0.0 // false northing, northern hemisphere +#define UTM_FN_S 10000000.0 // false northing, southern hemisphere +#define UTM_E2 (WGS84_E*WGS84_E) // e^2 +#define UTM_E4 (UTM_E2*UTM_E2) // e^4 +#define UTM_E6 (UTM_E4*UTM_E2) // e^6 +#define UTM_EP2 (UTM_E2/(1-UTM_E2)) // e'^2 + +/** + * Utility function to convert geodetic to UTM position + * + * Units in are floating point degrees (sign for east/west) + * + * Units out are meters + * + * @todo deprecate this interface in favor of LLtoUTM() + */ +static inline void UTM(double lat, double lon, double *x, double *y) +{ + // constants + static const double m0 = (1 - UTM_E2/4 - 3*UTM_E4/64 - 5*UTM_E6/256); + static const double m1 = -(3*UTM_E2/8 + 3*UTM_E4/32 + 45*UTM_E6/1024); + static const double m2 = (15*UTM_E4/256 + 45*UTM_E6/1024); + static const double m3 = -(35*UTM_E6/3072); + + // compute the central meridian + int cm = ((lon >= 0.0) + ? (static_cast(lon) - (static_cast(lon)) % 6 + 3) + : (static_cast(lon) - (static_cast(lon)) % 6 - 3)); + + // convert degrees into radians + double rlat = lat * RADIANS_PER_DEGREE; + double rlon = lon * RADIANS_PER_DEGREE; + double rlon0 = cm * RADIANS_PER_DEGREE; + + // compute trigonometric functions + double slat = sin(rlat); + double clat = cos(rlat); + double tlat = tan(rlat); + + // decide the false northing at origin + double fn = (lat > 0) ? UTM_FN_N : UTM_FN_S; + + double T = tlat * tlat; + double C = UTM_EP2 * clat * clat; + double A = (rlon - rlon0) * clat; + double M = WGS84_A * (m0*rlat + m1*sin(2*rlat) + + m2*sin(4*rlat) + m3*sin(6*rlat)); + double V = WGS84_A / sqrt(1 - UTM_E2*slat*slat); + + // compute the easting-northing coordinates + *x = UTM_FE + UTM_K0 * V * (A + (1-T+C)*pow(A, 3)/6 + + (5-18*T+T*T+72*C-58*UTM_EP2)*pow(A, 5)/120); + *y = fn + UTM_K0 * (M + V * tlat * (A*A/2 + + (5-T+9*C+4*C*C)*pow(A, 4)/24 + + ((61-58*T+T*T+600*C-330*UTM_EP2) + * pow(A, 6)/720))); + + return; +} + +/** + * Convert lat/long to UTM coords. + * + * East Longitudes are positive, West longitudes are negative. + * North latitudes are positive, South latitudes are negative + * Lat and Long are in fractional degrees + * + * @param[out] gamma meridian convergence at point (degrees). + */ +static inline void LLtoUTM(const double Lat, const double Long, + double &UTMNorthing, double &UTMEasting, + std::string &UTMZone, double &gamma) +{ + int zone; + bool northp; + double k_unused; + GeographicLib::UTMUPS::Forward(Lat, Long, zone, northp, UTMEasting, UTMNorthing, gamma, + k_unused, GeographicLib::UTMUPS::zonespec::MATCH); + GeographicLib::MGRS::Forward(zone, northp, UTMEasting, UTMNorthing, -1, UTMZone); +} + +/** + * Convert lat/long to UTM coords. + * + * East Longitudes are positive, West longitudes are negative. + * North latitudes are positive, South latitudes are negative + * Lat and Long are in fractional degrees + */ +static inline void LLtoUTM(const double Lat, const double Long, + double &UTMNorthing, double &UTMEasting, + std::string &UTMZone) +{ + double gamma = 0.0; + LLtoUTM(Lat, Long, UTMNorthing, UTMEasting, UTMZone, gamma); +} + +/** + * Converts UTM coords to lat/long. + * + * East Longitudes are positive, West longitudes are negative. + * North latitudes are positive, South latitudes are negative + * Lat and Long are in fractional degrees. + * + * @param[out] gamma meridian convergence at point (degrees). + */ +static inline void UTMtoLL(const double UTMNorthing, const double UTMEasting, + const std::string &UTMZone, double& Lat, double& Long, + double& /*gamma*/) +{ + int zone; + bool northp; + double x_unused; + double y_unused; + int prec_unused; + GeographicLib::MGRS::Reverse(UTMZone, zone, northp, x_unused, y_unused, prec_unused, true); + GeographicLib::UTMUPS::Reverse(zone, northp, UTMEasting, UTMNorthing, Lat, Long); +} + +/** + * Converts UTM coords to lat/long. + * + * East Longitudes are positive, West longitudes are negative. + * North latitudes are positive, South latitudes are negative + * Lat and Long are in fractional degrees. + */ +static inline void UTMtoLL(const double UTMNorthing, const double UTMEasting, + const std::string &UTMZone, double& Lat, double& Long) +{ + double gamma; + UTMtoLL(UTMNorthing, UTMEasting, UTMZone, Lat, Long, gamma); +} + +} // namespace NavsatConversions +} // namespace RobotLocalization + +#endif // ROBOT_LOCALIZATION_NAVSAT_CONVERSIONS_H diff --git a/Localizations/Packages/robot_localization/include/robot_localization/navsat_transform.h b/Localizations/Packages/robot_localization/include/robot_localization/navsat_transform.h new file mode 100644 index 0000000..eb50e26 --- /dev/null +++ b/Localizations/Packages/robot_localization/include/robot_localization/navsat_transform.h @@ -0,0 +1,382 @@ +/* + * Copyright (c) 2014, 2015, 2016, Charles River Analytics, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef ROBOT_LOCALIZATION_NAVSAT_TRANSFORM_H +#define ROBOT_LOCALIZATION_NAVSAT_TRANSFORM_H + +#include +#include +#include +#include + +#include + +#include +#include +#include + +#include +#include +#include +#include + +#include + +#include +#include +#include +#include + +#include + +namespace RobotLocalization +{ + +class NavSatTransform +{ + public: + //! @brief Constructor + //! + NavSatTransform(ros::NodeHandle nh, ros::NodeHandle nh_priv); + + //! @brief Destructor + //! + ~NavSatTransform(); + + private: + //! @brief callback function which is called for periodic updates + //! + void periodicUpdate(const ros::TimerEvent& event); + + //! @brief Computes the transform from the UTM frame to the odom frame + //! + void computeTransform(); + + //! @brief Callback for the datum service + //! + bool datumCallback(robot_localization::SetDatum::Request& request, robot_localization::SetDatum::Response&); + + //! @brief Callback for the to Lat Long service + //! + bool toLLCallback(robot_localization::ToLL::Request& request, robot_localization::ToLL::Response& response); + + //! @brief Callback for the from Lat Long service + //! + bool fromLLCallback(robot_localization::FromLL::Request& request, robot_localization::FromLL::Response& response); + + //! @brief Callback for the UTM zone service + //! + bool setUTMZoneCallback(robot_localization::SetUTMZone::Request& request, + robot_localization::SetUTMZone::Response& response); + + //! @brief Given the pose of the navsat sensor in the cartesian frame, removes the offset from the vehicle's + //! centroid and returns the cartesian-frame pose of said centroid. + //! + void getRobotOriginCartesianPose(const tf2::Transform &gps_cartesian_pose, + tf2::Transform &robot_cartesian_pose, + const ros::Time &transform_time); + + //! @brief Given the pose of the navsat sensor in the world frame, removes the offset from the vehicle's centroid + //! and returns the world-frame pose of said centroid. + //! + void getRobotOriginWorldPose(const tf2::Transform &gps_odom_pose, + tf2::Transform &robot_odom_pose, + const ros::Time &transform_time); + + //! @brief Callback for the GPS fix data + //! @param[in] msg The NavSatFix message to process + //! + void gpsFixCallback(const sensor_msgs::NavSatFixConstPtr& msg); + + //! @brief Callback for the IMU data + //! @param[in] msg The IMU message to process + //! + void imuCallback(const sensor_msgs::ImuConstPtr& msg); + + //! @brief Callback for the odom data + //! @param[in] msg The odometry message to process + //! + void odomCallback(const nav_msgs::OdometryConstPtr& msg); + + //! @brief Converts the odometry data back to GPS and broadcasts it + //! @param[out] filtered_gps The NavSatFix message to prepare + //! + bool prepareFilteredGps(sensor_msgs::NavSatFix &filtered_gps); + + //! @brief Prepares the GPS odometry message before sending + //! @param[out] gps_odom The odometry message to prepare + //! + bool prepareGpsOdometry(nav_msgs::Odometry &gps_odom); + + //! @brief Used for setting the GPS data that will be used to compute the transform + //! @param[in] msg The NavSatFix message to use in the transform + //! + void setTransformGps(const sensor_msgs::NavSatFixConstPtr& msg); + + //! @brief Used for setting the odometry data that will be used to compute the transform + //! @param[in] msg The odometry message to use in the transform + //! + void setTransformOdometry(const nav_msgs::OdometryConstPtr& msg); + + //! @brief Transforms the passed in pose from utm to map frame + //! @param[in] cartesian_pose the pose in cartesian frame to use to transform + //! + nav_msgs::Odometry cartesianToMap(const tf2::Transform& cartesian_pose) const; + + //! @brief Transforms the passed in point from map frame to lat/long + //! @param[in] point the point in map frame to use to transform + //! + void mapToLL(const tf2::Vector3& point, double& latitude, double& longitude, double& altitude) const; + + //! @brief Whether or not we broadcast the cartesian transform + //! + bool broadcast_cartesian_transform_; + + //! @brief Whether to broadcast the cartesian transform as parent frame, default as child + //! + bool broadcast_cartesian_transform_as_parent_frame_; + + //! @brief Whether or not we have new GPS data + //! + //! We only want to compute and broadcast our transformed GPS data if it's new. This variable keeps track of that. + //! + bool gps_updated_; + + //! @brief Whether or not the GPS fix is usable + //! + bool has_transform_gps_; + + //! @brief Signifies that we have received a usable IMU message + //! + bool has_transform_imu_; + + //! @brief Signifies that we have received a usable odometry message + //! + bool has_transform_odom_; + + //! @brief Whether or not we have new odometry data + //! + //! If we're creating filtered GPS messages, then we only want to broadcast them when new odometry data arrives. + //! + bool odom_updated_; + + //! @brief Whether or not we publish filtered GPS messages + //! + bool publish_gps_; + + //! @brief Whether or not we've computed a good heading + //! + bool transform_good_; + + //! @brief Whether we get our datum from the first GPS message or from the set_datum + //! service/parameter + //! + bool use_manual_datum_; + + //! @brief Whether we get the transform's yaw from the odometry or IMU source + //! + bool use_odometry_yaw_; + + //! @brief Whether we use a Local Cartesian (tangent plane ENU) or the UTM coordinates as our cartesian + //! + bool use_local_cartesian_; + + //! @brief When true, do not print warnings for tf lookup failures. + //! + bool tf_silent_failure_; + + //! @brief Local Cartesian projection around gps origin + //! + GeographicLib::LocalCartesian gps_local_cartesian_; + + //! @brief Whether or not to report 0 altitude + //! + //! If this parameter is true, we always report 0 for the altitude of the converted GPS odometry message. + //! + bool zero_altitude_; + + //! @brief Parameter that specifies the magnetic declination for the robot's environment. + //! + double magnetic_declination_; + + //! @brief UTM's meridian convergence + //! + //! Angle between projected meridian (True North) and UTM's grid Y-axis. + //! For UTM projection (Ellipsoidal Transverse Mercator) it is zero on the equator and non-zero everywhere else. + //! It increases as the poles are approached or as we're getting farther from central meridian. + //! + double utm_meridian_convergence_; + + //! @brief IMU's yaw offset + //! + //! Your IMU should read 0 when facing *magnetic* north. If it doesn't, this (parameterized) value gives the offset + //! (NOTE: if you have a magenetic declination, use the parameter setting for that). + //! + double yaw_offset_; + + //! @brief Frame ID of the robot's body frame + //! + //! This is needed for obtaining transforms from the robot's body frame to the frames of sensors (IMU and GPS) + //! + std::string base_link_frame_id_; + + //! @brief The cartesian frame ID, default as 'local_enu' if using Local Cartesian or 'utm' if using the UTM + //! coordinates as our cartesian. + //! + std::string cartesian_frame_id_; + + //! @brief The frame_id of the GPS message (specifies mounting location) + //! + std::string gps_frame_id_; + + //! @brief the UTM zone (zero means UPS) + //! + int utm_zone_; + + //! @brief hemisphere (true means north, false means south) + //! + bool northp_; + + //! @brief Frame ID of the GPS odometry output + //! + //! This will just match whatever your odometry message has + //! + std::string world_frame_id_; + + //! @brief Covariance for most recent odometry data + //! + Eigen::MatrixXd latest_odom_covariance_; + + //! @brief Covariance for most recent GPS/UTM/LocalCartesian data + //! + Eigen::MatrixXd latest_cartesian_covariance_; + + //! @brief Timestamp of the latest good GPS message + //! + //! We assign this value to the timestamp of the odometry message that we output + //! + ros::Time gps_update_time_; + + //! @brief Timestamp of the latest good odometry message + //! + //! We assign this value to the timestamp of the odometry message that we output + //! + ros::Time odom_update_time_; + + //! @brief Parameter that specifies the how long we wait for a transform to become available. + //! + ros::Duration transform_timeout_; + + //! @brief timer calling periodicUpdate + //! + ros::Timer periodicUpdateTimer_; + + //! @brief Latest IMU orientation + //! + tf2::Quaternion transform_orientation_; + + //! @brief Latest GPS data, stored as Cartesian coords + //! + tf2::Transform latest_cartesian_pose_; + + //! @brief Latest odometry pose data + //! + tf2::Transform latest_world_pose_; + + //! @brief Holds the cartesian (UTM or local ENU) pose that is used to compute the transform + //! + tf2::Transform transform_cartesian_pose_; + + //! @brief Latest IMU orientation + //! + tf2::Transform transform_world_pose_; + + //! @brief Holds the Cartesian->odom transform + //! + tf2::Transform cartesian_world_transform_; + + //! @brief Holds the odom->UTM transform for filtered GPS broadcast + //! + tf2::Transform cartesian_world_trans_inverse_; + + //! @brief Publiser for filtered gps data + //! + ros::Publisher filtered_gps_pub_; + + //! @brief GPS subscriber + //! + ros::Subscriber gps_sub_; + + //! @brief Subscribes to imu topic + //! + ros::Subscriber imu_sub_; + + //! @brief Odometry subscriber + //! + ros::Subscriber odom_sub_; + + //! @brief Publisher for gps data + //! + ros::Publisher gps_odom_pub_; + + //! @brief Service for set datum + //! + ros::ServiceServer datum_srv_; + + //! @brief Service for to Lat Long + //! + ros::ServiceServer to_ll_srv_; + + //! @brief Service for from Lat Long + //! + ros::ServiceServer from_ll_srv_; + + //! @brief Service for set UTM zone + //! + ros::ServiceServer set_utm_zone_srv_; + + //! @brief Transform buffer for managing coordinate transforms + //! + tf2_ros::Buffer tf_buffer_; + + //! @brief Transform listener for receiving transforms + //! + tf2_ros::TransformListener tf_listener_; + + //! @brief Used for publishing the static world_frame->cartesian transform + //! + tf2_ros::StaticTransformBroadcaster cartesian_broadcaster_; +}; + +} // namespace RobotLocalization + +#endif // ROBOT_LOCALIZATION_NAVSAT_TRANSFORM_H diff --git a/Localizations/Packages/robot_localization/include/robot_localization/robot_localization_estimator.h b/Localizations/Packages/robot_localization/include/robot_localization/robot_localization_estimator.h new file mode 100644 index 0000000..ef862f4 --- /dev/null +++ b/Localizations/Packages/robot_localization/include/robot_localization/robot_localization_estimator.h @@ -0,0 +1,222 @@ +/* + * Copyright (c) 2016, TNO IVS Helmond. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef ROBOT_LOCALIZATION_ROBOT_LOCALIZATION_ESTIMATOR_H +#define ROBOT_LOCALIZATION_ROBOT_LOCALIZATION_ESTIMATOR_H + +#include +#include +#include +#include + +#include "robot_localization/filter_base.h" +#include "robot_localization/filter_utilities.h" +#include "robot_localization/filter_common.h" + +namespace RobotLocalization +{ + +struct Twist +{ + Eigen::Vector3d linear; + Eigen::Vector3d angular; +}; + +//! @brief Robot Localization Estimator State +//! +//! The Estimator State data structure bundles the state information of the estimator. +//! +struct EstimatorState +{ + EstimatorState(): + time_stamp(0.0), + state(STATE_SIZE), + covariance(STATE_SIZE, STATE_SIZE) + { + state.setZero(); + covariance.setZero(); + } + + //! @brief Time at which this state is/was achieved + double time_stamp; + + //! @brief System state at time = time_stamp + Eigen::VectorXd state; + + //! @brief System state covariance at time = time_stamp + Eigen::MatrixXd covariance; + + friend std::ostream& operator<<(std::ostream &os, const EstimatorState& state) + { + return os << "state:\n - time_stamp: " << state.time_stamp << + "\n - state: \n" << state.state << + " - covariance: \n" << state.covariance; + } +}; + +namespace EstimatorResults +{ +enum EstimatorResult +{ + ExtrapolationIntoFuture = 0, + Interpolation, + ExtrapolationIntoPast, + Exact, + EmptyBuffer, + Failed +}; +} // namespace EstimatorResults +typedef EstimatorResults::EstimatorResult EstimatorResult; + +namespace FilterTypes +{ +enum FilterType +{ + EKF = 0, + UKF, + NotDefined +}; +} // namespace FilterTypes +typedef FilterTypes::FilterType FilterType; + +//! @brief Robot Localization Listener class +//! +//! The Robot Localization Estimator class buffers states of and inputs to a system and can interpolate and extrapolate +//! based on a given system model. +//! +class RobotLocalizationEstimator +{ +public: + //! @brief Constructor for the RobotLocalizationListener class + //! + //! @param[in] args - Generic argument container (not used here, but needed so that the ROS filters can pass arbitrary + //! arguments to templated filter types). + //! + explicit RobotLocalizationEstimator(unsigned int buffer_capacity, + FilterType filter_type, + const Eigen::MatrixXd& process_noise_covariance, + const std::vector& filter_args = std::vector()); + + //! @brief Destructor for the RobotLocalizationListener class + //! + virtual ~RobotLocalizationEstimator(); + + //! @brief Sets the current internal state of the listener. + //! + //! @param[in] state - The new state vector to set the internal state to + //! + void setState(const EstimatorState& state); + + //! @brief Returns the state at a given time + //! + //! Projects the current state and error matrices forward using a model of the robot's motion. + //! + //! @param[in] time - The time to which the prediction is being made + //! @param[out] state - The returned state at the given time + //! + //! @return GetStateResult enum + //! + EstimatorResult getState(const double time, EstimatorState &state) const; + + //! @brief Clears the internal state buffer + //! + void clearBuffer(); + + //! @brief Sets the buffer capacity + //! + //! @param[in] capacity - The new buffer capacity + //! + void setBufferCapacity(const int capacity); + + //! @brief Returns the buffer capacity + //! + //! Returns the number of EstimatorState objects that can be pushed to the buffer before old ones are dropped. (The + //! capacity of the buffer). + //! + //! @return buffer capacity + //! + unsigned int getBufferCapacity() const; + + //! @brief Returns the current buffer size + //! + //! Returns the number of EstimatorState objects currently in the buffer. + //! + //! @return current buffer size + //! + unsigned int getSize() const; + +private: + friend std::ostream& operator<<(std::ostream &os, const RobotLocalizationEstimator& rle) + { + for ( boost::circular_buffer::const_iterator it = rle.state_buffer_.begin(); + it != rle.state_buffer_.end(); ++it ) + { + os << *it << "\n"; + } + return os; + } + + //! @brief Extrapolates the given state to a requested time stamp + //! + //! @param[in] boundary_state - state from which to extrapolate + //! @param[in] requested_time - time stamp to extrapolate to + //! @param[out] state_at_req_time - predicted state at requested time + //! + void extrapolate(const EstimatorState& boundary_state, + const double requested_time, + EstimatorState& state_at_req_time) const; + + //! @brief Interpolates the given state to a requested time stamp + //! + //! @param[in] given_state_1 - last state update before requested time + //! @param[in] given_state_2 - next state update after requested time + //! @param[in] requested_time - time stamp to extrapolate to + //! @param[out] state_at_req_time - predicted state at requested time + //! + void interpolate(const EstimatorState& given_state_1, const EstimatorState& given_state_2, + const double requested_time, EstimatorState& state_at_req_time) const; + + //! + //! @brief The buffer holding the system states that have come in. Interpolation and extrapolation is done starting + //! from these states. + //! + boost::circular_buffer state_buffer_; + + //! + //! @brief A pointer to the filter instance that is used for extrapolation + //! + FilterBase* filter_; +}; + +} // namespace RobotLocalization + +#endif // ROBOT_LOCALIZATION_ROBOT_LOCALIZATION_ESTIMATOR_H diff --git a/Localizations/Packages/robot_localization/include/robot_localization/ros_filter.h b/Localizations/Packages/robot_localization/include/robot_localization/ros_filter.h new file mode 100644 index 0000000..9eb6bf7 --- /dev/null +++ b/Localizations/Packages/robot_localization/include/robot_localization/ros_filter.h @@ -0,0 +1,767 @@ +/* + * Copyright (c) 2014, 2015, 2016, Charles River Analytics, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef ROBOT_LOCALIZATION_ROS_FILTER_H +#define ROBOT_LOCALIZATION_ROS_FILTER_H + +#include "robot_localization/ros_filter_utilities.h" +#include "robot_localization/filter_common.h" +#include "robot_localization/filter_base.h" + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace RobotLocalization +{ + +struct CallbackData +{ + CallbackData(const std::string &topicName, + const std::vector &updateVector, + const int updateSum, + const bool differential, + const bool relative, + const bool pose_use_child_frame, + const double rejectionThreshold) : + topicName_(topicName), + updateVector_(updateVector), + updateSum_(updateSum), + differential_(differential), + relative_(relative), + pose_use_child_frame_(pose_use_child_frame), + rejectionThreshold_(rejectionThreshold) + { + } + + std::string topicName_; + std::vector updateVector_; + int updateSum_; + bool differential_; + bool relative_; + bool pose_use_child_frame_; + double rejectionThreshold_; +}; + +typedef std::priority_queue, Measurement> MeasurementQueue; +typedef std::deque MeasurementHistoryDeque; +typedef std::deque FilterStateHistoryDeque; + +template class RosFilter +{ + public: + //! @brief Constructor + //! + //! The RosFilter constructor makes sure that anyone using + //! this template is doing so with the correct object type + //! + explicit RosFilter(ros::NodeHandle nh, + ros::NodeHandle nh_priv, + std::string node_name, + std::vector args = std::vector()); + + //! @brief Constructor + //! + //! The RosFilter constructor makes sure that anyone using + //! this template is doing so with the correct object type + //! + explicit RosFilter(ros::NodeHandle nh, ros::NodeHandle nh_priv, std::vector args = std::vector()); + + //! @brief Destructor + //! + //! Clears out the message filters and topic subscribers. + //! + ~RosFilter(); + + //! @brief Initialize filter + // + void initialize(); + + //! @brief Resets the filter to its initial state + //! + void reset(); + + //! @brief Service callback to toggle processing measurements for a standby mode but continuing to publish + //! @param[in] request - The state requested, on (True) or off (False) + //! @param[out] response - status if upon success + //! @return boolean true if successful, false if not + bool toggleFilterProcessingCallback(robot_localization::ToggleFilterProcessing::Request&, + robot_localization::ToggleFilterProcessing::Response&); + + //! @brief Callback method for receiving all acceleration (IMU) messages + //! @param[in] msg - The ROS IMU message to take in. + //! @param[in] callbackData - Relevant static callback data + //! @param[in] targetFrame - The target frame_id into which to transform the data + //! + void accelerationCallback(const sensor_msgs::Imu::ConstPtr &msg, + const CallbackData &callbackData, + const std::string &targetFrame); + + //! @brief Callback method for receiving non-stamped control input + //! @param[in] msg - The ROS twist message to take in + //! + void controlCallback(const geometry_msgs::Twist::ConstPtr &msg); + + //! @brief Callback method for receiving stamped control input + //! @param[in] msg - The ROS stamped twist message to take in + //! + void controlCallback(const geometry_msgs::TwistStamped::ConstPtr &msg); + + //! @brief Adds a measurement to the queue of measurements to be processed + //! + //! @param[in] topicName - The name of the measurement source (only used for debugging) + //! @param[in] measurement - The measurement to enqueue + //! @param[in] measurementCovariance - The covariance of the measurement + //! @param[in] updateVector - The boolean vector that specifies which variables to update from this measurement + //! @param[in] mahalanobisThresh - Threshold, expressed as a Mahalanobis distance, for outlier rejection + //! @param[in] time - The time of arrival (in seconds) + //! + void enqueueMeasurement(const std::string &topicName, + const Eigen::VectorXd &measurement, + const Eigen::MatrixXd &measurementCovariance, + const std::vector &updateVector, + const double mahalanobisThresh, + const ros::Time &time); + + //! @brief Method for zeroing out 3D variables within measurements + //! @param[out] measurement - The measurement whose 3D variables will be zeroed out + //! @param[out] measurementCovariance - The covariance of the measurement + //! @param[out] updateVector - The boolean update vector of the measurement + //! + //! If we're in 2D mode, then for every measurement from every sensor, we call this. + //! It sets the 3D variables to 0, gives those variables tiny variances, and sets + //! their updateVector values to 1. + //! + void forceTwoD(Eigen::VectorXd &measurement, + Eigen::MatrixXd &measurementCovariance, + std::vector &updateVector); + + //! @brief Retrieves the EKF's output for broadcasting + //! @param[out] message - The standard ROS odometry message to be filled + //! @return true if the filter is initialized, false otherwise + //! + bool getFilteredOdometryMessage(nav_msgs::Odometry &message); + + //! @brief Retrieves the EKF's acceleration output for broadcasting + //! @param[out] message - The standard ROS acceleration message to be filled + //! @return true if the filter is initialized, false otherwise + //! + bool getFilteredAccelMessage(geometry_msgs::AccelWithCovarianceStamped &message); + + //! @brief Callback method for receiving all IMU messages + //! @param[in] msg - The ROS IMU message to take in. + //! @param[in] topicName - The topic name for the IMU message (only used for debug output) + //! @param[in] poseCallbackData - Relevant static callback data for orientation variables + //! @param[in] twistCallbackData - Relevant static callback data for angular velocity variables + //! @param[in] accelCallbackData - Relevant static callback data for linear acceleration variables + //! + //! This method separates out the orientation, angular velocity, and linear acceleration data and + //! passed each on to its respective callback. + //! + void imuCallback(const sensor_msgs::Imu::ConstPtr &msg, const std::string &topicName, + const CallbackData &poseCallbackData, const CallbackData &twistCallbackData, + const CallbackData &accelCallbackData); + + //! @brief Processes all measurements in the measurement queue, in temporal order + //! + //! @param[in] currentTime - The time at which to carry out integration (the current time) + //! + void integrateMeasurements(const ros::Time ¤tTime); + + //! @brief Differentiate angular velocity for angular acceleration + //! + //! @param[in] currentTime - The time at which to carry out differentiation (the current time) + //! + //! Maybe more state variables can be time-differentiated to estimate higher-order states, + //! but now we only focus on obtaining the angular acceleration. It implements a backward- + //! Euler differentiation. + //! + void differentiateMeasurements(const ros::Time ¤tTime); + + //! @brief Loads all parameters from file + //! + void loadParams(); + + //! @brief Callback method for receiving all odometry messages + //! @param[in] msg - The ROS odometry message to take in. + //! @param[in] topicName - The topic name for the odometry message (only used for debug output) + //! @param[in] poseCallbackData - Relevant static callback data for pose variables + //! @param[in] twistCallbackData - Relevant static callback data for twist variables + //! + //! This method simply separates out the pose and twist data into two new messages, and passes them into their + //! respective callbacks + //! + void odometryCallback(const nav_msgs::Odometry::ConstPtr &msg, const std::string &topicName, + const CallbackData &poseCallbackData, const CallbackData &twistCallbackData); + + //! @brief Callback method for receiving all pose messages + //! @param[in] msg - The ROS stamped pose with covariance message to take in + //! @param[in] callbackData - Relevant static callback data + //! @param[in] targetFrame - The target frame_id into which to transform the data + //! @param[in] poseSourceFrame - The source frame_id from which to transform the data + //! @param[in] imuData - Whether this data comes from an IMU + //! + void poseCallback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg, + const CallbackData &callbackData, + const std::string &targetFrame, + const std::string &poseSourceFrame, + const bool imuData); + + //! @brief Callback method for manually setting/resetting the internal pose estimate + //! @param[in] msg - The ROS stamped pose with covariance message to take in + //! + void setPoseCallback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg); + + //! @brief Service callback for manually setting/resetting the internal pose estimate + //! + //! @param[in] request - Custom service request with pose information + //! @return true if successful, false if not + bool setPoseSrvCallback(robot_localization::SetPose::Request& request, + robot_localization::SetPose::Response&); + + //! @brief Service callback for manually enable the filter + //! @param[in] request - N/A + //! @param[out] response - N/A + //! @return boolean true if successful, false if not + bool enableFilterSrvCallback(std_srvs::Empty::Request&, + std_srvs::Empty::Response&); + + //! @brief Callback method for receiving all twist messages + //! @param[in] msg - The ROS stamped twist with covariance message to take in. + //! @param[in] callbackData - Relevant static callback data + //! @param[in] targetFrame - The target frame_id into which to transform the data + //! + void twistCallback(const geometry_msgs::TwistWithCovarianceStamped::ConstPtr &msg, + const CallbackData &callbackData, + const std::string &targetFrame); + + //! @brief Validates filter outputs for NaNs and Infinite values + //! @param[out] message - The standard ROS odometry message to be validated + //! @return true if the filter output is valid, false otherwise + //! + bool validateFilterOutput(const nav_msgs::Odometry &message); + + protected: + //! @brief Finds the latest filter state before the given timestamp and makes it the current state again. + //! + //! This method also inserts all measurements between the older filter timestamp and now into the measurements + //! queue. + //! + //! @param[in] time - The time to which the filter state should revert + //! @return True if restoring the filter succeeded. False if not. + //! + bool revertTo(const double time); + + //! @brief Saves the current filter state in the queue of previous filter states + //! + //! These measurements will be used in backwards smoothing in the event that older measurements come in. + //! @param[in] filter - The filter base object whose state we want to save + //! + void saveFilterState(FilterBase &filter); + + //! @brief Removes measurements and filter states older than the given cutoff time. + //! @param[in] cutoffTime - Measurements and states older than this time will be dropped. + //! + void clearExpiredHistory(const double cutoffTime); + + //! @brief Clears measurement queue + //! + void clearMeasurementQueue(); + + //! @brief Adds a diagnostic message to the accumulating map and updates the error level + //! @param[in] errLevel - The error level of the diagnostic + //! @param[in] topicAndClass - The sensor topic (if relevant) and class of diagnostic + //! @param[in] message - Details of the diagnostic + //! @param[in] staticDiag - Whether or not this diagnostic information is static + //! + void addDiagnostic(const int errLevel, + const std::string &topicAndClass, + const std::string &message, + const bool staticDiag); + + //! @brief Aggregates all diagnostics so they can be published + //! @param[in] wrapper - The diagnostic status wrapper to update + //! + void aggregateDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &wrapper); + + //! @brief Utility method for copying covariances from ROS covariance arrays + //! to Eigen matrices + //! + //! This method copies the covariances and also does some data validation, which is + //! why it requires more parameters than just the covariance containers. + //! @param[in] arr - The source array for the covariance data + //! @param[in] covariance - The destination matrix for the covariance data + //! @param[in] topicName - The name of the source data topic (for debug purposes) + //! @param[in] updateVector - The update vector for the source topic + //! @param[in] offset - The "starting" location within the array/update vector + //! @param[in] dimension - The number of values to copy, starting at the offset + //! + void copyCovariance(const double *arr, + Eigen::MatrixXd &covariance, + const std::string &topicName, + const std::vector &updateVector, + const size_t offset, + const size_t dimension); + + //! @brief Utility method for copying covariances from Eigen matrices to ROS + //! covariance arrays + //! + //! @param[in] covariance - The source matrix for the covariance data + //! @param[in] arr - The destination array + //! @param[in] dimension - The number of values to copy + //! + void copyCovariance(const Eigen::MatrixXd &covariance, + double *arr, + const size_t dimension); + + //! @brief Loads fusion settings from the config file + //! @param[in] topicName - The name of the topic for which to load settings + //! @return The boolean vector of update settings for each variable for this topic + //! + std::vector loadUpdateConfig(const std::string &topicName); + + //! @brief Prepares an IMU message's linear acceleration for integration into the filter + //! @param[in] msg - The IMU message to prepare + //! @param[in] topicName - The name of the topic over which this message was received + //! @param[in] targetFrame - The target tf frame + //! @param[in] updateVector - The update vector for the data source + //! @param[in] measurement - The twist data converted to a measurement + //! @param[in] measurementCovariance - The covariance of the converted measurement + //! + bool prepareAcceleration(const sensor_msgs::Imu::ConstPtr &msg, + const std::string &topicName, + const std::string &targetFrame, + const bool relative, + std::vector &updateVector, + Eigen::VectorXd &measurement, + Eigen::MatrixXd &measurementCovariance); + + //! @brief Prepares a pose message for integration into the filter + //! @param[in] msg - The pose message to prepare + //! @param[in] topicName - The name of the topic over which this message was received + //! @param[in] targetFrame - The target tf frame + //! @param[in] sourceFrame - The source tf frame + //! @param[in] differential - Whether we're carrying out differential integration + //! @param[in] relative - Whether this measurement is processed relative to the first + //! @param[in] imuData - Whether this measurement is from an IMU + //! @param[in,out] updateVector - The update vector for the data source + //! @param[out] measurement - The pose data converted to a measurement + //! @param[out] measurementCovariance - The covariance of the converted measurement + //! @return true indicates that the measurement was successfully prepared, false otherwise + //! + bool preparePose(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg, + const std::string &topicName, + const std::string &targetFrame, + const std::string &sourceFrame, + const bool differential, + const bool relative, + const bool imuData, + std::vector &updateVector, + Eigen::VectorXd &measurement, + Eigen::MatrixXd &measurementCovariance); + + //! @brief Prepares a twist message for integration into the filter + //! @param[in] msg - The twist message to prepare + //! @param[in] topicName - The name of the topic over which this message was received + //! @param[in] targetFrame - The target tf frame + //! @param[in,out] updateVector - The update vector for the data source + //! @param[out] measurement - The twist data converted to a measurement + //! @param[out] measurementCovariance - The covariance of the converted measurement + //! @return true indicates that the measurement was successfully prepared, false otherwise + //! + bool prepareTwist(const geometry_msgs::TwistWithCovarianceStamped::ConstPtr &msg, + const std::string &topicName, + const std::string &targetFrame, + std::vector &updateVector, + Eigen::VectorXd &measurement, + Eigen::MatrixXd &measurementCovariance); + + + //! @brief callback function which is called for periodic updates + //! + void periodicUpdate(const ros::TimerEvent& event); + + //! @brief Start the Filter disabled at startup + //! + //! If this is true, the filter reads parameters and prepares publishers and subscribes + //! but does not integrate new messages into the state vector. + //! The filter can be enabled later using a service. + bool disabledAtStartup_; + + //! @brief Whether the filter is enabled or not. See disabledAtStartup_. + bool enabled_; + + //! Whether we'll allow old measurements to cause a re-publication of the updated state + bool permitCorrectedPublication_; + + //! @brief By default, the filter predicts and corrects up to the time of the latest measurement. If this is set + //! to true, the filter does the same, but then also predicts up to the current time step. + //! + bool predictToCurrentTime_; + + //! @brief Whether or not we print diagnostic messages to the /diagnostics topic + //! + bool printDiagnostics_; + + //! @brief Whether we publish the acceleration + //! + bool publishAcceleration_; + + //! @brief Whether we publish the transform from the world_frame to the base_link_frame + //! + bool publishTransform_; + + //! @brief Whether to reset the filters when backwards jump in time is detected + //! + //! This is usually the case when logs are being used and a jump in the logi + //! is done or if a log files restarts from the beginning. + //! + bool resetOnTimeJump_; + + //! @brief Whether or not we use smoothing + //! + bool smoothLaggedData_; + + //! @brief Whether the filter should process new measurements or not. + bool toggledOn_; + + //! @brief Whether or not we're in 2D mode + //! + //! If this is true, the filter binds all 3D variables (Z, + //! roll, pitch, and their respective velocities) to 0 for + //! every measurement. + //! + bool twoDMode_; + + //! @brief Whether or not we use a control term + //! + bool useControl_; + + //! @brief When true, do not print warnings for tf lookup failures. + //! + bool tfSilentFailure_; + + //! @brief The max (worst) dynamic diagnostic level. + //! + int dynamicDiagErrorLevel_; + + //! @brief The max (worst) static diagnostic level. + //! + int staticDiagErrorLevel_; + + //! @brief The frequency of the run loop + //! + double frequency_; + + //! @brief What is the acceleration in Z due to gravity (m/s^2)? Default is +9.80665. + //! + double gravitationalAcc_; + + //! @brief The depth of the history we track for smoothing/delayed measurement processing + //! + //! This is the guaranteed minimum buffer size for which previous states and measurements are kept. + //! + double historyLength_; + + //! @brief minimal frequency + //! + double minFrequency_; + + //! @brief maximal frequency + double maxFrequency_; + + //! @brief tf frame name for the robot's body frame + //! + std::string baseLinkFrameId_; + + //! @brief tf frame name for the robot's body frame + //! + //! When the final state is computed, we "override" the output transform and message to have this frame for its + //! child_frame_id. This helps to enable disconnected TF trees when multiple EKF instances are being run. + //! + std::string baseLinkOutputFrameId_; + + //! @brief tf frame name for the robot's map (world-fixed) frame + //! + std::string mapFrameId_; + + //! @brief tf frame name for the robot's odometry (world-fixed) frame + //! + std::string odomFrameId_; + + //! @brief tf frame name that is the parent frame of the transform that this node will calculate and broadcast. + //! + std::string worldFrameId_; + + //! @brief Used for outputting debug messages + //! + std::ofstream debugStream_; + + //! @brief Contains the state vector variable names in string format + //! + std::vector stateVariableNames_; + + //! @brief Vector to hold our subscribers until they go out of scope + //! + std::vector topicSubs_; + + //! @brief This object accumulates dynamic diagnostics, e.g., diagnostics relating + //! to sensor data. + //! + //! The values are considered transient and are cleared at every iteration. + //! + std::map dynamicDiagnostics_; + + //! @brief Stores the first measurement from each topic for relative measurements + //! + //! When a given sensor is being integrated in relative mode, its first measurement + //! is effectively treated as an offset, and future measurements have this first + //! measurement removed before they are fused. This variable stores the initial + //! measurements. Note that this is different from using differential mode, as in + //! differential mode, pose data is converted to twist data, resulting in boundless + //! error growth for the variables being fused. With relative measurements, the + //! vehicle will start with a 0 heading and position, but the measurements are still + //! fused absolutely. + std::map initialMeasurements_; + + //! @brief Store the last time a message from each topic was received + //! + //! If we're getting messages rapidly, we may accidentally get + //! an older message arriving after a newer one. This variable keeps + //! track of the most recent message time for each subscribed message + //! topic. We also use it when listening to odometry messages to + //! determine if we should be using messages from that topic. + //! + std::map lastMessageTimes_; + + //! @brief We also need the previous covariance matrix for differential data + //! + std::map previousMeasurementCovariances_; + + //! @brief Stores the last measurement from a given topic for differential integration + //! + //! To carry out differential integration, we have to (1) transform + //! that into the target frame (probably the frame specified by + //! @p odomFrameId_), (2) "subtract" the previous measurement from + //! the current measurement, and then (3) transform it again by the previous + //! state estimate. This holds the measurements used for step (2). + //! + std::map previousMeasurements_; + + //! @brief If including acceleration for each IMU input, whether or not we remove acceleration due to gravity + //! + std::map removeGravitationalAcc_; + + //! @brief This object accumulates static diagnostics, e.g., diagnostics relating + //! to the configuration parameters. + //! + //! The values are treated as static and always reported (i.e., this object is never cleared) + //! + std::map staticDiagnostics_; + + //! @brief Last time mark that time-differentiation is calculated + //! + ros::Time lastDiffTime_; + + //! @brief Last record of filtered angular velocity + //! + tf2::Vector3 lastStateTwistRot_; + + //! @brief Calculated angular acceleration from time-differencing + //! + tf2::Vector3 angular_acceleration_; + + //! @brief Covariance of the calculated angular acceleration + //! + Eigen::MatrixXd angular_acceleration_cov_; + + //! @brief The most recent control input + //! + Eigen::VectorXd latestControl_; + + //! @brief Message that contains our latest transform (i.e., state) + //! + //! We use the vehicle's latest state in a number of places, and often + //! use it as a transform, so this is the most convenient variable to + //! use as our global "current state" object + //! + geometry_msgs::TransformStamped worldBaseLinkTransMsg_; + + //! @brief last call of periodicUpdate + //! + ros::Time lastDiagTime_; + + //! @brief The time of the most recent published state + //! + ros::Time lastPublishedStamp_; + + //! @brief Store the last time set pose message was received + //! + //! If we receive a setPose message to reset the filter, we can get in + //! strange situations wherein we process the pose reset, but then even + //! after another spin cycle or two, we can get a new message with a time + //! stamp that is *older* than the setPose message's time stamp. This means + //! we have to check the message's time stamp against the lastSetPoseTime_. + ros::Time lastSetPoseTime_; + + //! @brief The time of the most recent control input + //! + ros::Time latestControlTime_; + + //! @brief For future (or past) dating the world_frame->base_link_frame transform + //! + ros::Duration tfTimeOffset_; + + //! @brief Parameter that specifies the how long we wait for a transform to become available. + //! + ros::Duration tfTimeout_; + + //! @brief Service that allows another node to toggle on/off filter processing while still publishing. + //! Uses a robot_localization ToggleFilterProcessing service. + //! + ros::ServiceServer toggleFilterProcessingSrv_; + + //! @brief timer calling periodicUpdate + //! + ros::Timer periodicUpdateTimer_; + + //! @brief An implicitly time ordered queue of past filter states used for smoothing. + // + // front() refers to the filter state with the earliest timestamp. + // back() refers to the filter state with the latest timestamp. + FilterStateHistoryDeque filterStateHistory_; + + //! @brief A deque of previous measurements which is implicitly ordered from earliest to latest measurement. + // when popped from the measurement priority queue. + // front() refers to the measurement with the earliest timestamp. + // back() refers to the measurement with the latest timestamp. + MeasurementHistoryDeque measurementHistory_; + + //! @brief We process measurements by queueing them up in + //! callbacks and processing them all at once within each + //! iteration + //! + MeasurementQueue measurementQueue_; + + //! @brief Our filter (EKF, UKF, etc.) + //! + T filter_; + + //! @brief Node handle + //! + ros::NodeHandle nh_; + + //! @brief Local node handle (for params) + //! + ros::NodeHandle nhLocal_; + + //! @brief optional acceleration publisher + //! + ros::Publisher accelPub_; + + //! @brief position publisher + //! + ros::Publisher positionPub_; + + //! @brief Subscribes to the control input topic + //! + ros::Subscriber controlSub_; + + //! @brief Subscribes to the set_pose topic (usually published from rviz). Message + //! type is geometry_msgs/PoseWithCovarianceStamped. + //! + ros::Subscriber setPoseSub_; + + //! @brief Service that allows another node to enable the filter. Uses a standard Empty service. + //! + ros::ServiceServer enableFilterSrv_; + + //! @brief Service that allows another node to change the current state and recieve a confirmation. Uses + //! a custom SetPose service. + //! + ros::ServiceServer setPoseSrv_; + + //! @brief Used for updating the diagnostics + //! + diagnostic_updater::Updater diagnosticUpdater_; + + //! @brief Transform buffer for managing coordinate transforms + //! + tf2_ros::Buffer tfBuffer_; + + //! @brief Transform listener for receiving transforms + //! + tf2_ros::TransformListener tfListener_; + + //! @brief broadcaster of worldTransform tfs + //! + tf2_ros::TransformBroadcaster worldTransformBroadcaster_; + + //! @brief optional signaling diagnostic frequency + //! + std::unique_ptr freqDiag_; +}; + +} // namespace RobotLocalization + +#endif // ROBOT_LOCALIZATION_ROS_FILTER_H diff --git a/Localizations/Packages/robot_localization/include/robot_localization/ros_filter_types.h b/Localizations/Packages/robot_localization/include/robot_localization/ros_filter_types.h new file mode 100644 index 0000000..635572b --- /dev/null +++ b/Localizations/Packages/robot_localization/include/robot_localization/ros_filter_types.h @@ -0,0 +1,48 @@ +/* + * Copyright (c) 2014, 2015, 2016, Charles River Analytics, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef ROBOT_LOCALIZATION_ROS_FILTER_TYPES_H +#define ROBOT_LOCALIZATION_ROS_FILTER_TYPES_H + +#include "robot_localization/ros_filter.h" +#include "robot_localization/ekf.h" +#include "robot_localization/ukf.h" + +namespace RobotLocalization +{ + +typedef RosFilter RosEkf; +typedef RosFilter RosUkf; + +} + +#endif // ROBOT_LOCALIZATION_ROS_FILTER_TYPES_H diff --git a/Localizations/Packages/robot_localization/include/robot_localization/ros_filter_utilities.h b/Localizations/Packages/robot_localization/include/robot_localization/ros_filter_utilities.h new file mode 100644 index 0000000..16346e3 --- /dev/null +++ b/Localizations/Packages/robot_localization/include/robot_localization/ros_filter_utilities.h @@ -0,0 +1,135 @@ +/* + * Copyright (c) 2014, 2015, 2016, Charles River Analytics, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef ROBOT_LOCALIZATION_ROS_FILTER_UTILITIES_H +#define ROBOT_LOCALIZATION_ROS_FILTER_UTILITIES_H + +#include +#include +#include + +#include + +#include +#include +#include +#include + +#define RF_DEBUG(msg) if (filter_.getDebug()) { debugStream_ << msg; } + +// Handy methods for debug output +std::ostream& operator<<(std::ostream& os, const tf2::Vector3 &vec); +std::ostream& operator<<(std::ostream& os, const tf2::Quaternion &quat); +std::ostream& operator<<(std::ostream& os, const tf2::Transform &trans); +std::ostream& operator<<(std::ostream& os, const std::vector &vec); + +namespace RobotLocalization +{ +namespace RosFilterUtilities +{ + +double getYaw(const tf2::Quaternion quat); + +//! @brief Method for safely obtaining transforms. +//! @param[in] buffer - tf buffer object to use for looking up the transform +//! @param[in] targetFrame - The target frame of the desired transform +//! @param[in] sourceFrame - The source frame of the desired transform +//! @param[in] time - The time at which we want the transform +//! @param[in] timeout - How long to block before falling back to last transform +//! @param[out] targetFrameTrans - The resulting transform object +//! @param[in] silent - Whether or not to print transform warnings +//! @return Sets the value of @p targetFrameTrans and returns true if successful, +//! false otherwise. +//! +//! This method attempts to obtain a transform from the @p sourceFrame to the @p +//! targetFrame at the specific @p time. If no transform is available at that time, +//! it attempts to simply obtain the latest transform. If that still fails, then the +//! method checks to see if the transform is going from a given frame_id to itself. +//! If any of these checks succeed, the method sets the value of @p targetFrameTrans +//! and returns true, otherwise it returns false. +//! +bool lookupTransformSafe(const tf2_ros::Buffer &buffer, + const std::string &targetFrame, + const std::string &sourceFrame, + const ros::Time &time, + const ros::Duration &timeout, + tf2::Transform &targetFrameTrans, + const bool silent = false); + +//! @brief Method for safely obtaining transforms. +//! @param[in] buffer - tf buffer object to use for looking up the transform +//! @param[in] targetFrame - The target frame of the desired transform +//! @param[in] sourceFrame - The source frame of the desired transform +//! @param[in] time - The time at which we want the transform +//! @param[out] targetFrameTrans - The resulting transform object +//! @param[in] silent - Whether or not to print transform warnings +//! @return Sets the value of @p targetFrameTrans and returns true if successful, +//! false otherwise. +//! +//! This method attempts to obtain a transform from the @p sourceFrame to the @p +//! targetFrame at the specific @p time. If no transform is available at that time, +//! it attempts to simply obtain the latest transform. If that still fails, then the +//! method checks to see if the transform is going from a given frame_id to itself. +//! If any of these checks succeed, the method sets the value of @p targetFrameTrans +//! and returns true, otherwise it returns false. +//! +bool lookupTransformSafe(const tf2_ros::Buffer &buffer, + const std::string &targetFrame, + const std::string &sourceFrame, + const ros::Time &time, + tf2::Transform &targetFrameTrans, + const bool silent = false); + +//! @brief Utility method for converting quaternion to RPY +//! @param[in] quat - The quaternion to convert +//! @param[out] roll - The converted roll +//! @param[out] pitch - The converted pitch +//! @param[out] yaw - The converted yaw +//! +void quatToRPY(const tf2::Quaternion &quat, double &roll, double &pitch, double &yaw); + +//! @brief Converts our Eigen state vector into a TF transform/pose +//! @param[in] state - The state to convert +//! @param[out] stateTF - The converted state +//! +void stateToTF(const Eigen::VectorXd &state, tf2::Transform &stateTF); + +//! @brief Converts a TF transform/pose into our Eigen state vector +//! @param[in] stateTF - The state to convert +//! @param[out] state - The converted state +//! +void TFtoState(const tf2::Transform &stateTF, Eigen::VectorXd &state); + +} // namespace RosFilterUtilities +} // namespace RobotLocalization + +#endif // ROBOT_LOCALIZATION_ROS_FILTER_UTILITIES_H diff --git a/Localizations/Packages/robot_localization/include/robot_localization/ros_robot_localization_listener.h b/Localizations/Packages/robot_localization/include/robot_localization/ros_robot_localization_listener.h new file mode 100644 index 0000000..d0d8ccc --- /dev/null +++ b/Localizations/Packages/robot_localization/include/robot_localization/ros_robot_localization_listener.h @@ -0,0 +1,171 @@ +/* + * Copyright (c) 2016, TNO IVS Helmond. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef ROBOT_LOCALIZATION_ROS_ROBOT_LOCALIZATION_LISTENER_H +#define ROBOT_LOCALIZATION_ROS_ROBOT_LOCALIZATION_LISTENER_H + +#include "robot_localization/robot_localization_estimator.h" + +#include + +#include +#include +#include +#include +#include +#include + +namespace RobotLocalization +{ + +//! @brief RosRobotLocalizationListener class +//! +//! This class wraps the RobotLocalizationEstimator. It listens to topics over which the (filtered) robot state is +//! published (odom and accel) and pushes them into its instance of the RobotLocalizationEstimator. It exposes a +//! getState method to offer the user the estimated state at a requested time. This class offers the option to run this +//! listener without the need to run a separate node. If you do wish to run this functionality in a separate node, +//! consider the robot localization listener node. +//! +class RosRobotLocalizationListener +{ +public: + //! @brief Constructor + //! + //! The RosRobotLocalizationListener constructor initializes nodehandles, subscribers, a filter for synchronized + //! listening to the topics it subscribes to, and an instance of the RobotLocalizationEstimator. + //! + RosRobotLocalizationListener(); + + //! @brief Destructor + //! + //! Empty destructor + //! + ~RosRobotLocalizationListener(); + + //! @brief Get a state from the localization estimator + //! + //! Requests the predicted state and covariance at a given time from the Robot Localization Estimator. + //! + //! @param[in] time - time of the requested state + //! @param[in] frame_id - frame id of which the state is requested. + //! @param[out] state - state at the requested time + //! @param[out] covariance - covariance at the requested time + //! + //! @return false if buffer is empty, true otherwise + //! + bool getState(const double time, const std::string& frame_id, + Eigen::VectorXd& state, Eigen::MatrixXd& covariance, + std::string world_frame_id = "") const; + + //! @brief Get a state from the localization estimator + //! + //! Overload of getState method for using ros::Time. + //! + //! @param[in] ros_time - ros time of the requested state + //! @param[in] frame_id - frame id of which the state is requested. + //! @param[out] state - state at the requested time + //! @param[out] covariance - covariance at the requested time + //! + //! @return false if buffer is empty, true otherwise + //! + bool getState(const ros::Time& ros_time, const std::string& frame_id, + Eigen::VectorXd& state, Eigen::MatrixXd& covariance, + const std::string& world_frame_id = "") const; + + //! + //! \brief getBaseFrameId Returns the base frame id of the localization listener + //! \return The base frame id + //! + const std::string& getBaseFrameId() const; + + //! + //! \brief getWorldFrameId Returns the world frame id of the localization listener + //! \return The world frame id + //! + const std::string& getWorldFrameId() const; + +private: + //! @brief Callback for odom and accel + //! + //! Puts the information from the odom and accel messages in a Robot Localization Estimator state and sets the most + //! recent state of the estimator. + //! + //! @param[in] odometry message + //! @param[in] accel message + //! + void odomAndAccelCallback(const nav_msgs::Odometry& odom, const geometry_msgs::AccelWithCovarianceStamped& accel); + + //! @brief The core state estimator that facilitates inter- and + //! extrapolation between buffered states. + //! + RobotLocalizationEstimator* estimator_; + + //! @brief A public handle to the ROS node + //! + ros::NodeHandle nh_; + + //! @brief A private handle to the ROS node + //! + ros::NodeHandle nh_p_; + + //! @brief Subscriber to the odometry state topic (output of a filter node) + //! + message_filters::Subscriber odom_sub_; + + //! @brief Subscriber to the acceleration state topic (output of a filter node) + //! + message_filters::Subscriber accel_sub_; + + //! @brief Waits for both an Odometry and an Accel message before calling a single callback function + //! + message_filters::TimeSynchronizer sync_; + + //! @brief Child frame id received from the Odometry message + //! + std::string base_frame_id_; + + //! @brief Frame id received from the odometry message + //! + std::string world_frame_id_; + + //! @brief Tf buffer for looking up transforms + //! + tf2_ros::Buffer tf_buffer_; + + //! @brief Transform listener to fill the tf_buffer + //! + tf2_ros::TransformListener tf_listener_; +}; + +} // namespace RobotLocalization + +#endif // ROBOT_LOCALIZATION_ROS_ROBOT_LOCALIZATION_LISTENER_H diff --git a/Localizations/Packages/robot_localization/include/robot_localization/ukf.h b/Localizations/Packages/robot_localization/include/robot_localization/ukf.h new file mode 100644 index 0000000..8f5bb63 --- /dev/null +++ b/Localizations/Packages/robot_localization/include/robot_localization/ukf.h @@ -0,0 +1,139 @@ +/* + * Copyright (c) 2014, 2015, 2016, Charles River Analytics, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef ROBOT_LOCALIZATION_UKF_H +#define ROBOT_LOCALIZATION_UKF_H + +#include "robot_localization/filter_base.h" + +#include +#include +#include +#include + +namespace RobotLocalization +{ + +//! @brief Unscented Kalman filter class +//! +//! Implementation of an unscenter Kalman filter (UKF). This +//! class derives from FilterBase and overrides the predict() +//! and correct() methods in keeping with the discrete time +//! UKF algorithm. The algorithm was derived from the UKF +//! Wikipedia article at +//! (http://en.wikipedia.org/wiki/Kalman_filter#Unscented_Kalman_filter) +//! ...and this paper: +//! J. J. LaViola, Jr., “A comparison of unscented and extended Kalman +//! filtering for estimating quaternion motion,” in Proc. American Control +//! Conf., Denver, CO, June 4–6, 2003, pp. 2435–2440 +//! Obtained here: http://www.cs.ucf.edu/~jjl/pubs/laviola_acc2003.pdf +//! +class Ukf: public FilterBase +{ + public: + //! @brief Constructor for the Ukf class + //! + //! @param[in] args - Generic argument container. It is assumed + //! that args[0] constains the alpha parameter, args[1] contains + //! the kappa parameter, and args[2] contains the beta parameter. + //! + explicit Ukf(std::vector args); + + //! @brief Destructor for the Ukf class + //! + ~Ukf(); + + //! @brief Carries out the correct step in the predict/update cycle. + //! + //! @param[in] measurement - The measurement to fuse with our estimate + //! + void correct(const Measurement &measurement); + + //! @brief Carries out the predict step in the predict/update cycle. + //! + //! Projects the state and error matrices forward using a model of + //! the vehicle's motion. + //! + //! @param[in] referenceTime - The time at which the prediction is being made + //! @param[in] delta - The time step over which to predict. + //! + void predict(const double referenceTime, const double delta); + + protected: + //! @brief Computes the weighted covariance and sigma points + //! + void generateSigmaPoints(); + + //! @brief Carries out the predict step for the posteriori state of a sigma + //! point. + //! + //! Projects the state and error matrices forward using a model of + //! the vehicle's motion. + //! + //! @param[in,out] sigmaPoint - The sigma point (state vector) to project + //! @param[in] delta - The time step over which to project + //! + void projectSigmaPoint(Eigen::VectorXd& sigmaPoint, double delta); + + //! @brief The UKF sigma points + //! + //! Used to sample possible next states during prediction. + //! + std::vector sigmaPoints_; + + //! @brief This matrix is used to generate the sigmaPoints_ + //! + Eigen::MatrixXd weightedCovarSqrt_; + + //! @brief The weights associated with each sigma point when generating + //! a new state + //! + std::vector stateWeights_; + + //! @brief The weights associated with each sigma point when calculating + //! a predicted estimateErrorCovariance_ + //! + std::vector covarWeights_; + + //! @brief Used in weight generation for the sigma points + //! + double lambda_; + + //! @brief Used to determine if we need to re-compute the sigma + //! points when carrying out multiple corrections + //! + bool uncorrected_; +}; + +} // namespace RobotLocalization + +#endif // ROBOT_LOCALIZATION_UKF_H diff --git a/Localizations/Packages/robot_localization/launch/dual_ekf_navsat_example.launch b/Localizations/Packages/robot_localization/launch/dual_ekf_navsat_example.launch new file mode 100644 index 0000000..bec3c16 --- /dev/null +++ b/Localizations/Packages/robot_localization/launch/dual_ekf_navsat_example.launch @@ -0,0 +1,27 @@ + + + + + + + + + + + + + + + + + diff --git a/Localizations/Packages/robot_localization/launch/ekf_nodelet_template.launch b/Localizations/Packages/robot_localization/launch/ekf_nodelet_template.launch new file mode 100644 index 0000000..f4f74ea --- /dev/null +++ b/Localizations/Packages/robot_localization/launch/ekf_nodelet_template.launch @@ -0,0 +1,38 @@ + + + + + + + + + + + + + + + + + + diff --git a/Localizations/Packages/robot_localization/launch/ekf_template.launch b/Localizations/Packages/robot_localization/launch/ekf_template.launch new file mode 100644 index 0000000..c5c2bbd --- /dev/null +++ b/Localizations/Packages/robot_localization/launch/ekf_template.launch @@ -0,0 +1,11 @@ + + + + + + + + diff --git a/Localizations/Packages/robot_localization/launch/navsat_transform_template.launch b/Localizations/Packages/robot_localization/launch/navsat_transform_template.launch new file mode 100644 index 0000000..9c2a458 --- /dev/null +++ b/Localizations/Packages/robot_localization/launch/navsat_transform_template.launch @@ -0,0 +1,51 @@ + + + + + + + + + + + diff --git a/Localizations/Packages/robot_localization/launch/ukf_template.launch b/Localizations/Packages/robot_localization/launch/ukf_template.launch new file mode 100644 index 0000000..9d2b0b2 --- /dev/null +++ b/Localizations/Packages/robot_localization/launch/ukf_template.launch @@ -0,0 +1,11 @@ + + + + + + + + diff --git a/Localizations/Packages/robot_localization/nodelet_plugins.xml b/Localizations/Packages/robot_localization/nodelet_plugins.xml new file mode 100644 index 0000000..56b98ef --- /dev/null +++ b/Localizations/Packages/robot_localization/nodelet_plugins.xml @@ -0,0 +1,27 @@ + + + + ekf_localization_node as nodelet + + + + + + + ukf_localization_node as nodelet + + + + + + + navsat_transform_node as nodelet + + + diff --git a/Localizations/Packages/robot_localization/package.xml b/Localizations/Packages/robot_localization/package.xml new file mode 100644 index 0000000..3602e6f --- /dev/null +++ b/Localizations/Packages/robot_localization/package.xml @@ -0,0 +1,54 @@ + + + robot_localization + 2.7.4 + Provides nonlinear state estimation through sensor fusion of an abritrary number of sensors. + + Tom Moore + + BSD + + http://ros.org/wiki/robot_localization + + Tom Moore + + catkin + + angles + cmake_modules + diagnostic_msgs + diagnostic_updater + eigen + eigen_conversions + geographiclib + geographic_msgs + geometry_msgs + message_filters + nav_msgs + nodelet + roscpp + sensor_msgs + std_msgs + std_srvs + tf2 + tf2_geometry_msgs + tf2_ros + yaml-cpp + + message_generation + python-catkin-pkg + python3-catkin-pkg + roslint + + message_runtime + + rosbag + rostest + rosunit + + + + + + + diff --git a/Localizations/Packages/robot_localization/params/dual_ekf_navsat_example.yaml b/Localizations/Packages/robot_localization/params/dual_ekf_navsat_example.yaml new file mode 100644 index 0000000..c6d749d --- /dev/null +++ b/Localizations/Packages/robot_localization/params/dual_ekf_navsat_example.yaml @@ -0,0 +1,166 @@ +# For parameter descriptions, please refer to the template parameter files for each node. + +ekf_se_odom: + frequency: 30 + sensor_timeout: 0.1 + two_d_mode: false + transform_time_offset: 0.0 + transform_timeout: 0.0 + print_diagnostics: true + debug: false + + map_frame: map + odom_frame: odom + base_link_frame: base_link + world_frame: odom + + odom0: odometry/wheel + odom0_config: [false, false, false, + false, false, false, + true, true, true, + false, false, true, + false, false, false] + odom0_queue_size: 10 + odom0_nodelay: true + odom0_differential: false + odom0_relative: false + + imu0: imu/data + imu0_config: [false, false, false, + true, true, false, + false, false, false, + true, true, true, + true, true, true] + imu0_nodelay: false + imu0_differential: false + imu0_relative: false + imu0_queue_size: 10 + imu0_remove_gravitational_acceleration: true + + use_control: false + + process_noise_covariance: [1e-3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 1e-3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 1e-3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0.3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0.3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0.5, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0.5, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0.1, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.3, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.3, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.3, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.3, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.3, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.3] + + initial_estimate_covariance: [1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 1.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 1.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 1.0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 1.0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 1.0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0] + +ekf_se_map: + frequency: 30 + sensor_timeout: 0.1 + two_d_mode: false + transform_time_offset: 0.0 + transform_timeout: 0.0 + print_diagnostics: true + debug: false + + map_frame: map + odom_frame: odom + base_link_frame: base_link + world_frame: map + + odom0: odometry/wheel + odom0_config: [false, false, false, + false, false, false, + true, true, true, + false, false, true, + false, false, false] + odom0_queue_size: 10 + odom0_nodelay: true + odom0_differential: false + odom0_relative: false + + odom1: odometry/gps + odom1_config: [true, true, false, + false, false, false, + false, false, false, + false, false, false, + false, false, false] + odom1_queue_size: 10 + odom1_nodelay: true + odom1_differential: false + odom1_relative: false + + imu0: imu/data + imu0_config: [false, false, false, + true, true, false, + false, false, false, + true, true, true, + true, true, true] + imu0_nodelay: true + imu0_differential: false + imu0_relative: false + imu0_queue_size: 10 + imu0_remove_gravitational_acceleration: true + + use_control: false + + process_noise_covariance: [1.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 1.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 1e-3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0.3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0.3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0.5, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0.5, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0.1, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.3, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.3, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.3, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.3, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.3, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.3] + + initial_estimate_covariance: [1.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 1.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 1.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 1.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 1.0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 1.0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 1.0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0] + +navsat_transform: + frequency: 30 + delay: 3.0 + magnetic_declination_radians: 0.0429351 # For lat/long 55.944831, -3.186998 + yaw_offset: 1.570796327 # IMU reads 0 facing magnetic north, not east + zero_altitude: false + broadcast_utm_transform: true + publish_filtered_gps: true + use_odometry_yaw: false + wait_for_datum: false + diff --git a/Localizations/Packages/robot_localization/params/ekf_template.yaml b/Localizations/Packages/robot_localization/params/ekf_template.yaml new file mode 100644 index 0000000..06e722e --- /dev/null +++ b/Localizations/Packages/robot_localization/params/ekf_template.yaml @@ -0,0 +1,255 @@ +# The frequency, in Hz, at which the filter will output a position estimate. Note that the filter will not begin +# computation until it receives at least one message from one of the inputs. It will then run continuously at the +# frequency specified here, regardless of whether it receives more measurements. Defaults to 30 if unspecified. +frequency: 30 + +silent_tf_failure: false +# The period, in seconds, after which we consider a sensor to have timed out. In this event, we carry out a predict +# cycle on the EKF without correcting it. This parameter can be thought of as the minimum frequency with which the +# filter will generate new output. Defaults to 1 / frequency if not specified. +sensor_timeout: 0.1 + +# ekf_localization_node and ukf_localization_node both use a 3D omnidirectional motion model. If this parameter is +# set to true, no 3D information will be used in your state estimate. Use this if you are operating in a planar +# environment and want to ignore the effect of small variations in the ground plane that might otherwise be detected +# by, for example, an IMU. Defaults to false if unspecified. +two_d_mode: false + +# Use this parameter to provide an offset to the transform generated by ekf_localization_node. This can be used for +# future dating the transform, which is required for interaction with some other packages. Defaults to 0.0 if +# unspecified. +transform_time_offset: 0.0 + +# Use this parameter to provide specify how long the tf listener should wait for a transform to become available. +# Defaults to 0.0 if unspecified. +transform_timeout: 0.0 + +# If you're having trouble, try setting this to true, and then echo the /diagnostics_agg topic to see if the node is +# unhappy with any settings or data. +print_diagnostics: true + +# Debug settings. Not for the faint of heart. Outputs a ludicrous amount of information to the file specified by +# debug_out_file. I hope you like matrices! Please note that setting this to true will have strongly deleterious +# effects on the performance of the node. Defaults to false if unspecified. +debug: false + +# Defaults to "robot_localization_debug.txt" if unspecified. Please specify the full path. +debug_out_file: /path/to/debug/file.txt + +# Whether to broadcast the transformation over the /tf topic. Defaults to true if unspecified. +publish_tf: true + +# Whether to publish the acceleration state. Defaults to false if unspecified. +publish_acceleration: false + +# Whether we'll allow old measurements to cause a re-publication of the updated state +permit_corrected_publication: false + +# REP-105 (http://www.ros.org/reps/rep-0105.html) specifies four principal coordinate frames: base_link, odom, map, and +# earth. base_link is the coordinate frame that is affixed to the robot. Both odom and map are world-fixed frames. +# The robot's position in the odom frame will drift over time, but is accurate in the short term and should be +# continuous. The odom frame is therefore the best frame for executing local motion plans. The map frame, like the odom +# frame, is a world-fixed coordinate frame, and while it contains the most globally accurate position estimate for your +# robot, it is subject to discrete jumps, e.g., due to the fusion of GPS data or a correction from a map-based +# localization node. The earth frame is used to relate multiple map frames by giving them a common reference frame. +# ekf_localization_node and ukf_localization_node are not concerned with the earth frame. +# Here is how to use the following settings: +# 1. Set the map_frame, odom_frame, and base_link frames to the appropriate frame names for your system. +# 1a. If your system does not have a map_frame, just remove it, and make sure "world_frame" is set to the value of +# odom_frame. +# 2. If you are fusing continuous position data such as wheel encoder odometry, visual odometry, or IMU data, set +# "world_frame" to your odom_frame value. This is the default behavior for robot_localization's state estimation nodes. +# 3. If you are fusing global absolute position data that is subject to discrete jumps (e.g., GPS or position updates +# from landmark observations) then: +# 3a. Set your "world_frame" to your map_frame value +# 3b. MAKE SURE something else is generating the odom->base_link transform. Note that this can even be another state +# estimation node from robot_localization! However, that instance should *not* fuse the global data. +map_frame: map # Defaults to "map" if unspecified +odom_frame: odom # Defaults to "odom" if unspecified +base_link_frame: base_link # Defaults to "base_link" if unspecified +world_frame: odom # Defaults to the value of odom_frame if unspecified + +# The filter accepts an arbitrary number of inputs from each input message type (nav_msgs/Odometry, +# geometry_msgs/PoseWithCovarianceStamped, geometry_msgs/TwistWithCovarianceStamped, +# sensor_msgs/Imu). To add an input, simply append the next number in the sequence to its "base" name, e.g., odom0, +# odom1, twist0, twist1, imu0, imu1, imu2, etc. The value should be the topic name. These parameters obviously have no +# default values, and must be specified. +odom0: example/odom + +# Each sensor reading updates some or all of the filter's state. These options give you greater control over which +# values from each measurement are fed to the filter. For example, if you have an odometry message as input, but only +# want to use its Z position value, then set the entire vector to false, except for the third entry. The order of the +# values is x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Note that not some message types +# do not provide some of the state variables estimated by the filter. For example, a TwistWithCovarianceStamped message +# has no pose information, so the first six values would be meaningless in that case. Each vector defaults to all false +# if unspecified, effectively making this parameter required for each sensor. +odom0_config: [true, true, false, + false, false, false, + false, false, false, + false, false, true, + false, false, false] + +# If you have high-frequency data or are running with a low frequency parameter value, then you may want to increase +# the size of the subscription queue so that more measurements are fused. +odom0_queue_size: 2 + +# [ADVANCED] Large messages in ROS can exhibit strange behavior when they arrive at a high frequency. This is a result +# of Nagle's algorithm. This option tells the ROS subscriber to use the tcpNoDelay option, which disables Nagle's +# algorithm. +odom0_nodelay: false + +# [ADVANCED] When measuring one pose variable with two sensors, a situation can arise in which both sensors under- +# report their covariances. This can lead to the filter rapidly jumping back and forth between each measurement as they +# arrive. In these cases, it often makes sense to (a) correct the measurement covariances, or (b) if velocity is also +# measured by one of the sensors, let one sensor measure pose, and the other velocity. However, doing (a) or (b) isn't +# always feasible, and so we expose the differential parameter. When differential mode is enabled, all absolute pose +# data is converted to velocity data by differentiating the absolute pose measurements. These velocities are then +# integrated as usual. NOTE: this only applies to sensors that provide pose measurements; setting differential to true +# for twist measurements has no effect. +odom0_differential: false + +# [ADVANCED] When the node starts, if this parameter is true, then the first measurement is treated as a "zero point" +# for all future measurements. While you can achieve the same effect with the differential paremeter, the key +# difference is that the relative parameter doesn't cause the measurement to be converted to a velocity before +# integrating it. If you simply want your measurements to start at 0 for a given sensor, set this to true. +odom0_relative: false + +# [ADVANCED] If your data is subject to outliers, use these threshold settings, expressed as Mahalanobis distances, to +# control how far away from the current vehicle state a sensor measurement is permitted to be. Each defaults to +# numeric_limits::max() if unspecified. It is strongly recommended that these parameters be removed if not +# required. Data is specified at the level of pose and twist variables, rather than for each variable in isolation. +# For messages that have both pose and twist data, the parameter specifies to which part of the message we are applying +# the thresholds. +odom0_pose_rejection_threshold: 5 +odom0_twist_rejection_threshold: 1 + +# Further input parameter examples +odom1: example/another_odom +odom1_config: [false, false, true, + false, false, false, + false, false, false, + false, false, true, + false, false, false] +odom1_differential: false +odom1_relative: true +odom1_queue_size: 2 +odom1_pose_rejection_threshold: 2 +odom1_twist_rejection_threshold: 0.2 +odom1_nodelay: false + +pose0: example/pose +pose0_config: [true, true, false, + false, false, false, + false, false, false, + false, false, false, + false, false, false] +pose0_differential: true +pose0_relative: false +pose0_queue_size: 5 +pose0_rejection_threshold: 2 # Note the difference in parameter name +pose0_nodelay: false + +twist0: example/twist +twist0_config: [false, false, false, + false, false, false, + true, true, true, + false, false, false, + false, false, false] +twist0_queue_size: 3 +twist0_rejection_threshold: 2 +twist0_nodelay: false + +imu0: example/imu +imu0_config: [false, false, false, + true, true, true, + false, false, false, + true, true, true, + true, true, true] +imu0_nodelay: false +imu0_differential: false +imu0_relative: true +imu0_queue_size: 5 +imu0_pose_rejection_threshold: 0.8 # Note the difference in parameter names +imu0_twist_rejection_threshold: 0.8 # +imu0_linear_acceleration_rejection_threshold: 0.8 # + +# [ADVANCED] Some IMUs automatically remove acceleration due to gravity, and others don't. If yours doesn't, please set +# this to true, and *make sure* your data conforms to REP-103, specifically, that the data is in ENU frame. +imu0_remove_gravitational_acceleration: true + +# [ADVANCED] The EKF and UKF models follow a standard predict/correct cycle. During prediction, if there is no +# acceleration reference, the velocity at time t+1 is simply predicted to be the same as the velocity at time t. During +# correction, this predicted value is fused with the measured value to produce the new velocity estimate. This can be +# problematic, as the final velocity will effectively be a weighted average of the old velocity and the new one. When +# this velocity is the integrated into a new pose, the result can be sluggish covergence. This effect is especially +# noticeable with LIDAR data during rotations. To get around it, users can try inflating the process_noise_covariance +# for the velocity variable in question, or decrease the variance of the variable in question in the measurement +# itself. In addition, users can also take advantage of the control command being issued to the robot at the time we +# make the prediction. If control is used, it will get converted into an acceleration term, which will be used during +# predicition. Note that if an acceleration measurement for the variable in question is available from one of the +# inputs, the control term will be ignored. +# Whether or not we use the control input during predicition. Defaults to false. +use_control: true +# Whether the input (assumed to be cmd_vel) is a geometry_msgs/Twist or geometry_msgs/TwistStamped message. Defaults to +# false. +stamped_control: false +# The last issued control command will be used in prediction for this period. Defaults to 0.2. +control_timeout: 0.2 +# Which velocities are being controlled. Order is vx, vy, vz, vroll, vpitch, vyaw. +control_config: [true, false, false, false, false, true] +# Places limits on how large the acceleration term will be. Should match your robot's kinematics. +acceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 3.4] +# Acceleration and deceleration limits are not always the same for robots. +deceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 4.5] +# If your robot cannot instantaneously reach its acceleration limit, the permitted change can be controlled with these +# gains +acceleration_gains: [0.8, 0.0, 0.0, 0.0, 0.0, 0.9] +# If your robot cannot instantaneously reach its deceleration limit, the permitted change can be controlled with these +# gains +deceleration_gains: [1.0, 0.0, 0.0, 0.0, 0.0, 1.0] + +# [ADVANCED] The process noise covariance matrix can be difficult to tune, and can vary for each application, so it is +# exposed as a configuration parameter. This matrix represents the noise we add to the total error after each +# prediction step. The better the omnidirectional motion model matches your system, the smaller these values can be. +# However, if users find that a given variable is slow to converge, one approach is to increase the +# process_noise_covariance diagonal value for the variable in question, which will cause the filter's predicted error +# to be larger, which will cause the filter to trust the incoming measurement more during correction. The values are +# ordered as x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Defaults to the matrix below if +# unspecified. +process_noise_covariance: [0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0.06, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0.06, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0.04, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.02, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.015] + +# [ADVANCED] This represents the initial value for the state estimate error covariance matrix. Setting a diagonal +# value (variance) to a large value will result in rapid convergence for initial measurements of the variable in +# question. Users should take care not to use large values for variables that will not be measured directly. The values +# are ordered as x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Defaults to the matrix below +#if unspecified. +initial_estimate_covariance: [1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9] + diff --git a/Localizations/Packages/robot_localization/params/navsat_transform_template.yaml b/Localizations/Packages/robot_localization/params/navsat_transform_template.yaml new file mode 100644 index 0000000..5571720 --- /dev/null +++ b/Localizations/Packages/robot_localization/params/navsat_transform_template.yaml @@ -0,0 +1,48 @@ +# Frequency of the main run loop +frequency: 30 + +# Delay time, in seconds, before we calculate the transform from the UTM frame to your world frame. This is especially +# important if you have use_odometry_yaw set to true. Defaults to 0. +delay: 3.0 + +# PLEASE READ: Like all nodes in robot_localization, this node assumes that your IMU data is reported in the ENU frame. +# Many IMUs report data in the NED frame, so you'll want to verify that your data is in the correct frame before using +# it. + +# If your IMU does not account for magnetic declination, enter the value for your location here. If you don't know it, +# see http://www.ngdc.noaa.gov/geomag-web/ (make sure to convert the value to radians). This parameter is mandatory. +magnetic_declination_radians: 0 + +# Your IMU's yaw, once the magentic_declination_radians value is added to it, should report 0 when facing east. If it +# doesn't, enter the offset here. Defaults to 0. +yaw_offset: 0.0 + +# If this is true, the altitude is set to 0 in the output odometry message. Defaults to false. +zero_altitude: false + +# If this is true, the transform world_frame->utm transform is broadcast for use by other nodes. Defaults to false. +broadcast_utm_transform: false + +# If this is true, the utm->world_frame transform will be published instead of the world_frame->utm transform. +# Note that broadcast_utm_transform still has to be enabled. Defaults to false. +broadcast_utm_transform_as_parent_frame: false + +# If this is true, all received odometry data is converted back to a lat/lon and published as a NavSatFix message as +# /gps/filtered. Defaults to false. +publish_filtered_gps: false + +# If this is true, the node ignores the IMU data and gets its heading from the odometry source (typically the +# /odometry/filtered topic coming from one of robot_localization's state estimation nodes). BE CAREFUL when using this! +# The yaw value in your odometry source *must* be world-referenced, e.g., you cannot use your odometry source for yaw +# if your yaw data is based purely on integrated velocities. Defaults to false. +use_odometry_yaw: false + +# If true, will retrieve the datum from the 'datum' parameter below, if available. If no 'datum' parameter exists, +# navsat_transform_node will wait until the user calls the 'datum' service with the SetDatum service message. +wait_for_datum: false + +# Instead of using the first GPS location and IMU-based heading for the local-frame origin, users can specify the +# origin (datum) using this parameter. The fields in the parameter represent latitude and longitude in decimal degrees, +# and heading in radians. As navsat_transform_node assumes an ENU standard, a 0 heading corresponds to east. +datum: [55.944904, -3.186693, 0.0] + diff --git a/Localizations/Packages/robot_localization/params/ukf_template.yaml b/Localizations/Packages/robot_localization/params/ukf_template.yaml new file mode 100644 index 0000000..683313f --- /dev/null +++ b/Localizations/Packages/robot_localization/params/ukf_template.yaml @@ -0,0 +1,267 @@ +# The frequency, in Hz, at which the filter will output a position estimate. Note that the filter will not begin +# computation until it receives at least one message from one of the inputs. It will then run continuously at the +# frequency specified here, regardless of whether it receives more measurements. Defaults to 30 if unspecified. +frequency: 30 + +# The period, in seconds, after which we consider a sensor to have timed out. In this event, we carry out a predict +# cycle on the EKF without correcting it. This parameter can be thought of as the minimum frequency with which the +# filter will generate new output. Defaults to 1 / frequency if not specified. +sensor_timeout: 0.1 + +# ekf_localization_node and ukf_localization_node both use a 3D omnidirectional motion model. If this parameter is +# set to true, no 3D information will be used in your state estimate. Use this if you are operating in a planar +# environment and want to ignore the effect of small variations in the ground plane that might otherwise be detected +# by, for example, an IMU. Defaults to false if unspecified. +two_d_mode: false + +# Use this parameter to provide an offset to the transform generated by ekf_localization_node. This can be used for +# future dating the transform, which is required for interaction with some other packages. Defaults to 0.0 if +# unspecified. +transform_time_offset: 0.0 + +# Use this parameter to provide specify how long the tf listener should wait for a transform to become available. +# Defaults to 0.0 if unspecified. +transform_timeout: 0.0 + +# If you're having trouble, try setting this to true, and then echo the /diagnostics_agg topic to see if the node is +# unhappy with any settings or data. +print_diagnostics: true + +# Whether we'll allow old measurements to cause a re-publication of the updated state +permit_corrected_publication: false + +# Debug settings. Not for the faint of heart. Outputs a ludicrous amount of information to the file specified by +# debug_out_file. I hope you like matrices! Please note that setting this to true will have strongly deleterious +# effects on the performance of the node. Defaults to false if unspecified. +debug: false + +# Defaults to "robot_localization_debug.txt" if unspecified. Please specify the full path. +debug_out_file: /path/to/debug/file.txt + +# Whether to broadcast the transformation over the /tf topic. Defaults to true if unspecified. +publish_tf: true + +# Whether to publish the acceleration state. Defaults to false if unspecified. +publish_acceleration: false + +# Whether we'll allow old measurements to cause a re-publication of the updated state +permit_corrected_publication: false + +# REP-105 (http://www.ros.org/reps/rep-0105.html) specifies four principal coordinate frames: base_link, odom, map, and +# earth. base_link is the coordinate frame that is affixed to the robot. Both odom and map are world-fixed frames. +# The robot's position in the odom frame will drift over time, but is accurate in the short term and should be +# continuous. The odom frame is therefore the best frame for executing local motion plans. The map frame, like the odom +# frame, is a world-fixed coordinate frame, and while it contains the most globally accurate position estimate for your +# robot, it is subject to discrete jumps, e.g., due to the fusion of GPS data or a correction from a map-based +# localization node. The earth frame is used to relate multiple map frames by giving them a common reference frame. +# ekf_localization_node and ukf_localization_node are not concerned with the earth frame. +# Here is how to use the following settings: +# 1. Set the map_frame, odom_frame, and base_link frames to the appropriate frame names for your system. +# 1a. If your system does not have a map_frame, just remove it, and make sure "world_frame" is set to the value of +# odom_frame. +# 2. If you are fusing continuous position data such as wheel encoder odometry, visual odometry, or IMU data, set +# "world_frame" to your odom_frame value. This is the default behavior for robot_localization's state estimation nodes. +# 3. If you are fusing global absolute position data that is subject to discrete jumps (e.g., GPS or position updates +# from landmark observations) then: +# 3a. Set your "world_frame" to your map_frame value +# 3b. MAKE SURE something else is generating the odom->base_link transform. Note that this can even be another state +# estimation node from robot_localization! However, that instance should *not* fuse the global data. +map_frame: map # Defaults to "map" if unspecified +odom_frame: odom # Defaults to "odom" if unspecified +base_link_frame: base_link # Defaults to "base_link" if unspecified +world_frame: odom # Defaults to the value of odom_frame if unspecified + +# The filter accepts an arbitrary number of inputs from each input message type (nav_msgs/Odometry, +# geometry_msgs/PoseWithCovarianceStamped, geometry_msgs/TwistWithCovarianceStamped, +# sensor_msgs/Imu). To add an input, simply append the next number in the sequence to its "base" name, e.g., odom0, +# odom1, twist0, twist1, imu0, imu1, imu2, etc. The value should be the topic name. These parameters obviously have no +# default values, and must be specified. +odom0: example/odom + +# Each sensor reading updates some or all of the filter's state. These options give you greater control over which +# values from each measurement are fed to the filter. For example, if you have an odometry message as input, but only +# want to use its Z position value, then set the entire vector to false, except for the third entry. The order of the +# values is x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Note that not some message types +# do not provide some of the state variables estimated by the filter. For example, a TwistWithCovarianceStamped message +# has no pose information, so the first six values would be meaningless in that case. Each vector defaults to all false +# if unspecified, effectively making this parameter required for each sensor. +odom0_config: [true, true, false, + false, false, false, + false, false, false, + false, false, true, + false, false, false] + +# If you have high-frequency data or are running with a low frequency parameter value, then you may want to increase +# the size of the subscription queue so that more measurements are fused. +odom0_queue_size: 2 + +# [ADVANCED] Large messages in ROS can exhibit strange behavior when they arrive at a high frequency. This is a result +# of Nagle's algorithm. This option tells the ROS subscriber to use the tcpNoDelay option, which disables Nagle's +# algorithm. +odom0_nodelay: false + +# [ADVANCED] When measuring one pose variable with two sensors, a situation can arise in which both sensors under- +# report their covariances. This can lead to the filter rapidly jumping back and forth between each measurement as they +# arrive. In these cases, it often makes sense to (a) correct the measurement covariances, or (b) if velocity is also +# measured by one of the sensors, let one sensor measure pose, and the other velocity. However, doing (a) or (b) isn't +# always feasible, and so we expose the differential parameter. When differential mode is enabled, all absolute pose +# data is converted to velocity data by differentiating the absolute pose measurements. These velocities are then +# integrated as usual. NOTE: this only applies to sensors that provide pose measurements; setting differential to true +# for twist measurements has no effect. +odom0_differential: false + +# [ADVANCED] When the node starts, if this parameter is true, then the first measurement is treated as a "zero point" +# for all future measurements. While you can achieve the same effect with the differential paremeter, the key +# difference is that the relative parameter doesn't cause the measurement to be converted to a velocity before +# integrating it. If you simply want your measurements to start at 0 for a given sensor, set this to true. +odom0_relative: false + +# [ADVANCED] If your data is subject to outliers, use these threshold settings, expressed as Mahalanobis distances, to +# control how far away from the current vehicle state a sensor measurement is permitted to be. Each defaults to +# numeric_limits::max() if unspecified. It is strongly recommended that these parameters be removed if not +# required. Data is specified at the level of pose and twist variables, rather than for each variable in isolation. +# For messages that have both pose and twist data, the parameter specifies to which part of the message we are applying +# the thresholds. +odom0_pose_rejection_threshold: 5 +odom0_twist_rejection_threshold: 1 + +# Further input parameter examples +odom1: example/another_odom +odom1_config: [false, false, true, + false, false, false, + false, false, false, + false, false, true, + false, false, false] +odom1_differential: false +odom1_relative: true +odom1_queue_size: 2 +odom1_pose_rejection_threshold: 2 +odom1_twist_rejection_threshold: 0.2 +odom1_nodelay: false + +pose0: example/pose +pose0_config: [true, true, false, + false, false, false, + false, false, false, + false, false, false, + false, false, false] +pose0_differential: true +pose0_relative: false +pose0_queue_size: 5 +pose0_rejection_threshold: 2 # Note the difference in parameter name +pose0_nodelay: false + +twist0: example/twist +twist0_config: [false, false, false, + false, false, false, + true, true, true, + false, false, false, + false, false, false] +twist0_queue_size: 3 +twist0_rejection_threshold: 2 +twist0_nodelay: false + +imu0: example/imu +imu0_config: [false, false, false, + true, true, true, + false, false, false, + true, true, true, + true, true, true] +imu0_nodelay: false +imu0_differential: false +imu0_relative: true +imu0_queue_size: 5 +imu0_pose_rejection_threshold: 0.8 # Note the difference in parameter names +imu0_twist_rejection_threshold: 0.8 # +imu0_linear_acceleration_rejection_threshold: 0.8 # + +# [ADVANCED] Some IMUs automatically remove acceleration due to gravity, and others don't. If yours doesn't, please set +# this to true, and *make sure* your data conforms to REP-103, specifically, that the data is in ENU frame. +imu0_remove_gravitational_acceleration: true + +# [ADVANCED] The EKF and UKF models follow a standard predict/correct cycle. During prediction, if there is no +# acceleration reference, the velocity at time t+1 is simply predicted to be the same as the velocity at time t. During +# correction, this predicted value is fused with the measured value to produce the new velocity estimate. This can be +# problematic, as the final velocity will effectively be a weighted average of the old velocity and the new one. When +# this velocity is the integrated into a new pose, the result can be sluggish covergence. This effect is especially +# noticeable with LIDAR data during rotations. To get around it, users can try inflating the process_noise_covariance +# for the velocity variable in question, or decrease the variance of the variable in question in the measurement +# itself. In addition, users can also take advantage of the control command being issued to the robot at the time we +# make the prediction. If control is used, it will get converted into an acceleration term, which will be used during +# predicition. Note that if an acceleration measurement for the variable in question is available from one of the +# inputs, the control term will be ignored. +# Whether or not we use the control input during predicition. Defaults to false. +use_control: true +# Whether the input (assumed to be cmd_vel) is a geometry_msgs/Twist or geometry_msgs/TwistStamped message. Defaults to +# false. +stamped_control: false +# The last issued control command will be used in prediction for this period. Defaults to 0.2. +control_timeout: 0.2 +# Which velocities are being controlled. Order is vx, vy, vz, vroll, vpitch, vyaw. +control_config: [true, false, false, false, false, true] +# Places limits on how large the acceleration term will be. Should match your robot's kinematics. +acceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 3.4] +# Acceleration and deceleration limits are not always the same for robots. +deceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 4.5] +# If your robot cannot instantaneously reach its acceleration limit, the permitted change can be controlled with these +# gains +acceleration_gains: [0.8, 0.0, 0.0, 0.0, 0.0, 0.9] +# If your robot cannot instantaneously reach its deceleration limit, the permitted change can be controlled with these +# gains +deceleration_gains: [1.0, 0.0, 0.0, 0.0, 0.0, 1.0] + +# [ADVANCED] The process noise covariance matrix can be difficult to tune, and can vary for each application, so it is +# exposed as a configuration parameter. This matrix represents the noise we add to the total error after each +# prediction step. The better the omnidirectional motion model matches your system, the smaller these values can be. +# However, if users find that a given variable is slow to converge, one approach is to increase the +# process_noise_covariance diagonal value for the variable in question, which will cause the filter's predicted error +# to be larger, which will cause the filter to trust the incoming measurement more during correction. The values are +# ordered as x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Defaults to the matrix below if +# unspecified. +process_noise_covariance: [0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0.06, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0.06, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0.04, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.02, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.015] + +# [ADVANCED] This represents the initial value for the state estimate error covariance matrix. Setting a diagonal +# value (variance) to a large value will result in rapid convergence for initial measurements of the variable in +# question. Users should take care not to use large values for variables that will not be measured directly. The values +# are ordered as x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Defaults to the matrix below +#if unspecified. +initial_estimate_covariance: [1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9] + +# [ADVANCED, UKF ONLY] The alpha and kappa variables control the spread of the sigma points. Unless you are familiar +# with UKFs, it's probably a good idea to leave these alone. +# Defaults to 0.001 if unspecified. +alpha: 0.001 +# Defaults to 0 if unspecified. +kappa: 0 + +# [ADVANCED, UKF ONLY] The beta variable relates to the distribution of the state vector. Again, it's probably best to +# leave this alone if you're uncertain. Defaults to 2 if unspecified. +beta: 2 diff --git a/Localizations/Packages/robot_localization/rosdoc.yaml b/Localizations/Packages/robot_localization/rosdoc.yaml new file mode 100644 index 0000000..c9c76e3 --- /dev/null +++ b/Localizations/Packages/robot_localization/rosdoc.yaml @@ -0,0 +1,6 @@ + - builder: sphinx + sphinx_root_dir: doc + - builder: doxygen + output_dir: api + file_patterns: '*.cpp *.h *.dox *.md' + exclude_patterns: '*/test/*' diff --git a/Localizations/Packages/robot_localization/src/ekf.cpp b/Localizations/Packages/robot_localization/src/ekf.cpp new file mode 100644 index 0000000..2076028 --- /dev/null +++ b/Localizations/Packages/robot_localization/src/ekf.cpp @@ -0,0 +1,389 @@ +/* + * Copyright (c) 2014, 2015, 2016, Charles River Analytics, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "robot_localization/ekf.h" +#include "robot_localization/filter_common.h" + +#include + +#include +#include +#include +#include + + +namespace RobotLocalization +{ + Ekf::Ekf(std::vector) : + FilterBase() // Must initialize filter base! + { + } + + Ekf::~Ekf() + { + } + + void Ekf::correct(const Measurement &measurement) + { + FB_DEBUG("---------------------- Ekf::correct ----------------------\n" << + "State is:\n" << state_ << "\n" + "Topic is:\n" << measurement.topicName_ << "\n" + "Measurement is:\n" << measurement.measurement_ << "\n" + "Measurement topic name is:\n" << measurement.topicName_ << "\n\n" + "Measurement covariance is:\n" << measurement.covariance_ << "\n"); + + // We don't want to update everything, so we need to build matrices that only update + // the measured parts of our state vector. Throughout prediction and correction, we + // attempt to maximize efficiency in Eigen. + + // First, determine how many state vector values we're updating + std::vector updateIndices; + for (size_t i = 0; i < measurement.updateVector_.size(); ++i) + { + if (measurement.updateVector_[i]) + { + // Handle nan and inf values in measurements + if (std::isnan(measurement.measurement_(i))) + { + FB_DEBUG("Value at index " << i << " was nan. Excluding from update.\n"); + } + else if (std::isinf(measurement.measurement_(i))) + { + FB_DEBUG("Value at index " << i << " was inf. Excluding from update.\n"); + } + else + { + updateIndices.push_back(i); + } + } + } + + FB_DEBUG("Update indices are:\n" << updateIndices << "\n"); + + size_t updateSize = updateIndices.size(); + + // Now set up the relevant matrices + Eigen::VectorXd stateSubset(updateSize); // x (in most literature) + Eigen::VectorXd measurementSubset(updateSize); // z + Eigen::MatrixXd measurementCovarianceSubset(updateSize, updateSize); // R + Eigen::MatrixXd stateToMeasurementSubset(updateSize, state_.rows()); // H + Eigen::MatrixXd kalmanGainSubset(state_.rows(), updateSize); // K + Eigen::VectorXd innovationSubset(updateSize); // z - Hx + + stateSubset.setZero(); + measurementSubset.setZero(); + measurementCovarianceSubset.setZero(); + stateToMeasurementSubset.setZero(); + kalmanGainSubset.setZero(); + innovationSubset.setZero(); + + // Now build the sub-matrices from the full-sized matrices + for (size_t i = 0; i < updateSize; ++i) + { + measurementSubset(i) = measurement.measurement_(updateIndices[i]); + stateSubset(i) = state_(updateIndices[i]); + + for (size_t j = 0; j < updateSize; ++j) + { + measurementCovarianceSubset(i, j) = measurement.covariance_(updateIndices[i], updateIndices[j]); + } + + // Handle negative (read: bad) covariances in the measurement. Rather + // than exclude the measurement or make up a covariance, just take + // the absolute value. + if (measurementCovarianceSubset(i, i) < 0) + { + FB_DEBUG("WARNING: Negative covariance for index " << i << + " of measurement (value is" << measurementCovarianceSubset(i, i) << + "). Using absolute value...\n"); + + measurementCovarianceSubset(i, i) = ::fabs(measurementCovarianceSubset(i, i)); + } + + // If the measurement variance for a given variable is very + // near 0 (as in e-50 or so) and the variance for that + // variable in the covariance matrix is also near zero, then + // the Kalman gain computation will blow up. Really, no + // measurement can be completely without error, so add a small + // amount in that case. + if (measurementCovarianceSubset(i, i) < 1e-9) + { + FB_DEBUG("WARNING: measurement had very small error covariance for index " << updateIndices[i] << + ". Adding some noise to maintain filter stability.\n"); + + measurementCovarianceSubset(i, i) = 1e-9; + } + } + + // The state-to-measurement function, h, will now be a measurement_size x full_state_size + // matrix, with ones in the (i, i) locations of the values to be updated + for (size_t i = 0; i < updateSize; ++i) + { + stateToMeasurementSubset(i, updateIndices[i]) = 1; + } + + FB_DEBUG("Current state subset is:\n" << stateSubset << + "\nMeasurement subset is:\n" << measurementSubset << + "\nMeasurement covariance subset is:\n" << measurementCovarianceSubset << + "\nState-to-measurement subset is:\n" << stateToMeasurementSubset << "\n"); + + // (1) Compute the Kalman gain: K = (PH') / (HPH' + R) + Eigen::MatrixXd pht = estimateErrorCovariance_ * stateToMeasurementSubset.transpose(); + Eigen::MatrixXd hphrInv = (stateToMeasurementSubset * pht + measurementCovarianceSubset).inverse(); + kalmanGainSubset.noalias() = pht * hphrInv; + + innovationSubset = (measurementSubset - stateSubset); + + // Wrap angles in the innovation + for (size_t i = 0; i < updateSize; ++i) + { + if (updateIndices[i] == StateMemberRoll || + updateIndices[i] == StateMemberPitch || + updateIndices[i] == StateMemberYaw) + { + while (innovationSubset(i) < -PI) + { + innovationSubset(i) += TAU; + } + + while (innovationSubset(i) > PI) + { + innovationSubset(i) -= TAU; + } + } + } + + // (2) Check Mahalanobis distance between mapped measurement and state. + if (checkMahalanobisThreshold(innovationSubset, hphrInv, measurement.mahalanobisThresh_)) + { + // (3) Apply the gain to the difference between the state and measurement: x = x + K(z - Hx) + state_.noalias() += kalmanGainSubset * innovationSubset; + + // (4) Update the estimate error covariance using the Joseph form: (I - KH)P(I - KH)' + KRK' + Eigen::MatrixXd gainResidual = identity_; + gainResidual.noalias() -= kalmanGainSubset * stateToMeasurementSubset; + estimateErrorCovariance_ = gainResidual * estimateErrorCovariance_ * gainResidual.transpose(); + estimateErrorCovariance_.noalias() += kalmanGainSubset * + measurementCovarianceSubset * + kalmanGainSubset.transpose(); + + // Handle wrapping of angles + wrapStateAngles(); + + FB_DEBUG("Kalman gain subset is:\n" << kalmanGainSubset << + "\nInnovation is:\n" << innovationSubset << + "\nCorrected full state is:\n" << state_ << + "\nCorrected full estimate error covariance is:\n" << estimateErrorCovariance_ << + "\n\n---------------------- /Ekf::correct ----------------------\n"); + } + } + + void Ekf::predict(const double referenceTime, const double delta) + { + FB_DEBUG("---------------------- Ekf::predict ----------------------\n" << + "delta is " << delta << "\n" << + "state is " << state_ << "\n"); + + double roll = state_(StateMemberRoll); + double pitch = state_(StateMemberPitch); + double yaw = state_(StateMemberYaw); + double xVel = state_(StateMemberVx); + double yVel = state_(StateMemberVy); + double zVel = state_(StateMemberVz); + double pitchVel = state_(StateMemberVpitch); + double yawVel = state_(StateMemberVyaw); + double xAcc = state_(StateMemberAx); + double yAcc = state_(StateMemberAy); + double zAcc = state_(StateMemberAz); + + // We'll need these trig calculations a lot. + double sp = ::sin(pitch); + double cp = ::cos(pitch); + double cpi = 1.0 / cp; + double tp = sp * cpi; + + double sr = ::sin(roll); + double cr = ::cos(roll); + + double sy = ::sin(yaw); + double cy = ::cos(yaw); + + prepareControl(referenceTime, delta); + + // Prepare the transfer function + transferFunction_(StateMemberX, StateMemberVx) = cy * cp * delta; + transferFunction_(StateMemberX, StateMemberVy) = (cy * sp * sr - sy * cr) * delta; + transferFunction_(StateMemberX, StateMemberVz) = (cy * sp * cr + sy * sr) * delta; + transferFunction_(StateMemberX, StateMemberAx) = 0.5 * transferFunction_(StateMemberX, StateMemberVx) * delta; + transferFunction_(StateMemberX, StateMemberAy) = 0.5 * transferFunction_(StateMemberX, StateMemberVy) * delta; + transferFunction_(StateMemberX, StateMemberAz) = 0.5 * transferFunction_(StateMemberX, StateMemberVz) * delta; + transferFunction_(StateMemberY, StateMemberVx) = sy * cp * delta; + transferFunction_(StateMemberY, StateMemberVy) = (sy * sp * sr + cy * cr) * delta; + transferFunction_(StateMemberY, StateMemberVz) = (sy * sp * cr - cy * sr) * delta; + transferFunction_(StateMemberY, StateMemberAx) = 0.5 * transferFunction_(StateMemberY, StateMemberVx) * delta; + transferFunction_(StateMemberY, StateMemberAy) = 0.5 * transferFunction_(StateMemberY, StateMemberVy) * delta; + transferFunction_(StateMemberY, StateMemberAz) = 0.5 * transferFunction_(StateMemberY, StateMemberVz) * delta; + transferFunction_(StateMemberZ, StateMemberVx) = -sp * delta; + transferFunction_(StateMemberZ, StateMemberVy) = cp * sr * delta; + transferFunction_(StateMemberZ, StateMemberVz) = cp * cr * delta; + transferFunction_(StateMemberZ, StateMemberAx) = 0.5 * transferFunction_(StateMemberZ, StateMemberVx) * delta; + transferFunction_(StateMemberZ, StateMemberAy) = 0.5 * transferFunction_(StateMemberZ, StateMemberVy) * delta; + transferFunction_(StateMemberZ, StateMemberAz) = 0.5 * transferFunction_(StateMemberZ, StateMemberVz) * delta; + transferFunction_(StateMemberRoll, StateMemberVroll) = delta; + transferFunction_(StateMemberRoll, StateMemberVpitch) = sr * tp * delta; + transferFunction_(StateMemberRoll, StateMemberVyaw) = cr * tp * delta; + transferFunction_(StateMemberPitch, StateMemberVpitch) = cr * delta; + transferFunction_(StateMemberPitch, StateMemberVyaw) = -sr * delta; + transferFunction_(StateMemberYaw, StateMemberVpitch) = sr * cpi * delta; + transferFunction_(StateMemberYaw, StateMemberVyaw) = cr * cpi * delta; + transferFunction_(StateMemberVx, StateMemberAx) = delta; + transferFunction_(StateMemberVy, StateMemberAy) = delta; + transferFunction_(StateMemberVz, StateMemberAz) = delta; + + // Prepare the transfer function Jacobian. This function is analytically derived from the + // transfer function. + double xCoeff = 0.0; + double yCoeff = 0.0; + double zCoeff = 0.0; + double oneHalfATSquared = 0.5 * delta * delta; + + yCoeff = cy * sp * cr + sy * sr; + zCoeff = -cy * sp * sr + sy * cr; + double dFx_dR = (yCoeff * yVel + zCoeff * zVel) * delta + + (yCoeff * yAcc + zCoeff * zAcc) * oneHalfATSquared; + double dFR_dR = 1.0 + (cr * tp * pitchVel - sr * tp * yawVel) * delta; + + xCoeff = -cy * sp; + yCoeff = cy * cp * sr; + zCoeff = cy * cp * cr; + double dFx_dP = (xCoeff * xVel + yCoeff * yVel + zCoeff * zVel) * delta + + (xCoeff * xAcc + yCoeff * yAcc + zCoeff * zAcc) * oneHalfATSquared; + double dFR_dP = (cpi * cpi * sr * pitchVel + cpi * cpi * cr * yawVel) * delta; + + xCoeff = -sy * cp; + yCoeff = -sy * sp * sr - cy * cr; + zCoeff = -sy * sp * cr + cy * sr; + double dFx_dY = (xCoeff * xVel + yCoeff * yVel + zCoeff * zVel) * delta + + (xCoeff * xAcc + yCoeff * yAcc + zCoeff * zAcc) * oneHalfATSquared; + + yCoeff = sy * sp * cr - cy * sr; + zCoeff = -sy * sp * sr - cy * cr; + double dFy_dR = (yCoeff * yVel + zCoeff * zVel) * delta + + (yCoeff * yAcc + zCoeff * zAcc) * oneHalfATSquared; + double dFP_dR = (-sr * pitchVel - cr * yawVel) * delta; + + xCoeff = -sy * sp; + yCoeff = sy * cp * sr; + zCoeff = sy * cp * cr; + double dFy_dP = (xCoeff * xVel + yCoeff * yVel + zCoeff * zVel) * delta + + (xCoeff * xAcc + yCoeff * yAcc + zCoeff * zAcc) * oneHalfATSquared; + + xCoeff = cy * cp; + yCoeff = cy * sp * sr - sy * cr; + zCoeff = cy * sp * cr + sy * sr; + double dFy_dY = (xCoeff * xVel + yCoeff * yVel + zCoeff * zVel) * delta + + (xCoeff * xAcc + yCoeff * yAcc + zCoeff * zAcc) * oneHalfATSquared; + + yCoeff = cp * cr; + zCoeff = -cp * sr; + double dFz_dR = (yCoeff * yVel + zCoeff * zVel) * delta + + (yCoeff * yAcc + zCoeff * zAcc) * oneHalfATSquared; + double dFY_dR = (cr * cpi * pitchVel - sr * cpi * yawVel) * delta; + + xCoeff = -cp; + yCoeff = -sp * sr; + zCoeff = -sp * cr; + double dFz_dP = (xCoeff * xVel + yCoeff * yVel + zCoeff * zVel) * delta + + (xCoeff * xAcc + yCoeff * yAcc + zCoeff * zAcc) * oneHalfATSquared; + double dFY_dP = (sr * tp * cpi * pitchVel + cr * tp * cpi * yawVel) * delta; + + // Much of the transfer function Jacobian is identical to the transfer function + transferFunctionJacobian_ = transferFunction_; + transferFunctionJacobian_(StateMemberX, StateMemberRoll) = dFx_dR; + transferFunctionJacobian_(StateMemberX, StateMemberPitch) = dFx_dP; + transferFunctionJacobian_(StateMemberX, StateMemberYaw) = dFx_dY; + transferFunctionJacobian_(StateMemberY, StateMemberRoll) = dFy_dR; + transferFunctionJacobian_(StateMemberY, StateMemberPitch) = dFy_dP; + transferFunctionJacobian_(StateMemberY, StateMemberYaw) = dFy_dY; + transferFunctionJacobian_(StateMemberZ, StateMemberRoll) = dFz_dR; + transferFunctionJacobian_(StateMemberZ, StateMemberPitch) = dFz_dP; + transferFunctionJacobian_(StateMemberRoll, StateMemberRoll) = dFR_dR; + transferFunctionJacobian_(StateMemberRoll, StateMemberPitch) = dFR_dP; + transferFunctionJacobian_(StateMemberPitch, StateMemberRoll) = dFP_dR; + transferFunctionJacobian_(StateMemberYaw, StateMemberRoll) = dFY_dR; + transferFunctionJacobian_(StateMemberYaw, StateMemberPitch) = dFY_dP; + + FB_DEBUG("Transfer function is:\n" << transferFunction_ << + "\nTransfer function Jacobian is:\n" << transferFunctionJacobian_ << + "\nProcess noise covariance is:\n" << processNoiseCovariance_ << + "\nCurrent state is:\n" << state_ << "\n"); + + Eigen::MatrixXd *processNoiseCovariance = &processNoiseCovariance_; + + if (useDynamicProcessNoiseCovariance_) + { + computeDynamicProcessNoiseCovariance(state_, delta); + processNoiseCovariance = &dynamicProcessNoiseCovariance_; + } + + // (1) Apply control terms, which are actually accelerations + state_(StateMemberVroll) += controlAcceleration_(ControlMemberVroll) * delta; + state_(StateMemberVpitch) += controlAcceleration_(ControlMemberVpitch) * delta; + state_(StateMemberVyaw) += controlAcceleration_(ControlMemberVyaw) * delta; + + state_(StateMemberAx) = (controlUpdateVector_[ControlMemberVx] ? + controlAcceleration_(ControlMemberVx) : state_(StateMemberAx)); + state_(StateMemberAy) = (controlUpdateVector_[ControlMemberVy] ? + controlAcceleration_(ControlMemberVy) : state_(StateMemberAy)); + state_(StateMemberAz) = (controlUpdateVector_[ControlMemberVz] ? + controlAcceleration_(ControlMemberVz) : state_(StateMemberAz)); + + // (2) Project the state forward: x = Ax + Bu (really, x = f(x, u)) + state_ = transferFunction_ * state_; + + // Handle wrapping + wrapStateAngles(); + + FB_DEBUG("Predicted state is:\n" << state_ << + "\nCurrent estimate error covariance is:\n" << estimateErrorCovariance_ << "\n"); + + // (3) Project the error forward: P = J * P * J' + Q + estimateErrorCovariance_ = (transferFunctionJacobian_ * + estimateErrorCovariance_ * + transferFunctionJacobian_.transpose()); + estimateErrorCovariance_.noalias() += delta * (*processNoiseCovariance); + + FB_DEBUG("Predicted estimate error covariance is:\n" << estimateErrorCovariance_ << + "\n\n--------------------- /Ekf::predict ----------------------\n"); + } + +} // namespace RobotLocalization diff --git a/Localizations/Packages/robot_localization/src/ekf_localization_node.cpp b/Localizations/Packages/robot_localization/src/ekf_localization_node.cpp new file mode 100644 index 0000000..797bbe9 --- /dev/null +++ b/Localizations/Packages/robot_localization/src/ekf_localization_node.cpp @@ -0,0 +1,51 @@ +/* + * Copyright (c) 2014, 2015, 2016, Charles River Analytics, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "robot_localization/ros_filter_types.h" + +#include + +#include + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "ekf_navigation_node"); + + ros::NodeHandle nh; + ros::NodeHandle nh_priv("~"); + + RobotLocalization::RosEkf ekf(nh, nh_priv); + ekf.initialize(); + ros::spin(); + + return EXIT_SUCCESS; +} diff --git a/Localizations/Packages/robot_localization/src/ekf_localization_nodelet.cpp b/Localizations/Packages/robot_localization/src/ekf_localization_nodelet.cpp new file mode 100644 index 0000000..337fa52 --- /dev/null +++ b/Localizations/Packages/robot_localization/src/ekf_localization_nodelet.cpp @@ -0,0 +1,64 @@ +/* + * Copyright (c) 2017 Simon Gene Gottlieb + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "robot_localization/ros_filter_types.h" + +#include +#include +#include + +#include + +namespace RobotLocalization +{ + +class EkfNodelet : public nodelet::Nodelet +{ +private: + std::unique_ptr ekf; + +public: + virtual void onInit() + { + NODELET_DEBUG("Initializing nodelet..."); + + ros::NodeHandle nh = getNodeHandle(); + ros::NodeHandle nh_priv = getPrivateNodeHandle(); + + ekf = std::make_unique(nh, nh_priv, getName()); + ekf->initialize(); + } +}; + +} // namespace RobotLocalization + +PLUGINLIB_EXPORT_CLASS(RobotLocalization::EkfNodelet, nodelet::Nodelet); diff --git a/Localizations/Packages/robot_localization/src/filter_base.cpp b/Localizations/Packages/robot_localization/src/filter_base.cpp new file mode 100644 index 0000000..efd181f --- /dev/null +++ b/Localizations/Packages/robot_localization/src/filter_base.cpp @@ -0,0 +1,403 @@ +/* + * Copyright (c) 2014, 2015, 2016, Charles River Analytics, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "robot_localization/filter_base.h" +#include "robot_localization/filter_common.h" + +#include +#include +#include +#include +#include + +namespace RobotLocalization +{ + FilterBase::FilterBase() : + initialized_(false), + useControl_(false), + useDynamicProcessNoiseCovariance_(false), + lastMeasurementTime_(0.0), + latestControlTime_(0.0), + controlTimeout_(0.0), + sensorTimeout_(0.0), + controlUpdateVector_(TWIST_SIZE, 0), + accelerationGains_(TWIST_SIZE, 0.0), + accelerationLimits_(TWIST_SIZE, 0.0), + decelerationGains_(TWIST_SIZE, 0.0), + decelerationLimits_(TWIST_SIZE, 0.0), + controlAcceleration_(TWIST_SIZE), + latestControl_(TWIST_SIZE), + predictedState_(STATE_SIZE), + state_(STATE_SIZE), + covarianceEpsilon_(STATE_SIZE, STATE_SIZE), + dynamicProcessNoiseCovariance_(STATE_SIZE, STATE_SIZE), + estimateErrorCovariance_(STATE_SIZE, STATE_SIZE), + identity_(STATE_SIZE, STATE_SIZE), + processNoiseCovariance_(STATE_SIZE, STATE_SIZE), + transferFunction_(STATE_SIZE, STATE_SIZE), + transferFunctionJacobian_(STATE_SIZE, STATE_SIZE), + debugStream_(NULL), + debug_(false) + { + reset(); + } + + FilterBase::~FilterBase() + { + } + + void FilterBase::reset() + { + initialized_ = false; + + // Clear the state and predicted state + state_.setZero(); + predictedState_.setZero(); + controlAcceleration_.setZero(); + + // Prepare the invariant parts of the transfer + // function + transferFunction_.setIdentity(); + + // Clear the Jacobian + transferFunctionJacobian_.setZero(); + + // Set the estimate error covariance. We want our measurements + // to be accepted rapidly when the filter starts, so we should + // initialize the state's covariance with large values. + estimateErrorCovariance_.setIdentity(); + estimateErrorCovariance_ *= 1e-9; + + // We need the identity for the update equations + identity_.setIdentity(); + + // Set the epsilon matrix to be a matrix with small values on the diagonal + // It is used to maintain the positive-definite property of the covariance + covarianceEpsilon_.setIdentity(); + covarianceEpsilon_ *= 0.001; + + // Assume 30Hz from sensor data (configurable) + sensorTimeout_ = 0.033333333; + + // Initialize our measurement time + lastMeasurementTime_ = 0; + + // These can be overridden via the launch parameters, + // but we need default values. + processNoiseCovariance_.setZero(); + processNoiseCovariance_(StateMemberX, StateMemberX) = 0.05; + processNoiseCovariance_(StateMemberY, StateMemberY) = 0.05; + processNoiseCovariance_(StateMemberZ, StateMemberZ) = 0.06; + processNoiseCovariance_(StateMemberRoll, StateMemberRoll) = 0.03; + processNoiseCovariance_(StateMemberPitch, StateMemberPitch) = 0.03; + processNoiseCovariance_(StateMemberYaw, StateMemberYaw) = 0.06; + processNoiseCovariance_(StateMemberVx, StateMemberVx) = 0.025; + processNoiseCovariance_(StateMemberVy, StateMemberVy) = 0.025; + processNoiseCovariance_(StateMemberVz, StateMemberVz) = 0.04; + processNoiseCovariance_(StateMemberVroll, StateMemberVroll) = 0.01; + processNoiseCovariance_(StateMemberVpitch, StateMemberVpitch) = 0.01; + processNoiseCovariance_(StateMemberVyaw, StateMemberVyaw) = 0.02; + processNoiseCovariance_(StateMemberAx, StateMemberAx) = 0.01; + processNoiseCovariance_(StateMemberAy, StateMemberAy) = 0.01; + processNoiseCovariance_(StateMemberAz, StateMemberAz) = 0.015; + + dynamicProcessNoiseCovariance_ = processNoiseCovariance_; + } + + void FilterBase::computeDynamicProcessNoiseCovariance(const Eigen::VectorXd &state, const double delta) + { + // A more principled approach would be to get the current velocity from the state, make a diagonal matrix from it, + // and then rotate it to be in the world frame (i.e., the same frame as the pose data). We could then use this + // rotated velocity matrix to scale the process noise covariance for the pose variables as + // rotatedVelocityMatrix * poseCovariance * rotatedVelocityMatrix' + // However, this presents trouble for robots that may incur rotational error as a result of linear motion (and + // vice-versa). Instead, we create a diagonal matrix whose diagonal values are the vector norm of the state's + // velocity. We use that to scale the process noise covariance. + Eigen::MatrixXd velocityMatrix(TWIST_SIZE, TWIST_SIZE); + velocityMatrix.setIdentity(); + velocityMatrix.diagonal() *= state.segment(POSITION_V_OFFSET, TWIST_SIZE).norm(); + + dynamicProcessNoiseCovariance_.block(POSITION_OFFSET, POSITION_OFFSET) = + velocityMatrix * + processNoiseCovariance_.block(POSITION_OFFSET, POSITION_OFFSET) * + velocityMatrix.transpose(); + } + + const Eigen::VectorXd& FilterBase::getControl() + { + return latestControl_; + } + + double FilterBase::getControlTime() + { + return latestControlTime_; + } + + bool FilterBase::getDebug() + { + return debug_; + } + + const Eigen::MatrixXd& FilterBase::getEstimateErrorCovariance() + { + return estimateErrorCovariance_; + } + + bool FilterBase::getInitializedStatus() + { + return initialized_; + } + + double FilterBase::getLastMeasurementTime() + { + return lastMeasurementTime_; + } + + const Eigen::VectorXd& FilterBase::getPredictedState() + { + return predictedState_; + } + + const Eigen::MatrixXd& FilterBase::getProcessNoiseCovariance() + { + return processNoiseCovariance_; + } + + double FilterBase::getSensorTimeout() + { + return sensorTimeout_; + } + + const Eigen::VectorXd& FilterBase::getState() + { + return state_; + } + + void FilterBase::processMeasurement(const Measurement &measurement) + { + FB_DEBUG("------ FilterBase::processMeasurement (" << measurement.topicName_ << ") ------\n"); + + double delta = 0.0; + + // If we've had a previous reading, then go through the predict/update + // cycle. Otherwise, set our state and covariance to whatever we get + // from this measurement. + if (initialized_) + { + // Determine how much time has passed since our last measurement + delta = measurement.time_ - lastMeasurementTime_; + + FB_DEBUG("Filter is already initialized. Carrying out predict/correct loop...\n" + "Measurement time is " << std::setprecision(20) << measurement.time_ << + ", last measurement time is " << lastMeasurementTime_ << ", delta is " << delta << "\n"); + + // Only want to carry out a prediction if it's + // forward in time. Otherwise, just correct. + if (delta > 0) + { + validateDelta(delta); + predict(measurement.time_, delta); + + // Return this to the user + predictedState_ = state_; + } + + correct(measurement); + } + else + { + FB_DEBUG("First measurement. Initializing filter.\n"); + + // Initialize the filter, but only with the values we're using + size_t measurementLength = measurement.updateVector_.size(); + for (size_t i = 0; i < measurementLength; ++i) + { + state_[i] = (measurement.updateVector_[i] ? measurement.measurement_[i] : state_[i]); + } + + // Same for covariance + for (size_t i = 0; i < measurementLength; ++i) + { + for (size_t j = 0; j < measurementLength; ++j) + { + estimateErrorCovariance_(i, j) = (measurement.updateVector_[i] && measurement.updateVector_[j] ? + measurement.covariance_(i, j) : + estimateErrorCovariance_(i, j)); + } + } + + initialized_ = true; + } + + if (delta >= 0.0) + { + lastMeasurementTime_ = measurement.time_; + } + + FB_DEBUG("------ /FilterBase::processMeasurement (" << measurement.topicName_ << ") ------\n"); + } + + void FilterBase::setControl(const Eigen::VectorXd &control, const double controlTime) + { + latestControl_ = control; + latestControlTime_ = controlTime; + } + + void FilterBase::setControlParams(const std::vector &updateVector, const double controlTimeout, + const std::vector &accelerationLimits, const std::vector &accelerationGains, + const std::vector &decelerationLimits, const std::vector &decelerationGains) + { + useControl_ = true; + controlUpdateVector_ = updateVector; + controlTimeout_ = controlTimeout; + accelerationLimits_ = accelerationLimits; + accelerationGains_ = accelerationGains; + decelerationLimits_ = decelerationLimits; + decelerationGains_ = decelerationGains; + } + + void FilterBase::setDebug(const bool debug, std::ostream *outStream) + { + if (debug) + { + if (outStream != NULL) + { + debugStream_ = outStream; + debug_ = true; + } + else + { + debug_ = false; + } + } + else + { + debug_ = false; + } + } + + void FilterBase::setUseDynamicProcessNoiseCovariance(const bool useDynamicProcessNoiseCovariance) + { + useDynamicProcessNoiseCovariance_ = useDynamicProcessNoiseCovariance; + } + + void FilterBase::setEstimateErrorCovariance(const Eigen::MatrixXd &estimateErrorCovariance) + { + estimateErrorCovariance_ = estimateErrorCovariance; + } + + void FilterBase::setLastMeasurementTime(const double lastMeasurementTime) + { + lastMeasurementTime_ = lastMeasurementTime; + } + + void FilterBase::setProcessNoiseCovariance(const Eigen::MatrixXd &processNoiseCovariance) + { + processNoiseCovariance_ = processNoiseCovariance; + dynamicProcessNoiseCovariance_ = processNoiseCovariance_; + } + + void FilterBase::setSensorTimeout(const double sensorTimeout) + { + sensorTimeout_ = sensorTimeout; + } + + void FilterBase::setState(const Eigen::VectorXd &state) + { + state_ = state; + } + + void FilterBase::validateDelta(double &delta) + { + // This handles issues with ROS time when use_sim_time is on and we're playing from bags. + if (delta > 100000.0) + { + FB_DEBUG("Delta was very large. Suspect playing from bag file. Setting to 0.01\n"); + + delta = 0.01; + } + } + + + void FilterBase::prepareControl(const double referenceTime, const double predictionDelta) + { + controlAcceleration_.setZero(); + + if (useControl_) + { + bool timedOut = ::fabs(referenceTime - latestControlTime_) >= controlTimeout_; + + if (timedOut) + { + FB_DEBUG("Control timed out. Reference time was " << referenceTime << ", latest control time was " << + latestControlTime_ << ", control timeout was " << controlTimeout_ << "\n"); + } + + for (size_t controlInd = 0; controlInd < TWIST_SIZE; ++controlInd) + { + if (controlUpdateVector_[controlInd]) + { + controlAcceleration_(controlInd) = computeControlAcceleration(state_(controlInd + POSITION_V_OFFSET), + (timedOut ? 0.0 : latestControl_(controlInd)), accelerationLimits_[controlInd], + accelerationGains_[controlInd], decelerationLimits_[controlInd], decelerationGains_[controlInd]); + } + } + } + } + + void FilterBase::wrapStateAngles() + { + state_(StateMemberRoll) = FilterUtilities::clampRotation(state_(StateMemberRoll)); + state_(StateMemberPitch) = FilterUtilities::clampRotation(state_(StateMemberPitch)); + state_(StateMemberYaw) = FilterUtilities::clampRotation(state_(StateMemberYaw)); + } + + bool FilterBase::checkMahalanobisThreshold(const Eigen::VectorXd &innovation, + const Eigen::MatrixXd &invCovariance, + const double nsigmas) + { + double sqMahalanobis = innovation.dot(invCovariance * innovation); + double threshold = nsigmas * nsigmas; + + if (sqMahalanobis >= threshold) + { + FB_DEBUG("Innovation mahalanobis distance test failed. Squared Mahalanobis is: " << sqMahalanobis << "\n" << + "Threshold is: " << threshold << "\n" << + "Innovation is: " << innovation << "\n" << + "Innovation covariance is:\n" << invCovariance << "\n"); + + return false; + } + + return true; + } +} // namespace RobotLocalization diff --git a/Localizations/Packages/robot_localization/src/filter_utilities.cpp b/Localizations/Packages/robot_localization/src/filter_utilities.cpp new file mode 100644 index 0000000..259644d --- /dev/null +++ b/Localizations/Packages/robot_localization/src/filter_utilities.cpp @@ -0,0 +1,146 @@ +/* + * Copyright (c) 2014, 2015, 2016, Charles River Analytics, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "robot_localization/filter_utilities.h" +#include "robot_localization/filter_common.h" + +#include +#include + +std::ostream& operator<<(std::ostream& os, const Eigen::MatrixXd &mat) +{ + os << "["; + + int rowCount = static_cast(mat.rows()); + + for (int row = 0; row < rowCount; ++row) + { + if (row > 0) + { + os << " "; + } + + for (int col = 0; col < mat.cols(); ++col) + { + os << std::setiosflags(std::ios::left) << std::setw(12) << std::setprecision(5) << mat(row, col); + } + + if (row < rowCount - 1) + { + os << "\n"; + } + } + + os << "]\n"; + + return os; +} + +std::ostream& operator<<(std::ostream& os, const Eigen::VectorXd &vec) +{ + os << "["; + for (int dim = 0; dim < vec.rows(); ++dim) + { + os << std::setiosflags(std::ios::left) << std::setw(12) << std::setprecision(5) << vec(dim); + } + os << "]\n"; + + return os; +} + +std::ostream& operator<<(std::ostream& os, const std::vector &vec) +{ + os << "["; + for (size_t dim = 0; dim < vec.size(); ++dim) + { + os << std::setiosflags(std::ios::left) << std::setw(12) << std::setprecision(5) << vec[dim]; + } + os << "]\n"; + + return os; +} + +std::ostream& operator<<(std::ostream& os, const std::vector &vec) +{ + os << "["; + for (size_t dim = 0; dim < vec.size(); ++dim) + { + os << std::setiosflags(std::ios::left) << std::setw(3) << (vec[dim] ? "t" : "f"); + } + os << "]\n"; + + return os; +} + +namespace RobotLocalization +{ + +namespace FilterUtilities +{ + void appendPrefix(std::string tfPrefix, std::string &frameId) + { + // Strip all leading slashes for tf2 compliance + if (!frameId.empty() && frameId.at(0) == '/') + { + frameId = frameId.substr(1); + } + + if (!tfPrefix.empty() && tfPrefix.at(0) == '/') + { + tfPrefix = tfPrefix.substr(1); + } + + // If we do have a tf prefix, then put a slash in between + if (!tfPrefix.empty()) + { + frameId = tfPrefix + "/" + frameId; + } + } + + double clampRotation(double rotation) + { + while (rotation > PI) + { + rotation -= TAU; + } + + while (rotation < -PI) + { + rotation += TAU; + } + + return rotation; + } + +} // namespace FilterUtilities + +} // namespace RobotLocalization diff --git a/Localizations/Packages/robot_localization/src/navsat_transform.cpp b/Localizations/Packages/robot_localization/src/navsat_transform.cpp new file mode 100644 index 0000000..5ec9f1b --- /dev/null +++ b/Localizations/Packages/robot_localization/src/navsat_transform.cpp @@ -0,0 +1,915 @@ +/* + * Copyright (c) 2014, 2015, 2016, Charles River Analytics, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "robot_localization/navsat_transform.h" +#include "robot_localization/filter_common.h" +#include "robot_localization/filter_utilities.h" +#include "robot_localization/navsat_conversions.h" +#include "robot_localization/ros_filter_utilities.h" + +#include +#include + +#include + +namespace RobotLocalization +{ + NavSatTransform::NavSatTransform(ros::NodeHandle nh, ros::NodeHandle nh_priv) : + broadcast_cartesian_transform_(false), + broadcast_cartesian_transform_as_parent_frame_(false), + gps_updated_(false), + has_transform_gps_(false), + has_transform_imu_(false), + has_transform_odom_(false), + odom_updated_(false), + publish_gps_(false), + transform_good_(false), + use_manual_datum_(false), + use_odometry_yaw_(false), + use_local_cartesian_(false), + zero_altitude_(false), + magnetic_declination_(0.0), + yaw_offset_(0.0), + base_link_frame_id_("base_link"), + gps_frame_id_(""), + utm_zone_(0), + world_frame_id_("odom"), + transform_timeout_(ros::Duration(0)), + tf_listener_(tf_buffer_) + { + ROS_INFO("Waiting for valid clock time..."); + ros::Time::waitForValid(); + ROS_INFO("Valid clock time received. Starting node."); + + latest_cartesian_covariance_.resize(POSE_SIZE, POSE_SIZE); + latest_odom_covariance_.resize(POSE_SIZE, POSE_SIZE); + + double frequency; + double delay = 0.0; + double transform_timeout = 0.0; + + // Load the parameters we need + nh_priv.getParam("magnetic_declination_radians", magnetic_declination_); + nh_priv.param("yaw_offset", yaw_offset_, 0.0); + nh_priv.param("broadcast_cartesian_transform", broadcast_cartesian_transform_, false); + nh_priv.param("broadcast_cartesian_transform_as_parent_frame", + broadcast_cartesian_transform_as_parent_frame_, false); + nh_priv.param("zero_altitude", zero_altitude_, false); + nh_priv.param("publish_filtered_gps", publish_gps_, false); + nh_priv.param("use_odometry_yaw", use_odometry_yaw_, false); + nh_priv.param("wait_for_datum", use_manual_datum_, false); + nh_priv.param("use_local_cartesian", use_local_cartesian_, false); + nh_priv.param("frequency", frequency, 10.0); + nh_priv.param("delay", delay, 0.0); + nh_priv.param("transform_timeout", transform_timeout, 0.0); + nh_priv.param("cartesian_frame_id", cartesian_frame_id_, std::string(use_local_cartesian_ ? "local_enu" : "utm")); + transform_timeout_.fromSec(transform_timeout); + + // Check for deprecated parameters + if (nh_priv.getParam("broadcast_utm_transform", broadcast_cartesian_transform_)) + { + ROS_WARN("navsat_transform, Parameter 'broadcast_utm_transform' has been deprecated. Please use" + "'broadcast_cartesian_transform' instead."); + } + if (nh_priv.getParam("broadcast_utm_transform_as_parent_frame", broadcast_cartesian_transform_as_parent_frame_)) + { + ROS_WARN("navsat_transform, Parameter 'broadcast_utm_transform_as_parent_frame' has been deprecated. Please use" + "'broadcast_cartesian_transform_as_parent_frame' instead."); + } + + // Check if tf warnings should be suppressed + nh.getParam("/silent_tf_failure", tf_silent_failure_); + + // Subscribe to the messages and services we need + datum_srv_ = nh.advertiseService("datum", &NavSatTransform::datumCallback, this); + + to_ll_srv_ = nh.advertiseService("toLL", &NavSatTransform::toLLCallback, this); + from_ll_srv_ = nh.advertiseService("fromLL", &NavSatTransform::fromLLCallback, this); + set_utm_zone_srv_ = nh.advertiseService("setUTMZone", &NavSatTransform::setUTMZoneCallback, this); + + if (use_manual_datum_ && nh_priv.hasParam("datum")) + { + XmlRpc::XmlRpcValue datum_config; + + try + { + double datum_lat; + double datum_lon; + double datum_yaw; + + nh_priv.getParam("datum", datum_config); + + // Handle datum specification. Users should always specify a baseLinkFrameId_ in the + // datum config, but we had a release where it wasn't used, so we'll maintain compatibility. + ROS_ASSERT(datum_config.getType() == XmlRpc::XmlRpcValue::TypeArray); + ROS_ASSERT(datum_config.size() >= 3); + + if (datum_config.size() > 3) + { + ROS_WARN_STREAM("Deprecated datum parameter configuration detected. Only the first three parameters " + "(latitude, longitude, yaw) will be used. frame_ids will be derived from odometry and navsat inputs."); + } + + std::ostringstream ostr; + ostr << std::setprecision(20) << datum_config[0] << " " << datum_config[1] << " " << datum_config[2]; + std::istringstream istr(ostr.str()); + istr >> datum_lat >> datum_lon >> datum_yaw; + + // Try to resolve tf_prefix + std::string tf_prefix = ""; + std::string tf_prefix_path = ""; + if (nh_priv.searchParam("tf_prefix", tf_prefix_path)) + { + nh_priv.getParam(tf_prefix_path, tf_prefix); + } + + // Append the tf prefix in a tf2-friendly manner + FilterUtilities::appendPrefix(tf_prefix, world_frame_id_); + FilterUtilities::appendPrefix(tf_prefix, base_link_frame_id_); + + robot_localization::SetDatum::Request request; + request.geo_pose.position.latitude = datum_lat; + request.geo_pose.position.longitude = datum_lon; + request.geo_pose.position.altitude = 0.0; + tf2::Quaternion quat; + quat.setRPY(0.0, 0.0, datum_yaw); + request.geo_pose.orientation = tf2::toMsg(quat); + robot_localization::SetDatum::Response response; + datumCallback(request, response); + } + catch (XmlRpc::XmlRpcException &e) + { + ROS_ERROR_STREAM("ERROR reading sensor config: " << e.getMessage() << + " for process_noise_covariance (type: " << datum_config.getType() << ")"); + } + } + + odom_sub_ = nh.subscribe("odometry/filtered", 1, &NavSatTransform::odomCallback, this); + gps_sub_ = nh.subscribe("gps/fix", 1, &NavSatTransform::gpsFixCallback, this); + + if (!use_odometry_yaw_ && !use_manual_datum_) + { + imu_sub_ = nh.subscribe("imu/data", 1, &NavSatTransform::imuCallback, this); + } + + gps_odom_pub_ = nh.advertise("odometry/gps", 10); + + if (publish_gps_) + { + filtered_gps_pub_ = nh.advertise("gps/filtered", 10); + } + + // Sleep for the parameterized amount of time, to give + // other nodes time to start up (not always necessary) + ros::Duration start_delay(delay); + start_delay.sleep(); + + periodicUpdateTimer_ = nh.createTimer(ros::Duration(1./frequency), &NavSatTransform::periodicUpdate, this); + } + + NavSatTransform::~NavSatTransform() + { + } + +// void NavSatTransform::run() + void NavSatTransform::periodicUpdate(const ros::TimerEvent& event) + { + if (!transform_good_) + { + computeTransform(); + + if (transform_good_ && !use_odometry_yaw_ && !use_manual_datum_) + { + // Once we have the transform, we don't need the IMU + imu_sub_.shutdown(); + } + } + else + { + nav_msgs::Odometry gps_odom; + if (prepareGpsOdometry(gps_odom)) + { + gps_odom_pub_.publish(gps_odom); + } + + if (publish_gps_) + { + sensor_msgs::NavSatFix odom_gps; + if (prepareFilteredGps(odom_gps)) + { + filtered_gps_pub_.publish(odom_gps); + } + } + } + } + + void NavSatTransform::computeTransform() + { + // Only do this if: + // 1. We haven't computed the odom_frame->cartesian_frame transform before + // 2. We've received the data we need + if (!transform_good_ && + has_transform_odom_ && + has_transform_gps_ && + has_transform_imu_) + { + // The cartesian pose we have is given at the location of the GPS sensor on the robot. We need to get the + // cartesian pose of the robot's origin. + tf2::Transform transform_cartesian_pose_corrected; + if (!use_manual_datum_) + { + getRobotOriginCartesianPose(transform_cartesian_pose_, transform_cartesian_pose_corrected, ros::Time(0)); + } + else + { + transform_cartesian_pose_corrected = transform_cartesian_pose_; + } + + // Get the IMU's current RPY values. Need the raw values (for yaw, anyway). + tf2::Matrix3x3 mat(transform_orientation_); + + // Convert to RPY + double imu_roll; + double imu_pitch; + double imu_yaw; + mat.getRPY(imu_roll, imu_pitch, imu_yaw); + + /* The IMU's heading was likely originally reported w.r.t. magnetic north. + * However, all the nodes in robot_localization assume that orientation data, + * including that reported by IMUs, is reported in an ENU frame, with a 0 yaw + * value being reported when facing east and increasing counter-clockwise (i.e., + * towards north). To make the world frame ENU aligned, where X is east + * and Y is north, we have to take into account three additional considerations: + * 1. The IMU may have its non-ENU frame data transformed to ENU, but there's + * a possibility that its data has not been corrected for magnetic + * declination. We need to account for this. A positive magnetic + * declination is counter-clockwise in an ENU frame. Therefore, if + * we have a magnetic declination of N radians, then when the sensor + * is facing a heading of N, it reports 0. Therefore, we need to add + * the declination angle. + * 2. To account for any other offsets that may not be accounted for by the + * IMU driver or any interim processing node, we expose a yaw offset that + * lets users work with navsat_transform_node. + * 3. UTM grid isn't aligned with True East\North. To account for the difference + * we need to add meridian convergence angle when using UTM. This value will be + * 0.0 when use_local_cartesian is TRUE. + */ + imu_yaw += (magnetic_declination_ + yaw_offset_ + utm_meridian_convergence_); + + ROS_INFO_STREAM("Corrected for magnetic declination of " << std::fixed << magnetic_declination_ << + ", user-specified offset of " << yaw_offset_ << + " and meridian convergence of " << utm_meridian_convergence_ << "." << + " Transform heading factor is now " << imu_yaw); + + // Convert to tf-friendly structures + tf2::Quaternion imu_quat; + imu_quat.setRPY(0.0, 0.0, imu_yaw); + + // The transform order will be orig_odom_pos * orig_cartesian_pos_inverse * cur_cartesian_pos. + // Doing it this way will allow us to cope with having non-zero odometry position + // when we get our first GPS message. + tf2::Transform cartesian_pose_with_orientation; + cartesian_pose_with_orientation.setOrigin(transform_cartesian_pose_corrected.getOrigin()); + cartesian_pose_with_orientation.setRotation(imu_quat); + + // Remove roll and pitch from odometry pose + // Must be done because roll and pitch is removed from cartesian_pose_with_orientation + double odom_roll, odom_pitch, odom_yaw; + tf2::Matrix3x3(transform_world_pose_.getRotation()).getRPY(odom_roll, odom_pitch, odom_yaw); + tf2::Quaternion odom_quat; + odom_quat.setRPY(0.0, 0.0, odom_yaw); + tf2::Transform transform_world_pose_yaw_only(transform_world_pose_); + transform_world_pose_yaw_only.setRotation(odom_quat); + + cartesian_world_transform_.mult(transform_world_pose_yaw_only, cartesian_pose_with_orientation.inverse()); + + cartesian_world_trans_inverse_ = cartesian_world_transform_.inverse(); + + ROS_INFO_STREAM("Transform world frame pose is: " << transform_world_pose_); + ROS_INFO_STREAM("World frame->cartesian transform is " << cartesian_world_transform_); + + transform_good_ = true; + + // Send out the (static) Cartesian transform in case anyone else would like to use it. + if (broadcast_cartesian_transform_) + { + geometry_msgs::TransformStamped cartesian_transform_stamped; + cartesian_transform_stamped.header.stamp = ros::Time::now(); + cartesian_transform_stamped.header.frame_id = (broadcast_cartesian_transform_as_parent_frame_ ? + cartesian_frame_id_ : world_frame_id_); + cartesian_transform_stamped.child_frame_id = (broadcast_cartesian_transform_as_parent_frame_ ? + world_frame_id_ : cartesian_frame_id_); + cartesian_transform_stamped.transform = (broadcast_cartesian_transform_as_parent_frame_ ? + tf2::toMsg(cartesian_world_trans_inverse_) : + tf2::toMsg(cartesian_world_transform_)); + cartesian_transform_stamped.transform.translation.z = (zero_altitude_ ? + 0.0 : cartesian_transform_stamped.transform.translation.z); + cartesian_broadcaster_.sendTransform(cartesian_transform_stamped); + } + } + } + + bool NavSatTransform::datumCallback(robot_localization::SetDatum::Request& request, + robot_localization::SetDatum::Response&) + { + // If we get a service call with a manual datum, even if we already computed the transform using the robot's + // initial pose, then we want to assume that we are using a datum from now on, and we want other methods to + // not attempt to transform the values we are specifying here. + use_manual_datum_ = true; + + transform_good_ = false; + + sensor_msgs::NavSatFix *fix = new sensor_msgs::NavSatFix(); + fix->latitude = request.geo_pose.position.latitude; + fix->longitude = request.geo_pose.position.longitude; + fix->altitude = request.geo_pose.position.altitude; + fix->header.stamp = ros::Time::now(); + fix->position_covariance[0] = 0.1; + fix->position_covariance[4] = 0.1; + fix->position_covariance[8] = 0.1; + fix->position_covariance_type = sensor_msgs::NavSatStatus::STATUS_FIX; + sensor_msgs::NavSatFixConstPtr fix_ptr(fix); + setTransformGps(fix_ptr); + + nav_msgs::Odometry *odom = new nav_msgs::Odometry(); + odom->pose.pose.orientation.x = 0; + odom->pose.pose.orientation.y = 0; + odom->pose.pose.orientation.z = 0; + odom->pose.pose.orientation.w = 1; + odom->pose.pose.position.x = 0; + odom->pose.pose.position.y = 0; + odom->pose.pose.position.z = 0; + odom->header.frame_id = world_frame_id_; + odom->child_frame_id = base_link_frame_id_; + nav_msgs::OdometryConstPtr odom_ptr(odom); + setTransformOdometry(odom_ptr); + + sensor_msgs::Imu *imu = new sensor_msgs::Imu(); + imu->orientation = request.geo_pose.orientation; + imu->header.frame_id = base_link_frame_id_; + sensor_msgs::ImuConstPtr imu_ptr(imu); + imuCallback(imu_ptr); + + return true; + } + + bool NavSatTransform::toLLCallback(robot_localization::ToLL::Request& request, + robot_localization::ToLL::Response& response) + { + if (!transform_good_) + { + ROS_ERROR("No transform available (yet)"); + return false; + } + tf2::Vector3 point; + tf2::fromMsg(request.map_point, point); + mapToLL(point, response.ll_point.latitude, response.ll_point.longitude, response.ll_point.altitude); + + return true; + } + + bool NavSatTransform::fromLLCallback(robot_localization::FromLL::Request& request, + robot_localization::FromLL::Response& response) + { + double altitude = request.ll_point.altitude; + double longitude = request.ll_point.longitude; + double latitude = request.ll_point.latitude; + + tf2::Transform cartesian_pose; + + double cartesian_x; + double cartesian_y; + double cartesian_z; + + if (use_local_cartesian_) + { + gps_local_cartesian_.Forward(latitude, longitude, altitude, cartesian_x, cartesian_y, cartesian_z); + } + else + { + int zone_tmp; + bool nortp_tmp; + try + { + GeographicLib::UTMUPS::Forward(latitude, longitude, zone_tmp, nortp_tmp, cartesian_x, cartesian_y, utm_zone_); + } + catch (const GeographicLib::GeographicErr& e) + { + ROS_ERROR_STREAM_THROTTLE(1.0, e.what()); + return false; + } + } + + cartesian_pose.setOrigin(tf2::Vector3(cartesian_x, cartesian_y, altitude)); + + nav_msgs::Odometry gps_odom; + + if (!transform_good_) + { + ROS_ERROR("No transform available (yet)"); + return false; + } + + response.map_point = cartesianToMap(cartesian_pose).pose.pose.position; + + return true; + } + + bool NavSatTransform::setUTMZoneCallback(robot_localization::SetUTMZone::Request& request, + robot_localization::SetUTMZone::Response& response) + { + double x_unused; + double y_unused; + int prec_unused; + GeographicLib::MGRS::Reverse(request.utm_zone, utm_zone_, northp_, x_unused, y_unused, prec_unused, true); + ROS_INFO("UTM zone set to %d %s", utm_zone_, northp_ ? "north" : "south"); + return true; + } + + nav_msgs::Odometry NavSatTransform::cartesianToMap(const tf2::Transform& cartesian_pose) const + { + nav_msgs::Odometry gps_odom{}; + + tf2::Transform transformed_cartesian_gps{}; + + transformed_cartesian_gps.mult(cartesian_world_transform_, cartesian_pose); + transformed_cartesian_gps.setRotation(tf2::Quaternion::getIdentity()); + + // Set header information stamp because we would like to know the robot's position at that timestamp + gps_odom.header.frame_id = world_frame_id_; + gps_odom.header.stamp = gps_update_time_; + + // Now fill out the message. Set the orientation to the identity. + tf2::toMsg(transformed_cartesian_gps, gps_odom.pose.pose); + gps_odom.pose.pose.position.z = (zero_altitude_ ? 0.0 : gps_odom.pose.pose.position.z); + + return gps_odom; + } + + void NavSatTransform::mapToLL(const tf2::Vector3& point, double& latitude, double& longitude, double& altitude) const + { + tf2::Transform odom_as_cartesian{}; + + tf2::Transform pose{}; + pose.setOrigin(point); + pose.setRotation(tf2::Quaternion::getIdentity()); + + odom_as_cartesian.mult(cartesian_world_trans_inverse_, pose); + odom_as_cartesian.setRotation(tf2::Quaternion::getIdentity()); + + if (use_local_cartesian_) + { + double altitude_tmp = 0.0; + gps_local_cartesian_.Reverse(odom_as_cartesian.getOrigin().getX(), + odom_as_cartesian.getOrigin().getY(), + 0.0, + latitude, + longitude, + altitude_tmp); + altitude = odom_as_cartesian.getOrigin().getZ(); + } + else + { + GeographicLib::UTMUPS::Reverse(utm_zone_, + northp_, + odom_as_cartesian.getOrigin().getX(), + odom_as_cartesian.getOrigin().getY(), + latitude, + longitude); + altitude = odom_as_cartesian.getOrigin().getZ(); + } + } + + void NavSatTransform::getRobotOriginCartesianPose(const tf2::Transform &gps_cartesian_pose, + tf2::Transform &robot_cartesian_pose, + const ros::Time &transform_time) + { + robot_cartesian_pose.setIdentity(); + + // Get linear offset from origin for the GPS + tf2::Transform offset; + bool can_transform = RosFilterUtilities::lookupTransformSafe(tf_buffer_, + base_link_frame_id_, + gps_frame_id_, + transform_time, + ros::Duration(transform_timeout_), + offset, + tf_silent_failure_); + + if (can_transform) + { + // Get the orientation we'll use for our Cartesian->world transform + tf2::Quaternion cartesian_orientation = transform_orientation_; + tf2::Matrix3x3 mat(cartesian_orientation); + + // Add the offsets + double roll; + double pitch; + double yaw; + mat.getRPY(roll, pitch, yaw); + yaw += (magnetic_declination_ + yaw_offset_ + utm_meridian_convergence_); + cartesian_orientation.setRPY(roll, pitch, yaw); + + // Rotate the GPS linear offset by the orientation + // Zero out the orientation, because the GPS orientation is meaningless, and if it's non-zero, it will make the + // the computation of robot_cartesian_pose erroneous. + offset.setOrigin(tf2::quatRotate(cartesian_orientation, offset.getOrigin())); + offset.setRotation(tf2::Quaternion::getIdentity()); + + // Update the initial pose + robot_cartesian_pose = offset.inverse() * gps_cartesian_pose; + } + else + { + if (gps_frame_id_ != "") + { + ROS_WARN_STREAM_ONCE("Unable to obtain " << base_link_frame_id_ << "->" << gps_frame_id_ << + " transform. Will assume navsat device is mounted at robot's origin"); + } + + robot_cartesian_pose = gps_cartesian_pose; + } + } + + void NavSatTransform::getRobotOriginWorldPose(const tf2::Transform &gps_odom_pose, + tf2::Transform &robot_odom_pose, + const ros::Time &transform_time) + { + robot_odom_pose.setIdentity(); + + // Remove the offset from base_link + tf2::Transform gps_offset_rotated; + bool can_transform = RosFilterUtilities::lookupTransformSafe(tf_buffer_, + base_link_frame_id_, + gps_frame_id_, + transform_time, + transform_timeout_, + gps_offset_rotated, + tf_silent_failure_); + + if (can_transform) + { + tf2::Transform robot_orientation; + can_transform = RosFilterUtilities::lookupTransformSafe(tf_buffer_, + world_frame_id_, + base_link_frame_id_, + transform_time, + transform_timeout_, + robot_orientation, + tf_silent_failure_); + + if (can_transform) + { + // Zero out rotation because we don't care about the orientation of the + // GPS receiver relative to base_link + gps_offset_rotated.setOrigin(tf2::quatRotate(robot_orientation.getRotation(), gps_offset_rotated.getOrigin())); + gps_offset_rotated.setRotation(tf2::Quaternion::getIdentity()); + robot_odom_pose = gps_offset_rotated.inverse() * gps_odom_pose; + } + else + { + ROS_WARN_STREAM_THROTTLE(5.0, "Could not obtain " << world_frame_id_ << "->" << base_link_frame_id_ << + " transform. Will not remove offset of navsat device from robot's origin."); + } + } + else + { + ROS_WARN_STREAM_THROTTLE(5.0, "Could not obtain " << base_link_frame_id_ << "->" << gps_frame_id_ << + " transform. Will not remove offset of navsat device from robot's origin."); + } + } + + void NavSatTransform::gpsFixCallback(const sensor_msgs::NavSatFixConstPtr& msg) + { + gps_frame_id_ = msg->header.frame_id; + + if (gps_frame_id_.empty()) + { + ROS_WARN_STREAM_ONCE("NavSatFix message has empty frame_id. Will assume navsat device is mounted at robot's " + "origin."); + } + + // Make sure the GPS data is usable + bool good_gps = (msg->status.status != sensor_msgs::NavSatStatus::STATUS_NO_FIX && + !std::isnan(msg->altitude) && + !std::isnan(msg->latitude) && + !std::isnan(msg->longitude)); + + if (good_gps) + { + // If we haven't computed the transform yet, then + // store this message as the initial GPS data to use + if (!transform_good_ && !use_manual_datum_) + { + setTransformGps(msg); + } + + double cartesian_x = 0.0; + double cartesian_y = 0.0; + double cartesian_z = 0.0; + if (use_local_cartesian_) + { + gps_local_cartesian_.Forward(msg->latitude, msg->longitude, msg->altitude, + cartesian_x, cartesian_y, cartesian_z); + } + else + { + // Transform to UTM using the fixed utm_zone_ + int zone_tmp; + bool northp_tmp; + try + { + GeographicLib::UTMUPS::Forward(msg->latitude, msg->longitude, + zone_tmp, northp_tmp, cartesian_x, cartesian_y, utm_zone_); + } + catch (const GeographicLib::GeographicErr& e) + { + ROS_ERROR_STREAM_THROTTLE(1.0, e.what()); + return; + } + } + latest_cartesian_pose_.setOrigin(tf2::Vector3(cartesian_x, cartesian_y, msg->altitude)); + latest_cartesian_covariance_.setZero(); + + // Copy the measurement's covariance matrix so that we can rotate it later + for (size_t i = 0; i < POSITION_SIZE; i++) + { + for (size_t j = 0; j < POSITION_SIZE; j++) + { + latest_cartesian_covariance_(i, j) = msg->position_covariance[POSITION_SIZE * i + j]; + } + } + + gps_update_time_ = msg->header.stamp; + gps_updated_ = true; + } + } + + void NavSatTransform::imuCallback(const sensor_msgs::ImuConstPtr& msg) + { + // We need the baseLinkFrameId_ from the odometry message, so + // we need to wait until we receive it. + if (has_transform_odom_) + { + /* This method only gets called if we don't yet have the + * IMU data (the subscriber gets shut down once we compute + * the transform), so we can assumed that every IMU message + * that comes here is meant to be used for that purpose. */ + tf2::fromMsg(msg->orientation, transform_orientation_); + + // Correct for the IMU's orientation w.r.t. base_link + tf2::Transform target_frame_trans; + bool can_transform = RosFilterUtilities::lookupTransformSafe(tf_buffer_, + base_link_frame_id_, + msg->header.frame_id, + msg->header.stamp, + transform_timeout_, + target_frame_trans, + tf_silent_failure_); + + if (can_transform) + { + double roll_offset = 0; + double pitch_offset = 0; + double yaw_offset = 0; + double roll = 0; + double pitch = 0; + double yaw = 0; + RosFilterUtilities::quatToRPY(target_frame_trans.getRotation(), roll_offset, pitch_offset, yaw_offset); + RosFilterUtilities::quatToRPY(transform_orientation_, roll, pitch, yaw); + + ROS_DEBUG_STREAM("Initial orientation is " << transform_orientation_); + + // Apply the offset (making sure to bound them), and throw them in a vector + tf2::Vector3 rpy_angles(FilterUtilities::clampRotation(roll - roll_offset), + FilterUtilities::clampRotation(pitch - pitch_offset), + FilterUtilities::clampRotation(yaw - yaw_offset)); + + // Now we need to rotate the roll and pitch by the yaw offset value. + // Imagine a case where an IMU is mounted facing sideways. In that case + // pitch for the IMU's world frame is roll for the robot. + tf2::Matrix3x3 mat; + mat.setRPY(0.0, 0.0, yaw_offset); + rpy_angles = mat * rpy_angles; + transform_orientation_.setRPY(rpy_angles.getX(), rpy_angles.getY(), rpy_angles.getZ()); + + ROS_DEBUG_STREAM("Initial corrected orientation roll, pitch, yaw is (" << + rpy_angles.getX() << ", " << rpy_angles.getY() << ", " << rpy_angles.getZ() << ")"); + + has_transform_imu_ = true; + } + } + } + + void NavSatTransform::odomCallback(const nav_msgs::OdometryConstPtr& msg) + { + world_frame_id_ = msg->header.frame_id; + base_link_frame_id_ = msg->child_frame_id; + + if (!transform_good_ && !use_manual_datum_) + { + setTransformOdometry(msg); + } + + tf2::fromMsg(msg->pose.pose, latest_world_pose_); + latest_odom_covariance_.setZero(); + for (size_t row = 0; row < POSE_SIZE; ++row) + { + for (size_t col = 0; col < POSE_SIZE; ++col) + { + latest_odom_covariance_(row, col) = msg->pose.covariance[row * POSE_SIZE + col]; + } + } + + odom_update_time_ = msg->header.stamp; + odom_updated_ = true; + } + + + bool NavSatTransform::prepareFilteredGps(sensor_msgs::NavSatFix &filtered_gps) + { + bool new_data = false; + + if (transform_good_ && odom_updated_) + { + mapToLL(latest_world_pose_.getOrigin(), filtered_gps.latitude, filtered_gps.longitude, filtered_gps.altitude); + + // Rotate the covariance as well + tf2::Matrix3x3 rot(cartesian_world_trans_inverse_.getRotation()); + Eigen::MatrixXd rot_6d(POSE_SIZE, POSE_SIZE); + rot_6d.setIdentity(); + + for (size_t rInd = 0; rInd < POSITION_SIZE; ++rInd) + { + rot_6d(rInd, 0) = rot.getRow(rInd).getX(); + rot_6d(rInd, 1) = rot.getRow(rInd).getY(); + rot_6d(rInd, 2) = rot.getRow(rInd).getZ(); + rot_6d(rInd+POSITION_SIZE, 3) = rot.getRow(rInd).getX(); + rot_6d(rInd+POSITION_SIZE, 4) = rot.getRow(rInd).getY(); + rot_6d(rInd+POSITION_SIZE, 5) = rot.getRow(rInd).getZ(); + } + + // Rotate the covariance + latest_odom_covariance_ = rot_6d * latest_odom_covariance_.eval() * rot_6d.transpose(); + + // Copy the measurement's covariance matrix back + for (size_t i = 0; i < POSITION_SIZE; i++) + { + for (size_t j = 0; j < POSITION_SIZE; j++) + { + filtered_gps.position_covariance[POSITION_SIZE * i + j] = latest_odom_covariance_(i, j); + } + } + + filtered_gps.position_covariance_type = sensor_msgs::NavSatFix::COVARIANCE_TYPE_KNOWN; + filtered_gps.status.status = sensor_msgs::NavSatStatus::STATUS_GBAS_FIX; + filtered_gps.header.frame_id = base_link_frame_id_; + filtered_gps.header.stamp = odom_update_time_; + + // Mark this GPS as used + odom_updated_ = false; + new_data = true; + } + + return new_data; + } + + bool NavSatTransform::prepareGpsOdometry(nav_msgs::Odometry &gps_odom) + { + bool new_data = false; + + if (transform_good_ && gps_updated_ && odom_updated_) + { + gps_odom = cartesianToMap(latest_cartesian_pose_); + + tf2::Transform transformed_cartesian_gps; + tf2::fromMsg(gps_odom.pose.pose, transformed_cartesian_gps); + + // Want the pose of the vehicle origin, not the GPS + tf2::Transform transformed_cartesian_robot; + getRobotOriginWorldPose(transformed_cartesian_gps, transformed_cartesian_robot, gps_odom.header.stamp); + + // Rotate the covariance as well + tf2::Matrix3x3 rot(cartesian_world_transform_.getRotation()); + Eigen::MatrixXd rot_6d(POSE_SIZE, POSE_SIZE); + rot_6d.setIdentity(); + + for (size_t rInd = 0; rInd < POSITION_SIZE; ++rInd) + { + rot_6d(rInd, 0) = rot.getRow(rInd).getX(); + rot_6d(rInd, 1) = rot.getRow(rInd).getY(); + rot_6d(rInd, 2) = rot.getRow(rInd).getZ(); + rot_6d(rInd+POSITION_SIZE, 3) = rot.getRow(rInd).getX(); + rot_6d(rInd+POSITION_SIZE, 4) = rot.getRow(rInd).getY(); + rot_6d(rInd+POSITION_SIZE, 5) = rot.getRow(rInd).getZ(); + } + + // Rotate the covariance + latest_cartesian_covariance_ = rot_6d * latest_cartesian_covariance_.eval() * rot_6d.transpose(); + + // Now fill out the message. Set the orientation to the identity. + tf2::toMsg(transformed_cartesian_robot, gps_odom.pose.pose); + gps_odom.pose.pose.position.z = (zero_altitude_ ? 0.0 : gps_odom.pose.pose.position.z); + + // Copy the measurement's covariance matrix so that we can rotate it later + for (size_t i = 0; i < POSE_SIZE; i++) + { + for (size_t j = 0; j < POSE_SIZE; j++) + { + gps_odom.pose.covariance[POSE_SIZE * i + j] = latest_cartesian_covariance_(i, j); + } + } + + // Mark this GPS as used + gps_updated_ = false; + new_data = true; + } + + return new_data; + } + + void NavSatTransform::setTransformGps(const sensor_msgs::NavSatFixConstPtr& msg) + { + double cartesian_x = 0; + double cartesian_y = 0; + double cartesian_z = 0; + if (use_local_cartesian_) + { + const double hae_altitude = 0.0; + gps_local_cartesian_.Reset(msg->latitude, msg->longitude, hae_altitude); + gps_local_cartesian_.Forward(msg->latitude, msg->longitude, msg->altitude, cartesian_x, cartesian_y, cartesian_z); + + // UTM meridian convergence is not meaningful when using local cartesian, so set it to 0.0 + utm_meridian_convergence_ = 0.0; + } + else + { + double k_tmp; + double utm_meridian_convergence_degrees; + GeographicLib::UTMUPS::Forward(msg->latitude, msg->longitude, utm_zone_, northp_, + cartesian_x, cartesian_y, utm_meridian_convergence_degrees, k_tmp); + utm_meridian_convergence_ = utm_meridian_convergence_degrees * NavsatConversions::RADIANS_PER_DEGREE; + } + + ROS_INFO_STREAM("Datum (latitude, longitude, altitude) is (" << std::fixed << msg->latitude << ", " << + msg->longitude << ", " << msg->altitude << ")"); + ROS_INFO_STREAM("Datum " << ((use_local_cartesian_)? "Local Cartesian" : "UTM") << + " coordinate is (" << std::fixed << cartesian_x << ", " << cartesian_y << ") zone " << utm_zone_); + + transform_cartesian_pose_.setOrigin(tf2::Vector3(cartesian_x, cartesian_y, msg->altitude)); + transform_cartesian_pose_.setRotation(tf2::Quaternion::getIdentity()); + has_transform_gps_ = true; + } + + void NavSatTransform::setTransformOdometry(const nav_msgs::OdometryConstPtr& msg) + { + tf2::fromMsg(msg->pose.pose, transform_world_pose_); + has_transform_odom_ = true; + + ROS_INFO_STREAM_ONCE("Initial odometry pose is " << transform_world_pose_); + + // Users can optionally use the (potentially fused) heading from + // the odometry source, which may have multiple fused sources of + // heading data, and so would act as a better heading for the + // Cartesian->world_frame transform. + if (!transform_good_ && use_odometry_yaw_ && !use_manual_datum_) + { + sensor_msgs::Imu *imu = new sensor_msgs::Imu(); + imu->orientation = msg->pose.pose.orientation; + imu->header.frame_id = msg->child_frame_id; + imu->header.stamp = msg->header.stamp; + sensor_msgs::ImuConstPtr imuPtr(imu); + imuCallback(imuPtr); + } + } + +} // namespace RobotLocalization diff --git a/Localizations/Packages/robot_localization/src/navsat_transform_node.cpp b/Localizations/Packages/robot_localization/src/navsat_transform_node.cpp new file mode 100644 index 0000000..16023fa --- /dev/null +++ b/Localizations/Packages/robot_localization/src/navsat_transform_node.cpp @@ -0,0 +1,51 @@ +/* + * Copyright (c) 2014, 2015, 2016, Charles River Analytics, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "robot_localization/navsat_transform.h" + +#include + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "navsat_transform_node"); + + ros::NodeHandle nh; + ros::NodeHandle nh_priv("~"); + + + RobotLocalization::NavSatTransform trans(nh, nh_priv); + ros::spin(); + + return EXIT_SUCCESS; +} + + diff --git a/Localizations/Packages/robot_localization/src/navsat_transform_nodelet.cpp b/Localizations/Packages/robot_localization/src/navsat_transform_nodelet.cpp new file mode 100644 index 0000000..994ddc1 --- /dev/null +++ b/Localizations/Packages/robot_localization/src/navsat_transform_nodelet.cpp @@ -0,0 +1,63 @@ +/* + * Copyright (c) 2017 Simon Gene Gottlieb + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "robot_localization/navsat_transform.h" + +#include +#include +#include + +#include + +namespace RobotLocalization +{ + +class NavSatTransformNodelet : public nodelet::Nodelet +{ +private: + std::unique_ptr trans; + +public: + virtual void onInit() + { + NODELET_DEBUG("Initializing nodelet..."); + + ros::NodeHandle nh = getNodeHandle(); + ros::NodeHandle nh_priv = getPrivateNodeHandle(); + + trans = std::make_unique(nh, nh_priv); + } +}; + +} // namespace RobotLocalization + +PLUGINLIB_EXPORT_CLASS(RobotLocalization::NavSatTransformNodelet, nodelet::Nodelet); diff --git a/Localizations/Packages/robot_localization/src/robot_localization_estimator.cpp b/Localizations/Packages/robot_localization/src/robot_localization_estimator.cpp new file mode 100644 index 0000000..9de64c3 --- /dev/null +++ b/Localizations/Packages/robot_localization/src/robot_localization_estimator.cpp @@ -0,0 +1,212 @@ +/* + * Copyright (c) 2016, TNO IVS Helmond. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "robot_localization/robot_localization_estimator.h" +#include "robot_localization/ekf.h" +#include "robot_localization/ukf.h" + +#include + +namespace RobotLocalization +{ +RobotLocalizationEstimator::RobotLocalizationEstimator(unsigned int buffer_capacity, + FilterType filter_type, + const Eigen::MatrixXd& process_noise_covariance, + const std::vector& filter_args) +{ + state_buffer_.set_capacity(buffer_capacity); + + // Set up the filter that is used for predictions + if ( filter_type == FilterTypes::EKF ) + { + filter_ = new Ekf; + } + else if ( filter_type == FilterTypes::UKF ) + { + filter_ = new Ukf(filter_args); + } + + filter_->setProcessNoiseCovariance(process_noise_covariance); +} + +RobotLocalizationEstimator::~RobotLocalizationEstimator() +{ + delete filter_; +} + +void RobotLocalizationEstimator::setState(const EstimatorState& state) +{ + // If newly received state is newer than any in the buffer, push back + if ( state_buffer_.empty() || state.time_stamp > state_buffer_.back().time_stamp ) + { + state_buffer_.push_back(state); + } + // If it is older, put it in the right position + else + { + for ( boost::circular_buffer::iterator it = state_buffer_.begin(); it != state_buffer_.end(); ++it ) + { + if ( state.time_stamp < it->time_stamp ) + { + state_buffer_.insert(it, state); + return; + } + } + } +} + +EstimatorResult RobotLocalizationEstimator::getState(const double time, + EstimatorState& state) const +{ + // If there's nothing in the buffer, there's nothing to give. + if ( state_buffer_.size() == 0 ) + { + return EstimatorResults::EmptyBuffer; + } + + // Set state to the most recent one for now + state = state_buffer_.back(); + + // Go through buffer from new to old + EstimatorState last_state_before_time = state_buffer_.front(); + EstimatorState next_state_after_time = state_buffer_.back(); + bool previous_state_found = false; + bool next_state_found = false; + + for (boost::circular_buffer::const_reverse_iterator it = state_buffer_.rbegin(); + it != state_buffer_.rend(); ++it) + { + /* If the time stamp of the current state from the buffer is + * older than the requested time, store it as the last state + * before the requested time. If it is younger, save it as the + * next one after, and go on to find the last one before. + */ + if ( it->time_stamp == time ) + { + state = *it; + return EstimatorResults::Exact; + } + else if ( it->time_stamp <= time ) + { + last_state_before_time = *it; + previous_state_found = true; + break; + } + else + { + next_state_after_time = *it; + next_state_found = true; + } + } + + // If we found a previous state and a next state, we can do interpolation + if ( previous_state_found && next_state_found ) + { + interpolate(last_state_before_time, next_state_after_time, time, state); + return EstimatorResults::Interpolation; + } + + // If only a previous state is found, we can do extrapolation into the future + else if ( previous_state_found ) + { + extrapolate(last_state_before_time, time, state); + return EstimatorResults::ExtrapolationIntoFuture; + } + + // If only a next state is found, we'll have to extrapolate into the past. + else if ( next_state_found ) + { + extrapolate(next_state_after_time, time, state); + return EstimatorResults::ExtrapolationIntoPast; + } + + else + { + return EstimatorResults::Failed; + } +} + +void RobotLocalizationEstimator::setBufferCapacity(const int capacity) +{ + state_buffer_.set_capacity(capacity); +} + +void RobotLocalizationEstimator::clearBuffer() +{ + state_buffer_.clear(); +} + +unsigned int RobotLocalizationEstimator::getBufferCapacity() const +{ + return state_buffer_.capacity(); +} + +unsigned int RobotLocalizationEstimator::getSize() const +{ + return state_buffer_.size(); +} + +void RobotLocalizationEstimator::extrapolate(const EstimatorState& boundary_state, + const double requested_time, + EstimatorState& state_at_req_time) const +{ + // Set up the filter with the boundary state + filter_->setState(boundary_state.state); + filter_->setEstimateErrorCovariance(boundary_state.covariance); + + // Calculate how much time we need to extrapolate into the future + double delta = requested_time - boundary_state.time_stamp; + + // Use the filter to predict + filter_->predict(boundary_state.time_stamp, delta); + + state_at_req_time.time_stamp = requested_time; + state_at_req_time.state = filter_->getState(); + state_at_req_time.covariance = filter_->getEstimateErrorCovariance(); + + return; +} + +void RobotLocalizationEstimator::interpolate(const EstimatorState& given_state_1, + const EstimatorState& given_state_2, + const double requested_time, + EstimatorState& state_at_req_time) const +{ + /* + * TODO: Right now, we only extrapolate from the last known state before the requested time. But as the state after + * the requested time is also known, we may want to perform interpolation between states. + */ + extrapolate(given_state_1, requested_time, state_at_req_time); + return; +} + +} // namespace RobotLocalization diff --git a/Localizations/Packages/robot_localization/src/robot_localization_listener_node.cpp b/Localizations/Packages/robot_localization/src/robot_localization_listener_node.cpp new file mode 100644 index 0000000..697070c --- /dev/null +++ b/Localizations/Packages/robot_localization/src/robot_localization_listener_node.cpp @@ -0,0 +1,98 @@ +/* + * Copyright (c) 2016, TNO IVS Helmond. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "robot_localization/ros_robot_localization_listener.h" +#include "robot_localization/GetState.h" + +#include + +namespace RobotLocalization +{ + +class RobotLocalizationListenerNode +{ +public: + RobotLocalizationListenerNode() + { + service_ = n_.advertiseService("get_state", &RobotLocalizationListenerNode::getStateCallback, this); + } + + std::string getService() + { + return service_.getService(); + } + +private: + RosRobotLocalizationListener rll_; + ros::NodeHandle n_; + ros::ServiceServer service_; + + bool getStateCallback(robot_localization::GetState::Request &req, + robot_localization::GetState::Response &res) + { + Eigen::VectorXd state(STATE_SIZE); + Eigen::MatrixXd covariance(STATE_SIZE, STATE_SIZE); + + if ( !rll_.getState(req.time_stamp, req.frame_id, state, covariance) ) + { + ROS_ERROR("Robot Localization Listener Node: Listener instance returned false at getState call."); + return false; + } + + for (size_t i = 0; i < STATE_SIZE; i++) + { + res.state[i] = (*(state.data() + i)); + } + + for (size_t i = 0; i < STATE_SIZE * STATE_SIZE; i++) + { + res.covariance[i] = (*(covariance.data() + i)); + } + + ROS_DEBUG("Robot Localization Listener Node: Listener responded with state and covariance at the requested time."); + return true; + } +}; + +} // namespace RobotLocalization + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "robot_localization_listener_node"); + + RobotLocalization::RobotLocalizationListenerNode rlln; + ROS_INFO_STREAM("Robot Localization Listener Node: Ready to handle GetState requests at " << rlln.getService()); + + ros::spin(); + + return 0; +} diff --git a/Localizations/Packages/robot_localization/src/ros_filter.cpp b/Localizations/Packages/robot_localization/src/ros_filter.cpp new file mode 100644 index 0000000..4cacacf --- /dev/null +++ b/Localizations/Packages/robot_localization/src/ros_filter.cpp @@ -0,0 +1,3361 @@ +/* + * Copyright (c) 2014, 2015, 2016, Charles River Analytics, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "robot_localization/ros_filter.h" +#include "robot_localization/filter_utilities.h" +#include "robot_localization/ekf.h" +#include "robot_localization/ukf.h" + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#if defined(_WIN32) && defined(ERROR) + #undef ERROR +#endif + +namespace RobotLocalization +{ + template + RosFilter::RosFilter(ros::NodeHandle nh, + ros::NodeHandle nh_priv, + std::string node_name, + std::vector args) : + disabledAtStartup_(false), + enabled_(false), + predictToCurrentTime_(false), + printDiagnostics_(true), + publishAcceleration_(false), + publishTransform_(true), + resetOnTimeJump_(false), + smoothLaggedData_(false), + toggledOn_(true), + twoDMode_(false), + useControl_(false), + dynamicDiagErrorLevel_(diagnostic_msgs::DiagnosticStatus::OK), + staticDiagErrorLevel_(diagnostic_msgs::DiagnosticStatus::OK), + frequency_(30.0), + gravitationalAcc_(9.80665), + historyLength_(0), + minFrequency_(frequency_ - 2.0), + maxFrequency_(frequency_ + 2.0), + baseLinkFrameId_("base_link"), + mapFrameId_("map"), + odomFrameId_("odom"), + worldFrameId_(odomFrameId_), + lastDiagTime_(0), + lastSetPoseTime_(0), + latestControlTime_(0), + tfTimeOffset_(ros::Duration(0)), + tfTimeout_(ros::Duration(0)), + filter_(args), + nh_(nh), + nhLocal_(nh_priv), + diagnosticUpdater_(nh, nh_priv, node_name), + tfListener_(tfBuffer_) + { + stateVariableNames_.push_back("X"); + stateVariableNames_.push_back("Y"); + stateVariableNames_.push_back("Z"); + stateVariableNames_.push_back("ROLL"); + stateVariableNames_.push_back("PITCH"); + stateVariableNames_.push_back("YAW"); + stateVariableNames_.push_back("X_VELOCITY"); + stateVariableNames_.push_back("Y_VELOCITY"); + stateVariableNames_.push_back("Z_VELOCITY"); + stateVariableNames_.push_back("ROLL_VELOCITY"); + stateVariableNames_.push_back("PITCH_VELOCITY"); + stateVariableNames_.push_back("YAW_VELOCITY"); + stateVariableNames_.push_back("X_ACCELERATION"); + stateVariableNames_.push_back("Y_ACCELERATION"); + stateVariableNames_.push_back("Z_ACCELERATION"); + + diagnosticUpdater_.setHardwareID("none"); + } + + template + RosFilter::RosFilter(ros::NodeHandle nh, ros::NodeHandle nh_priv, std::vector args) : + RosFilter::RosFilter(nh, nh_priv, ros::this_node::getName(), args) + {} + + template + RosFilter::~RosFilter() + { + topicSubs_.clear(); + } + + template + void RosFilter::initialize() + { + loadParams(); + + if (printDiagnostics_) + { + diagnosticUpdater_.add("Filter diagnostic updater", this, &RosFilter::aggregateDiagnostics); + } + + // Set up the frequency diagnostic + minFrequency_ = frequency_ - 2; + maxFrequency_ = frequency_ + 2; + freqDiag_ = std::make_unique( + "odometry/filtered", + diagnosticUpdater_, + diagnostic_updater::FrequencyStatusParam( + &minFrequency_, + &maxFrequency_, + 0.1, + 10)); + + // Publisher + positionPub_ = nh_.advertise("odometry/filtered", 20); + + // Optional acceleration publisher + if (publishAcceleration_) + { + accelPub_ = nh_.advertise("accel/filtered", 20); + } + + lastDiagTime_ = ros::Time::now(); + + periodicUpdateTimer_ = nh_.createTimer(ros::Duration(1./frequency_), &RosFilter::periodicUpdate, this); + } + + template + void RosFilter::reset() + { + // Get rid of any initial poses (pretend we've never had a measurement) + initialMeasurements_.clear(); + previousMeasurements_.clear(); + previousMeasurementCovariances_.clear(); + + clearMeasurementQueue(); + + filterStateHistory_.clear(); + measurementHistory_.clear(); + + // Also set the last set pose time, so we ignore all messages + // that occur before it + lastSetPoseTime_ = ros::Time(0); + + // clear tf buffer to avoid TF_OLD_DATA errors + tfBuffer_.clear(); + + // clear last message timestamp, so older messages will be accepted + lastMessageTimes_.clear(); + + // reset filter to uninitialized state + filter_.reset(); + + // clear all waiting callbacks + ros::getGlobalCallbackQueue()->clear(); + } + + template + bool RosFilter::toggleFilterProcessingCallback(robot_localization::ToggleFilterProcessing::Request& req, + robot_localization::ToggleFilterProcessing::Response& resp) + { + if (req.on == toggledOn_) + { + ROS_WARN_STREAM("Service was called to toggle filter processing but state was already as requested."); + resp.status = false; + } + else + { + ROS_INFO("Toggling filter measurement filtering to %s.", req.on ? "On" : "Off"); + toggledOn_ = req.on; + resp.status = true; + } + return true; + } + + // @todo: Replace with AccelWithCovarianceStamped + template + void RosFilter::accelerationCallback(const sensor_msgs::Imu::ConstPtr &msg, const CallbackData &callbackData, + const std::string &targetFrame) + { + // If we've just reset the filter, then we want to ignore any messages + // that arrive with an older timestamp + if (msg->header.stamp <= lastSetPoseTime_) + { + return; + } + + const std::string &topicName = callbackData.topicName_; + + RF_DEBUG("------ RosFilter::accelerationCallback (" << topicName << ") ------\n" + "Twist message:\n" << *msg); + + if (lastMessageTimes_.count(topicName) == 0) + { + lastMessageTimes_.insert(std::pair(topicName, msg->header.stamp)); + } + + // Make sure this message is newer than the last one + if (msg->header.stamp >= lastMessageTimes_[topicName]) + { + RF_DEBUG("Update vector for " << topicName << " is:\n" << topicName); + + Eigen::VectorXd measurement(STATE_SIZE); + Eigen::MatrixXd measurementCovariance(STATE_SIZE, STATE_SIZE); + + measurement.setZero(); + measurementCovariance.setZero(); + + // Make sure we're actually updating at least one of these variables + std::vector updateVectorCorrected = callbackData.updateVector_; + + // Prepare the twist data for inclusion in the filter + if (prepareAcceleration(msg, topicName, targetFrame, callbackData.relative_, updateVectorCorrected, measurement, + measurementCovariance)) + { + // Store the measurement. Add an "acceleration" suffix so we know what kind of measurement + // we're dealing with when we debug the core filter logic. + enqueueMeasurement(topicName, + measurement, + measurementCovariance, + updateVectorCorrected, + callbackData.rejectionThreshold_, + msg->header.stamp); + + RF_DEBUG("Enqueued new measurement for " << topicName << "_acceleration\n"); + } + else + { + RF_DEBUG("Did *not* enqueue measurement for " << topicName << "_acceleration\n"); + } + + lastMessageTimes_[topicName] = msg->header.stamp; + + RF_DEBUG("Last message time for " << topicName << " is now " << + lastMessageTimes_[topicName] << "\n"); + } + else if (resetOnTimeJump_ && ros::Time::isSimTime()) + { + reset(); + } + else + { + std::stringstream stream; + stream << "The " << topicName << " message has a timestamp before that of the previous message received," << + " this message will be ignored. This may indicate a bad timestamp. (message time: " << + msg->header.stamp.toSec() << ")"; + addDiagnostic(diagnostic_msgs::DiagnosticStatus::WARN, + topicName + "_timestamp", + stream.str(), + false); + + RF_DEBUG("Message is too old. Last message time for " << topicName << + " is " << lastMessageTimes_[topicName] << ", current message time is " << + msg->header.stamp << ".\n"); + } + + RF_DEBUG("\n----- /RosFilter::accelerationCallback (" << topicName << ") ------\n"); + } + + template + void RosFilter::controlCallback(const geometry_msgs::Twist::ConstPtr &msg) + { + geometry_msgs::TwistStampedPtr twistStampedPtr = geometry_msgs::TwistStampedPtr(new geometry_msgs::TwistStamped()); + twistStampedPtr->twist = *msg; + twistStampedPtr->header.frame_id = baseLinkFrameId_; + twistStampedPtr->header.stamp = ros::Time::now(); + controlCallback(twistStampedPtr); + } + + template + void RosFilter::controlCallback(const geometry_msgs::TwistStamped::ConstPtr &msg) + { + if (msg->header.frame_id == baseLinkFrameId_ || msg->header.frame_id == "") + { + latestControl_(ControlMemberVx) = msg->twist.linear.x; + latestControl_(ControlMemberVy) = msg->twist.linear.y; + latestControl_(ControlMemberVz) = msg->twist.linear.z; + latestControl_(ControlMemberVroll) = msg->twist.angular.x; + latestControl_(ControlMemberVpitch) = msg->twist.angular.y; + latestControl_(ControlMemberVyaw) = msg->twist.angular.z; + latestControlTime_ = msg->header.stamp; + + // Update the filter with this control term + filter_.setControl(latestControl_, msg->header.stamp.toSec()); + } + else + { + ROS_WARN_STREAM_THROTTLE(5.0, "Commanded velocities must be given in the robot's body frame (" << + baseLinkFrameId_ << "). Message frame was " << msg->header.frame_id); + } + } + + template + void RosFilter::enqueueMeasurement(const std::string &topicName, + const Eigen::VectorXd &measurement, + const Eigen::MatrixXd &measurementCovariance, + const std::vector &updateVector, + const double mahalanobisThresh, + const ros::Time &time) + { + MeasurementPtr meas = MeasurementPtr(new Measurement()); + + meas->topicName_ = topicName; + meas->measurement_ = measurement; + meas->covariance_ = measurementCovariance; + meas->updateVector_ = updateVector; + meas->time_ = time.toSec(); + meas->mahalanobisThresh_ = mahalanobisThresh; + meas->latestControl_ = latestControl_; + meas->latestControlTime_ = latestControlTime_.toSec(); + measurementQueue_.push(meas); + } + + template + void RosFilter::forceTwoD(Eigen::VectorXd &measurement, + Eigen::MatrixXd &measurementCovariance, + std::vector &updateVector) + { + measurement(StateMemberZ) = 0.0; + measurement(StateMemberRoll) = 0.0; + measurement(StateMemberPitch) = 0.0; + measurement(StateMemberVz) = 0.0; + measurement(StateMemberVroll) = 0.0; + measurement(StateMemberVpitch) = 0.0; + measurement(StateMemberAz) = 0.0; + + measurementCovariance(StateMemberZ, StateMemberZ) = 1e-6; + measurementCovariance(StateMemberRoll, StateMemberRoll) = 1e-6; + measurementCovariance(StateMemberPitch, StateMemberPitch) = 1e-6; + measurementCovariance(StateMemberVz, StateMemberVz) = 1e-6; + measurementCovariance(StateMemberVroll, StateMemberVroll) = 1e-6; + measurementCovariance(StateMemberVpitch, StateMemberVpitch) = 1e-6; + measurementCovariance(StateMemberAz, StateMemberAz) = 1e-6; + + updateVector[StateMemberZ] = 1; + updateVector[StateMemberRoll] = 1; + updateVector[StateMemberPitch] = 1; + updateVector[StateMemberVz] = 1; + updateVector[StateMemberVroll] = 1; + updateVector[StateMemberVpitch] = 1; + updateVector[StateMemberAz] = 1; + } + + template + bool RosFilter::getFilteredOdometryMessage(nav_msgs::Odometry &message) + { + // If the filter has received a measurement at some point... + if (filter_.getInitializedStatus()) + { + // Grab our current state and covariance estimates + const Eigen::VectorXd &state = filter_.getState(); + const Eigen::MatrixXd &estimateErrorCovariance = filter_.getEstimateErrorCovariance(); + + // Convert from roll, pitch, and yaw back to quaternion for + // orientation values + tf2::Quaternion quat; + quat.setRPY(state(StateMemberRoll), state(StateMemberPitch), state(StateMemberYaw)); + + // Fill out the message + message.pose.pose.position.x = state(StateMemberX); + message.pose.pose.position.y = state(StateMemberY); + message.pose.pose.position.z = state(StateMemberZ); + message.pose.pose.orientation.x = quat.x(); + message.pose.pose.orientation.y = quat.y(); + message.pose.pose.orientation.z = quat.z(); + message.pose.pose.orientation.w = quat.w(); + message.twist.twist.linear.x = state(StateMemberVx); + message.twist.twist.linear.y = state(StateMemberVy); + message.twist.twist.linear.z = state(StateMemberVz); + message.twist.twist.angular.x = state(StateMemberVroll); + message.twist.twist.angular.y = state(StateMemberVpitch); + message.twist.twist.angular.z = state(StateMemberVyaw); + + // Our covariance matrix layout doesn't quite match + for (size_t i = 0; i < POSE_SIZE; i++) + { + for (size_t j = 0; j < POSE_SIZE; j++) + { + message.pose.covariance[POSE_SIZE * i + j] = estimateErrorCovariance(i, j); + } + } + + // POSE_SIZE and TWIST_SIZE are currently the same size, but we can spare a few + // cycles to be meticulous and not index a twist covariance array on the size of + // a pose covariance array + for (size_t i = 0; i < TWIST_SIZE; i++) + { + for (size_t j = 0; j < TWIST_SIZE; j++) + { + message.twist.covariance[TWIST_SIZE * i + j] = + estimateErrorCovariance(i + POSITION_V_OFFSET, j + POSITION_V_OFFSET); + } + } + + message.header.stamp = ros::Time(filter_.getLastMeasurementTime()); + message.header.frame_id = worldFrameId_; + message.child_frame_id = baseLinkOutputFrameId_; + } + + return filter_.getInitializedStatus(); + } + + template + bool RosFilter::getFilteredAccelMessage(geometry_msgs::AccelWithCovarianceStamped &message) + { + // If the filter has received a measurement at some point... + if (filter_.getInitializedStatus()) + { + // Grab our current state and covariance estimates + const Eigen::VectorXd &state = filter_.getState(); + const Eigen::MatrixXd &estimateErrorCovariance = filter_.getEstimateErrorCovariance(); + + //! Fill out the accel_msg + message.accel.accel.linear.x = state(StateMemberAx); + message.accel.accel.linear.y = state(StateMemberAy); + message.accel.accel.linear.z = state(StateMemberAz); + message.accel.accel.angular.x = angular_acceleration_.x(); + message.accel.accel.angular.y = angular_acceleration_.y(); + message.accel.accel.angular.z = angular_acceleration_.z(); + // Fill the covariance (only the left-upper matrix since we are not estimating + // the rotational accelerations arround the axes + for (size_t i = 0; i < ACCELERATION_SIZE; i++) + { + for (size_t j = 0; j < ACCELERATION_SIZE; j++) + { + // We use the POSE_SIZE since the accel cov matrix of ROS is 6x6 + message.accel.covariance[POSE_SIZE * i + j] = + estimateErrorCovariance(i + POSITION_A_OFFSET, j + POSITION_A_OFFSET); + } + } + for (size_t i = ACCELERATION_SIZE; i < POSE_SIZE; i++) + { + for (size_t j = ACCELERATION_SIZE; j < POSE_SIZE; j++) + { + // fill out the angular portion. We assume the linear and angular portions are independent. + message.accel.covariance[POSE_SIZE * i + j] = + angular_acceleration_cov_(i - ACCELERATION_SIZE, j - ACCELERATION_SIZE); + } + } + + // Fill header information + message.header.stamp = ros::Time(filter_.getLastMeasurementTime()); + message.header.frame_id = baseLinkOutputFrameId_; + } + + return filter_.getInitializedStatus(); + } + + template + void RosFilter::imuCallback(const sensor_msgs::Imu::ConstPtr &msg, + const std::string &topicName, + const CallbackData &poseCallbackData, + const CallbackData &twistCallbackData, + const CallbackData &accelCallbackData) + { + RF_DEBUG("------ RosFilter::imuCallback (" << topicName << ") ------\n" << "IMU message:\n" << *msg); + + // If we've just reset the filter, then we want to ignore any messages + // that arrive with an older timestamp + if (msg->header.stamp <= lastSetPoseTime_) + { + std::stringstream stream; + stream << "The " << topicName << " message has a timestamp equal to or before the last filter reset, " << + "this message will be ignored. This may indicate an empty or bad timestamp. (message time: " << + msg->header.stamp.toSec() << ")"; + addDiagnostic(diagnostic_msgs::DiagnosticStatus::WARN, + topicName + "_timestamp", + stream.str(), + false); + RF_DEBUG("Received message that preceded the most recent pose reset. Ignoring..."); + + return; + } + + // As with the odometry message, we can separate out the pose- and twist-related variables + // in the IMU message and pass them to the pose and twist callbacks (filters) + if (poseCallbackData.updateSum_ > 0) + { + // Per the IMU message specification, if the IMU does not provide orientation, + // then its first covariance value should be set to -1, and we should ignore + // that portion of the message. robot_localization allows users to explicitly + // ignore data using its parameters, but we should also be compliant with + // message specs. + if (::fabs(msg->orientation_covariance[0] + 1) < 1e-9) + { + RF_DEBUG("Received IMU message with -1 as its first covariance value for orientation. " + "Ignoring orientation..."); + } + else + { + // Extract the pose (orientation) data, pass it to its filter + geometry_msgs::PoseWithCovarianceStamped *posPtr = new geometry_msgs::PoseWithCovarianceStamped(); + posPtr->header = msg->header; + posPtr->pose.pose.orientation = msg->orientation; + + // Copy the covariance for roll, pitch, and yaw + for (size_t i = 0; i < ORIENTATION_SIZE; i++) + { + for (size_t j = 0; j < ORIENTATION_SIZE; j++) + { + posPtr->pose.covariance[POSE_SIZE * (i + ORIENTATION_SIZE) + (j + ORIENTATION_SIZE)] = + msg->orientation_covariance[ORIENTATION_SIZE * i + j]; + } + } + + // IMU data gets handled a bit differently, since the message is ambiguous and has only a single frame_id, + // even though the data in it is reported in two different frames. As we assume users will specify a base_link + // to imu transform, we make the target and child frame baseLinkFrameId_ and tell the poseCallback that it is + // working with IMU data. This will cause it to apply different logic to the data. + geometry_msgs::PoseWithCovarianceStampedConstPtr pptr(posPtr); + poseCallback(pptr, poseCallbackData, baseLinkFrameId_, + baseLinkFrameId_, true); + } + } + + if (twistCallbackData.updateSum_ > 0) + { + // Ignore rotational velocity if the first covariance value is -1 + if (::fabs(msg->angular_velocity_covariance[0] + 1) < 1e-9) + { + RF_DEBUG("Received IMU message with -1 as its first covariance value for angular " + "velocity. Ignoring angular velocity..."); + } + else + { + // Repeat for velocity + geometry_msgs::TwistWithCovarianceStamped *twistPtr = new geometry_msgs::TwistWithCovarianceStamped(); + twistPtr->header = msg->header; + twistPtr->twist.twist.angular = msg->angular_velocity; + + // Copy the covariance + for (size_t i = 0; i < ORIENTATION_SIZE; i++) + { + for (size_t j = 0; j < ORIENTATION_SIZE; j++) + { + twistPtr->twist.covariance[TWIST_SIZE * (i + ORIENTATION_SIZE) + (j + ORIENTATION_SIZE)] = + msg->angular_velocity_covariance[ORIENTATION_SIZE * i + j]; + } + } + + geometry_msgs::TwistWithCovarianceStampedConstPtr tptr(twistPtr); + twistCallback(tptr, twistCallbackData, baseLinkFrameId_); + } + } + + if (accelCallbackData.updateSum_ > 0) + { + // Ignore linear acceleration if the first covariance value is -1 + if (::fabs(msg->linear_acceleration_covariance[0] + 1) < 1e-9) + { + RF_DEBUG("Received IMU message with -1 as its first covariance value for linear " + "acceleration. Ignoring linear acceleration..."); + } + else + { + // Pass the message on + accelerationCallback(msg, accelCallbackData, baseLinkFrameId_); + } + } + + RF_DEBUG("\n----- /RosFilter::imuCallback (" << topicName << ") ------\n"); + } + + template + void RosFilter::integrateMeasurements(const ros::Time ¤tTime) + { + const double currentTimeSec = currentTime.toSec(); + + RF_DEBUG("------ RosFilter::integrateMeasurements ------\n\n" + "Integration time is " << std::setprecision(20) << currentTimeSec << "\n" + << measurementQueue_.size() << " measurements in queue.\n"); + + bool predictToCurrentTime = predictToCurrentTime_; + + // If we have any measurements in the queue, process them + if (!measurementQueue_.empty()) + { + // Check if the first measurement we're going to process is older than the filter's last measurement. + // This means we have received an out-of-sequence message (one with an old timestamp), and we need to + // revert both the filter state and measurement queue to the first state that preceded the time stamp + // of our first measurement. + const MeasurementPtr& firstMeasurement = measurementQueue_.top(); + int restoredMeasurementCount = 0; + if (smoothLaggedData_ && firstMeasurement->time_ < filter_.getLastMeasurementTime()) + { + RF_DEBUG("Received a measurement that was " << filter_.getLastMeasurementTime() - firstMeasurement->time_ << + " seconds in the past. Reverting filter state and measurement queue..."); + + int originalCount = static_cast(measurementQueue_.size()); + const double firstMeasurementTime = firstMeasurement->time_; + const std::string firstMeasurementTopic = firstMeasurement->topicName_; + if (!revertTo(firstMeasurementTime - 1e-9)) // revertTo may invalidate firstMeasurement + { + RF_DEBUG("ERROR: history interval is too small to revert to time " << firstMeasurementTime << "\n"); + ROS_WARN_STREAM_DELAYED_THROTTLE(historyLength_, "Received old measurement for topic " << + firstMeasurementTopic << ", but history interval is insufficiently sized. Measurement time is " << + std::setprecision(20) << firstMeasurementTime << ", current time is " << currentTime.toSec() << + ", history length is " << historyLength_ << "."); + restoredMeasurementCount = 0; + } + + restoredMeasurementCount = static_cast(measurementQueue_.size()) - originalCount; + } + + while (!measurementQueue_.empty() && ros::ok()) + { + MeasurementPtr measurement = measurementQueue_.top(); + + // If we've reached a measurement that has a time later than now, it should wait until a future iteration. + // Since measurements are stored in a priority queue, all remaining measurements will be in the future. + if (measurement->time_ > currentTime.toSec()) + { + break; + } + + measurementQueue_.pop(); + + // When we receive control messages, we call this directly in the control callback. However, we also associate + // a control with each sensor message so that we can support lagged smoothing. As we cannot guarantee that the + // new control callback will fire before a new measurement, we should only perform this operation if we are + // processing messages from the history. Otherwise, we may get a new measurement, store the "old" latest + // control, then receive a control, call setControl, and then overwrite that value with this one (i.e., with + // the "old" control we associated with the measurement). + if (useControl_ && restoredMeasurementCount > 0) + { + filter_.setControl(measurement->latestControl_, measurement->latestControlTime_); + restoredMeasurementCount--; + } + + // This will call predict and, if necessary, correct + filter_.processMeasurement(*(measurement.get())); + + // Store old states and measurements if we're smoothing + if (smoothLaggedData_) + { + // Invariant still holds: measurementHistoryDeque_.back().time_ < measurementQueue_.top().time_ + measurementHistory_.push_back(measurement); + + // We should only save the filter state once per unique timstamp + if (measurementQueue_.empty() || + ::fabs(measurementQueue_.top()->time_ - filter_.getLastMeasurementTime()) > 1e-9) + { + saveFilterState(filter_); + } + } + } + } + else if (filter_.getInitializedStatus()) + { + // In the event that we don't get any measurements for a long time, + // we still need to continue to estimate our state. Therefore, we + // should project the state forward here. + double lastUpdateDelta = currentTimeSec - filter_.getLastMeasurementTime(); + + // If we get a large delta, then continuously predict until + if (lastUpdateDelta >= filter_.getSensorTimeout()) + { + predictToCurrentTime = true; + + RF_DEBUG("Sensor timeout! Last measurement time was " << filter_.getLastMeasurementTime() << + ", current time is " << currentTimeSec << + ", delta is " << lastUpdateDelta << "\n"); + } + } + else + { + RF_DEBUG("Filter not yet initialized.\n"); + } + + if (filter_.getInitializedStatus() && predictToCurrentTime) + { + double lastUpdateDelta = currentTimeSec - filter_.getLastMeasurementTime(); + + filter_.validateDelta(lastUpdateDelta); + filter_.predict(currentTimeSec, lastUpdateDelta); + + // Update the last measurement time and last update time + filter_.setLastMeasurementTime(filter_.getLastMeasurementTime() + lastUpdateDelta); + } + + RF_DEBUG("\n----- /RosFilter::integrateMeasurements ------\n"); + } + + template + void RosFilter::differentiateMeasurements(const ros::Time ¤tTime) + { + if (filter_.getInitializedStatus()) + { + const double dt = (currentTime - lastDiffTime_).toSec(); + const Eigen::VectorXd &state = filter_.getState(); + // Specific to angular acceleration for now... + tf2::Vector3 newStateTwistRot(state(StateMemberVroll), + state(StateMemberVpitch), + state(StateMemberVyaw)); + angular_acceleration_ = (newStateTwistRot - lastStateTwistRot_)/dt; + const Eigen::MatrixXd &cov = filter_.getEstimateErrorCovariance(); + for (size_t i = 0; i < 3; i ++) + { + for (size_t j = 0; j < 3; j ++) + { + angular_acceleration_cov_(i, j) = cov(i+ORIENTATION_V_OFFSET, j+ORIENTATION_V_OFFSET) + * 2. / (dt * dt); + } + } + lastStateTwistRot_ = newStateTwistRot; + lastDiffTime_ = currentTime; + } + } + + template + void RosFilter::loadParams() + { + /* For diagnostic purposes, collect information about how many different + * sources are measuring each absolute pose variable and do not have + * differential integration enabled. + */ + std::map absPoseVarCounts; + absPoseVarCounts[StateMemberX] = 0; + absPoseVarCounts[StateMemberY] = 0; + absPoseVarCounts[StateMemberZ] = 0; + absPoseVarCounts[StateMemberRoll] = 0; + absPoseVarCounts[StateMemberPitch] = 0; + absPoseVarCounts[StateMemberYaw] = 0; + + // Same for twist variables + std::map twistVarCounts; + twistVarCounts[StateMemberVx] = 0; + twistVarCounts[StateMemberVy] = 0; + twistVarCounts[StateMemberVz] = 0; + twistVarCounts[StateMemberVroll] = 0; + twistVarCounts[StateMemberVpitch] = 0; + twistVarCounts[StateMemberVyaw] = 0; + + // Determine if we'll be printing diagnostic information + nhLocal_.param("print_diagnostics", printDiagnostics_, true); + + // Check for custom gravitational acceleration value + nhLocal_.param("gravitational_acceleration", gravitationalAcc_, 9.80665); + + // Grab the debug param. If true, the node will produce a LOT of output. + bool debug; + nhLocal_.param("debug", debug, false); + + if (debug) + { + std::string debugOutFile; + + try + { + nhLocal_.param("debug_out_file", debugOutFile, std::string("robot_localization_debug.txt")); + debugStream_.open(debugOutFile.c_str()); + + // Make sure we succeeded + if (debugStream_.is_open()) + { + filter_.setDebug(debug, &debugStream_); + } + else + { + ROS_WARN_STREAM("RosFilter::loadParams() - unable to create debug output file " << debugOutFile); + } + } + catch(const std::exception &e) + { + ROS_WARN_STREAM("RosFilter::loadParams() - unable to create debug output file" << debugOutFile + << ". Error was " << e.what() << "\n"); + } + } + + // These params specify the name of the robot's body frame (typically + // base_link) and odometry frame (typically odom) + nhLocal_.param("map_frame", mapFrameId_, std::string("map")); + nhLocal_.param("odom_frame", odomFrameId_, std::string("odom")); + nhLocal_.param("base_link_frame", baseLinkFrameId_, std::string("base_link")); + nhLocal_.param("base_link_frame_output", baseLinkOutputFrameId_, baseLinkFrameId_); + + /* + * These parameters are designed to enforce compliance with REP-105: + * http://www.ros.org/reps/rep-0105.html + * When fusing absolute position data from sensors such as GPS, the state + * estimate can undergo discrete jumps. According to REP-105, we want three + * coordinate frames: map, odom, and base_link. The map frame can have + * discontinuities, but is the frame with the most accurate position estimate + * for the robot and should not suffer from drift. The odom frame drifts over + * time, but is guaranteed to be continuous and is accurate enough for local + * planning and navigation. The base_link frame is affixed to the robot. The + * intention is that some odometry source broadcasts the odom->base_link + * transform. The localization software should broadcast map->base_link. + * However, tf does not allow multiple parents for a coordinate frame, so + * we must *compute* map->base_link, but then use the existing odom->base_link + * transform to compute *and broadcast* map->odom. + * + * The state estimation nodes in robot_localization therefore have two "modes." + * If your world_frame parameter value matches the odom_frame parameter value, + * then robot_localization will assume someone else is broadcasting a transform + * from odom_frame->base_link_frame, and it will compute the + * map_frame->odom_frame transform. Otherwise, it will simply compute the + * odom_frame->base_link_frame transform. + * + * The default is the latter behavior (broadcast of odom->base_link). + */ + nhLocal_.param("world_frame", worldFrameId_, odomFrameId_); + + ROS_FATAL_COND(mapFrameId_ == odomFrameId_ || + odomFrameId_ == baseLinkFrameId_ || + mapFrameId_ == baseLinkFrameId_ || + odomFrameId_ == baseLinkOutputFrameId_ || + mapFrameId_ == baseLinkOutputFrameId_, + "Invalid frame configuration! The values for map_frame, odom_frame, " + "and base_link_frame must be unique. If using a base_link_frame_output values, it " + "must not match the map_frame or odom_frame."); + + // Try to resolve tf_prefix + std::string tfPrefix = ""; + std::string tfPrefixPath = ""; + if (nhLocal_.searchParam("tf_prefix", tfPrefixPath)) + { + nhLocal_.getParam(tfPrefixPath, tfPrefix); + } + + // Append the tf prefix in a tf2-friendly manner + FilterUtilities::appendPrefix(tfPrefix, mapFrameId_); + FilterUtilities::appendPrefix(tfPrefix, odomFrameId_); + FilterUtilities::appendPrefix(tfPrefix, baseLinkFrameId_); + FilterUtilities::appendPrefix(tfPrefix, baseLinkOutputFrameId_); + FilterUtilities::appendPrefix(tfPrefix, worldFrameId_); + + // Whether we're publshing the world_frame->base_link_frame transform + nhLocal_.param("publish_tf", publishTransform_, true); + + // Whether we're publishing the acceleration state transform + nhLocal_.param("publish_acceleration", publishAcceleration_, false); + + // Whether we'll allow old measurements to cause a re-publication of the updated state + nhLocal_.param("permit_corrected_publication", permitCorrectedPublication_, false); + + // Transform future dating + double offsetTmp; + nhLocal_.param("transform_time_offset", offsetTmp, 0.0); + tfTimeOffset_.fromSec(offsetTmp); + + // Transform timeout + double timeoutTmp; + nhLocal_.param("transform_timeout", timeoutTmp, 0.0); + tfTimeout_.fromSec(timeoutTmp); + + // Update frequency and sensor timeout + double sensorTimeout; + nhLocal_.param("frequency", frequency_, 30.0); + nhLocal_.param("sensor_timeout", sensorTimeout, 1.0 / frequency_); + filter_.setSensorTimeout(sensorTimeout); + + // Determine if we're in 2D mode + nhLocal_.param("two_d_mode", twoDMode_, false); + + // Smoothing window size + nhLocal_.param("smooth_lagged_data", smoothLaggedData_, false); + nhLocal_.param("history_length", historyLength_, 0.0); + + // Wether we reset filter on jump back in time + nhLocal_.param("reset_on_time_jump", resetOnTimeJump_, false); + + if (!smoothLaggedData_ && ::fabs(historyLength_) > 1e-9) + { + ROS_WARN_STREAM("Filter history interval of " << historyLength_ << + " specified, but smooth_lagged_data is set to false. Lagged data will not be smoothed."); + } + + if (smoothLaggedData_ && historyLength_ < -1e9) + { + ROS_WARN_STREAM("Negative history interval of " << historyLength_ << + " specified. Absolute value will be assumed."); + } + + historyLength_ = ::fabs(historyLength_); + + nhLocal_.param("predict_to_current_time", predictToCurrentTime_, false); + + // Determine if we're using a control term + bool stampedControl = false; + double controlTimeout = sensorTimeout; + std::vector controlUpdateVector(TWIST_SIZE, 0); + std::vector accelerationLimits(TWIST_SIZE, 1.0); + std::vector accelerationGains(TWIST_SIZE, 1.0); + std::vector decelerationLimits(TWIST_SIZE, 1.0); + std::vector decelerationGains(TWIST_SIZE, 1.0); + + nhLocal_.param("use_control", useControl_, false); + nhLocal_.param("stamped_control", stampedControl, false); + nhLocal_.param("control_timeout", controlTimeout, sensorTimeout); + + if (useControl_) + { + if (nhLocal_.getParam("control_config", controlUpdateVector)) + { + if (controlUpdateVector.size() != TWIST_SIZE) + { + ROS_ERROR_STREAM("Control configuration must be of size " << TWIST_SIZE << ". Provided config was of " + "size " << controlUpdateVector.size() << ". No control term will be used."); + useControl_ = false; + } + } + else + { + ROS_ERROR_STREAM("use_control is set to true, but control_config is missing. No control term will be used."); + useControl_ = false; + } + + if (nhLocal_.getParam("acceleration_limits", accelerationLimits)) + { + if (accelerationLimits.size() != TWIST_SIZE) + { + ROS_ERROR_STREAM("Acceleration configuration must be of size " << TWIST_SIZE << ". Provided config was of " + "size " << accelerationLimits.size() << ". No control term will be used."); + useControl_ = false; + } + } + else + { + ROS_WARN_STREAM("use_control is set to true, but acceleration_limits is missing. Will use default values."); + } + + if (nhLocal_.getParam("acceleration_gains", accelerationGains)) + { + const int size = accelerationGains.size(); + if (size != TWIST_SIZE) + { + ROS_ERROR_STREAM("Acceleration gain configuration must be of size " << TWIST_SIZE << + ". Provided config was of size " << size << ". All gains will be assumed to be 1."); + std::fill_n(accelerationGains.begin(), std::min(size, TWIST_SIZE), 1.0); + accelerationGains.resize(TWIST_SIZE, 1.0); + } + } + + if (nhLocal_.getParam("deceleration_limits", decelerationLimits)) + { + if (decelerationLimits.size() != TWIST_SIZE) + { + ROS_ERROR_STREAM("Deceleration configuration must be of size " << TWIST_SIZE << + ". Provided config was of size " << decelerationLimits.size() << ". No control term will be used."); + useControl_ = false; + } + } + else + { + ROS_INFO_STREAM("use_control is set to true, but no deceleration_limits specified. Will use acceleration " + "limits."); + decelerationLimits = accelerationLimits; + } + + if (nhLocal_.getParam("deceleration_gains", decelerationGains)) + { + const int size = decelerationGains.size(); + if (size != TWIST_SIZE) + { + ROS_ERROR_STREAM("Deceleration gain configuration must be of size " << TWIST_SIZE << + ". Provided config was of size " << size << ". All gains will be assumed to be 1."); + std::fill_n(decelerationGains.begin(), std::min(size, TWIST_SIZE), 1.0); + decelerationGains.resize(TWIST_SIZE, 1.0); + } + } + else + { + ROS_INFO_STREAM("use_control is set to true, but no deceleration_gains specified. Will use acceleration " + "gains."); + decelerationGains = accelerationGains; + } + } + + bool dynamicProcessNoiseCovariance = false; + nhLocal_.param("dynamic_process_noise_covariance", dynamicProcessNoiseCovariance, false); + filter_.setUseDynamicProcessNoiseCovariance(dynamicProcessNoiseCovariance); + + std::vector initialState(STATE_SIZE, 0.0); + if (nhLocal_.getParam("initial_state", initialState)) + { + if (initialState.size() != STATE_SIZE) + { + ROS_ERROR_STREAM("Initial state must be of size " << STATE_SIZE << ". Provided config was of size " << + initialState.size() << ". The initial state will be ignored."); + } + else + { + Eigen::Map eigenState(initialState.data(), initialState.size()); + filter_.setState(eigenState); + } + } + + // Check if the filter should start or not + nhLocal_.param("disabled_at_startup", disabledAtStartup_, false); + enabled_ = !disabledAtStartup_; + + // Check if tf warnings should be suppressed + nh_.getParam("/silent_tf_failure", tfSilentFailure_); + + // Debugging writes to file + RF_DEBUG("tf_prefix is " << tfPrefix << + "\nmap_frame is " << mapFrameId_ << + "\nodom_frame is " << odomFrameId_ << + "\nbase_link_frame is " << baseLinkFrameId_ << + "\base_link_frame_output is " << baseLinkOutputFrameId_ << + "\nworld_frame is " << worldFrameId_ << + "\ntransform_time_offset is " << tfTimeOffset_.toSec() << + "\ntransform_timeout is " << tfTimeout_.toSec() << + "\nfrequency is " << frequency_ << + "\nsensor_timeout is " << filter_.getSensorTimeout() << + "\ntwo_d_mode is " << std::boolalpha << twoDMode_ << + "\nsmooth_lagged_data is " << std::boolalpha << smoothLaggedData_ << + "\nhistory_length is " << historyLength_ << + "\nuse_control is " << std::boolalpha << useControl_ << + "\nstamped_control is " << std::boolalpha << stampedControl << + "\ncontrol_config is " << controlUpdateVector << + "\ncontrol_timeout is " << controlTimeout << + "\nacceleration_limits are " << accelerationLimits << + "\nacceleration_gains are " << accelerationGains << + "\ndeceleration_limits are " << decelerationLimits << + "\ndeceleration_gains are " << decelerationGains << + "\ninitial state is " << filter_.getState() << + "\ndynamic_process_noise_covariance is " << std::boolalpha << dynamicProcessNoiseCovariance << + "\npermit_corrected_publication is " << std::boolalpha << permitCorrectedPublication_ << + "\nprint_diagnostics is " << std::boolalpha << printDiagnostics_ << + "\nsuppress tf warnings is " << std::boolalpha << tfSilentFailure_ << "\n" "\n"); + + // Create a subscriber for manually setting/resetting pose + setPoseSub_ = nh_.subscribe("set_pose", + 1, + &RosFilter::setPoseCallback, + this, ros::TransportHints().tcpNoDelay(false)); + + // Create a service for manually setting/resetting pose + setPoseSrv_ = nh_.advertiseService("set_pose", &RosFilter::setPoseSrvCallback, this); + + // Create a service for manually enabling the filter + enableFilterSrv_ = nhLocal_.advertiseService("enable", &RosFilter::enableFilterSrvCallback, this); + + // Create a service for toggling processing new measurements while still publishing + toggleFilterProcessingSrv_ = + nhLocal_.advertiseService("toggle", &RosFilter::toggleFilterProcessingCallback, this); + + // Init the last measurement time so we don't get a huge initial delta + filter_.setLastMeasurementTime(ros::Time::now().toSec()); + + // Now pull in each topic to which we want to subscribe. + // Start with odom. + size_t topicInd = 0; + bool moreParams = false; + do + { + // Build the string in the form of "odomX", where X is the odom topic number, + // then check if we have any parameters with that value. Users need to make + // sure they don't have gaps in their configs (e.g., odom0 and then odom2) + std::stringstream ss; + ss << "odom" << topicInd++; + std::string odomTopicName = ss.str(); + moreParams = nhLocal_.hasParam(odomTopicName); + + if (moreParams) + { + // Determine if we want to integrate this sensor differentially + bool differential; + nhLocal_.param(odomTopicName + std::string("_differential"), differential, false); + + // Determine if we want to integrate this sensor relatively + bool relative; + nhLocal_.param(odomTopicName + std::string("_relative"), relative, false); + + if (relative && differential) + { + ROS_WARN_STREAM("Both " << odomTopicName << "_differential" << " and " << odomTopicName << + "_relative were set to true. Using differential mode."); + + relative = false; + } + + // Consider odometry transformation from the child_frame_id instead of the base_link_frame_id + bool pose_use_child_frame; + nhLocal_.param(odomTopicName + std::string("_pose_use_child_frame"), pose_use_child_frame, false); + + std::string odomTopic; + nhLocal_.getParam(odomTopicName, odomTopic); + + // Check for pose rejection threshold + double poseMahalanobisThresh; + nhLocal_.param(odomTopicName + std::string("_pose_rejection_threshold"), + poseMahalanobisThresh, + std::numeric_limits::max()); + + // Check for twist rejection threshold + double twistMahalanobisThresh; + nhLocal_.param(odomTopicName + std::string("_twist_rejection_threshold"), + twistMahalanobisThresh, + std::numeric_limits::max()); + + // Now pull in its boolean update vector configuration. Create separate vectors for pose + // and twist data, and then zero out the opposite values in each vector (no pose data in + // the twist update vector and vice-versa). + std::vector updateVec = loadUpdateConfig(odomTopicName); + std::vector poseUpdateVec = updateVec; + std::fill(poseUpdateVec.begin() + POSITION_V_OFFSET, poseUpdateVec.begin() + POSITION_V_OFFSET + TWIST_SIZE, 0); + std::vector twistUpdateVec = updateVec; + std::fill(twistUpdateVec.begin() + POSITION_OFFSET, twistUpdateVec.begin() + POSITION_OFFSET + POSE_SIZE, 0); + + int poseUpdateSum = std::accumulate(poseUpdateVec.begin(), poseUpdateVec.end(), 0); + int twistUpdateSum = std::accumulate(twistUpdateVec.begin(), twistUpdateVec.end(), 0); + int odomQueueSize = 1; + nhLocal_.param(odomTopicName + "_queue_size", odomQueueSize, 1); + + const CallbackData poseCallbackData(odomTopicName + "_pose", poseUpdateVec, poseUpdateSum, differential, + relative, pose_use_child_frame, poseMahalanobisThresh); + const CallbackData twistCallbackData(odomTopicName + "_twist", twistUpdateVec, twistUpdateSum, false, false, + false, twistMahalanobisThresh); + + bool nodelayOdom = false; + nhLocal_.param(odomTopicName + "_nodelay", nodelayOdom, false); + + // Store the odometry topic subscribers so they don't go out of scope. + if (poseUpdateSum + twistUpdateSum > 0) + { + topicSubs_.push_back( + nh_.subscribe(odomTopic, odomQueueSize, + boost::bind(&RosFilter::odometryCallback, this, boost::placeholders::_1, + odomTopicName, poseCallbackData, twistCallbackData), + ros::VoidPtr(), ros::TransportHints().tcpNoDelay(nodelayOdom))); + } + else + { + std::stringstream stream; + stream << odomTopic << " is listed as an input topic, but all update variables are false"; + + addDiagnostic(diagnostic_msgs::DiagnosticStatus::WARN, + odomTopic + "_configuration", + stream.str(), + true); + } + + if (poseUpdateSum > 0) + { + if (differential) + { + twistVarCounts[StateMemberVx] += poseUpdateVec[StateMemberX]; + twistVarCounts[StateMemberVy] += poseUpdateVec[StateMemberY]; + twistVarCounts[StateMemberVz] += poseUpdateVec[StateMemberZ]; + twistVarCounts[StateMemberVroll] += poseUpdateVec[StateMemberRoll]; + twistVarCounts[StateMemberVpitch] += poseUpdateVec[StateMemberPitch]; + twistVarCounts[StateMemberVyaw] += poseUpdateVec[StateMemberYaw]; + } + else + { + absPoseVarCounts[StateMemberX] += poseUpdateVec[StateMemberX]; + absPoseVarCounts[StateMemberY] += poseUpdateVec[StateMemberY]; + absPoseVarCounts[StateMemberZ] += poseUpdateVec[StateMemberZ]; + absPoseVarCounts[StateMemberRoll] += poseUpdateVec[StateMemberRoll]; + absPoseVarCounts[StateMemberPitch] += poseUpdateVec[StateMemberPitch]; + absPoseVarCounts[StateMemberYaw] += poseUpdateVec[StateMemberYaw]; + } + } + + if (twistUpdateSum > 0) + { + twistVarCounts[StateMemberVx] += twistUpdateVec[StateMemberVx]; + twistVarCounts[StateMemberVy] += twistUpdateVec[StateMemberVx]; + twistVarCounts[StateMemberVz] += twistUpdateVec[StateMemberVz]; + twistVarCounts[StateMemberVroll] += twistUpdateVec[StateMemberVroll]; + twistVarCounts[StateMemberVpitch] += twistUpdateVec[StateMemberVpitch]; + twistVarCounts[StateMemberVyaw] += twistUpdateVec[StateMemberVyaw]; + } + + RF_DEBUG("Subscribed to " << odomTopic << " (" << odomTopicName << ")\n\t" << + odomTopicName << "_differential is " << (differential ? "true" : "false") << "\n\t" << + odomTopicName << "_pose_rejection_threshold is " << poseMahalanobisThresh << "\n\t" << + odomTopicName << "_twist_rejection_threshold is " << twistMahalanobisThresh << "\n\t" << + odomTopicName << "_queue_size is " << odomQueueSize << "\n\t" << + odomTopicName << " pose update vector is " << poseUpdateVec << "\t"<< + odomTopicName << " twist update vector is " << twistUpdateVec); + } + } + while (moreParams); + + // Repeat for pose + topicInd = 0; + moreParams = false; + do + { + std::stringstream ss; + ss << "pose" << topicInd++; + std::string poseTopicName = ss.str(); + moreParams = nhLocal_.hasParam(poseTopicName); + + if (moreParams) + { + bool differential; + nhLocal_.param(poseTopicName + std::string("_differential"), differential, false); + + // Determine if we want to integrate this sensor relatively + bool relative; + nhLocal_.param(poseTopicName + std::string("_relative"), relative, false); + + if (relative && differential) + { + ROS_WARN_STREAM("Both " << poseTopicName << "_differential" << " and " << poseTopicName << + "_relative were set to true. Using differential mode."); + + relative = false; + } + + std::string poseTopic; + nhLocal_.getParam(poseTopicName, poseTopic); + + // Check for pose rejection threshold + double poseMahalanobisThresh; + nhLocal_.param(poseTopicName + std::string("_rejection_threshold"), + poseMahalanobisThresh, + std::numeric_limits::max()); + + int poseQueueSize = 1; + nhLocal_.param(poseTopicName + "_queue_size", poseQueueSize, 1); + + bool nodelayPose = false; + nhLocal_.param(poseTopicName + "_nodelay", nodelayPose, false); + + // Pull in the sensor's config, zero out values that are invalid for the pose type + std::vector poseUpdateVec = loadUpdateConfig(poseTopicName); + std::fill(poseUpdateVec.begin() + POSITION_V_OFFSET, + poseUpdateVec.begin() + POSITION_V_OFFSET + TWIST_SIZE, + 0); + std::fill(poseUpdateVec.begin() + POSITION_A_OFFSET, + poseUpdateVec.begin() + POSITION_A_OFFSET + ACCELERATION_SIZE, + 0); + + int poseUpdateSum = std::accumulate(poseUpdateVec.begin(), poseUpdateVec.end(), 0); + + if (poseUpdateSum > 0) + { + const CallbackData callbackData(poseTopicName, poseUpdateVec, poseUpdateSum, differential, relative, + false, poseMahalanobisThresh); + + topicSubs_.push_back( + nh_.subscribe(poseTopic, poseQueueSize, + boost::bind(&RosFilter::poseCallback, this, boost::placeholders::_1, + callbackData, worldFrameId_, baseLinkFrameId_, false), + ros::VoidPtr(), ros::TransportHints().tcpNoDelay(nodelayPose))); + + if (differential) + { + twistVarCounts[StateMemberVx] += poseUpdateVec[StateMemberX]; + twistVarCounts[StateMemberVy] += poseUpdateVec[StateMemberY]; + twistVarCounts[StateMemberVz] += poseUpdateVec[StateMemberZ]; + twistVarCounts[StateMemberVroll] += poseUpdateVec[StateMemberRoll]; + twistVarCounts[StateMemberVpitch] += poseUpdateVec[StateMemberPitch]; + twistVarCounts[StateMemberVyaw] += poseUpdateVec[StateMemberYaw]; + } + else + { + absPoseVarCounts[StateMemberX] += poseUpdateVec[StateMemberX]; + absPoseVarCounts[StateMemberY] += poseUpdateVec[StateMemberY]; + absPoseVarCounts[StateMemberZ] += poseUpdateVec[StateMemberZ]; + absPoseVarCounts[StateMemberRoll] += poseUpdateVec[StateMemberRoll]; + absPoseVarCounts[StateMemberPitch] += poseUpdateVec[StateMemberPitch]; + absPoseVarCounts[StateMemberYaw] += poseUpdateVec[StateMemberYaw]; + } + } + else + { + ROS_WARN_STREAM("Warning: " << poseTopic << " is listed as an input topic, " + "but all pose update variables are false"); + } + + RF_DEBUG("Subscribed to " << poseTopic << " (" << poseTopicName << ")\n\t" << + poseTopicName << "_differential is " << (differential ? "true" : "false") << "\n\t" << + poseTopicName << "_rejection_threshold is " << poseMahalanobisThresh << "\n\t" << + poseTopicName << "_queue_size is " << poseQueueSize << "\n\t" << + poseTopicName << " update vector is " << poseUpdateVec); + } + } + while (moreParams); + + // Repeat for twist + topicInd = 0; + moreParams = false; + do + { + std::stringstream ss; + ss << "twist" << topicInd++; + std::string twistTopicName = ss.str(); + moreParams = nhLocal_.hasParam(twistTopicName); + + if (moreParams) + { + std::string twistTopic; + nhLocal_.getParam(twistTopicName, twistTopic); + + // Check for twist rejection threshold + double twistMahalanobisThresh; + nhLocal_.param(twistTopicName + std::string("_rejection_threshold"), + twistMahalanobisThresh, + std::numeric_limits::max()); + + int twistQueueSize = 1; + nhLocal_.param(twistTopicName + "_queue_size", twistQueueSize, 1); + + bool nodelayTwist = false; + nhLocal_.param(twistTopicName + "_nodelay", nodelayTwist, false); + + // Pull in the sensor's config, zero out values that are invalid for the twist type + std::vector twistUpdateVec = loadUpdateConfig(twistTopicName); + std::fill(twistUpdateVec.begin() + POSITION_OFFSET, twistUpdateVec.begin() + POSITION_OFFSET + POSE_SIZE, 0); + + int twistUpdateSum = std::accumulate(twistUpdateVec.begin(), twistUpdateVec.end(), 0); + + if (twistUpdateSum > 0) + { + const CallbackData callbackData(twistTopicName, twistUpdateVec, twistUpdateSum, false, false, + false, twistMahalanobisThresh); + + topicSubs_.push_back( + nh_.subscribe(twistTopic, twistQueueSize, + boost::bind(&RosFilter::twistCallback, this, boost::placeholders::_1, + callbackData, baseLinkFrameId_), + ros::VoidPtr(), ros::TransportHints().tcpNoDelay(nodelayTwist))); + + twistVarCounts[StateMemberVx] += twistUpdateVec[StateMemberVx]; + twistVarCounts[StateMemberVy] += twistUpdateVec[StateMemberVy]; + twistVarCounts[StateMemberVz] += twistUpdateVec[StateMemberVz]; + twistVarCounts[StateMemberVroll] += twistUpdateVec[StateMemberVroll]; + twistVarCounts[StateMemberVpitch] += twistUpdateVec[StateMemberVpitch]; + twistVarCounts[StateMemberVyaw] += twistUpdateVec[StateMemberVyaw]; + } + else + { + ROS_WARN_STREAM("Warning: " << twistTopic << " is listed as an input topic, " + "but all twist update variables are false"); + } + + RF_DEBUG("Subscribed to " << twistTopic << " (" << twistTopicName << ")\n\t" << + twistTopicName << "_rejection_threshold is " << twistMahalanobisThresh << "\n\t" << + twistTopicName << "_queue_size is " << twistQueueSize << "\n\t" << + twistTopicName << " update vector is " << twistUpdateVec); + } + } + while (moreParams); + + // Repeat for IMU + topicInd = 0; + moreParams = false; + do + { + std::stringstream ss; + ss << "imu" << topicInd++; + std::string imuTopicName = ss.str(); + moreParams = nhLocal_.hasParam(imuTopicName); + + if (moreParams) + { + bool differential; + nhLocal_.param(imuTopicName + std::string("_differential"), differential, false); + + // Determine if we want to integrate this sensor relatively + bool relative; + nhLocal_.param(imuTopicName + std::string("_relative"), relative, false); + + if (relative && differential) + { + ROS_WARN_STREAM("Both " << imuTopicName << "_differential" << " and " << imuTopicName << + "_relative were set to true. Using differential mode."); + + relative = false; + } + + std::string imuTopic; + nhLocal_.getParam(imuTopicName, imuTopic); + + // Check for pose rejection threshold + double poseMahalanobisThresh; + nhLocal_.param(imuTopicName + std::string("_pose_rejection_threshold"), + poseMahalanobisThresh, + std::numeric_limits::max()); + + // Check for angular velocity rejection threshold + double twistMahalanobisThresh; + std::string imuTwistRejectionName = + imuTopicName + std::string("_twist_rejection_threshold"); + nhLocal_.param(imuTwistRejectionName, twistMahalanobisThresh, std::numeric_limits::max()); + + // Check for acceleration rejection threshold + double accelMahalanobisThresh; + nhLocal_.param(imuTopicName + std::string("_linear_acceleration_rejection_threshold"), + accelMahalanobisThresh, + std::numeric_limits::max()); + + bool removeGravAcc = false; + nhLocal_.param(imuTopicName + "_remove_gravitational_acceleration", removeGravAcc, false); + removeGravitationalAcc_[imuTopicName + "_acceleration"] = removeGravAcc; + + // Now pull in its boolean update vector configuration and differential + // update configuration (as this contains pose information) + std::vector updateVec = loadUpdateConfig(imuTopicName); + + // sanity checks for update config settings + std::vector positionUpdateVec(updateVec.begin() + POSITION_OFFSET, + updateVec.begin() + POSITION_OFFSET + POSITION_SIZE); + int positionUpdateSum = std::accumulate(positionUpdateVec.begin(), positionUpdateVec.end(), 0); + if (positionUpdateSum > 0) + { + ROS_WARN_STREAM("Warning: Some position entries in parameter " << imuTopicName << "_config are listed true, " + "but sensor_msgs/Imu contains no information about position"); + } + std::vector linearVelocityUpdateVec(updateVec.begin() + POSITION_V_OFFSET, + updateVec.begin() + POSITION_V_OFFSET + LINEAR_VELOCITY_SIZE); + int linearVelocityUpdateSum = std::accumulate(linearVelocityUpdateVec.begin(), + linearVelocityUpdateVec.end(), + 0); + if (linearVelocityUpdateSum > 0) + { + ROS_WARN_STREAM("Warning: Some linear velocity entries in parameter " << imuTopicName << "_config are listed " + "true, but an sensor_msgs/Imu contains no information about linear velocities"); + } + + std::vector poseUpdateVec = updateVec; + // IMU message contains no information about position, filter everything except orientation + std::fill(poseUpdateVec.begin() + POSITION_OFFSET, + poseUpdateVec.begin() + POSITION_OFFSET + POSITION_SIZE, + 0); + std::fill(poseUpdateVec.begin() + POSITION_V_OFFSET, + poseUpdateVec.begin() + POSITION_V_OFFSET + TWIST_SIZE, + 0); + std::fill(poseUpdateVec.begin() + POSITION_A_OFFSET, + poseUpdateVec.begin() + POSITION_A_OFFSET + ACCELERATION_SIZE, + 0); + + std::vector twistUpdateVec = updateVec; + // IMU message contains no information about linear speeds, filter everything except angular velocity + std::fill(twistUpdateVec.begin() + POSITION_OFFSET, + twistUpdateVec.begin() + POSITION_OFFSET + POSE_SIZE, + 0); + std::fill(twistUpdateVec.begin() + POSITION_V_OFFSET, + twistUpdateVec.begin() + POSITION_V_OFFSET + LINEAR_VELOCITY_SIZE, + 0); + std::fill(twistUpdateVec.begin() + POSITION_A_OFFSET, + twistUpdateVec.begin() + POSITION_A_OFFSET + ACCELERATION_SIZE, + 0); + + std::vector accelUpdateVec = updateVec; + std::fill(accelUpdateVec.begin() + POSITION_OFFSET, + accelUpdateVec.begin() + POSITION_OFFSET + POSE_SIZE, + 0); + std::fill(accelUpdateVec.begin() + POSITION_V_OFFSET, + accelUpdateVec.begin() + POSITION_V_OFFSET + TWIST_SIZE, + 0); + + int poseUpdateSum = std::accumulate(poseUpdateVec.begin(), poseUpdateVec.end(), 0); + int twistUpdateSum = std::accumulate(twistUpdateVec.begin(), twistUpdateVec.end(), 0); + int accelUpdateSum = std::accumulate(accelUpdateVec.begin(), accelUpdateVec.end(), 0); + + // Check if we're using control input for any of the acceleration variables; turn off if so + if (static_cast(controlUpdateVector[ControlMemberVx]) && static_cast(accelUpdateVec[StateMemberAx])) + { + ROS_WARN_STREAM("X acceleration is being measured from IMU; X velocity control input is disabled"); + controlUpdateVector[ControlMemberVx] = 0; + } + if (static_cast(controlUpdateVector[ControlMemberVy]) && static_cast(accelUpdateVec[StateMemberAy])) + { + ROS_WARN_STREAM("Y acceleration is being measured from IMU; Y velocity control input is disabled"); + controlUpdateVector[ControlMemberVy] = 0; + } + if (static_cast(controlUpdateVector[ControlMemberVz]) && static_cast(accelUpdateVec[StateMemberAz])) + { + ROS_WARN_STREAM("Z acceleration is being measured from IMU; Z velocity control input is disabled"); + controlUpdateVector[ControlMemberVz] = 0; + } + + int imuQueueSize = 1; + nhLocal_.param(imuTopicName + "_queue_size", imuQueueSize, 1); + + bool nodelayImu = false; + nhLocal_.param(imuTopicName + "_nodelay", nodelayImu, false); + + if (poseUpdateSum + twistUpdateSum + accelUpdateSum > 0) + { + const CallbackData poseCallbackData(imuTopicName + "_pose", poseUpdateVec, poseUpdateSum, differential, + relative, false, poseMahalanobisThresh); + const CallbackData twistCallbackData(imuTopicName + "_twist", twistUpdateVec, twistUpdateSum, differential, + relative, false, twistMahalanobisThresh); + const CallbackData accelCallbackData(imuTopicName + "_acceleration", accelUpdateVec, accelUpdateSum, + differential, relative, false, accelMahalanobisThresh); + + topicSubs_.push_back( + nh_.subscribe(imuTopic, imuQueueSize, + boost::bind(&RosFilter::imuCallback, this, boost::placeholders::_1, + imuTopicName, poseCallbackData, twistCallbackData, + accelCallbackData), ros::VoidPtr(), ros::TransportHints().tcpNoDelay(nodelayImu))); + } + else + { + ROS_WARN_STREAM("Warning: " << imuTopic << " is listed as an input topic, " + "but all its update variables are false"); + } + + if (poseUpdateSum > 0) + { + if (differential) + { + twistVarCounts[StateMemberVroll] += poseUpdateVec[StateMemberRoll]; + twistVarCounts[StateMemberVpitch] += poseUpdateVec[StateMemberPitch]; + twistVarCounts[StateMemberVyaw] += poseUpdateVec[StateMemberYaw]; + } + else + { + absPoseVarCounts[StateMemberRoll] += poseUpdateVec[StateMemberRoll]; + absPoseVarCounts[StateMemberPitch] += poseUpdateVec[StateMemberPitch]; + absPoseVarCounts[StateMemberYaw] += poseUpdateVec[StateMemberYaw]; + } + } + + if (twistUpdateSum > 0) + { + twistVarCounts[StateMemberVroll] += twistUpdateVec[StateMemberVroll]; + twistVarCounts[StateMemberVpitch] += twistUpdateVec[StateMemberVpitch]; + twistVarCounts[StateMemberVyaw] += twistUpdateVec[StateMemberVyaw]; + } + + RF_DEBUG("Subscribed to " << imuTopic << " (" << imuTopicName << ")\n\t" << + imuTopicName << "_differential is " << (differential ? "true" : "false") << "\n\t" << + imuTopicName << "_pose_rejection_threshold is " << poseMahalanobisThresh << "\n\t" << + imuTopicName << "_twist_rejection_threshold is " << twistMahalanobisThresh << "\n\t" << + imuTopicName << "_linear_acceleration_rejection_threshold is " << accelMahalanobisThresh << "\n\t" << + imuTopicName << "_remove_gravitational_acceleration is " << + (removeGravAcc ? "true" : "false") << "\n\t" << + imuTopicName << "_queue_size is " << imuQueueSize << "\n\t" << + imuTopicName << " pose update vector is " << poseUpdateVec << "\t"<< + imuTopicName << " twist update vector is " << twistUpdateVec << "\t" << + imuTopicName << " acceleration update vector is " << accelUpdateVec); + } + } + while (moreParams); + angular_acceleration_cov_.resize(ORIENTATION_SIZE, ORIENTATION_SIZE); + angular_acceleration_cov_.setZero(); + + // Now that we've checked if IMU linear acceleration is being used, we can determine our final control parameters + if (useControl_ && std::accumulate(controlUpdateVector.begin(), controlUpdateVector.end(), 0) == 0) + { + ROS_ERROR_STREAM("use_control is set to true, but control_config has only false values. No control term " + "will be used."); + useControl_ = false; + } + + // If we're using control, set the parameters and create the necessary subscribers + if (useControl_) + { + latestControl_.resize(TWIST_SIZE); + latestControl_.setZero(); + + filter_.setControlParams(controlUpdateVector, controlTimeout, accelerationLimits, accelerationGains, + decelerationLimits, decelerationGains); + + if (stampedControl) + { + controlSub_ = nh_.subscribe("cmd_vel", 1, &RosFilter::controlCallback, this); + } + else + { + controlSub_ = nh_.subscribe("cmd_vel", 1, &RosFilter::controlCallback, this); + } + } + + /* Warn users about: + * 1. Multiple non-differential input sources + * 2. No absolute *or* velocity measurements for pose variables + */ + if (printDiagnostics_) + { + for (int stateVar = StateMemberX; stateVar <= StateMemberYaw; ++stateVar) + { + if (absPoseVarCounts[static_cast(stateVar)] > 1) + { + std::stringstream stream; + stream << absPoseVarCounts[static_cast(stateVar - POSITION_OFFSET)] << + " absolute pose inputs detected for " << stateVariableNames_[stateVar] << + ". This may result in oscillations. Please ensure that your variances for each " + "measured variable are set appropriately."; + + addDiagnostic(diagnostic_msgs::DiagnosticStatus::WARN, + stateVariableNames_[stateVar] + "_configuration", + stream.str(), + true); + } + else if (absPoseVarCounts[static_cast(stateVar)] == 0) + { + if ((static_cast(stateVar) == StateMemberX && + twistVarCounts[static_cast(StateMemberVx)] == 0) || + (static_cast(stateVar) == StateMemberY && + twistVarCounts[static_cast(StateMemberVy)] == 0) || + (static_cast(stateVar) == StateMemberZ && + twistVarCounts[static_cast(StateMemberVz)] == 0 && + twoDMode_ == false) || + (static_cast(stateVar) == StateMemberRoll && + twistVarCounts[static_cast(StateMemberVroll)] == 0 && + twoDMode_ == false) || + (static_cast(stateVar) == StateMemberPitch && + twistVarCounts[static_cast(StateMemberVpitch)] == 0 && + twoDMode_ == false) || + (static_cast(stateVar) == StateMemberYaw && + twistVarCounts[static_cast(StateMemberVyaw)] == 0)) + { + std::stringstream stream; + stream << "Neither " << stateVariableNames_[stateVar] << " nor its " + "velocity is being measured. This will result in unbounded " + "error growth and erratic filter behavior."; + + addDiagnostic(diagnostic_msgs::DiagnosticStatus::ERROR, + stateVariableNames_[stateVar] + "_configuration", + stream.str(), + true); + } + } + } + } + + // Load up the process noise covariance (from the launch file/parameter server) + Eigen::MatrixXd processNoiseCovariance(STATE_SIZE, STATE_SIZE); + processNoiseCovariance.setZero(); + XmlRpc::XmlRpcValue processNoiseCovarConfig; + + if (nhLocal_.hasParam("process_noise_covariance")) + { + try + { + nhLocal_.getParam("process_noise_covariance", processNoiseCovarConfig); + + ROS_ASSERT(processNoiseCovarConfig.getType() == XmlRpc::XmlRpcValue::TypeArray); + + int matSize = processNoiseCovariance.rows(); + + for (int i = 0; i < matSize; i++) + { + for (int j = 0; j < matSize; j++) + { + try + { + // These matrices can cause problems if all the types + // aren't specified with decimal points. Handle that + // using string streams. + std::ostringstream ostr; + ostr << processNoiseCovarConfig[matSize * i + j]; + std::istringstream istr(ostr.str()); + istr >> processNoiseCovariance(i, j); + } + catch(XmlRpc::XmlRpcException &e) + { + throw e; + } + catch(...) + { + throw; + } + } + } + + RF_DEBUG("Process noise covariance is:\n" << processNoiseCovariance << "\n"); + } + catch (XmlRpc::XmlRpcException &e) + { + ROS_ERROR_STREAM("ERROR reading sensor config: " << + e.getMessage() << + " for process_noise_covariance (type: " << + processNoiseCovarConfig.getType() << ")"); + } + + filter_.setProcessNoiseCovariance(processNoiseCovariance); + } + + // Load up the process noise covariance (from the launch file/parameter server) + Eigen::MatrixXd initialEstimateErrorCovariance(STATE_SIZE, STATE_SIZE); + initialEstimateErrorCovariance.setZero(); + XmlRpc::XmlRpcValue estimateErrorCovarConfig; + + if (nhLocal_.hasParam("initial_estimate_covariance")) + { + try + { + nhLocal_.getParam("initial_estimate_covariance", estimateErrorCovarConfig); + + ROS_ASSERT(estimateErrorCovarConfig.getType() == XmlRpc::XmlRpcValue::TypeArray); + + int matSize = initialEstimateErrorCovariance.rows(); + + for (int i = 0; i < matSize; i++) + { + for (int j = 0; j < matSize; j++) + { + try + { + // These matrices can cause problems if all the types + // aren't specified with decimal points. Handle that + // using string streams. + std::ostringstream ostr; + ostr << estimateErrorCovarConfig[matSize * i + j]; + std::istringstream istr(ostr.str()); + istr >> initialEstimateErrorCovariance(i, j); + } + catch(XmlRpc::XmlRpcException &e) + { + throw e; + } + catch(...) + { + throw; + } + } + } + + RF_DEBUG("Initial estimate error covariance is:\n" << initialEstimateErrorCovariance << "\n"); + } + catch (XmlRpc::XmlRpcException &e) + { + ROS_ERROR_STREAM("ERROR reading initial_estimate_covariance (type: " << + estimateErrorCovarConfig.getType() << + "): " << + e.getMessage()); + } + catch(...) + { + ROS_ERROR_STREAM( + "ERROR reading initial_estimate_covariance (type: " << estimateErrorCovarConfig.getType() << ")"); + } + + filter_.setEstimateErrorCovariance(initialEstimateErrorCovariance); + } + } + + template + void RosFilter::odometryCallback(const nav_msgs::Odometry::ConstPtr &msg, const std::string &topicName, + const CallbackData &poseCallbackData, const CallbackData &twistCallbackData) + { + // If we've just reset the filter, then we want to ignore any messages + // that arrive with an older timestamp + if (msg->header.stamp <= lastSetPoseTime_) + { + std::stringstream stream; + stream << "The " << topicName << " message has a timestamp equal to or before the last filter reset, " << + "this message will be ignored. This may indicate an empty or bad timestamp. (message time: " << + msg->header.stamp.toSec() << ")"; + addDiagnostic(diagnostic_msgs::DiagnosticStatus::WARN, + topicName + "_timestamp", + stream.str(), + false); + RF_DEBUG("Received message that preceded the most recent pose reset. Ignoring..."); + + return; + } + + RF_DEBUG("------ RosFilter::odometryCallback (" << topicName << ") ------\n" << "Odometry message:\n" << *msg); + + if (poseCallbackData.updateSum_ > 0) + { + // Grab the pose portion of the message and pass it to the poseCallback + geometry_msgs::PoseWithCovarianceStamped *posPtr = new geometry_msgs::PoseWithCovarianceStamped(); + posPtr->header = msg->header; + posPtr->pose = msg->pose; // Entire pose object, also copies covariance + + geometry_msgs::PoseWithCovarianceStampedConstPtr pptr(posPtr); + if (poseCallbackData.pose_use_child_frame_) + { + poseCallback(pptr, poseCallbackData, worldFrameId_, msg->child_frame_id, false); + } + else + { + poseCallback(pptr, poseCallbackData, worldFrameId_, baseLinkFrameId_, false); + } + } + + if (twistCallbackData.updateSum_ > 0) + { + // Grab the twist portion of the message and pass it to the twistCallback + geometry_msgs::TwistWithCovarianceStamped *twistPtr = new geometry_msgs::TwistWithCovarianceStamped(); + twistPtr->header = msg->header; + twistPtr->header.frame_id = msg->child_frame_id; + twistPtr->twist = msg->twist; // Entire twist object, also copies covariance + + geometry_msgs::TwistWithCovarianceStampedConstPtr tptr(twistPtr); + twistCallback(tptr, twistCallbackData, baseLinkFrameId_); + } + + RF_DEBUG("\n----- /RosFilter::odometryCallback (" << topicName << ") ------\n"); + } + + template + void RosFilter::poseCallback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg, + const CallbackData &callbackData, + const std::string &targetFrame, + const std::string &poseSourceFrame, + const bool imuData) + { + const std::string &topicName = callbackData.topicName_; + + // If we've just reset the filter, then we want to ignore any messages + // that arrive with an older timestamp + if (msg->header.stamp <= lastSetPoseTime_) + { + std::stringstream stream; + stream << "The " << topicName << " message has a timestamp equal to or before the last filter reset, " << + "this message will be ignored. This may indicate an empty or bad timestamp. (message time: " << + msg->header.stamp.toSec() << ")"; + addDiagnostic(diagnostic_msgs::DiagnosticStatus::WARN, + topicName + "_timestamp", + stream.str(), + false); + return; + } + + RF_DEBUG("------ RosFilter::poseCallback (" << topicName << ") ------\n" << + "Pose message:\n" << *msg); + + // Put the initial value in the lastMessagTimes_ for this variable if it's empty + if (lastMessageTimes_.count(topicName) == 0) + { + lastMessageTimes_.insert(std::pair(topicName, msg->header.stamp)); + } + + // Make sure this message is newer than the last one + if (msg->header.stamp >= lastMessageTimes_[topicName]) + { + RF_DEBUG("Update vector for " << topicName << " is:\n" << callbackData.updateVector_); + + Eigen::VectorXd measurement(STATE_SIZE); + Eigen::MatrixXd measurementCovariance(STATE_SIZE, STATE_SIZE); + + measurement.setZero(); + measurementCovariance.setZero(); + + // Make sure we're actually updating at least one of these variables + std::vector updateVectorCorrected = callbackData.updateVector_; + + // Prepare the pose data for inclusion in the filter + if (preparePose(msg, + topicName, + targetFrame, + poseSourceFrame, + callbackData.differential_, + callbackData.relative_, + imuData, + updateVectorCorrected, + measurement, + measurementCovariance)) + { + // Store the measurement. Add a "pose" suffix so we know what kind of measurement + // we're dealing with when we debug the core filter logic. + enqueueMeasurement(topicName, + measurement, + measurementCovariance, + updateVectorCorrected, + callbackData.rejectionThreshold_, + msg->header.stamp); + + RF_DEBUG("Enqueued new measurement for " << topicName << "\n"); + } + else + { + RF_DEBUG("Did *not* enqueue measurement for " << topicName << "\n"); + } + + lastMessageTimes_[topicName] = msg->header.stamp; + + RF_DEBUG("Last message time for " << topicName << " is now " << + lastMessageTimes_[topicName] << "\n"); + } + else if (resetOnTimeJump_ && ros::Time::isSimTime()) + { + reset(); + } + else + { + std::stringstream stream; + stream << "The " << topicName << " message has a timestamp before that of the previous message received," << + " this message will be ignored. This may indicate a bad timestamp. (message time: " << + msg->header.stamp.toSec() << ")"; + addDiagnostic(diagnostic_msgs::DiagnosticStatus::WARN, + topicName + "_timestamp", + stream.str(), + false); + + RF_DEBUG("Message is too old. Last message time for " << topicName << " is " + << lastMessageTimes_[topicName] << ", current message time is " + << msg->header.stamp << ".\n"); + } + + RF_DEBUG("\n----- /RosFilter::poseCallback (" << topicName << ") ------\n"); + } + + template + void RosFilter::periodicUpdate(const ros::TimerEvent& event) + { + // Warn the user if the update took too long (> 2 cycles) + const double loop_elapsed = (event.current_real - event.last_expected).toSec(); + if (loop_elapsed > 2./frequency_) + { + ROS_WARN_STREAM("Failed to meet update rate! Took " << std::setprecision(20) << loop_elapsed); + } + + // Wait for the filter to be enabled + if (!enabled_) + { + ROS_INFO_STREAM_ONCE("Filter is disabled. To enable it call the " << enableFilterSrv_.getService() << + " service"); + return; + } + + ros::Time curTime = ros::Time::now(); + + if (toggledOn_) + { + // Now we'll integrate any measurements we've received if requested, and update angular acceleration. + integrateMeasurements(curTime); + differentiateMeasurements(curTime); + } + else + { + // clear out measurements since we're not currently processing new entries + clearMeasurementQueue(); + + // Reset last measurement time so we don't get a large time delta on toggle on + if (filter_.getInitializedStatus()) + { + filter_.setLastMeasurementTime(ros::Time::now().toSec()); + } + } + + // Get latest state and publish it + nav_msgs::Odometry filteredPosition; + + bool corrected_data = false; + + if (getFilteredOdometryMessage(filteredPosition)) + { + worldBaseLinkTransMsg_.transform = tf2::toMsg(tf2::Transform::getIdentity()); + worldBaseLinkTransMsg_.header.stamp = filteredPosition.header.stamp + tfTimeOffset_; + worldBaseLinkTransMsg_.header.frame_id = filteredPosition.header.frame_id; + worldBaseLinkTransMsg_.child_frame_id = filteredPosition.child_frame_id; + + worldBaseLinkTransMsg_.transform.translation.x = filteredPosition.pose.pose.position.x; + worldBaseLinkTransMsg_.transform.translation.y = filteredPosition.pose.pose.position.y; + worldBaseLinkTransMsg_.transform.translation.z = filteredPosition.pose.pose.position.z; + worldBaseLinkTransMsg_.transform.rotation = filteredPosition.pose.pose.orientation; + + // the filteredPosition is the message containing the state and covariances: nav_msgs Odometry + + if (!validateFilterOutput(filteredPosition)) + { + ROS_ERROR_STREAM("Critical Error, NaNs were detected in the output state of the filter." << + " This was likely due to poorly conditioned process, noise, or sensor covariances."); + } + + // If we're trying to publish with the same time stamp, it means that we had a measurement get inserted into the + // filter history, and our state estimate was updated after it was already published. As of ROS Noetic, TF2 will + // issue warnings whenever this occurs, so we make this behavior optional. + // Just for safety, we also check for the condition where the last published stamp is *later* than this stamp. + // This should never happen, but we should handle the case anyway. + corrected_data = (!permitCorrectedPublication_ && lastPublishedStamp_ >= filteredPosition.header.stamp); + + // If the worldFrameId_ is the odomFrameId_ frame, then we can just send the transform. If the + // worldFrameId_ is the mapFrameId_ frame, we'll have some work to do. + if (publishTransform_ && !corrected_data) + { + if (filteredPosition.header.frame_id == odomFrameId_) + { + worldTransformBroadcaster_.sendTransform(worldBaseLinkTransMsg_); + } + else if (filteredPosition.header.frame_id == mapFrameId_) + { + /* + * First, see these two references: + * http://wiki.ros.org/tf/Overview/Using%20Published%20Transforms#lookupTransform + * http://wiki.ros.org/geometry/CoordinateFrameConventions#Transform_Direction + * We have a transform from mapFrameId_->baseLinkFrameId_, but it would actually transform + * a given pose from baseLinkFrameId_->mapFrameId_. We then used lookupTransform, whose + * first two arguments are target frame and source frame, to get a transform from + * baseLinkFrameId_->odomFrameId_. However, this transform would actually transform data + * from odomFrameId_->baseLinkFrameId_. Now imagine that we have a position in the + * mapFrameId_ frame. First, we multiply it by the inverse of the + * mapFrameId_->baseLinkFrameId, which will transform that data from mapFrameId_ to + * baseLinkFrameId_. Now we want to go from baseLinkFrameId_->odomFrameId_, but the + * transform we have takes data from odomFrameId_->baseLinkFrameId_, so we need its + * inverse as well. We have now transformed our data from mapFrameId_ to odomFrameId_. + * However, if we want other users to be able to do the same, we need to broadcast + * the inverse of that entire transform. + */ + + tf2::Transform baseLinkOdomTrans; + if (RosFilterUtilities::lookupTransformSafe( + tfBuffer_, + baseLinkFrameId_, + odomFrameId_, + filteredPosition.header.stamp, + tfTimeout_, + baseLinkOdomTrans, + tfSilentFailure_)) + { + tf2::Transform worldBaseLinkTrans; + tf2::fromMsg(worldBaseLinkTransMsg_.transform, worldBaseLinkTrans); + + tf2::Transform mapOdomTrans; + mapOdomTrans.mult(worldBaseLinkTrans, baseLinkOdomTrans); + + geometry_msgs::TransformStamped mapOdomTransMsg; + mapOdomTransMsg.transform = tf2::toMsg(mapOdomTrans); + mapOdomTransMsg.header.stamp = filteredPosition.header.stamp + tfTimeOffset_; + mapOdomTransMsg.header.frame_id = mapFrameId_; + mapOdomTransMsg.child_frame_id = odomFrameId_; + + worldTransformBroadcaster_.sendTransform(mapOdomTransMsg); + } + else + { + ROS_ERROR_STREAM_DELAYED_THROTTLE(5.0, "Could not obtain transform from " << odomFrameId_ << + "->" << baseLinkFrameId_); + } + } + else + { + ROS_ERROR_STREAM("Odometry message frame_id was " << filteredPosition.header.frame_id << + ", expected " << mapFrameId_ << " or " << odomFrameId_); + } + } + + // Fire off the position and the transform + if (!corrected_data) + { + positionPub_.publish(filteredPosition); + } + + // Retain the last published stamp so we can detect repeated transforms in future cycles + lastPublishedStamp_ = filteredPosition.header.stamp; + + if (printDiagnostics_) + { + freqDiag_->tick(); + } + } + + // Publish the acceleration if desired and filter is initialized + geometry_msgs::AccelWithCovarianceStamped filteredAcceleration; + if (publishAcceleration_ && getFilteredAccelMessage(filteredAcceleration) && !corrected_data) + { + accelPub_.publish(filteredAcceleration); + } + + /* Diagnostics can behave strangely when playing back from bag + * files and using simulated time, so we have to check for + * time suddenly moving backwards as well as the standard + * timeout criterion before publishing. */ + double diagDuration = (curTime - lastDiagTime_).toSec(); + if (printDiagnostics_ && (diagDuration >= diagnosticUpdater_.getPeriod() || diagDuration < 0.0)) + { + diagnosticUpdater_.force_update(); + lastDiagTime_ = curTime; + } + + // Clear out expired history data + if (smoothLaggedData_) + { + clearExpiredHistory(filter_.getLastMeasurementTime() - historyLength_); + } + } + + template + void RosFilter::setPoseCallback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg) + { + RF_DEBUG("------ RosFilter::setPoseCallback ------\nPose message:\n" << *msg); + + ROS_INFO_STREAM("Received set_pose request with value\n" << *msg); + + std::string topicName("setPose"); + + // Get rid of any initial poses (pretend we've never had a measurement) + initialMeasurements_.clear(); + previousMeasurements_.clear(); + previousMeasurementCovariances_.clear(); + + clearMeasurementQueue(); + + filterStateHistory_.clear(); + measurementHistory_.clear(); + + // Also set the last set pose time, so we ignore all messages + // that occur before it + lastSetPoseTime_ = msg->header.stamp; + + // Set the state vector to the reported pose + Eigen::VectorXd measurement(STATE_SIZE); + Eigen::MatrixXd measurementCovariance(STATE_SIZE, STATE_SIZE); + std::vector updateVector(STATE_SIZE, true); + + // We only measure pose variables, so initialize the vector to 0 + measurement.setZero(); + + // Set this to the identity and let the message reset it + measurementCovariance.setIdentity(); + measurementCovariance *= 1e-6; + + // Prepare the pose data (really just using this to transform it into the target frame). + // Twist data is going to get zeroed out. + // Since pose messages do not provide a child_frame_id, it defaults to baseLinkFrameId_ + preparePose(msg, topicName, worldFrameId_, baseLinkFrameId_, false, false, + false, updateVector, measurement, measurementCovariance); + + // For the state + filter_.setState(measurement); + filter_.setEstimateErrorCovariance(measurementCovariance); + + filter_.setLastMeasurementTime(ros::Time::now().toSec()); + + RF_DEBUG("\n------ /RosFilter::setPoseCallback ------\n"); + } + + template + bool RosFilter::setPoseSrvCallback(robot_localization::SetPose::Request& request, + robot_localization::SetPose::Response&) + { + geometry_msgs::PoseWithCovarianceStamped::Ptr msg; + msg = boost::make_shared(request.pose); + setPoseCallback(msg); + + return true; + } + + template + bool RosFilter::enableFilterSrvCallback(std_srvs::Empty::Request&, + std_srvs::Empty::Response&) + { + RF_DEBUG("\n[" << ros::this_node::getName() << ":]" << " ------ /RosFilter::enableFilterSrvCallback ------\n"); + if (enabled_) + { + ROS_WARN_STREAM("[" << ros::this_node::getName() << ":] Asking for enabling filter service, but the filter was " + "already enabled! Use param disabled_at_startup."); + } + else + { + ROS_INFO_STREAM("[" << ros::this_node::getName() << ":] Enabling filter..."); + enabled_ = true; + } + return true; + } + + template + void RosFilter::twistCallback(const geometry_msgs::TwistWithCovarianceStamped::ConstPtr &msg, + const CallbackData &callbackData, + const std::string &targetFrame) + { + const std::string &topicName = callbackData.topicName_; + + // If we've just reset the filter, then we want to ignore any messages + // that arrive with an older timestamp + if (msg->header.stamp <= lastSetPoseTime_) + { + std::stringstream stream; + stream << "The " << topicName << " message has a timestamp equal to or before the last filter reset, " << + "this message will be ignored. This may indicate an empty or bad timestamp. (message time: " << + msg->header.stamp.toSec() << ")"; + addDiagnostic(diagnostic_msgs::DiagnosticStatus::WARN, + topicName + "_timestamp", + stream.str(), + false); + return; + } + + RF_DEBUG("------ RosFilter::twistCallback (" << topicName << ") ------\n" + "Twist message:\n" << *msg); + + if (lastMessageTimes_.count(topicName) == 0) + { + lastMessageTimes_.insert(std::pair(topicName, msg->header.stamp)); + } + + // Make sure this message is newer than the last one + if (msg->header.stamp >= lastMessageTimes_[topicName]) + { + RF_DEBUG("Update vector for " << topicName << " is:\n" << callbackData.updateVector_); + + Eigen::VectorXd measurement(STATE_SIZE); + Eigen::MatrixXd measurementCovariance(STATE_SIZE, STATE_SIZE); + + measurement.setZero(); + measurementCovariance.setZero(); + + // Make sure we're actually updating at least one of these variables + std::vector updateVectorCorrected = callbackData.updateVector_; + + // Prepare the twist data for inclusion in the filter + if (prepareTwist(msg, topicName, targetFrame, updateVectorCorrected, measurement, measurementCovariance)) + { + // Store the measurement. Add a "twist" suffix so we know what kind of measurement + // we're dealing with when we debug the core filter logic. + enqueueMeasurement(topicName, + measurement, + measurementCovariance, + updateVectorCorrected, + callbackData.rejectionThreshold_, + msg->header.stamp); + + RF_DEBUG("Enqueued new measurement for " << topicName << "_twist\n"); + } + else + { + RF_DEBUG("Did *not* enqueue measurement for " << topicName << "_twist\n"); + } + + lastMessageTimes_[topicName] = msg->header.stamp; + + RF_DEBUG("Last message time for " << topicName << " is now " << + lastMessageTimes_[topicName] << "\n"); + } + else if (resetOnTimeJump_ && ros::Time::isSimTime()) + { + reset(); + } + else + { + std::stringstream stream; + stream << "The " << topicName << " message has a timestamp before that of the previous message received," << + " this message will be ignored. This may indicate a bad timestamp. (message time: " << + msg->header.stamp.toSec() << ")"; + addDiagnostic(diagnostic_msgs::DiagnosticStatus::WARN, + topicName + "_timestamp", + stream.str(), + false); + + RF_DEBUG("Message is too old. Last message time for " << topicName << " is " << lastMessageTimes_[topicName] << + ", current message time is " << msg->header.stamp << ".\n"); + } + + RF_DEBUG("\n----- /RosFilter::twistCallback (" << topicName << ") ------\n"); + } + + template + void RosFilter::addDiagnostic(const int errLevel, + const std::string &topicAndClass, + const std::string &message, + const bool staticDiag) + { + if (staticDiag) + { + staticDiagnostics_[topicAndClass] = message; + staticDiagErrorLevel_ = std::max(staticDiagErrorLevel_, errLevel); + } + else + { + dynamicDiagnostics_[topicAndClass] = message; + dynamicDiagErrorLevel_ = std::max(dynamicDiagErrorLevel_, errLevel); + } + } + + template + void RosFilter::aggregateDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &wrapper) + { + wrapper.clear(); + wrapper.clearSummary(); + + int maxErrLevel = std::max(staticDiagErrorLevel_, dynamicDiagErrorLevel_); + + // Report the overall status + switch (maxErrLevel) + { + case diagnostic_msgs::DiagnosticStatus::ERROR: + wrapper.summary(maxErrLevel, + "Erroneous data or settings detected for a robot_localization state estimation node."); + break; + case diagnostic_msgs::DiagnosticStatus::WARN: + wrapper.summary(maxErrLevel, + "Potentially erroneous data or settings detected for " + "a robot_localization state estimation node."); + break; + case diagnostic_msgs::DiagnosticStatus::STALE: + wrapper.summary(maxErrLevel, + "The state of the robot_localization state estimation node is stale."); + break; + case diagnostic_msgs::DiagnosticStatus::OK: + wrapper.summary(maxErrLevel, + "The robot_localization state estimation node appears to be functioning properly."); + break; + default: + break; + } + + // Aggregate all the static messages + for (std::map::iterator diagIt = staticDiagnostics_.begin(); + diagIt != staticDiagnostics_.end(); + ++diagIt) + { + wrapper.add(diagIt->first, diagIt->second); + } + + // Aggregate all the dynamic messages, then clear them + for (std::map::iterator diagIt = dynamicDiagnostics_.begin(); + diagIt != dynamicDiagnostics_.end(); + ++diagIt) + { + wrapper.add(diagIt->first, diagIt->second); + } + dynamicDiagnostics_.clear(); + + // Reset the warning level for the dynamic diagnostic messages + dynamicDiagErrorLevel_ = diagnostic_msgs::DiagnosticStatus::OK; + } + + template + void RosFilter::copyCovariance(const double *arr, + Eigen::MatrixXd &covariance, + const std::string &topicName, + const std::vector &updateVector, + const size_t offset, + const size_t dimension) + { + for (size_t i = 0; i < dimension; i++) + { + for (size_t j = 0; j < dimension; j++) + { + covariance(i, j) = arr[dimension * i + j]; + + if (printDiagnostics_) + { + std::string iVar = stateVariableNames_[offset + i]; + + if (covariance(i, j) > 1e3 && (updateVector[offset + i] || updateVector[offset + j])) + { + std::string jVar = stateVariableNames_[offset + j]; + + std::stringstream stream; + stream << "The covariance at position (" << dimension * i + j << "), which corresponds to " << + (i == j ? iVar + " variance" : iVar + " and " + jVar + " covariance") << + ", the value is extremely large (" << covariance(i, j) << "), but the update vector for " << + (i == j ? iVar : iVar + " and/or " + jVar) << " is set to true. This may produce undesirable results."; + + addDiagnostic(diagnostic_msgs::DiagnosticStatus::WARN, + topicName + "_covariance", + stream.str(), + false); + } + else if (updateVector[i] && i == j && covariance(i, j) == 0) + { + std::stringstream stream; + stream << "The covariance at position (" << dimension * i + j << "), which corresponds to " << + iVar << " variance, was zero. This will be replaced with a small value to maintain filter " + "stability, but should be corrected at the message origin node."; + + addDiagnostic(diagnostic_msgs::DiagnosticStatus::WARN, + topicName + "_covariance", + stream.str(), + false); + } + else if (updateVector[i] && i == j && covariance(i, j) < 0) + { + std::stringstream stream; + stream << "The covariance at position (" << dimension * i + j << + "), which corresponds to " << iVar << " variance, was negative. This will be replaced with a " + "small positive value to maintain filter stability, but should be corrected at the message " + "origin node."; + + addDiagnostic(diagnostic_msgs::DiagnosticStatus::WARN, + topicName + "_covariance", + stream.str(), + false); + } + } + } + } + } + + template + void RosFilter::copyCovariance(const Eigen::MatrixXd &covariance, + double *arr, + const size_t dimension) + { + for (size_t i = 0; i < dimension; i++) + { + for (size_t j = 0; j < dimension; j++) + { + arr[dimension * i + j] = covariance(i, j); + } + } + } + + template + std::vector RosFilter::loadUpdateConfig(const std::string &topicName) + { + XmlRpc::XmlRpcValue topicConfig; + std::vector updateVector(STATE_SIZE, 0); + std::string topicConfigName = topicName + "_config"; + + try + { + nhLocal_.getParam(topicConfigName, topicConfig); + + ROS_ASSERT(topicConfig.getType() == XmlRpc::XmlRpcValue::TypeArray); + + if (topicConfig.size() != STATE_SIZE) + { + ROS_WARN_STREAM("Configuration vector for " << topicConfigName << " should have 15 entries."); + } + + for (int i = 0; i < topicConfig.size(); i++) + { + // The double cast looks strange, but we'll get exceptions if we + // remove the cast to bool. vector is discouraged, so updateVector + // uses integers. + updateVector[i] = static_cast(static_cast(topicConfig[i])); + } + } + catch (XmlRpc::XmlRpcException &e) + { + ROS_FATAL_STREAM("Could not read sensor update configuration for topic " << topicName << + " (type: " << topicConfig.getType() << ", expected: " << XmlRpc::XmlRpcValue::TypeArray + << "). Error was " << e.getMessage() << "\n"); + } + + return updateVector; + } + + template + bool RosFilter::prepareAcceleration(const sensor_msgs::Imu::ConstPtr &msg, + const std::string &topicName, + const std::string &targetFrame, + const bool relative, + std::vector &updateVector, + Eigen::VectorXd &measurement, + Eigen::MatrixXd &measurementCovariance) + { + RF_DEBUG("------ RosFilter::prepareAcceleration (" << topicName << ") ------\n"); + + // 1. Get the measurement into a vector + tf2::Vector3 accTmp(msg->linear_acceleration.x, + msg->linear_acceleration.y, + msg->linear_acceleration.z); + + // Set relevant header info + std::string msgFrame = (msg->header.frame_id == "" ? baseLinkFrameId_ : msg->header.frame_id); + + // 2. robot_localization lets users configure which variables from the sensor should be + // fused with the filter. This is specified at the sensor level. However, the data + // may go through transforms before being fused with the state estimate. In that case, + // we need to know which of the transformed variables came from the pre-transformed + // "approved" variables (i.e., the ones that had "true" in their xxx_config parameter). + // To do this, we create a pose from the original upate vector, which contains only + // zeros and ones. This pose goes through the same transforms as the measurement. The + // non-zero values that result will be used to modify the updateVector. + tf2::Matrix3x3 maskAcc(updateVector[StateMemberAx], 0, 0, + 0, updateVector[StateMemberAy], 0, + 0, 0, updateVector[StateMemberAz]); + + // 3. We'll need to rotate the covariance as well + Eigen::MatrixXd covarianceRotated(ACCELERATION_SIZE, ACCELERATION_SIZE); + covarianceRotated.setZero(); + + this->copyCovariance(&(msg->linear_acceleration_covariance[0]), + covarianceRotated, + topicName, + updateVector, + POSITION_A_OFFSET, + ACCELERATION_SIZE); + + RF_DEBUG("Original measurement as tf object: " << accTmp << + "\nOriginal update vector:\n" << updateVector << + "\nOriginal covariance matrix:\n" << covarianceRotated << "\n"); + + // 4. We need to transform this into the target frame (probably base_link) + // It's unlikely that we'll get a velocity measurement in another frame, but + // we have to handle the situation. + tf2::Transform targetFrameTrans; + bool canTransform = RosFilterUtilities::lookupTransformSafe(tfBuffer_, + targetFrame, + msgFrame, + msg->header.stamp, + tfTimeout_, + targetFrameTrans, + tfSilentFailure_); + + if (canTransform) + { + const Eigen::VectorXd &state = filter_.getState(); + + // We don't know if the user has already handled the removal + // of normal forces, so we use a parameter + if (removeGravitationalAcc_[topicName]) + { + tf2::Vector3 normAcc(0, 0, gravitationalAcc_); + tf2::Transform trans; + tf2::Vector3 rotNorm; + + if (::fabs(msg->orientation_covariance[0] + 1) < 1e-9) + { + // Imu message contains no orientation, so we should use orientation + // from filter state to transform and remove acceleration + tf2::Matrix3x3 stateTmp; + stateTmp.setRPY(state(StateMemberRoll), + state(StateMemberPitch), + state(StateMemberYaw)); + + // transform state orientation to IMU frame + trans.setBasis(stateTmp * targetFrameTrans.getBasis()); + rotNorm = trans.getBasis().inverse() * normAcc; + } + else + { + tf2::Quaternion curAttitude; + tf2::fromMsg(msg->orientation, curAttitude); + if (fabs(curAttitude.length() - 1.0) > 0.01) + { + ROS_WARN_ONCE("An input was not normalized, this should NOT happen, but will normalize."); + curAttitude.normalize(); + } + trans.setRotation(curAttitude); + if (!relative) + { + // curAttitude is the true world-frame attitude of the sensor + rotNorm = trans.getBasis().inverse() * normAcc; + } + else + { + // curAttitude is relative to the initial pose of the sensor. + // Assumption: IMU sensor is rigidly attached to the base_link (but a static rotation is possible). + rotNorm = targetFrameTrans.getBasis().inverse() * trans.getBasis().inverse() * normAcc; + } + } + accTmp.setX(accTmp.getX() - rotNorm.getX()); + accTmp.setY(accTmp.getY() - rotNorm.getY()); + accTmp.setZ(accTmp.getZ() - rotNorm.getZ()); + + RF_DEBUG("Orientation is " << trans.getRotation() << + "Acceleration due to gravity is " << rotNorm << + "After removing acceleration due to gravity, acceleration is " << accTmp << "\n"); + } + + // Transform to correct frame + tf2::Vector3 stateTwistRot(state(StateMemberVroll), + state(StateMemberVpitch), + state(StateMemberVyaw)); + accTmp = targetFrameTrans.getBasis() * accTmp + targetFrameTrans.getOrigin().cross(angular_acceleration_) + - targetFrameTrans.getOrigin().cross(stateTwistRot).cross(stateTwistRot); + maskAcc = targetFrameTrans.getBasis() * maskAcc; + + // Now use the mask values to determine which update vector values should be true + updateVector[StateMemberAx] = static_cast( + maskAcc.getRow(StateMemberAx - POSITION_A_OFFSET).length() >= 1e-6); + updateVector[StateMemberAy] = static_cast( + maskAcc.getRow(StateMemberAy - POSITION_A_OFFSET).length() >= 1e-6); + updateVector[StateMemberAz] = static_cast( + maskAcc.getRow(StateMemberAz - POSITION_A_OFFSET).length() >= 1e-6); + + RF_DEBUG(msg->header.frame_id << "->" << targetFrame << " transform:\n" << targetFrameTrans << + "\nAfter applying transform to " << targetFrame << ", update vector is:\n" << updateVector << + "\nAfter applying transform to " << targetFrame << ", measurement is:\n" << accTmp << "\n"); + + // 5. Now rotate the covariance: create an augmented + // matrix that contains a 3D rotation matrix in the + // upper-left and lower-right quadrants, and zeros + // elsewhere + tf2::Matrix3x3 rot(targetFrameTrans.getRotation()); + Eigen::MatrixXd rot3d(ACCELERATION_SIZE, ACCELERATION_SIZE); + rot3d.setIdentity(); + + for (size_t rInd = 0; rInd < ACCELERATION_SIZE; ++rInd) + { + rot3d(rInd, 0) = rot.getRow(rInd).getX(); + rot3d(rInd, 1) = rot.getRow(rInd).getY(); + rot3d(rInd, 2) = rot.getRow(rInd).getZ(); + } + + // Carry out the rotation + covarianceRotated = rot3d * covarianceRotated.eval() * rot3d.transpose(); + + RF_DEBUG("Transformed covariance is \n" << covarianceRotated << "\n"); + + // 6. Store our corrected measurement and covariance + measurement(StateMemberAx) = accTmp.getX(); + measurement(StateMemberAy) = accTmp.getY(); + measurement(StateMemberAz) = accTmp.getZ(); + + // Copy the covariances + measurementCovariance.block(POSITION_A_OFFSET, POSITION_A_OFFSET, ACCELERATION_SIZE, ACCELERATION_SIZE) = + covarianceRotated.block(0, 0, ACCELERATION_SIZE, ACCELERATION_SIZE); + + // 7. Handle 2D mode + if (twoDMode_) + { + forceTwoD(measurement, measurementCovariance, updateVector); + } + } + else + { + RF_DEBUG("Could not transform measurement into " << targetFrame << ". Ignoring...\n"); + } + + RF_DEBUG("\n----- /RosFilter::prepareAcceleration(" << topicName << ") ------\n"); + + return canTransform; + } + + template + bool RosFilter::preparePose(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg, + const std::string &topicName, + const std::string &targetFrame, + const std::string &sourceFrame, + const bool differential, + const bool relative, + const bool imuData, + std::vector &updateVector, + Eigen::VectorXd &measurement, + Eigen::MatrixXd &measurementCovariance) + { + bool retVal = false; + + RF_DEBUG("------ RosFilter::preparePose (" << topicName << ") ------\n"); + + // 1. Get the measurement into a tf-friendly transform (pose) object + tf2::Stamped poseTmp; + + // We'll need this later for storing this measurement for differential integration + tf2::Transform curMeasurement; + + // Handle issues where frame_id data is not filled out properly + // @todo: verify that this is necessary still. New IMU handling may + // have rendered this obsolete. + std::string finalTargetFrame; + if (targetFrame == "") + { + if (msg->header.frame_id == "") + { + // Blank target and message frames mean we can just + // use our world_frame + finalTargetFrame = worldFrameId_; + poseTmp.frame_id_ = finalTargetFrame; + } + else + { // (targetFrame == "") + // A blank target frame means we shouldn't bother + // transforming the data + finalTargetFrame = msg->header.frame_id; + poseTmp.frame_id_ = finalTargetFrame; + } + } + else + { + // Otherwise, we should use our target frame + finalTargetFrame = targetFrame; + poseTmp.frame_id_ = (differential && !imuData ? finalTargetFrame : msg->header.frame_id); + } + + RF_DEBUG("Final target frame for " << topicName << " is " << finalTargetFrame << "\n"); + + poseTmp.stamp_ = msg->header.stamp; + + // Fill out the position data + poseTmp.setOrigin(tf2::Vector3(msg->pose.pose.position.x, + msg->pose.pose.position.y, + msg->pose.pose.position.z)); + + tf2::Quaternion orientation; + + // Handle bad (empty) quaternions + if (msg->pose.pose.orientation.x == 0 && msg->pose.pose.orientation.y == 0 && + msg->pose.pose.orientation.z == 0 && msg->pose.pose.orientation.w == 0) + { + orientation.setValue(0.0, 0.0, 0.0, 1.0); + + if (updateVector[StateMemberRoll] || updateVector[StateMemberPitch] || updateVector[StateMemberYaw]) + { + std::stringstream stream; + stream << "The " << topicName << " message contains an invalid orientation quaternion, " << + "but its configuration is such that orientation data is being used. Correcting..."; + + addDiagnostic(diagnostic_msgs::DiagnosticStatus::WARN, + topicName + "_orientation", + stream.str(), + false); + } + } + else + { + tf2::fromMsg(msg->pose.pose.orientation, orientation); + if (fabs(orientation.length() - 1.0) > 0.01) + { + ROS_WARN_ONCE("An input was not normalized, this should NOT happen, but will normalize."); + orientation.normalize(); + } + } + + // Fill out the orientation data + poseTmp.setRotation(orientation); + + // 2. Get the target frame transformation + tf2::Transform targetFrameTrans; + bool canTransform = RosFilterUtilities::lookupTransformSafe(tfBuffer_, + finalTargetFrame, + poseTmp.frame_id_, + poseTmp.stamp_, + tfTimeout_, + targetFrameTrans, + tfSilentFailure_); + + // handling multiple odometry origins: convert to the origin adherent to base_link. + // make pose refer to the baseLinkFrame as source + tf2::Transform sourceFrameTrans; + bool canSrcTransform = false; + if ( sourceFrame != baseLinkFrameId_ ) + { + canSrcTransform = RosFilterUtilities::lookupTransformSafe(tfBuffer_, + sourceFrame, + baseLinkFrameId_, + poseTmp.stamp_, + tfTimeout_, + sourceFrameTrans, + tfSilentFailure_); + } + + // 3. Make sure we can work with this data before carrying on + if (canTransform) + { + /* 4. robot_localization lets users configure which variables from the sensor should be + * fused with the filter. This is specified at the sensor level. However, the data + * may go through transforms before being fused with the state estimate. In that case, + * we need to know which of the transformed variables came from the pre-transformed + * "approved" variables (i.e., the ones that had "true" in their xxx_config parameter). + * To do this, we construct matrices using the update vector values on the diagonals, + * pass this matrix through the rotation, and use the length of each row to determine + * the transformed update vector. The process is slightly different for IMUs, as the + * coordinate frame transform is really the base_link->imu_frame transform, and not + * a transform from some other world-fixed frame (even though the IMU data itself *is* + * reported in a world fixed frame). */ + tf2::Matrix3x3 maskPosition(updateVector[StateMemberX], 0, 0, + 0, updateVector[StateMemberY], 0, + 0, 0, updateVector[StateMemberZ]); + + tf2::Matrix3x3 maskOrientation(updateVector[StateMemberRoll], 0, 0, + 0, updateVector[StateMemberPitch], 0, + 0, 0, updateVector[StateMemberYaw]); + + if (imuData) + { + /* We have to treat IMU orientation data differently. Even though we are dealing with pose + * data when we work with orientations, for IMUs, the frame_id is the frame in which the + * sensor is mounted, and not the coordinate frame of the IMU. Imagine an IMU that is mounted + * facing sideways. The pitch in the IMU frame becomes roll for the vehicle. This means that + * we need to rotate roll and pitch angles by the IMU's mounting yaw offset, and we must apply + * similar treatment to its update mask and covariance. */ + + double dummy, yaw; + targetFrameTrans.getBasis().getRPY(dummy, dummy, yaw); + tf2::Matrix3x3 transTmp; + transTmp.setRPY(0.0, 0.0, yaw); + + maskPosition = transTmp * maskPosition; + maskOrientation = transTmp * maskOrientation; + } + else + { + maskPosition = targetFrameTrans.getBasis() * maskPosition; + maskOrientation = targetFrameTrans.getBasis() * maskOrientation; + } + + // Now copy the mask values back into the update vector: any row with a significant vector length + // indicates that we want to set that variable to true in the update vector. + updateVector[StateMemberX] = static_cast( + maskPosition.getRow(StateMemberX - POSITION_OFFSET).length() >= 1e-6); + updateVector[StateMemberY] = static_cast( + maskPosition.getRow(StateMemberY - POSITION_OFFSET).length() >= 1e-6); + updateVector[StateMemberZ] = static_cast( + maskPosition.getRow(StateMemberZ - POSITION_OFFSET).length() >= 1e-6); + updateVector[StateMemberRoll] = static_cast( + maskOrientation.getRow(StateMemberRoll - ORIENTATION_OFFSET).length() >= 1e-6); + updateVector[StateMemberPitch] = static_cast( + maskOrientation.getRow(StateMemberPitch - ORIENTATION_OFFSET).length() >= 1e-6); + updateVector[StateMemberYaw] = static_cast( + maskOrientation.getRow(StateMemberYaw - ORIENTATION_OFFSET).length() >= 1e-6); + + // 5a. We'll need to rotate the covariance as well. Create a container and copy over the + // covariance data + Eigen::MatrixXd covariance(POSE_SIZE, POSE_SIZE); + covariance.setZero(); + copyCovariance(&(msg->pose.covariance[0]), covariance, topicName, updateVector, POSITION_OFFSET, POSE_SIZE); + + // 5b. Now rotate the covariance: create an augmented matrix that + // contains a 3D rotation matrix in the upper-left and lower-right + // quadrants, with zeros elsewhere. + tf2::Matrix3x3 rot; + Eigen::MatrixXd rot6d(POSE_SIZE, POSE_SIZE); + rot6d.setIdentity(); + Eigen::MatrixXd covarianceRotated; + + // Transform pose covariance due to a different pose source origin + if (canSrcTransform) + { + rot.setRotation(sourceFrameTrans.getRotation()); + for (size_t rInd = 0; rInd < POSITION_SIZE; ++rInd) + { + rot6d(rInd, 0) = rot.getRow(rInd).getX(); + rot6d(rInd, 1) = rot.getRow(rInd).getY(); + rot6d(rInd, 2) = rot.getRow(rInd).getZ(); + rot6d(rInd+POSITION_SIZE, 3) = rot.getRow(rInd).getX(); + rot6d(rInd+POSITION_SIZE, 4) = rot.getRow(rInd).getY(); + rot6d(rInd+POSITION_SIZE, 5) = rot.getRow(rInd).getZ(); + } + // since the transformation is a post-multiply + covariance = rot6d.transpose() * covariance.eval() * rot6d; + } + + rot6d.setIdentity(); + + if (imuData) + { + // Apply the same special logic to the IMU covariance rotation + double dummy, yaw; + targetFrameTrans.getBasis().getRPY(dummy, dummy, yaw); + rot.setRPY(0.0, 0.0, yaw); + } + else + { + rot.setRotation(targetFrameTrans.getRotation()); + } + + for (size_t rInd = 0; rInd < POSITION_SIZE; ++rInd) + { + rot6d(rInd, 0) = rot.getRow(rInd).getX(); + rot6d(rInd, 1) = rot.getRow(rInd).getY(); + rot6d(rInd, 2) = rot.getRow(rInd).getZ(); + rot6d(rInd+POSITION_SIZE, 3) = rot.getRow(rInd).getX(); + rot6d(rInd+POSITION_SIZE, 4) = rot.getRow(rInd).getY(); + rot6d(rInd+POSITION_SIZE, 5) = rot.getRow(rInd).getZ(); + } + + // Now carry out the rotation + covarianceRotated = rot6d * covariance * rot6d.transpose(); + + RF_DEBUG("After rotating into the " << finalTargetFrame << + " frame, covariance is \n" << covarianceRotated << "\n"); + + /* 6a. For IMU data, the transform that we get is the transform from the body + * frame of the robot (e.g., base_link) to the mounting frame of the robot. It + * is *not* the coordinate frame in which the IMU orientation data is reported. + * If the IMU is mounted in a non-neutral orientation, we need to remove those + * offsets, and then we need to potentially "swap" roll and pitch. + * Note that this transform does NOT handle NED->ENU conversions. Data is assumed + * to be in the ENU frame when it is received. + * */ + if (imuData) + { + // First, convert the transform and measurement rotation to RPY + // @todo: There must be a way to handle this with quaternions. Need to look into it. + double rollOffset = 0; + double pitchOffset = 0; + double yawOffset = 0; + double roll = 0; + double pitch = 0; + double yaw = 0; + RosFilterUtilities::quatToRPY(targetFrameTrans.getRotation(), rollOffset, pitchOffset, yawOffset); + RosFilterUtilities::quatToRPY(poseTmp.getRotation(), roll, pitch, yaw); + + // 6b. Apply the offset (making sure to bound them), and throw them in a vector + tf2::Vector3 rpyAngles(FilterUtilities::clampRotation(roll - rollOffset), + FilterUtilities::clampRotation(pitch - pitchOffset), + FilterUtilities::clampRotation(yaw - yawOffset)); + + // 6c. Now we need to rotate the roll and pitch by the yaw offset value. + // Imagine a case where an IMU is mounted facing sideways. In that case + // pitch for the IMU's world frame is roll for the robot. + tf2::Matrix3x3 mat; + mat.setRPY(0.0, 0.0, yawOffset); + rpyAngles = mat * rpyAngles; + poseTmp.getBasis().setRPY(rpyAngles.getX(), rpyAngles.getY(), rpyAngles.getZ()); + + // We will use this target transformation later on, but + // we've already transformed this data as if the IMU + // were mounted neutrall on the robot, so we can just + // make the transform the identity. + targetFrameTrans.setIdentity(); + } + + // 7. Two cases: if we're in differential mode, we need to generate a twist + // message. Otherwise, we just transform it to the target frame. + if (differential) + { + bool success = false; + + // We're going to be playing with poseTmp, so store it, + // as we'll need to save its current value for the next + // measurement. + curMeasurement = poseTmp; + + // Make sure we have previous measurements to work with + if (previousMeasurements_.count(topicName) > 0 && previousMeasurementCovariances_.count(topicName) > 0) + { + // 7a. If we are carrying out differential integration and + // we have a previous measurement for this sensor,then we + // need to apply the inverse of that measurement to this new + // measurement to produce a "delta" measurement between the two. + // Even if we're not using all of the variables from this sensor, + // we need to use the whole measurement to determine the delta + // to the new measurement + tf2::Transform prevMeasurement = previousMeasurements_[topicName]; + poseTmp.setData(prevMeasurement.inverseTimes(poseTmp)); + + RF_DEBUG("Previous measurement:\n" << previousMeasurements_[topicName] << + "\nAfter removing previous measurement, measurement delta is:\n" << poseTmp << "\n"); + + // 7b. Now we we have a measurement delta in the frame_id of the + // message, but we want that delta to be in the target frame, so + // we need to apply the rotation of the target frame transform. + targetFrameTrans.setOrigin(tf2::Vector3(0.0, 0.0, 0.0)); + poseTmp.mult(targetFrameTrans, poseTmp); + + RF_DEBUG("After rotating to the target frame, measurement delta is:\n" << poseTmp << "\n"); + + // 7c. Now use the time difference from the last message to compute + // translational and rotational velocities + double dt = msg->header.stamp.toSec() - lastMessageTimes_[topicName].toSec(); + double xVel = poseTmp.getOrigin().getX() / dt; + double yVel = poseTmp.getOrigin().getY() / dt; + double zVel = poseTmp.getOrigin().getZ() / dt; + + double rollVel = 0; + double pitchVel = 0; + double yawVel = 0; + + RosFilterUtilities::quatToRPY(poseTmp.getRotation(), rollVel, pitchVel, yawVel); + rollVel /= dt; + pitchVel /= dt; + yawVel /= dt; + + RF_DEBUG("Previous message time was " << lastMessageTimes_[topicName].toSec() << + ", current message time is " << msg->header.stamp.toSec() << ", delta is " << + dt << ", velocity is (vX, vY, vZ): (" << xVel << ", " << yVel << ", " << zVel << + ")\n" << "(vRoll, vPitch, vYaw): (" << rollVel << ", " << pitchVel << ", " << + yawVel << ")\n"); + + // 7d. Fill out the velocity data in the message + geometry_msgs::TwistWithCovarianceStamped *twistPtr = new geometry_msgs::TwistWithCovarianceStamped(); + twistPtr->header = msg->header; + twistPtr->header.frame_id = sourceFrame; + twistPtr->twist.twist.linear.x = xVel; + twistPtr->twist.twist.linear.y = yVel; + twistPtr->twist.twist.linear.z = zVel; + twistPtr->twist.twist.angular.x = rollVel; + twistPtr->twist.twist.angular.y = pitchVel; + twistPtr->twist.twist.angular.z = yawVel; + std::vector twistUpdateVec(STATE_SIZE, false); + std::copy(updateVector.begin() + POSITION_OFFSET, + updateVector.begin() + POSE_SIZE, + twistUpdateVec.begin() + POSITION_V_OFFSET); + std::copy(twistUpdateVec.begin(), twistUpdateVec.end(), updateVector.begin()); + geometry_msgs::TwistWithCovarianceStampedConstPtr ptr(twistPtr); + + // 7e. Now rotate the previous covariance for this measurement to get it + // into the target frame, and add the current measurement's rotated covariance + // to the previous measurement's rotated covariance, and multiply by the time delta. + Eigen::MatrixXd prevCovarRotated = rot6d * previousMeasurementCovariances_[topicName] * rot6d.transpose(); + covarianceRotated = (covarianceRotated.eval() + prevCovarRotated) * dt; + copyCovariance(covarianceRotated, &(twistPtr->twist.covariance[0]), POSE_SIZE); + + RF_DEBUG("Previous measurement covariance:\n" << previousMeasurementCovariances_[topicName] << + "\nPrevious measurement covariance rotated:\n" << prevCovarRotated << + "\nFinal twist covariance:\n" << covarianceRotated << "\n"); + + // Now pass this on to prepareTwist, which will convert it to the required frame + success = prepareTwist(ptr, + topicName + "_twist", + baseLinkFrameId_, + updateVector, + measurement, + measurementCovariance); + } + + // 7f. Update the previous measurement and measurement covariance + previousMeasurements_[topicName] = curMeasurement; + previousMeasurementCovariances_[topicName] = covariance; + + retVal = success; + } + else + { + // make pose refer to the baseLinkFrame as source + // canSrcTransform == true => ( sourceFrame != baseLinkFrameId_ ) + if (canSrcTransform) + { + poseTmp.setData(poseTmp * sourceFrameTrans); + } + + // 7g. If we're in relative mode, remove the initial measurement + if (relative) + { + if (initialMeasurements_.count(topicName) == 0) + { + initialMeasurements_.insert(std::pair(topicName, poseTmp)); + } + + tf2::Transform initialMeasurement = initialMeasurements_[topicName]; + poseTmp.setData(initialMeasurement.inverseTimes(poseTmp)); + } + + // 7h. Apply the target frame transformation to the pose object. + poseTmp.mult(targetFrameTrans, poseTmp); + poseTmp.frame_id_ = finalTargetFrame; + + // 7i. Finally, copy everything into our measurement and covariance objects + measurement(StateMemberX) = poseTmp.getOrigin().x(); + measurement(StateMemberY) = poseTmp.getOrigin().y(); + measurement(StateMemberZ) = poseTmp.getOrigin().z(); + + // The filter needs roll, pitch, and yaw values instead of quaternions + double roll, pitch, yaw; + RosFilterUtilities::quatToRPY(poseTmp.getRotation(), roll, pitch, yaw); + measurement(StateMemberRoll) = roll; + measurement(StateMemberPitch) = pitch; + measurement(StateMemberYaw) = yaw; + + measurementCovariance.block(0, 0, POSE_SIZE, POSE_SIZE) = covarianceRotated.block(0, 0, POSE_SIZE, POSE_SIZE); + + // 8. Handle 2D mode + if (twoDMode_) + { + forceTwoD(measurement, measurementCovariance, updateVector); + } + + retVal = true; + } + } + else + { + retVal = false; + + RF_DEBUG("Could not transform measurement into " << finalTargetFrame << ". Ignoring..."); + } + + RF_DEBUG("\n----- /RosFilter::preparePose (" << topicName << ") ------\n"); + + return retVal; + } + + template + bool RosFilter::prepareTwist(const geometry_msgs::TwistWithCovarianceStamped::ConstPtr &msg, + const std::string &topicName, + const std::string &targetFrame, + std::vector &updateVector, + Eigen::VectorXd &measurement, + Eigen::MatrixXd &measurementCovariance) + { + RF_DEBUG("------ RosFilter::prepareTwist (" << topicName << ") ------\n"); + + // 1. Get the measurement into two separate vector objects. + tf2::Vector3 twistLin(msg->twist.twist.linear.x, + msg->twist.twist.linear.y, + msg->twist.twist.linear.z); + tf2::Vector3 measTwistRot(msg->twist.twist.angular.x, + msg->twist.twist.angular.y, + msg->twist.twist.angular.z); + + // 1a. This sensor may or may not measure rotational velocity. Regardless, + // if it measures linear velocity, then later on, we'll need to remove "false" + // linear velocity resulting from angular velocity and the translational offset + // of the sensor from the vehicle origin. + const Eigen::VectorXd &state = filter_.getState(); + tf2::Vector3 stateTwistRot(state(StateMemberVroll), + state(StateMemberVpitch), + state(StateMemberVyaw)); + + // Determine the frame_id of the data + std::string msgFrame = (msg->header.frame_id == "" ? targetFrame : msg->header.frame_id); + + // 2. robot_localization lets users configure which variables from the sensor should be + // fused with the filter. This is specified at the sensor level. However, the data + // may go through transforms before being fused with the state estimate. In that case, + // we need to know which of the transformed variables came from the pre-transformed + // "approved" variables (i.e., the ones that had "true" in their xxx_config parameter). + // To do this, we construct matrices using the update vector values on the diagonals, + // pass this matrix through the rotation, and use the length of each row to determine + // the transformed update vector. + tf2::Matrix3x3 maskLin(updateVector[StateMemberVx], 0, 0, + 0, updateVector[StateMemberVy], 0, + 0, 0, updateVector[StateMemberVz]); + + tf2::Matrix3x3 maskRot(updateVector[StateMemberVroll], 0, 0, + 0, updateVector[StateMemberVpitch], 0, + 0, 0, updateVector[StateMemberVyaw]); + + // 3. We'll need to rotate the covariance as well + Eigen::MatrixXd covarianceRotated(TWIST_SIZE, TWIST_SIZE); + covarianceRotated.setZero(); + + copyCovariance(&(msg->twist.covariance[0]), + covarianceRotated, + topicName, + updateVector, + POSITION_V_OFFSET, + TWIST_SIZE); + + RF_DEBUG("Original measurement as tf object:\nLinear: " << twistLin << + "Rotational: " << measTwistRot << + "\nOriginal update vector:\n" << updateVector << + "\nOriginal covariance matrix:\n" << covarianceRotated << "\n"); + + // 4. We need to transform this into the target frame (probably base_link) + tf2::Transform targetFrameTrans; + bool canTransform = RosFilterUtilities::lookupTransformSafe(tfBuffer_, + targetFrame, + msgFrame, + msg->header.stamp, + tfTimeout_, + targetFrameTrans, + tfSilentFailure_); + + if (canTransform) + { + // Transform to correct frame. Note that we can get linear velocity + // as a result of the sensor offset and rotational velocity + measTwistRot = targetFrameTrans.getBasis() * measTwistRot; + twistLin = targetFrameTrans.getBasis() * twistLin + targetFrameTrans.getOrigin().cross(stateTwistRot); + maskLin = targetFrameTrans.getBasis() * maskLin; + maskRot = targetFrameTrans.getBasis() * maskRot; + + // Now copy the mask values back into the update vector + updateVector[StateMemberVx] = static_cast( + maskLin.getRow(StateMemberVx - POSITION_V_OFFSET).length() >= 1e-6); + updateVector[StateMemberVy] = static_cast( + maskLin.getRow(StateMemberVy - POSITION_V_OFFSET).length() >= 1e-6); + updateVector[StateMemberVz] = static_cast( + maskLin.getRow(StateMemberVz - POSITION_V_OFFSET).length() >= 1e-6); + updateVector[StateMemberVroll] = static_cast( + maskRot.getRow(StateMemberVroll - ORIENTATION_V_OFFSET).length() >= 1e-6); + updateVector[StateMemberVpitch] = static_cast( + maskRot.getRow(StateMemberVpitch - ORIENTATION_V_OFFSET).length() >= 1e-6); + updateVector[StateMemberVyaw] = static_cast( + maskRot.getRow(StateMemberVyaw - ORIENTATION_V_OFFSET).length() >= 1e-6); + + RF_DEBUG(msg->header.frame_id << "->" << targetFrame << " transform:\n" << targetFrameTrans << + "\nAfter applying transform to " << targetFrame << ", update vector is:\n" << updateVector << + "\nAfter applying transform to " << targetFrame << ", measurement is:\n" << + "Linear: " << twistLin << "Rotational: " << measTwistRot << "\n"); + + // 5. Now rotate the covariance: create an augmented + // matrix that contains a 3D rotation matrix in the + // upper-left and lower-right quadrants, and zeros + // elsewhere + tf2::Matrix3x3 rot(targetFrameTrans.getRotation()); + Eigen::MatrixXd rot6d(TWIST_SIZE, TWIST_SIZE); + rot6d.setIdentity(); + + for (size_t rInd = 0; rInd < POSITION_SIZE; ++rInd) + { + rot6d(rInd, 0) = rot.getRow(rInd).getX(); + rot6d(rInd, 1) = rot.getRow(rInd).getY(); + rot6d(rInd, 2) = rot.getRow(rInd).getZ(); + rot6d(rInd+POSITION_SIZE, 3) = rot.getRow(rInd).getX(); + rot6d(rInd+POSITION_SIZE, 4) = rot.getRow(rInd).getY(); + rot6d(rInd+POSITION_SIZE, 5) = rot.getRow(rInd).getZ(); + } + + // Carry out the rotation + covarianceRotated = rot6d * covarianceRotated.eval() * rot6d.transpose(); + + RF_DEBUG("Transformed covariance is \n" << covarianceRotated << "\n"); + + // 6. Store our corrected measurement and covariance + measurement(StateMemberVx) = twistLin.getX(); + measurement(StateMemberVy) = twistLin.getY(); + measurement(StateMemberVz) = twistLin.getZ(); + measurement(StateMemberVroll) = measTwistRot.getX(); + measurement(StateMemberVpitch) = measTwistRot.getY(); + measurement(StateMemberVyaw) = measTwistRot.getZ(); + + // Copy the covariances + measurementCovariance.block(POSITION_V_OFFSET, POSITION_V_OFFSET, TWIST_SIZE, TWIST_SIZE) = + covarianceRotated.block(0, 0, TWIST_SIZE, TWIST_SIZE); + + // 7. Handle 2D mode + if (twoDMode_) + { + forceTwoD(measurement, measurementCovariance, updateVector); + } + } + else + { + RF_DEBUG("Could not transform measurement into " << targetFrame << ". Ignoring..."); + } + + RF_DEBUG("\n----- /RosFilter::prepareTwist (" << topicName << ") ------\n"); + + return canTransform; + } + + template + void RosFilter::saveFilterState(FilterBase& filter) + { + FilterStatePtr state = FilterStatePtr(new FilterState()); + state->state_ = Eigen::VectorXd(filter.getState()); + state->estimateErrorCovariance_ = Eigen::MatrixXd(filter.getEstimateErrorCovariance()); + state->lastMeasurementTime_ = filter.getLastMeasurementTime(); + state->latestControl_ = Eigen::VectorXd(filter.getControl()); + state->latestControlTime_ = filter.getControlTime(); + filterStateHistory_.push_back(state); + RF_DEBUG("Saved state with timestamp " << std::setprecision(20) << state->lastMeasurementTime_ << + " to history. " << filterStateHistory_.size() << " measurements are in the queue.\n"); + } + + template + bool RosFilter::revertTo(const double time) + { + RF_DEBUG("\n----- RosFilter::revertTo -----\n"); + RF_DEBUG("\nRequested time was " << std::setprecision(20) << time << "\n") + + size_t history_size = filterStateHistory_.size(); + + // Walk back through the queue until we reach a filter state whose time stamp is less than or equal to the + // requested time. Since every saved state after that time will be overwritten/corrected, we can pop from + // the queue. If the history is insufficiently short, we just take the oldest state we have. + FilterStatePtr lastHistoryState; + while (!filterStateHistory_.empty() && filterStateHistory_.back()->lastMeasurementTime_ > time) + { + lastHistoryState = filterStateHistory_.back(); + filterStateHistory_.pop_back(); + } + + // If the state history is not empty at this point, it means that our history was large enough, and we + // should revert to the state at the back of the history deque. + bool retVal = false; + if (!filterStateHistory_.empty()) + { + retVal = true; + lastHistoryState = filterStateHistory_.back(); + } + else + { + RF_DEBUG("Insufficient history to revert to time " << time << "\n"); + + if (lastHistoryState) + { + RF_DEBUG("Will revert to oldest state at " << lastHistoryState->latestControlTime_ << ".\n"); + ROS_WARN_STREAM_DELAYED_THROTTLE(historyLength_, "Could not revert to state with time " << + std::setprecision(20) << time << ". Instead reverted to state with time " << + lastHistoryState->lastMeasurementTime_ << ". History size was " << history_size); + } + } + + // If we have a valid reversion state, revert + if (lastHistoryState) + { + // Reset filter to the latest state from the queue. + const FilterStatePtr &state = lastHistoryState; + filter_.setState(state->state_); + filter_.setEstimateErrorCovariance(state->estimateErrorCovariance_); + filter_.setLastMeasurementTime(state->lastMeasurementTime_); + + RF_DEBUG("Reverted to state with time " << std::setprecision(20) << state->lastMeasurementTime_ << "\n"); + + // Repeat for measurements, but push every measurement onto the measurement queue as we go + int restored_measurements = 0; + while (!measurementHistory_.empty() && measurementHistory_.back()->time_ > time) + { + // Don't need to restore measurements that predate our earliest state time + if (state->lastMeasurementTime_ <= measurementHistory_.back()->time_) + { + measurementQueue_.push(measurementHistory_.back()); + restored_measurements++; + } + + measurementHistory_.pop_back(); + } + + RF_DEBUG("Restored " << restored_measurements << " to measurement queue.\n"); + } + + RF_DEBUG("\n----- /RosFilter::revertTo\n"); + + return retVal; + } + + template + bool RosFilter::validateFilterOutput(const nav_msgs::Odometry &message) + { + return !std::isnan(message.pose.pose.position.x) && !std::isinf(message.pose.pose.position.x) && + !std::isnan(message.pose.pose.position.y) && !std::isinf(message.pose.pose.position.y) && + !std::isnan(message.pose.pose.position.z) && !std::isinf(message.pose.pose.position.z) && + !std::isnan(message.pose.pose.orientation.x) && !std::isinf(message.pose.pose.orientation.x) && + !std::isnan(message.pose.pose.orientation.y) && !std::isinf(message.pose.pose.orientation.y) && + !std::isnan(message.pose.pose.orientation.z) && !std::isinf(message.pose.pose.orientation.z) && + !std::isnan(message.pose.pose.orientation.w) && !std::isinf(message.pose.pose.orientation.w) && + !std::isnan(message.twist.twist.linear.x) && !std::isinf(message.twist.twist.linear.x) && + !std::isnan(message.twist.twist.linear.y) && !std::isinf(message.twist.twist.linear.y) && + !std::isnan(message.twist.twist.linear.z) && !std::isinf(message.twist.twist.linear.z) && + !std::isnan(message.twist.twist.angular.x) && !std::isinf(message.twist.twist.angular.x) && + !std::isnan(message.twist.twist.angular.y) && !std::isinf(message.twist.twist.angular.y) && + !std::isnan(message.twist.twist.angular.z) && !std::isinf(message.twist.twist.angular.z); + } + + template + void RosFilter::clearExpiredHistory(const double cutOffTime) + { + RF_DEBUG("\n----- RosFilter::clearExpiredHistory -----" << + "\nCutoff time is " << cutOffTime << "\n"); + + int poppedMeasurements = 0; + int poppedStates = 0; + + while (!measurementHistory_.empty() && measurementHistory_.front()->time_ < cutOffTime) + { + measurementHistory_.pop_front(); + poppedMeasurements++; + } + + while (!filterStateHistory_.empty() && filterStateHistory_.front()->lastMeasurementTime_ < cutOffTime) + { + filterStateHistory_.pop_front(); + poppedStates++; + } + + RF_DEBUG("\nPopped " << poppedMeasurements << " measurements and " << + poppedStates << " states from their respective queues." << + "\n---- /RosFilter::clearExpiredHistory ----\n"); + } + + template + void RosFilter::clearMeasurementQueue() + { + while (!measurementQueue_.empty() && ros::ok()) + { + measurementQueue_.pop(); + } + return; + } +} // namespace RobotLocalization + +// Instantiations of classes is required when template class code +// is placed in a .cpp file. +template class RobotLocalization::RosFilter; +template class RobotLocalization::RosFilter; diff --git a/Localizations/Packages/robot_localization/src/ros_filter_utilities.cpp b/Localizations/Packages/robot_localization/src/ros_filter_utilities.cpp new file mode 100644 index 0000000..e1a5bfb --- /dev/null +++ b/Localizations/Packages/robot_localization/src/ros_filter_utilities.cpp @@ -0,0 +1,195 @@ +/* + * Copyright (c) 2014, 2015, 2016, Charles River Analytics, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "robot_localization/ros_filter_utilities.h" +#include "robot_localization/filter_common.h" + +#include +#include + +#include +#include + +std::ostream& operator<<(std::ostream& os, const tf2::Vector3 &vec) +{ + os << "(" << std::setprecision(20) << vec.getX() << " " << vec.getY() << " " << vec.getZ() << ")\n"; + + return os; +} + +std::ostream& operator<<(std::ostream& os, const tf2::Quaternion &quat) +{ + double roll, pitch, yaw; + tf2::Matrix3x3 orTmp(quat); + orTmp.getRPY(roll, pitch, yaw); + + os << "(" << std::setprecision(20) << roll << ", " << pitch << ", " << yaw << ")\n"; + + return os; +} + +std::ostream& operator<<(std::ostream& os, const tf2::Transform &trans) +{ + os << "Origin: " << trans.getOrigin() << + "Rotation (RPY): " << trans.getRotation(); + + return os; +} + +std::ostream& operator<<(std::ostream& os, const std::vector &vec) +{ + os << "(" << std::setprecision(20); + + for (size_t i = 0; i < vec.size(); ++i) + { + os << vec[i] << " "; + } + + os << ")\n"; + + return os; +} + +namespace RobotLocalization +{ +namespace RosFilterUtilities +{ + + double getYaw(const tf2::Quaternion quat) + { + tf2::Matrix3x3 mat(quat); + + double dummy; + double yaw; + mat.getRPY(dummy, dummy, yaw); + + return yaw; + } + + bool lookupTransformSafe(const tf2_ros::Buffer &buffer, + const std::string &targetFrame, + const std::string &sourceFrame, + const ros::Time &time, + const ros::Duration &timeout, + tf2::Transform &targetFrameTrans, + const bool silent) + { + bool retVal = true; + + // First try to transform the data at the requested time + try + { + tf2::fromMsg(buffer.lookupTransform(targetFrame, sourceFrame, time, timeout).transform, + targetFrameTrans); + } + catch (tf2::TransformException &ex) + { + // The issue might be that the transforms that are available are not close + // enough temporally to be used. In that case, just use the latest available + // transform and warn the user. + try + { + tf2::fromMsg(buffer.lookupTransform(targetFrame, sourceFrame, ros::Time(0)).transform, + targetFrameTrans); + + if (!silent) + { + ROS_WARN_STREAM_THROTTLE(2.0, "Transform from " << sourceFrame << " to " << targetFrame << + " was unavailable for the time requested. Using latest instead.\n"); + } + } + catch(tf2::TransformException &ex) + { + if (!silent) + { + ROS_WARN_STREAM_THROTTLE(2.0, "Could not obtain transform from " << sourceFrame << + " to " << targetFrame << ". Error was " << ex.what() << "\n"); + } + + retVal = false; + } + } + + // Transforming from a frame id to itself can fail when the tf tree isn't + // being broadcast (e.g., for some bag files). This is the only failure that + // would throw an exception, so check for this situation before giving up. + if (!retVal) + { + if (targetFrame == sourceFrame) + { + targetFrameTrans.setIdentity(); + retVal = true; + } + } + + return retVal; + } + + bool lookupTransformSafe(const tf2_ros::Buffer &buffer, + const std::string &targetFrame, + const std::string &sourceFrame, + const ros::Time &time, + tf2::Transform &targetFrameTrans, + const bool silent) + { + return lookupTransformSafe(buffer, targetFrame, sourceFrame, time, ros::Duration(0), targetFrameTrans, silent); + } + + void quatToRPY(const tf2::Quaternion &quat, double &roll, double &pitch, double &yaw) + { + tf2::Matrix3x3 orTmp(quat); + orTmp.getRPY(roll, pitch, yaw); + } + + void stateToTF(const Eigen::VectorXd &state, tf2::Transform &stateTF) + { + stateTF.setOrigin(tf2::Vector3(state(StateMemberX), + state(StateMemberY), + state(StateMemberZ))); + tf2::Quaternion quat; + quat.setRPY(state(StateMemberRoll), + state(StateMemberPitch), + state(StateMemberYaw)); + + stateTF.setRotation(quat); + } + + void TFtoState(const tf2::Transform &stateTF, Eigen::VectorXd &state) + { + state(StateMemberX) = stateTF.getOrigin().getX(); + state(StateMemberY) = stateTF.getOrigin().getY(); + state(StateMemberZ) = stateTF.getOrigin().getZ(); + quatToRPY(stateTF.getRotation(), state(StateMemberRoll), state(StateMemberPitch), state(StateMemberYaw)); + } + +} // namespace RosFilterUtilities +} // namespace RobotLocalization diff --git a/Localizations/Packages/robot_localization/src/ros_robot_localization_listener.cpp b/Localizations/Packages/robot_localization/src/ros_robot_localization_listener.cpp new file mode 100644 index 0000000..85e5465 --- /dev/null +++ b/Localizations/Packages/robot_localization/src/ros_robot_localization_listener.cpp @@ -0,0 +1,539 @@ +/* + * Copyright (c) 2016, TNO IVS Helmond. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "robot_localization/ros_robot_localization_listener.h" +#include "robot_localization/ros_filter_utilities.h" + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include + +namespace RobotLocalization +{ + +FilterType filterTypeFromString(const std::string& filter_type_str) +{ + if ( filter_type_str == "ekf" ) + { + return FilterTypes::EKF; + } + else if ( filter_type_str == "ukf" ) + { + return FilterTypes::UKF; + } + else + { + return FilterTypes::NotDefined; + } +} + +RosRobotLocalizationListener::RosRobotLocalizationListener(): + nh_p_("robot_localization"), + odom_sub_(nh_, "odometry/filtered", 1), + accel_sub_(nh_, "accel/filtered", 1), + sync_(odom_sub_, accel_sub_, 10), + base_frame_id_(""), + world_frame_id_(""), + tf_listener_(tf_buffer_) +{ + int buffer_size; + nh_p_.param("buffer_size", buffer_size, 10); + + std::string param_ns; + nh_p_.param("parameter_namespace", param_ns, nh_p_.getNamespace()); + + ros::NodeHandle nh_param(param_ns); + + std::string filter_type_str; + nh_param.param("filter_type", filter_type_str, std::string("ekf")); + FilterType filter_type = filterTypeFromString(filter_type_str); + if ( filter_type == FilterTypes::NotDefined ) + { + ROS_ERROR("RosRobotLocalizationListener: Parameter filter_type invalid"); + return; + } + + // Load up the process noise covariance (from the launch file/parameter server) + // TODO(reinzor): this code is copied from ros_filter. In a refactor, this could be moved to a function in + // ros_filter_utilities + Eigen::MatrixXd process_noise_covariance(STATE_SIZE, STATE_SIZE); + process_noise_covariance.setZero(); + XmlRpc::XmlRpcValue process_noise_covar_config; + + if (!nh_param.hasParam("process_noise_covariance")) + { + ROS_FATAL_STREAM("Process noise covariance not found in the robot localization listener config (namespace " << + nh_param.getNamespace() << ")! Use the ~parameter_namespace to specify the parameter namespace."); + } + else + { + try + { + nh_param.getParam("process_noise_covariance", process_noise_covar_config); + + ROS_ASSERT(process_noise_covar_config.getType() == XmlRpc::XmlRpcValue::TypeArray); + + int mat_size = process_noise_covariance.rows(); + + for (int i = 0; i < mat_size; i++) + { + for (int j = 0; j < mat_size; j++) + { + try + { + // These matrices can cause problems if all the types + // aren't specified with decimal points. Handle that + // using string streams. + std::ostringstream ostr; + process_noise_covar_config[mat_size * i + j].write(ostr); + std::istringstream istr(ostr.str()); + istr >> process_noise_covariance(i, j); + } + catch(XmlRpc::XmlRpcException &e) + { + throw e; + } + catch(...) + { + throw; + } + } + } + + ROS_DEBUG_STREAM("Process noise covariance is:\n" << process_noise_covariance << "\n"); + } + catch (XmlRpc::XmlRpcException &e) + { + ROS_ERROR_STREAM("ERROR reading robot localization listener config: " << + e.getMessage() << + " for process_noise_covariance (type: " << + process_noise_covar_config.getType() << ")"); + } + } + + std::vector filter_args; + nh_param.param("filter_args", filter_args, std::vector()); + + estimator_ = new RobotLocalizationEstimator(buffer_size, filter_type, process_noise_covariance, filter_args); + + sync_.registerCallback(&RosRobotLocalizationListener::odomAndAccelCallback, this); + + ROS_INFO_STREAM("Ros Robot Localization Listener: Listening to topics " << + odom_sub_.getTopic() << " and " << accel_sub_.getTopic()); + + // Wait until the base and world frames are set by the incoming messages + while (ros::ok() && base_frame_id_.empty()) + { + ros::spinOnce(); + ROS_INFO_STREAM_THROTTLE(1.0, "Ros Robot Localization Listener: Waiting for incoming messages on topics " << + odom_sub_.getTopic() << " and " << accel_sub_.getTopic()); + ros::Duration(0.1).sleep(); + } +} + +RosRobotLocalizationListener::~RosRobotLocalizationListener() +{ + delete estimator_; +} + +void RosRobotLocalizationListener::odomAndAccelCallback(const nav_msgs::Odometry& odom, + const geometry_msgs::AccelWithCovarianceStamped& accel) +{ + // Instantiate a state that can be added to the robot localization estimator + EstimatorState state; + + // Set its time stamp and the state received from the messages + state.time_stamp = odom.header.stamp.toSec(); + + // Get the base frame id from the odom message + if ( base_frame_id_.empty() ) + base_frame_id_ = odom.child_frame_id; + + // Get the world frame id from the odom message + if ( world_frame_id_.empty() ) + world_frame_id_ = odom.header.frame_id; + + // Pose: Position + state.state(StateMemberX) = odom.pose.pose.position.x; + state.state(StateMemberY) = odom.pose.pose.position.y; + state.state(StateMemberZ) = odom.pose.pose.position.z; + + // Pose: Orientation + tf2::Quaternion orientation_quat; + tf2::fromMsg(odom.pose.pose.orientation, orientation_quat); + double roll, pitch, yaw; + RosFilterUtilities::quatToRPY(orientation_quat, roll, pitch, yaw); + + state.state(StateMemberRoll) = roll; + state.state(StateMemberPitch) = pitch; + state.state(StateMemberYaw) = yaw; + + // Pose: Covariance + for ( unsigned int i = 0; i < POSE_SIZE; i++ ) + { + for ( unsigned int j = 0; j < POSE_SIZE; j++ ) + { + state.covariance(POSITION_OFFSET + i, POSITION_OFFSET + j) = odom.pose.covariance[i*POSE_SIZE + j]; + } + } + + // Velocity: Linear + state.state(StateMemberVx) = odom.twist.twist.linear.x; + state.state(StateMemberVy) = odom.twist.twist.linear.y; + state.state(StateMemberVz) = odom.twist.twist.linear.z; + + // Velocity: Angular + state.state(StateMemberVroll) = odom.twist.twist.angular.x; + state.state(StateMemberVpitch) = odom.twist.twist.angular.y; + state.state(StateMemberVyaw) = odom.twist.twist.angular.z; + + // Velocity: Covariance + for ( unsigned int i = 0; i < TWIST_SIZE; i++ ) + { + for ( unsigned int j = 0; j < TWIST_SIZE; j++ ) + { + state.covariance(POSITION_V_OFFSET + i, POSITION_V_OFFSET + j) = odom.twist.covariance[i*TWIST_SIZE + j]; + } + } + + // Acceleration: Linear + state.state(StateMemberAx) = accel.accel.accel.linear.x; + state.state(StateMemberAy) = accel.accel.accel.linear.y; + state.state(StateMemberAz) = accel.accel.accel.linear.z; + + // Acceleration: Angular is not available in state + + // Acceleration: Covariance + for ( unsigned int i = 0; i < ACCELERATION_SIZE; i++ ) + { + for ( unsigned int j = 0; j < ACCELERATION_SIZE; j++ ) + { + state.covariance(POSITION_A_OFFSET + i, POSITION_A_OFFSET + j) = accel.accel.covariance[i*TWIST_SIZE + j]; + } + } + + // Add the state to the buffer, so that we can later interpolate between this and earlier states + estimator_->setState(state); + + return; +} + +bool findAncestorRecursiveYAML(YAML::Node& tree, const std::string& source_frame, const std::string& target_frame) +{ + if ( source_frame == target_frame ) + { + return true; + } + + std::string parent_frame = tree[source_frame]["parent"].Scalar(); + + if ( parent_frame.empty() ) + { + return false; + } + + return findAncestorRecursiveYAML(tree, parent_frame, target_frame); +} + +// Cache, assumption that the tree parent child order does not change over time +static std::map > ancestor_map; +static std::map > descendant_map; +bool findAncestor(const tf2_ros::Buffer& buffer, const std::string& source_frame, const std::string& target_frame) +{ + // Check cache + const std::vector& ancestors = ancestor_map[source_frame]; + if (std::find(ancestors.begin(), ancestors.end(), target_frame) != ancestors.end()) + { + return true; + } + const std::vector& descendants = descendant_map[source_frame]; + if (std::find(descendants.begin(), descendants.end(), target_frame) != descendants.end()) + { + return false; + } + + std::stringstream frames_stream(buffer.allFramesAsYAML()); + YAML::Node frames_yaml = YAML::Load(frames_stream); + + bool target_frame_is_ancestor = findAncestorRecursiveYAML(frames_yaml, source_frame, target_frame); + bool target_frame_is_descendant = findAncestorRecursiveYAML(frames_yaml, target_frame, source_frame); + + // Caching + if (target_frame_is_ancestor) + { + ancestor_map[source_frame].push_back(target_frame); + } + if (target_frame_is_descendant) + { + descendant_map[source_frame].push_back(target_frame); + } + + return target_frame_is_ancestor; +} + + +bool RosRobotLocalizationListener::getState(const double time, + const std::string& frame_id, + Eigen::VectorXd& state, + Eigen::MatrixXd& covariance, + std::string world_frame_id) const +{ + EstimatorState estimator_state; + state.resize(STATE_SIZE); + state.setZero(); + covariance.resize(STATE_SIZE, STATE_SIZE); + covariance.setZero(); + + if ( base_frame_id_.empty() || world_frame_id_.empty() ) + { + if ( estimator_->getSize() == 0 ) + { + ROS_WARN("Ros Robot Localization Listener: The base or world frame id is not set. " + "No odom/accel messages have come in."); + } + else + { + ROS_ERROR("Ros Robot Localization Listener: The base or world frame id is not set. " + "Are the child_frame_id and the header.frame_id in the odom messages set?"); + } + + return false; + } + + if ( estimator_->getState(time, estimator_state) == EstimatorResults::ExtrapolationIntoPast ) + { + ROS_WARN("Ros Robot Localization Listener: A state is requested at a time stamp older than the oldest in the " + "estimator buffer. The result is an extrapolation into the past. Maybe you should increase the buffer " + "size?"); + } + + // If no world_frame_id is specified, we will default to the world frame_id of the received odometry message + if (world_frame_id.empty()) + { + world_frame_id = world_frame_id_; + } + + if ( frame_id == base_frame_id_ && world_frame_id == world_frame_id_ ) + { + // If the state of the base frame is requested and the world frame equals the world frame of the robot_localization + // estimator, we can simply return the state we got from the state estimator + state = estimator_state.state; + covariance = estimator_state.covariance; + return true; + } + + // - - - - - - - - - - - - - - - - - - + // Get the transformation between the requested world frame and the world_frame of the estimator + // - - - - - - - - - - - - - - - - - - + Eigen::Affine3d world_pose_requested_frame; + + // If the requested frame is the same as the tracker, set to identity + if (world_frame_id == world_frame_id_) + { + world_pose_requested_frame.setIdentity(); + } + else + { + geometry_msgs::TransformStamped world_requested_to_world_transform; + try + { + // TODO(reinzor): magic number + world_requested_to_world_transform = tf_buffer_.lookupTransform(world_frame_id, + world_frame_id_, + ros::Time(time), + ros::Duration(0.1)); + + if ( findAncestor(tf_buffer_, world_frame_id, base_frame_id_) ) + { + ROS_ERROR_STREAM("You are trying to get the state with respect to world frame " << world_frame_id << + ", but this frame is a child of robot base frame " << base_frame_id_ << + ", so this doesn't make sense."); + return false; + } + } + catch ( const tf2::TransformException &e ) + { + ROS_WARN_STREAM("Ros Robot Localization Listener: Could not look up transform: " << e.what()); + return false; + } + + // Convert to pose + tf::transformMsgToEigen(world_requested_to_world_transform.transform, world_pose_requested_frame); + } + + // - - - - - - - - - - - - - - - - - - + // Calculate the state of the requested frame from the state of the base frame. + // - - - - - - - - - - - - - - - - - - + + // First get the transform from base to target + geometry_msgs::TransformStamped base_to_target_transform; + try + { + base_to_target_transform = tf_buffer_.lookupTransform(base_frame_id_, + frame_id, + ros::Time(time), + ros::Duration(0.1)); // TODO(reinzor): magic number + + // Check that frame_id is a child of the base frame. If it is not, it does not make sense to request its state. + // Do this after tf lookup, so we know that there is a connection. + if ( !findAncestor(tf_buffer_, frame_id, base_frame_id_) ) + { + ROS_ERROR_STREAM("You are trying to get the state of " << frame_id << ", but this frame is not a child of the " + "base frame: " << base_frame_id_ << "."); + return false; + } + } + catch ( const tf2::TransformException &e ) + { + ROS_WARN_STREAM("Ros Robot Localization Listener: Could not look up transform: " << e.what()); + return false; + } + + // And convert it to an eigen Affine transformation + Eigen::Affine3d target_pose_base; + tf::transformMsgToEigen(base_to_target_transform.transform, target_pose_base); + + // Then convert the base pose to an Eigen Affine transformation + Eigen::Vector3d base_position(estimator_state.state(StateMemberX), + estimator_state.state(StateMemberY), + estimator_state.state(StateMemberZ)); + + Eigen::AngleAxisd roll_angle(estimator_state.state(StateMemberRoll), Eigen::Vector3d::UnitX()); + Eigen::AngleAxisd pitch_angle(estimator_state.state(StateMemberPitch), Eigen::Vector3d::UnitY()); + Eigen::AngleAxisd yaw_angle(estimator_state.state(StateMemberYaw), Eigen::Vector3d::UnitZ()); + + Eigen::Quaterniond base_orientation(yaw_angle * pitch_angle * roll_angle); + + Eigen::Affine3d base_pose(Eigen::Translation3d(base_position) * base_orientation); + + // Now we can calculate the transform from odom to the requested frame (target)... + Eigen::Affine3d target_pose_odom = world_pose_requested_frame * base_pose * target_pose_base; + + // ... and put it in the output state + state(StateMemberX) = target_pose_odom.translation().x(); + state(StateMemberY) = target_pose_odom.translation().y(); + state(StateMemberZ) = target_pose_odom.translation().z(); + + Eigen::Vector3d ypr = target_pose_odom.rotation().eulerAngles(2, 1, 0); + + state(StateMemberRoll) = ypr[2]; + state(StateMemberPitch) = ypr[1]; + state(StateMemberYaw) = ypr[0]; + + // Now let's calculate the twist of the target frame + // First get the base's twist + Twist base_velocity; + Twist target_velocity_base; + base_velocity.linear = Eigen::Vector3d(estimator_state.state(StateMemberVx), + estimator_state.state(StateMemberVy), + estimator_state.state(StateMemberVz)); + base_velocity.angular = Eigen::Vector3d(estimator_state.state(StateMemberVroll), + estimator_state.state(StateMemberVpitch), + estimator_state.state(StateMemberVyaw)); + + // Then calculate the target frame's twist as a result of the base's twist. + /* + * We first calculate the coordinates of the velocity vectors (linear and angular) in the base frame. We have to keep + * in mind that a rotation of the base frame, together with the translational offset of the target frame from the base + * frame, induces a translational velocity of the target frame. + */ + target_velocity_base.linear = base_velocity.linear + base_velocity.angular.cross(target_pose_base.translation()); + target_velocity_base.angular = base_velocity.angular; + + // Now we can transform that to the target frame + Twist target_velocity; + target_velocity.linear = target_pose_base.rotation().transpose() * target_velocity_base.linear; + target_velocity.angular = target_pose_base.rotation().transpose() * target_velocity_base.angular; + + state(StateMemberVx) = target_velocity.linear(0); + state(StateMemberVy) = target_velocity.linear(1); + state(StateMemberVz) = target_velocity.linear(2); + + state(StateMemberVroll) = target_velocity.angular(0); + state(StateMemberVpitch) = target_velocity.angular(1); + state(StateMemberVyaw) = target_velocity.angular(2); + + // Rotate the covariance as well + Eigen::MatrixXd rot_6d(POSE_SIZE, POSE_SIZE); + rot_6d.setIdentity(); + + rot_6d.block(POSITION_OFFSET, POSITION_OFFSET) = target_pose_base.rotation(); + rot_6d.block(ORIENTATION_OFFSET, ORIENTATION_OFFSET) = + target_pose_base.rotation(); + + // Rotate the covariance + covariance.block(POSITION_OFFSET, POSITION_OFFSET) = + rot_6d * estimator_state.covariance.block(POSITION_OFFSET, POSITION_OFFSET) * + rot_6d.transpose(); + + return true; +} + +bool RosRobotLocalizationListener::getState(const ros::Time& ros_time, const std::string& frame_id, + Eigen::VectorXd& state, Eigen::MatrixXd& covariance, + const std::string& world_frame_id) const +{ + double time; + if ( ros_time.isZero() ) + { + ROS_INFO("Ros Robot Localization Listener: State requested at time = zero, returning state at current time"); + time = ros::Time::now().toSec(); + } + else + { + time = ros_time.toSec(); + } + + return getState(time, frame_id, state, covariance, world_frame_id); +} + +const std::string& RosRobotLocalizationListener::getBaseFrameId() const +{ + return base_frame_id_; +} + +const std::string& RosRobotLocalizationListener::getWorldFrameId() const +{ + return world_frame_id_; +} + +} // namespace RobotLocalization + diff --git a/Localizations/Packages/robot_localization/src/ukf.cpp b/Localizations/Packages/robot_localization/src/ukf.cpp new file mode 100644 index 0000000..5b55a43 --- /dev/null +++ b/Localizations/Packages/robot_localization/src/ukf.cpp @@ -0,0 +1,458 @@ +/* + * Copyright (c) 2014, 2015, 2016, Charles River Analytics, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "robot_localization/ukf.h" +#include "robot_localization/filter_common.h" + +#include +#include +#include + +#include + + +namespace RobotLocalization +{ + Ukf::Ukf(std::vector args) : + FilterBase(), // Must initialize filter base! + uncorrected_(true) + { + assert(args.size() == 3); + + double alpha = args[0]; + double kappa = args[1]; + double beta = args[2]; + + size_t sigmaCount = (STATE_SIZE << 1) + 1; + sigmaPoints_.resize(sigmaCount, Eigen::VectorXd(STATE_SIZE)); + + // Prepare constants + lambda_ = alpha * alpha * (STATE_SIZE + kappa) - STATE_SIZE; + + stateWeights_.resize(sigmaCount); + covarWeights_.resize(sigmaCount); + + stateWeights_[0] = lambda_ / (STATE_SIZE + lambda_); + covarWeights_[0] = stateWeights_[0] + (1 - (alpha * alpha) + beta); + sigmaPoints_[0].setZero(); + for (size_t i = 1; i < sigmaCount; ++i) + { + sigmaPoints_[i].setZero(); + stateWeights_[i] = 1 / (2 * (STATE_SIZE + lambda_)); + covarWeights_[i] = stateWeights_[i]; + } + } + + Ukf::~Ukf() + { + } + + void Ukf::correct(const Measurement &measurement) + { + FB_DEBUG("---------------------- Ukf::correct ----------------------\n" << + "State is:\n" << state_ << + "\nMeasurement is:\n" << measurement.measurement_ << + "\nMeasurement covariance is:\n" << measurement.covariance_ << "\n"); + + // In our implementation, it may be that after we call predict once, we call correct + // several times in succession (multiple measurements with different time stamps). In + // that event, the sigma points need to be updated to reflect the current state. + // Throughout prediction and correction, we attempt to maximize efficiency in Eigen. + if (!uncorrected_) + { + generateSigmaPoints(); + } + + // We don't want to update everything, so we need to build matrices that only update + // the measured parts of our state vector + + // First, determine how many state vector values we're updating + std::vector updateIndices; + for (size_t i = 0; i < measurement.updateVector_.size(); ++i) + { + if (measurement.updateVector_[i]) + { + // Handle nan and inf values in measurements + if (std::isnan(measurement.measurement_(i))) + { + FB_DEBUG("Value at index " << i << " was nan. Excluding from update.\n"); + } + else if (std::isinf(measurement.measurement_(i))) + { + FB_DEBUG("Value at index " << i << " was inf. Excluding from update.\n"); + } + else + { + updateIndices.push_back(i); + } + } + } + + FB_DEBUG("Update indices are:\n" << updateIndices << "\n"); + + size_t updateSize = updateIndices.size(); + + // Now set up the relevant matrices + Eigen::VectorXd stateSubset(updateSize); // x (in most literature) + Eigen::VectorXd measurementSubset(updateSize); // z + Eigen::MatrixXd measurementCovarianceSubset(updateSize, updateSize); // R + Eigen::MatrixXd stateToMeasurementSubset(updateSize, STATE_SIZE); // H + Eigen::MatrixXd kalmanGainSubset(STATE_SIZE, updateSize); // K + Eigen::VectorXd innovationSubset(updateSize); // z - Hx + Eigen::VectorXd predictedMeasurement(updateSize); + Eigen::VectorXd sigmaDiff(updateSize); + Eigen::MatrixXd predictedMeasCovar(updateSize, updateSize); + Eigen::MatrixXd crossCovar(STATE_SIZE, updateSize); + + std::vector sigmaPointMeasurements(sigmaPoints_.size(), Eigen::VectorXd(updateSize)); + + stateSubset.setZero(); + measurementSubset.setZero(); + measurementCovarianceSubset.setZero(); + stateToMeasurementSubset.setZero(); + kalmanGainSubset.setZero(); + innovationSubset.setZero(); + predictedMeasurement.setZero(); + predictedMeasCovar.setZero(); + crossCovar.setZero(); + + // Now build the sub-matrices from the full-sized matrices + for (size_t i = 0; i < updateSize; ++i) + { + measurementSubset(i) = measurement.measurement_(updateIndices[i]); + stateSubset(i) = state_(updateIndices[i]); + + for (size_t j = 0; j < updateSize; ++j) + { + measurementCovarianceSubset(i, j) = measurement.covariance_(updateIndices[i], updateIndices[j]); + } + + // Handle negative (read: bad) covariances in the measurement. Rather + // than exclude the measurement or make up a covariance, just take + // the absolute value. + if (measurementCovarianceSubset(i, i) < 0) + { + FB_DEBUG("WARNING: Negative covariance for index " << i << + " of measurement (value is" << measurementCovarianceSubset(i, i) << + "). Using absolute value...\n"); + + measurementCovarianceSubset(i, i) = ::fabs(measurementCovarianceSubset(i, i)); + } + + // If the measurement variance for a given variable is very + // near 0 (as in e-50 or so) and the variance for that + // variable in the covariance matrix is also near zero, then + // the Kalman gain computation will blow up. Really, no + // measurement can be completely without error, so add a small + // amount in that case. + if (measurementCovarianceSubset(i, i) < 1e-9) + { + measurementCovarianceSubset(i, i) = 1e-9; + + FB_DEBUG("WARNING: measurement had very small error covariance for index " << + updateIndices[i] << + ". Adding some noise to maintain filter stability.\n"); + } + } + + // The state-to-measurement function, h, will now be a measurement_size x full_state_size + // matrix, with ones in the (i, i) locations of the values to be updated + for (size_t i = 0; i < updateSize; ++i) + { + stateToMeasurementSubset(i, updateIndices[i]) = 1; + } + + FB_DEBUG("Current state subset is:\n" << stateSubset << + "\nMeasurement subset is:\n" << measurementSubset << + "\nMeasurement covariance subset is:\n" << measurementCovarianceSubset << + "\nState-to-measurement subset is:\n" << stateToMeasurementSubset << "\n"); + + double roll_sum_x {}; + double roll_sum_y {}; + double pitch_sum_x {}; + double pitch_sum_y {}; + double yaw_sum_x {}; + double yaw_sum_y {}; + + // (1) Generate sigma points, use them to generate a predicted measurement + for (size_t sigmaInd = 0; sigmaInd < sigmaPoints_.size(); ++sigmaInd) + { + sigmaPointMeasurements[sigmaInd] = stateToMeasurementSubset * sigmaPoints_[sigmaInd]; + predictedMeasurement.noalias() += stateWeights_[sigmaInd] * sigmaPointMeasurements[sigmaInd]; + + // Euler angle averaging requires special care + for (size_t i = 0; i < updateSize; ++i) + { + if (updateIndices[i] == StateMemberRoll) + { + roll_sum_x += stateWeights_[sigmaInd] * ::cos(sigmaPointMeasurements[sigmaInd](i)); + roll_sum_y += stateWeights_[sigmaInd] * ::sin(sigmaPointMeasurements[sigmaInd](i)); + } + else if (updateIndices[i] == StateMemberPitch) + { + pitch_sum_x += stateWeights_[sigmaInd] * ::cos(sigmaPointMeasurements[sigmaInd](i)); + pitch_sum_y += stateWeights_[sigmaInd] * ::sin(sigmaPointMeasurements[sigmaInd](i)); + } + else if (updateIndices[i] == StateMemberYaw) + { + yaw_sum_x += stateWeights_[sigmaInd] * ::cos(sigmaPointMeasurements[sigmaInd](i)); + yaw_sum_y += stateWeights_[sigmaInd] * ::sin(sigmaPointMeasurements[sigmaInd](i)); + } + } + } + + // Wrap angles in the innovation + for (size_t i = 0; i < updateSize; ++i) + { + if (updateIndices[i] == StateMemberRoll) + { + predictedMeasurement(i) = ::atan2(roll_sum_y, roll_sum_x); + } + else if (updateIndices[i] == StateMemberPitch) + { + predictedMeasurement(i) = ::atan2(pitch_sum_y, pitch_sum_x); + } + else if (updateIndices[i] == StateMemberYaw) + { + predictedMeasurement(i) = ::atan2(yaw_sum_y, yaw_sum_x); + } + } + + // (2) Use the sigma point measurements and predicted measurement to compute a predicted + // measurement covariance matrix P_zz and a state/measurement cross-covariance matrix P_xz. + for (size_t sigmaInd = 0; sigmaInd < sigmaPoints_.size(); ++sigmaInd) + { + sigmaDiff = sigmaPointMeasurements[sigmaInd] - predictedMeasurement; + Eigen::VectorXd sigmaStateDiff = sigmaPoints_[sigmaInd] - state_; + + for (size_t i = 0; i < updateSize; ++i) + { + if (updateIndices[i] == StateMemberRoll || + updateIndices[i] == StateMemberPitch || + updateIndices[i] == StateMemberYaw) + { + sigmaDiff(i) = angles::normalize_angle(sigmaDiff(i)); + sigmaStateDiff(i) = angles::normalize_angle(sigmaStateDiff(i)); + } + } + + predictedMeasCovar.noalias() += covarWeights_[sigmaInd] * (sigmaDiff * sigmaDiff.transpose()); + crossCovar.noalias() += covarWeights_[sigmaInd] * (sigmaStateDiff * sigmaDiff.transpose()); + } + + // (3) Compute the Kalman gain, making sure to use the actual measurement covariance: K = P_xz * (P_zz + R)^-1 + Eigen::MatrixXd invInnovCov = (predictedMeasCovar + measurementCovarianceSubset).inverse(); + kalmanGainSubset = crossCovar * invInnovCov; + + // (4) Apply the gain to the difference between the actual and predicted measurements: x = x + K(z - z_hat) + innovationSubset = (measurementSubset - predictedMeasurement); + + // Wrap angles in the innovation + for (size_t i = 0; i < updateSize; ++i) + { + if (updateIndices[i] == StateMemberRoll || + updateIndices[i] == StateMemberPitch || + updateIndices[i] == StateMemberYaw) + { + innovationSubset(i) = angles::normalize_angle(innovationSubset(i)); + } + } + + // (5) Check Mahalanobis distance of innovation + if (checkMahalanobisThreshold(innovationSubset, invInnovCov, measurement.mahalanobisThresh_)) + { + state_.noalias() += kalmanGainSubset * innovationSubset; + + // (6) Compute the new estimate error covariance P = P - (K * P_zz * K') + estimateErrorCovariance_.noalias() -= (kalmanGainSubset * predictedMeasCovar * kalmanGainSubset.transpose()); + + wrapStateAngles(); + + // Mark that we need to re-compute sigma points for successive corrections + uncorrected_ = false; + + FB_DEBUG("Predicated measurement covariance is:\n" << predictedMeasCovar << + "\nCross covariance is:\n" << crossCovar << + "\nKalman gain subset is:\n" << kalmanGainSubset << + "\nInnovation:\n" << innovationSubset << + "\nCorrected full state is:\n" << state_ << + "\nCorrected full estimate error covariance is:\n" << estimateErrorCovariance_ << + "\n\n---------------------- /Ukf::correct ----------------------\n"); + } + } + + void Ukf::predict(const double referenceTime, const double delta) + { + FB_DEBUG("---------------------- Ukf::predict ----------------------\n" << + "delta is " << delta << + "\nstate is " << state_ << "\n"); + + prepareControl(referenceTime, delta); + + generateSigmaPoints(); + + double roll_sum_x {}; + double roll_sum_y {}; + double pitch_sum_x {}; + double pitch_sum_y {}; + double yaw_sum_x {}; + double yaw_sum_y {}; + + // Sum the weighted sigma points to generate a new state prediction + state_.setZero(); + for (size_t sigmaInd = 0; sigmaInd < sigmaPoints_.size(); ++sigmaInd) + { + // Apply the state transition function to this sigma point + projectSigmaPoint(sigmaPoints_[sigmaInd], delta); + state_.noalias() += stateWeights_[sigmaInd] * sigmaPoints_[sigmaInd]; + + // Euler angle averaging requires special care + roll_sum_x += stateWeights_[sigmaInd] * ::cos(sigmaPoints_[sigmaInd](StateMemberRoll)); + roll_sum_y += stateWeights_[sigmaInd] * ::sin(sigmaPoints_[sigmaInd](StateMemberRoll)); + pitch_sum_x += stateWeights_[sigmaInd] * ::cos(sigmaPoints_[sigmaInd](StateMemberPitch)); + pitch_sum_y += stateWeights_[sigmaInd] * ::sin(sigmaPoints_[sigmaInd](StateMemberPitch)); + yaw_sum_x += stateWeights_[sigmaInd] * ::cos(sigmaPoints_[sigmaInd](StateMemberYaw)); + yaw_sum_y += stateWeights_[sigmaInd] * ::sin(sigmaPoints_[sigmaInd](StateMemberYaw)); + } + + // Recover average Euler angles + state_(StateMemberRoll) = ::atan2(roll_sum_y, roll_sum_x); + state_(StateMemberPitch) = ::atan2(pitch_sum_y, pitch_sum_x); + state_(StateMemberYaw) = ::atan2(yaw_sum_y, yaw_sum_x); + + // Now use the sigma points and the predicted state to compute a predicted covariance + estimateErrorCovariance_.setZero(); + Eigen::VectorXd sigmaDiff(STATE_SIZE); + for (size_t sigmaInd = 0; sigmaInd < sigmaPoints_.size(); ++sigmaInd) + { + sigmaDiff = (sigmaPoints_[sigmaInd] - state_); + + sigmaDiff(StateMemberRoll) = angles::normalize_angle(sigmaDiff(StateMemberRoll)); + sigmaDiff(StateMemberPitch) = angles::normalize_angle(sigmaDiff(StateMemberPitch)); + sigmaDiff(StateMemberYaw) = angles::normalize_angle(sigmaDiff(StateMemberYaw)); + + estimateErrorCovariance_.noalias() += covarWeights_[sigmaInd] * (sigmaDiff * sigmaDiff.transpose()); + } + + // Not strictly in the theoretical UKF formulation, but necessary here + // to ensure that we actually incorporate the processNoiseCovariance_ + Eigen::MatrixXd *processNoiseCovariance = &processNoiseCovariance_; + + if (useDynamicProcessNoiseCovariance_) + { + computeDynamicProcessNoiseCovariance(state_, delta); + processNoiseCovariance = &dynamicProcessNoiseCovariance_; + } + + estimateErrorCovariance_.noalias() += delta * (*processNoiseCovariance); + + // Keep the angles bounded + wrapStateAngles(); + + // Mark that we can keep these sigma points + uncorrected_ = true; + + FB_DEBUG("Predicted state is:\n" << state_ << + "\nPredicted estimate error covariance is:\n" << estimateErrorCovariance_ << + "\n\n--------------------- /Ukf::predict ----------------------\n"); + } + + void Ukf::generateSigmaPoints() + { + // Take the square root of a small fraction of the estimateErrorCovariance_ using LL' decomposition + weightedCovarSqrt_ = ((static_cast(STATE_SIZE) + lambda_) * estimateErrorCovariance_).llt().matrixL(); + + // Compute sigma points + + // First sigma point is the current state + sigmaPoints_[0] = state_; + + // Next STATE_SIZE sigma points are state + weightedCovarSqrt_[ith column] + // STATE_SIZE sigma points after that are state - weightedCovarSqrt_[ith column] + for (size_t sigmaInd = 0; sigmaInd < STATE_SIZE; ++sigmaInd) + { + sigmaPoints_[sigmaInd + 1] = state_ + weightedCovarSqrt_.col(sigmaInd); + sigmaPoints_[sigmaInd + 1 + STATE_SIZE] = state_ - weightedCovarSqrt_.col(sigmaInd); + } + } + + void Ukf::projectSigmaPoint(Eigen::VectorXd& sigmaPoint, double delta) + { + double roll = sigmaPoint(StateMemberRoll); + double pitch = sigmaPoint(StateMemberPitch); + double yaw = sigmaPoint(StateMemberYaw); + + // We'll need these trig calculations a lot. + double sp = ::sin(pitch); + double cp = ::cos(pitch); + double cpi = 1.0 / cp; + double tp = sp * cpi; + + double sr = ::sin(roll); + double cr = ::cos(roll); + + double sy = ::sin(yaw); + double cy = ::cos(yaw); + + // Prepare the transfer function + transferFunction_(StateMemberX, StateMemberVx) = cy * cp * delta; + transferFunction_(StateMemberX, StateMemberVy) = (cy * sp * sr - sy * cr) * delta; + transferFunction_(StateMemberX, StateMemberVz) = (cy * sp * cr + sy * sr) * delta; + transferFunction_(StateMemberX, StateMemberAx) = 0.5 * transferFunction_(StateMemberX, StateMemberVx) * delta; + transferFunction_(StateMemberX, StateMemberAy) = 0.5 * transferFunction_(StateMemberX, StateMemberVy) * delta; + transferFunction_(StateMemberX, StateMemberAz) = 0.5 * transferFunction_(StateMemberX, StateMemberVz) * delta; + transferFunction_(StateMemberY, StateMemberVx) = sy * cp * delta; + transferFunction_(StateMemberY, StateMemberVy) = (sy * sp * sr + cy * cr) * delta; + transferFunction_(StateMemberY, StateMemberVz) = (sy * sp * cr - cy * sr) * delta; + transferFunction_(StateMemberY, StateMemberAx) = 0.5 * transferFunction_(StateMemberY, StateMemberVx) * delta; + transferFunction_(StateMemberY, StateMemberAy) = 0.5 * transferFunction_(StateMemberY, StateMemberVy) * delta; + transferFunction_(StateMemberY, StateMemberAz) = 0.5 * transferFunction_(StateMemberY, StateMemberVz) * delta; + transferFunction_(StateMemberZ, StateMemberVx) = -sp * delta; + transferFunction_(StateMemberZ, StateMemberVy) = cp * sr * delta; + transferFunction_(StateMemberZ, StateMemberVz) = cp * cr * delta; + transferFunction_(StateMemberZ, StateMemberAx) = 0.5 * transferFunction_(StateMemberZ, StateMemberVx) * delta; + transferFunction_(StateMemberZ, StateMemberAy) = 0.5 * transferFunction_(StateMemberZ, StateMemberVy) * delta; + transferFunction_(StateMemberZ, StateMemberAz) = 0.5 * transferFunction_(StateMemberZ, StateMemberVz) * delta; + transferFunction_(StateMemberRoll, StateMemberVroll) = delta; + transferFunction_(StateMemberRoll, StateMemberVpitch) = sr * tp * delta; + transferFunction_(StateMemberRoll, StateMemberVyaw) = cr * tp * delta; + transferFunction_(StateMemberPitch, StateMemberVpitch) = cr * delta; + transferFunction_(StateMemberPitch, StateMemberVyaw) = -sr * delta; + transferFunction_(StateMemberYaw, StateMemberVpitch) = sr * cpi * delta; + transferFunction_(StateMemberYaw, StateMemberVyaw) = cr * cpi * delta; + transferFunction_(StateMemberVx, StateMemberAx) = delta; + transferFunction_(StateMemberVy, StateMemberAy) = delta; + transferFunction_(StateMemberVz, StateMemberAz) = delta; + + sigmaPoint.applyOnTheLeft(transferFunction_); + } +} // namespace RobotLocalization diff --git a/Localizations/Packages/robot_localization/src/ukf_localization_node.cpp b/Localizations/Packages/robot_localization/src/ukf_localization_node.cpp new file mode 100644 index 0000000..6f1d99e --- /dev/null +++ b/Localizations/Packages/robot_localization/src/ukf_localization_node.cpp @@ -0,0 +1,57 @@ +/* + * Copyright (c) 2014, 2015, 2016, Charles River Analytics, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "robot_localization/ros_filter_types.h" + +#include + +#include +#include + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "ukf_navigation_node"); + ros::NodeHandle nh; + ros::NodeHandle nhLocal("~"); + + std::vector args(3, 0); + + nhLocal.param("alpha", args[0], 0.001); + nhLocal.param("kappa", args[1], 0.0); + nhLocal.param("beta", args[2], 2.0); + + RobotLocalization::RosUkf ukf(nh, nhLocal, args); + ukf.initialize(); + ros::spin(); + + return EXIT_SUCCESS; +} diff --git a/Localizations/Packages/robot_localization/src/ukf_localization_nodelet.cpp b/Localizations/Packages/robot_localization/src/ukf_localization_nodelet.cpp new file mode 100644 index 0000000..f33e1fd --- /dev/null +++ b/Localizations/Packages/robot_localization/src/ukf_localization_nodelet.cpp @@ -0,0 +1,71 @@ +/* + * Copyright (c) 2017 Simon Gene Gottlieb + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "robot_localization/ros_filter_types.h" + +#include +#include +#include + +#include +#include + +namespace RobotLocalization +{ + +class UkfNodelet : public nodelet::Nodelet +{ +private: + std::unique_ptr ukf; + +public: + virtual void onInit() + { + NODELET_DEBUG("Initializing nodelet..."); + + ros::NodeHandle nh = getNodeHandle(); + ros::NodeHandle nh_priv = getPrivateNodeHandle(); + + std::vector args(3, 0); + + nh_priv.param("alpha", args[0], 0.001); + nh_priv.param("kappa", args[1], 0.0); + nh_priv.param("beta", args[2], 2.0); + + ukf = std::make_unique(nh, nh_priv, getName(), args); + ukf->initialize(); + } +}; + +} // namespace RobotLocalization + +PLUGINLIB_EXPORT_CLASS(RobotLocalization::UkfNodelet, nodelet::Nodelet); diff --git a/Localizations/Packages/robot_localization/srv/FromLL.srv b/Localizations/Packages/robot_localization/srv/FromLL.srv new file mode 100644 index 0000000..60c9d54 --- /dev/null +++ b/Localizations/Packages/robot_localization/srv/FromLL.srv @@ -0,0 +1,3 @@ +geographic_msgs/GeoPoint ll_point +--- +geometry_msgs/Point map_point diff --git a/Localizations/Packages/robot_localization/srv/GetState.srv b/Localizations/Packages/robot_localization/srv/GetState.srv new file mode 100644 index 0000000..3d9bb86 --- /dev/null +++ b/Localizations/Packages/robot_localization/srv/GetState.srv @@ -0,0 +1,8 @@ +time time_stamp +string frame_id +--- +# State vector: x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az +float64[15] state + +# Covariance matrix in row-major order +float64[225] covariance diff --git a/Localizations/Packages/robot_localization/srv/SetDatum.srv b/Localizations/Packages/robot_localization/srv/SetDatum.srv new file mode 100644 index 0000000..25cbe4a --- /dev/null +++ b/Localizations/Packages/robot_localization/srv/SetDatum.srv @@ -0,0 +1,2 @@ +geographic_msgs/GeoPose geo_pose +--- diff --git a/Localizations/Packages/robot_localization/srv/SetPose.srv b/Localizations/Packages/robot_localization/srv/SetPose.srv new file mode 100644 index 0000000..7089d1a --- /dev/null +++ b/Localizations/Packages/robot_localization/srv/SetPose.srv @@ -0,0 +1,2 @@ +geometry_msgs/PoseWithCovarianceStamped pose +--- diff --git a/Localizations/Packages/robot_localization/srv/SetUTMZone.srv b/Localizations/Packages/robot_localization/srv/SetUTMZone.srv new file mode 100644 index 0000000..c96c3c4 --- /dev/null +++ b/Localizations/Packages/robot_localization/srv/SetUTMZone.srv @@ -0,0 +1,4 @@ +# Set an UTM zone navsat_transform should stick to. +# Example: 31U +string utm_zone +--- diff --git a/Localizations/Packages/robot_localization/srv/ToLL.srv b/Localizations/Packages/robot_localization/srv/ToLL.srv new file mode 100644 index 0000000..e99723d --- /dev/null +++ b/Localizations/Packages/robot_localization/srv/ToLL.srv @@ -0,0 +1,3 @@ +geometry_msgs/Point map_point +--- +geographic_msgs/GeoPoint ll_point diff --git a/Localizations/Packages/robot_localization/srv/ToggleFilterProcessing.srv b/Localizations/Packages/robot_localization/srv/ToggleFilterProcessing.srv new file mode 100644 index 0000000..01f8bcd --- /dev/null +++ b/Localizations/Packages/robot_localization/srv/ToggleFilterProcessing.srv @@ -0,0 +1,3 @@ +bool on +--- +bool status diff --git a/Localizations/Packages/robot_localization/test/record_all_stats.sh b/Localizations/Packages/robot_localization/test/record_all_stats.sh new file mode 100644 index 0000000..9a39760 --- /dev/null +++ b/Localizations/Packages/robot_localization/test/record_all_stats.sh @@ -0,0 +1,9 @@ +#!/bin/bash + +./stat_recorder.sh test_ekf_localization_node_bag1.test $1/ekf1.txt +./stat_recorder.sh test_ekf_localization_node_bag2.test $1/ekf2.txt +./stat_recorder.sh test_ekf_localization_node_bag3.test $1/ekf3.txt + +./stat_recorder.sh test_ukf_localization_node_bag1.test $1/ukf1.txt +./stat_recorder.sh test_ukf_localization_node_bag2.test $1/ukf2.txt +./stat_recorder.sh test_ukf_localization_node_bag3.test $1/ukf3.txt diff --git a/Localizations/Packages/robot_localization/test/stat_recorder.sh b/Localizations/Packages/robot_localization/test/stat_recorder.sh new file mode 100644 index 0000000..afc236c --- /dev/null +++ b/Localizations/Packages/robot_localization/test/stat_recorder.sh @@ -0,0 +1,18 @@ +#!/bin/bash + +rm $2 +echo "x = [" > $2 + +i="0" + +while [ $i -lt 30 ] +do +rostest robot_localization $1 output_final_position:=true output_location:=$2 +i=$[$i+1] +sleep 3 +done + +echo "]" >> $2 +echo "mean(x)" >> $2 +echo "sqrt(sum((4 * std(x)).^2))" >> $2 + diff --git a/Localizations/Packages/robot_localization/test/test1.bag b/Localizations/Packages/robot_localization/test/test1.bag new file mode 100644 index 0000000..565f14c Binary files /dev/null and b/Localizations/Packages/robot_localization/test/test1.bag differ diff --git a/Localizations/Packages/robot_localization/test/test2.bag b/Localizations/Packages/robot_localization/test/test2.bag new file mode 100644 index 0000000..0559965 Binary files /dev/null and b/Localizations/Packages/robot_localization/test/test2.bag differ diff --git a/Localizations/Packages/robot_localization/test/test3.bag b/Localizations/Packages/robot_localization/test/test3.bag new file mode 100644 index 0000000..6e139cb Binary files /dev/null and b/Localizations/Packages/robot_localization/test/test3.bag differ diff --git a/Localizations/Packages/robot_localization/test/test_ekf.cpp b/Localizations/Packages/robot_localization/test/test_ekf.cpp new file mode 100644 index 0000000..63f8b67 --- /dev/null +++ b/Localizations/Packages/robot_localization/test/test_ekf.cpp @@ -0,0 +1,135 @@ +/* + * Copyright (c) 2014, 2015, 2016, Charles River Analytics, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "robot_localization/ros_filter_types.h" + +#include + +#include +#include + +using RobotLocalization::Ekf; +using RobotLocalization::RosEkf; +using RobotLocalization::STATE_SIZE; + +class RosEkfPassThrough : public RosEkf +{ + public: + RosEkfPassThrough() : RosEkf(ros::NodeHandle(), ros::NodeHandle("~")) + { + } + + Ekf &getFilter() + { + return filter_; + } +}; + +TEST(EkfTest, Measurements) +{ + RosEkfPassThrough ekf; + + Eigen::MatrixXd initialCovar(15, 15); + initialCovar.setIdentity(); + initialCovar *= 0.5; + ekf.getFilter().setEstimateErrorCovariance(initialCovar); + + Eigen::VectorXd measurement(STATE_SIZE); + measurement.setIdentity(); + for (size_t i = 0; i < STATE_SIZE; ++i) + { + measurement[i] = i * 0.01 * STATE_SIZE; + } + + Eigen::MatrixXd measurementCovariance(STATE_SIZE, STATE_SIZE); + measurementCovariance.setIdentity(); + for (size_t i = 0; i < STATE_SIZE; ++i) + { + measurementCovariance(i, i) = 1e-9; + } + + std::vector updateVector(STATE_SIZE, true); + + // Ensure that measurements are being placed in the queue correctly + ros::Time time; + time.fromSec(1000); + ekf.enqueueMeasurement("odom0", + measurement, + measurementCovariance, + updateVector, + std::numeric_limits::max(), + time); + + ekf.integrateMeasurements(ros::Time(1001)); + + EXPECT_EQ(ekf.getFilter().getState(), measurement); + EXPECT_EQ(ekf.getFilter().getEstimateErrorCovariance(), measurementCovariance); + + ekf.getFilter().setEstimateErrorCovariance(initialCovar); + + // Now fuse another measurement and check the output. + // We know what the filter's state should be when + // this is complete, so we'll check the difference and + // make sure it's suitably small. + Eigen::VectorXd measurement2 = measurement; + + measurement2 *= 2.0; + + for (size_t i = 0; i < STATE_SIZE; ++i) + { + measurementCovariance(i, i) = 1e-9; + } + + time.fromSec(1002); + ekf.enqueueMeasurement("odom0", + measurement2, + measurementCovariance, + updateVector, + std::numeric_limits::max(), + time); + + ekf.integrateMeasurements(ros::Time(1003)); + + measurement = measurement2.eval() - ekf.getFilter().getState(); + for (size_t i = 0; i < STATE_SIZE; ++i) + { + EXPECT_LT(::fabs(measurement[i]), 0.001); + } +} + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "ekf"); + + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/Localizations/Packages/robot_localization/test/test_ekf.test b/Localizations/Packages/robot_localization/test/test_ekf.test new file mode 100644 index 0000000..92f8235 --- /dev/null +++ b/Localizations/Packages/robot_localization/test/test_ekf.test @@ -0,0 +1,7 @@ + + + + + + + diff --git a/Localizations/Packages/robot_localization/test/test_ekf_localization_node_bag1.test b/Localizations/Packages/robot_localization/test/test_ekf_localization_node_bag1.test new file mode 100644 index 0000000..8d37e92 --- /dev/null +++ b/Localizations/Packages/robot_localization/test/test_ekf_localization_node_bag1.test @@ -0,0 +1,105 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + [false, false, false, + false, false, false, + true, true, true, + false, false, false, + false, false, false] + + [false, false, false, + true, true, true, + false, false, false, + true, true, false, + true, true, true] + + + + + + + + + + + + + + + + + + [0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0.06, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0.06, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0.04, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.02, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.015] + + [1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9] + + + + + + + + + + + + + diff --git a/Localizations/Packages/robot_localization/test/test_ekf_localization_node_bag2.test b/Localizations/Packages/robot_localization/test/test_ekf_localization_node_bag2.test new file mode 100644 index 0000000..5ea3285 --- /dev/null +++ b/Localizations/Packages/robot_localization/test/test_ekf_localization_node_bag2.test @@ -0,0 +1,85 @@ + + + + + + + + + + + + + + + + + + + + + + + + + [false, false, false, + false, false, false, + true, true, true, + false, false, true, + false, false, false] + + [false, false, false, + true, true, true, + false, false, false, + true, true, true, + true, true, true] + + + + + + + [0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0.4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0.06, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0.05, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.002, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.002, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.004, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01] + + [1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9] + + + + + + + + + + + + + diff --git a/Localizations/Packages/robot_localization/test/test_ekf_localization_node_bag3.test b/Localizations/Packages/robot_localization/test/test_ekf_localization_node_bag3.test new file mode 100644 index 0000000..eb0c6bf --- /dev/null +++ b/Localizations/Packages/robot_localization/test/test_ekf_localization_node_bag3.test @@ -0,0 +1,69 @@ + + + + + + + + + + + + + + + + + + + + + + + + + [false, false, true, + false, false, false, + true, true, false, + false, false, true, + false, false, false] + + [false, false, false, + true, true, true, + false, false, false, + true, true, true, + false, false, false] + + + + + + + [0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.4, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.002, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.002, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.004, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01] + + + + + + + + + + + + + diff --git a/Localizations/Packages/robot_localization/test/test_ekf_localization_node_interfaces.cpp b/Localizations/Packages/robot_localization/test/test_ekf_localization_node_interfaces.cpp new file mode 100644 index 0000000..bf501cd --- /dev/null +++ b/Localizations/Packages/robot_localization/test/test_ekf_localization_node_interfaces.cpp @@ -0,0 +1,1075 @@ +/* + * Copyright (c) 2014, 2015, 2016, Charles River Analytics, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "robot_localization/SetPose.h" + +#include +#include +#include +#include +#include + +#include +#include + +#include +#include + +nav_msgs::Odometry filtered_; + +bool stateUpdated_; + +void resetFilter() +{ + ros::NodeHandle nh; + ros::ServiceClient client = nh.serviceClient("/set_pose"); + + robot_localization::SetPose setPose; + setPose.request.pose.pose.pose.orientation.w = 1; + setPose.request.pose.header.frame_id = "odom"; + for (size_t ind = 0; ind < 36; ind+=7) + { + setPose.request.pose.pose.covariance[ind] = 1e-6; + } + + setPose.request.pose.header.stamp = ros::Time::now(); + client.call(setPose); + setPose.request.pose.header.seq++; + ros::spinOnce(); + ros::Duration(0.01).sleep(); + stateUpdated_ = false; + + double deltaX = 0.0; + double deltaY = 0.0; + double deltaZ = 0.0; + + while (!stateUpdated_ || ::sqrt(deltaX * deltaX + deltaY * deltaY + deltaZ * deltaZ) > 0.1) + { + ros::spinOnce(); + ros::Duration(0.01).sleep(); + + deltaX = filtered_.pose.pose.position.x - setPose.request.pose.pose.pose.position.x; + deltaY = filtered_.pose.pose.position.y - setPose.request.pose.pose.pose.position.y; + deltaZ = filtered_.pose.pose.position.z - setPose.request.pose.pose.pose.position.z; + } + + EXPECT_LT(::sqrt(deltaX * deltaX + deltaY * deltaY + deltaZ * deltaZ), 0.1); +} + +void filterCallback(const nav_msgs::OdometryConstPtr &msg) +{ + filtered_ = *msg; + stateUpdated_ = true; +} + +TEST(InterfacesTest, OdomPoseBasicIO) +{ + stateUpdated_ = false; + + ros::NodeHandle nh; + ros::Publisher odomPub = nh.advertise("/odom_input0", 5); + ros::Subscriber filteredSub = nh.subscribe("/odometry/filtered", 1, &filterCallback); + + nav_msgs::Odometry odom; + odom.pose.pose.position.x = 20.0; + odom.pose.pose.position.y = 10.0; + odom.pose.pose.position.z = -40.0; + + odom.pose.covariance[0] = 2.0; + odom.pose.covariance[7] = 2.0; + odom.pose.covariance[14] = 2.0; + + odom.header.frame_id = "odom"; + odom.child_frame_id = "base_link"; + + ros::Rate loopRate(50); + for (size_t i = 0; i < 50; ++i) + { + odom.header.stamp = ros::Time::now(); + odomPub.publish(odom); + ros::spinOnce(); + + loopRate.sleep(); + + odom.header.seq++; + } + + // Now check the values from the callback + EXPECT_LT(::fabs(filtered_.pose.pose.position.x - odom.pose.pose.position.x), 0.01); + EXPECT_LT(::fabs(filtered_.pose.pose.position.y), 0.01); // Configuration for this variable for this sensor is false + EXPECT_LT(::fabs(filtered_.pose.pose.position.z - odom.pose.pose.position.z), 0.01); + + EXPECT_LT(filtered_.pose.covariance[0], 0.5); + EXPECT_LT(filtered_.pose.covariance[7], 0.25); // Configuration for this variable for this sensor is false + EXPECT_LT(filtered_.pose.covariance[14], 0.5); + + resetFilter(); +} + +TEST(InterfacesTest, OdomTwistBasicIO) +{ + ros::NodeHandle nh; + ros::Publisher odomPub = nh.advertise("/odom_input2", 5); + ros::Subscriber filteredSub = nh.subscribe("/odometry/filtered", 1, &filterCallback); + + nav_msgs::Odometry odom; + odom.twist.twist.linear.x = 5.0; + odom.twist.twist.linear.y = 0.0; + odom.twist.twist.linear.z = 0.0; + odom.twist.twist.angular.x = 0.0; + odom.twist.twist.angular.y = 0.0; + odom.twist.twist.angular.z = 0.0; + + for (size_t ind = 0; ind < 36; ind+=7) + { + odom.twist.covariance[ind] = 1e-6; + } + + odom.header.frame_id = "odom"; + odom.child_frame_id = "base_link"; + + ros::Rate loopRate(20); + for (size_t i = 0; i < 400; ++i) + { + odom.header.stamp = ros::Time::now(); + odomPub.publish(odom); + ros::spinOnce(); + + loopRate.sleep(); + + odom.header.seq++; + } + ros::spinOnce(); + + EXPECT_LT(::fabs(filtered_.twist.twist.linear.x - odom.twist.twist.linear.x), 0.1); + EXPECT_LT(::fabs(filtered_.pose.pose.position.x - 100.0), 2.0); + + resetFilter(); + + odom.twist.twist.linear.x = 0.0; + odom.twist.twist.linear.y = 5.0; + + loopRate = ros::Rate(20); + for (size_t i = 0; i < 200; ++i) + { + odom.header.stamp = ros::Time::now(); + odomPub.publish(odom); + ros::spinOnce(); + + loopRate.sleep(); + + odom.header.seq++; + } + ros::spinOnce(); + + EXPECT_LT(::fabs(filtered_.twist.twist.linear.y - odom.twist.twist.linear.y), 0.1); + EXPECT_LT(::fabs(filtered_.pose.pose.position.y - 50.0), 1.0); + + resetFilter(); + + odom.twist.twist.linear.y = 0.0; + odom.twist.twist.linear.z = 5.0; + + loopRate = ros::Rate(20); + for (size_t i = 0; i < 100; ++i) + { + odom.header.stamp = ros::Time::now(); + odomPub.publish(odom); + ros::spinOnce(); + + loopRate.sleep(); + + odom.header.seq++; + } + ros::spinOnce(); + + EXPECT_LT(::fabs(filtered_.twist.twist.linear.z - odom.twist.twist.linear.z), 0.1); + EXPECT_LT(::fabs(filtered_.pose.pose.position.z - 25.0), 1.0); + + resetFilter(); + + odom.twist.twist.linear.z = 0.0; + odom.twist.twist.linear.x = 1.0; + odom.twist.twist.angular.z = (M_PI/2) / (100.0 * 0.05); + + loopRate = ros::Rate(20); + for (size_t i = 0; i < 100; ++i) + { + odom.header.stamp = ros::Time::now(); + odomPub.publish(odom); + ros::spinOnce(); + + loopRate.sleep(); + + odom.header.seq++; + } + ros::spinOnce(); + + EXPECT_LT(::fabs(filtered_.twist.twist.linear.x - odom.twist.twist.linear.x), 0.1); + EXPECT_LT(::fabs(filtered_.twist.twist.angular.z - odom.twist.twist.angular.z), 0.1); + EXPECT_LT(::fabs(filtered_.pose.pose.position.x - filtered_.pose.pose.position.y), 0.5); + + resetFilter(); + + odom.twist.twist.linear.x = 0.0; + odom.twist.twist.angular.z = 0.0; + odom.twist.twist.angular.x = -(M_PI/2) / (100.0 * 0.05); + + // First, roll the vehicle on its side + loopRate = ros::Rate(20); + for (size_t i = 0; i < 100; ++i) + { + odom.header.stamp = ros::Time::now(); + odomPub.publish(odom); + ros::spinOnce(); + + loopRate.sleep(); + + odom.header.seq++; + } + ros::spinOnce(); + + odom.twist.twist.angular.x = 0.0; + odom.twist.twist.angular.y = (M_PI/2) / (100.0 * 0.05); + + // Now, pitch it down (positive pitch velocity in vehicle frame) + loopRate = ros::Rate(20); + for (size_t i = 0; i < 100; ++i) + { + odom.header.stamp = ros::Time::now(); + odomPub.publish(odom); + ros::spinOnce(); + + loopRate.sleep(); + + odom.header.seq++; + } + ros::spinOnce(); + + odom.twist.twist.angular.y = 0.0; + odom.twist.twist.linear.x = 3.0; + + // We should now be on our side and facing -Y. Move forward in + // the vehicle frame X direction, and make sure Y decreases in + // the world frame. + loopRate = ros::Rate(20); + for (size_t i = 0; i < 100; ++i) + { + odom.header.stamp = ros::Time::now(); + odomPub.publish(odom); + ros::spinOnce(); + + loopRate.sleep(); + + odom.header.seq++; + } + ros::spinOnce(); + + EXPECT_LT(::fabs(filtered_.twist.twist.linear.x - odom.twist.twist.linear.x), 0.1); + EXPECT_LT(::fabs(filtered_.pose.pose.position.y + 15), 1.0); + + resetFilter(); +} + +TEST(InterfacesTest, PoseBasicIO) +{ + ros::NodeHandle nh; + ros::Publisher posePub = nh.advertise("/pose_input0", 5); + ros::Subscriber filteredSub = nh.subscribe("/odometry/filtered", 1, &filterCallback); + + geometry_msgs::PoseWithCovarianceStamped pose; + pose.pose.pose.position.x = 20.0; + pose.pose.pose.position.y = 10.0; + pose.pose.pose.position.z = -40.0; + pose.pose.pose.orientation.x = 0; + pose.pose.pose.orientation.y = 0; + pose.pose.pose.orientation.z = 0; + pose.pose.pose.orientation.w = 1; + + for (size_t ind = 0; ind < 36; ind+=7) + { + pose.pose.covariance[ind] = 1e-6; + } + + pose.header.frame_id = "odom"; + + ros::Rate loopRate = ros::Rate(50); + for (size_t i = 0; i < 50; ++i) + { + pose.header.stamp = ros::Time::now(); + posePub.publish(pose); + ros::spinOnce(); + + loopRate.sleep(); + + pose.header.seq++; + } + + // Now check the values from the callback + EXPECT_LT(::fabs(filtered_.pose.pose.position.x - pose.pose.pose.position.x), 0.1); + EXPECT_LT(::fabs(filtered_.pose.pose.position.y), 0.1); // Configuration for this variable for this sensor is false + EXPECT_LT(::fabs(filtered_.pose.pose.position.z - pose.pose.pose.position.z), 0.1); + + EXPECT_LT(filtered_.pose.covariance[0], 0.5); + EXPECT_LT(filtered_.pose.covariance[7], 0.25); // Configuration for this variable for this sensor is false + EXPECT_LT(filtered_.pose.covariance[14], 0.5); + + resetFilter(); +} + +TEST(InterfacesTest, TwistBasicIO) +{ + ros::NodeHandle nh; + ros::Publisher twistPub = nh.advertise("/twist_input0", 5); + ros::Subscriber filteredSub = nh.subscribe("/odometry/filtered", 1, &filterCallback); + + geometry_msgs::TwistWithCovarianceStamped twist; + twist.twist.twist.linear.x = 5.0; + twist.twist.twist.linear.y = 0; + twist.twist.twist.linear.z = 0; + twist.twist.twist.angular.x = 0; + twist.twist.twist.angular.y = 0; + twist.twist.twist.angular.z = 0; + + for (size_t ind = 0; ind < 36; ind+=7) + { + twist.twist.covariance[ind] = 1e-6; + } + + twist.header.frame_id = "base_link"; + + ros::Rate loopRate = ros::Rate(20); + for (size_t i = 0; i < 400; ++i) + { + twist.header.stamp = ros::Time::now(); + twistPub.publish(twist); + ros::spinOnce(); + + loopRate.sleep(); + + twist.header.seq++; + } + ros::spinOnce(); + + EXPECT_LT(::fabs(filtered_.twist.twist.linear.x - twist.twist.twist.linear.x), 0.1); + EXPECT_LT(::fabs(filtered_.pose.pose.position.x - 100.0), 2.0); + + resetFilter(); + + twist.twist.twist.linear.x = 0.0; + twist.twist.twist.linear.y = 5.0; + + loopRate = ros::Rate(20); + for (size_t i = 0; i < 200; ++i) + { + twist.header.stamp = ros::Time::now(); + twistPub.publish(twist); + ros::spinOnce(); + + loopRate.sleep(); + + twist.header.seq++; + } + ros::spinOnce(); + + EXPECT_LT(::fabs(filtered_.twist.twist.linear.y - twist.twist.twist.linear.y), 0.1); + EXPECT_LT(::fabs(filtered_.pose.pose.position.y - 50.0), 1.0); + + resetFilter(); + + twist.twist.twist.linear.y = 0.0; + twist.twist.twist.linear.z = 5.0; + + loopRate = ros::Rate(20); + for (size_t i = 0; i < 100; ++i) + { + twist.header.stamp = ros::Time::now(); + twistPub.publish(twist); + ros::spinOnce(); + + loopRate.sleep(); + + twist.header.seq++; + } + ros::spinOnce(); + + EXPECT_LT(::fabs(filtered_.twist.twist.linear.z - twist.twist.twist.linear.z), 0.1); + EXPECT_LT(::fabs(filtered_.pose.pose.position.z - 25.0), 1.0); + + resetFilter(); + + twist.twist.twist.linear.z = 0.0; + twist.twist.twist.linear.x = 1.0; + twist.twist.twist.angular.z = (M_PI/2) / (100.0 * 0.05); + + loopRate = ros::Rate(20); + for (size_t i = 0; i < 100; ++i) + { + twist.header.stamp = ros::Time::now(); + twistPub.publish(twist); + ros::spinOnce(); + + loopRate.sleep(); + + twist.header.seq++; + } + ros::spinOnce(); + + EXPECT_LT(::fabs(filtered_.twist.twist.linear.x - twist.twist.twist.linear.x), 0.1); + EXPECT_LT(::fabs(filtered_.twist.twist.angular.z - twist.twist.twist.angular.z), 0.1); + EXPECT_LT(::fabs(filtered_.pose.pose.position.x - filtered_.pose.pose.position.y), 0.5); + + resetFilter(); + + twist.twist.twist.linear.x = 0.0; + twist.twist.twist.angular.z = 0.0; + twist.twist.twist.angular.x = -(M_PI/2) / (100.0 * 0.05); + + // First, roll the vehicle on its side + loopRate = ros::Rate(20); + for (size_t i = 0; i < 100; ++i) + { + twist.header.stamp = ros::Time::now(); + twistPub.publish(twist); + ros::spinOnce(); + + loopRate.sleep(); + + twist.header.seq++; + } + ros::spinOnce(); + + twist.twist.twist.angular.x = 0.0; + twist.twist.twist.angular.y = (M_PI/2) / (100.0 * 0.05); + + // Now, pitch it down (positive pitch velocity in vehicle frame) + loopRate = ros::Rate(20); + for (size_t i = 0; i < 100; ++i) + { + twist.header.stamp = ros::Time::now(); + twistPub.publish(twist); + ros::spinOnce(); + + loopRate.sleep(); + + twist.header.seq++; + } + ros::spinOnce(); + + twist.twist.twist.angular.y = 0.0; + twist.twist.twist.linear.x = 3.0; + + // We should now be on our side and facing -Y. Move forward in + // the vehicle frame X direction, and make sure Y decreases in + // the world frame. + loopRate = ros::Rate(20); + for (size_t i = 0; i < 100; ++i) + { + twist.header.stamp = ros::Time::now(); + twistPub.publish(twist); + ros::spinOnce(); + + loopRate.sleep(); + + twist.header.seq++; + } + ros::spinOnce(); + + EXPECT_LT(::fabs(filtered_.twist.twist.linear.x - twist.twist.twist.linear.x), 0.1); + EXPECT_LT(::fabs(filtered_.pose.pose.position.y + 15), 1.0); + + resetFilter(); +} + +TEST(InterfacesTest, ImuPoseBasicIO) +{ + ros::NodeHandle nh; + ros::Publisher imuPub = nh.advertise("/imu_input0", 5); + ros::Subscriber filteredSub = nh.subscribe("/odometry/filtered", 1, &filterCallback); + + sensor_msgs::Imu imu; + tf2::Quaternion quat; + quat.setRPY(M_PI/4, -M_PI/4, M_PI/2); + imu.orientation = tf2::toMsg(quat); + + for (size_t ind = 0; ind < 9; ind+=4) + { + imu.orientation_covariance[ind] = 1e-6; + } + + imu.header.frame_id = "base_link"; + + // Make sure the pose reset worked. Test will timeout + // if this fails. + ros::Rate loopRate(50); + for (size_t i = 0; i < 50; ++i) + { + imu.header.stamp = ros::Time::now(); + imuPub.publish(imu); + ros::spinOnce(); + + loopRate.sleep(); + + imu.header.seq++; + } + + // Now check the values from the callback + tf2::fromMsg(filtered_.pose.pose.orientation, quat); + tf2::Matrix3x3 mat(quat); + double r, p, y; + mat.getRPY(r, p, y); + EXPECT_LT(::fabs(r - M_PI/4), 0.1); + EXPECT_LT(::fabs(p + M_PI/4), 0.1); + EXPECT_LT(::fabs(y - M_PI/2), 0.1); + + EXPECT_LT(filtered_.pose.covariance[21], 0.5); + EXPECT_LT(filtered_.pose.covariance[28], 0.25); + EXPECT_LT(filtered_.pose.covariance[35], 0.5); + + resetFilter(); + + // Test to see if the orientation data is ignored when we set the + // first covariance value to -1 + sensor_msgs::Imu imuIgnore; + imuIgnore.orientation.x = 0.1; + imuIgnore.orientation.y = 0.2; + imuIgnore.orientation.z = 0.3; + imuIgnore.orientation.w = 0.4; + imuIgnore.orientation_covariance[0] = -1; + + loopRate = ros::Rate(50); + for (size_t i = 0; i < 50; ++i) + { + imuIgnore.header.stamp = ros::Time::now(); + imuPub.publish(imuIgnore); + loopRate.sleep(); + ros::spinOnce(); + + imuIgnore.header.seq++; + } + + tf2::fromMsg(filtered_.pose.pose.orientation, quat); + mat.setRotation(quat); + mat.getRPY(r, p, y); + EXPECT_LT(::fabs(r), 1e-3); + EXPECT_LT(::fabs(p), 1e-3); + EXPECT_LT(::fabs(y), 1e-3); + + resetFilter(); +} + +TEST(InterfacesTest, ImuTwistBasicIO) +{ + ros::NodeHandle nh; + ros::Publisher imuPub = nh.advertise("/imu_input1", 5); + ros::Subscriber filteredSub = nh.subscribe("/odometry/filtered", 1, &filterCallback); + + sensor_msgs::Imu imu; + tf2::Quaternion quat; + imu.angular_velocity.x = (M_PI / 2.0); + + for (size_t ind = 0; ind < 9; ind+=4) + { + imu.angular_velocity_covariance[ind] = 1e-6; + } + + imu.header.frame_id = "base_link"; + + ros::Rate loopRate(50); + for (size_t i = 0; i < 50; ++i) + { + imu.header.stamp = ros::Time::now(); + imuPub.publish(imu); + loopRate.sleep(); + ros::spinOnce(); + + imu.header.seq++; + } + + // Now check the values from the callback + tf2::fromMsg(filtered_.pose.pose.orientation, quat); + tf2::Matrix3x3 mat(quat); + double r, p, y; + mat.getRPY(r, p, y); + + // Tolerances may seem loose, but the initial state covariances + // are tiny, so the filter is sluggish to accept velocity data + EXPECT_LT(::fabs(r - M_PI / 2.0), 0.7); + EXPECT_LT(::fabs(p), 0.1); + EXPECT_LT(::fabs(y), 0.1); + + EXPECT_LT(filtered_.twist.covariance[21], 1e-3); + EXPECT_LT(filtered_.pose.covariance[21], 0.1); + + resetFilter(); + + imu.angular_velocity.x = 0.0; + imu.angular_velocity.y = -(M_PI / 2.0); + + // Make sure the pose reset worked. Test will timeout + // if this fails. + loopRate = ros::Rate(50); + for (size_t i = 0; i < 50; ++i) + { + imu.header.stamp = ros::Time::now(); + imuPub.publish(imu); + loopRate.sleep(); + ros::spinOnce(); + + imu.header.seq++; + } + + // Now check the values from the callback + tf2::fromMsg(filtered_.pose.pose.orientation, quat); + mat.setRotation(quat); + mat.getRPY(r, p, y); + EXPECT_LT(::fabs(r), 0.1); + EXPECT_LT(::fabs(p + M_PI / 2.0), 0.7); + EXPECT_LT(::fabs(y), 0.1); + + EXPECT_LT(filtered_.twist.covariance[28], 1e-3); + EXPECT_LT(filtered_.pose.covariance[28], 0.1); + + resetFilter(); + + imu.angular_velocity.y = 0; + imu.angular_velocity.z = M_PI / 4.0; + + // Make sure the pose reset worked. Test will timeout + // if this fails. + loopRate = ros::Rate(50); + for (size_t i = 0; i < 50; ++i) + { + imu.header.stamp = ros::Time::now(); + imuPub.publish(imu); + loopRate.sleep(); + ros::spinOnce(); + + imu.header.seq++; + } + + // Now check the values from the callback + tf2::fromMsg(filtered_.pose.pose.orientation, quat); + mat.setRotation(quat); + mat.getRPY(r, p, y); + EXPECT_LT(::fabs(r), 0.1); + EXPECT_LT(::fabs(p), 0.1); + EXPECT_LT(::fabs(y - M_PI / 4.0), 0.7); + + EXPECT_LT(filtered_.twist.covariance[35], 1e-3); + EXPECT_LT(filtered_.pose.covariance[35], 0.1); + + resetFilter(); + + // Test to see if the angular velocity data is ignored when we set the + // first covariance value to -1 + sensor_msgs::Imu imuIgnore; + imuIgnore.angular_velocity.x = 100; + imuIgnore.angular_velocity.y = 100; + imuIgnore.angular_velocity.z = 100; + imuIgnore.angular_velocity_covariance[0] = -1; + + loopRate = ros::Rate(50); + for (size_t i = 0; i < 50; ++i) + { + imuIgnore.header.stamp = ros::Time::now(); + imuPub.publish(imuIgnore); + loopRate.sleep(); + ros::spinOnce(); + + imuIgnore.header.seq++; + } + + tf2::fromMsg(filtered_.pose.pose.orientation, quat); + mat.setRotation(quat); + mat.getRPY(r, p, y); + EXPECT_LT(::fabs(r), 1e-3); + EXPECT_LT(::fabs(p), 1e-3); + EXPECT_LT(::fabs(y), 1e-3); + + resetFilter(); +} + +TEST(InterfacesTest, ImuAccBasicIO) +{ + ros::NodeHandle nh; + ros::Publisher imuPub = nh.advertise("/imu_input2", 5); + ros::Subscriber filteredSub = nh.subscribe("/odometry/filtered", 1, &filterCallback); + + sensor_msgs::Imu imu; + imu.header.frame_id = "base_link"; + imu.linear_acceleration_covariance[0] = 1e-6; + imu.linear_acceleration_covariance[4] = 1e-6; + imu.linear_acceleration_covariance[8] = 1e-6; + + imu.linear_acceleration.x = 1; + imu.linear_acceleration.y = -1; + imu.linear_acceleration.z = 1; + + // Move with constant acceleration for 1 second, + // then check our velocity. + ros::Rate loopRate(50); + for (size_t i = 0; i < 50; ++i) + { + imu.header.stamp = ros::Time::now(); + imuPub.publish(imu); + loopRate.sleep(); + ros::spinOnce(); + + imu.header.seq++; + } + + EXPECT_LT(::fabs(filtered_.twist.twist.linear.x - 1.0), 0.4); + EXPECT_LT(::fabs(filtered_.twist.twist.linear.y + 1.0), 0.4); + EXPECT_LT(::fabs(filtered_.twist.twist.linear.z - 1.0), 0.4); + + imu.linear_acceleration.x = 0.0; + imu.linear_acceleration.y = 0.0; + imu.linear_acceleration.z = 0.0; + + // Now move for another second, and see where we + // end up + loopRate = ros::Rate(50); + for (size_t i = 0; i < 50; ++i) + { + imu.header.stamp = ros::Time::now(); + imuPub.publish(imu); + loopRate.sleep(); + ros::spinOnce(); + + imu.header.seq++; + } + + EXPECT_LT(::fabs(filtered_.pose.pose.position.x - 1.2), 0.4); + EXPECT_LT(::fabs(filtered_.pose.pose.position.y + 1.2), 0.4); + EXPECT_LT(::fabs(filtered_.pose.pose.position.z - 1.2), 0.4); + + resetFilter(); + + // Test to see if the linear acceleration data is ignored when we set the + // first covariance value to -1 + sensor_msgs::Imu imuIgnore; + imuIgnore.linear_acceleration.x = 1000; + imuIgnore.linear_acceleration.y = 1000; + imuIgnore.linear_acceleration.z = 1000; + imuIgnore.linear_acceleration_covariance[0] = -1; + + loopRate = ros::Rate(50); + for (size_t i = 0; i < 50; ++i) + { + imuIgnore.header.stamp = ros::Time::now(); + imuPub.publish(imuIgnore); + loopRate.sleep(); + ros::spinOnce(); + + imuIgnore.header.seq++; + } + + EXPECT_LT(::fabs(filtered_.pose.pose.position.x), 1e-3); + EXPECT_LT(::fabs(filtered_.pose.pose.position.y), 1e-3); + EXPECT_LT(::fabs(filtered_.pose.pose.position.z), 1e-3); + + resetFilter(); +} + +TEST(InterfacesTest, OdomDifferentialIO) +{ + ros::NodeHandle nh; + ros::Publisher odomPub = nh.advertise("/odom_input1", 5); + ros::Subscriber filteredSub = nh.subscribe("/odometry/filtered", 1, &filterCallback); + + nav_msgs::Odometry odom; + odom.pose.pose.position.x = 20.0; + odom.pose.pose.position.y = 10.0; + odom.pose.pose.position.z = -40.0; + + odom.pose.pose.orientation.w = 1; + + odom.pose.covariance[0] = 2.0; + odom.pose.covariance[7] = 2.0; + odom.pose.covariance[14] = 2.0; + odom.pose.covariance[21] = 0.2; + odom.pose.covariance[28] = 0.2; + odom.pose.covariance[35] = 0.2; + + odom.header.frame_id = "odom"; + odom.child_frame_id = "base_link"; + + // No guaranteeing that the zero state + // we're expecting to see here isn't just + // a result of zeroing it out previously, + // so check 10 times in succession. + size_t zeroCount = 0; + while (zeroCount++ < 10) + { + odom.header.stamp = ros::Time::now(); + odomPub.publish(odom); + ros::spinOnce(); + + EXPECT_LT(::fabs(filtered_.pose.pose.position.x), 0.01); + EXPECT_LT(::fabs(filtered_.pose.pose.position.y), 0.01); + EXPECT_LT(::fabs(filtered_.pose.pose.position.z), 0.01); + EXPECT_LT(::fabs(filtered_.pose.pose.orientation.x), 0.01); + EXPECT_LT(::fabs(filtered_.pose.pose.orientation.y), 0.01); + EXPECT_LT(::fabs(filtered_.pose.pose.orientation.z), 0.01); + EXPECT_LT(::fabs(filtered_.pose.pose.orientation.w - 1), 0.01); + + ros::Duration(0.1).sleep(); + + odom.header.seq++; + } + + for (size_t ind = 0; ind < 36; ind+=7) + { + odom.pose.covariance[ind] = 1e-6; + } + + // Slowly move the position, and hope that the + // differential position keeps up + ros::Rate loopRate(20); + for (size_t i = 0; i < 100; ++i) + { + odom.pose.pose.position.x += 0.01; + odom.pose.pose.position.y += 0.02; + odom.pose.pose.position.z -= 0.03; + + odom.header.stamp = ros::Time::now(); + odomPub.publish(odom); + ros::spinOnce(); + + loopRate.sleep(); + + odom.header.seq++; + } + ros::spinOnce(); + + EXPECT_LT(::fabs(filtered_.pose.pose.position.x - 1), 0.2); + EXPECT_LT(::fabs(filtered_.pose.pose.position.y - 2), 0.4); + EXPECT_LT(::fabs(filtered_.pose.pose.position.z + 3), 0.6); + + resetFilter(); +} + +TEST(InterfacesTest, PoseDifferentialIO) +{ + ros::NodeHandle nh; + ros::Publisher posePub = nh.advertise("/pose_input1", 5); + ros::Subscriber filteredSub = nh.subscribe("/odometry/filtered", 1, &filterCallback); + + geometry_msgs::PoseWithCovarianceStamped pose; + pose.pose.pose.position.x = 20.0; + pose.pose.pose.position.y = 10.0; + pose.pose.pose.position.z = -40.0; + + pose.pose.pose.orientation.w = 1; + + pose.pose.covariance[0] = 2.0; + pose.pose.covariance[7] = 2.0; + pose.pose.covariance[14] = 2.0; + pose.pose.covariance[21] = 0.2; + pose.pose.covariance[28] = 0.2; + pose.pose.covariance[35] = 0.2; + + pose.header.frame_id = "odom"; + + // No guaranteeing that the zero state + // we're expecting to see here isn't just + // a result of zeroing it out previously, + // so check 10 times in succession. + size_t zeroCount = 0; + while (zeroCount++ < 10) + { + pose.header.stamp = ros::Time::now(); + posePub.publish(pose); + ros::spinOnce(); + + EXPECT_LT(::fabs(filtered_.pose.pose.position.x), 0.01); + EXPECT_LT(::fabs(filtered_.pose.pose.position.y), 0.01); + EXPECT_LT(::fabs(filtered_.pose.pose.position.z), 0.01); + EXPECT_LT(::fabs(filtered_.pose.pose.orientation.x), 0.01); + EXPECT_LT(::fabs(filtered_.pose.pose.orientation.y), 0.01); + EXPECT_LT(::fabs(filtered_.pose.pose.orientation.z), 0.01); + EXPECT_LT(::fabs(filtered_.pose.pose.orientation.w - 1), 0.01); + + ros::Duration(0.1).sleep(); + + pose.header.seq++; + } + + // ...but only if we give the measurement a tiny covariance + for (size_t ind = 0; ind < 36; ind+=7) + { + pose.pose.covariance[ind] = 1e-6; + } + + // Issue this location repeatedly, and see if we get + // a final reported position of (1, 2, -3) + ros::Rate loopRate(20); + for (size_t i = 0; i < 100; ++i) + { + pose.pose.pose.position.x += 0.01; + pose.pose.pose.position.y += 0.02; + pose.pose.pose.position.z -= 0.03; + + pose.header.stamp = ros::Time::now(); + posePub.publish(pose); + ros::spinOnce(); + + loopRate.sleep(); + + pose.header.seq++; + } + ros::spinOnce(); + + EXPECT_LT(::fabs(filtered_.pose.pose.position.x - 1), 0.2); + EXPECT_LT(::fabs(filtered_.pose.pose.position.y - 2), 0.4); + EXPECT_LT(::fabs(filtered_.pose.pose.position.z + 3), 0.6); + + resetFilter(); +} + +TEST(InterfacesTest, ImuDifferentialIO) +{ + ros::NodeHandle nh; + ros::Publisher imu0Pub = nh.advertise("/imu_input0", 5); + ros::Publisher imu1Pub = nh.advertise("/imu_input1", 5); + ros::Publisher imuPub = nh.advertise("/imu_input3", 5); + ros::Subscriber filteredSub = nh.subscribe("/odometry/filtered", 1, &filterCallback); + + sensor_msgs::Imu imu; + imu.header.frame_id = "base_link"; + tf2::Quaternion quat; + const double roll = M_PI/2.0; + const double pitch = -M_PI; + const double yaw = -M_PI/4.0; + quat.setRPY(roll, pitch, yaw); + imu.orientation = tf2::toMsg(quat); + + imu.orientation_covariance[0] = 0.02; + imu.orientation_covariance[4] = 0.02; + imu.orientation_covariance[8] = 0.02; + + imu.angular_velocity_covariance[0] = 0.02; + imu.angular_velocity_covariance[4] = 0.02; + imu.angular_velocity_covariance[8] = 0.02; + + size_t setCount = 0; + while (setCount++ < 10) + { + imu.header.stamp = ros::Time::now(); + imu0Pub.publish(imu); // Use this to move the absolute orientation + imu1Pub.publish(imu); // Use this to keep velocities at 0 + ros::spinOnce(); + + ros::Duration(0.1).sleep(); + + imu.header.seq++; + } + + size_t zeroCount = 0; + while (zeroCount++ < 10) + { + imu.header.stamp = ros::Time::now(); + imuPub.publish(imu); + ros::spinOnce(); + + ros::Duration(0.1).sleep(); + + imu.header.seq++; + } + + double rollFinal = roll; + double pitchFinal = pitch; + double yawFinal = yaw; + + // Move the orientation slowly, and see if we + // can push it to 0 + ros::Rate loopRate(20); + for (size_t i = 0; i < 100; ++i) + { + yawFinal -= 0.01 * (3.0 * M_PI/4.0); + + quat.setRPY(rollFinal, pitchFinal, yawFinal); + + imu.orientation = tf2::toMsg(quat); + imu.header.stamp = ros::Time::now(); + imuPub.publish(imu); + ros::spinOnce(); + + loopRate.sleep(); + + imu.header.seq++; + } + ros::spinOnce(); + + // Move the orientation slowly, and see if we + // can push it to 0 + loopRate = ros::Rate(20); + for (size_t i = 0; i < 100; ++i) + { + rollFinal += 0.01 * (M_PI/2.0); + + quat.setRPY(rollFinal, pitchFinal, yawFinal); + + imu.orientation = tf2::toMsg(quat); + imu.header.stamp = ros::Time::now(); + imuPub.publish(imu); + ros::spinOnce(); + + loopRate.sleep(); + + imu.header.seq++; + } + ros::spinOnce(); + + tf2::fromMsg(filtered_.pose.pose.orientation, quat); + tf2::Matrix3x3 mat(quat); + mat.getRPY(rollFinal, pitchFinal, yawFinal); + EXPECT_LT(::fabs(rollFinal), 0.2); + EXPECT_LT(::fabs(pitchFinal), 0.2); + EXPECT_LT(::fabs(yawFinal), 0.2); + + resetFilter(); +} + +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + + ros::init(argc, argv, "ekf_navigation_node-test-interfaces"); + ros::Time::init(); + + // Give ekf_localization_node time to initialize + ros::Duration(2.0).sleep(); + + return RUN_ALL_TESTS(); +} diff --git a/Localizations/Packages/robot_localization/test/test_ekf_localization_node_interfaces.test b/Localizations/Packages/robot_localization/test/test_ekf_localization_node_interfaces.test new file mode 100644 index 0000000..f440b92 --- /dev/null +++ b/Localizations/Packages/robot_localization/test/test_ekf_localization_node_interfaces.test @@ -0,0 +1,68 @@ + + + + + + + + + + + + + + + + + + + + + + + + + [true, false, true, false, false, false, false, false, false, false, false, false, false, false, false] + [true, true, true, true, true, true, false, false, false, false, false, false, false, false, false] + [false, false, false, false, false, false, true, true, true, true, true, true, false, false, false] + + [true, false, true, false, false, false, false, false, false, false, false, false, false, false, false] + [true, true, true, true, true, true, false, false, false, false, false, false, false, false, false] + + [false, false, false, false, false, false, true, true, true, true, true, true, false, false, false] + + [false, false, false, true, true, true, false, false, false, false, false, false, false, false, false] + [false, false, false, false, false, false, false, false, false, true, true, true, false, false, false] + [false, false, false, false, false, false, false, false, false, false, false, false, true, true, true] + [false, false, false, true, true, true, false, false, false, false, false, false, false, false, false] + + + + + + + + + + + [0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.4, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.002, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.002, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.004, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01] + + + + + + diff --git a/Localizations/Packages/robot_localization/test/test_ekf_localization_nodelet_bag1.test b/Localizations/Packages/robot_localization/test/test_ekf_localization_nodelet_bag1.test new file mode 100644 index 0000000..b49c7fe --- /dev/null +++ b/Localizations/Packages/robot_localization/test/test_ekf_localization_nodelet_bag1.test @@ -0,0 +1,104 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + [false, false, false, + false, false, false, + true, true, true, + false, false, false, + false, false, false] + + [false, false, false, + true, true, true, + false, false, false, + true, true, false, + true, true, true] + + + + + + + + + + + + + + + + + + [0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0.06, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0.06, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0.04, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.02, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.015] + + [1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9] + + + + + + + + + + + + + diff --git a/Localizations/Packages/robot_localization/test/test_filter_base.cpp b/Localizations/Packages/robot_localization/test/test_filter_base.cpp new file mode 100644 index 0000000..8ae42f3 --- /dev/null +++ b/Localizations/Packages/robot_localization/test/test_filter_base.cpp @@ -0,0 +1,216 @@ +/* + * Copyright (c) 2014, 2015, 2016, Charles River Analytics, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "robot_localization/filter_base.h" +#include "robot_localization/filter_common.h" + +#include + +#include + +#include +#include +#include + +using RobotLocalization::STATE_SIZE; +using RobotLocalization::Measurement; + +namespace RobotLocalization +{ + +class FilterDerived : public FilterBase +{ + public: + double val; + + FilterDerived() : val(0) { } + + void correct(const Measurement &measurement) + { + EXPECT_EQ(val, measurement.time_); + EXPECT_EQ(measurement.topicName_, "topic"); + + EXPECT_EQ(measurement.updateVector_.size(), 10u); + for (size_t i = 0; i < measurement.updateVector_.size(); ++i) + { + EXPECT_EQ(measurement.updateVector_[i], true); + } + } + + void predict(const double refTime, const double delta) + { + val = delta; + } +}; + +class FilterDerived2 : public FilterBase +{ + public: + FilterDerived2() { } + + void correct(const Measurement &measurement) + { + } + + void predict(const double refTime, const double delta) + { + } + + void processMeasurement(const Measurement &measurement) + { + FilterBase::processMeasurement(measurement); + } +}; + +} // namespace RobotLocalization + +TEST(FilterBaseTest, MeasurementStruct) +{ + RobotLocalization::Measurement meas1; + RobotLocalization::Measurement meas2; + + EXPECT_EQ(meas1.topicName_, std::string("")); + EXPECT_EQ(meas1.time_, 0); + EXPECT_EQ(meas2.time_, 0); + + // Comparison test is true if the first + // argument is > the second, so should + // be false if they're equal. + EXPECT_EQ(meas1(meas1, meas2), false); + EXPECT_EQ(meas2(meas2, meas1), false); + + meas1.time_ = 100; + meas2.time_ = 200; + + EXPECT_EQ(meas1(meas1, meas2), false); + EXPECT_EQ(meas1(meas2, meas1), true); + EXPECT_EQ(meas2(meas1, meas2), false); + EXPECT_EQ(meas2(meas2, meas1), true); +} + +TEST(FilterBaseTest, DerivedFilterGetSet) +{ + using RobotLocalization::FilterDerived; + + FilterDerived derived; + + // With the ostream argument as NULL, + // the debug flag will remain false. + derived.setDebug(true); + + EXPECT_FALSE(derived.getDebug()); + + // Now set the stream and do it again + std::stringstream os; + derived.setDebug(true, &os); + + EXPECT_TRUE(derived.getDebug()); + + // Simple get/set checks + double timeout = 7.4; + derived.setSensorTimeout(timeout); + EXPECT_EQ(derived.getSensorTimeout(), timeout); + + double lastMeasTime = 3.83; + derived.setLastMeasurementTime(lastMeasTime); + EXPECT_EQ(derived.getLastMeasurementTime(), lastMeasTime); + + Eigen::MatrixXd pnCovar(STATE_SIZE, STATE_SIZE); + for (size_t i = 0; i < STATE_SIZE; ++i) + { + for (size_t j = 0; j < STATE_SIZE; ++j) + { + pnCovar(i, j) = static_cast(i * j); + } + } + derived.setProcessNoiseCovariance(pnCovar); + EXPECT_EQ(derived.getProcessNoiseCovariance(), pnCovar); + + Eigen::VectorXd state(STATE_SIZE); + state.setZero(); + derived.setState(state); + EXPECT_EQ(derived.getState(), state); + + EXPECT_EQ(derived.getInitializedStatus(), false); +} + +TEST(FilterBaseTest, MeasurementProcessing) +{ + using RobotLocalization::FilterDerived2; + + FilterDerived2 derived; + + Measurement meas; + + Eigen::VectorXd measurement(STATE_SIZE); + for (size_t i = 0; i < STATE_SIZE; ++i) + { + measurement[i] = 0.1 * static_cast(i); + } + + Eigen::MatrixXd measurementCovariance(STATE_SIZE, STATE_SIZE); + for (size_t i = 0; i < STATE_SIZE; ++i) + { + for (size_t j = 0; j < STATE_SIZE; ++j) + { + measurementCovariance(i, j) = 0.1 * static_cast(i * j); + } + } + + meas.topicName_ = "odomTest"; + meas.measurement_ = measurement; + meas.covariance_ = measurementCovariance; + meas.updateVector_.resize(STATE_SIZE, true); + meas.time_ = 1000; + + // The filter shouldn't be initializedyet + EXPECT_FALSE(derived.getInitializedStatus()); + + derived.processMeasurement(meas); + + // Now it's initialized, and the entire filter state + // should be equal to the first state + EXPECT_TRUE(derived.getInitializedStatus()); + EXPECT_EQ(derived.getState(), measurement); + + // Process a measurement and make sure it updates the + // lastMeasurementTime variable + meas.time_ = 1002; + derived.processMeasurement(meas); + EXPECT_EQ(derived.getLastMeasurementTime(), meas.time_); +} + +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/Localizations/Packages/robot_localization/test/test_filter_base_diagnostics_timestamps.cpp b/Localizations/Packages/robot_localization/test/test_filter_base_diagnostics_timestamps.cpp new file mode 100644 index 0000000..0fa6fda --- /dev/null +++ b/Localizations/Packages/robot_localization/test/test_filter_base_diagnostics_timestamps.cpp @@ -0,0 +1,434 @@ +/* + * Copyright (c) 2014, 2015, 2016, Charles River Analytics, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include +#include +#include + +#include "robot_localization/filter_base.h" +#include "robot_localization/filter_common.h" +#include "robot_localization/SetPose.h" + + +#include + +#include + +#include + +namespace RobotLocalization +{ + +/* + Convenience functions to get valid messages. +*/ + +geometry_msgs::PoseWithCovarianceStamped getValidPose() +{ + geometry_msgs::PoseWithCovarianceStamped pose_msg; + pose_msg.header.frame_id = "base_link"; + pose_msg.pose.pose.position.x = 1; + pose_msg.pose.pose.orientation.w = 1; + for (size_t i = 0; i < 6 ; i++) + { + pose_msg.pose.covariance[i*6 + i] = 1; + } + return pose_msg; +} + +geometry_msgs::TwistWithCovarianceStamped getValidTwist() +{ + geometry_msgs::TwistWithCovarianceStamped twist_msg; + twist_msg.header.frame_id = "base_link"; + for (size_t i = 0; i < 6 ; i++) + { + twist_msg.twist.covariance[i*6 + i] = 1; + } + return twist_msg; +} + + +sensor_msgs::Imu getValidImu() +{ + sensor_msgs::Imu imu_msg; + imu_msg.header.frame_id = "base_link"; + imu_msg.orientation.w = 1; + for (size_t i = 0; i < 3 ; i++) + { + imu_msg.orientation_covariance[i * 3 + i] = 1; + imu_msg.angular_velocity_covariance[i * 3 + i] = 1; + imu_msg.linear_acceleration_covariance[i * 3 + i] = 1; + } + return imu_msg; +} + +nav_msgs::Odometry getValidOdometry() +{ + nav_msgs::Odometry odom_msg; + odom_msg.header.frame_id = "odom"; + odom_msg.child_frame_id = "base_link"; + odom_msg.pose = getValidPose().pose; + odom_msg.twist = getValidTwist().twist; + return odom_msg; +} + +/* + Helper class to handle the setup and message publishing for the testcases. + + It provides convenience to send valid messages with a specified timestamp. + + All diagnostic messages are stored into the public diagnostics attribute. +*/ +class DiagnosticsHelper +{ + private: + ros::Publisher odom_pub_; + ros::Publisher pose_pub_; + ros::Publisher twist_pub_; + ros::Publisher imu_pub_; + + geometry_msgs::PoseWithCovarianceStamped pose_msg_; + geometry_msgs::TwistWithCovarianceStamped twist_msg_; + nav_msgs::Odometry odom_msg_; + sensor_msgs::Imu imu_msg_; + + ros::Subscriber diagnostic_sub_; + ros::ServiceClient set_pose_; + + public: + std::vector< diagnostic_msgs::DiagnosticArray > diagnostics; + + DiagnosticsHelper() + { + ros::NodeHandle nh; + ros::NodeHandle nhLocal("~"); + + // ready the valid messages. + pose_msg_ = getValidPose(); + twist_msg_ = getValidTwist(); + odom_msg_ = getValidOdometry(); + imu_msg_ = getValidImu(); + + // subscribe to diagnostics and create publishers for the odometry messages. + odom_pub_ = nh.advertise("example/odom", 10); + pose_pub_ = nh.advertise("example/pose", 10); + twist_pub_ = nh.advertise("example/twist", 10); + imu_pub_ = nh.advertise("example/imu/data", 10); + + diagnostic_sub_ = nh.subscribe("/diagnostics", 10, &DiagnosticsHelper::diagnosticCallback, this); + set_pose_ = nh.serviceClient("/set_pose"); + } + + void diagnosticCallback(const diagnostic_msgs::DiagnosticArrayPtr &msg) + { + diagnostics.push_back(*msg); + } + + void publishMessages(ros::Time t) + { + odom_msg_.header.stamp = t; + odom_msg_.header.seq++; + odom_pub_.publish(odom_msg_); + + pose_msg_.header.stamp = t; + pose_msg_.header.seq++; + pose_pub_.publish(pose_msg_); + + twist_msg_.header.stamp = t; + twist_msg_.header.seq++; + twist_pub_.publish(twist_msg_); + + imu_msg_.header.stamp = t; + imu_msg_.header.seq++; + imu_pub_.publish(imu_msg_); + } + + void setPose(ros::Time t) + { + robot_localization::SetPose pose_; + pose_.request.pose = getValidPose(); + pose_.request.pose.header.stamp = t; + set_pose_.call(pose_); + } +}; + +} // namespace RobotLocalization + +/* + First test; we run for a bit; then send messagse with an empty timestamp. + Then we check if the diagnostics showed a warning. +*/ +TEST(FilterBaseDiagnosticsTest, EmptyTimestamps) +{ + RobotLocalization::DiagnosticsHelper dh_; + + // keep track of which diagnostic messages are detected. + bool received_warning_imu = false; + bool received_warning_odom = false; + bool received_warning_twist = false; + bool received_warning_pose = false; + + // For about a second, send correct messages. + ros::Rate loopRate(10); + for (size_t i = 0; i < 10; ++i) + { + ros::spinOnce(); + dh_.publishMessages(ros::Time::now()); + loopRate.sleep(); + } + + ros::spinOnce(); + + // create an empty timestamp and send all messages with this empty timestamp. + ros::Time empty; + empty.fromSec(0); + + dh_.publishMessages(empty); + + ros::spinOnce(); + + // The filter runs and sends the diagnostics every second. + // Just run this for two seconds to ensure we get all the diagnostic message. + for (size_t i = 0; i < 20; ++i) + { + ros::spinOnce(); + loopRate.sleep(); + } + + /* + Now the diagnostic messages have to be investigated to see whether they contain our warning. + */ + for (size_t i=0; i < dh_.diagnostics.size(); i++) + { + for (size_t status_index=0; status_index < dh_.diagnostics[i].status.size(); status_index++) + { + for (size_t key=0; key < dh_.diagnostics[i].status[status_index].values.size(); key++) + { + diagnostic_msgs::KeyValue kv = dh_.diagnostics[i].status[status_index].values[key]; + // Now the keys can be checked to see whether we found our warning. + + if (kv.key == "imu0_timestamp") + { + received_warning_imu = true; + } + if (kv.key == "odom0_timestamp") + { + received_warning_odom = true; + } + if (kv.key == "twist0_timestamp") + { + received_warning_twist = true; + } + if (kv.key == "pose0_timestamp") + { + received_warning_pose = true; + } + } + } + } + EXPECT_TRUE(received_warning_imu); + EXPECT_TRUE(received_warning_odom); + EXPECT_TRUE(received_warning_twist); + EXPECT_TRUE(received_warning_pose); +} + +TEST(FilterBaseDiagnosticsTest, TimestampsBeforeSetPose) +{ + RobotLocalization::DiagnosticsHelper dh_; + + // keep track of which diagnostic messages are detected. + bool received_warning_imu = false; + bool received_warning_odom = false; + bool received_warning_twist = false; + bool received_warning_pose = false; + + // For about a second, send correct messages. + ros::Rate loopRate(10); + for (size_t i = 0; i < 10; ++i) + { + ros::spinOnce(); + dh_.publishMessages(ros::Time::now()); + loopRate.sleep(); + } + ros::spinOnce(); + + // Set the pose to the current timestamp. + dh_.setPose(ros::Time::now()); + ros::spinOnce(); + + // send messages from one second before that pose reset. + dh_.publishMessages(ros::Time::now() - ros::Duration(1)); + + // The filter runs and sends the diagnostics every second. + // Just run this for two seconds to ensure we get all the diagnostic message. + for (size_t i = 0; i < 20; ++i) + { + ros::spinOnce(); + loopRate.sleep(); + } + + /* + Now the diagnostic messages have to be investigated to see whether they contain our warning. + */ + for (size_t i=0; i < dh_.diagnostics.size(); i++) + { + for (size_t status_index=0; status_index < dh_.diagnostics[i].status.size(); status_index++) + { + for (size_t key=0; key < dh_.diagnostics[i].status[status_index].values.size(); key++) + { + diagnostic_msgs::KeyValue kv = dh_.diagnostics[i].status[status_index].values[key]; + // Now the keys can be checked to see whether we found our warning. + + if (kv.key == "imu0_timestamp") + { + received_warning_imu = true; + } + if (kv.key == "odom0_timestamp") + { + received_warning_odom = true; + } + if (kv.key == "twist0_timestamp") + { + received_warning_twist = true; + } + if (kv.key == "pose0_timestamp") + { + received_warning_pose = true; + } + } + } + } + EXPECT_TRUE(received_warning_imu); + EXPECT_TRUE(received_warning_odom); + EXPECT_TRUE(received_warning_twist); + EXPECT_TRUE(received_warning_pose); +} + +TEST(FilterBaseDiagnosticsTest, TimestampsBeforePrevious) +{ + RobotLocalization::DiagnosticsHelper dh_; + + // keep track of which diagnostic messages are detected. + // we have more things to check here because the messages get split over + // various callbacks if they pass the check if they predate the set_pose time. + bool received_warning_imu_accel = false; + bool received_warning_imu_pose = false; + bool received_warning_imu_twist = false; + bool received_warning_odom_twist = false; + bool received_warning_twist = false; + bool received_warning_pose = false; + + // For two seconds send correct messages. + ros::Rate loopRate(10); + for (size_t i = 0; i < 20; ++i) + { + ros::spinOnce(); + dh_.publishMessages(ros::Time::now()); + loopRate.sleep(); + } + ros::spinOnce(); + + // Send message that is one second in the past. + dh_.publishMessages(ros::Time::now() - ros::Duration(1)); + + // The filter runs and sends the diagnostics every second. + // Just run this for two seconds to ensure we get all the diagnostic message. + for (size_t i = 0; i < 20; ++i) + { + ros::spinOnce(); + loopRate.sleep(); + } + + /* + Now the diagnostic messages have to be investigated to see whether they contain our warning. + */ + for (size_t i=0; i < dh_.diagnostics.size(); i++) + { + for (size_t status_index=0; status_index < dh_.diagnostics[i].status.size(); status_index++) + { + for (size_t key=0; key < dh_.diagnostics[i].status[status_index].values.size(); key++) + { + diagnostic_msgs::KeyValue kv = dh_.diagnostics[i].status[status_index].values[key]; + // Now the keys can be checked to see whether we found our warning. + + if (kv.key == "imu0_acceleration_timestamp") + { + received_warning_imu_accel = true; + } + if (kv.key == "imu0_pose_timestamp") + { + received_warning_imu_pose = true; + } + if (kv.key == "imu0_twist_timestamp") + { + received_warning_imu_twist = true; + } + + if (kv.key == "odom0_twist_timestamp") + { + received_warning_twist = true; + } + + if (kv.key == "pose0_timestamp") + { + received_warning_pose = true; + } + if (kv.key == "twist0_timestamp") + { + received_warning_odom_twist = true; + } + } + } + } + + EXPECT_TRUE(received_warning_imu_accel); + EXPECT_TRUE(received_warning_imu_pose); + EXPECT_TRUE(received_warning_imu_twist); + EXPECT_TRUE(received_warning_odom_twist); + EXPECT_TRUE(received_warning_pose); + EXPECT_TRUE(received_warning_twist); +} + + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "filter_base_diagnostics_timestamps-test-interfaces"); + ros::Time::init(); + + // Give ekf_localization_node time to initialize + ros::Duration(2.0).sleep(); + + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/Localizations/Packages/robot_localization/test/test_filter_base_diagnostics_timestamps.test b/Localizations/Packages/robot_localization/test/test_filter_base_diagnostics_timestamps.test new file mode 100644 index 0000000..40cf75b --- /dev/null +++ b/Localizations/Packages/robot_localization/test/test_filter_base_diagnostics_timestamps.test @@ -0,0 +1,44 @@ + + + + + + + + + + + + [false, false, false, + false, false, false, + true, false, false, + false, false, false, + false, false, false] + + [true, true, false, + false, false, false, + false, false, false, + false, false, false, + false, false, false] + + [false, false, false, + false, false, false, + true, true, true, + true, true, true, + false, false, false] + + [false, false, false, + true, true, true, + false, false, false, + true, true, true, + true, true, true] + + + + + + + + diff --git a/Localizations/Packages/robot_localization/test/test_localization_node_bag_pose_tester.cpp b/Localizations/Packages/robot_localization/test/test_localization_node_bag_pose_tester.cpp new file mode 100644 index 0000000..a38fa58 --- /dev/null +++ b/Localizations/Packages/robot_localization/test/test_localization_node_bag_pose_tester.cpp @@ -0,0 +1,111 @@ +/* + * Copyright (c) 2014, 2015, 2016, Charles River Analytics, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include + +#include + +#include +#include +#include + +nav_msgs::Odometry filtered_; + +void filterCallback(const nav_msgs::OdometryConstPtr &msg) +{ + filtered_ = *msg; +} + +TEST(BagTest, PoseCheck) +{ + ros::NodeHandle nh; + ros::NodeHandle nhLocal("~"); + + double finalX = 0; + double finalY = 0; + double finalZ = 0; + double tolerance = 0; + bool outputFinalPosition = false; + std::string finalPositionFile; + + nhLocal.getParam("final_x", finalX); + nhLocal.getParam("final_y", finalY); + nhLocal.getParam("final_z", finalZ); + nhLocal.getParam("tolerance", tolerance); + nhLocal.param("output_final_position", outputFinalPosition, false); + nhLocal.param("output_location", finalPositionFile, std::string("test.txt")); + + ros::Subscriber filteredSub = nh.subscribe("/odometry/filtered", 1, &filterCallback); + + while (ros::ok()) + { + ros::spinOnce(); + ros::Duration(0.0333333).sleep(); + } + + if (outputFinalPosition) + { + try + { + std::ofstream posOut; + posOut.open(finalPositionFile.c_str(), std::ofstream::app); + posOut << filtered_.pose.pose.position.x << " " << filtered_.pose.pose.position.y << " " << + filtered_.pose.pose.position.z << std::endl; + posOut.close(); + } + catch(...) + { + ROS_ERROR_STREAM("Unable to open output file.\n"); + } + } + + double xDiff = filtered_.pose.pose.position.x - finalX; + double yDiff = filtered_.pose.pose.position.y - finalY; + double zDiff = filtered_.pose.pose.position.z - finalZ; + + EXPECT_LT(::sqrt(xDiff*xDiff + yDiff*yDiff + zDiff*zDiff), tolerance); +} + +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + + ros::init(argc, argv, "localization_node-bag-pose-tester"); + ros::Time::init(); + + // Give ekf_localization_node time to initialize + ros::Duration(2.0).sleep(); + + return RUN_ALL_TESTS(); +} + diff --git a/Localizations/Packages/robot_localization/test/test_navsat_conversions.cpp b/Localizations/Packages/robot_localization/test/test_navsat_conversions.cpp new file mode 100644 index 0000000..15f85ab --- /dev/null +++ b/Localizations/Packages/robot_localization/test/test_navsat_conversions.cpp @@ -0,0 +1,69 @@ +/* + * Copyright (c) 2021, Charles River Analytics, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "robot_localization/navsat_conversions.h" + +#include + +#include + +void NavsatConversionsTest(const double lat, const double lon, + const double UTMNorthing, const double UTMEasting, + const std::string UTMZone, const double gamma) +{ + double UTMNorthing_new; + double UTMEasting_new; + std::string UTMZone_new; + double gamma_new; + RobotLocalization::NavsatConversions::LLtoUTM(lat, lon, UTMNorthing_new, UTMEasting_new, UTMZone_new, gamma_new); + EXPECT_NEAR(UTMNorthing, UTMNorthing_new, 1e-2); + EXPECT_NEAR(UTMEasting, UTMEasting_new, 1e-2); + EXPECT_EQ(UTMZone, UTMZone_new); + EXPECT_NEAR(gamma, gamma_new, 1e-2); + double lat_new; + double lon_new; + RobotLocalization::NavsatConversions::UTMtoLL(UTMNorthing, UTMEasting, UTMZone, lat_new, lon_new); + EXPECT_NEAR(lat_new, lat, 1e-5); + EXPECT_NEAR(lon_new, lon, 1e-5); +} + +TEST(NavsatConversionsTest, UtmTest) +{ + NavsatConversionsTest(51.423964, 5.494271, 5699924.709, 673409.989, "31U", 1.950); + NavsatConversionsTest(-43.530955, 172.636645, 5178919.718, 632246.802, "59G", -1.127); +} + +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/Localizations/Packages/robot_localization/test/test_navsat_transform.cpp b/Localizations/Packages/robot_localization/test/test_navsat_transform.cpp new file mode 100644 index 0000000..72db4e5 --- /dev/null +++ b/Localizations/Packages/robot_localization/test/test_navsat_transform.cpp @@ -0,0 +1,91 @@ +/* + * Copyright (c) 2021, Charles River Analytics, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "robot_localization/navsat_transform.h" +#include +#include +#include + +#include + +#include + +TEST(NavSatTransformUTMJumpTest, UtmTest) +{ + ros::NodeHandle nh; + ros::ServiceClient set_datum_client = nh.serviceClient("/datum"); + ros::ServiceClient from_ll_client = nh.serviceClient("/fromLL"); + + EXPECT_TRUE(set_datum_client.waitForExistence(ros::Duration(5))); + + // Initialise the navsat_transform node to a UTM zone + robot_localization::SetDatum set_datum_srv; + set_datum_srv.request.geo_pose.position.latitude = 1; + set_datum_srv.request.geo_pose.position.longitude = 4; + set_datum_srv.request.geo_pose.orientation.w = 1; + EXPECT_TRUE(set_datum_client.call(set_datum_srv)); + + // Let the node figure out its transforms + ros::Duration(0.2).sleep(); + + // Request the GPS point of said point: + robot_localization::FromLL from_ll_srv; + from_ll_srv.request.ll_point.latitude = 10; + from_ll_srv.request.ll_point.longitude = 4.5; + EXPECT_TRUE(from_ll_client.call(from_ll_srv)); + auto initial_response = from_ll_srv.response; + + // Request GPS point also in that zone + from_ll_srv.request.ll_point.longitude = 5.5; + EXPECT_TRUE(from_ll_client.call(from_ll_srv)); + auto same_zone_response = from_ll_srv.response; + + // 1° of longitude is about 111 kilometers in length + EXPECT_NEAR(initial_response.map_point.x, same_zone_response.map_point.x, 120e3); + EXPECT_NEAR(initial_response.map_point.y, same_zone_response.map_point.y, 120e3); + + // Request GPS point from neighboring zone (zone crossing is at 6 degrees) + from_ll_srv.request.ll_point.longitude = 6.5; + from_ll_client.call(from_ll_srv); + auto neighbour_zone_response = from_ll_srv.response; + + EXPECT_NEAR(initial_response.map_point.x, neighbour_zone_response.map_point.x, 2*120e3); + EXPECT_NEAR(initial_response.map_point.y, neighbour_zone_response.map_point.y, 2*120e3); +} + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "test_navsat_transform"); + + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/Localizations/Packages/robot_localization/test/test_navsat_transform.test b/Localizations/Packages/robot_localization/test/test_navsat_transform.test new file mode 100644 index 0000000..d8873b2 --- /dev/null +++ b/Localizations/Packages/robot_localization/test/test_navsat_transform.test @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/Localizations/Packages/robot_localization/test/test_robot_localization_estimator.cpp b/Localizations/Packages/robot_localization/test/test_robot_localization_estimator.cpp new file mode 100644 index 0000000..757423e --- /dev/null +++ b/Localizations/Packages/robot_localization/test/test_robot_localization_estimator.cpp @@ -0,0 +1,169 @@ +/* + * Copyright (c) 2014, 2015, 2016, Charles River Analytics, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "robot_localization/robot_localization_estimator.h" + +#include +#include + +#include + +TEST(RLETest, StateBuffer) +{ + // Generate a few estimator states + std::vector states; + + for ( int i = 0; i < 10; i++ ) + { + /* + * t = i s; + * x = i m; + * vx = 1.0 m/s; + */ + RobotLocalization::EstimatorState state; + state.time_stamp = i; + state.state(RobotLocalization::StateMemberX) = i; + state.state(RobotLocalization::StateMemberY) = 0; + state.state(RobotLocalization::StateMemberZ) = 0; + + state.state(RobotLocalization::StateMemberRoll) = 0; + state.state(RobotLocalization::StateMemberPitch) = 0; + state.state(RobotLocalization::StateMemberYaw) = 0; + + state.state(RobotLocalization::StateMemberVx) = 1; + state.state(RobotLocalization::StateMemberVy) = 0; + state.state(RobotLocalization::StateMemberVz) = 0; + + state.state(RobotLocalization::StateMemberVroll) = 0; + state.state(RobotLocalization::StateMemberVpitch) = 0; + state.state(RobotLocalization::StateMemberVyaw) = 0; + + state.state(RobotLocalization::StateMemberAx) = 0; + state.state(RobotLocalization::StateMemberAy) = 0; + state.state(RobotLocalization::StateMemberAz) = 0; + states.push_back(state); + } + + // Instantiate a robot localization estimator with a buffer capacity of 5 + int buffer_capacity = 5; + Eigen::MatrixXd process_noise_covariance = Eigen::MatrixXd::Identity(RobotLocalization::STATE_SIZE, + RobotLocalization::STATE_SIZE); + RobotLocalization::RobotLocalizationEstimator estimator(buffer_capacity, RobotLocalization::FilterTypes::EKF, + process_noise_covariance); + + RobotLocalization::EstimatorState state; + + // Add the states in chronological order + for ( int i = 0; i < 6; i++ ) + { + estimator.setState(states[i]); + + // Check that the state is added correctly + estimator.getState(states[i].time_stamp, state); + EXPECT_EQ(state.time_stamp, states[i].time_stamp); + } + + // We filled the buffer with more states that it can hold, so its size should now be equal to the capacity + EXPECT_EQ(static_cast(estimator.getSize()), buffer_capacity); + + // Clear the buffer and check if it's really empty afterwards + estimator.clearBuffer(); + EXPECT_EQ(estimator.getSize(), 0u); + + // Add states at time 1 through 3 inclusive to the buffer (buffer is not yet full) + for ( int i = 1; i < 4; i++ ) + { + estimator.setState(states[i]); + } + + // Now add a state at time 0, but let's change it a bit (set StateMemberY=12) so that we can inspect if it is + // correctly added to the buffer. + RobotLocalization::EstimatorState state_2 = states[0]; + state_2.state(RobotLocalization::StateMemberY) = 12; + estimator.setState(state_2); + EXPECT_EQ(RobotLocalization::EstimatorResults::Exact, + estimator.getState(states[0].time_stamp, state)); + + // Check if the state is correctly added + EXPECT_EQ(state.state, state_2.state); + + // Add some more states. State at t=0 should now be dropped, so we should get the prediction, which means y=0 + for ( int i = 5; i < 8; i++ ) + { + estimator.setState(states[i]); + } + EXPECT_EQ(RobotLocalization::EstimatorResults::ExtrapolationIntoPast, + estimator.getState(states[0].time_stamp, state)); + EXPECT_EQ(states[0].state, state.state); + + // Estimate a state that is not in the buffer, but can be determined by interpolation. The predicted state vector + // should be equal to the designed state at the requested time. + EXPECT_EQ(RobotLocalization::EstimatorResults::Interpolation, + estimator.getState(states[4].time_stamp, state)); + EXPECT_EQ(states[4].state, state.state); + + // Estimate a state that is not in the buffer, but can be determined by extrapolation into the future. The predicted + // state vector should be equal to the designed state at the requested time. + EXPECT_EQ(RobotLocalization::EstimatorResults::ExtrapolationIntoFuture, + estimator.getState(states[8].time_stamp, state)); + EXPECT_EQ(states[8].state, state.state); + + // Add missing state somewhere in the middle + estimator.setState(states[4]); + + // Overwrite state at t=3 (oldest state now in the buffer) and check if it's correctly overwritten. + state_2 = states[3]; + state_2.state(RobotLocalization::StateMemberVy) = -1.0; + estimator.setState(state_2); + EXPECT_EQ(RobotLocalization::EstimatorResults::Exact, + estimator.getState(states[3].time_stamp, state)); + EXPECT_EQ(state_2.state, state.state); + + // Add state that came too late + estimator.setState(states[0]); + + // Check if getState needed to do extrapolation into the past + EXPECT_EQ(estimator.getState(states[0].time_stamp, state), + RobotLocalization::EstimatorResults::ExtrapolationIntoPast); + + // Check state at t=0. This can only work correctly if the state at t=3 is + // overwritten and the state at zero is not in the buffer. + EXPECT_DOUBLE_EQ(3.0, state.state(RobotLocalization::StateMemberY)); +} + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "test_robot_localization_estimator"); + + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/Localizations/Packages/robot_localization/test/test_robot_localization_estimator.test b/Localizations/Packages/robot_localization/test/test_robot_localization_estimator.test new file mode 100644 index 0000000..ebd8db5 --- /dev/null +++ b/Localizations/Packages/robot_localization/test/test_robot_localization_estimator.test @@ -0,0 +1,7 @@ + + + + + + + diff --git a/Localizations/Packages/robot_localization/test/test_ros_robot_localization_listener.cpp b/Localizations/Packages/robot_localization/test/test_ros_robot_localization_listener.cpp new file mode 100644 index 0000000..00cfc88 --- /dev/null +++ b/Localizations/Packages/robot_localization/test/test_ros_robot_localization_listener.cpp @@ -0,0 +1,132 @@ +/* + * Copyright (c) 2016, TNO IVS, Helmond + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "robot_localization/ros_robot_localization_listener.h" +#include "robot_localization/filter_common.h" + +#include + +#include +#include + +#include + +RobotLocalization::RosRobotLocalizationListener* g_listener; + +TEST(LocalizationListenerTest, testGetStateOfBaseLink) +{ + ros::spinOnce(); + + ros::Time time2(1001); + + Eigen::VectorXd state(RobotLocalization::STATE_SIZE); + Eigen::MatrixXd covariance(RobotLocalization::STATE_SIZE, RobotLocalization::STATE_SIZE); + + std::string base_frame("base_link"); + g_listener->getState(time2, base_frame, state, covariance); + + EXPECT_DOUBLE_EQ(1.0, state(RobotLocalization::StateMemberX)); + EXPECT_DOUBLE_EQ(0.0, state(RobotLocalization::StateMemberY)); + EXPECT_DOUBLE_EQ(0.0, state(RobotLocalization::StateMemberZ)); + + EXPECT_FLOAT_EQ(M_PI/4, state(RobotLocalization::StateMemberRoll)); + EXPECT_FLOAT_EQ(0.0, state(RobotLocalization::StateMemberPitch)); + EXPECT_FLOAT_EQ(0.0, state(RobotLocalization::StateMemberYaw)); + + EXPECT_DOUBLE_EQ(M_PI/4.0, state(RobotLocalization::StateMemberVroll)); + EXPECT_DOUBLE_EQ(0.0, state(RobotLocalization::StateMemberVpitch)); + EXPECT_DOUBLE_EQ(0.0, state(RobotLocalization::StateMemberVyaw)); +} + +TEST(LocalizationListenerTest, GetStateOfRelatedFrame) +{ + ros::spinOnce(); + + Eigen::VectorXd state(RobotLocalization::STATE_SIZE); + Eigen::MatrixXd covariance(RobotLocalization::STATE_SIZE, RobotLocalization::STATE_SIZE); + + ros::Time time1(1000); + ros::Time time2(1001); + + std::string sensor_frame("sensor"); + + EXPECT_TRUE(g_listener->getState(time1, sensor_frame, state, covariance) ); + + EXPECT_FLOAT_EQ(0.0, state(RobotLocalization::StateMemberX)); + EXPECT_FLOAT_EQ(1.0, state(RobotLocalization::StateMemberY)); + EXPECT_FLOAT_EQ(0.0, state(RobotLocalization::StateMemberZ)); + + EXPECT_FLOAT_EQ(0.0, state(RobotLocalization::StateMemberRoll)); + EXPECT_FLOAT_EQ(0.0, state(RobotLocalization::StateMemberPitch)); + EXPECT_FLOAT_EQ(M_PI/2, state(RobotLocalization::StateMemberYaw)); + + EXPECT_TRUE(1e-12 > state(RobotLocalization::StateMemberVx)); + EXPECT_FLOAT_EQ(-1.0, state(RobotLocalization::StateMemberVy)); + EXPECT_FLOAT_EQ(M_PI/4.0, state(RobotLocalization::StateMemberVz)); + + EXPECT_TRUE(1e-12 > state(RobotLocalization::StateMemberVroll)); + EXPECT_FLOAT_EQ(-M_PI/4.0, state(RobotLocalization::StateMemberVpitch)); + EXPECT_FLOAT_EQ(0.0, state(RobotLocalization::StateMemberVyaw)); + + EXPECT_TRUE(g_listener->getState(time2, sensor_frame, state, covariance)); + + EXPECT_FLOAT_EQ(1.0, state(RobotLocalization::StateMemberX)); + EXPECT_FLOAT_EQ(sqrt(2)/2.0, state(RobotLocalization::StateMemberY)); + EXPECT_FLOAT_EQ(sqrt(2)/2.0, state(RobotLocalization::StateMemberZ)); + + EXPECT_TRUE(1e-12 > state(RobotLocalization::StateMemberRoll)); + EXPECT_TRUE(1e-12 > fabs(-M_PI/4.0 - state(RobotLocalization::StateMemberPitch))); + EXPECT_FLOAT_EQ(M_PI/2, state(RobotLocalization::StateMemberYaw)); + + EXPECT_TRUE(1e-12 > state(RobotLocalization::StateMemberVx)); + EXPECT_FLOAT_EQ(-1.0, state(RobotLocalization::StateMemberVy)); + EXPECT_FLOAT_EQ(M_PI/4, state(RobotLocalization::StateMemberVz)); + + EXPECT_TRUE(1e-12 > state(RobotLocalization::StateMemberVroll)); + EXPECT_FLOAT_EQ(-M_PI/4.0, state(RobotLocalization::StateMemberVpitch)); + EXPECT_FLOAT_EQ(0, state(RobotLocalization::StateMemberVyaw)); +} + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "test_robot_localization_estimator"); + + g_listener = new RobotLocalization::RosRobotLocalizationListener(); + + testing::InitGoogleTest(&argc, argv); + + int res = RUN_ALL_TESTS(); + + delete g_listener; + + return res; +} diff --git a/Localizations/Packages/robot_localization/test/test_ros_robot_localization_listener.test b/Localizations/Packages/robot_localization/test/test_ros_robot_localization_listener.test new file mode 100644 index 0000000..e706abd --- /dev/null +++ b/Localizations/Packages/robot_localization/test/test_ros_robot_localization_listener.test @@ -0,0 +1,68 @@ + + + + + + + + + + + + + + + + + + + + + + + + [true, false, true, false, false, false, false, false, false, false, false, false, false, false, false] + [true, true, true, true, true, true, false, false, false, false, false, false, false, false, false] + [false, false, false, false, false, false, true, true, true, true, true, true, false, false, false] + + [true, false, true, false, false, false, false, false, false, false, false, false, false, false, false] + [true, true, true, true, true, true, false, false, false, false, false, false, false, false, false] + + [false, false, false, false, false, false, true, true, true, true, true, true, false, false, false] + + [false, false, false, true, true, true, false, false, false, false, false, false, false, false, false] + [false, false, false, false, false, false, false, false, false, true, true, true, false, false, false] + [false, false, false, false, false, false, false, false, false, false, false, false, true, true, true] + [false, false, false, true, true, true, false, false, false, false, false, false, false, false, false] + + + + + + + + + + + [0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.4, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.002, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.002, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.004, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01] + + + + + + + diff --git a/Localizations/Packages/robot_localization/test/test_ros_robot_localization_listener_publisher.cpp b/Localizations/Packages/robot_localization/test/test_ros_robot_localization_listener_publisher.cpp new file mode 100644 index 0000000..5dbff22 --- /dev/null +++ b/Localizations/Packages/robot_localization/test/test_ros_robot_localization_listener_publisher.cpp @@ -0,0 +1,117 @@ +/* + * Copyright (c) 2016, TNO IVS, Helmond + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include + +#include +#include +#include + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "test_robot_localization_listener_publisher"); + + ros::NodeHandle nh; + ros::Publisher odom_pub = nh.advertise("odometry/filtered", 1); + ros::Publisher accel_pub = nh.advertise("accel/filtered", 1); + tf2_ros::StaticTransformBroadcaster transform_broadcaster; + + ros::Time end_time = ros::Time::now() + ros::Duration(10); + while (ros::ok() && ros::Time::now() < end_time) + { + ros::Time time1(1000); + double x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az; + x = y = z = roll = pitch = yaw = vy = vz = vroll = vpitch = vyaw = ax = ay = az = 0.0; + vx = 1.0; + vroll = M_PI/4.0; + + tf2::Quaternion q; + q.setRPY(0, 0, 0); + + nav_msgs::Odometry odom_msg; + odom_msg.header.stamp = time1; + odom_msg.header.frame_id = "map"; + odom_msg.child_frame_id = "base_link"; + odom_msg.pose.pose.position.x = x; + odom_msg.pose.pose.position.y = y; + odom_msg.pose.pose.position.y = z; + odom_msg.pose.pose.orientation.x = q.x(); + odom_msg.pose.pose.orientation.y = q.y(); + odom_msg.pose.pose.orientation.z = q.z(); + odom_msg.pose.pose.orientation.w = q.w(); + + odom_msg.twist.twist.linear.x = vx; + odom_msg.twist.twist.linear.y = vy; + odom_msg.twist.twist.linear.z = vz; + odom_msg.twist.twist.angular.x = vroll; + odom_msg.twist.twist.angular.y = vpitch; + odom_msg.twist.twist.angular.z = vyaw; + + geometry_msgs::AccelWithCovarianceStamped accel_msg; + accel_msg.header.stamp = time1; + accel_msg.header.frame_id = "base_link"; + accel_msg.accel.accel.linear.x = ax; + accel_msg.accel.accel.linear.y = ay; + accel_msg.accel.accel.linear.z = az; + accel_msg.accel.accel.angular.x = 0; + accel_msg.accel.accel.angular.y = 0; + accel_msg.accel.accel.angular.z = 0; + + odom_pub.publish(odom_msg); + accel_pub.publish(accel_msg); + + geometry_msgs::TransformStamped transformStamped; + + transformStamped.header.stamp = ros::Time::now(); + transformStamped.header.frame_id = "base_link"; + transformStamped.child_frame_id = "sensor"; + transformStamped.transform.translation.x = 0.0; + transformStamped.transform.translation.y = 1.0; + transformStamped.transform.translation.z = 0.0; + { + tf2::Quaternion q; + q.setRPY(0, 0, M_PI/2); + transformStamped.transform.rotation.x = q.x(); + transformStamped.transform.rotation.y = q.y(); + transformStamped.transform.rotation.z = q.z(); + transformStamped.transform.rotation.w = q.w(); + + transform_broadcaster.sendTransform(transformStamped); + } + + ros::Duration(0.1).sleep(); + } + + return 0; +} diff --git a/Localizations/Packages/robot_localization/test/test_ukf.cpp b/Localizations/Packages/robot_localization/test/test_ukf.cpp new file mode 100644 index 0000000..4348863 --- /dev/null +++ b/Localizations/Packages/robot_localization/test/test_ukf.cpp @@ -0,0 +1,141 @@ +/* + * Copyright (c) 2014, 2015, 2016, Charles River Analytics, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "robot_localization/ros_filter_types.h" + +#include + +#include +#include + +using RobotLocalization::Ukf; +using RobotLocalization::RosUkf; +using RobotLocalization::STATE_SIZE; + +class RosUkfPassThrough : public RosUkf +{ + public: + explicit RosUkfPassThrough(std::vector &args) : RosUkf(ros::NodeHandle(), ros::NodeHandle("~"), args) + { + } + + Ukf &getFilter() + { + return filter_; + } +}; + +TEST(UkfTest, Measurements) +{ + std::vector args; + args.push_back(0.001); + args.push_back(0); + args.push_back(2); + + RosUkfPassThrough ukf(args); + + Eigen::MatrixXd initialCovar(15, 15); + initialCovar.setIdentity(); + initialCovar *= 0.5; + ukf.getFilter().setEstimateErrorCovariance(initialCovar); + + EXPECT_EQ(ukf.getFilter().getEstimateErrorCovariance(), initialCovar); + + Eigen::VectorXd measurement(STATE_SIZE); + for (size_t i = 0; i < STATE_SIZE; ++i) + { + measurement[i] = i * 0.01 * STATE_SIZE; + } + + Eigen::MatrixXd measurementCovariance(STATE_SIZE, STATE_SIZE); + measurementCovariance.setIdentity(); + for (size_t i = 0; i < STATE_SIZE; ++i) + { + measurementCovariance(i, i) = 1e-9; + } + + std::vector updateVector(STATE_SIZE, true); + + // Ensure that measurements are being placed in the queue correctly + ros::Time time; + time.fromSec(1000); + ukf.enqueueMeasurement("odom0", + measurement, + measurementCovariance, + updateVector, + std::numeric_limits::max(), + time); + + ukf.integrateMeasurements(ros::Time(1001)); + + EXPECT_EQ(ukf.getFilter().getState(), measurement); + EXPECT_EQ(ukf.getFilter().getEstimateErrorCovariance(), measurementCovariance); + + ukf.getFilter().setEstimateErrorCovariance(initialCovar); + + // Now fuse another measurement and check the output. + // We know what the filter's state should be when + // this is complete, so we'll check the difference and + // make sure it's suitably small. + Eigen::VectorXd measurement2 = measurement; + + measurement2 *= 2.0; + + for (size_t i = 0; i < STATE_SIZE; ++i) + { + measurementCovariance(i, i) = 1e-9; + } + + time.fromSec(1002); + ukf.enqueueMeasurement("odom0", + measurement2, + measurementCovariance, + updateVector, + std::numeric_limits::max(), + time); + + ukf.integrateMeasurements(ros::Time(1003)); + + measurement = measurement2.eval() - ukf.getFilter().getState(); + for (size_t i = 0; i < STATE_SIZE; ++i) + { + EXPECT_LT(::fabs(measurement[i]), 0.001); + } +} + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "ukf"); + + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/Localizations/Packages/robot_localization/test/test_ukf.test b/Localizations/Packages/robot_localization/test/test_ukf.test new file mode 100644 index 0000000..211480a --- /dev/null +++ b/Localizations/Packages/robot_localization/test/test_ukf.test @@ -0,0 +1,7 @@ + + + + + + + diff --git a/Localizations/Packages/robot_localization/test/test_ukf_localization_node_bag1.test b/Localizations/Packages/robot_localization/test/test_ukf_localization_node_bag1.test new file mode 100644 index 0000000..1592c65 --- /dev/null +++ b/Localizations/Packages/robot_localization/test/test_ukf_localization_node_bag1.test @@ -0,0 +1,110 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + [false, false, false, + false, false, false, + true, true, true, + false, false, false, + false, false, false] + + [false, false, false, + true, true, true, + false, false, false, + true, true, false, + true, true, true] + + + + + + + + + + + + + + + + + + [0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0.06, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0.06, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0.04, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.02, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.015] + + [1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9] + + + + + + + + + + + + + + + + + diff --git a/Localizations/Packages/robot_localization/test/test_ukf_localization_node_bag2.test b/Localizations/Packages/robot_localization/test/test_ukf_localization_node_bag2.test new file mode 100644 index 0000000..5fbbe16 --- /dev/null +++ b/Localizations/Packages/robot_localization/test/test_ukf_localization_node_bag2.test @@ -0,0 +1,91 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + [false, false, false, + false, false, false, + true, true, true, + false, false, true, + false, false, false] + + [false, false, false, + true, true, true, + false, false, false, + true, true, true, + true, true, true] + + + + + + + [0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0.4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0.06, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0.05, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.002, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.002, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.004, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01] + + [1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9] + + + + + + + + + + + + + + + + + diff --git a/Localizations/Packages/robot_localization/test/test_ukf_localization_node_bag3.test b/Localizations/Packages/robot_localization/test/test_ukf_localization_node_bag3.test new file mode 100644 index 0000000..d2c0e6a --- /dev/null +++ b/Localizations/Packages/robot_localization/test/test_ukf_localization_node_bag3.test @@ -0,0 +1,79 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + [false, false, true, + false, false, false, + true, true, false, + false, false, true, + false, false, false] + + [false, false, false, + true, true, true, + false, false, false, + true, true, true, + false, false, false] + + + + + + + + + + + + + + + [0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.4, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.002, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.002, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.004, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01] + + + + + + + + + + + + + diff --git a/Localizations/Packages/robot_localization/test/test_ukf_localization_node_interfaces.cpp b/Localizations/Packages/robot_localization/test/test_ukf_localization_node_interfaces.cpp new file mode 100644 index 0000000..153acb0 --- /dev/null +++ b/Localizations/Packages/robot_localization/test/test_ukf_localization_node_interfaces.cpp @@ -0,0 +1,1079 @@ +/* + * Copyright (c) 2014, 2015, 2016, Charles River Analytics, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "robot_localization/SetPose.h" + +#include +#include +#include +#include +#include + +#include +#include + +#include + +#include + +nav_msgs::Odometry filtered_; + +bool stateUpdated_; + +void resetFilter() +{ + ros::NodeHandle nh; + ros::ServiceClient client = nh.serviceClient("/set_pose"); + + robot_localization::SetPose setPose; + setPose.request.pose.pose.pose.orientation.w = 1; + setPose.request.pose.header.frame_id = "odom"; + for (size_t ind = 0; ind < 36; ind+=7) + { + setPose.request.pose.pose.covariance[ind] = 1e-6; + } + + setPose.request.pose.header.stamp = ros::Time::now(); + client.call(setPose); + setPose.request.pose.header.seq++; + ros::spinOnce(); + ros::Duration(0.01).sleep(); + stateUpdated_ = false; + + double deltaX = 0.0; + double deltaY = 0.0; + double deltaZ = 0.0; + + while (!stateUpdated_ || ::sqrt(deltaX * deltaX + deltaY * deltaY + deltaZ * deltaZ) > 0.1) + { + ros::spinOnce(); + ros::Duration(0.01).sleep(); + + deltaX = filtered_.pose.pose.position.x - setPose.request.pose.pose.pose.position.x; + deltaY = filtered_.pose.pose.position.y - setPose.request.pose.pose.pose.position.y; + deltaZ = filtered_.pose.pose.position.z - setPose.request.pose.pose.pose.position.z; + } + + EXPECT_LT(::sqrt(deltaX * deltaX + deltaY * deltaY + deltaZ * deltaZ), 0.1); +} + +void filterCallback(const nav_msgs::OdometryConstPtr &msg) +{ + filtered_ = *msg; + stateUpdated_ = true; +} + +TEST(InterfacesTest, OdomPoseBasicIO) +{ + stateUpdated_ = false; + + ros::NodeHandle nh; + ros::Publisher odomPub = nh.advertise("/odom_input0", 5); + ros::Subscriber filteredSub = nh.subscribe("/odometry/filtered", 1, &filterCallback); + + nav_msgs::Odometry odom; + odom.pose.pose.position.x = 20.0; + odom.pose.pose.position.y = 10.0; + odom.pose.pose.position.z = -40.0; + + odom.pose.covariance[0] = 2.0; + odom.pose.covariance[7] = 2.0; + odom.pose.covariance[14] = 2.0; + + odom.header.frame_id = "odom"; + odom.child_frame_id = "base_link"; + + ros::Rate loopRate(50); + for (size_t i = 0; i < 50; ++i) + { + odom.header.stamp = ros::Time::now(); + odomPub.publish(odom); + ros::spinOnce(); + + loopRate.sleep(); + + odom.header.seq++; + } + + // Now check the values from the callback + EXPECT_LT(::fabs(filtered_.pose.pose.position.x - odom.pose.pose.position.x), 0.01); + EXPECT_LT(::fabs(filtered_.pose.pose.position.y), 0.01); // Configuration for this variable for this sensor is false + EXPECT_LT(::fabs(filtered_.pose.pose.position.z - odom.pose.pose.position.z), 0.01); + + EXPECT_LT(filtered_.pose.covariance[0], 0.5); + EXPECT_LT(filtered_.pose.covariance[7], 0.25); // Configuration for this variable for this sensor is false + EXPECT_LT(filtered_.pose.covariance[14], 0.6); + + resetFilter(); +} + +TEST(InterfacesTest, OdomTwistBasicIO) +{ + ros::NodeHandle nh; + ros::Publisher odomPub = nh.advertise("/odom_input2", 5); + ros::Subscriber filteredSub = nh.subscribe("/odometry/filtered", 1, &filterCallback); + + nav_msgs::Odometry odom; + odom.twist.twist.linear.x = 5.0; + odom.twist.twist.linear.y = 0.0; + odom.twist.twist.linear.z = 0.0; + odom.twist.twist.angular.x = 0.0; + odom.twist.twist.angular.y = 0.0; + odom.twist.twist.angular.z = 0.0; + + for (size_t ind = 0; ind < 36; ind+=7) + { + odom.twist.covariance[ind] = 1e-6; + } + + odom.header.frame_id = "odom"; + odom.child_frame_id = "base_link"; + + ros::Rate loopRate(20); + for (size_t i = 0; i < 400; ++i) + { + odom.header.stamp = ros::Time::now(); + odomPub.publish(odom); + ros::spinOnce(); + + loopRate.sleep(); + + odom.header.seq++; + } + ros::spinOnce(); + + EXPECT_LT(::fabs(filtered_.twist.twist.linear.x - odom.twist.twist.linear.x), 0.1); + EXPECT_LT(::fabs(filtered_.pose.pose.position.x - 100.0), 2.0); + + resetFilter(); + + odom.twist.twist.linear.x = 0.0; + odom.twist.twist.linear.y = 5.0; + + loopRate = ros::Rate(20); + for (size_t i = 0; i < 200; ++i) + { + odom.header.stamp = ros::Time::now(); + odomPub.publish(odom); + ros::spinOnce(); + + loopRate.sleep(); + + odom.header.seq++; + } + ros::spinOnce(); + + EXPECT_LT(::fabs(filtered_.twist.twist.linear.y - odom.twist.twist.linear.y), 0.1); + EXPECT_LT(::fabs(filtered_.pose.pose.position.y - 50.0), 1.0); + + resetFilter(); + + odom.twist.twist.linear.y = 0.0; + odom.twist.twist.linear.z = 5.0; + + loopRate = ros::Rate(20); + for (size_t i = 0; i < 100; ++i) + { + odom.header.stamp = ros::Time::now(); + odomPub.publish(odom); + ros::spinOnce(); + + loopRate.sleep(); + + odom.header.seq++; + } + ros::spinOnce(); + + EXPECT_LT(::fabs(filtered_.twist.twist.linear.z - odom.twist.twist.linear.z), 0.1); + EXPECT_LT(::fabs(filtered_.pose.pose.position.z - 25.0), 1.0); + + resetFilter(); + + odom.twist.twist.linear.z = 0.0; + odom.twist.twist.linear.x = 1.0; + odom.twist.twist.angular.z = (M_PI/2) / (100.0 * 0.05); + + loopRate = ros::Rate(20); + for (size_t i = 0; i < 100; ++i) + { + odom.header.stamp = ros::Time::now(); + odomPub.publish(odom); + ros::spinOnce(); + + loopRate.sleep(); + + odom.header.seq++; + } + ros::spinOnce(); + + EXPECT_LT(::fabs(filtered_.twist.twist.linear.x - odom.twist.twist.linear.x), 0.1); + EXPECT_LT(::fabs(filtered_.twist.twist.angular.z - odom.twist.twist.angular.z), 0.1); + EXPECT_LT(::fabs(filtered_.pose.pose.position.x - filtered_.pose.pose.position.y), 0.5); + + resetFilter(); + + odom.twist.twist.linear.x = 0.0; + odom.twist.twist.angular.z = 0.0; + odom.twist.twist.angular.x = -(M_PI/2) / (100.0 * 0.05); + + // First, roll the vehicle on its side + loopRate = ros::Rate(20); + for (size_t i = 0; i < 100; ++i) + { + odom.header.stamp = ros::Time::now(); + odomPub.publish(odom); + ros::spinOnce(); + + loopRate.sleep(); + + odom.header.seq++; + } + ros::spinOnce(); + + odom.twist.twist.angular.x = 0.0; + odom.twist.twist.angular.y = (M_PI/2) / (100.0 * 0.05); + + // Now, pitch it down (positive pitch velocity in vehicle frame) + loopRate = ros::Rate(20); + for (size_t i = 0; i < 100; ++i) + { + odom.header.stamp = ros::Time::now(); + odomPub.publish(odom); + ros::spinOnce(); + + loopRate.sleep(); + + odom.header.seq++; + } + ros::spinOnce(); + + odom.twist.twist.angular.y = 0.0; + odom.twist.twist.linear.x = 3.0; + + // We should now be on our side and facing -Y. Move forward in + // the vehicle frame X direction, and make sure Y decreases in + // the world frame. + loopRate = ros::Rate(20); + for (size_t i = 0; i < 100; ++i) + { + odom.header.stamp = ros::Time::now(); + odomPub.publish(odom); + ros::spinOnce(); + + loopRate.sleep(); + + odom.header.seq++; + } + ros::spinOnce(); + + EXPECT_LT(::fabs(filtered_.twist.twist.linear.x - odom.twist.twist.linear.x), 0.1); + EXPECT_LT(::fabs(filtered_.pose.pose.position.y + 15), 1.0); + + resetFilter(); +} + +TEST(InterfacesTest, PoseBasicIO) +{ + ros::NodeHandle nh; + ros::Publisher posePub = nh.advertise("/pose_input0", 5); + ros::Subscriber filteredSub = nh.subscribe("/odometry/filtered", 1, &filterCallback); + + geometry_msgs::PoseWithCovarianceStamped pose; + pose.pose.pose.position.x = 20.0; + pose.pose.pose.position.y = 10.0; + pose.pose.pose.position.z = -40.0; + pose.pose.pose.orientation.x = 0; + pose.pose.pose.orientation.y = 0; + pose.pose.pose.orientation.z = 0; + pose.pose.pose.orientation.w = 1; + + for (size_t ind = 0; ind < 36; ind+=7) + { + pose.pose.covariance[ind] = 1e-6; + } + + pose.header.frame_id = "odom"; + + ros::Rate loopRate = ros::Rate(50); + for (size_t i = 0; i < 50; ++i) + { + pose.header.stamp = ros::Time::now(); + posePub.publish(pose); + ros::spinOnce(); + + loopRate.sleep(); + + pose.header.seq++; + } + + // Now check the values from the callback + EXPECT_LT(::fabs(filtered_.pose.pose.position.x - pose.pose.pose.position.x), 0.1); + EXPECT_LT(::fabs(filtered_.pose.pose.position.y), 0.1); // Configuration for this variable for this sensor is false + EXPECT_LT(::fabs(filtered_.pose.pose.position.z - pose.pose.pose.position.z), 0.1); + + EXPECT_LT(filtered_.pose.covariance[0], 0.5); + EXPECT_LT(filtered_.pose.covariance[7], 0.25); // Configuration for this variable for this sensor is false + EXPECT_LT(filtered_.pose.covariance[14], 0.5); + + resetFilter(); +} + +TEST(InterfacesTest, TwistBasicIO) +{ + ros::NodeHandle nh; + ros::Publisher twistPub = nh.advertise("/twist_input0", 5); + ros::Subscriber filteredSub = nh.subscribe("/odometry/filtered", 1, &filterCallback); + + geometry_msgs::TwistWithCovarianceStamped twist; + twist.twist.twist.linear.x = 5.0; + twist.twist.twist.linear.y = 0; + twist.twist.twist.linear.z = 0; + twist.twist.twist.angular.x = 0; + twist.twist.twist.angular.y = 0; + twist.twist.twist.angular.z = 0; + + for (size_t ind = 0; ind < 36; ind+=7) + { + twist.twist.covariance[ind] = 1e-6; + } + + twist.header.frame_id = "base_link"; + + ros::Rate loopRate = ros::Rate(20); + for (size_t i = 0; i < 400; ++i) + { + twist.header.stamp = ros::Time::now(); + twistPub.publish(twist); + ros::spinOnce(); + + loopRate.sleep(); + + twist.header.seq++; + } + ros::spinOnce(); + + EXPECT_LT(::fabs(filtered_.twist.twist.linear.x - twist.twist.twist.linear.x), 0.1); + EXPECT_LT(::fabs(filtered_.pose.pose.position.x - 100.0), 2.0); + + resetFilter(); + + twist.twist.twist.linear.x = 0.0; + twist.twist.twist.linear.y = 5.0; + + loopRate = ros::Rate(20); + for (size_t i = 0; i < 200; ++i) + { + twist.header.stamp = ros::Time::now(); + twistPub.publish(twist); + ros::spinOnce(); + + loopRate.sleep(); + + twist.header.seq++; + } + ros::spinOnce(); + + EXPECT_LT(::fabs(filtered_.twist.twist.linear.y - twist.twist.twist.linear.y), 0.1); + EXPECT_LT(::fabs(filtered_.pose.pose.position.y - 50.0), 1.0); + + resetFilter(); + + twist.twist.twist.linear.y = 0.0; + twist.twist.twist.linear.z = 5.0; + + loopRate = ros::Rate(20); + for (size_t i = 0; i < 100; ++i) + { + twist.header.stamp = ros::Time::now(); + twistPub.publish(twist); + ros::spinOnce(); + + loopRate.sleep(); + + twist.header.seq++; + } + ros::spinOnce(); + + EXPECT_LT(::fabs(filtered_.twist.twist.linear.z - twist.twist.twist.linear.z), 0.1); + EXPECT_LT(::fabs(filtered_.pose.pose.position.z - 25.0), 1.0); + + resetFilter(); + + twist.twist.twist.linear.z = 0.0; + twist.twist.twist.linear.x = 1.0; + twist.twist.twist.angular.z = (M_PI/2) / (100.0 * 0.05); + + loopRate = ros::Rate(20); + for (size_t i = 0; i < 100; ++i) + { + twist.header.stamp = ros::Time::now(); + twistPub.publish(twist); + ros::spinOnce(); + + loopRate.sleep(); + + twist.header.seq++; + } + ros::spinOnce(); + + EXPECT_LT(::fabs(filtered_.twist.twist.linear.x - twist.twist.twist.linear.x), 0.1); + EXPECT_LT(::fabs(filtered_.twist.twist.angular.z - twist.twist.twist.angular.z), 0.1); + EXPECT_LT(::fabs(filtered_.pose.pose.position.x - filtered_.pose.pose.position.y), 0.5); + + resetFilter(); + + twist.twist.twist.linear.x = 0.0; + twist.twist.twist.angular.z = 0.0; + twist.twist.twist.angular.x = -(M_PI/2) / (100.0 * 0.05); + + // First, roll the vehicle on its side + loopRate = ros::Rate(20); + for (size_t i = 0; i < 100; ++i) + { + twist.header.stamp = ros::Time::now(); + twistPub.publish(twist); + ros::spinOnce(); + + loopRate.sleep(); + + twist.header.seq++; + } + ros::spinOnce(); + + twist.twist.twist.angular.x = 0.0; + twist.twist.twist.angular.y = (M_PI/2) / (100.0 * 0.05); + + // Now, pitch it down (positive pitch velocity in vehicle frame) + loopRate = ros::Rate(20); + for (size_t i = 0; i < 100; ++i) + { + twist.header.stamp = ros::Time::now(); + twistPub.publish(twist); + ros::spinOnce(); + + loopRate.sleep(); + + twist.header.seq++; + } + ros::spinOnce(); + + twist.twist.twist.angular.y = 0.0; + twist.twist.twist.linear.x = 3.0; + + // We should now be on our side and facing -Y. Move forward in + // the vehicle frame X direction, and make sure Y decreases in + // the world frame. + loopRate = ros::Rate(20); + for (size_t i = 0; i < 100; ++i) + { + twist.header.stamp = ros::Time::now(); + twistPub.publish(twist); + ros::spinOnce(); + + loopRate.sleep(); + + twist.header.seq++; + } + ros::spinOnce(); + + EXPECT_LT(::fabs(filtered_.twist.twist.linear.x - twist.twist.twist.linear.x), 0.1); + EXPECT_LT(::fabs(filtered_.pose.pose.position.y + 15), 1.0); + + resetFilter(); +} + +TEST(InterfacesTest, ImuPoseBasicIO) +{ + ros::NodeHandle nh; + ros::Publisher imuPub = nh.advertise("/imu_input0", 5); + ros::Subscriber filteredSub = nh.subscribe("/odometry/filtered", 1, &filterCallback); + + sensor_msgs::Imu imu; + tf2::Quaternion quat; + quat.setRPY(M_PI/4, -M_PI/4, M_PI/2); + imu.orientation = tf2::toMsg(quat); + + for (size_t ind = 0; ind < 9; ind+=4) + { + imu.orientation_covariance[ind] = 1e-6; + } + + imu.header.frame_id = "base_link"; + + // Make sure the pose reset worked. Test will timeout + // if this fails. + ros::Rate loopRate(50); + for (size_t i = 0; i < 50; ++i) + { + imu.header.stamp = ros::Time::now(); + imuPub.publish(imu); + ros::spinOnce(); + + loopRate.sleep(); + + imu.header.seq++; + } + + // Now check the values from the callback + tf2::fromMsg(filtered_.pose.pose.orientation, quat); + tf2::Matrix3x3 mat(quat); + double r, p, y; + mat.getRPY(r, p, y); + EXPECT_LT(::fabs(r - M_PI/4), 0.1); + EXPECT_LT(::fabs(p + M_PI/4), 0.1); + EXPECT_LT(::fabs(y - M_PI/2), 0.1); + + EXPECT_LT(filtered_.pose.covariance[21], 0.5); + EXPECT_LT(filtered_.pose.covariance[28], 0.25); + EXPECT_LT(filtered_.pose.covariance[35], 0.5); + + resetFilter(); + + // Test to see if the orientation data is ignored when we set the + // first covariance value to -1 + sensor_msgs::Imu imuIgnore; + imuIgnore.orientation.x = 0.1; + imuIgnore.orientation.y = 0.2; + imuIgnore.orientation.z = 0.3; + imuIgnore.orientation.w = 0.4; + imuIgnore.orientation_covariance[0] = -1; + + loopRate = ros::Rate(50); + for (size_t i = 0; i < 50; ++i) + { + imuIgnore.header.stamp = ros::Time::now(); + imuPub.publish(imuIgnore); + loopRate.sleep(); + ros::spinOnce(); + + imuIgnore.header.seq++; + } + + tf2::fromMsg(filtered_.pose.pose.orientation, quat); + mat.setRotation(quat); + mat.getRPY(r, p, y); + EXPECT_LT(::fabs(r), 1e-3); + EXPECT_LT(::fabs(p), 1e-3); + EXPECT_LT(::fabs(y), 1e-3); + + resetFilter(); +} + +TEST(InterfacesTest, ImuTwistBasicIO) +{ + ros::NodeHandle nh; + ros::Publisher imuPub = nh.advertise("/imu_input1", 5); + ros::Subscriber filteredSub = nh.subscribe("/odometry/filtered", 1, &filterCallback); + + sensor_msgs::Imu imu; + tf2::Quaternion quat; + imu.angular_velocity.x = (M_PI / 2.0); + + for (size_t ind = 0; ind < 9; ind+=4) + { + imu.angular_velocity_covariance[ind] = 1e-6; + } + + imu.header.frame_id = "base_link"; + + ros::Rate loopRate(50); + for (size_t i = 0; i < 50; ++i) + { + imu.header.stamp = ros::Time::now(); + imuPub.publish(imu); + loopRate.sleep(); + ros::spinOnce(); + + imu.header.seq++; + } + + // Now check the values from the callback + tf2::fromMsg(filtered_.pose.pose.orientation, quat); + tf2::Matrix3x3 mat(quat); + double r, p, y; + mat.getRPY(r, p, y); + + // Tolerances may seem loose, but the initial state covariances + // are tiny, so the filter is sluggish to accept velocity data + EXPECT_LT(::fabs(r - M_PI / 2.0), 0.7); + EXPECT_LT(::fabs(p), 0.1); + EXPECT_LT(::fabs(y), 0.1); + + EXPECT_LT(filtered_.twist.covariance[21], 1e-3); + EXPECT_LT(filtered_.pose.covariance[21], 0.1); + + resetFilter(); + + imu.angular_velocity.x = 0.0; + imu.angular_velocity.y = -(M_PI / 2.0); + + // Make sure the pose reset worked. Test will timeout + // if this fails. + loopRate = ros::Rate(50); + for (size_t i = 0; i < 50; ++i) + { + imu.header.stamp = ros::Time::now(); + imuPub.publish(imu); + loopRate.sleep(); + ros::spinOnce(); + + imu.header.seq++; + } + + // Now check the values from the callback + tf2::fromMsg(filtered_.pose.pose.orientation, quat); + + tf2::Quaternion expected_quat(tf2::Vector3(0., 1., 0.), -M_PI/2.0); + + EXPECT_LT(std::abs(tf2Angle(expected_quat.getAxis(), quat.getAxis())), 0.1); + EXPECT_LT(std::abs(expected_quat.getAngle() - quat.getAngle()), 0.7); + + EXPECT_LT(filtered_.twist.covariance[28], 1e-3); + EXPECT_LT(filtered_.pose.covariance[28], 0.1); + + resetFilter(); + + imu.angular_velocity.y = 0; + imu.angular_velocity.z = M_PI / 4.0; + + // Make sure the pose reset worked. Test will timeout + // if this fails. + loopRate = ros::Rate(50); + for (size_t i = 0; i < 50; ++i) + { + imu.header.stamp = ros::Time::now(); + imuPub.publish(imu); + loopRate.sleep(); + ros::spinOnce(); + + imu.header.seq++; + } + + // Now check the values from the callback + tf2::fromMsg(filtered_.pose.pose.orientation, quat); + mat.setRotation(quat); + mat.getRPY(r, p, y); + EXPECT_LT(::fabs(r), 0.1); + EXPECT_LT(::fabs(p), 0.1); + EXPECT_LT(::fabs(y - M_PI / 4.0), 0.7); + + EXPECT_LT(filtered_.twist.covariance[35], 1e-3); + EXPECT_LT(filtered_.pose.covariance[35], 0.1); + + resetFilter(); + + // Test to see if the angular velocity data is ignored when we set the + // first covariance value to -1 + sensor_msgs::Imu imuIgnore; + imuIgnore.angular_velocity.x = 100; + imuIgnore.angular_velocity.y = 100; + imuIgnore.angular_velocity.z = 100; + imuIgnore.angular_velocity_covariance[0] = -1; + + loopRate = ros::Rate(50); + for (size_t i = 0; i < 50; ++i) + { + imuIgnore.header.stamp = ros::Time::now(); + imuPub.publish(imuIgnore); + loopRate.sleep(); + ros::spinOnce(); + + imuIgnore.header.seq++; + } + + tf2::fromMsg(filtered_.pose.pose.orientation, quat); + mat.setRotation(quat); + mat.getRPY(r, p, y); + EXPECT_LT(::fabs(r), 1e-3); + EXPECT_LT(::fabs(p), 1e-3); + EXPECT_LT(::fabs(y), 1e-3); + + resetFilter(); +} + +TEST(InterfacesTest, ImuAccBasicIO) +{ + ros::NodeHandle nh; + ros::Publisher imuPub = nh.advertise("/imu_input2", 5); + ros::Subscriber filteredSub = nh.subscribe("/odometry/filtered", 1, &filterCallback); + + sensor_msgs::Imu imu; + imu.header.frame_id = "base_link"; + imu.linear_acceleration_covariance[0] = 1e-6; + imu.linear_acceleration_covariance[4] = 1e-6; + imu.linear_acceleration_covariance[8] = 1e-6; + + imu.linear_acceleration.x = 1; + imu.linear_acceleration.y = -1; + imu.linear_acceleration.z = 1; + + // Move with constant acceleration for 1 second, + // then check our velocity. + ros::Rate loopRate(50); + for (size_t i = 0; i < 50; ++i) + { + imu.header.stamp = ros::Time::now(); + imuPub.publish(imu); + loopRate.sleep(); + ros::spinOnce(); + + imu.header.seq++; + } + + EXPECT_LT(::fabs(filtered_.twist.twist.linear.x - 1.0), 0.4); + EXPECT_LT(::fabs(filtered_.twist.twist.linear.y + 1.0), 0.4); + EXPECT_LT(::fabs(filtered_.twist.twist.linear.z - 1.0), 0.4); + + imu.linear_acceleration.x = 0.0; + imu.linear_acceleration.y = 0.0; + imu.linear_acceleration.z = 0.0; + + // Now move for another second, and see where we + // end up + loopRate = ros::Rate(50); + for (size_t i = 0; i < 50; ++i) + { + imu.header.stamp = ros::Time::now(); + imuPub.publish(imu); + loopRate.sleep(); + ros::spinOnce(); + + imu.header.seq++; + } + + EXPECT_LT(::fabs(filtered_.pose.pose.position.x - 1.2), 0.4); + EXPECT_LT(::fabs(filtered_.pose.pose.position.y + 1.2), 0.4); + EXPECT_LT(::fabs(filtered_.pose.pose.position.z - 1.2), 0.4); + + resetFilter(); + + // Test to see if the linear acceleration data is ignored when we set the + // first covariance value to -1 + sensor_msgs::Imu imuIgnore; + imuIgnore.linear_acceleration.x = 1000; + imuIgnore.linear_acceleration.y = 1000; + imuIgnore.linear_acceleration.z = 1000; + imuIgnore.linear_acceleration_covariance[0] = -1; + + loopRate = ros::Rate(50); + for (size_t i = 0; i < 50; ++i) + { + imuIgnore.header.stamp = ros::Time::now(); + imuPub.publish(imuIgnore); + loopRate.sleep(); + ros::spinOnce(); + + imuIgnore.header.seq++; + } + + EXPECT_LT(::fabs(filtered_.pose.pose.position.x), 1e-3); + EXPECT_LT(::fabs(filtered_.pose.pose.position.y), 1e-3); + EXPECT_LT(::fabs(filtered_.pose.pose.position.z), 1e-3); + + resetFilter(); +} + +TEST(InterfacesTest, OdomDifferentialIO) +{ + ros::NodeHandle nh; + ros::Publisher odomPub = nh.advertise("/odom_input1", 5); + ros::Subscriber filteredSub = nh.subscribe("/odometry/filtered", 1, &filterCallback); + + nav_msgs::Odometry odom; + odom.pose.pose.position.x = 20.0; + odom.pose.pose.position.y = 10.0; + odom.pose.pose.position.z = -40.0; + + odom.pose.pose.orientation.w = 1; + + odom.pose.covariance[0] = 2.0; + odom.pose.covariance[7] = 2.0; + odom.pose.covariance[14] = 2.0; + odom.pose.covariance[21] = 0.2; + odom.pose.covariance[28] = 0.2; + odom.pose.covariance[35] = 0.2; + + odom.header.frame_id = "odom"; + odom.child_frame_id = "base_link"; + + // No guaranteeing that the zero state + // we're expecting to see here isn't just + // a result of zeroing it out previously, + // so check 10 times in succession. + size_t zeroCount = 0; + while (zeroCount++ < 10) + { + odom.header.stamp = ros::Time::now(); + odomPub.publish(odom); + ros::spinOnce(); + + EXPECT_LT(::fabs(filtered_.pose.pose.position.x), 0.01); + EXPECT_LT(::fabs(filtered_.pose.pose.position.y), 0.01); + EXPECT_LT(::fabs(filtered_.pose.pose.position.z), 0.01); + EXPECT_LT(::fabs(filtered_.pose.pose.orientation.x), 0.01); + EXPECT_LT(::fabs(filtered_.pose.pose.orientation.y), 0.01); + EXPECT_LT(::fabs(filtered_.pose.pose.orientation.z), 0.01); + EXPECT_LT(::fabs(filtered_.pose.pose.orientation.w - 1), 0.01); + + ros::Duration(0.1).sleep(); + + odom.header.seq++; + } + + for (size_t ind = 0; ind < 36; ind+=7) + { + odom.pose.covariance[ind] = 1e-6; + } + + // Slowly move the position, and hope that the + // differential position keeps up + ros::Rate loopRate(20); + for (size_t i = 0; i < 100; ++i) + { + odom.pose.pose.position.x += 0.01; + odom.pose.pose.position.y += 0.02; + odom.pose.pose.position.z -= 0.03; + + odom.header.stamp = ros::Time::now(); + odomPub.publish(odom); + ros::spinOnce(); + + loopRate.sleep(); + + odom.header.seq++; + } + ros::spinOnce(); + + EXPECT_LT(::fabs(filtered_.pose.pose.position.x - 1), 0.2); + EXPECT_LT(::fabs(filtered_.pose.pose.position.y - 2), 0.4); + EXPECT_LT(::fabs(filtered_.pose.pose.position.z + 3), 0.6); + + resetFilter(); +} + +TEST(InterfacesTest, PoseDifferentialIO) +{ + ros::NodeHandle nh; + ros::Publisher posePub = nh.advertise("/pose_input1", 5); + ros::Subscriber filteredSub = nh.subscribe("/odometry/filtered", 1, &filterCallback); + + geometry_msgs::PoseWithCovarianceStamped pose; + pose.pose.pose.position.x = 20.0; + pose.pose.pose.position.y = 10.0; + pose.pose.pose.position.z = -40.0; + + pose.pose.pose.orientation.w = 1; + + pose.pose.covariance[0] = 2.0; + pose.pose.covariance[7] = 2.0; + pose.pose.covariance[14] = 2.0; + pose.pose.covariance[21] = 0.2; + pose.pose.covariance[28] = 0.2; + pose.pose.covariance[35] = 0.2; + + pose.header.frame_id = "odom"; + + // No guaranteeing that the zero state + // we're expecting to see here isn't just + // a result of zeroing it out previously, + // so check 10 times in succession. + size_t zeroCount = 0; + while (zeroCount++ < 10) + { + pose.header.stamp = ros::Time::now(); + posePub.publish(pose); + ros::spinOnce(); + + EXPECT_LT(::fabs(filtered_.pose.pose.position.x), 0.01); + EXPECT_LT(::fabs(filtered_.pose.pose.position.y), 0.01); + EXPECT_LT(::fabs(filtered_.pose.pose.position.z), 0.01); + EXPECT_LT(::fabs(filtered_.pose.pose.orientation.x), 0.01); + EXPECT_LT(::fabs(filtered_.pose.pose.orientation.y), 0.01); + EXPECT_LT(::fabs(filtered_.pose.pose.orientation.z), 0.01); + EXPECT_LT(::fabs(filtered_.pose.pose.orientation.w - 1), 0.01); + + ros::Duration(0.1).sleep(); + + pose.header.seq++; + } + + // ...but only if we give the measurement a tiny covariance + for (size_t ind = 0; ind < 36; ind+=7) + { + pose.pose.covariance[ind] = 1e-6; + } + + // Issue this location repeatedly, and see if we get + // a final reported position of (1, 2, -3) + ros::Rate loopRate(20); + for (size_t i = 0; i < 100; ++i) + { + pose.pose.pose.position.x += 0.01; + pose.pose.pose.position.y += 0.02; + pose.pose.pose.position.z -= 0.03; + + pose.header.stamp = ros::Time::now(); + posePub.publish(pose); + ros::spinOnce(); + + loopRate.sleep(); + + pose.header.seq++; + } + ros::spinOnce(); + + EXPECT_LT(::fabs(filtered_.pose.pose.position.x - 1), 0.2); + EXPECT_LT(::fabs(filtered_.pose.pose.position.y - 2), 0.4); + EXPECT_LT(::fabs(filtered_.pose.pose.position.z + 3), 0.6); + + resetFilter(); +} + +TEST(InterfacesTest, ImuDifferentialIO) +{ + ros::NodeHandle nh; + ros::Publisher imu0Pub = nh.advertise("/imu_input0", 5); + ros::Publisher imu1Pub = nh.advertise("/imu_input1", 5); + ros::Publisher imuPub = nh.advertise("/imu_input3", 5); + ros::Subscriber filteredSub = nh.subscribe("/odometry/filtered", 1, &filterCallback); + + sensor_msgs::Imu imu; + imu.header.frame_id = "base_link"; + tf2::Quaternion quat; + const double roll = M_PI/2.0; + const double pitch = -M_PI; + const double yaw = -M_PI/4.0; + quat.setRPY(roll, pitch, yaw); + imu.orientation = tf2::toMsg(quat); + + imu.orientation_covariance[0] = 0.02; + imu.orientation_covariance[4] = 0.02; + imu.orientation_covariance[8] = 0.02; + + imu.angular_velocity_covariance[0] = 0.02; + imu.angular_velocity_covariance[4] = 0.02; + imu.angular_velocity_covariance[8] = 0.02; + + ros::Duration(0.1).sleep(); + ros::spinOnce(); + + size_t setCount = 0; + while (setCount++ < 1000) + { + imu.header.stamp = ros::Time::now(); + imu0Pub.publish(imu); // Use this to move the absolute orientation + imu1Pub.publish(imu); // Use this to keep velocities at 0 + ros::spinOnce(); + + ros::Duration(0.001).sleep(); + + imu.header.seq++; + } + + size_t zeroCount = 0; + while (zeroCount++ < 10) + { + imu.header.stamp = ros::Time::now(); + imuPub.publish(imu); + ros::spinOnce(); + + ros::Duration(0.1).sleep(); + + imu.header.seq++; + } + + double rollFinal = roll; + double pitchFinal = pitch; + double yawFinal = yaw; + + // Move the orientation slowly, and see if we + // can push it to 0 + ros::Rate loopRate(20); + for (size_t i = 0; i < 100; ++i) + { + yawFinal -= 0.01 * (3.0 * M_PI/4.0); + + quat.setRPY(rollFinal, pitchFinal, yawFinal); + + imu.orientation = tf2::toMsg(quat); + imu.header.stamp = ros::Time::now(); + imuPub.publish(imu); + ros::spinOnce(); + + loopRate.sleep(); + + imu.header.seq++; + } + ros::spinOnce(); + + // Move the orientation slowly, and see if we + // can push it to 0 + loopRate = ros::Rate(20); + for (size_t i = 0; i < 100; ++i) + { + rollFinal += 0.01 * (M_PI/2.0); + + quat.setRPY(rollFinal, pitchFinal, yawFinal); + + imu.orientation = tf2::toMsg(quat); + imu.header.stamp = ros::Time::now(); + imuPub.publish(imu); + ros::spinOnce(); + + loopRate.sleep(); + + imu.header.seq++; + } + ros::spinOnce(); + + tf2::fromMsg(filtered_.pose.pose.orientation, quat); + tf2::Matrix3x3 mat(quat); + mat.getRPY(rollFinal, pitchFinal, yawFinal); + EXPECT_LT(::fabs(rollFinal), 0.2); + EXPECT_LT(::fabs(pitchFinal), 0.2); + EXPECT_LT(::fabs(yawFinal), 0.2); + + resetFilter(); +} + +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + + ros::init(argc, argv, "ukf_navigation_node-test-interfaces"); + ros::Time::init(); + + // Give ukf_localization_node time to initialize + ros::Duration(2.0).sleep(); + + return RUN_ALL_TESTS(); +} diff --git a/Localizations/Packages/robot_localization/test/test_ukf_localization_node_interfaces.test b/Localizations/Packages/robot_localization/test/test_ukf_localization_node_interfaces.test new file mode 100644 index 0000000..8959361 --- /dev/null +++ b/Localizations/Packages/robot_localization/test/test_ukf_localization_node_interfaces.test @@ -0,0 +1,73 @@ + + + + + + + + + + + + + + + + + + + + + + + + + [true, false, true, false, false, false, false, false, false, false, false, false, false, false, false] + [true, true, true, true, true, true, false, false, false, false, false, false, false, false, false] + [false, false, false, false, false, false, true, true, true, true, true, true, false, false, false] + + [true, false, true, false, false, false, false, false, false, false, false, false, false, false, false] + [true, true, true, true, true, true, false, false, false, false, false, false, false, false, false] + + [false, false, false, false, false, false, true, true, true, true, true, true, false, false, false] + + [false, false, false, true, true, true, false, false, false, false, false, false, false, false, false] + [false, false, false, false, false, false, false, false, false, true, true, true, false, false, false] + [false, false, false, false, false, false, false, false, false, false, false, false, true, true, true] + [false, false, false, true, true, true, false, false, false, false, false, false, false, false, false] + + + + + + + + + + + + [1e-6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 1e-6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 1e-6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 1e-6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 1e-6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 1e-6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.002, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.002, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.004, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01] + + + + + + + + + + diff --git a/Localizations/Packages/robot_localization/test/test_ukf_localization_nodelet_bag1.test b/Localizations/Packages/robot_localization/test/test_ukf_localization_nodelet_bag1.test new file mode 100644 index 0000000..546ad80 --- /dev/null +++ b/Localizations/Packages/robot_localization/test/test_ukf_localization_nodelet_bag1.test @@ -0,0 +1,111 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + [false, false, false, + false, false, false, + true, true, true, + false, false, false, + false, false, false] + + [false, false, false, + true, true, true, + false, false, false, + true, true, false, + true, true, true] + + + + + + + + + + + + + + + + + + [0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0.06, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0.06, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0.04, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.02, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.015] + + [1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9] + + + + + + + + + + + + + + + + + diff --git a/Localizations/Packages/slam_toolbox/Dockerfile b/Localizations/Packages/slam_toolbox/Dockerfile new file mode 100644 index 0000000..92658d0 --- /dev/null +++ b/Localizations/Packages/slam_toolbox/Dockerfile @@ -0,0 +1,26 @@ +FROM ros:melodic-ros-base-bionic + +# USE BASH +SHELL ["/bin/bash", "-c"] + +# RUN LINE BELOW TO REMOVE debconf ERRORS (MUST RUN BEFORE ANY apt-get CALLS) +RUN echo 'debconf debconf/frontend select Noninteractive' | debconf-set-selections + +RUN apt-get update && apt-get upgrade -y && apt-get install -y --no-install-recommends apt-utils + +# slam_toolbox +RUN mkdir -p catkin_ws/src +RUN cd catkin_ws/src && git clone -b noetic-devel https://github.com/SteveMacenski/slam_toolbox.git +RUN source /opt/ros/melodic/setup.bash \ + && cd catkin_ws \ + && rosdep update \ + && rosdep install -y -r --from-paths src --ignore-src --rosdistro=melodic -y + +RUN apt install python-catkin-tools -y +RUN source /opt/ros/melodic/setup.bash \ + && cd catkin_ws/src \ + && catkin_init_workspace \ + && cd .. \ + && catkin config --install \ + && catkin build -DCMAKE_BUILD_TYPE=Release + diff --git a/Localizations/Packages/slam_toolbox/LICENSE b/Localizations/Packages/slam_toolbox/LICENSE new file mode 100644 index 0000000..8000a6f --- /dev/null +++ b/Localizations/Packages/slam_toolbox/LICENSE @@ -0,0 +1,504 @@ + GNU LESSER GENERAL PUBLIC LICENSE + Version 2.1, February 1999 + + Copyright (C) 1991, 1999 Free Software Foundation, Inc. + 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + Everyone is permitted to copy and distribute verbatim copies + of this license document, but changing it is not allowed. + +[This is the first released version of the Lesser GPL. It also counts + as the successor of the GNU Library Public License, version 2, hence + the version number 2.1.] + + Preamble + + The licenses for most software are designed to take away your +freedom to share and change it. By contrast, the GNU General Public +Licenses are intended to guarantee your freedom to share and change +free software--to make sure the software is free for all its users. + + This license, the Lesser General Public License, applies to some +specially designated software packages--typically libraries--of the +Free Software Foundation and other authors who decide to use it. You +can use it too, but we suggest you first think carefully about whether +this license or the ordinary General Public License is the better +strategy to use in any particular case, based on the explanations below. + + When we speak of free software, we are referring to freedom of use, +not price. Our General Public Licenses are designed to make sure that +you have the freedom to distribute copies of free software (and charge +for this service if you wish); that you receive source code or can get +it if you want it; that you can change the software and use pieces of +it in new free programs; and that you are informed that you can do +these things. + + To protect your rights, we need to make restrictions that forbid +distributors to deny you these rights or to ask you to surrender these +rights. These restrictions translate to certain responsibilities for +you if you distribute copies of the library or if you modify it. + + For example, if you distribute copies of the library, whether gratis +or for a fee, you must give the recipients all the rights that we gave +you. You must make sure that they, too, receive or can get the source +code. If you link other code with the library, you must provide +complete object files to the recipients, so that they can relink them +with the library after making changes to the library and recompiling +it. And you must show them these terms so they know their rights. + + We protect your rights with a two-step method: (1) we copyright the +library, and (2) we offer you this license, which gives you legal +permission to copy, distribute and/or modify the library. + + To protect each distributor, we want to make it very clear that +there is no warranty for the free library. Also, if the library is +modified by someone else and passed on, the recipients should know +that what they have is not the original version, so that the original +author's reputation will not be affected by problems that might be +introduced by others. + + Finally, software patents pose a constant threat to the existence of +any free program. We wish to make sure that a company cannot +effectively restrict the users of a free program by obtaining a +restrictive license from a patent holder. Therefore, we insist that +any patent license obtained for a version of the library must be +consistent with the full freedom of use specified in this license. + + Most GNU software, including some libraries, is covered by the +ordinary GNU General Public License. This license, the GNU Lesser +General Public License, applies to certain designated libraries, and +is quite different from the ordinary General Public License. We use +this license for certain libraries in order to permit linking those +libraries into non-free programs. + + When a program is linked with a library, whether statically or using +a shared library, the combination of the two is legally speaking a +combined work, a derivative of the original library. The ordinary +General Public License therefore permits such linking only if the +entire combination fits its criteria of freedom. The Lesser General +Public License permits more lax criteria for linking other code with +the library. + + We call this license the "Lesser" General Public License because it +does Less to protect the user's freedom than the ordinary General +Public License. It also provides other free software developers Less +of an advantage over competing non-free programs. These disadvantages +are the reason we use the ordinary General Public License for many +libraries. However, the Lesser license provides advantages in certain +special circumstances. + + For example, on rare occasions, there may be a special need to +encourage the widest possible use of a certain library, so that it becomes +a de-facto standard. To achieve this, non-free programs must be +allowed to use the library. A more frequent case is that a free +library does the same job as widely used non-free libraries. In this +case, there is little to gain by limiting the free library to free +software only, so we use the Lesser General Public License. + + In other cases, permission to use a particular library in non-free +programs enables a greater number of people to use a large body of +free software. For example, permission to use the GNU C Library in +non-free programs enables many more people to use the whole GNU +operating system, as well as its variant, the GNU/Linux operating +system. + + Although the Lesser General Public License is Less protective of the +users' freedom, it does ensure that the user of a program that is +linked with the Library has the freedom and the wherewithal to run +that program using a modified version of the Library. + + The precise terms and conditions for copying, distribution and +modification follow. Pay close attention to the difference between a +"work based on the library" and a "work that uses the library". The +former contains code derived from the library, whereas the latter must +be combined with the library in order to run. + + GNU LESSER GENERAL PUBLIC LICENSE + TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION + + 0. This License Agreement applies to any software library or other +program which contains a notice placed by the copyright holder or +other authorized party saying it may be distributed under the terms of +this Lesser General Public License (also called "this License"). +Each licensee is addressed as "you". + + A "library" means a collection of software functions and/or data +prepared so as to be conveniently linked with application programs +(which use some of those functions and data) to form executables. + + The "Library", below, refers to any such software library or work +which has been distributed under these terms. A "work based on the +Library" means either the Library or any derivative work under +copyright law: that is to say, a work containing the Library or a +portion of it, either verbatim or with modifications and/or translated +straightforwardly into another language. (Hereinafter, translation is +included without limitation in the term "modification".) + + "Source code" for a work means the preferred form of the work for +making modifications to it. For a library, complete source code means +all the source code for all modules it contains, plus any associated +interface definition files, plus the scripts used to control compilation +and installation of the library. + + Activities other than copying, distribution and modification are not +covered by this License; they are outside its scope. The act of +running a program using the Library is not restricted, and output from +such a program is covered only if its contents constitute a work based +on the Library (independent of the use of the Library in a tool for +writing it). Whether that is true depends on what the Library does +and what the program that uses the Library does. + + 1. You may copy and distribute verbatim copies of the Library's +complete source code as you receive it, in any medium, provided that +you conspicuously and appropriately publish on each copy an +appropriate copyright notice and disclaimer of warranty; keep intact +all the notices that refer to this License and to the absence of any +warranty; and distribute a copy of this License along with the +Library. + + You may charge a fee for the physical act of transferring a copy, +and you may at your option offer warranty protection in exchange for a +fee. + + 2. You may modify your copy or copies of the Library or any portion +of it, thus forming a work based on the Library, and copy and +distribute such modifications or work under the terms of Section 1 +above, provided that you also meet all of these conditions: + + a) The modified work must itself be a software library. + + b) You must cause the files modified to carry prominent notices + stating that you changed the files and the date of any change. + + c) You must cause the whole of the work to be licensed at no + charge to all third parties under the terms of this License. + + d) If a facility in the modified Library refers to a function or a + table of data to be supplied by an application program that uses + the facility, other than as an argument passed when the facility + is invoked, then you must make a good faith effort to ensure that, + in the event an application does not supply such function or + table, the facility still operates, and performs whatever part of + its purpose remains meaningful. + + (For example, a function in a library to compute square roots has + a purpose that is entirely well-defined independent of the + application. Therefore, Subsection 2d requires that any + application-supplied function or table used by this function must + be optional: if the application does not supply it, the square + root function must still compute square roots.) + +These requirements apply to the modified work as a whole. If +identifiable sections of that work are not derived from the Library, +and can be reasonably considered independent and separate works in +themselves, then this License, and its terms, do not apply to those +sections when you distribute them as separate works. But when you +distribute the same sections as part of a whole which is a work based +on the Library, the distribution of the whole must be on the terms of +this License, whose permissions for other licensees extend to the +entire whole, and thus to each and every part regardless of who wrote +it. + +Thus, it is not the intent of this section to claim rights or contest +your rights to work written entirely by you; rather, the intent is to +exercise the right to control the distribution of derivative or +collective works based on the Library. + +In addition, mere aggregation of another work not based on the Library +with the Library (or with a work based on the Library) on a volume of +a storage or distribution medium does not bring the other work under +the scope of this License. + + 3. You may opt to apply the terms of the ordinary GNU General Public +License instead of this License to a given copy of the Library. To do +this, you must alter all the notices that refer to this License, so +that they refer to the ordinary GNU General Public License, version 2, +instead of to this License. (If a newer version than version 2 of the +ordinary GNU General Public License has appeared, then you can specify +that version instead if you wish.) Do not make any other change in +these notices. + + Once this change is made in a given copy, it is irreversible for +that copy, so the ordinary GNU General Public License applies to all +subsequent copies and derivative works made from that copy. + + This option is useful when you wish to copy part of the code of +the Library into a program that is not a library. + + 4. You may copy and distribute the Library (or a portion or +derivative of it, under Section 2) in object code or executable form +under the terms of Sections 1 and 2 above provided that you accompany +it with the complete corresponding machine-readable source code, which +must be distributed under the terms of Sections 1 and 2 above on a +medium customarily used for software interchange. + + If distribution of object code is made by offering access to copy +from a designated place, then offering equivalent access to copy the +source code from the same place satisfies the requirement to +distribute the source code, even though third parties are not +compelled to copy the source along with the object code. + + 5. A program that contains no derivative of any portion of the +Library, but is designed to work with the Library by being compiled or +linked with it, is called a "work that uses the Library". Such a +work, in isolation, is not a derivative work of the Library, and +therefore falls outside the scope of this License. + + However, linking a "work that uses the Library" with the Library +creates an executable that is a derivative of the Library (because it +contains portions of the Library), rather than a "work that uses the +library". The executable is therefore covered by this License. +Section 6 states terms for distribution of such executables. + + When a "work that uses the Library" uses material from a header file +that is part of the Library, the object code for the work may be a +derivative work of the Library even though the source code is not. +Whether this is true is especially significant if the work can be +linked without the Library, or if the work is itself a library. The +threshold for this to be true is not precisely defined by law. + + If such an object file uses only numerical parameters, data +structure layouts and accessors, and small macros and small inline +functions (ten lines or less in length), then the use of the object +file is unrestricted, regardless of whether it is legally a derivative +work. (Executables containing this object code plus portions of the +Library will still fall under Section 6.) + + Otherwise, if the work is a derivative of the Library, you may +distribute the object code for the work under the terms of Section 6. +Any executables containing that work also fall under Section 6, +whether or not they are linked directly with the Library itself. + + 6. As an exception to the Sections above, you may also combine or +link a "work that uses the Library" with the Library to produce a +work containing portions of the Library, and distribute that work +under terms of your choice, provided that the terms permit +modification of the work for the customer's own use and reverse +engineering for debugging such modifications. + + You must give prominent notice with each copy of the work that the +Library is used in it and that the Library and its use are covered by +this License. You must supply a copy of this License. If the work +during execution displays copyright notices, you must include the +copyright notice for the Library among them, as well as a reference +directing the user to the copy of this License. Also, you must do one +of these things: + + a) Accompany the work with the complete corresponding + machine-readable source code for the Library including whatever + changes were used in the work (which must be distributed under + Sections 1 and 2 above); and, if the work is an executable linked + with the Library, with the complete machine-readable "work that + uses the Library", as object code and/or source code, so that the + user can modify the Library and then relink to produce a modified + executable containing the modified Library. (It is understood + that the user who changes the contents of definitions files in the + Library will not necessarily be able to recompile the application + to use the modified definitions.) + + b) Use a suitable shared library mechanism for linking with the + Library. A suitable mechanism is one that (1) uses at run time a + copy of the library already present on the user's computer system, + rather than copying library functions into the executable, and (2) + will operate properly with a modified version of the library, if + the user installs one, as long as the modified version is + interface-compatible with the version that the work was made with. + + c) Accompany the work with a written offer, valid for at + least three years, to give the same user the materials + specified in Subsection 6a, above, for a charge no more + than the cost of performing this distribution. + + d) If distribution of the work is made by offering access to copy + from a designated place, offer equivalent access to copy the above + specified materials from the same place. + + e) Verify that the user has already received a copy of these + materials or that you have already sent this user a copy. + + For an executable, the required form of the "work that uses the +Library" must include any data and utility programs needed for +reproducing the executable from it. However, as a special exception, +the materials to be distributed need not include anything that is +normally distributed (in either source or binary form) with the major +components (compiler, kernel, and so on) of the operating system on +which the executable runs, unless that component itself accompanies +the executable. + + It may happen that this requirement contradicts the license +restrictions of other proprietary libraries that do not normally +accompany the operating system. Such a contradiction means you cannot +use both them and the Library together in an executable that you +distribute. + + 7. You may place library facilities that are a work based on the +Library side-by-side in a single library together with other library +facilities not covered by this License, and distribute such a combined +library, provided that the separate distribution of the work based on +the Library and of the other library facilities is otherwise +permitted, and provided that you do these two things: + + a) Accompany the combined library with a copy of the same work + based on the Library, uncombined with any other library + facilities. This must be distributed under the terms of the + Sections above. + + b) Give prominent notice with the combined library of the fact + that part of it is a work based on the Library, and explaining + where to find the accompanying uncombined form of the same work. + + 8. You may not copy, modify, sublicense, link with, or distribute +the Library except as expressly provided under this License. Any +attempt otherwise to copy, modify, sublicense, link with, or +distribute the Library is void, and will automatically terminate your +rights under this License. However, parties who have received copies, +or rights, from you under this License will not have their licenses +terminated so long as such parties remain in full compliance. + + 9. You are not required to accept this License, since you have not +signed it. However, nothing else grants you permission to modify or +distribute the Library or its derivative works. These actions are +prohibited by law if you do not accept this License. Therefore, by +modifying or distributing the Library (or any work based on the +Library), you indicate your acceptance of this License to do so, and +all its terms and conditions for copying, distributing or modifying +the Library or works based on it. + + 10. Each time you redistribute the Library (or any work based on the +Library), the recipient automatically receives a license from the +original licensor to copy, distribute, link with or modify the Library +subject to these terms and conditions. You may not impose any further +restrictions on the recipients' exercise of the rights granted herein. +You are not responsible for enforcing compliance by third parties with +this License. + + 11. If, as a consequence of a court judgment or allegation of patent +infringement or for any other reason (not limited to patent issues), +conditions are imposed on you (whether by court order, agreement or +otherwise) that contradict the conditions of this License, they do not +excuse you from the conditions of this License. If you cannot +distribute so as to satisfy simultaneously your obligations under this +License and any other pertinent obligations, then as a consequence you +may not distribute the Library at all. For example, if a patent +license would not permit royalty-free redistribution of the Library by +all those who receive copies directly or indirectly through you, then +the only way you could satisfy both it and this License would be to +refrain entirely from distribution of the Library. + +If any portion of this section is held invalid or unenforceable under any +particular circumstance, the balance of the section is intended to apply, +and the section as a whole is intended to apply in other circumstances. + +It is not the purpose of this section to induce you to infringe any +patents or other property right claims or to contest validity of any +such claims; this section has the sole purpose of protecting the +integrity of the free software distribution system which is +implemented by public license practices. Many people have made +generous contributions to the wide range of software distributed +through that system in reliance on consistent application of that +system; it is up to the author/donor to decide if he or she is willing +to distribute software through any other system and a licensee cannot +impose that choice. + +This section is intended to make thoroughly clear what is believed to +be a consequence of the rest of this License. + + 12. If the distribution and/or use of the Library is restricted in +certain countries either by patents or by copyrighted interfaces, the +original copyright holder who places the Library under this License may add +an explicit geographical distribution limitation excluding those countries, +so that distribution is permitted only in or among countries not thus +excluded. In such case, this License incorporates the limitation as if +written in the body of this License. + + 13. The Free Software Foundation may publish revised and/or new +versions of the Lesser General Public License from time to time. +Such new versions will be similar in spirit to the present version, +but may differ in detail to address new problems or concerns. + +Each version is given a distinguishing version number. If the Library +specifies a version number of this License which applies to it and +"any later version", you have the option of following the terms and +conditions either of that version or of any later version published by +the Free Software Foundation. If the Library does not specify a +license version number, you may choose any version ever published by +the Free Software Foundation. + + 14. If you wish to incorporate parts of the Library into other free +programs whose distribution conditions are incompatible with these, +write to the author to ask for permission. For software which is +copyrighted by the Free Software Foundation, write to the Free +Software Foundation; we sometimes make exceptions for this. Our +decision will be guided by the two goals of preserving the free status +of all derivatives of our free software and of promoting the sharing +and reuse of software generally. + + NO WARRANTY + + 15. BECAUSE THE LIBRARY IS LICENSED FREE OF CHARGE, THERE IS NO +WARRANTY FOR THE LIBRARY, TO THE EXTENT PERMITTED BY APPLICABLE LAW. +EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR +OTHER PARTIES PROVIDE THE LIBRARY "AS IS" WITHOUT WARRANTY OF ANY +KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE +LIBRARY IS WITH YOU. SHOULD THE LIBRARY PROVE DEFECTIVE, YOU ASSUME +THE COST OF ALL NECESSARY SERVICING, REPAIR OR CORRECTION. + + 16. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN +WRITING WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY +AND/OR REDISTRIBUTE THE LIBRARY AS PERMITTED ABOVE, BE LIABLE TO YOU +FOR DAMAGES, INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR +CONSEQUENTIAL DAMAGES ARISING OUT OF THE USE OR INABILITY TO USE THE +LIBRARY (INCLUDING BUT NOT LIMITED TO LOSS OF DATA OR DATA BEING +RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD PARTIES OR A +FAILURE OF THE LIBRARY TO OPERATE WITH ANY OTHER SOFTWARE), EVEN IF +SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH +DAMAGES. + + END OF TERMS AND CONDITIONS + + How to Apply These Terms to Your New Libraries + + If you develop a new library, and you want it to be of the greatest +possible use to the public, we recommend making it free software that +everyone can redistribute and change. You can do so by permitting +redistribution under these terms (or, alternatively, under the terms of the +ordinary General Public License). + + To apply these terms, attach the following notices to the library. It is +safest to attach them to the start of each source file to most effectively +convey the exclusion of warranty; and each file should have at least the +"copyright" line and a pointer to where the full notice is found. + + + Copyright (C) + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with this library; if not, write to the Free Software + Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 + USA + +Also add information on how to contact you by electronic and paper mail. + +You should also get your employer (if you work as a programmer) or your +school, if any, to sign a "copyright disclaimer" for the library, if +necessary. Here is a sample; alter the names: + + Yoyodyne, Inc., hereby disclaims all copyright interest in the + library `Frob' (a library for tweaking knobs) written by James Random + Hacker. + + , 1 April 1990 + Ty Coon, President of Vice + +That's all there is to it! diff --git a/Localizations/Packages/slam_toolbox/README.md b/Localizations/Packages/slam_toolbox/README.md new file mode 100644 index 0000000..c274e4b --- /dev/null +++ b/Localizations/Packages/slam_toolbox/README.md @@ -0,0 +1,364 @@ +## Slam Toolbox + +| DockerHub | [![Build Status](https://img.shields.io/docker/cloud/build/stevemacenski/slam-toolbox.svg?label=build)](https://hub.docker.com/r/stevemacenski/slam-toolbox) | [![Build Status](https://img.shields.io/docker/pulls/stevemacenski/slam-toolbox.svg?maxAge=2592000)](https://hub.docker.com/r/stevemacenski/slam-toolbox) | +|-----|----|----| +| **Build Farm** | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mdev__slam_toolbox__ubuntu_bionic_amd64)](http://build.ros.org/view/Kbin_uX64/job/Mdev__slam_toolbox__ubuntu_bionic_amd64/) | N/A | + +We've received feedback from users and have robots operating in the following environments with SLAM Toolbox: +- Retail +- Warehouses +- Libraries +- Research + +### Cite This Work + +You can find this work [here](https://joss.theoj.org/papers/10.21105/joss.02783) and clicking on the image below. + +> Macenski, S., Jambrecic I., "SLAM Toolbox: SLAM for the dynamic world", Journal of Open Source Software, 6(61), 2783, 2021. + +> Macenski, S., "On Use of SLAM Toolbox, A fresh(er) look at mapping and localization for the dynamic world", ROSCon 2019. + +[![IMAGE ALT TEXT](https://user-images.githubusercontent.com/14944147/74176653-f69beb80-4bec-11ea-906a-a233541a6064.png)](https://vimeo.com/378682207) + +# Introduction + + +Slam Toolbox is a set of tools and capabilities for 2D SLAM built by [Steve Macenski](https://www.linkedin.com/in/steven-macenski-41a985101) while at [Simbe Robotics](https://www.simberobotics.com/) and in his free time. + +This project contains the ability to do most everything any other available SLAM library, both free and paid, and more. This includes: +- Ordinary point-and-shoot 2D SLAM mobile robotics folks expect (start, map, save pgm file) with some nice built in utilities like saving maps +- Continuing to refine, remap, or continue mapping a saved (serialized) pose-graph at any time +- life-long mapping: load a saved pose-graph continue mapping in a space while also removing extraneous information from newly added scans +- an optimization-based localization mode built on the pose-graph. Optionally run localization mode without a prior map for "lidar odometry" mode with local loop closures +- synchronous and asynchronous modes of mapping +- kinematic map merging (with an elastic graph manipulation merging technique in the works) +- plugin-based optimization solvers with a new optimized Google Ceres based plugin +- RVIZ plugin for interacting with the tools +- graph manipulation tools in RVIZ to manipulate nodes and connections during mapping +- Map serialization and lossless data storage +- ... more but those are the highlights + +For running on live production robots, I recommend using the snap or from the build farm: slam-toolbox, it has optimizations in it that make it about 10x faster. You need the deb/source install for the other developer level tools that don't need to be on the robot (rviz plugins, etc). + +This package has been benchmarked mapping building at 5x+ realtime up to about 30,000 sqft and 3x realtime up to about 60,000 sqft. with the largest area (I'm aware of) used was a 200,000 sq.ft. building in synchronous mode (e.i. processing all scans, regardless of lag), and *much* larger spaces in asynchronous mode. + +The video below was collected at [Circuit Launch](https://www.circuitlaunch.com/) in Oakland, California. Thanks to [Silicon Valley Robotics](https://svrobo.org/) & Circuit Launch for being a testbed for some of this work. This data is currently available upon request, but its going to be included in a larger open-source dataset down the line. + +![map_image](/images/circuit_launch.gif?raw=true "Map Image") + +An overview of how the map was generated is presented below: +![slam_toolbox_sync_diagram](/images/slam_toolbox_sync.png) +1. ROS Node: SLAM toolbox is run in synchronous mode, which generates a ROS node. This node subscribes to laser scan and odometry topics, and publishes map to odom transform and a map. +2. Get odometry and LIDAR data: A callback for the laser topic will generate a pose (using odometry) and a laser scan tied at that node. These PosedScan objects form a queue, which are processed by the algorithm. +3. Process Data: The queue of PosedScan objects are used to construct a pose graph; odometry is refined using laser scan matching. This pose graph is used to compute robot pose, and find loop closures. If a loop closure is found, the pose graph is optimized, and pose estimates are updated. Pose estimates are used to compute and publish a map to odom transform for the robot. +4. Mapping: Laser scans associated with each pose in the pose graph are used to construct and publish a map. + +# 03/23/2021 Note On Serialized Files + +As of 03/23/2021, the contents of the serialized files has changed. For all new users after this date, this regard this section it does not impact you. + +If you have previously existing serialized files (e.g. not `pgm` maps, but `.posegraph` serialized slam sessions), after this date, you may need to take some action to maintain current features. Unfortunately, an ABI breaking change was required to be made in order to fix a very large bug affecting any 360 or non-axially-mounted LIDAR system. + +[This Discourse post](https://discourse.ros.org/t/request-for-input-potential-existing-slam-toolbox-serialized-file-invalidation/19520) highlights the issues. The frame storing the scan data for the optimizer was incorrect leading to explosions or flipping of maps for 360 and non-axially-aligned robots when using conservative loss functions. This change permanently fixes this issue, however it changes the frame of reference that this data is stored and serialized in. If your system as a non-360 lidar and it is mounted with its frame aligned with the robot base frame, you're unlikely to notice a problem and can disregard this statement. For all others noticing issues, you have the following options: +- Use the `-devel-unfixed` branch rather than `-devel`, which contains the unfixed version of this distribution's release which will be maintained in parallel to the main branches to have an option to continue with your working solution +- Convert your serialized files into the new reference frame with an offline utility +- Take the raw data and rerun the SLAM sessions to get a new serialized file with the right content + +More of the conversation can be seen on tickets #198 and #281. I apologize for the inconvenience, however this solves a very large bug that was impacting a large number of users. I've worked hard to make sure there's a viable path forward for everyone. + +# LifeLong Mapping + + + +LifeLong mapping is the concept of being able to map a space, completely or partially, and over time, refine and update that map as you continue to interact with the space. Our approach implements this and also takes care to allow for the application of operating in the cloud, as well as mapping with many robots in a shared space (cloud distributed mapping). While Slam Toolbox can also just be used for a point-and-shoot mapping of a space and saving that map as a .pgm file as maps are traditionally stored in, it also allows you to save the pose-graph and metadata losslessly to reload later with the same or different robot and continue to map the space. + +Our lifelong mapping consists of a few key steps +- Serialization and Deserialization to store and reload map information +- KD-Tree search matching to locate the robot in its position on reinitalization +- pose-graph optimizition based SLAM with 2D scan matching abstraction + +This will allow the user to create and update existing maps, then serialize the data for use in other mapping sessions, something sorely lacking from most SLAM implementations and nearly all planar SLAM implementations. Other good libraries that do this include RTab-Map and Cartoprapher, though they themselves have their own quirks that make them (in my opinion) unusable for production robotics applications. This library provides the mechanics to save not only the data, but the pose graph, and associated metadata to work with. This has been used to create maps by merging techniques (taking 2 or more serialized objects and creating 1 globally consistent one) as well as continuous mapping techniques (updating 1, same, serialized map object over time and refining it). The major benefit of this over RTab-Map or Cartoprapher is the maturity of the underlying (but heavily modified) `open_karto` library the project is based on. The scan matcher of Karto is well known as an extremely good matcher for 2D laser scans and modified versions of Karto can be found in companies across the world. + +Slam Toolbox supports all the major modes: +- Starting from a predefined dock (assuming to be near start region) +- Starting at any particular node - select a node ID to start near +- Starting in any particular area - indicate current pose in the map frame to start at, like AMCL + +In the RVIZ interface (see section below) you'll be able to re-localize in a map or continue mapping graphically or programatically using ROS services. + +On time of writing: there a **highly** experimental implementation of what I call "true lifelong" mapping that does support the method for removing nodes over time as well as adding nodes, this results in a true ability to map for life since the computation is bounded by removing extraneous or outdated information. Its recommended to run the non-full LifeLong mapping mode in the cloud for the increased computational burdens if you'd like to be continuously refining a map. However a real and desperately needed application of this is to have multi-session mapping to update just a section of the map or map half an area at a time to create a full (and then static) map for AMCL or Slam Toolbox localization mode, which this will handle in spades. The immediate plan is to create a mode within LifeLong mapping to decay old nodes to bound the computation and allow it to run on the edge by refining the experimental node. Continuing mapping (lifelong) should be used to build a complete map then switch to the pose-graph deformation localization mode until node decay is implemented, and **you should not see any substantial performance impacts**. + + +# Localization + + + +Localization mode consists of 3 things: +- Loads existing serialized map into the node +- Maintains a rolling buffer of recent scans in the pose-graph +- After expiring from the buffer scans are removed and the underlying map is not affected + +Localization methods on image map files has been around for years and works relatively well. There has not been a great deal of work in academia to refine these algorithms to a degree that satesfies me. However SLAM is a rich and well benchmarked topic. The inspiration of this work was the concept of "Can we make localization, SLAM again?" such that we can take advantage of all the nice things about SLAM for localization, but remove the unbounded computational increase. + +To enable, set `mode: localization` in the configuration file to allow for the Ceres plugin to set itself correctly to be able to quickly add *and remove* nodes and constraints from the pose graph, but isn't strictly required, but a performance optimization. The localization mode will automatically load your pose graph, take the first scan and match it against the local area to further refine your estimated position, and start localizing. + +To minimize the amount of changes required for moving to this mode over AMCL, we also expose a subscriber to the `/initialpose` topic used by AMCL to relocalize to a position, which also hooks up to the `2D Pose Estimation` tool in RVIZ. This way you can enter localization mode with our approach but continue to use the same API as you expect from AMCL for ease of integration. + +In summary, this approach I dub `elastic pose-graph localization` is where we take existing map pose-graphs and localized with-in them with a rolling window of recent scans. This way we can localize in an existing map using the scan matcher, but not update the underlaying map long-term should something go wrong. It can be considered a replacement to AMCL and results is not needing any .pgm maps ever again. The lifelong mapping/continuous slam mode above will do better if you'd like to modify the underlying graph while moving. + +## Tools + +### Plugin based Optimizers + +I have created a pluginlib interface for the ScanSolver abstract class so that you can change optimizers on runtime to test many different ones if you like. + +Then I generated plugins for a few different solvers that people might be interested in. I like to swap them out for benchmarking and make sure its the same code running for all. I have supported Ceres, G2O, SPA, and GTSAM. + +GTSAM/G2O/SPA is currently "unsupported" although all the code is there. They don't outperform Ceres settings I describe below so I stopped compiling them to save on build time, but they're there and work if you would like to use them. PRs to implement other optimizer plugins are welcome. + +### Map Merging - Example uses of serialized raw data & posegraphs + +#### Kinematic + +This uses RVIZ and the plugin to load any number of posegraphs that will show up in RVIZ under `map_N` and a set of interactive markers to allow you to move them around. Once you have them all positioned relative to each other in the way you like, you can merge the submaps into a global `map` which can be downloaded with your map server implementation of choice. + +It's more of a demonstration of other things you can do once you have the raw data to work with, but I don't suspect many people will get much use out of it unless you're used to stitching maps by hand. + +More information in the RVIZ Plugin section below. + +#### Pose Graph Merging + +This is under development. + +This is to solve the problem of merging many maps together with an initial guess of location in an elastic sense. This is something you just can't get if you don't have the full pose-graph and raw data to work with -- which we have from our continuous mapping work. + +Hint: This is also really good for multi-robot map updating as well :) + +### RVIZ Plugin + +An rviz plugin is furnished to help with manual loop closures and online / offline mapping. By default interactive mode is off (allowing you to move nodes) as this takes quite a toll on rviz. When you want to move nodes, tick the interactive box, move what you want, and save changes to prompt a manual loop closure. Clear if you made a mistake. When done, exit interactive mode again. + +There's also a tool to help you control online and offline data. You can at any time stop processing new scans or accepting new scans into the queue. This is desirable when you want to allow the package to catch up while the robot sits still (**This option is only meaningful in synchronous mode. In asynchronous mode the robot will never fall behind.**) or you want to stop processing new scans while you do a manual loop closure / manual "help". If there's more in the queue than you want, you may also clear it. + +Additionally there's exposed buttons for the serialization and deserialization services to load an old pose-graph to update and refine, or continue mapping, then save back to file. The "Start By Dock" checkbox will try to scan match against the first node (assuming you started at your dock) to give you an odometry estimate to start with. Another option is to start using an inputted position in the GUI or by calling the underlying service. Additionally, you can use the current odometric position estimation if you happened to have just paused the robot or not moved much between runs. Finally (and most usefully), you can use the RVIZ tool for **2D Pose Estimation** to tell it where to go in **localization mode** just like AMCL. + +Additionally the RVIZ plugin will allow you to add serialized map files as submaps in RVIZ. They will be displayed with an interactive marker you can translate and rotate to match up, then generate a composite map with the Generate Map button. At that point the composite map is being broadcasted on the `/map` topic and you can save it with the `map_saver`. + +It's recommended to always continue mapping near the dock, if that's not possible, look into the starting from pose or map merging techniques. This RVIZ plugin is mostly here as a debug utility, but if you often find yourself mapping areas using rviz already, I'd just have it open. All the RVIZ buttons are implemented using services that a master application can control. + +The interface is shown below. + +![rviz_plugin](/images/rviz_plugin.png?raw=true "Rviz Plugin") + +### Graph Manipulation + +By enabling `Interactive Mode`, the graph nodes will change from markers to interactive markers which you can manipulate. When you move a node(s), you can Save Changes and it will send the updated position to the pose-graph and cause an optimization run to occur to change the pose-graph with your new node location. This is helpful if the robot gets pushed, slips, runs into a wall, or otherwise has drifting odometry and you would like to manually correct it. + +When a map is sufficiently large, the number of interactive markers in RVIZ may be too large and RVIZ may start to lag. I only recommend using this feature as a testing debug tool and not for production. However if you are able to make it work with 10,000 interactive markers, I'll merge that PR in a heartbeat. Otherwise I'd restrict the use of this feature to small maps or with limited time to make a quick change and return to static mode by unchecking the box. + +## Metrics + +If you're a weirdo like me and you want to see how I came up with the settings I had for the Ceres optimizer, see below. + +![ceres_solver_comparison](https://user-images.githubusercontent.com/14944147/41576505-a6802d76-733c-11e8-8eca-334da2c8bd50.png) + +The data sets present solve time vs number of nodes in the pose graph on a large dataset, as that is not open source, but suffice to say that the settings I recommend work well. I think anyone would be hardset in a normal application to exceed or find that another solver type is better (that super low curve on the bottom one, yeah, that's it). Benchmark on a low power 7th gen i7 machine. + +It can map _very_ large spaces with reasonable CPU and memory consumption. My default settings increase O(N) on number of elements in the pose graph. I recommend from extensive testing to use the `SPARSE_NORMAL_CHOLESKY` solver with Ceres and the `SCHUR_JACOBI` preconditioner. Using `LM` at the trust region strategy is comparable to the dogleg subspace strategy, but `LM` is much better supported so why argue with it. + +You can get away without a loss function if your odometry is good (ie likelihood for outliers is extremely low). If you have an abnormal application or expect wheel slippage, I might recommend a `HuberLoss` function, which is a really good catch-all loss function if you're looking for a place to start. All these options and more are available from the ROS parameter server. + +# API + +The following are the services/topics that are exposed for use. See the rviz plugin for an implementation of their use. + +## Subscribed topics + +| scan | `sensor_msgs/LaserScan` | the input scan from your laser to utilize | +|-----|----|----| +| **tf** | N/A | a valid transform from your configured odom_frame to base_frame | + + +## Published topics + +| Topic | Type | Description | +|-----|----|----| +| map | `nav_msgs/OccupancyGrid` | occupancy grid representation of the pose-graph at `map_update_interval` frequency | +| pose | `geometry_msgs/PoseWithCovarianceStamped` | pose of the base_frame in the configured map_frame along with the covariance calculated from the scan match | + +## Exposed Services + +| Topic | Type | Description | +|-----|----|----| +| `/slam_toolbox/clear_changes` | `slam_toolbox/Clear` | Clear all manual pose-graph manipulation changes pending | +| `/slam_toolbox/deserialize_map` | `slam_toolbox/DeserializePoseGraph` | Load a saved serialized pose-graph files from disk | +| `/slam_toolbox/dynamic_map` | `nav_msgs/OccupancyGrid` | Request the current state of the pose-graph as an occupancy grid | +| `/slam_toolbox/manual_loop_closure` | `slam_toolbox/LoopClosure` | Request the manual changes to the pose-graph pending to be processed | +| `/slam_toolbox/pause_new_measurements` | `slam_toolbox/Pause` | Pause processing of new incoming laser scans by the toolbox | +| `/slam_toolbox/save_map` | `slam_toolbox/SaveMap` | Save the map image file of the pose-graph that is useable for display or AMCL localization. It is a simple wrapper on `map_server/map_saver` but is useful. | +| `/slam_toolbox/serialize_map` | `slam_toolbox/SerializePoseGraph` | Save the map pose-graph and datathat is useable for continued mapping, slam_toolbox localization, offline manipulation, and more | +| `/slam_toolbox/toggle_interactive_mode` | `slam_toolbox/ToggleInteractive` | Toggling in and out of interactive mode, publishing interactive markers of the nodes and their positions to be updated in an application | + +# Configuration + +The following settings and options are exposed to you. My default configuration is given in `config` directory. + +## Solver Params + +`solver_plugin` - The type of nonlinear solver to utilize for karto's scan solver. Options: `solver_plugins::CeresSolver`, `solver_plugins::SpaSolver`, `solver_plugins::G2oSolver`. Default: `solver_plugins::CeresSolver`. + +`ceres_linear_solver` - The linear solver for Ceres to use. Options: `SPARSE_NORMAL_CHOLESKY`, `SPARSE_SCHUR`, `ITERATIVE_SCHUR`, `CGNR`. Defaults to `SPARSE_NORMAL_CHOLESKY`. + +`ceres_preconditioner` - The preconditioner to use with that solver. Options: `JACOBI`, `IDENTITY` (none), `SCHUR_JACOBI`. Defaults to `JACOBI`. + +`ceres_trust_strategy` - The trust region strategy. Line searach strategies are not exposed because they perform poorly for this use. Options: `LEVENBERG_MARQUARDT`, `DOGLEG`. Default: `LEVENBERG_MARQUARDT`. + +`ceres_dogleg_type` - The dogleg strategy to use if the trust strategy is `DOGLEG`. Options: `TRADITIONAL_DOGLEG`, `SUBSPACE_DOGLEG`. Default: `TRADITIONAL_DOGLEG` + +`ceres_loss_function` - The type of loss function to reject outlier measurements. None is equatable to a squared loss. Options: `None`, `HuberLoss`, `CauchyLoss`. Default: `None`. + +`mode` - "mapping" or "localization" mode for performance optimizations in the Ceres problem creation + +## Toolbox Params + +`odom_frame` - Odometry frame + +`map_frame` - Map frame + +`base_frame` - Base frame + +`map_file_name` - Name of the pose-graph file to load on startup if available + +`map_start_pose` - Pose to start pose-graph mapping/localization in, if available + +`map_start_at_dock` - Starting pose-graph loading at the dock (first node), if available. If both pose and dock are set, it will use pose + +`debug_logging` - Change logger to debug + +`throttle_scans` - Number of scans to throttle in synchronous mode + +`transform_publish_period` - The map to odom transform publish period. 0 will not publish transforms + +`map_update_interval` - Interval to update the 2D occupancy map for other applications / visualization + +`enable_interactive_mode` - Whether or not to allow for interactive mode to be enabled. Interactive mode will retain a cache of laser scans mapped to their ID for visualization in interactive mode. As a result the memory for the process will increase. This is manually disabled in localization and lifelong modes since they would increase the memory utilization over time. Valid for either mapping or continued mapping modes. + +`position_covariance_scale` - Amount to scale position covariance when publishing pose from scan match. This can be used to tune the influence of the pose position in a downstream localization filter. The covariance represents the uncertainty of the measurement, so scaling up the covariance will result in the pose position having less influence on downstream filters. Default: 1.0 + +`yaw_covariance_scale` - Amount to scale yaw covariance when publishing pose from scan match. See description of position_covariance_scale. Default: 1.0 + +`resolution` - Resolution of the 2D occupancy map to generate + +`max_laser_range` - Maximum laser range to use for 2D occupancy map rastering + +`minimum_time_interval` - The minimum duration of time between scans to be processed in synchronous mode + +`transform_timeout` - TF timeout for looking up transforms + +`tf_buffer_duration` - Duration to store TF messages for lookup. Set high if running offline at multiple times speed in synchronous mode. + +`stack_size_to_use` - The number of bytes to reset the stack size to, to enable serialization/deserialization of files. A liberal default is 40000000, but less is fine. + +`minimum_travel_distance` - Minimum distance of travel before processing a new scan + +## Matcher Params + +`use_scan_matching` - whether to use scan matching to refine odometric pose (uh, why would you not?) + +`use_scan_barycenter` - Whether to use the barycenter or scan pose + +`minimum_travel_heading` - Minimum changing in heading to justify an update + +`scan_buffer_size` - The number of scans to buffer into a chain, also used as the number of scans in the circular buffer of localization mode + +`scan_buffer_maximum_scan_distance` - Maximum distance of a scan from the pose before removing the scan from the buffer + +`link_match_minimum_response_fine` - The threshold link matching algorithm response for fine resolution to pass + +`link_scan_maximum_distance` - Maximum distance between linked scans to be valid + +`loop_search_maximum_distance` - Maximum threshold of distance for scans to be considered for loop closure + +`do_loop_closing` - Whether to do loop closure (if you're not sure, the answer is "true") + +`loop_match_minimum_chain_size` - The minimum chain length of scans to look for loop closure + +`loop_match_maximum_variance_coarse` - The threshold variance in coarse search to pass to refine + +`loop_match_minimum_response_coarse` - The threshold response of the loop closure algorithm in coarse search to pass to refine + +`loop_match_minimum_response_fine` - The threshold response of the loop closure algorithm in fine search to pass to refine + +`correlation_search_space_dimension` - Search grid size to do scan correlation over + +`correlation_search_space_resolution` - Search grid resolution to do scan correlation over + +`correlation_search_space_smear_deviation` - Amount of multimodal smearing to smooth out responses + +`loop_search_space_dimension` - Size of the search grid over the loop closure algorith + +`loop_search_space_resolution` - Search grid resolution to do loop closure over + +`loop_search_space_smear_deviation` - Amount of multimodal smearing to smooth out responses + +`distance_variance_penalty` - A penalty to apply to a matched scan as it differs from the odometric pose + +`angle_variance_penalty` - A penalty to apply to a matched scan as it differs from the odometric pose + +`fine_search_angle_offset` - Range of angles to test for fine scan matching + +`coarse_search_angle_offset` - Range of angles to test for coarse scan matching + +`coarse_angle_resolution` - Resolution of angles over the Offset range to test in scan matching + +`minimum_angle_penalty` - Smallest penalty an angle can have to ensure the size doesn't blow up + +`minimum_distance_penalty` - Smallest penalty a scan can have to ensure the size doesn't blow up + +`use_response_expansion` - Whether to automatically increase the search grid size if no viable match is found + +# Install + +ROSDep will take care of the major things + +``` +rosdep install -q -y -r --from-paths src --ignore-src +``` + +Also released in Melodic / Dashing to the ROS build farm to install debians. + +Run your catkin build procedure of choice. + +You can run via `roslaunch slam_toolbox online_sync.launch` + +# Etc + +## NanoFlann! + +In order to do some operations quickly for continued mapping and localization, I make liberal use of NanoFlann (shout out!). + + +## Brief incursion into snaps + +Snap are completely isolated containerized packages that one can run through the Canonical organization on a large number of Linux distributions. They're similar to Docker containers but it doesn't share the kernel or any of the libraries, and rather has everything internal as essentially a seperate partitioned operating system based on Ubuntu Core. + +We package up slam toolbox in this way for a nice multiple-on speed up in execution from a couple of pretty nuanced reasons in this particular project, but generally speaking you shouldn't expect a speedup from a snap. There's a generate snap script in the `snap` directory to create a snap. + +Since Snaps are totally isolated and there's no override flags like in Docker, there's only a couple of fixed directories that both the snap and the host system can write and read from, including SNAP_COMMON (usually in `/var/snap/[snap name]/common`). Therefore, this is the place that if you're serializing and deserializing maps, you need to have them accessible to that directory. + +You can optionally store all your serialized maps there, move maps there as needed, take maps from there after serialization, or do my favorite option and `link` the directories with `ln` to where ever you normally store your maps and you're wanting to dump your serialized map files. + +Example of `ln`: +``` +# Source Linked +sudo ln -s /home/steve/maps/serialized_map/ /var/snap/slam-toolbox/common +``` + +and then all you have to do when you specify a map to use is set the filename to `slam-toolbox/map_name` and it should work no matter if you're running in a snap, docker, or on bare metal. The `-s` makes a symbol link so rather than `/var/snap/slam-toolbox/common/*` containing the maps, `/var/snap/slam-toolbox/common/serialized_map/*` will. By default on bare metal, the maps will be saved in `.ros` + + +## More Gifs! + +![map_image](/images/mapping_steves_apartment.gif?raw=true "Map Image") + +If someone from iRobot can use this to tell me my Roomba serial number by correlating to its maps, I'll buy them lunch and probably try to hire them. diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox/.gitignore b/Localizations/Packages/slam_toolbox/slam_toolbox/.gitignore new file mode 100644 index 0000000..fd13d28 --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox/.gitignore @@ -0,0 +1 @@ +snap_ws/* diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox/CMake/FindCSparse.cmake b/Localizations/Packages/slam_toolbox/slam_toolbox/CMake/FindCSparse.cmake new file mode 100644 index 0000000..cdc728d --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox/CMake/FindCSparse.cmake @@ -0,0 +1,25 @@ +# Look for csparse; note the difference in the directory specifications! +FIND_PATH(CSPARSE_INCLUDE_DIR NAMES cs.h + PATHS + /usr/include/suitesparse + /usr/include + /opt/local/include + /usr/local/include + /sw/include + /usr/include/ufsparse + /opt/local/include/ufsparse + /usr/local/include/ufsparse + /sw/include/ufsparse + ) + +FIND_LIBRARY(CSPARSE_LIBRARY NAMES cxsparse + PATHS + /usr/lib + /usr/local/lib + /opt/local/lib + /sw/lib + ) + +include(FindPackageHandleStandardArgs) +find_package_handle_standard_args(CSPARSE DEFAULT_MSG + CSPARSE_INCLUDE_DIR CSPARSE_LIBRARY) \ No newline at end of file diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox/CMake/FindCholmod.cmake b/Localizations/Packages/slam_toolbox/slam_toolbox/CMake/FindCholmod.cmake new file mode 100644 index 0000000..3f098ec --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox/CMake/FindCholmod.cmake @@ -0,0 +1,89 @@ +# Cholmod lib usually requires linking to a blas and lapack library. +# It is up to the user of this module to find a BLAS and link to it. + +if (CHOLMOD_INCLUDE_DIR AND CHOLMOD_LIBRARIES) + set(CHOLMOD_FIND_QUIETLY TRUE) +endif (CHOLMOD_INCLUDE_DIR AND CHOLMOD_LIBRARIES) + +find_path(CHOLMOD_INCLUDE_DIR + NAMES + cholmod.h + PATHS + $ENV{CHOLMODDIR} + ${INCLUDE_INSTALL_DIR} + PATH_SUFFIXES + suitesparse + ufsparse +) + +find_library(CHOLMOD_LIBRARY cholmod PATHS $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR}) +set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARY}) + +if(CHOLMOD_LIBRARIES) + + get_filename_component(CHOLMOD_LIBDIR ${CHOLMOD_LIBRARIES} PATH) + + find_library(AMD_LIBRARY amd PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR}) + if (AMD_LIBRARY) + set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${AMD_LIBRARY}) + else () + set(CHOLMOD_LIBRARIES FALSE) + endif () + +endif(CHOLMOD_LIBRARIES) + +if(CHOLMOD_LIBRARIES) + + find_library(COLAMD_LIBRARY colamd PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR}) + if (COLAMD_LIBRARY) + set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${COLAMD_LIBRARY}) + else () + set(CHOLMOD_LIBRARIES FALSE) + endif () + +endif(CHOLMOD_LIBRARIES) + +if(CHOLMOD_LIBRARIES) + + find_library(CAMD_LIBRARY camd PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR}) + if (CAMD_LIBRARY) + set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${CAMD_LIBRARY}) + else () + set(CHOLMOD_LIBRARIES FALSE) + endif () + +endif(CHOLMOD_LIBRARIES) + +if(CHOLMOD_LIBRARIES) + + find_library(CCOLAMD_LIBRARY ccolamd PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR}) + if (CCOLAMD_LIBRARY) + set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${CCOLAMD_LIBRARY}) + else () + set(CHOLMOD_LIBRARIES FALSE) + endif () + +endif(CHOLMOD_LIBRARIES) + +if(CHOLMOD_LIBRARIES) + + find_library(CHOLMOD_METIS_LIBRARY metis PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR}) + if (CHOLMOD_METIS_LIBRARY) + set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${CHOLMOD_METIS_LIBRARY}) + endif () + +endif(CHOLMOD_LIBRARIES) + +if(CHOLMOD_LIBRARIES) + find_library(SUITESPARSECONFIG_LIBRARY NAMES suitesparseconfig + PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR}) + if (SUITESPARSECONFIG_LIBRARY) + set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${SUITESPARSECONFIG_LIBRARY}) + endif () +endif(CHOLMOD_LIBRARIES) + +include(FindPackageHandleStandardArgs) +find_package_handle_standard_args(CHOLMOD DEFAULT_MSG + CHOLMOD_INCLUDE_DIR CHOLMOD_LIBRARIES) + +mark_as_advanced(CHOLMOD_LIBRARIES) \ No newline at end of file diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox/CMake/FindG2O.cmake b/Localizations/Packages/slam_toolbox/slam_toolbox/CMake/FindG2O.cmake new file mode 100644 index 0000000..0249002 --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox/CMake/FindG2O.cmake @@ -0,0 +1,122 @@ +# Find the header files + +FIND_PATH(G2O_INCLUDE_DIR g2o/core/base_vertex.h + ${G2O_ROOT}/include + $ENV{G2O_ROOT}/include + $ENV{G2O_ROOT} + /usr/local/include + /usr/include + /opt/local/include + /sw/local/include + /sw/include + /opt/ros/indigo/include + /opt/ros/jade/include + /opt/ros/kinetic/include + NO_DEFAULT_PATH + ) + +# Macro to unify finding both the debug and release versions of the +# libraries; this is adapted from the OpenSceneGraph FIND_LIBRARY +# macro. + +MACRO(FIND_G2O_LIBRARY MYLIBRARY MYLIBRARYNAME) + + FIND_LIBRARY("${MYLIBRARY}_DEBUG" + NAMES "g2o_${MYLIBRARYNAME}_d" + PATHS + ${G2O_ROOT}/lib/Debug + ${G2O_ROOT}/lib + $ENV{G2O_ROOT}/lib/Debug + $ENV{G2O_ROOT}/lib + NO_DEFAULT_PATH + ) + + FIND_LIBRARY("${MYLIBRARY}_DEBUG" + NAMES "g2o_${MYLIBRARYNAME}_d" + PATHS + ~/Library/Frameworks + /Library/Frameworks + /usr/local/lib + /usr/local/lib64 + /usr/lib + /usr/lib64 + /opt/local/lib + /sw/local/lib + /sw/lib + /opt/ros/indigo/lib + /opt/ros/jade/lib + /opt/ros/kinetic/lib + ) + + FIND_LIBRARY(${MYLIBRARY} + NAMES "g2o_${MYLIBRARYNAME}" + PATHS + ${G2O_ROOT}/lib/Release + ${G2O_ROOT}/lib + $ENV{G2O_ROOT}/lib/Release + $ENV{G2O_ROOT}/lib + NO_DEFAULT_PATH + ) + + FIND_LIBRARY(${MYLIBRARY} + NAMES "g2o_${MYLIBRARYNAME}" + PATHS + ~/Library/Frameworks + /Library/Frameworks + /usr/local/lib + /usr/local/lib64 + /usr/lib + /usr/lib64 + /opt/local/lib + /sw/local/lib + /sw/lib + /opt/ros/indigo/lib + /opt/ros/jade/lib + /opt/ros/kinetic/lib + ) + + IF(NOT ${MYLIBRARY}_DEBUG) + IF(MYLIBRARY) + SET(${MYLIBRARY}_DEBUG ${MYLIBRARY}) + ENDIF(MYLIBRARY) + ENDIF( NOT ${MYLIBRARY}_DEBUG) + +ENDMACRO(FIND_G2O_LIBRARY LIBRARY LIBRARYNAME) + +# Find the core elements +FIND_G2O_LIBRARY(G2O_STUFF_LIBRARY stuff) +FIND_G2O_LIBRARY(G2O_CORE_LIBRARY core) + +# Find the CLI library +FIND_G2O_LIBRARY(G2O_CLI_LIBRARY cli) + +# Find the pluggable solvers +FIND_G2O_LIBRARY(G2O_SOLVER_CHOLMOD solver_cholmod) +FIND_G2O_LIBRARY(G2O_SOLVER_CSPARSE solver_csparse) +FIND_G2O_LIBRARY(G2O_SOLVER_CSPARSE_EXTENSION csparse_extension) +FIND_G2O_LIBRARY(G2O_SOLVER_DENSE solver_dense) +FIND_G2O_LIBRARY(G2O_SOLVER_PCG solver_pcg) +FIND_G2O_LIBRARY(G2O_SOLVER_SLAM2D_LINEAR solver_slam2d_linear) +FIND_G2O_LIBRARY(G2O_SOLVER_STRUCTURE_ONLY solver_structure_only) +FIND_G2O_LIBRARY(G2O_SOLVER_EIGEN solver_eigen) + +# Find the predefined types +FIND_G2O_LIBRARY(G2O_TYPES_DATA types_data) +FIND_G2O_LIBRARY(G2O_TYPES_ICP types_icp) +FIND_G2O_LIBRARY(G2O_TYPES_SBA types_sba) +FIND_G2O_LIBRARY(G2O_TYPES_SCLAM2D types_sclam2d) +FIND_G2O_LIBRARY(G2O_TYPES_SIM3 types_sim3) +FIND_G2O_LIBRARY(G2O_TYPES_SLAM2D types_slam2d) +FIND_G2O_LIBRARY(G2O_TYPES_SLAM3D types_slam3d) + +# G2O solvers declared found if we found at least one solver +SET(G2O_SOLVERS_FOUND "NO") +IF(G2O_SOLVER_CHOLMOD OR G2O_SOLVER_CSPARSE OR G2O_SOLVER_DENSE OR G2O_SOLVER_PCG OR G2O_SOLVER_SLAM2D_LINEAR OR G2O_SOLVER_STRUCTURE_ONLY OR G2O_SOLVER_EIGEN) + SET(G2O_SOLVERS_FOUND "YES") +ENDIF(G2O_SOLVER_CHOLMOD OR G2O_SOLVER_CSPARSE OR G2O_SOLVER_DENSE OR G2O_SOLVER_PCG OR G2O_SOLVER_SLAM2D_LINEAR OR G2O_SOLVER_STRUCTURE_ONLY OR G2O_SOLVER_EIGEN) + +# G2O itself declared found if we found the core libraries and at least one solver +SET(G2O_FOUND "NO") +IF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND) + SET(G2O_FOUND "YES") +ENDIF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND) \ No newline at end of file diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox/CMake/FindGTSAM.cmake b/Localizations/Packages/slam_toolbox/slam_toolbox/CMake/FindGTSAM.cmake new file mode 100644 index 0000000..aa07270 --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox/CMake/FindGTSAM.cmake @@ -0,0 +1,85 @@ +# This is FindGTSAM.cmake +# CMake module to locate the GTSAM package +# +# The following cache variables may be set before calling this script: +# +# GTSAM_DIR (or GTSAM_ROOT): (Optional) The install prefix OR source tree of gtsam (e.g. /usr/local or src/gtsam) +# GTSAM_BUILD_NAME: (Optional) If compiling against a source tree, the name of the build directory +# within it (e.g build-debug). Without this defined, this script tries to +# intelligently find the build directory based on the project's build directory name +# or based on the build type (Debug/Release/etc). +# +# The following variables will be defined: +# +# GTSAM_FOUND : TRUE if the package has been successfully found +# GTSAM_INCLUDE_DIR : paths to GTSAM's INCLUDE directories +# GTSAM_LIBS : paths to GTSAM's libraries +# +# NOTES on compiling against an uninstalled GTSAM build tree: +# - A GTSAM source tree will be automatically searched for in the directory +# 'gtsam' next to your project directory, after searching +# CMAKE_INSTALL_PREFIX and $HOME, but before searching /usr/local and /usr. +# - The build directory will be searched first with the same name as your +# project's build directory, e.g. if you build from 'MyProject/build-optimized', +# 'gtsam/build-optimized' will be searched first. Next, a build directory for +# your project's build type, e.g. if CMAKE_BUILD_TYPE in your project is +# 'Release', then 'gtsam/build-release' will be searched next. Finally, plain +# 'gtsam/build' will be searched. +# - You can control the gtsam build directory name directly by defining the CMake +# cache variable 'GTSAM_BUILD_NAME', then only 'gtsam/${GTSAM_BUILD_NAME} will +# be searched. +# - Use the standard CMAKE_PREFIX_PATH, or GTSAM_DIR, to find a specific gtsam +# directory. + +# Get path suffixes to help look for gtsam +if(GTSAM_BUILD_NAME) + set(gtsam_build_names "${GTSAM_BUILD_NAME}/gtsam") +else() + # lowercase build type + string(TOLOWER "${CMAKE_BUILD_TYPE}" build_type_suffix) + # build suffix of this project + get_filename_component(my_build_name "${CMAKE_BINARY_DIR}" NAME) + + set(gtsam_build_names "${my_build_name}/gtsam" "build-${build_type_suffix}/gtsam" "build/gtsam") +endif() + +# Use GTSAM_ROOT or GTSAM_DIR equivalently +if(GTSAM_ROOT AND NOT GTSAM_DIR) + set(GTSAM_DIR "${GTSAM_ROOT}") +endif() + +if(GTSAM_DIR) + # Find include dirs + find_path(GTSAM_INCLUDE_DIR gtsam/inference/FactorGraph.h + PATHS "${GTSAM_DIR}/include" "${GTSAM_DIR}" NO_DEFAULT_PATH + DOC "GTSAM include directories") + + # Find libraries + find_library(GTSAM_LIBS NAMES gtsam + HINTS "${GTSAM_DIR}/lib" "${GTSAM_DIR}" NO_DEFAULT_PATH + PATH_SUFFIXES ${gtsam_build_names} + DOC "GTSAM libraries") +else() + # Find include dirs + set(extra_include_paths ${CMAKE_INSTALL_PREFIX}/include "$ENV{HOME}/include" "${PROJECT_SOURCE_DIR}/../gtsam" /usr/local/include /usr/include) + find_path(GTSAM_INCLUDE_DIR gtsam/inference/FactorGraph.h + PATHS ${extra_include_paths} + DOC "GTSAM include directories") + if(NOT GTSAM_INCLUDE_DIR) + message(STATUS "Searched for gtsam headers in default paths plus ${extra_include_paths}") + endif() + + # Find libraries + find_library(GTSAM_LIBS NAMES gtsam + HINTS ${CMAKE_INSTALL_PREFIX}/lib "$ENV{HOME}/lib" "${PROJECT_SOURCE_DIR}/../gtsam" /usr/local/lib /usr/lib + PATH_SUFFIXES ${gtsam_build_names} + DOC "GTSAM libraries") +endif() + +# handle the QUIETLY and REQUIRED arguments and set GTSAM_FOUND to TRUE +# if all listed variables are TRUE +include(FindPackageHandleStandardArgs) +find_package_handle_standard_args(GTSAM DEFAULT_MSG + GTSAM_LIBS GTSAM_INCLUDE_DIR) + + diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox/CMake/FindMKL.cmake b/Localizations/Packages/slam_toolbox/slam_toolbox/CMake/FindMKL.cmake new file mode 100644 index 0000000..8fb3be2 --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox/CMake/FindMKL.cmake @@ -0,0 +1,250 @@ +# This file is adapted from the one in OpenMEEG: http://www-sop.inria.fr/athena/software/OpenMEEG/ +# - Try to find the Intel Math Kernel Library +# Once done this will define +# +# MKL_FOUND - system has MKL +# MKL_ROOT_DIR - path to the MKL base directory +# MKL_INCLUDE_DIR - the MKL include directory +# MKL_LIBRARIES - MKL libraries +# +# There are few sets of libraries: +# Array indexes modes: +# LP - 32 bit indexes of arrays +# ILP - 64 bit indexes of arrays +# Threading: +# SEQUENTIAL - no threading +# INTEL - Intel threading library +# GNU - GNU threading library +# MPI support +# NOMPI - no MPI support +# INTEL - Intel MPI library +# OPEN - Open MPI library +# SGI - SGI MPT Library + +# linux +IF(UNIX AND NOT APPLE) + IF(${CMAKE_HOST_SYSTEM_PROCESSOR} STREQUAL "x86_64") + SET(MKL_ARCH_DIR "intel64") + ELSE() + SET(MKL_ARCH_DIR "32") + ENDIF() +ENDIF() +# macos +IF(APPLE) + SET(MKL_ARCH_DIR "intel64") +ENDIF() + +IF(FORCE_BUILD_32BITS) + set(MKL_ARCH_DIR "32") +ENDIF() +# windows +IF(WIN32) + IF(${CMAKE_SIZEOF_VOID_P} EQUAL 8) + SET(MKL_ARCH_DIR "intel64") + ELSE() + SET(MKL_ARCH_DIR "ia32") + ENDIF() +ENDIF() + +SET(MKL_THREAD_VARIANTS SEQUENTIAL GNUTHREAD INTELTHREAD) +SET(MKL_MODE_VARIANTS ILP LP) +SET(MKL_MPI_VARIANTS NOMPI INTELMPI OPENMPI SGIMPT) + +FIND_PATH(MKL_ROOT_DIR + include/mkl_cblas.h + PATHS + $ENV{MKLDIR} + /opt/intel/mkl/ + /opt/intel/mkl/*/ + /opt/intel/cmkl/ + /opt/intel/cmkl/*/ + /opt/intel/*/mkl/ + /Library/Frameworks/Intel_MKL.framework/Versions/Current/lib/universal + "C:/Program Files (x86)/Intel/ComposerXE-2011/mkl" + "C:/Program Files (x86)/Intel/Composer XE 2013/mkl" + "C:/Program Files/Intel/MKL/*/" + "C:/Program Files/Intel/ComposerXE-2011/mkl" + "C:/Program Files/Intel/Composer XE 2013/mkl" +) + +FIND_PATH(MKL_INCLUDE_DIR + mkl_cblas.h + PATHS + ${MKL_ROOT_DIR}/include + ${INCLUDE_INSTALL_DIR} +) + +FIND_PATH(MKL_FFTW_INCLUDE_DIR + fftw3.h + PATH_SUFFIXES fftw + PATHS + ${MKL_ROOT_DIR}/include + ${INCLUDE_INSTALL_DIR} + NO_DEFAULT_PATH +) + +IF(WIN32) + SET(MKL_LIB_SEARCHPATH $ENV{ICC_LIB_DIR} $ENV{MKL_LIB_DIR} "${MKL_ROOT_DIR}/lib/${MKL_ARCH_DIR}" "${MKL_ROOT_DIR}/../compiler" "${MKL_ROOT_DIR}/../compiler/lib/${MKL_ARCH_DIR}") + + IF (MKL_INCLUDE_DIR MATCHES "10.") + IF(CMAKE_CL_64) + SET(MKL_LIBS mkl_solver_lp64 mkl_core mkl_intel_lp64 mkl_intel_thread libguide mkl_lapack95_lp64 mkl_blas95_lp64) + ELSE() + SET(MKL_LIBS mkl_solver mkl_core mkl_intel_c mkl_intel_s mkl_intel_thread libguide mkl_lapack95 mkl_blas95) + ENDIF() + ELSEIF(MKL_INCLUDE_DIR MATCHES "2013") # version 11 ... + IF(CMAKE_CL_64) + SET(MKL_LIBS mkl_core mkl_intel_lp64 mkl_intel_thread libiomp5md mkl_lapack95_lp64 mkl_blas95_lp64) + ELSE() + SET(MKL_LIBS mkl_core mkl_intel_c mkl_intel_s mkl_intel_thread libiomp5md mkl_lapack95 mkl_blas95) + ENDIF() + ELSE() # old MKL 9 + SET(MKL_LIBS mkl_solver mkl_c libguide mkl_lapack mkl_ia32) + ENDIF() + + IF (MKL_INCLUDE_DIR MATCHES "10.3") + SET(MKL_LIBS ${MKL_LIBS} libiomp5md) + ENDIF() + + FOREACH (LIB ${MKL_LIBS}) + FIND_LIBRARY(${LIB}_PATH ${LIB} PATHS ${MKL_LIB_SEARCHPATH} ENV LIBRARY_PATH) + IF(${LIB}_PATH) + SET(MKL_LIBRARIES ${MKL_LIBRARIES} ${${LIB}_PATH}) + ELSE() + MESSAGE(STATUS "Could not find ${LIB}: disabling MKL") + ENDIF() + ENDFOREACH() + SET(MKL_FOUND ON) +ELSE() # UNIX and macOS + FIND_LIBRARY(MKL_CORE_LIBRARY + mkl_core + PATHS + ${MKL_ROOT_DIR}/lib/${MKL_ARCH_DIR} + ${MKL_ROOT_DIR}/lib/ + ) + + # Threading libraries + FIND_LIBRARY(MKL_SEQUENTIAL_LIBRARY + mkl_sequential + PATHS + ${MKL_ROOT_DIR}/lib/${MKL_ARCH_DIR} + ${MKL_ROOT_DIR}/lib/ + ) + + FIND_LIBRARY(MKL_INTELTHREAD_LIBRARY + mkl_intel_thread + PATHS + ${MKL_ROOT_DIR}/lib/${MKL_ARCH_DIR} + ${MKL_ROOT_DIR}/lib/ + ) + + # MKL on Mac OS doesn't ship with GNU thread versions, only Intel versions (see above) + IF(NOT APPLE) + FIND_LIBRARY(MKL_GNUTHREAD_LIBRARY + mkl_gnu_thread + PATHS + ${MKL_ROOT_DIR}/lib/${MKL_ARCH_DIR} + ${MKL_ROOT_DIR}/lib/ + ) + ENDIF() + + # Intel Libraries + IF("${MKL_ARCH_DIR}" STREQUAL "32") + FIND_LIBRARY(MKL_LP_LIBRARY + mkl_intel + PATHS + ${MKL_ROOT_DIR}/lib/${MKL_ARCH_DIR} + ${MKL_ROOT_DIR}/lib/ + ) + + FIND_LIBRARY(MKL_ILP_LIBRARY + mkl_intel + PATHS + ${MKL_ROOT_DIR}/lib/${MKL_ARCH_DIR} + ${MKL_ROOT_DIR}/lib/ + ) + else() + FIND_LIBRARY(MKL_LP_LIBRARY + mkl_intel_lp64 + PATHS + ${MKL_ROOT_DIR}/lib/${MKL_ARCH_DIR} + ${MKL_ROOT_DIR}/lib/ + ) + + FIND_LIBRARY(MKL_ILP_LIBRARY + mkl_intel_ilp64 + PATHS + ${MKL_ROOT_DIR}/lib/${MKL_ARCH_DIR} + ${MKL_ROOT_DIR}/lib/ + ) + ENDIF() + + # Lapack + FIND_LIBRARY(MKL_LAPACK_LIBRARY + mkl_lapack + PATHS + ${MKL_ROOT_DIR}/lib/${MKL_ARCH_DIR} + ${MKL_ROOT_DIR}/lib/ + ) + + IF(NOT MKL_LAPACK_LIBRARY) + FIND_LIBRARY(MKL_LAPACK_LIBRARY + mkl_lapack95_lp64 + PATHS + ${MKL_ROOT_DIR}/lib/${MKL_ARCH_DIR} + ${MKL_ROOT_DIR}/lib/ + ) + ENDIF() + + # iomp5 + IF("${MKL_ARCH_DIR}" STREQUAL "32") + IF(UNIX AND NOT APPLE) + FIND_LIBRARY(MKL_IOMP5_LIBRARY + iomp5 + PATHS + ${MKL_ROOT_DIR}/../lib/ia32 + ) + ELSE() + SET(MKL_IOMP5_LIBRARY "") # no need for mac + ENDIF() + else() + IF(UNIX AND NOT APPLE) + FIND_LIBRARY(MKL_IOMP5_LIBRARY + iomp5 + PATHS + ${MKL_ROOT_DIR}/../lib/intel64 + ) + ELSE() + SET(MKL_IOMP5_LIBRARY "") # no need for mac + ENDIF() + ENDIF() + + foreach (MODEVAR ${MKL_MODE_VARIANTS}) + foreach (THREADVAR ${MKL_THREAD_VARIANTS}) + if (MKL_CORE_LIBRARY AND MKL_${MODEVAR}_LIBRARY AND MKL_${THREADVAR}_LIBRARY) + set(MKL_${MODEVAR}_${THREADVAR}_LIBRARIES + ${MKL_${MODEVAR}_LIBRARY} ${MKL_${THREADVAR}_LIBRARY} ${MKL_CORE_LIBRARY} + ${MKL_LAPACK_LIBRARY} ${MKL_IOMP5_LIBRARY}) + # message("${MODEVAR} ${THREADVAR} ${MKL_${MODEVAR}_${THREADVAR}_LIBRARIES}") # for debug + endif() + endforeach() + endforeach() + + IF(APPLE) + SET(MKL_LIBRARIES ${MKL_LP_INTELTHREAD_LIBRARIES}) + ELSE() + SET(MKL_LIBRARIES ${MKL_LP_GNUTHREAD_LIBRARIES}) + ENDIF() + + MARK_AS_ADVANCED(MKL_CORE_LIBRARY MKL_LP_LIBRARY MKL_ILP_LIBRARY + MKL_SEQUENTIAL_LIBRARY MKL_INTELTHREAD_LIBRARY MKL_GNUTHREAD_LIBRARY) +ENDIF() + +INCLUDE(FindPackageHandleStandardArgs) +find_package_handle_standard_args(MKL DEFAULT_MSG MKL_INCLUDE_DIR MKL_LIBRARIES) + +#if(MKL_FOUND) +# LINK_DIRECTORIES(${MKL_ROOT_DIR}/lib/${MKL_ARCH_DIR}) # hack +#endif() + +MARK_AS_ADVANCED(MKL_INCLUDE_DIR MKL_LIBRARIES) \ No newline at end of file diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox/CMake/FindSuiteSparse.cmake b/Localizations/Packages/slam_toolbox/slam_toolbox/CMake/FindSuiteSparse.cmake new file mode 100644 index 0000000..94b7614 --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox/CMake/FindSuiteSparse.cmake @@ -0,0 +1,127 @@ +FIND_PATH(CHOLMOD_INCLUDE_DIR NAMES cholmod.h amd.h camd.h + PATHS + ${SUITE_SPARSE_ROOT}/include + /usr/include/suitesparse + /usr/include/ufsparse + /opt/local/include/ufsparse + /usr/local/include/ufsparse + /sw/include/ufsparse + ) + +FIND_LIBRARY(CHOLMOD_LIBRARY NAMES cholmod + PATHS + ${SUITE_SPARSE_ROOT}/lib + /usr/lib + /usr/local/lib + /opt/local/lib + /sw/lib + ) + +FIND_LIBRARY(AMD_LIBRARY NAMES SHARED NAMES amd + PATHS + ${SUITE_SPARSE_ROOT}/lib + /usr/lib + /usr/local/lib + /opt/local/lib + /sw/lib + ) + +FIND_LIBRARY(CAMD_LIBRARY NAMES camd + PATHS + ${SUITE_SPARSE_ROOT}/lib + /usr/lib + /usr/local/lib + /opt/local/lib + /sw/lib + ) + +FIND_LIBRARY(SUITESPARSECONFIG_LIBRARY NAMES suitesparseconfig + PATHS + ${SUITE_SPARSE_ROOT}/lib + /usr/lib + /usr/local/lib + /opt/local/lib + /sw/lib + ) + + +# Different platforms seemingly require linking against different sets of libraries +IF(CYGWIN) + FIND_PACKAGE(PkgConfig) + FIND_LIBRARY(COLAMD_LIBRARY NAMES colamd + PATHS + /usr/lib + /usr/local/lib + /opt/local/lib + /sw/lib + ) + PKG_CHECK_MODULES(LAPACK lapack) + + SET(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARY} ${AMD_LIBRARY} ${CAMD_LIBRARY} ${COLAMD_LIBRARY} ${CCOLAMD_LIBRARY} ${LAPACK_LIBRARIES}) + +# MacPorts build of the SparseSuite requires linking against extra libraries + +ELSEIF(APPLE) + + FIND_LIBRARY(COLAMD_LIBRARY NAMES colamd + PATHS + /usr/lib + /usr/local/lib + /opt/local/lib + /sw/lib + ) + + FIND_LIBRARY(CCOLAMD_LIBRARY NAMES ccolamd + PATHS + /usr/lib + /usr/local/lib + /opt/local/lib + /sw/lib + ) + + FIND_LIBRARY(METIS_LIBRARY NAMES metis + PATHS + /usr/lib + /usr/local/lib + /opt/local/lib + /sw/lib + ) + + SET(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARY} ${AMD_LIBRARY} ${CAMD_LIBRARY} ${COLAMD_LIBRARY} ${CCOLAMD_LIBRARY} ${METIS_LIBRARY} "-framework Accelerate") +ELSE(APPLE) + SET(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARY} ${AMD_LIBRARY}) +ENDIF(CYGWIN) + +IF(CHOLMOD_INCLUDE_DIR AND CHOLMOD_LIBRARIES) + SET(CHOLMOD_FOUND TRUE) +ELSE(CHOLMOD_INCLUDE_DIR AND CHOLMOD_LIBRARIES) + SET(CHOLMOD_FOUND FALSE) +ENDIF(CHOLMOD_INCLUDE_DIR AND CHOLMOD_LIBRARIES) + +# Look for csparse; note the difference in the directory specifications! +FIND_PATH(CSPARSE_INCLUDE_DIR NAMES cs.h + PATHS + /usr/include/suitesparse + /usr/include + /opt/local/include + /usr/local/include + /sw/include + /usr/include/ufsparse + /opt/local/include/ufsparse + /usr/local/include/ufsparse + /sw/include/ufsparse + ) + +FIND_LIBRARY(CSPARSE_LIBRARY NAMES cxsparse + PATHS + /usr/lib + /usr/local/lib + /opt/local/lib + /sw/lib + ) + +IF(CSPARSE_INCLUDE_DIR AND CSPARSE_LIBRARY) + SET(CSPARSE_FOUND TRUE) +ELSE(CSPARSE_INCLUDE_DIR AND CSPARSE_LIBRARY) + SET(CSPARSE_FOUND FALSE) +ENDIF(CSPARSE_INCLUDE_DIR AND CSPARSE_LIBRARY) diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox/CMake/FindTBB.cmake b/Localizations/Packages/slam_toolbox/slam_toolbox/CMake/FindTBB.cmake new file mode 100644 index 0000000..6fc1ab3 --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox/CMake/FindTBB.cmake @@ -0,0 +1,421 @@ +# - Find ThreadingBuildingBlocks include dirs and libraries +# Use this module by invoking find_package with the form: +# find_package(TBB +# [REQUIRED] # Fail with error if TBB is not found +# ) # +# Once done, this will define +# +# TBB_FOUND - system has TBB +# TBB_INCLUDE_DIRS - the TBB include directories +# TBB_LIBRARIES - TBB libraries to be lined, doesn't include malloc or +# malloc proxy +# TBB::tbb - imported target for the TBB library +# +# TBB_VERSION_MAJOR - Major Product Version Number +# TBB_VERSION_MINOR - Minor Product Version Number +# TBB_INTERFACE_VERSION - Engineering Focused Version Number +# TBB_COMPATIBLE_INTERFACE_VERSION - The oldest major interface version +# still supported. This uses the engineering +# focused interface version numbers. +# +# TBB_MALLOC_FOUND - system has TBB malloc library +# TBB_MALLOC_INCLUDE_DIRS - the TBB malloc include directories +# TBB_MALLOC_LIBRARIES - The TBB malloc libraries to be lined +# TBB::malloc - imported target for the TBB malloc library +# +# TBB_MALLOC_PROXY_FOUND - system has TBB malloc proxy library +# TBB_MALLOC_PROXY_INCLUDE_DIRS = the TBB malloc proxy include directories +# TBB_MALLOC_PROXY_LIBRARIES - The TBB malloc proxy libraries to be lined +# TBB::malloc_proxy - imported target for the TBB malloc proxy library +# +# +# This module reads hints about search locations from variables: +# ENV TBB_ARCH_PLATFORM - for eg. set it to "mic" for Xeon Phi builds +# ENV TBB_ROOT or just TBB_ROOT - root directory of tbb installation +# ENV TBB_BUILD_PREFIX - specifies the build prefix for user built tbb +# libraries. Should be specified with ENV TBB_ROOT +# and optionally... +# ENV TBB_BUILD_DIR - if build directory is different than ${TBB_ROOT}/build +# +# +# Modified by Robert Maynard from the original OGRE source +# +#------------------------------------------------------------------- +# This file is part of the CMake build system for OGRE +# (Object-oriented Graphics Rendering Engine) +# For the latest info, see http://www.ogre3d.org/ +# +# The contents of this file are placed in the public domain. Feel +# free to make use of it in any way you like. +#------------------------------------------------------------------- +# +#============================================================================= +# Copyright 2010-2012 Kitware, Inc. +# Copyright 2012 Rolf Eike Beer +# +# Distributed under the OSI-approved BSD License (the "License"); +# see accompanying file Copyright.txt for details. +# +# This software is distributed WITHOUT ANY WARRANTY; without even the +# implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. +# See the License for more information. +#============================================================================= +# (To distribute this file outside of CMake, substitute the full +# License text for the above reference.) + + +#============================================================================= +# FindTBB helper functions and macros +# + +#==================================================== +# Fix the library path in case it is a linker script +#==================================================== +function(tbb_extract_real_library library real_library) + if(NOT UNIX OR NOT EXISTS ${library}) + set(${real_library} "${library}" PARENT_SCOPE) + return() + endif() + + #Read in the first 4 bytes and see if they are the ELF magic number + set(_elf_magic "7f454c46") + file(READ ${library} _hex_data OFFSET 0 LIMIT 4 HEX) + if(_hex_data STREQUAL _elf_magic) + #we have opened a elf binary so this is what + #we should link to + set(${real_library} "${library}" PARENT_SCOPE) + return() + endif() + + file(READ ${library} _data OFFSET 0 LIMIT 1024) + if("${_data}" MATCHES "INPUT \\(([^(]+)\\)") + #extract out the .so name from REGEX MATCH command + set(_proper_so_name "${CMAKE_MATCH_1}") + + #construct path to the real .so which is presumed to be in the same directory + #as the input file + get_filename_component(_so_dir "${library}" DIRECTORY) + set(${real_library} "${_so_dir}/${_proper_so_name}" PARENT_SCOPE) + else() + #unable to determine what this library is so just hope everything works + #and pass it unmodified. + set(${real_library} "${library}" PARENT_SCOPE) + endif() +endfunction() + +#=============================================== +# Do the final processing for the package find. +#=============================================== +macro(findpkg_finish PREFIX TARGET_NAME) + # skip if already processed during this run + if (NOT ${PREFIX}_FOUND) + if (${PREFIX}_INCLUDE_DIR AND ${PREFIX}_LIBRARY) + set(${PREFIX}_FOUND TRUE) + set (${PREFIX}_INCLUDE_DIRS ${${PREFIX}_INCLUDE_DIR}) + set (${PREFIX}_LIBRARIES ${${PREFIX}_LIBRARY}) + else () + if (${PREFIX}_FIND_REQUIRED AND NOT ${PREFIX}_FIND_QUIETLY) + message(FATAL_ERROR "Required library ${PREFIX} not found.") + endif () + endif () + + if (NOT TARGET "TBB::${TARGET_NAME}") + if (${PREFIX}_LIBRARY_RELEASE) + tbb_extract_real_library(${${PREFIX}_LIBRARY_RELEASE} real_release) + endif () + if (${PREFIX}_LIBRARY_DEBUG) + tbb_extract_real_library(${${PREFIX}_LIBRARY_DEBUG} real_debug) + endif () + add_library(TBB::${TARGET_NAME} UNKNOWN IMPORTED) + set_target_properties(TBB::${TARGET_NAME} PROPERTIES + INTERFACE_INCLUDE_DIRECTORIES "${${PREFIX}_INCLUDE_DIR}") + if (${PREFIX}_LIBRARY_DEBUG AND ${PREFIX}_LIBRARY_RELEASE) + set_target_properties(TBB::${TARGET_NAME} PROPERTIES + IMPORTED_LOCATION "${real_release}" + IMPORTED_LOCATION_DEBUG "${real_debug}" + IMPORTED_LOCATION_RELEASE "${real_release}") + elseif (${PREFIX}_LIBRARY_RELEASE) + set_target_properties(TBB::${TARGET_NAME} PROPERTIES + IMPORTED_LOCATION "${real_release}") + elseif (${PREFIX}_LIBRARY_DEBUG) + set_target_properties(TBB::${TARGET_NAME} PROPERTIES + IMPORTED_LOCATION "${real_debug}") + endif () + endif () + + #mark the following variables as internal variables + mark_as_advanced(${PREFIX}_INCLUDE_DIR + ${PREFIX}_LIBRARY + ${PREFIX}_LIBRARY_DEBUG + ${PREFIX}_LIBRARY_RELEASE) + endif () +endmacro() + +#=============================================== +# Generate debug names from given release names +#=============================================== +macro(get_debug_names PREFIX) + foreach(i ${${PREFIX}}) + set(${PREFIX}_DEBUG ${${PREFIX}_DEBUG} ${i}d ${i}D ${i}_d ${i}_D ${i}_debug ${i}) + endforeach() +endmacro() + +#=============================================== +# See if we have env vars to help us find tbb +#=============================================== +macro(getenv_path VAR) + set(ENV_${VAR} $ENV{${VAR}}) + # replace won't work if var is blank + if (ENV_${VAR}) + string( REGEX REPLACE "\\\\" "/" ENV_${VAR} ${ENV_${VAR}} ) + endif () +endmacro() + +#=============================================== +# Couple a set of release AND debug libraries +#=============================================== +macro(make_library_set PREFIX) + if (${PREFIX}_RELEASE AND ${PREFIX}_DEBUG) + set(${PREFIX} optimized ${${PREFIX}_RELEASE} debug ${${PREFIX}_DEBUG}) + elseif (${PREFIX}_RELEASE) + set(${PREFIX} ${${PREFIX}_RELEASE}) + elseif (${PREFIX}_DEBUG) + set(${PREFIX} ${${PREFIX}_DEBUG}) + endif () +endmacro() + + +#============================================================================= +# Now to actually find TBB +# + +# Get path, convert backslashes as ${ENV_${var}} +getenv_path(TBB_ROOT) + +# initialize search paths +set(TBB_PREFIX_PATH ${TBB_ROOT} ${ENV_TBB_ROOT}) +set(TBB_INC_SEARCH_PATH "") +set(TBB_LIB_SEARCH_PATH "") + + +# If user built from sources +set(TBB_BUILD_PREFIX $ENV{TBB_BUILD_PREFIX}) +if (TBB_BUILD_PREFIX AND ENV_TBB_ROOT) + getenv_path(TBB_BUILD_DIR) + if (NOT ENV_TBB_BUILD_DIR) + set(ENV_TBB_BUILD_DIR ${ENV_TBB_ROOT}/build) + endif () + + # include directory under ${ENV_TBB_ROOT}/include + list(APPEND TBB_LIB_SEARCH_PATH + ${ENV_TBB_BUILD_DIR}/${TBB_BUILD_PREFIX}_release + ${ENV_TBB_BUILD_DIR}/${TBB_BUILD_PREFIX}_debug) +endif () + + +# For Windows, let's assume that the user might be using the precompiled +# TBB packages from the main website. These use a rather awkward directory +# structure (at least for automatically finding the right files) depending +# on platform and compiler, but we'll do our best to accommodate it. +# Not adding the same effort for the precompiled linux builds, though. Those +# have different versions for CC compiler versions and linux kernels which +# will never adequately match the user's setup, so there is no feasible way +# to detect the "best" version to use. The user will have to manually +# select the right files. (Chances are the distributions are shipping their +# custom version of tbb, anyway, so the problem is probably nonexistent.) +if (WIN32 AND MSVC) + set(COMPILER_PREFIX "vc7.1") + if (MSVC_VERSION EQUAL 1400) + set(COMPILER_PREFIX "vc8") + elseif(MSVC_VERSION EQUAL 1500) + set(COMPILER_PREFIX "vc9") + elseif(MSVC_VERSION EQUAL 1600) + set(COMPILER_PREFIX "vc10") + elseif(MSVC_VERSION EQUAL 1700) + set(COMPILER_PREFIX "vc11") + elseif(MSVC_VERSION EQUAL 1800) + set(COMPILER_PREFIX "vc12") + elseif(MSVC_VERSION EQUAL 1900) + set(COMPILER_PREFIX "vc14") + endif () + + # for each prefix path, add ia32/64\${COMPILER_PREFIX}\lib to the lib search path + foreach (dir IN LISTS TBB_PREFIX_PATH) + if (CMAKE_CL_64) + list(APPEND TBB_LIB_SEARCH_PATH ${dir}/ia64/${COMPILER_PREFIX}/lib) + list(APPEND TBB_LIB_SEARCH_PATH ${dir}/lib/ia64/${COMPILER_PREFIX}) + list(APPEND TBB_LIB_SEARCH_PATH ${dir}/intel64/${COMPILER_PREFIX}/lib) + list(APPEND TBB_LIB_SEARCH_PATH ${dir}/lib/intel64/${COMPILER_PREFIX}) + else () + list(APPEND TBB_LIB_SEARCH_PATH ${dir}/ia32/${COMPILER_PREFIX}/lib) + list(APPEND TBB_LIB_SEARCH_PATH ${dir}/lib/ia32/${COMPILER_PREFIX}) + endif () + endforeach () +endif () + +# For OS X binary distribution, choose libc++ based libraries for Mavericks (10.9) +# and above and AppleClang +if (CMAKE_SYSTEM_NAME STREQUAL "Darwin" AND + NOT CMAKE_SYSTEM_VERSION VERSION_LESS 13.0) + set (USE_LIBCXX OFF) + cmake_policy(GET CMP0025 POLICY_VAR) + + if (POLICY_VAR STREQUAL "NEW") + if (CMAKE_CXX_COMPILER_ID STREQUAL "AppleClang") + set (USE_LIBCXX ON) + endif () + else () + if (CMAKE_CXX_COMPILER_ID STREQUAL "Clang") + set (USE_LIBCXX ON) + endif () + endif () + + if (USE_LIBCXX) + foreach (dir IN LISTS TBB_PREFIX_PATH) + list (APPEND TBB_LIB_SEARCH_PATH ${dir}/lib/libc++ ${dir}/libc++/lib) + endforeach () + endif () +endif () + +# check compiler ABI +if (CMAKE_CXX_COMPILER_ID STREQUAL "GNU") + set(COMPILER_PREFIX) + if (NOT CMAKE_CXX_COMPILER_VERSION VERSION_LESS 4.7) + list(APPEND COMPILER_PREFIX "gcc4.7") + endif() + if (NOT CMAKE_CXX_COMPILER_VERSION VERSION_LESS 4.4) + list(APPEND COMPILER_PREFIX "gcc4.4") + endif() + list(APPEND COMPILER_PREFIX "gcc4.1") +elseif(CMAKE_CXX_COMPILER_ID MATCHES "Clang") + set(COMPILER_PREFIX) + if (NOT CMAKE_CXX_COMPILER_VERSION VERSION_LESS 3.6) + list(APPEND COMPILER_PREFIX "gcc4.7") + endif() + list(APPEND COMPILER_PREFIX "gcc4.4") +else() # Assume compatibility with 4.4 for other compilers + list(APPEND COMPILER_PREFIX "gcc4.4") +endif () + +# if platform architecture is explicitly specified +set(TBB_ARCH_PLATFORM $ENV{TBB_ARCH_PLATFORM}) +if (TBB_ARCH_PLATFORM) + foreach (dir IN LISTS TBB_PREFIX_PATH) + list(APPEND TBB_LIB_SEARCH_PATH ${dir}/${TBB_ARCH_PLATFORM}/lib) + list(APPEND TBB_LIB_SEARCH_PATH ${dir}/lib/${TBB_ARCH_PLATFORM}) + endforeach () +endif () + +foreach (dir IN LISTS TBB_PREFIX_PATH) + foreach (prefix IN LISTS COMPILER_PREFIX) + if (CMAKE_SIZEOF_VOID_P EQUAL 8) + list(APPEND TBB_LIB_SEARCH_PATH ${dir}/lib/intel64) + list(APPEND TBB_LIB_SEARCH_PATH ${dir}/lib/intel64/${prefix}) + list(APPEND TBB_LIB_SEARCH_PATH ${dir}/intel64/lib) + list(APPEND TBB_LIB_SEARCH_PATH ${dir}/intel64/${prefix}/lib) + else () + list(APPEND TBB_LIB_SEARCH_PATH ${dir}/lib/ia32) + list(APPEND TBB_LIB_SEARCH_PATH ${dir}/lib/ia32/${prefix}) + list(APPEND TBB_LIB_SEARCH_PATH ${dir}/ia32/lib) + list(APPEND TBB_LIB_SEARCH_PATH ${dir}/ia32/${prefix}/lib) + endif () + endforeach() +endforeach () + +# add general search paths +foreach (dir IN LISTS TBB_PREFIX_PATH) + list(APPEND TBB_LIB_SEARCH_PATH ${dir}/lib ${dir}/Lib ${dir}/lib/tbb + ${dir}/Libs) + list(APPEND TBB_INC_SEARCH_PATH ${dir}/include ${dir}/Include + ${dir}/include/tbb) +endforeach () + +set(TBB_LIBRARY_NAMES tbb) +get_debug_names(TBB_LIBRARY_NAMES) + + +find_path(TBB_INCLUDE_DIR + NAMES tbb/tbb.h + PATHS ${TBB_INC_SEARCH_PATH}) + +find_library(TBB_LIBRARY_RELEASE + NAMES ${TBB_LIBRARY_NAMES} + PATHS ${TBB_LIB_SEARCH_PATH}) +find_library(TBB_LIBRARY_DEBUG + NAMES ${TBB_LIBRARY_NAMES_DEBUG} + PATHS ${TBB_LIB_SEARCH_PATH}) +make_library_set(TBB_LIBRARY) + +findpkg_finish(TBB tbb) + +#if we haven't found TBB no point on going any further +if (NOT TBB_FOUND) + return() +endif () + +#============================================================================= +# Look for TBB's malloc package +set(TBB_MALLOC_LIBRARY_NAMES tbbmalloc) +get_debug_names(TBB_MALLOC_LIBRARY_NAMES) + +find_path(TBB_MALLOC_INCLUDE_DIR + NAMES tbb/tbb.h + PATHS ${TBB_INC_SEARCH_PATH}) + +find_library(TBB_MALLOC_LIBRARY_RELEASE + NAMES ${TBB_MALLOC_LIBRARY_NAMES} + PATHS ${TBB_LIB_SEARCH_PATH}) +find_library(TBB_MALLOC_LIBRARY_DEBUG + NAMES ${TBB_MALLOC_LIBRARY_NAMES_DEBUG} + PATHS ${TBB_LIB_SEARCH_PATH}) +make_library_set(TBB_MALLOC_LIBRARY) + +findpkg_finish(TBB_MALLOC tbbmalloc) + +#============================================================================= +# Look for TBB's malloc proxy package +set(TBB_MALLOC_PROXY_LIBRARY_NAMES tbbmalloc_proxy) +get_debug_names(TBB_MALLOC_PROXY_LIBRARY_NAMES) + +find_path(TBB_MALLOC_PROXY_INCLUDE_DIR + NAMES tbb/tbbmalloc_proxy.h + PATHS ${TBB_INC_SEARCH_PATH}) + +find_library(TBB_MALLOC_PROXY_LIBRARY_RELEASE + NAMES ${TBB_MALLOC_PROXY_LIBRARY_NAMES} + PATHS ${TBB_LIB_SEARCH_PATH}) +find_library(TBB_MALLOC_PROXY_LIBRARY_DEBUG + NAMES ${TBB_MALLOC_PROXY_LIBRARY_NAMES_DEBUG} + PATHS ${TBB_LIB_SEARCH_PATH}) +make_library_set(TBB_MALLOC_PROXY_LIBRARY) + +findpkg_finish(TBB_MALLOC_PROXY tbbmalloc_proxy) + + +#============================================================================= +#parse all the version numbers from tbb +if(NOT TBB_VERSION) + + #only read the start of the file + file(STRINGS + "${TBB_INCLUDE_DIR}/tbb/tbb_stddef.h" + TBB_VERSION_CONTENTS + REGEX "VERSION") + + string(REGEX REPLACE + ".*#define TBB_VERSION_MAJOR ([0-9]+).*" "\\1" + TBB_VERSION_MAJOR "${TBB_VERSION_CONTENTS}") + + string(REGEX REPLACE + ".*#define TBB_VERSION_MINOR ([0-9]+).*" "\\1" + TBB_VERSION_MINOR "${TBB_VERSION_CONTENTS}") + + string(REGEX REPLACE + ".*#define TBB_INTERFACE_VERSION ([0-9]+).*" "\\1" + TBB_INTERFACE_VERSION "${TBB_VERSION_CONTENTS}") + + string(REGEX REPLACE + ".*#define TBB_COMPATIBLE_INTERFACE_VERSION ([0-9]+).*" "\\1" + TBB_COMPATIBLE_INTERFACE_VERSION "${TBB_VERSION_CONTENTS}") + +endif() \ No newline at end of file diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox/CMakeLists.txt b/Localizations/Packages/slam_toolbox/slam_toolbox/CMakeLists.txt new file mode 100644 index 0000000..930f717 --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox/CMakeLists.txt @@ -0,0 +1,173 @@ +cmake_minimum_required(VERSION 3.0.2) +project(slam_toolbox) + +add_compile_options(-std=c++17) + +set(CMAKE_BUILD_TYPE Release) #None, Debug, Release, RelWithDebInfo, MinSizeRel +set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_CURRENT_LIST_DIR}/CMake/") +list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_LIST_DIR}/lib/karto_sdk/cmake) +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fpermissive -std=c++17") + +#karto_sdk lib +set(BUILD_SHARED_LIBS ON) +add_subdirectory(lib/karto_sdk) + +find_package(catkin REQUIRED COMPONENTS + cmake_modules + message_filters + nav_msgs + karto_sdk + rosconsole + roscpp + sensor_msgs + sparse_bundle_adjustment + tf2 + tf2_ros + tf + visualization_msgs + pluginlib + tf2_geometry_msgs + interactive_markers + map_server + slam_toolbox_msgs +) + +find_package(PkgConfig REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(CSparse REQUIRED) +find_package(G2O REQUIRED) +find_package(Cholmod REQUIRED) +find_package(LAPACK REQUIRED) +find_package(Ceres REQUIRED COMPONENTS SuiteSparse) +find_package(rostest REQUIRED) + +set(CMAKE_INCLUDE_CURRENT_DIR ON) +add_definitions(-DQT_NO_KEYWORDS) +find_package(Boost REQUIRED system serialization filesystem thread) + +add_definitions(${EIGEN3_DEFINITIONS}) + +catkin_package( + INCLUDE_DIRS + include + ${Boost_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIRS} + ${TBB_INCLUDE_DIRS} + LIBRARIES + ceres_solver_plugin + slam_toolbox_rviz + async_slam_toolbox + sync_slam_toolbox + localization_slam_toolbox + lifelong_slam_toolbox + CATKIN_DEPENDS + message_filters + nav_msgs + rosconsole + roscpp + sparse_bundle_adjustment + sensor_msgs + tf2 + tf + tf2_ros + visualization_msgs + pluginlib + message_runtime + tf2_geometry_msgs + interactive_markers + slam_toolbox_msgs + karto_sdk + DEPENDS + EIGEN3 + Boost +) + +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIRS} + ${CHOLMOD_INCLUDE_DIR} + ${Boost_INCLUDE_DIRS} + ${TBB_INCLUDE_DIRS} +) + +#### Ceres Plugin +add_library(ceres_solver_plugin solvers/ceres_solver.cpp) +target_link_libraries(ceres_solver_plugin ${catkin_LIBRARIES} + ${CERES_LIBRARIES} + ${Boost_LIBRARIES} + ${TBB_LIBRARIES} +) + +#### Tool lib for mapping +add_library(toolbox_common src/slam_toolbox_common.cpp src/map_saver.cpp src/loop_closure_assistant.cpp src/laser_utils.cpp src/slam_mapper.cpp) +target_link_libraries(toolbox_common kartoSlamToolbox ${catkin_LIBRARIES} ${Boost_LIBRARIES}) + +#### Mapping executibles +add_library(async_slam_toolbox src/slam_toolbox_async.cpp) +add_dependencies(async_slam_toolbox ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(async_slam_toolbox toolbox_common kartoSlamToolbox ${catkin_LIBRARIES} ${Boost_LIBRARIES}) +add_executable(async_slam_toolbox_node src/slam_toolbox_async_node.cpp ) +target_link_libraries(async_slam_toolbox_node async_slam_toolbox) + +add_library(sync_slam_toolbox src/slam_toolbox_sync.cpp) +add_dependencies(sync_slam_toolbox ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(sync_slam_toolbox toolbox_common kartoSlamToolbox ${catkin_LIBRARIES} ${Boost_LIBRARIES}) +add_executable(sync_slam_toolbox_node src/slam_toolbox_sync_node.cpp ) +target_link_libraries(sync_slam_toolbox_node sync_slam_toolbox) + +add_library(localization_slam_toolbox src/slam_toolbox_localization.cpp) +add_dependencies(localization_slam_toolbox ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(localization_slam_toolbox toolbox_common kartoSlamToolbox ${catkin_LIBRARIES} ${Boost_LIBRARIES}) +add_executable(localization_slam_toolbox_node src/slam_toolbox_localization_node.cpp ) +target_link_libraries(localization_slam_toolbox_node localization_slam_toolbox) + +add_library(lifelong_slam_toolbox src/experimental/slam_toolbox_lifelong.cpp) +add_dependencies(lifelong_slam_toolbox ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(lifelong_slam_toolbox toolbox_common kartoSlamToolbox ${catkin_LIBRARIES} ${Boost_LIBRARIES}) +add_executable(lifelong_slam_toolbox_node src/experimental/slam_toolbox_lifelong_node.cpp ) +target_link_libraries(lifelong_slam_toolbox_node lifelong_slam_toolbox) + +#### Merging maps tool +add_executable(merge_maps_kinematic src/merge_maps_kinematic.cpp) +target_link_libraries(merge_maps_kinematic toolbox_common) + +#### testing +#if(CATKIN_ENABLE_TESTING) +# include_directories(test) +# catkin_add_gtest(lifelong_metrics_test test/lifelong_metrics_test.cpp) +# target_link_libraries(lifelong_metrics_test lifelong_slam_toolbox) +#endif() + +#### Install +install(TARGETS async_slam_toolbox_node sync_slam_toolbox_node localization_slam_toolbox_node lifelong_slam_toolbox_node + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(TARGETS toolbox_common + async_slam_toolbox + sync_slam_toolbox + localization_slam_toolbox + lifelong_slam_toolbox + ceres_solver_plugin + merge_maps_kinematic + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +) + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +) + +install(DIRECTORY launch/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +install(DIRECTORY config + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +install(FILES solver_plugins.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox/config/mapper_params_lifelong.yaml b/Localizations/Packages/slam_toolbox/slam_toolbox/config/mapper_params_lifelong.yaml new file mode 100644 index 0000000..2998688 --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox/config/mapper_params_lifelong.yaml @@ -0,0 +1,79 @@ +# Plugin params +solver_plugin: solver_plugins::CeresSolver +ceres_linear_solver: SPARSE_NORMAL_CHOLESKY +ceres_preconditioner: SCHUR_JACOBI +ceres_trust_strategy: LEVENBERG_MARQUARDT +ceres_dogleg_type: TRADITIONAL_DOGLEG +ceres_loss_function: None + +# ROS Parameters +odom_frame: odom +map_frame: map +base_frame: base_footprint +scan_topic: /scan +mode: mapping + +# lifelong params +lifelong_search_use_tree: false +lifelong_minimum_score: 0.1 +lifelong_iou_match: 0.85 +lifelong_node_removal_score: 0.04 +lifelong_overlap_score_scale: 0.06 +lifelong_constraint_multiplier: 0.08 +lifelong_nearby_penalty: 0.001 +lifelong_candidates_scale: 0.03 + +# if you'd like to immediately start continuing a map at a given pose +# or at the dock, but they are mutually exclusive, if pose is given +# will use pose +#map_file_name: test_steve +#map_start_pose: [0.0, 0.0, 0.0] +#map_start_at_dock: true + +debug_logging: false +throttle_scans: 1 +transform_publish_period: 0.02 #if 0 never publishes odometry +map_update_interval: 5.0 +resolution: 0.05 +max_laser_range: 20.0 #for rastering images +minimum_time_interval: 0.5 +transform_timeout: 0.2 +tf_buffer_duration: 10. +stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps + +# General Parameters +use_scan_matching: true +use_scan_barycenter: true +minimum_travel_distance: 0.5 +minimum_travel_heading: 0.5 +scan_buffer_size: 10 +scan_buffer_maximum_scan_distance: 10 +link_match_minimum_response_fine: 0.1 +link_scan_maximum_distance: 1.5 +loop_search_maximum_distance: 3.0 +do_loop_closing: true +loop_match_minimum_chain_size: 10 +loop_match_maximum_variance_coarse: 3.0 +loop_match_minimum_response_coarse: 0.35 +loop_match_minimum_response_fine: 0.45 + +# Correlation Parameters - Correlation Parameters +correlation_search_space_dimension: 0.5 +correlation_search_space_resolution: 0.01 +correlation_search_space_smear_deviation: 0.1 + +# Correlation Parameters - Loop Closure Parameters +loop_search_space_dimension: 8.0 +loop_search_space_resolution: 0.05 +loop_search_space_smear_deviation: 0.03 + +# Scan Matcher Parameters +distance_variance_penalty: 0.5 +angle_variance_penalty: 1.0 + +fine_search_angle_offset: 0.00349 +coarse_search_angle_offset: 0.349 +coarse_angle_resolution: 0.0349 +minimum_angle_penalty: 0.9 +minimum_distance_penalty: 0.5 +use_response_expansion: true diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox/config/mapper_params_localization.yaml b/Localizations/Packages/slam_toolbox/slam_toolbox/config/mapper_params_localization.yaml new file mode 100644 index 0000000..a8385a5 --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox/config/mapper_params_localization.yaml @@ -0,0 +1,66 @@ +# Plugin params +solver_plugin: solver_plugins::CeresSolver +ceres_linear_solver: SPARSE_NORMAL_CHOLESKY +ceres_preconditioner: SCHUR_JACOBI +ceres_trust_strategy: LEVENBERG_MARQUARDT +ceres_dogleg_type: TRADITIONAL_DOGLEG +ceres_loss_function: None + +# ROS Parameters +odom_frame: odom +map_frame: map +base_frame: base_footprint +scan_topic: /scan +mode: localization #mapping + +# if you'd like to start localizing on bringup in a map and pose +map_file_name: testmap +#map_start_pose: [5.0, 1.0, 0.0] + +debug_logging: false +throttle_scans: 1 +transform_publish_period: 0.02 #if 0 never publishes odometry +map_update_interval: 5.0 +resolution: 0.05 +max_laser_range: 20.0 #for rastering images +minimum_time_interval: 0.5 +transform_timeout: 0.2 +tf_buffer_duration: 30. +stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps + +# General Parameters +use_scan_matching: true +use_scan_barycenter: true +minimum_travel_distance: 0.5 +minimum_travel_heading: 0.5 +scan_buffer_size: 3 +scan_buffer_maximum_scan_distance: 10 +link_match_minimum_response_fine: 0.1 +link_scan_maximum_distance: 1.5 +do_loop_closing: true +loop_match_minimum_chain_size: 3 +loop_match_maximum_variance_coarse: 3.0 +loop_match_minimum_response_coarse: 0.35 +loop_match_minimum_response_fine: 0.45 + +# Correlation Parameters - Correlation Parameters +correlation_search_space_dimension: 0.5 +correlation_search_space_resolution: 0.01 +correlation_search_space_smear_deviation: 0.1 + +# Correlation Parameters - Loop Closure Parameters +loop_search_space_dimension: 8.0 +loop_search_space_resolution: 0.05 +loop_search_space_smear_deviation: 0.03 +loop_search_maximum_distance: 5.0 + +# Scan Matcher Parameters +distance_variance_penalty: 0.5 +angle_variance_penalty: 1.0 + +fine_search_angle_offset: 0.00349 +coarse_search_angle_offset: 0.349 +coarse_angle_resolution: 0.0349 +minimum_angle_penalty: 0.9 +minimum_distance_penalty: 0.5 +use_response_expansion: true diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox/config/mapper_params_offline.yaml b/Localizations/Packages/slam_toolbox/slam_toolbox/config/mapper_params_offline.yaml new file mode 100644 index 0000000..30a92ed --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox/config/mapper_params_offline.yaml @@ -0,0 +1,62 @@ +# Plugin params +solver_plugin: solver_plugins::CeresSolver +ceres_linear_solver: SPARSE_NORMAL_CHOLESKY +ceres_preconditioner: SCHUR_JACOBI +ceres_trust_strategy: LEVENBERG_MARQUARDT +ceres_dogleg_type: TRADITIONAL_DOGLEG +ceres_loss_function: None + +# ROS Parameters +odom_frame: odom +map_frame: map +base_frame: base_link +scan_topic: /scan +mode: mapping #localization +debug_logging: false +throttle_scans: 1 +transform_publish_period: 0.02 #if 0 never publishes odometry +map_update_interval: 10.0 +resolution: 0.05 +max_laser_range: 20.0 #for rastering images +minimum_time_interval: 0.5 +transform_timeout: 0.2 +tf_buffer_duration: 14400. +stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps +enable_interactive_mode: true + +# General Parameters +use_scan_matching: true +use_scan_barycenter: true +minimum_travel_distance: 0.5 +minimum_travel_heading: 0.5 +scan_buffer_size: 10 +scan_buffer_maximum_scan_distance: 10 +link_match_minimum_response_fine: 0.1 +link_scan_maximum_distance: 1.5 +loop_search_maximum_distance: 3.0 +do_loop_closing: true +loop_match_minimum_chain_size: 10 +loop_match_maximum_variance_coarse: 3.0 +loop_match_minimum_response_coarse: 0.35 +loop_match_minimum_response_fine: 0.45 + +# Correlation Parameters - Correlation Parameters +correlation_search_space_dimension: 0.5 +correlation_search_space_resolution: 0.01 +correlation_search_space_smear_deviation: 0.1 + +# Correlation Parameters - Loop Closure Parameters +loop_search_space_dimension: 8.0 +loop_search_space_resolution: 0.05 +loop_search_space_smear_deviation: 0.03 + +# Scan Matcher Parameters +distance_variance_penalty: 0.5 +angle_variance_penalty: 1.0 + +fine_search_angle_offset: 0.00349 +coarse_search_angle_offset: 0.349 +coarse_angle_resolution: 0.0349 +minimum_angle_penalty: 0.9 +minimum_distance_penalty: 0.5 +use_response_expansion: true diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox/config/mapper_params_online_async.yaml b/Localizations/Packages/slam_toolbox/slam_toolbox/config/mapper_params_online_async.yaml new file mode 100644 index 0000000..395ac0f --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox/config/mapper_params_online_async.yaml @@ -0,0 +1,70 @@ +# Plugin params +solver_plugin: solver_plugins::CeresSolver +ceres_linear_solver: SPARSE_NORMAL_CHOLESKY +ceres_preconditioner: SCHUR_JACOBI +ceres_trust_strategy: LEVENBERG_MARQUARDT +ceres_dogleg_type: TRADITIONAL_DOGLEG +ceres_loss_function: None + +# ROS Parameters +odom_frame: odom +map_frame: map +base_frame: base_link +scan_topic: /f_scan +mode: mapping #localization + +# if you'd like to immediately start continuing a map at a given pose +# or at the dock, but they are mutually exclusive, if pose is given +# will use pose +#map_file_name: test_steve +# map_start_pose: [0.0, 0.0, 0.0] +#map_start_at_dock: true + +debug_logging: false +throttle_scans: 1 +transform_publish_period: 0.02 #if 0 never publishes odometry +map_update_interval: 0.5 +resolution: 0.05 +max_laser_range: 25.0 #for rastering images +minimum_time_interval: 0.5 +transform_timeout: 0.2 +tf_buffer_duration: 30. +stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps +enable_interactive_mode: true + +# General Parameters +use_scan_matching: true +use_scan_barycenter: true +minimum_travel_distance: 0.5 +minimum_travel_heading: 0.5 +scan_buffer_size: 10 +scan_buffer_maximum_scan_distance: 10 +link_match_minimum_response_fine: 0.1 +link_scan_maximum_distance: 1.5 +loop_search_maximum_distance: 3.0 +do_loop_closing: true +loop_match_minimum_chain_size: 10 +loop_match_maximum_variance_coarse: 3.0 +loop_match_minimum_response_coarse: 0.35 +loop_match_minimum_response_fine: 0.45 + +# Correlation Parameters - Correlation Parameters +correlation_search_space_dimension: 0.5 +correlation_search_space_resolution: 0.01 +correlation_search_space_smear_deviation: 0.1 + +# Correlation Parameters - Loop Closure Parameters +loop_search_space_dimension: 8.0 +loop_search_space_resolution: 0.05 +loop_search_space_smear_deviation: 0.03 + +# Scan Matcher Parameters +distance_variance_penalty: 0.5 +angle_variance_penalty: 1.0 + +fine_search_angle_offset: 0.00349 +coarse_search_angle_offset: 0.349 +coarse_angle_resolution: 0.0349 +minimum_angle_penalty: 0.9 +minimum_distance_penalty: 0.5 +use_response_expansion: true diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox/config/mapper_params_online_sync.yaml b/Localizations/Packages/slam_toolbox/slam_toolbox/config/mapper_params_online_sync.yaml new file mode 100644 index 0000000..ae45785 --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox/config/mapper_params_online_sync.yaml @@ -0,0 +1,70 @@ +# Plugin params +solver_plugin: solver_plugins::CeresSolver +ceres_linear_solver: SPARSE_NORMAL_CHOLESKY +ceres_preconditioner: SCHUR_JACOBI +ceres_trust_strategy: LEVENBERG_MARQUARDT +ceres_dogleg_type: TRADITIONAL_DOGLEG +ceres_loss_function: None + +# ROS Parameters +odom_frame: odom +map_frame: map +base_frame: base_footprint +scan_topic: /scan +mode: mapping #localization + +# if you'd like to immediately start continuing a map at a given pose +# or at the dock, but they are mutually exclusive, if pose is given +# will use pose +#map_file_name: test_steve +#map_start_pose: [0.0, 0.0, 0.0] +#map_start_at_dock: true + +debug_logging: false +throttle_scans: 1 +transform_publish_period: 0.02 #if 0 never publishes odometry +map_update_interval: 5.0 +resolution: 0.05 +max_laser_range: 20.0 #for rastering images +minimum_time_interval: 0.5 +transform_timeout: 0.2 +tf_buffer_duration: 30. +stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps +enable_interactive_mode: true + +# General Parameters +use_scan_matching: true +use_scan_barycenter: true +minimum_travel_distance: 0.5 +minimum_travel_heading: 0.5 +scan_buffer_size: 10 +scan_buffer_maximum_scan_distance: 10 +link_match_minimum_response_fine: 0.1 +link_scan_maximum_distance: 1.5 +loop_search_maximum_distance: 3.0 +do_loop_closing: true +loop_match_minimum_chain_size: 10 +loop_match_maximum_variance_coarse: 3.0 +loop_match_minimum_response_coarse: 0.35 +loop_match_minimum_response_fine: 0.45 + +# Correlation Parameters - Correlation Parameters +correlation_search_space_dimension: 0.5 +correlation_search_space_resolution: 0.01 +correlation_search_space_smear_deviation: 0.1 + +# Correlation Parameters - Loop Closure Parameters +loop_search_space_dimension: 8.0 +loop_search_space_resolution: 0.05 +loop_search_space_smear_deviation: 0.03 + +# Scan Matcher Parameters +distance_variance_penalty: 0.5 +angle_variance_penalty: 1.0 + +fine_search_angle_offset: 0.00349 +coarse_search_angle_offset: 0.349 +coarse_angle_resolution: 0.0349 +minimum_angle_penalty: 0.9 +minimum_distance_penalty: 0.5 +use_response_expansion: true diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox/config/slam_toolbox_default.rviz b/Localizations/Packages/slam_toolbox/slam_toolbox/config/slam_toolbox_default.rviz new file mode 100644 index 0000000..17fdd7a --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox/config/slam_toolbox_default.rviz @@ -0,0 +1,146 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /InteractiveMarkers1 + - /MarkerArray1 + Splitter Ratio: 0.5 + Tree Height: 1562 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.588679016 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" + - Class: slam_toolbox::SlamToolboxPlugin + Name: SlamToolboxPlugin +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 0; 0; 0 + Enabled: true + Line Style: + Line Width: 0.0299999993 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 100 + Reference Frame: + Value: true + - Alpha: 0.699999988 + Class: rviz/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: Map + Topic: /map + Unreliable: false + Use Timestamp: false + Value: true + - Class: rviz/InteractiveMarkers + Enable Transparency: true + Enabled: true + Name: InteractiveMarkers + Show Axes: true + Show Descriptions: true + Show Visual Aids: true + Update Topic: /slam_toolbox/update + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /slam_toolbox/karto_graph_visualization + Name: MarkerArray + Namespaces: + slam_toolbox: true + Queue Size: 100 + Value: true + Enabled: true + Global Options: + Background Color: 237; 237; 237 + Default Light: true + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Topic: /initialpose + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/XYOrbit + Distance: 220.73793 + Enable Stereo Rendering: + Stereo Eye Separation: 0.0599999987 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: -10.1544714 + Y: -12.9322882 + Z: 0.0100896517 + Focal Shape Fixed Size: false + Focal Shape Size: 0.0500000007 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.00999999978 + Pitch: 0.729796767 + Target Frame: map + Value: XYOrbit (rviz) + Yaw: 2.80506206 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1965 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 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 + Selection: + collapsed: false + SlamToolboxPlugin: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1863 + X: 1876 + Y: 98 diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp b/Localizations/Packages/slam_toolbox/slam_toolbox/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp new file mode 100644 index 0000000..5d4c458 --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp @@ -0,0 +1,70 @@ +/* + * slam_toolbox + * Copyright (c) 2019, Samsung Research America + * + * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE + * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY + * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS + * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED. + * + * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO + * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS + * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND + * CONDITIONS. + * + */ + +/* Author: Steven Macenski */ + +#ifndef SLAM_TOOLBOX_SLAM_TOOLBOX_LIFELONG_H_ +#define SLAM_TOOLBOX_SLAM_TOOLBOX_LIFELONG_H_ + +#include "slam_toolbox/slam_toolbox_common.hpp" + +namespace slam_toolbox +{ + +using namespace ::karto; + +class LifelongSlamToolbox : public SlamToolbox +{ +public: + LifelongSlamToolbox(ros::NodeHandle& nh); + ~LifelongSlamToolbox() {}; + + // computation metrics + double computeObjectiveScore(const double& intersect_over_union, const double& area_overlap, const double& reading_overlap, const int& num_constraints, const double& initial_score, const int& num_candidates) const; + static double computeIntersect(LocalizedRangeScan* s1, LocalizedRangeScan* s2); + static double computeIntersectOverUnion(LocalizedRangeScan* s1, LocalizedRangeScan* s2); + static double computeAreaOverlapRatio(LocalizedRangeScan* ref_scan, LocalizedRangeScan* candidate_scan); + static double computeReadingOverlapRatio(LocalizedRangeScan* ref_scan, LocalizedRangeScan* candidate_scan); + static void computeIntersectBounds(LocalizedRangeScan* s1, LocalizedRangeScan* s2, double& x_l, double& x_u, double& y_l, double& y_u); + +protected: + virtual void laserCallback( + const sensor_msgs::LaserScan::ConstPtr& scan) override final; + virtual bool deserializePoseGraphCallback( + slam_toolbox_msgs::DeserializePoseGraph::Request& req, + slam_toolbox_msgs::DeserializePoseGraph::Response& resp) override final; + + void evaluateNodeDepreciation(LocalizedRangeScan* range_scan); + void removeFromSlamGraph(Vertex* vertex); + double computeScore(LocalizedRangeScan* reference_scan, Vertex* candidate, const double& initial_score, const int& num_candidates); + ScoredVertices computeScores(Vertices& near_scans, LocalizedRangeScan* range_scan); + Vertices FindScansWithinRadius(LocalizedRangeScan* scan, const double& radius); + void updateScoresSlamGraph(const double& score, Vertex* vertex); + void checkIsNotNormalized(const double& value); + + bool use_tree_; + double iou_thresh_; + double removal_score_; + double overlap_scale_; + double constraint_scale_; + double candidates_scale_; + double iou_match_; + double nearby_penalty_; +}; + +} + +#endif //SLAM_TOOLBOX_SLAM_TOOLBOX_LIFELONG_H_ diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox/include/slam_toolbox/get_pose_helper.hpp b/Localizations/Packages/slam_toolbox/slam_toolbox/include/slam_toolbox/get_pose_helper.hpp new file mode 100644 index 0000000..5a67053 --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox/include/slam_toolbox/get_pose_helper.hpp @@ -0,0 +1,71 @@ +/* + * snap_utils + * Copyright (c) 2019, Samsung Research America + * + * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE + * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY + * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS + * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED. + * + * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO + * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS + * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND + * CONDITIONS. + * + */ + +/* Author: Steven Macenski */ + +#ifndef SLAM_TOOLBOX_POSE_UTILS_H_ +#define SLAM_TOOLBOX_POSE_UTILS_H_ + +#include +#include "slam_toolbox/toolbox_types.hpp" +#include "karto_sdk/Mapper.h" + +namespace pose_utils +{ + +// helper to get the robots position +class GetPoseHelper +{ +public: + GetPoseHelper(tf2_ros::Buffer* tf, + const std::string& base_frame, + const std::string& odom_frame) + : tf_(tf), base_frame_(base_frame), odom_frame_(odom_frame) + { + }; + + bool getOdomPose(karto::Pose2& karto_pose, const ros::Time& t) + { + geometry_msgs::TransformStamped base_ident, odom_pose; + base_ident.header.stamp = t; + base_ident.header.frame_id = base_frame_; + base_ident.transform.rotation.w = 1.0; + + try + { + odom_pose = tf_->transform(base_ident, odom_frame_); + } + catch(tf2::TransformException e) + { + ROS_WARN("Failed to compute odom pose, skipping scan (%s)", e.what()); + return false; + } + + const double yaw = tf2::getYaw(odom_pose.transform.rotation); + karto_pose = karto::Pose2(odom_pose.transform.translation.x, + odom_pose.transform.translation.y, yaw); + + return true; + }; + +private: + tf2_ros::Buffer* tf_; + std::string base_frame_, odom_frame_; +}; + +} // end namespace + +#endif //SLAM_TOOLBOX_POSE_UTILS_H_ diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox/include/slam_toolbox/laser_utils.hpp b/Localizations/Packages/slam_toolbox/slam_toolbox/include/slam_toolbox/laser_utils.hpp new file mode 100644 index 0000000..d492a59 --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox/include/slam_toolbox/laser_utils.hpp @@ -0,0 +1,106 @@ +/* + * toolbox_types + * Copyright (c) 2019, Samsung Research America + * + * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE + * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY + * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS + * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED. + * + * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO + * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS + * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND + * CONDITIONS. + * + */ + +/* Author: Steven Macenski */ + +#ifndef SLAM_TOOLBOX_LASER_UTILS_H_ +#define SLAM_TOOLBOX_LASER_UTILS_H_ + +#include + +#include "ros/ros.h" +#include "slam_toolbox/toolbox_types.hpp" +#include "tf2/utils.h" +#include "karto_sdk/Mapper.h" + +namespace laser_utils +{ + +// Convert a laser scan to a vector of readings +inline std::vector scanToReadings(const sensor_msgs::LaserScan& scan, const bool& inverted) +{ + std::vector readings; + + if (inverted) + { + for(std::vector::const_reverse_iterator it = scan.ranges.rbegin(); it != scan.ranges.rend(); ++it) + { + readings.push_back(*it); + } + } + else + { + for(std::vector::const_iterator it = scan.ranges.begin(); it != scan.ranges.end(); ++it) + { + readings.push_back(*it); + } + } + + return readings; +}; + +// Store laser scanner information +class LaserMetadata +{ +public: + LaserMetadata(); + ~LaserMetadata(); + LaserMetadata(karto::LaserRangeFinder* lsr, bool invert); + bool isInverted() const; + karto::LaserRangeFinder* getLaser(); + void invertScan(sensor_msgs::LaserScan& scan) const; + +private: + karto::LaserRangeFinder* laser; + bool inverted; +}; + +// Help take a scan from a laser and create a laser object +class LaserAssistant +{ +public: + LaserAssistant(ros::NodeHandle& nh, tf2_ros::Buffer* tf, const std::string& base_frame); + ~LaserAssistant(); + LaserMetadata toLaserMetadata(sensor_msgs::LaserScan scan); + +private: + karto::LaserRangeFinder* makeLaser(const double& mountingYaw); + bool isInverted(double& mountingYaw); + + ros::NodeHandle nh_; + tf2_ros::Buffer* tf_; + sensor_msgs::LaserScan scan_; + std::string frame_, base_frame_; + geometry_msgs::TransformStamped laser_pose_; +}; + +// Hold some scans and utilities around them +class ScanHolder +{ +public: + ScanHolder(std::map& lasers); + ~ScanHolder(); + sensor_msgs::LaserScan getCorrectedScan(const int& id); + void addScan(const sensor_msgs::LaserScan scan); + +private: + std::unique_ptr > current_scans_; + std::map& lasers_; +}; + +} // end namespace + +#endif //SLAM_TOOLBOX_LASER_UTILS_H_ diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox/include/slam_toolbox/loop_closure_assistant.hpp b/Localizations/Packages/slam_toolbox/slam_toolbox/include/slam_toolbox/loop_closure_assistant.hpp new file mode 100644 index 0000000..98611d8 --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox/include/slam_toolbox/loop_closure_assistant.hpp @@ -0,0 +1,79 @@ +/* + * loop_closure_assistant + * Copyright (c) 2019, Samsung Research America + * + * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE + * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY + * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS + * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED. + * + * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO + * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS + * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND + * CONDITIONS. + * + */ + +/* Author: Steven Macenski */ + +#ifndef SLAM_TOOLBOX_LOOP_CLOSURE_ASSISTANT_H_ +#define SLAM_TOOLBOX_LOOP_CLOSURE_ASSISTANT_H_ + +#include +#include +#include + +#include "ros/ros.h" +#include "interactive_markers/interactive_marker_server.h" +#include "interactive_markers/menu_handler.h" +#include "tf2_ros/transform_broadcaster.h" +#include "tf2/utils.h" + +#include "karto_sdk/Mapper.h" +#include "slam_toolbox/toolbox_types.hpp" +#include "slam_toolbox/laser_utils.hpp" +#include "slam_toolbox/visualization_utils.hpp" + +namespace loop_closure_assistant +{ + +using namespace ::toolbox_types; + +class LoopClosureAssistant +{ +public: + LoopClosureAssistant(ros::NodeHandle& node, karto::Mapper* mapper, laser_utils::ScanHolder* scan_holder, PausedState& state, ProcessType& processor_type); + + void clearMovedNodes(); + void processInteractiveFeedback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback); + void publishGraph(); + void setMapper(karto::Mapper * mapper); + +private: + bool manualLoopClosureCallback(slam_toolbox_msgs::LoopClosure::Request& req, slam_toolbox_msgs::LoopClosure::Response& resp); + bool clearChangesCallback(slam_toolbox_msgs::Clear::Request& req, slam_toolbox_msgs::Clear::Response& resp); + bool interactiveModeCallback(slam_toolbox_msgs::ToggleInteractive::Request &req, slam_toolbox_msgs::ToggleInteractive::Response &resp); + void moveNode(const int& id, const Eigen::Vector3d& pose); + void addMovedNodes(const int& id, Eigen::Vector3d vec); + + std::unique_ptr tfB_; + laser_utils::ScanHolder* scan_holder_; + ros::Publisher scan_publisher_, marker_publisher_; + ros::ServiceServer ssClear_manual_, ssLoopClosure_, ssInteractive_; + boost::mutex moved_nodes_mutex_; + std::map moved_nodes_; + karto::Mapper* mapper_; + karto::ScanSolver* solver_; + std::unique_ptr interactive_server_; + visualization_msgs::MarkerArray marker_array_; + boost::mutex interactive_mutex_; + bool interactive_mode_, enable_interactive_mode_; + ros::NodeHandle& nh_; + std::string map_frame_; + PausedState& state_; + ProcessType& processor_type_; +}; + +} // end namespace + +#endif //SLAM_TOOLBOX_LOOP_CLOSURE_ASSISTANT_H_ diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox/include/slam_toolbox/map_saver.hpp b/Localizations/Packages/slam_toolbox/slam_toolbox/include/slam_toolbox/map_saver.hpp new file mode 100644 index 0000000..154e5ec --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox/include/slam_toolbox/map_saver.hpp @@ -0,0 +1,50 @@ +/* + * map_saver + * Copyright (c) 2019, Samsung Research America + * + * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE + * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY + * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS + * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED. + * + * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO + * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS + * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND + * CONDITIONS. + * + */ + +/* Author: Steven Macenski */ + +#ifndef SLAM_TOOLBOX_MAP_SAVER_H_ +#define SLAM_TOOLBOX_MAP_SAVER_H_ + +#include +#include "ros/ros.h" +#include "slam_toolbox/toolbox_msgs.hpp" + +namespace map_saver +{ + +// a service to save a map with a given name as requested +class MapSaver +{ +public: + MapSaver(ros::NodeHandle& nh, const std::string& service_name); + +protected: + bool saveMapCallback(slam_toolbox_msgs::SaveMap::Request& req, + slam_toolbox_msgs::SaveMap::Response& resp); + void mapCallback(const nav_msgs::OccupancyGrid& msg); + +private: + ros::NodeHandle nh_; + ros::ServiceServer server_; + ros::Subscriber sub_; + std::string service_name_, map_name_; + bool received_map_; +}; + +} // end namespace + +#endif //SLAM_TOOLBOX_MAP_SAVER_H_ diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox/include/slam_toolbox/merge_maps_kinematic.hpp b/Localizations/Packages/slam_toolbox/slam_toolbox/include/slam_toolbox/merge_maps_kinematic.hpp new file mode 100644 index 0000000..0c17fe8 --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox/include/slam_toolbox/merge_maps_kinematic.hpp @@ -0,0 +1,96 @@ +/* + * Author + * Copyright (c) 2018, Simbe Robotics, Inc. + * + * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE + * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY + * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS + * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED. + * + * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO + * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS + * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND + * CONDITIONS. + * + */ + +/* Author: Steven Macenski */ + +#ifndef SLAM_TOOLBOX_MERGE_MAPS_KINEMATIC_H_ +#define SLAM_TOOLBOX_MERGE_MAPS_KINEMATIC_H_ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "ros/ros.h" +#include "interactive_markers/interactive_marker_server.h" +#include "interactive_markers/menu_handler.h" +#include "tf2_ros/transform_broadcaster.h" +#include "tf2_ros/transform_listener.h" +#include "tf2_ros/message_filter.h" +#include "tf2/LinearMath/Matrix3x3.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.h" +#include "tf2/utils.h" + +#include "karto_sdk/Mapper.h" +#include "slam_toolbox/toolbox_types.hpp" +#include "slam_toolbox/laser_utils.hpp" +#include "slam_toolbox/visualization_utils.hpp" + +using namespace toolbox_types; + +class MergeMapsKinematic +{ +typedef std::vector::iterator LocalizedRangeScansVecIt; +typedef karto::LocalizedRangeScanVector::iterator LocalizedRangeScansIt; + +public: + MergeMapsKinematic(); + ~MergeMapsKinematic(); + +private: + + // setup + void setup(); + + // callback + bool mergeMapCallback(slam_toolbox_msgs::MergeMaps::Request& req, slam_toolbox_msgs::MergeMaps::Response& resp); + bool addSubmapCallback(slam_toolbox_msgs::AddSubmap::Request& req, slam_toolbox_msgs::AddSubmap::Response& resp); + void processInteractiveFeedback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback); + void kartoToROSOccupancyGrid(const karto::LocalizedRangeScanVector& scans, nav_msgs::GetMap::Response& map); + void transformScan(LocalizedRangeScansIt iter, tf2::Transform& submap_correction); + + //apply transformation to correct pose + karto::Pose2 applyCorrection(const karto::Pose2& pose, const tf2::Transform& submap_correction); + karto::Vector2 applyCorrection(const karto::Vector2& pose, const tf2::Transform& submap_correction); + + // ROS-y-ness + ros::NodeHandle nh_; + std::vector sstS_, sstmS_; + ros::ServiceServer ssMap_, ssSubmap_; + + //karto bookkeeping + std::map lasers_; + std::vector > dataset_vec_; + + // TF + std::unique_ptr tfB_; + + // visualization + std::unique_ptr interactive_server_; + + // state + std::map submap_locations_; + std::vector scans_vec_; + std::map submap_marker_transform_; + double resolution_; + int num_submaps_; +}; + +#endif //SLAM_TOOLBOX_MERGE_MAPS_KINEMATIC_H_ diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox/include/slam_toolbox/serialization.hpp b/Localizations/Packages/slam_toolbox/slam_toolbox/include/slam_toolbox/serialization.hpp new file mode 100644 index 0000000..7a109f7 --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox/include/slam_toolbox/serialization.hpp @@ -0,0 +1,81 @@ +/* + * Author + * Copyright (c) 2018, Simbe Robotics, Inc. + * + * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE + * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY + * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS + * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED. + * + * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO + * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS + * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND + * CONDITIONS. + * + */ + +/* Author: Steven Macenski */ + +#ifndef SLAM_TOOLBOX_SERIALIZATION_H_ +#define SLAM_TOOLBOX_SERIALIZATION_H_ + +#include +#include +#include +#include +#include +#include + +namespace serialization +{ + +inline bool fileExists(const std::string& name) +{ + struct stat buffer; + return (stat (name.c_str(), &buffer) == 0); +} + +inline void write(const std::string& filename, + karto::Mapper& mapper, + karto::Dataset& dataset) +{ + try + { + mapper.SaveToFile(filename + std::string(".posegraph")); + dataset.SaveToFile(filename + std::string(".data")); + } + catch (boost::archive::archive_exception e) + { + ROS_ERROR("Failed to write file: Exception %s", e.what()); + } +} + +inline bool read(const std::string& filename, + karto::Mapper& mapper, + karto::Dataset& dataset) +{ + if (!fileExists(filename + std::string(".posegraph"))) + { + ROS_ERROR("serialization::Read: Failed to open " + "requested file: %s.", filename.c_str()); + return false; + } + + try + { + mapper.LoadFromFile(filename + std::string(".posegraph")); + dataset.LoadFromFile(filename + std::string(".data")); + return true; + } + catch (boost::archive::archive_exception e) + { + ROS_ERROR("serialization::Read: Failed to read file: " + "Exception: %s", e.what()); + } + + return false; +} + +} // end namespace + +#endif //SLAM_TOOLBOX_SERIALIZATION_H_ \ No newline at end of file diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox/include/slam_toolbox/slam_mapper.hpp b/Localizations/Packages/slam_toolbox/slam_toolbox/include/slam_toolbox/slam_mapper.hpp new file mode 100644 index 0000000..76de541 --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox/include/slam_toolbox/slam_mapper.hpp @@ -0,0 +1,67 @@ +/* + * Author + * Copyright (c) 2019 Samsung Research America + * + * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE + * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY + * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS + * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED. + * + * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO + * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS + * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND + * CONDITIONS. + * + */ + +/* Author: Steven Macenski */ + +#ifndef SLAM_TOOLBOX_SLAM_MAPPER_H_ +#define SLAM_TOOLBOX_SLAM_MAPPER_H_ + +#include "ros/ros.h" +#include "karto_sdk/Mapper.h" +#include "karto_sdk/Karto.h" +#include "tf2/utils.h" + +namespace mapper_utils +{ + +using namespace ::karto; + +class SMapper +{ +public: + SMapper(); + ~SMapper(); + + // get occupancy grid from scans + karto::OccupancyGrid* getOccupancyGrid(const double& resolution); + + // convert Karto pose to TF pose + tf2::Transform toTfPose(const karto::Pose2& pose) const; + + // convert TF pose to karto pose + karto::Pose2 toKartoPose(const tf2::Transform& pose) const; + + void configure(const ros::NodeHandle& nh); + void Reset(); + + // // processors + // kt_bool ProcessAtDock(LocalizedRangeScan* pScan); + // kt_bool ProcessAgainstNode(LocalizedRangeScan* pScan, const int& nodeId); + // kt_bool ProcessAgainstNodesNearBy(LocalizedRangeScan* pScan); + // kt_bool ProcessLocalization(LocalizedRangeScan* pScan); + + void setMapper(karto::Mapper* mapper); + karto::Mapper* getMapper(); + + void clearLocalizationBuffer(); + +protected: + std::unique_ptr mapper_; +}; + +} // end namespace + +#endif //SLAM_TOOLBOX_SLAM_MAPPER_H_ diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox/include/slam_toolbox/slam_toolbox_async.hpp b/Localizations/Packages/slam_toolbox/slam_toolbox/include/slam_toolbox/slam_toolbox_async.hpp new file mode 100644 index 0000000..b65f549 --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox/include/slam_toolbox/slam_toolbox_async.hpp @@ -0,0 +1,44 @@ +/* + * slam_toolbox + * Copyright Work Modifications (c) 2018, Simbe Robotics, Inc. + * Copyright Work Modifications (c) 2019, Steve Macenski + * + * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE + * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY + * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS + * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED. + * + * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO + * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS + * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND + * CONDITIONS. + * + */ + +/* Author: Steven Macenski */ + +#ifndef SLAM_TOOLBOX_SLAM_TOOLBOX_ASYNC_H_ +#define SLAM_TOOLBOX_SLAM_TOOLBOX_ASYNC_H_ + +#include "slam_toolbox/slam_toolbox_common.hpp" + +namespace slam_toolbox +{ + +class AsynchronousSlamToolbox : public SlamToolbox +{ +public: + AsynchronousSlamToolbox(ros::NodeHandle& nh); + ~AsynchronousSlamToolbox() {}; + +protected: + virtual void laserCallback( + const sensor_msgs::LaserScan::ConstPtr& scan) override final; + virtual bool deserializePoseGraphCallback( + slam_toolbox_msgs::DeserializePoseGraph::Request& req, + slam_toolbox_msgs::DeserializePoseGraph::Response& resp) override final; +}; + +} + +#endif //SLAM_TOOLBOX_SLAM_TOOLBOX_ASYNC_H_ diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox/include/slam_toolbox/slam_toolbox_common.hpp b/Localizations/Packages/slam_toolbox/slam_toolbox/include/slam_toolbox/slam_toolbox_common.hpp new file mode 100644 index 0000000..5dd2bf0 --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox/include/slam_toolbox/slam_toolbox_common.hpp @@ -0,0 +1,156 @@ +/* + * slam_toolbox + * Copyright (c) 2008, Willow Garage, Inc. + * Copyright Work Modifications (c) 2018, Simbe Robotics, Inc. + * Copyright Work Modifications (c) 2019, Steve Macenski + * + * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE + * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY + * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS + * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED. + * + * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO + * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS + * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND + * CONDITIONS. + * + */ + +#ifndef SLAM_TOOLBOX_SLAM_TOOLBOX_COMMON_H_ +#define SLAM_TOOLBOX_SLAM_TOOLBOX_COMMON_H_ + +#include "ros/ros.h" +#include "message_filters/subscriber.h" +#include "tf2_ros/transform_broadcaster.h" +#include "tf2_ros/transform_listener.h" +#include "tf2_ros/message_filter.h" +#include "tf2/LinearMath/Matrix3x3.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.h" + +#include "pluginlib/class_loader.h" + +#include "slam_toolbox/toolbox_types.hpp" +#include "slam_toolbox/slam_mapper.hpp" +#include "slam_toolbox/snap_utils.hpp" +#include "slam_toolbox/laser_utils.hpp" +#include "slam_toolbox/get_pose_helper.hpp" +#include "slam_toolbox/map_saver.hpp" +#include "slam_toolbox/loop_closure_assistant.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace slam_toolbox +{ + +// dirty, dirty cheat I love +using namespace ::toolbox_types; + +class SlamToolbox +{ +public: + SlamToolbox(ros::NodeHandle& nh); + ~SlamToolbox(); + bool getInitialized() {return this->initialized_;} + +protected: + // threads + void publishVisualizations(); + void publishTransformLoop(const double& transform_publish_period); + + // setup + void setParams(ros::NodeHandle& nh); + void setSolver(ros::NodeHandle& private_nh_); + void setROSInterfaces(ros::NodeHandle& node); + + // callbacks + virtual void laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan) = 0; + bool mapCallback(nav_msgs::GetMap::Request& req, + nav_msgs::GetMap::Response& res); + virtual bool serializePoseGraphCallback(slam_toolbox_msgs::SerializePoseGraph::Request& req, + slam_toolbox_msgs::SerializePoseGraph::Response& resp); + virtual bool deserializePoseGraphCallback(slam_toolbox_msgs::DeserializePoseGraph::Request& req, + slam_toolbox_msgs::DeserializePoseGraph::Response& resp); + virtual bool resetCallback(slam_toolbox_msgs::Reset::Request& req, + slam_toolbox_msgs::Reset::Response& resp); + void loadSerializedPoseGraph(std::unique_ptr&, std::unique_ptr&); + void loadPoseGraphByParams(ros::NodeHandle& nh); + + // functional bits + karto::LaserRangeFinder* getLaser(const sensor_msgs::LaserScan::ConstPtr& scan); + virtual karto::LocalizedRangeScan* addScan(karto::LaserRangeFinder* laser, const sensor_msgs::LaserScan::ConstPtr& scan, + karto::Pose2& karto_pose); + karto::LocalizedRangeScan* addScan(karto::LaserRangeFinder* laser, PosedScan& scanWPose); + bool updateMap(); + tf2::Stamped setTransformFromPoses(const karto::Pose2& pose, + const karto::Pose2& karto_pose, const ros::Time& t, const bool& update_reprocessing_transform); + karto::LocalizedRangeScan* getLocalizedRangeScan(karto::LaserRangeFinder* laser, + const sensor_msgs::LaserScan::ConstPtr& scan, + karto::Pose2& karto_pose); + bool shouldStartWithPoseGraph(std::string& filename, geometry_msgs::Pose2D& pose, bool& start_at_dock); + bool shouldProcessScan(const sensor_msgs::LaserScan::ConstPtr& scan, const karto::Pose2& pose); + void publishPose(const karto::Pose2 & pose, const karto::Matrix3 & cov, const ros::Time & t); + + // pausing bits + bool isPaused(const PausedApplication& app); + bool pauseNewMeasurementsCallback(slam_toolbox_msgs::Pause::Request& req, + slam_toolbox_msgs::Pause::Response& resp); + + // ROS-y-ness + ros::NodeHandle nh_; + std::unique_ptr tf_; + std::unique_ptr tfL_; + std::unique_ptr tfB_; + std::unique_ptr > scan_filter_sub_; + std::unique_ptr > scan_filter_; + ros::Publisher sst_, sstm_, pose_pub_; + ros::ServiceServer ssMap_, ssPauseMeasurements_, ssSerialize_, ssDesserialize_, ssReset_; + + // Storage for ROS parameters + std::string odom_frame_, map_frame_, base_frame_, map_name_, scan_topic_; + ros::Duration transform_timeout_, tf_buffer_dur_, minimum_time_interval_; + int throttle_scans_; + + double resolution_; + double position_covariance_scale_; + double yaw_covariance_scale_; + bool first_measurement_, enable_interactive_mode_; + + // Book keeping + std::unique_ptr smapper_; + std::unique_ptr dataset_; + std::map lasers_; + + // helpers + std::unique_ptr laser_assistant_; + std::unique_ptr pose_helper_; + std::unique_ptr map_saver_; + std::unique_ptr closure_assistant_; + std::unique_ptr scan_holder_; + + // Internal state + bool initialized_; + bool using_loop_; + std::vector > threads_; + tf2::Transform map_to_odom_; + boost::mutex map_to_odom_mutex_, smapper_mutex_, pose_mutex_; + PausedState state_; + nav_msgs::GetMap::Response map_; + ProcessType processor_type_; + std::unique_ptr process_near_pose_; + tf2::Transform reprocessing_transform_; + + // pluginlib + pluginlib::ClassLoader solver_loader_; + boost::shared_ptr solver_; +}; + +} // end namespace + +#endif //SLAM_TOOLBOX_SLAM_TOOLBOX_COMMON_H_ diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox/include/slam_toolbox/slam_toolbox_localization.hpp b/Localizations/Packages/slam_toolbox/slam_toolbox/include/slam_toolbox/slam_toolbox_localization.hpp new file mode 100644 index 0000000..594fced --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox/include/slam_toolbox/slam_toolbox_localization.hpp @@ -0,0 +1,62 @@ +/* + * slam_toolbox + * Copyright Work Modifications (c) 2019, Steve Macenski + * + * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE + * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY + * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS + * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED. + * + * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO + * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS + * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND + * CONDITIONS. + * + */ + +/* Author: Steven Macenski */ + +#ifndef SLAM_TOOLBOX_SLAM_TOOLBOX_LOCALIZATION_H_ +#define SLAM_TOOLBOX_SLAM_TOOLBOX_LOCALIZATION_H_ + +#include "slam_toolbox/slam_toolbox_common.hpp" +#include "std_srvs/Empty.h" + +namespace slam_toolbox +{ + +using namespace ::karto; + +class LocalizationSlamToolbox : public SlamToolbox +{ +public: + LocalizationSlamToolbox(ros::NodeHandle& nh); + ~LocalizationSlamToolbox() {}; + +protected: + virtual void laserCallback( + const sensor_msgs::LaserScan::ConstPtr& scan) override final; + void localizePoseCallback( + const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg); + + bool clearLocalizationBuffer( + std_srvs::Empty::Request& req, + std_srvs::Empty::Response& resp); + virtual bool serializePoseGraphCallback( + slam_toolbox_msgs::SerializePoseGraph::Request& req, + slam_toolbox_msgs::SerializePoseGraph::Response& resp) override final; + virtual bool deserializePoseGraphCallback( + slam_toolbox_msgs::DeserializePoseGraph::Request& req, + slam_toolbox_msgs::DeserializePoseGraph::Response& resp) override final; + + virtual LocalizedRangeScan* addScan(karto::LaserRangeFinder* laser, + const sensor_msgs::LaserScan::ConstPtr& scan, + karto::Pose2& karto_pose) override final; + + ros::Subscriber localization_pose_sub_; + ros::ServiceServer clear_localization_; +}; + +} + +#endif //SLAM_TOOLBOX_SLAM_TOOLBOX_LOCALIZATION_H_ diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox/include/slam_toolbox/slam_toolbox_sync.hpp b/Localizations/Packages/slam_toolbox/slam_toolbox/include/slam_toolbox/slam_toolbox_sync.hpp new file mode 100644 index 0000000..f6edb87 --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox/include/slam_toolbox/slam_toolbox_sync.hpp @@ -0,0 +1,50 @@ +/* + * slam_toolbox + * Copyright Work Modifications (c) 2018, Simbe Robotics, Inc. + * Copyright Work Modifications (c) 2019, Steve Macenski + * + * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE + * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY + * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS + * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED. + * + * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO + * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS + * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND + * CONDITIONS. + * + */ + +/* Author: Steven Macenski */ + +#ifndef SLAM_TOOLBOX_SLAM_TOOLBOX_SYNC_H_ +#define SLAM_TOOLBOX_SLAM_TOOLBOX_SYNC_H_ + +#include "slam_toolbox/slam_toolbox_common.hpp" + +namespace slam_toolbox +{ + +class SynchronousSlamToolbox : public SlamToolbox +{ +public: + SynchronousSlamToolbox(ros::NodeHandle& nh); + ~SynchronousSlamToolbox() {}; + void run(); + +protected: + virtual void laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan) override final; + bool clearQueueCallback(slam_toolbox_msgs::ClearQueue::Request& req, slam_toolbox_msgs::ClearQueue::Response& resp); + virtual bool deserializePoseGraphCallback(slam_toolbox_msgs::DeserializePoseGraph::Request& req, + slam_toolbox_msgs::DeserializePoseGraph::Response& resp) override final; + virtual bool resetCallback(slam_toolbox_msgs::Reset::Request& req, + slam_toolbox_msgs::Reset::Response& resp) override final; + + std::queue q_; + ros::ServiceServer ssClear_; + boost::mutex q_mutex_; +}; + +} + +#endif //SLAM_TOOLBOX_SLAM_TOOLBOX_SYNC_NODE_H_ diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox/include/slam_toolbox/snap_utils.hpp b/Localizations/Packages/slam_toolbox/slam_toolbox/include/slam_toolbox/snap_utils.hpp new file mode 100644 index 0000000..15b1999 --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox/include/slam_toolbox/snap_utils.hpp @@ -0,0 +1,45 @@ +/* + * snap_utils + * Copyright (c) 2019, Samsung Research America + * + * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE + * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY + * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS + * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED. + * + * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO + * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS + * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND + * CONDITIONS. + * + */ + +/* Author: Steven Macenski */ + +#ifndef SLAM_TOOLBOX_SNAP_UTILS_H_ +#define SLAM_TOOLBOX_SNAP_UTILS_H_ + +namespace snap_utils +{ + +// whether this is running in a snap container +inline bool isInSnap() +{ + char* snap_common = getenv("SNAP_COMMON"); + if (snap_common != NULL) + { + return true; + } + return false; +}; + +// get path of shared space +inline std::string getSnapPath() +{ + char* snap_common = getenv("SNAP_COMMON"); + return std::string(snap_common); +} + +} // end namespace + +#endif //SLAM_TOOLBOX_SNAP_UTILS_H_ diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox/include/slam_toolbox/toolbox_msgs.hpp b/Localizations/Packages/slam_toolbox/slam_toolbox/include/slam_toolbox/toolbox_msgs.hpp new file mode 100644 index 0000000..bc85a09 --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox/include/slam_toolbox/toolbox_msgs.hpp @@ -0,0 +1,45 @@ +/* + * slam_toolbox + * Copyright (c) 2019, Steve Macenski + * + * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE + * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY + * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS + * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED. + * + * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO + * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS + * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND + * CONDITIONS. + * + */ + +/* Author: Steven Macenski */ + +#ifndef SLAM_TOOLBOX_TOOLBOX_MSGS_H_ +#define SLAM_TOOLBOX_TOOLBOX_MSGS_H_ + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include "slam_toolbox_msgs/Pause.h" +#include "slam_toolbox_msgs/ClearQueue.h" +#include "slam_toolbox_msgs/ToggleInteractive.h" +#include "slam_toolbox_msgs/Clear.h" +#include "slam_toolbox_msgs/ClearQueue.h" +#include "slam_toolbox_msgs/SaveMap.h" +#include "slam_toolbox_msgs/LoopClosure.h" +#include "slam_toolbox_msgs/SerializePoseGraph.h" +#include "slam_toolbox_msgs/DeserializePoseGraph.h" +#include "slam_toolbox_msgs/MergeMaps.h" +#include "slam_toolbox_msgs/AddSubmap.h" +#include "slam_toolbox_msgs/Reset.h" + +#endif //SLAM_TOOLBOX_TOOLBOX_MSGS_H_ \ No newline at end of file diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox/include/slam_toolbox/toolbox_types.hpp b/Localizations/Packages/slam_toolbox/slam_toolbox/include/slam_toolbox/toolbox_types.hpp new file mode 100644 index 0000000..6cbacfa --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox/include/slam_toolbox/toolbox_types.hpp @@ -0,0 +1,129 @@ +/* + * toolbox_types + * Copyright (c) 2019, Samsung Research America + * + * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE + * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY + * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS + * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED. + * + * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO + * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS + * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND + * CONDITIONS. + * + */ + +/* Author: Steven Macenski */ + +#ifndef SLAM_TOOLBOX_TOOLBOX_TYPES_H_ +#define SLAM_TOOLBOX_TOOLBOX_TYPES_H_ + +#include +#include + +#include "tf/transform_datatypes.h" +#include "tf2_ros/buffer.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.h" +#include "sensor_msgs/LaserScan.h" +#include "geometry_msgs/PoseWithCovarianceStamped.h" +#include "slam_toolbox/toolbox_msgs.hpp" +#include "karto_sdk/Mapper.h" + +// compute linear index for given map coords +#define MAP_IDX(sx, i, j) ((sx) * (j) + (i)) + +namespace toolbox_types +{ + +// object containing a scan pointer and a position +struct PosedScan +{ + PosedScan(sensor_msgs::LaserScan::ConstPtr scan_in, karto::Pose2 pose_in) : + scan(scan_in), pose(pose_in) + { + } + sensor_msgs::LaserScan::ConstPtr scan; + karto::Pose2 pose; +}; + +// object containing a vertex pointer and an updated score +struct ScoredVertex +{ + ScoredVertex(karto::Vertex* vertex, double score) + : vertex_(vertex), score_(score) + { + } + + double GetScore() + { + return score_; + } + + karto::Vertex* GetVertex() + { + return vertex_; + } + + karto::Vertex* vertex_; + double score_; +}; + +typedef std::vector ScoredVertices; +typedef std::vector*> Vertices; + +// types of pause functionality available +enum PausedApplication +{ + PROCESSING = 0, + VISUALIZING_GRAPH = 1, + NEW_MEASUREMENTS = 2 +}; + +// types of sensor processing +enum ProcessType +{ + PROCESS = 0, + PROCESS_FIRST_NODE = 1, + PROCESS_NEAR_REGION = 2, + PROCESS_LOCALIZATION = 3 +}; + +// Pause state +struct PausedState +{ + PausedState() + { + state_map_[NEW_MEASUREMENTS] = false; + state_map_[VISUALIZING_GRAPH] = false; + state_map_[PROCESSING] = false; + }; + + void set(const PausedApplication& app, const bool& state) + { + boost::mutex::scoped_lock lock(pause_mutex_); + state_map_[app] = state; + }; + + bool get(const PausedApplication& app) + { + boost::mutex::scoped_lock lock(pause_mutex_); + return state_map_[app]; + }; + + std::map state_map_; + boost::mutex pause_mutex_; +}; + +typedef std::map*>> VerticeMap; +typedef std::vector*> EdgeVector; +typedef std::map*> ScanMap; +typedef std::vector*> ScanVector; +typedef slam_toolbox_msgs::DeserializePoseGraph::Request procType; + +typedef std::unordered_map::iterator GraphIterator; +typedef std::unordered_map::const_iterator ConstGraphIterator; + +} // end namespace + +#endif //SLAM_TOOLBOX_TOOLBOX_TYPES_H_ diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox/include/slam_toolbox/visualization_utils.hpp b/Localizations/Packages/slam_toolbox/slam_toolbox/include/slam_toolbox/visualization_utils.hpp new file mode 100644 index 0000000..601d9d5 --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox/include/slam_toolbox/visualization_utils.hpp @@ -0,0 +1,171 @@ +/* + * visualization_utils + * Copyright (c) 2019, Samsung Research America + * + * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE + * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY + * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS + * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED. + * + * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO + * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS + * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND + * CONDITIONS. + * + */ + +/* Author: Steven Macenski */ + +#ifndef SLAM_TOOLBOX_VISUALIZATION_UTILS_H_ +#define SLAM_TOOLBOX_VISUALIZATION_UTILS_H_ + +namespace vis_utils +{ + +inline visualization_msgs::Marker toVertexMarker( + const std::string& frame, + const std::string& ns, + const double& scale) +{ + visualization_msgs::Marker marker; + + marker.header.frame_id = frame; + marker.header.stamp = ros::Time::now(); + marker.ns = ns; + marker.type = visualization_msgs::Marker::SPHERE; + marker.pose.position.z = 0.0; + marker.pose.orientation.w = 1.; + marker.scale.x = scale; + marker.scale.y = scale; + marker.scale.z = scale; + marker.color.r = 1.0; + marker.color.g = 0; + marker.color.b = 0.0; + marker.color.a = 1.; + marker.action = visualization_msgs::Marker::ADD; + marker.lifetime = ros::Duration(0.); + + return marker; +} + +inline visualization_msgs::Marker toEdgeMarker( + const std::string& frame, + const std::string& ns, + const double& width) +{ + visualization_msgs::Marker marker; + + marker.header.frame_id = frame; + marker.header.stamp = ros::Time::now(); + marker.ns = ns; + marker.type = visualization_msgs::Marker::LINE_STRIP; + marker.pose.position.z = 0.0; + marker.pose.orientation.w = 1.; + marker.points.resize(2); + marker.scale.x = width; + marker.scale.y = 0; + marker.scale.z = 0; + marker.color.r = 0.0; + marker.color.g = 0; + marker.color.b = 1.0; + marker.color.a = 1.; + marker.action = visualization_msgs::Marker::ADD; + marker.lifetime = ros::Duration(0.); + + return marker; +} + +inline visualization_msgs::InteractiveMarker toInteractiveMarker( + visualization_msgs::Marker& marker, + const double& scale) +{ + // marker basics + visualization_msgs::InteractiveMarker int_marker; + int_marker.header.frame_id = marker.header.frame_id; + int_marker.header.stamp = ros::Time::now(); + int_marker.name = std::to_string(marker.id); + int_marker.pose.orientation.w = 1.; + int_marker.pose.position.x = marker.pose.position.x; + int_marker.pose.position.y = marker.pose.position.y; + int_marker.scale = scale; + + // translate control + visualization_msgs::InteractiveMarkerControl control; + control.orientation_mode = + visualization_msgs::InteractiveMarkerControl::FIXED; + control.always_visible = true; + control.orientation.w = 0; + control.orientation.x = 0.7071; + control.orientation.y = 0; + control.orientation.z = 0.7071; + control.interaction_mode = + visualization_msgs::InteractiveMarkerControl::MOVE_PLANE; + control.markers.push_back( marker ); + int_marker.controls.push_back( control ); + + // rotate control + visualization_msgs::InteractiveMarkerControl control_rot; + control_rot.orientation_mode = + visualization_msgs::InteractiveMarkerControl::FIXED; + control_rot.always_visible = true; + control_rot.orientation.w = 0; + control_rot.orientation.x = 0.7071; + control_rot.orientation.y = 0; + control_rot.orientation.z = 0.7071; + control_rot.interaction_mode = + visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS; + int_marker.controls.push_back( control_rot ); + + return int_marker; +} + +inline void toNavMap( + const karto::OccupancyGrid* occ_grid, + nav_msgs::OccupancyGrid& map) +{ + // Translate to ROS format + kt_int32s width = occ_grid->GetWidth(); + kt_int32s height = occ_grid->GetHeight(); + karto::Vector2 offset = + occ_grid->GetCoordinateConverter()->GetOffset(); + + if(map.info.width != (unsigned int) width || + map.info.height != (unsigned int) height || + map.info.origin.position.x != offset.GetX() || + map.info.origin.position.y != offset.GetY()) + { + map.info.origin.position.x = offset.GetX(); + map.info.origin.position.y = offset.GetY(); + map.info.width = width; + map.info.height = height; + map.data.resize(map.info.width * map.info.height); + } + + for (kt_int32s y = 0; y < height; y++) + { + for (kt_int32s x = 0; x < width; x++) + { + kt_int8u value = occ_grid->GetValue(karto::Vector2(x, y)); + switch (value) + { + case karto::GridStates_Unknown: + map.data[MAP_IDX(map.info.width, x, y)] = -1; + break; + case karto::GridStates_Occupied: + map.data[MAP_IDX(map.info.width, x, y)] = 100; + break; + case karto::GridStates_Free: + map.data[MAP_IDX(map.info.width, x, y)] = 0; + break; + default: + ROS_WARN("Encountered unknown cell value at %d, %d", x, y); + break; + } + } + } + return; +} + +} // end namespace + +#endif //SLAM_TOOLBOX_VISUALIZATION_UTILS_H_ diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox/launch/lifelong.launch b/Localizations/Packages/slam_toolbox/slam_toolbox/launch/lifelong.launch new file mode 100644 index 0000000..eb75ba8 --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox/launch/lifelong.launch @@ -0,0 +1,7 @@ + + + + + + + diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox/launch/localization.launch b/Localizations/Packages/slam_toolbox/slam_toolbox/launch/localization.launch new file mode 100644 index 0000000..4209ff0 --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox/launch/localization.launch @@ -0,0 +1,11 @@ + + + + + + + + + + + diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox/launch/merge_maps_kinematic.launch b/Localizations/Packages/slam_toolbox/slam_toolbox/launch/merge_maps_kinematic.launch new file mode 100644 index 0000000..a48768a --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox/launch/merge_maps_kinematic.launch @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox/launch/offline.launch b/Localizations/Packages/slam_toolbox/slam_toolbox/launch/offline.launch new file mode 100644 index 0000000..307e872 --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox/launch/offline.launch @@ -0,0 +1,7 @@ + + + + + + + diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox/launch/online_async.launch b/Localizations/Packages/slam_toolbox/slam_toolbox/launch/online_async.launch new file mode 100644 index 0000000..862e2db --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox/launch/online_async.launch @@ -0,0 +1,11 @@ + + + + + + + + + + + diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox/launch/online_sync.launch b/Localizations/Packages/slam_toolbox/slam_toolbox/launch/online_sync.launch new file mode 100644 index 0000000..6e2b154 --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox/launch/online_sync.launch @@ -0,0 +1,7 @@ + + + + + + + diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox/lib/karto_sdk/Authors b/Localizations/Packages/slam_toolbox/slam_toolbox/lib/karto_sdk/Authors new file mode 100644 index 0000000..3800580 --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox/lib/karto_sdk/Authors @@ -0,0 +1,7 @@ +Karto Open Source Library 1.0 + +Contributors: +------------------------------- +Michael A. Eriksen (eriksen@ai.sri.com) +Benson Limketkai (bensonl@ai.sri.com) +Steven Macenski (steven.macenski@simberobotics.com) \ No newline at end of file diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox/lib/karto_sdk/CHANGELOG.rst b/Localizations/Packages/slam_toolbox/slam_toolbox/lib/karto_sdk/CHANGELOG.rst new file mode 100644 index 0000000..1e44e3b --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox/lib/karto_sdk/CHANGELOG.rst @@ -0,0 +1,37 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package karto_sdk +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +1.1.4 (2016-03-02) +------------------ +* update build status badges +* Adds LocalizedRangeScanWithPoints range scan +* Contributors: Michael Ferguson, Russell Toris + +1.1.3 (2016-02-16) +------------------ +* Link against, and export depend on, boost +* Contributors: Hai Nguyen, Michael Ferguson + +1.1.2 (2015-07-13) +------------------ +* Added getters and setters for parameters inside Mapper so they can be changed via the ROS param server. +* Contributors: Luc Bettaieb, Michael Ferguson + +1.1.1 (2015-05-07) +------------------ +* Makes FindValidPoints robust to the first point in the scan being a NaN +* Bump minimum cmake version requirement +* Fix cppcheck warnings +* Fix newlines (dos2unix) & superfluous whitespace +* Protect functions that throw away const-ness (check dirty flag) with mutex +* Don't modify scan during loop closure check - work on a copy of it +* removed useless return to avoid cppcheck error +* Add Mapper::SetUseScanMatching +* Remove html entities from log output +* Fix NANs cause raytracing to take forever +* Contributors: Daniel Pinyol, Michael Ferguson, Paul Mathieu, Siegfried-A. Gevatter Pujals, liz-murphy + +1.1.0 (2014-06-15) +------------------ +* Release as a pure catkin ROS package diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox/lib/karto_sdk/CMakeLists.txt b/Localizations/Packages/slam_toolbox/slam_toolbox/lib/karto_sdk/CMakeLists.txt new file mode 100644 index 0000000..ad6ef02 --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox/lib/karto_sdk/CMakeLists.txt @@ -0,0 +1,34 @@ +cmake_minimum_required(VERSION 2.8.3) +project(karto_sdk) + +set(CMAKE_BUILD_TYPE Release) #None, Debug, Release, RelWithDebInfo, MinSizeRel +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -ftemplate-backtrace-limit=0") +set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake/") + +find_package(catkin REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(Boost REQUIRED system serialization filesystem thread) +find_package(TBB REQUIRED) +add_compile_options(-std=c++17) + +catkin_package( + DEPENDS + Boost + TBB + INCLUDE_DIRS + include + ${TBB_INCLUDE_DIRS} + LIBRARIES + kartoSlamToolbox +) + +add_definitions(${EIGEN3_DEFINITIONS}) + +include_directories(include ${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS} ${TBB_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}) +add_library(kartoSlamToolbox SHARED src/Karto.cpp src/Mapper.cpp) +target_link_libraries(kartoSlamToolbox ${Boost_LIBRARIES} ${TBB_LIBRARIES}) + +install(DIRECTORY include/ DESTINATION include) +install(TARGETS kartoSlamToolbox + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +) diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox/lib/karto_sdk/LICENSE b/Localizations/Packages/slam_toolbox/slam_toolbox/lib/karto_sdk/LICENSE new file mode 100644 index 0000000..65c5ca8 --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox/lib/karto_sdk/LICENSE @@ -0,0 +1,165 @@ + GNU LESSER GENERAL PUBLIC LICENSE + Version 3, 29 June 2007 + + Copyright (C) 2007 Free Software Foundation, Inc. + Everyone is permitted to copy and distribute verbatim copies + of this license document, but changing it is not allowed. + + + This version of the GNU Lesser General Public License incorporates +the terms and conditions of version 3 of the GNU General Public +License, supplemented by the additional permissions listed below. + + 0. Additional Definitions. + + As used herein, "this License" refers to version 3 of the GNU Lesser +General Public License, and the "GNU GPL" refers to version 3 of the GNU +General Public License. + + "The Library" refers to a covered work governed by this License, +other than an Application or a Combined Work as defined below. + + An "Application" is any work that makes use of an interface provided +by the Library, but which is not otherwise based on the Library. +Defining a subclass of a class defined by the Library is deemed a mode +of using an interface provided by the Library. + + A "Combined Work" is a work produced by combining or linking an +Application with the Library. The particular version of the Library +with which the Combined Work was made is also called the "Linked +Version". + + The "Minimal Corresponding Source" for a Combined Work means the +Corresponding Source for the Combined Work, excluding any source code +for portions of the Combined Work that, considered in isolation, are +based on the Application, and not on the Linked Version. + + The "Corresponding Application Code" for a Combined Work means the +object code and/or source code for the Application, including any data +and utility programs needed for reproducing the Combined Work from the +Application, but excluding the System Libraries of the Combined Work. + + 1. Exception to Section 3 of the GNU GPL. + + You may convey a covered work under sections 3 and 4 of this License +without being bound by section 3 of the GNU GPL. + + 2. Conveying Modified Versions. + + If you modify a copy of the Library, and, in your modifications, a +facility refers to a function or data to be supplied by an Application +that uses the facility (other than as an argument passed when the +facility is invoked), then you may convey a copy of the modified +version: + + a) under this License, provided that you make a good faith effort to + ensure that, in the event an Application does not supply the + function or data, the facility still operates, and performs + whatever part of its purpose remains meaningful, or + + b) under the GNU GPL, with none of the additional permissions of + this License applicable to that copy. + + 3. Object Code Incorporating Material from Library Header Files. + + The object code form of an Application may incorporate material from +a header file that is part of the Library. You may convey such object +code under terms of your choice, provided that, if the incorporated +material is not limited to numerical parameters, data structure +layouts and accessors, or small macros, inline functions and templates +(ten or fewer lines in length), you do both of the following: + + a) Give prominent notice with each copy of the object code that the + Library is used in it and that the Library and its use are + covered by this License. + + b) Accompany the object code with a copy of the GNU GPL and this license + document. + + 4. Combined Works. + + You may convey a Combined Work under terms of your choice that, +taken together, effectively do not restrict modification of the +portions of the Library contained in the Combined Work and reverse +engineering for debugging such modifications, if you also do each of +the following: + + a) Give prominent notice with each copy of the Combined Work that + the Library is used in it and that the Library and its use are + covered by this License. + + b) Accompany the Combined Work with a copy of the GNU GPL and this license + document. + + c) For a Combined Work that displays copyright notices during + execution, include the copyright notice for the Library among + these notices, as well as a reference directing the user to the + copies of the GNU GPL and this license document. + + d) Do one of the following: + + 0) Convey the Minimal Corresponding Source under the terms of this + License, and the Corresponding Application Code in a form + suitable for, and under terms that permit, the user to + recombine or relink the Application with a modified version of + the Linked Version to produce a modified Combined Work, in the + manner specified by section 6 of the GNU GPL for conveying + Corresponding Source. + + 1) Use a suitable shared library mechanism for linking with the + Library. A suitable mechanism is one that (a) uses at run time + a copy of the Library already present on the user's computer + system, and (b) will operate properly with a modified version + of the Library that is interface-compatible with the Linked + Version. + + e) Provide Installation Information, but only if you would otherwise + be required to provide such information under section 6 of the + GNU GPL, and only to the extent that such information is + necessary to install and execute a modified version of the + Combined Work produced by recombining or relinking the + Application with a modified version of the Linked Version. (If + you use option 4d0, the Installation Information must accompany + the Minimal Corresponding Source and Corresponding Application + Code. If you use option 4d1, you must provide the Installation + Information in the manner specified by section 6 of the GNU GPL + for conveying Corresponding Source.) + + 5. Combined Libraries. + + You may place library facilities that are a work based on the +Library side by side in a single library together with other library +facilities that are not Applications and are not covered by this +License, and convey such a combined library under terms of your +choice, if you do both of the following: + + a) Accompany the combined library with a copy of the same work based + on the Library, uncombined with any other library facilities, + conveyed under the terms of this License. + + b) Give prominent notice with the combined library that part of it + is a work based on the Library, and explaining where to find the + accompanying uncombined form of the same work. + + 6. Revised Versions of the GNU Lesser General Public License. + + The Free Software Foundation may publish revised and/or new versions +of the GNU Lesser General Public License from time to time. Such new +versions will be similar in spirit to the present version, but may +differ in detail to address new problems or concerns. + + Each version is given a distinguishing version number. If the +Library as you received it specifies that a certain numbered version +of the GNU Lesser General Public License "or any later version" +applies to it, you have the option of following the terms and +conditions either of that published version or of any later version +published by the Free Software Foundation. If the Library as you +received it does not specify a version number of the GNU Lesser +General Public License, you may choose any version of the GNU Lesser +General Public License ever published by the Free Software Foundation. + + If the Library as you received it specifies that a proxy can decide +whether future versions of the GNU Lesser General Public License shall +apply, that proxy's public statement of acceptance of any version is +permanent authorization for you to choose that version for the +Library. diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox/lib/karto_sdk/cmake/FindTBB.cmake b/Localizations/Packages/slam_toolbox/slam_toolbox/lib/karto_sdk/cmake/FindTBB.cmake new file mode 100644 index 0000000..2b41805 --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox/lib/karto_sdk/cmake/FindTBB.cmake @@ -0,0 +1,292 @@ +# - Find ThreadingBuildingBlocks include dirs and libraries +# Use this module by invoking find_package with the form: +# find_package(TBB +# [REQUIRED] # Fail with error if TBB is not found +# ) # +# Once done, this will define +# +# TBB_FOUND - system has TBB +# TBB_INCLUDE_DIRS - the TBB include directories +# TBB_LIBRARIES - TBB libraries to be lined, doesn't include malloc or +# malloc proxy +# +# TBB_VERSION_MAJOR - Major Product Version Number +# TBB_VERSION_MINOR - Minor Product Version Number +# TBB_INTERFACE_VERSION - Engineering Focused Version Number +# TBB_COMPATIBLE_INTERFACE_VERSION - The oldest major interface version +# still supported. This uses the engineering +# focused interface version numbers. +# +# TBB_MALLOC_FOUND - system has TBB malloc library +# TBB_MALLOC_INCLUDE_DIRS - the TBB malloc include directories +# TBB_MALLOC_LIBRARIES - The TBB malloc libraries to be lined +# +# TBB_MALLOC_PROXY_FOUND - system has TBB malloc proxy library +# TBB_MALLOC_PROXY_INCLUDE_DIRS = the TBB malloc proxy include directories +# TBB_MALLOC_PROXY_LIBRARIES - The TBB malloc proxy libraries to be lined +# +# +# This module reads hints about search locations from variables: +# ENV TBB_ARCH_PLATFORM for windows only +# ENV TBB_ROOT or just TBB_ROOT +# +# +# +# Modified by Robert Maynard from the original OGRE source +# +#------------------------------------------------------------------- +# This file is part of the CMake build system for OGRE +# (Object-oriented Graphics Rendering Engine) +# For the latest info, see http://www.ogre3d.org/ +# +# The contents of this file are placed in the public domain. Feel +# free to make use of it in any way you like. +#------------------------------------------------------------------- +# +#============================================================================= +# Copyright 2010-2012 Kitware, Inc. +# Copyright 2012 Rolf Eike Beer +# +# Distributed under the OSI-approved BSD License (the "License"); +# see accompanying file Copyright.txt for details. +# +# This software is distributed WITHOUT ANY WARRANTY; without even the +# implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. +# See the License for more information. +#============================================================================= +# (To distribute this file outside of CMake, substitute the full +# License text for the above reference.) + + +#============================================================================= +# FindTBB helper functions and macros +# + +#=============================================== +# Create search paths based on prefix path +#=============================================== +macro(create_search_paths PREFIX) + foreach(dir ${${PREFIX}_PREFIX_PATH}) + set(${PREFIX}_INC_SEARCH_PATH ${${PREFIX}_INC_SEARCH_PATH} + ${dir}/include ${dir}/Include ${dir}/include/${PREFIX}) + set(${PREFIX}_LIB_SEARCH_PATH ${${PREFIX}_LIB_SEARCH_PATH} + ${dir}/lib ${dir}/Lib ${dir}/lib/${PREFIX} ${dir}/Libs) + endforeach(dir) +endmacro(create_search_paths) + +#=============================================== +# Do the final processing for the package find. +#=============================================== +macro(findpkg_finish PREFIX) + # skip if already processed during this run + if (NOT ${PREFIX}_FOUND) + if (${PREFIX}_INCLUDE_DIR AND ${PREFIX}_LIBRARY) + set(${PREFIX}_FOUND TRUE) + set (${PREFIX}_INCLUDE_DIRS ${${PREFIX}_INCLUDE_DIR}) + set (${PREFIX}_LIBRARIES ${${PREFIX}_LIBRARY}) + else () + if (${PREFIX}_FIND_REQUIRED AND NOT ${PREFIX}_FIND_QUIETLY) + message(FATAL_ERROR "Required library ${PREFIX} not found.") + endif () + endif () + + #mark the following variables as internal variables + mark_as_advanced(${PREFIX}_INCLUDE_DIR + ${PREFIX}_LIBRARY + ${PREFIX}_LIBRARY_DEBUG + ${PREFIX}_LIBRARY_RELEASE) + endif () +endmacro(findpkg_finish) + +#=============================================== +# Generate debug names from given RELEASEease names +#=============================================== +macro(get_debug_names PREFIX) + foreach(i ${${PREFIX}}) + set(${PREFIX}_DEBUG ${${PREFIX}_DEBUG} ${i}d ${i}D ${i}_d ${i}_D ${i}_debug ${i}) + endforeach(i) +endmacro(get_debug_names) + +#=============================================== +# See if we have env vars to help us find tbb +#=============================================== +macro(getenv_path VAR) + set(ENV_${VAR} $ENV{${VAR}}) + # replace won't work if var is blank + if (ENV_${VAR}) + string( REGEX REPLACE "\\\\" "/" ENV_${VAR} ${ENV_${VAR}} ) + endif () +endmacro(getenv_path) + +#=============================================== +# Couple a set of RELEASEease AND debug libraries +#=============================================== +macro(make_library_set PREFIX) + if (${PREFIX}_RELEASE AND ${PREFIX}_DEBUG) + set(${PREFIX} optimized ${${PREFIX}_RELEASE} debug ${${PREFIX}_DEBUG}) + elseif (${PREFIX}_RELEASE) + set(${PREFIX} ${${PREFIX}_RELEASE}) + elseif (${PREFIX}_DEBUG) + set(${PREFIX} ${${PREFIX}_DEBUG}) + endif () +endmacro(make_library_set) + + +#============================================================================= +# Now to actually find TBB +# + +# Get path, convert backslashes as ${ENV_${var}} +getenv_path(TBB_ROOT) +# construct search paths +set(TBB_PREFIX_PATH ${TBB_ROOT} ${ENV_TBB_ROOT}) +create_search_paths(TBB) + +# get the arch, only used by windows +if($ENV{TBB_ARCH_PLATFORM}) + set(TBB_ARCH_PLATFORM $ENV{TBB_ARCH_PLATFORM}) +endif() + +# For Windows, let's assume that the user might be using the precompiled +# TBB packages from the main website. These use a rather awkward directory +# structure (at least for automatically finding the right files) depending +# on platform and compiler, but we'll do our best to accommodate it. +# Not adding the same effort for the precompiled linux builds, though. Those +# have different versions for CC compiler versions and linux kernels which +# will never adequately match the user's setup, so there is no feasible way +# to detect the "best" version to use. The user will have to manually +# select the right files. (Chances are the distributions are shipping their +# custom version of tbb, anyway, so the problem is probably nonexistant.) +if (WIN32 AND MSVC) + set(COMPILER_PREFIX "vc7.1") + if (MSVC_VERSION EQUAL 1400) + set(COMPILER_PREFIX "vc8") + elseif(MSVC_VERSION EQUAL 1500) + set(COMPILER_PREFIX "vc9") + elseif(MSVC_VERSION EQUAL 1600) + set(COMPILER_PREFIX "vc10") + elseif(MSVC_VERSION EQUAL 1700) + set(COMPILER_PREFIX "vc11") + elseif(MSVC_VERSION EQUAL 1800) + set(COMPILER_PREFIX "vc12") + endif () + + # for each prefix path, add ia32/64\${COMPILER_PREFIX}\lib to the lib search path + foreach (dir ${TBB_PREFIX_PATH}) + if (CMAKE_CL_64) + list(APPEND TBB_LIB_SEARCH_PATH ${dir}/ia64/${COMPILER_PREFIX}/lib) + list(APPEND TBB_LIB_SEARCH_PATH ${dir}/lib/ia64/${COMPILER_PREFIX}) + list(APPEND TBB_LIB_SEARCH_PATH ${dir}/intel64/${COMPILER_PREFIX}/lib) + list(APPEND TBB_LIB_SEARCH_PATH ${dir}/lib/intel64/${COMPILER_PREFIX}) + else () + list(APPEND TBB_LIB_SEARCH_PATH ${dir}/ia32/${COMPILER_PREFIX}/lib) + list(APPEND TBB_LIB_SEARCH_PATH ${dir}/lib/ia32/${COMPILER_PREFIX}) + endif () + endforeach () +endif () + +foreach (dir ${TBB_PREFIX_PATH}) + list(APPEND TBB_LIB_SEARCH_PATH ${dir}/${TBB_ARCH_PLATFORM}/lib) + list(APPEND TBB_LIB_SEARCH_PATH ${dir}/lib/${TBB_ARCH_PLATFORM}) + list(APPEND TBB_LIB_SEARCH_PATH ${dir}/lib) +endforeach () + + +set(TBB_LIBRARY_NAMES tbb) +get_debug_names(TBB_LIBRARY_NAMES) + + +find_path(TBB_INCLUDE_DIR + NAMES tbb/tbb.h + PATHS ${TBB_INC_SEARCH_PATH}) + +find_library(TBB_LIBRARY_RELEASE + NAMES ${TBB_LIBRARY_NAMES} + PATHS ${TBB_LIB_SEARCH_PATH}) +find_library(TBB_LIBRARY_DEBUG + NAMES ${TBB_LIBRARY_NAMES_DEBUG} + PATHS ${TBB_LIB_SEARCH_PATH}) +make_library_set(TBB_LIBRARY) + +findpkg_finish(TBB) + +#on unix we need to also link to rt +if(UNIX AND NOT APPLE) + list(APPEND TBB_LIBRARIES rt) +endif() + +#if we haven't found TBB no point on going any further +if (NOT TBB_FOUND) + return() +endif () + +#============================================================================= +# Look for TBB's malloc package +set(TBB_MALLOC_LIBRARY_NAMES tbbmalloc) +get_debug_names(TBB_MALLOC_LIBRARY_NAMES) + +find_path(TBB_MALLOC_INCLUDE_DIR + NAMES tbb/tbb.h + PATHS ${TBB_INC_SEARCH_PATH}) + +find_library(TBB_MALLOC_LIBRARY_RELEASE + NAMES ${TBB_MALLOC_LIBRARY_NAMES} + PATHS ${TBB_LIB_SEARCH_PATH}) +find_library(TBB_MALLOC_LIBRARY_DEBUG + NAMES ${TBB_MALLOC_LIBRARY_NAMES_DEBUG} + PATHS ${TBB_LIB_SEARCH_PATH}) +make_library_set(TBB_MALLOC_LIBRARY) + +findpkg_finish(TBB_MALLOC) + +#============================================================================= +# Look for TBB's malloc proxy package +set(TBB_MALLOC_PROXY_LIBRARY_NAMES tbbmalloc_proxy) +get_debug_names(TBB_MALLOC_PROXY_LIBRARY_NAMES) + +find_path(TBB_MALLOC_PROXY_INCLUDE_DIR + NAMES tbb/tbbmalloc_proxy.h + PATHS ${TBB_INC_SEARCH_PATH}) + +find_library(TBB_MALLOC_PROXY_LIBRARY_RELEASE + NAMES ${TBB_MALLOC_PROXY_LIBRARY_NAMES} + PATHS ${TBB_LIB_SEARCH_PATH}) +find_library(TBB_MALLOC_PROXY_LIBRARY_DEBUG + NAMES ${TBB_MALLOC_PROXY_LIBRARY_NAMES_DEBUG} + PATHS ${TBB_LIB_SEARCH_PATH}) +make_library_set(TBB_MALLOC_PROXY_LIBRARY) + +findpkg_finish(TBB_MALLOC_PROXY) + +#----------------------------------------------------------------------------- +# setup timing libs we need to link too + + +#============================================================================= +#parse all the version numbers from tbb +if(NOT TBB_VERSION) + + #only read the start of the file + file(READ + "${TBB_INCLUDE_DIR}/tbb/tbb_stddef.h" + TBB_VERSION_CONTENTS + LIMIT 2048) + + string(REGEX REPLACE + ".*#define TBB_VERSION_MAJOR ([0-9]+).*" "\\1" + TBB_VERSION_MAJOR "${TBB_VERSION_CONTENTS}") + + string(REGEX REPLACE + ".*#define TBB_VERSION_MINOR ([0-9]+).*" "\\1" + TBB_VERSION_MINOR "${TBB_VERSION_CONTENTS}") + + string(REGEX REPLACE + ".*#define TBB_INTERFACE_VERSION ([0-9]+).*" "\\1" + TBB_INTERFACE_VERSION "${TBB_VERSION_CONTENTS}") + + string(REGEX REPLACE + ".*#define TBB_COMPATIBLE_INTERFACE_VERSION ([0-9]+).*" "\\1" + TBB_COMPATIBLE_INTERFACE_VERSION "${TBB_VERSION_CONTENTS}") + +endif() + diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox/lib/karto_sdk/include/karto_sdk/Karto.h b/Localizations/Packages/slam_toolbox/slam_toolbox/lib/karto_sdk/include/karto_sdk/Karto.h new file mode 100644 index 0000000..5373b46 --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox/lib/karto_sdk/include/karto_sdk/Karto.h @@ -0,0 +1,7199 @@ +/* + * Copyright 2010 SRI International + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with this program. If not, see . + */ + +#ifndef karto_sdk_KARTO_H +#define karto_sdk_KARTO_H + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#ifdef USE_POCO +#include +#endif + +#include +#include + +#define KARTO_Object(name) \ + virtual const char* GetClassName() const { return #name; } \ + virtual kt_objecttype GetObjectType() const { return ObjectType_##name; } + +typedef kt_int32u kt_objecttype; + +const kt_objecttype ObjectType_None = 0x00000000; +const kt_objecttype ObjectType_Sensor = 0x00001000; +const kt_objecttype ObjectType_SensorData = 0x00002000; +const kt_objecttype ObjectType_CustomData = 0x00004000; +const kt_objecttype ObjectType_Misc = 0x10000000; + +const kt_objecttype ObjectType_Drive = ObjectType_Sensor | 0x01; +const kt_objecttype ObjectType_LaserRangeFinder = ObjectType_Sensor | 0x02; +const kt_objecttype ObjectType_Camera = ObjectType_Sensor | 0x04; + +const kt_objecttype ObjectType_DrivePose = ObjectType_SensorData | 0x01; +const kt_objecttype ObjectType_LaserRangeScan = ObjectType_SensorData | 0x02; +const kt_objecttype ObjectType_LocalizedRangeScan = ObjectType_SensorData | 0x04; +const kt_objecttype ObjectType_CameraImage = ObjectType_SensorData | 0x08; +const kt_objecttype ObjectType_LocalizedRangeScanWithPoints = ObjectType_SensorData | 0x16; + +const kt_objecttype ObjectType_Header = ObjectType_Misc | 0x01; +const kt_objecttype ObjectType_Parameters = ObjectType_Misc | 0x02; +const kt_objecttype ObjectType_DatasetInfo = ObjectType_Misc | 0x04; +const kt_objecttype ObjectType_Module = ObjectType_Misc | 0x08; + +namespace karto +{ + + /** + * \defgroup OpenKarto OpenKarto Module + */ + /*@{*/ + + /** + * Exception class. All exceptions thrown from Karto will inherit from this class or be of this class + */ + class KARTO_EXPORT Exception + { + public: + /** + * Constructor with exception message + * @param rMessage exception message (default: "Karto Exception") + * @param errorCode error code (default: 0) + */ + Exception(const std::string& rMessage = "Karto Exception", kt_int32s errorCode = 0) + : m_Message(rMessage) + , m_ErrorCode(errorCode) + { + } + + /** + * Copy constructor + */ + Exception(const Exception& rException) + : m_Message(rException.m_Message) + , m_ErrorCode(rException.m_ErrorCode) + { + } + + /** + * Destructor + */ + virtual ~Exception() + { + } + + public: + /** + * Assignment operator + */ + Exception& operator = (const Exception& rException) + { + m_Message = rException.m_Message; + m_ErrorCode = rException.m_ErrorCode; + + return *this; + } + + public: + /** + * Gets the exception message + * @return error message as string + */ + const std::string& GetErrorMessage() const + { + return m_Message; + } + + /** + * Gets error code + * @return error code + */ + kt_int32s GetErrorCode() + { + return m_ErrorCode; + } + + public: + /** + * Write exception to output stream + * @param rStream output stream + * @param rException exception to write + */ + friend KARTO_EXPORT std::ostream& operator << (std::ostream& rStream, Exception& rException); + + private: + std::string m_Message; + kt_int32s m_ErrorCode; + }; // class Exception + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + /** + * Subclass this class to make a non-copyable class (copy + * constructor and assignment operator are private) + */ + class KARTO_EXPORT NonCopyable + { + private: + NonCopyable(const NonCopyable&) = delete; + const NonCopyable& operator=(const NonCopyable&) = delete; + + public: + NonCopyable() + { + } + + virtual ~NonCopyable() + { + } + + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + } + }; // class NonCopyable + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + /** + * Singleton class ensures only one instance of T is created + */ + template + class Singleton + { + public: + /** + * Constructor + */ + Singleton() + : m_pPointer(NULL) + { + } + + /** + * Destructor + */ + virtual ~Singleton() + { + delete m_pPointer; + } + + /** + * Gets the singleton + * @return singleton + */ + T* Get() + { +#ifdef USE_POCO + Poco::FastMutex::ScopedLock lock(m_Mutex); +#endif + if (m_pPointer == NULL) + { + m_pPointer = new T; + } + + return m_pPointer; + } + + private: + T* m_pPointer; + +#ifdef USE_POCO + Poco::FastMutex m_Mutex; +#endif + + private: + Singleton(const Singleton&); + const Singleton& operator=(const Singleton&); + }; + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + /** + * Functor + */ + class KARTO_EXPORT Functor + { + public: + /** + * Functor function + */ + virtual void operator() (kt_int32u) {}; + }; // Functor + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + class AbstractParameter; + + /** + * Type declaration of AbstractParameter vector + */ + typedef std::vector ParameterVector; + + /** + * Parameter manager. + */ + class KARTO_EXPORT ParameterManager : public NonCopyable + { + public: + /** + * Default constructor + */ + ParameterManager() + { + } + + /** + * Destructor + */ + virtual ~ParameterManager() + { + Clear(); + } + + public: + /** + * Adds the parameter to this manager + * @param pParameter + */ + void Add(AbstractParameter* pParameter); + + /** + * Gets the parameter of the given name + * @param rName + * @return parameter of given name + */ + AbstractParameter* Get(const std::string& rName) + { + if (m_ParameterLookup.find(rName) != m_ParameterLookup.end()) + { + return m_ParameterLookup[rName]; + } + + std::cout << "Unknown parameter: " << rName << std::endl; + + return NULL; + } + + /** + * Clears the manager of all parameters + */ + void Clear(); + + /** + * Gets all parameters + * @return vector of all parameters + */ + inline const ParameterVector& GetParameterVector() const + { + return m_Parameters; + } + + public: + /** + * Gets the parameter with the given name + * @param rName + * @return parameter of given name + */ + AbstractParameter* operator() (const std::string& rName) + { + return Get(rName); + } + + /** + * Serialization: class ParameterManager + */ + + private: + ParameterManager(const ParameterManager&); + const ParameterManager& operator=(const ParameterManager&); + + private: + ParameterVector m_Parameters; + std::map m_ParameterLookup; + + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(NonCopyable); + ar & BOOST_SERIALIZATION_NVP(m_Parameters); + ar & BOOST_SERIALIZATION_NVP(m_ParameterLookup); + } + + }; // ParameterManager + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + // valid names + // 'Test' -- no scope + // '/Test' -- no scope will be parsed to 'Test' + // '/scope/Test' - 'scope' scope and 'Test' name + // '/scope/name/Test' - 'scope/name' scope and 'Test' name + // + class Name + { + public: + /** + * Constructor + */ + Name() + { + } + + /** + * Constructor + */ + Name(const std::string& rName) + { + Parse(rName); + } + + /** + * Constructor + */ + Name(const Name& rOther) + : m_Scope(rOther.m_Scope), m_Name(rOther.m_Name) + { + } + + /** + * Destructor + */ + virtual ~Name() + { + } + + public: + /** + * Gets the name of this name + * @return name + */ + inline const std::string& GetName() const + { + return m_Name; + } + + /** + * Sets the name + * @param rName name + */ + inline void SetName(const std::string& rName) + { + std::string::size_type pos = rName.find_last_of('/'); + if (pos != 0 && pos != std::string::npos) + { + throw Exception("Name can't contain a scope!"); + } + + m_Name = rName; + } + + /** + * Gets the scope of this name + * @return scope + */ + inline const std::string& GetScope() const + { + return m_Scope; + } + + /** + * Sets the scope of this name + * @param rScope scope + */ + inline void SetScope(const std::string& rScope) + { + m_Scope = rScope; + } + + /** + * Returns a string representation of this name + * @return string representation of this name + */ + inline std::string ToString() const + { + if (m_Scope.empty()) + { + return m_Name; + } + else + { + std::string name; + name.append("/"); + name.append(m_Scope); + name.append("/"); + name.append(m_Name); + + return name; + } + } + + public: + /** + * Assignment operator. + */ + Name& operator = (const Name& rOther) + { + if (&rOther != this) + { + m_Name = rOther.m_Name; + m_Scope = rOther.m_Scope; + } + + return *this; + } + + /** + * Equality operator. + */ + kt_bool operator == (const Name& rOther) const + { + return (m_Name == rOther.m_Name) && (m_Scope == rOther.m_Scope); + } + + /** + * Inequality operator. + */ + kt_bool operator != (const Name& rOther) const + { + return !(*this == rOther); + } + + /** + * Less than operator. + */ + kt_bool operator < (const Name& rOther) const + { + return this->ToString() < rOther.ToString(); + } + + /** + * Write Name onto output stream + * @param rStream output stream + * @param rName to write + */ + friend inline std::ostream& operator << (std::ostream& rStream, const Name& rName) + { + rStream << rName.ToString(); + return rStream; + } + + private: + /** + * Parse the given string into a Name object + * @param rName name + */ + void Parse(const std::string& rName) + { + std::string::size_type pos = rName.find_last_of('/'); + + if (pos == std::string::npos) + { + m_Name = rName; + } + else + { + m_Scope = rName.substr(0, pos); + m_Name = rName.substr(pos+1, rName.size()); + + // remove '/' from m_Scope if first!! + if (m_Scope.size() > 0 && m_Scope[0] == '/') + { + m_Scope = m_Scope.substr(1, m_Scope.size()); + } + } + } + + /** + * Validates the given string as a Name + * @param rName name + */ + void Validate(const std::string& rName) + { + if (rName.empty()) + { + return; + } + + char c = rName[0]; + if (IsValidFirst(c)) + { + for (size_t i = 1; i < rName.size(); ++i) + { + c = rName[i]; + if (!IsValid(c)) + { + throw Exception("Invalid character in name. Valid characters must be within the ranges A-Z, a-z, 0-9, '/', '_' and '-'."); + } + } + } + else + { + throw Exception("Invalid first character in name. Valid characters must be within the ranges A-Z, a-z, and '/'."); + } + } + + /** + * Whether the character is valid as a first character (alphanumeric or /) + * @param c character + * @return true if the character is a valid first character + */ + inline kt_bool IsValidFirst(char c) + { + return (isalpha(c) || c == '/'); + } + + /** + * Whether the character is a valid character (alphanumeric, /, _, or -) + * @param c character + * @return true if the character is valid + */ + inline kt_bool IsValid(char c) + { + return (isalnum(c) || c == '/' || c == '_' || c == '-'); + } + + private: + std::string m_Name; + std::string m_Scope; + /** + * Serialization: class Name + */ + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + ar & BOOST_SERIALIZATION_NVP(m_Name); + ar & BOOST_SERIALIZATION_NVP(m_Scope); + } + }; + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + /** + * Abstract base class for Karto objects. + */ + class KARTO_EXPORT Object : public NonCopyable + { + public: + /** + * Default constructor + */ + Object(); + + /** + * Constructs an object with the given name + * @param rName + */ + Object(const Name& rName); + + /** + * Default constructor + */ + virtual ~Object(); + + public: + /** + * Gets the name of this object + * @return name + */ + inline const Name& GetName() const + { + return m_Name; + } + + /** + * Gets the class name of this object + * @return class name + */ + virtual const char* GetClassName() const = 0; + + /** + * Gets the type of this object + * @return object type + */ + virtual kt_objecttype GetObjectType() const = 0; + + /** + * Gets the parameter manager of this dataset + * @return parameter manager + */ + virtual inline ParameterManager* GetParameterManager() + { + return m_pParameterManager; + } + + /** + * Gets the named parameter + * @param rName name of parameter + * @return parameter + */ + inline AbstractParameter* GetParameter(const std::string& rName) const + { + return m_pParameterManager->Get(rName); + } + + /** + * Sets the parameter with the given name with the given value + * @param rName name + * @param value value + */ + template + inline void SetParameter(const std::string& rName, T value); + + /** + * Gets all parameters + * @return parameters + */ + inline const ParameterVector& GetParameters() const + { + return m_pParameterManager->GetParameterVector(); + } + + Object(const Object&); + const Object& operator=(const Object&); + + private: + Name m_Name; + ParameterManager* m_pParameterManager; + /** + * Serialization: class Object + */ + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(NonCopyable); + ar & BOOST_SERIALIZATION_NVP(m_pParameterManager); + ar & BOOST_SERIALIZATION_NVP(m_Name); + } + }; + BOOST_SERIALIZATION_ASSUME_ABSTRACT(Object) + + /** + * Type declaration of Object vector + */ + typedef std::vector ObjectVector; + typedef std::map DataMap; + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + /** + * Whether the object is a sensor + * @param pObject object + * @return whether the object is a sensor + */ + inline kt_bool IsSensor(Object* pObject) + { + return (pObject->GetObjectType() & ObjectType_Sensor) == ObjectType_Sensor; + } + + /** + * Whether the object is sensor data + * @param pObject object + * @return whether the object is sensor data + */ + inline kt_bool IsSensorData(Object* pObject) + { + return (pObject->GetObjectType() & ObjectType_SensorData) == ObjectType_SensorData; + } + + /** + * Whether the object is a laser range finder + * @param pObject object + * @return whether the object is a laser range finder + */ + inline kt_bool IsLaserRangeFinder(Object* pObject) + { + return (pObject->GetObjectType() & ObjectType_LaserRangeFinder) == ObjectType_LaserRangeFinder; + } + + /** + * Whether the object is a localized range scan + * @param pObject object + * @return whether the object is a localized range scan + */ + inline kt_bool IsLocalizedRangeScan(Object* pObject) + { + return (pObject->GetObjectType() & ObjectType_LocalizedRangeScan) == ObjectType_LocalizedRangeScan; + } + + /** + * Whether the object is a localized range scan with points + * @param pObject object + * @return whether the object is a localized range scan with points + */ + inline kt_bool IsLocalizedRangeScanWithPoints(Object* pObject) + { + return (pObject->GetObjectType() & ObjectType_LocalizedRangeScanWithPoints) == ObjectType_LocalizedRangeScanWithPoints; + } + + /** + * Whether the object is a Parameters object + * @param pObject object + * @return whether the object is a Parameters object + */ + inline kt_bool IsParameters(Object* pObject) + { + return (pObject->GetObjectType() & ObjectType_Parameters) == ObjectType_Parameters; + } + + /** + * Whether the object is a DatasetInfo object + * @param pObject object + * @return whether the object is a DatasetInfo object + */ + inline kt_bool IsDatasetInfo(Object* pObject) + { + return (pObject->GetObjectType() & ObjectType_DatasetInfo) == ObjectType_DatasetInfo; + } + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + /** + * Abstract base class for Karto modules. + */ + class KARTO_EXPORT Module : public Object + { + public: + // @cond EXCLUDE + KARTO_Object(Module) + // @endcond + + public: + /** + * Construct a Module + * @param rName module name + */ + Module(const std::string& rName); + + /** + * Destructor + */ + virtual ~Module(); + + public: + /** + * Reset the module + */ + virtual void Reset() = 0; + + /** + * Process an Object + */ + virtual kt_bool Process(karto::Object*) + { + return false; + } + + private: + Module(const Module&); + const Module& operator=(const Module&); + + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Object); + } + }; + BOOST_SERIALIZATION_ASSUME_ABSTRACT(Module) + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + /** + * Represents a size (width, height) in 2-dimensional real space. + */ + template + class Size2 + { + public: + /** + * Default constructor + */ + Size2() + : m_Width(0) + , m_Height(0) + { + } + + /** + * Constructor initializing point location + * @param width + * @param height + */ + Size2(T width, T height) + : m_Width(width) + , m_Height(height) + { + } + + /** + * Copy constructor + * @param rOther + */ + Size2(const Size2& rOther) + : m_Width(rOther.m_Width) + , m_Height(rOther.m_Height) + { + } + + public: + /** + * Gets the width + * @return the width + */ + inline const T GetWidth() const + { + return m_Width; + } + + /** + * Sets the width + * @param width + */ + inline void SetWidth(T width) + { + m_Width = width; + } + + /** + * Gets the height + * @return the height + */ + inline const T GetHeight() const + { + return m_Height; + } + + /** + * Sets the height + * @param height + */ + inline void SetHeight(T height) + { + m_Height = height; + } + + /** + * Assignment operator + */ + inline Size2& operator = (const Size2& rOther) + { + m_Width = rOther.m_Width; + m_Height = rOther.m_Height; + + return(*this); + } + + /** + * Equality operator + */ + inline kt_bool operator == (const Size2& rOther) const + { + return (m_Width == rOther.m_Width && m_Height == rOther.m_Height); + } + + /** + * Inequality operator + */ + inline kt_bool operator != (const Size2& rOther) const + { + return (m_Width != rOther.m_Width || m_Height != rOther.m_Height); + } + + /** + * Write Size2 onto output stream + * @param rStream output stream + * @param rSize to write + */ + friend inline std::ostream& operator << (std::ostream& rStream, const Size2& rSize) + { + rStream << "(" << rSize.m_Width << ", " << rSize.m_Height << ")"; + return rStream; + } + + private: + T m_Width; + T m_Height; + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + ar & BOOST_SERIALIZATION_NVP(m_Width); + ar & BOOST_SERIALIZATION_NVP(m_Height); + } + }; // Size2 + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + /** + * Represents a vector (x, y) in 2-dimensional real space. + */ + template + class Vector2 + { + public: + /** + * Default constructor + */ + Vector2() + { + m_Values[0] = 0; + m_Values[1] = 0; + } + + /** + * Constructor initializing vector location + * @param x + * @param y + */ + Vector2(T x, T y) + { + m_Values[0] = x; + m_Values[1] = y; + } + + public: + /** + * Gets the x-coordinate of this vector2 + * @return the x-coordinate of the vector2 + */ + inline const T& GetX() const + { + return m_Values[0]; + } + + /** + * Sets the x-coordinate of this vector2 + * @param x the x-coordinate of the vector2 + */ + inline void SetX(const T& x) + { + m_Values[0] = x; + } + + /** + * Gets the y-coordinate of this vector2 + * @return the y-coordinate of the vector2 + */ + inline const T& GetY() const + { + return m_Values[1]; + } + + /** + * Sets the y-coordinate of this vector2 + * @param y the y-coordinate of the vector2 + */ + inline void SetY(const T& y) + { + m_Values[1] = y; + } + + /** + * Floor point operator + * @param rOther + */ + inline void MakeFloor(const Vector2& rOther) + { + if ( rOther.m_Values[0] < m_Values[0] ) m_Values[0] = rOther.m_Values[0]; + if ( rOther.m_Values[1] < m_Values[1] ) m_Values[1] = rOther.m_Values[1]; + } + + /** + * Ceiling point operator + * @param rOther + */ + inline void MakeCeil(const Vector2& rOther) + { + if ( rOther.m_Values[0] > m_Values[0] ) m_Values[0] = rOther.m_Values[0]; + if ( rOther.m_Values[1] > m_Values[1] ) m_Values[1] = rOther.m_Values[1]; + } + + /** + * Returns the square of the length of the vector + * @return square of the length of the vector + */ + inline kt_double SquaredLength() const + { + return math::Square(m_Values[0]) + math::Square(m_Values[1]); + } + + /** + * Returns the length of the vector (x and y). + * @return length of the vector + */ + inline kt_double Length() const + { + return sqrt(SquaredLength()); + } + + /** + * Returns the square distance to the given vector + * @returns square distance to the given vector + */ + inline kt_double SquaredDistance(const Vector2& rOther) const + { + return (*this - rOther).SquaredLength(); + } + + /** + * Gets the distance to the other vector2 + * @param rOther + * @return distance to other vector2 + */ + inline kt_double Distance(const Vector2& rOther) const + { + return sqrt(SquaredDistance(rOther)); + } + + public: + /** + * In place Vector2 addition. + */ + inline void operator += (const Vector2& rOther) + { + m_Values[0] += rOther.m_Values[0]; + m_Values[1] += rOther.m_Values[1]; + } + + /** + * In place Vector2 subtraction. + */ + inline void operator -= (const Vector2& rOther) + { + m_Values[0] -= rOther.m_Values[0]; + m_Values[1] -= rOther.m_Values[1]; + } + + /** + * Addition operator + * @param rOther + * @return vector resulting from adding this vector with the given vector + */ + inline const Vector2 operator + (const Vector2& rOther) const + { + return Vector2(m_Values[0] + rOther.m_Values[0], m_Values[1] + rOther.m_Values[1]); + } + + /** + * Subtraction operator + * @param rOther + * @return vector resulting from subtracting this vector from the given vector + */ + inline const Vector2 operator - (const Vector2& rOther) const + { + return Vector2(m_Values[0] - rOther.m_Values[0], m_Values[1] - rOther.m_Values[1]); + } + + /** + * In place scalar division operator + * @param scalar + */ + inline void operator /= (T scalar) + { + m_Values[0] /= scalar; + m_Values[1] /= scalar; + } + + /** + * Divides a Vector2 + * @param scalar + * @return scalar product + */ + inline const Vector2 operator / (T scalar) const + { + return Vector2(m_Values[0] / scalar, m_Values[1] / scalar); + } + + /** + * Computes the dot product between the two vectors + * @param rOther + * @return dot product + */ + inline kt_double operator * (const Vector2& rOther) const + { + return m_Values[0] * rOther.m_Values[0] + m_Values[1] * rOther.m_Values[1]; + } + + /** + * Scales the vector by the given scalar + * @param scalar + */ + inline const Vector2 operator * (T scalar) const + { + return Vector2(m_Values[0] * scalar, m_Values[1] * scalar); + } + + /** + * Subtract the vector by the given scalar + * @param scalar + */ + inline const Vector2 operator - (T scalar) const + { + return Vector2(m_Values[0] - scalar, m_Values[1] - scalar); + } + + /** + * In place scalar multiplication operator + * @param scalar + */ + inline void operator *= (T scalar) + { + m_Values[0] *= scalar; + m_Values[1] *= scalar; + } + + /** + * Equality operator returns true if the corresponding x, y values of each Vector2 are the same values. + * @param rOther + */ + inline kt_bool operator == (const Vector2& rOther) const + { + return (m_Values[0] == rOther.m_Values[0] && m_Values[1] == rOther.m_Values[1]); + } + + /** + * Inequality operator returns true if any of the corresponding x, y values of each Vector2 not the same. + * @param rOther + */ + inline kt_bool operator != (const Vector2& rOther) const + { + return (m_Values[0] != rOther.m_Values[0] || m_Values[1] != rOther.m_Values[1]); + } + + /** + * Less than operator + * @param rOther + * @return true if left vector is less than right vector + */ + inline kt_bool operator < (const Vector2& rOther) const + { + if (m_Values[0] < rOther.m_Values[0]) + return true; + else if (m_Values[0] > rOther.m_Values[0]) + return false; + else + return (m_Values[1] < rOther.m_Values[1]); + } + + /** + * Write Vector2 onto output stream + * @param rStream output stream + * @param rVector to write + */ + friend inline std::ostream& operator << (std::ostream& rStream, const Vector2& rVector) + { + rStream << rVector.GetX() << " " << rVector.GetY(); + return rStream; + } + + /** + * Read Vector2 from input stream + * @param rStream input stream + */ + friend inline std::istream& operator >> (std::istream& rStream, const Vector2& /*rVector*/) + { + // Implement me!! TODO(lucbettaieb): What the what? Do I need to implement this? + return rStream; + } + + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + ar & boost::serialization::make_nvp("m_Values_0", m_Values[0]); + ar & boost::serialization::make_nvp("m_Values_1", m_Values[1]); + } + + private: + T m_Values[2]; + }; // Vector2 + + /** + * Type declaration of Vector2 vector + */ + typedef std::vector< Vector2 > PointVectorDouble; + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + /** + * Represents a vector (x, y, z) in 3-dimensional real space. + */ + template + class Vector3 + { + public: + /** + * Default constructor + */ + Vector3() + { + m_Values[0] = 0; + m_Values[1] = 0; + m_Values[2] = 0; + } + + /** + * Constructor initializing point location + * @param x + * @param y + * @param z + */ + Vector3(T x, T y, T z) + { + m_Values[0] = x; + m_Values[1] = y; + m_Values[2] = z; + } + + /** + * Copy constructor + * @param rOther + */ + Vector3(const Vector3& rOther) + { + m_Values[0] = rOther.m_Values[0]; + m_Values[1] = rOther.m_Values[1]; + m_Values[2] = rOther.m_Values[2]; + } + + public: + /** + * Gets the x-component of this vector + * @return x-component + */ + inline const T& GetX() const + { + return m_Values[0]; + } + + /** + * Sets the x-component of this vector + * @param x + */ + inline void SetX(const T& x) + { + m_Values[0] = x; + } + + /** + * Gets the y-component of this vector + * @return y-component + */ + inline const T& GetY() const + { + return m_Values[1]; + } + + /** + * Sets the y-component of this vector + * @param y + */ + inline void SetY(const T& y) + { + m_Values[1] = y; + } + + /** + * Gets the z-component of this vector + * @return z-component + */ + inline const T& GetZ() const + { + return m_Values[2]; + } + + /** + * Sets the z-component of this vector + * @param z + */ + inline void SetZ(const T& z) + { + m_Values[2] = z; + } + + /** + * Floor vector operator + * @param rOther Vector3d + */ + inline void MakeFloor(const Vector3& rOther) + { + if (rOther.m_Values[0] < m_Values[0]) m_Values[0] = rOther.m_Values[0]; + if (rOther.m_Values[1] < m_Values[1]) m_Values[1] = rOther.m_Values[1]; + if (rOther.m_Values[2] < m_Values[2]) m_Values[2] = rOther.m_Values[2]; + } + + /** + * Ceiling vector operator + * @param rOther Vector3d + */ + inline void MakeCeil(const Vector3& rOther) + { + if (rOther.m_Values[0] > m_Values[0]) m_Values[0] = rOther.m_Values[0]; + if (rOther.m_Values[1] > m_Values[1]) m_Values[1] = rOther.m_Values[1]; + if (rOther.m_Values[2] > m_Values[2]) m_Values[2] = rOther.m_Values[2]; + } + + /** + * Returns the square of the length of the vector + * @return square of the length of the vector + */ + inline kt_double SquaredLength() const + { + return math::Square(m_Values[0]) + math::Square(m_Values[1]) + math::Square(m_Values[2]); + } + + /** + * Returns the length of the vector. + * @return Length of the vector + */ + inline kt_double Length() const + { + return sqrt(SquaredLength()); + } + + /** + * Returns a string representation of this vector + * @return string representation of this vector + */ + inline std::string ToString() const + { + std::stringstream converter; + converter.precision(std::numeric_limits::digits10); + + converter << GetX() << " " << GetY() << " " << GetZ(); + + return converter.str(); + } + + public: + /** + * Assignment operator + */ + inline Vector3& operator = (const Vector3& rOther) + { + m_Values[0] = rOther.m_Values[0]; + m_Values[1] = rOther.m_Values[1]; + m_Values[2] = rOther.m_Values[2]; + + return *this; + } + + /** + * Binary vector add. + * @param rOther + * @return vector sum + */ + inline const Vector3 operator + (const Vector3& rOther) const + { + return Vector3(m_Values[0] + rOther.m_Values[0], + m_Values[1] + rOther.m_Values[1], + m_Values[2] + rOther.m_Values[2]); + } + + /** + * Binary vector add. + * @param scalar + * @return sum + */ + inline const Vector3 operator + (kt_double scalar) const + { + return Vector3(m_Values[0] + scalar, + m_Values[1] + scalar, + m_Values[2] + scalar); + } + + /** + * Binary vector subtract. + * @param rOther + * @return vector difference + */ + inline const Vector3 operator - (const Vector3& rOther) const + { + return Vector3(m_Values[0] - rOther.m_Values[0], + m_Values[1] - rOther.m_Values[1], + m_Values[2] - rOther.m_Values[2]); + } + + /** + * Binary vector subtract. + * @param scalar + * @return difference + */ + inline const Vector3 operator - (kt_double scalar) const + { + return Vector3(m_Values[0] - scalar, m_Values[1] - scalar, m_Values[2] - scalar); + } + + /** + * Scales the vector by the given scalar + * @param scalar + */ + inline const Vector3 operator * (T scalar) const + { + return Vector3(m_Values[0] * scalar, m_Values[1] * scalar, m_Values[2] * scalar); + } + + /** + * Equality operator returns true if the corresponding x, y, z values of each Vector3 are the same values. + * @param rOther + */ + inline kt_bool operator == (const Vector3& rOther) const + { + return (m_Values[0] == rOther.m_Values[0] && + m_Values[1] == rOther.m_Values[1] && + m_Values[2] == rOther.m_Values[2]); + } + + /** + * Inequality operator returns true if any of the corresponding x, y, z values of each Vector3 not the same. + * @param rOther + */ + inline kt_bool operator != (const Vector3& rOther) const + { + return (m_Values[0] != rOther.m_Values[0] || + m_Values[1] != rOther.m_Values[1] || + m_Values[2] != rOther.m_Values[2]); + } + + /** + * Write Vector3 onto output stream + * @param rStream output stream + * @param rVector to write + */ + friend inline std::ostream& operator << (std::ostream& rStream, const Vector3& rVector) + { + rStream << rVector.ToString(); + return rStream; + } + + /** + * Read Vector3 from input stream + * @param rStream input stream + */ + friend inline std::istream& operator >> (std::istream& rStream, const Vector3& /*rVector*/) + { + // Implement me!! + return rStream; + } + + private: + T m_Values[3]; + }; // Vector3 + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + /** + * Defines an orientation as a quaternion rotation using the positive Z axis as the zero reference. + *
+ * Q = w + ix + jy + kz
+ * w = c_1 * c_2 * c_3 - s_1 * s_2 * s_3
+ * x = s_1 * s_2 * c_3 + c_1 * c_2 * s_3
+ * y = s_1 * c_2 * c_3 + c_1 * s_2 * s_3
+ * z = c_1 * s_2 * c_3 - s_1 * c_2 * s_3
+ * where
+ * c_1 = cos(theta/2)
+ * c_2 = cos(phi/2)
+ * c_3 = cos(psi/2)
+ * s_1 = sin(theta/2)
+ * s_2 = sin(phi/2)
+ * s_3 = sin(psi/2)
+ * and
+ * theta is the angle of rotation about the Y axis measured from the Z axis.
+ * phi is the angle of rotation about the Z axis measured from the X axis.
+ * psi is the angle of rotation about the X axis measured from the Y axis.
+ * (All angles are right-handed.) + */ + class Quaternion + { + public: + /** + * Create a quaternion with default (x=0, y=0, z=0, w=1) values + */ + inline Quaternion() + { + m_Values[0] = 0.0; + m_Values[1] = 0.0; + m_Values[2] = 0.0; + m_Values[3] = 1.0; + } + + /** + * Create a quaternion using x, y, z, w values. + * @param x + * @param y + * @param z + * @param w + */ + inline Quaternion(kt_double x, kt_double y, kt_double z, kt_double w) + { + m_Values[0] = x; + m_Values[1] = y; + m_Values[2] = z; + m_Values[3] = w; + } + + /** + * Copy constructor + */ + inline Quaternion(const Quaternion& rQuaternion) + { + m_Values[0] = rQuaternion.m_Values[0]; + m_Values[1] = rQuaternion.m_Values[1]; + m_Values[2] = rQuaternion.m_Values[2]; + m_Values[3] = rQuaternion.m_Values[3]; + } + + public: + /** + * Returns the X-value + * @return Return the X-value of the quaternion + */ + inline kt_double GetX() const + { + return m_Values[0]; + } + + /** + * Sets the X-value + * @param x X-value of the quaternion + */ + inline void SetX(kt_double x) + { + m_Values[0] = x; + } + + /** + * Returns the Y-value + * @return Return the Y-value of the quaternion + */ + inline kt_double GetY() const + { + return m_Values[1]; + } + + /** + * Sets the Y-value + * @param y Y-value of the quaternion + */ + inline void SetY(kt_double y) + { + m_Values[1] = y; + } + + /** + * Returns the Z-value + * @return Return the Z-value of the quaternion + */ + inline kt_double GetZ() const + { + return m_Values[2]; + } + + /** + * Sets the Z-value + * @param z Z-value of the quaternion + */ + inline void SetZ(kt_double z) + { + m_Values[2] = z; + } + + /** + * Returns the W-value + * @return Return the W-value of the quaternion + */ + inline kt_double GetW() const + { + return m_Values[3]; + } + + /** + * Sets the W-value + * @param w W-value of the quaternion + */ + inline void SetW(kt_double w) + { + m_Values[3] = w; + } + + /** + * Converts this quaternion into Euler angles + * Source: http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/index.htm + * @param rYaw + * @param rPitch + * @param rRoll + */ + void ToEulerAngles(kt_double& rYaw, kt_double& rPitch, kt_double& rRoll) const + { + kt_double test = m_Values[0] * m_Values[1] + m_Values[2] * m_Values[3]; + + if (test > 0.499) + { + // singularity at north pole + rYaw = 2 * atan2(m_Values[0], m_Values[3]);; + rPitch = KT_PI_2; + rRoll = 0; + } + else if (test < -0.499) + { + // singularity at south pole + rYaw = -2 * atan2(m_Values[0], m_Values[3]); + rPitch = -KT_PI_2; + rRoll = 0; + } + else + { + kt_double sqx = m_Values[0] * m_Values[0]; + kt_double sqy = m_Values[1] * m_Values[1]; + kt_double sqz = m_Values[2] * m_Values[2]; + + rYaw = atan2(2 * m_Values[1] * m_Values[3] - 2 * m_Values[0] * m_Values[2], 1 - 2 * sqy - 2 * sqz); + rPitch = asin(2 * test); + rRoll = atan2(2 * m_Values[0] * m_Values[3] - 2 * m_Values[1] * m_Values[2], 1 - 2 * sqx - 2 * sqz); + } + } + + /** + * Set x,y,z,w values of the quaternion based on Euler angles. + * Source: http://www.euclideanspace.com/maths/geometry/rotations/conversions/eulerToQuaternion/index.htm + * @param yaw + * @param pitch + * @param roll + */ + void FromEulerAngles(kt_double yaw, kt_double pitch, kt_double roll) + { + kt_double angle; + + angle = yaw * 0.5; + kt_double cYaw = cos(angle); + kt_double sYaw = sin(angle); + + angle = pitch * 0.5; + kt_double cPitch = cos(angle); + kt_double sPitch = sin(angle); + + angle = roll * 0.5; + kt_double cRoll = cos(angle); + kt_double sRoll = sin(angle); + + m_Values[0] = sYaw * sPitch * cRoll + cYaw * cPitch * sRoll; + m_Values[1] = sYaw * cPitch * cRoll + cYaw * sPitch * sRoll; + m_Values[2] = cYaw * sPitch * cRoll - sYaw * cPitch * sRoll; + m_Values[3] = cYaw * cPitch * cRoll - sYaw * sPitch * sRoll; + } + + /** + * Assignment operator + * @param rQuaternion + */ + inline Quaternion& operator = (const Quaternion& rQuaternion) + { + m_Values[0] = rQuaternion.m_Values[0]; + m_Values[1] = rQuaternion.m_Values[1]; + m_Values[2] = rQuaternion.m_Values[2]; + m_Values[3] = rQuaternion.m_Values[3]; + + return(*this); + } + + /** + * Equality operator returns true if the corresponding x, y, z, w values of each quaternion are the same values. + * @param rOther + */ + inline kt_bool operator == (const Quaternion& rOther) const + { + return (m_Values[0] == rOther.m_Values[0] && + m_Values[1] == rOther.m_Values[1] && + m_Values[2] == rOther.m_Values[2] && + m_Values[3] == rOther.m_Values[3]); + } + + /** + * Inequality operator returns true if any of the corresponding x, y, z, w values of each quaternion not the same. + * @param rOther + */ + inline kt_bool operator != (const Quaternion& rOther) const + { + return (m_Values[0] != rOther.m_Values[0] || + m_Values[1] != rOther.m_Values[1] || + m_Values[2] != rOther.m_Values[2] || + m_Values[3] != rOther.m_Values[3]); + } + + /** + * Write this quaternion onto output stream + * @param rStream output stream + * @param rQuaternion + */ + friend inline std::ostream& operator << (std::ostream& rStream, const Quaternion& rQuaternion) + { + rStream << rQuaternion.m_Values[0] << " " + << rQuaternion.m_Values[1] << " " + << rQuaternion.m_Values[2] << " " + << rQuaternion.m_Values[3]; + return rStream; + } + + private: + kt_double m_Values[4]; + }; + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + /** + * Stores x, y, width and height that represents the location and size of a rectangle + * (x, y) is at bottom left in mapper! + */ + template + class Rectangle2 + { + public: + /** + * Default constructor + */ + Rectangle2() + { + } + + /** + * Constructor initializing rectangle parameters + * @param x x-coordinate of left edge of rectangle + * @param y y-coordinate of bottom edge of rectangle + * @param width width of rectangle + * @param height height of rectangle + */ + Rectangle2(T x, T y, T width, T height) + : m_Position(x, y) + , m_Size(width, height) + { + } + + /** + * Constructor initializing rectangle parameters + * @param rPosition (x,y)-coordinate of rectangle + * @param rSize Size of the rectangle + */ + Rectangle2(const Vector2& rPosition, const Size2& rSize) + : m_Position(rPosition) + , m_Size(rSize) + { + } + + /** + * Copy constructor + */ + Rectangle2(const Rectangle2& rOther) + : m_Position(rOther.m_Position) + , m_Size(rOther.m_Size) + { + } + + public: + /** + * Gets the x-coordinate of the left edge of this rectangle + * @return the x-coordinate of the left edge of this rectangle + */ + inline T GetX() const + { + return m_Position.GetX(); + } + + /** + * Sets the x-coordinate of the left edge of this rectangle + * @param x the x-coordinate of the left edge of this rectangle + */ + inline void SetX(T x) + { + m_Position.SetX(x); + } + + /** + * Gets the y-coordinate of the bottom edge of this rectangle + * @return the y-coordinate of the bottom edge of this rectangle + */ + inline T GetY() const + { + return m_Position.GetY(); + } + + /** + * Sets the y-coordinate of the bottom edge of this rectangle + * @param y the y-coordinate of the bottom edge of this rectangle + */ + inline void SetY(T y) + { + m_Position.SetY(y); + } + + /** + * Gets the width of this rectangle + * @return the width of this rectangle + */ + inline T GetWidth() const + { + return m_Size.GetWidth(); + } + + /** + * Sets the width of this rectangle + * @param width the width of this rectangle + */ + inline void SetWidth(T width) + { + m_Size.SetWidth(width); + } + + /** + * Gets the height of this rectangle + * @return the height of this rectangle + */ + inline T GetHeight() const + { + return m_Size.GetHeight(); + } + + /** + * Sets the height of this rectangle + * @param height the height of this rectangle + */ + inline void SetHeight(T height) + { + m_Size.SetHeight(height); + } + + /** + * Gets the position of this rectangle + * @return the position of this rectangle + */ + inline const Vector2& GetPosition() const + { + return m_Position; + } + + /** + * Sets the position of this rectangle + * @param rX x + * @param rY y + */ + inline void SetPosition(const T& rX, const T& rY) + { + m_Position = Vector2(rX, rY); + } + + /** + * Sets the position of this rectangle + * @param rPosition position + */ + inline void SetPosition(const Vector2& rPosition) + { + m_Position = rPosition; + } + + /** + * Gets the size of this rectangle + * @return the size of this rectangle + */ + inline const Size2& GetSize() const + { + return m_Size; + } + + /** + * Sets the size of this rectangle + * @param rSize size + */ + inline void SetSize(const Size2& rSize) + { + m_Size = rSize; + } + + /** + * Gets the center of this rectangle + * @return the center of this rectangle + */ + inline const Vector2 GetCenter() const + { + return Vector2(m_Position.GetX() + m_Size.GetWidth() * 0.5, m_Position.GetY() + m_Size.GetHeight() * 0.5); + } + + public: + /** + * Assignment operator + */ + Rectangle2& operator = (const Rectangle2& rOther) + { + m_Position = rOther.m_Position; + m_Size = rOther.m_Size; + + return *this; + } + + /** + * Equality operator + */ + inline kt_bool operator == (const Rectangle2& rOther) const + { + return (m_Position == rOther.m_Position && m_Size == rOther.m_Size); + } + + /** + * Inequality operator + */ + inline kt_bool operator != (const Rectangle2& rOther) const + { + return (m_Position != rOther.m_Position || m_Size != rOther.m_Size); + } + + private: + Vector2 m_Position; + Size2 m_Size; + /** + * Serialization: class Rectangle2 + */ + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + ar & BOOST_SERIALIZATION_NVP(m_Position); + ar & BOOST_SERIALIZATION_NVP(m_Size); + } + }; // Rectangle2 + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + class Pose3; + + /** + * Defines a position (x, y) in 2-dimensional space and heading. + */ + class Pose2 + { + public: + /** + * Default Constructor + */ + Pose2() + : m_Heading(0.0) + { + } + + /** + * Constructor initializing pose parameters + * @param rPosition position + * @param heading heading + **/ + Pose2(const Vector2& rPosition, kt_double heading) + : m_Position(rPosition) + , m_Heading(heading) + { + } + + /** + * Constructor initializing pose parameters + * @param x x-coordinate + * @param y y-coordinate + * @param heading heading + **/ + Pose2(kt_double x, kt_double y, kt_double heading) + : m_Position(x, y) + , m_Heading(heading) + { + } + + /** + * Constructs a Pose2 object from a Pose3. + */ + Pose2(const Pose3& rPose); + + /** + * Copy constructor + */ + Pose2(const Pose2& rOther) + : m_Position(rOther.m_Position) + , m_Heading(rOther.m_Heading) + { + } + + public: + /** + * Returns the x-coordinate + * @return the x-coordinate of the pose + */ + inline kt_double GetX() const + { + return m_Position.GetX(); + } + + /** + * Sets the x-coordinate + * @param x the x-coordinate of the pose + */ + inline void SetX(kt_double x) + { + m_Position.SetX(x); + } + + /** + * Returns the y-coordinate + * @return the y-coordinate of the pose + */ + inline kt_double GetY() const + { + return m_Position.GetY(); + } + + /** + * Sets the y-coordinate + * @param y the y-coordinate of the pose + */ + inline void SetY(kt_double y) + { + m_Position.SetY(y); + } + + /** + * Returns the position + * @return the position of the pose + */ + inline const Vector2& GetPosition() const + { + return m_Position; + } + + /** + * Sets the position + * @param rPosition of the pose + */ + inline void SetPosition(const Vector2& rPosition) + { + m_Position = rPosition; + } + + /** + * Returns the heading of the pose (in radians) + * @return the heading of the pose + */ + inline kt_double GetHeading() const + { + return m_Heading; + } + + /** + * Sets the heading + * @param heading of the pose + */ + inline void SetHeading(kt_double heading) + { + m_Heading = heading; + } + + /** + * Return the squared distance between two Pose2 + * @return squared distance + */ + inline kt_double SquaredDistance(const Pose2& rOther) const + { + return m_Position.SquaredDistance(rOther.m_Position); + } + + public: + /** + * Assignment operator + */ + inline Pose2& operator = (const Pose2& rOther) + { + m_Position = rOther.m_Position; + m_Heading = rOther.m_Heading; + + return *this; + } + + /** + * Equality operator + */ + inline kt_bool operator == (const Pose2& rOther) const + { + return (m_Position == rOther.m_Position && m_Heading == rOther.m_Heading); + } + + /** + * Inequality operator + */ + inline kt_bool operator != (const Pose2& rOther) const + { + return (m_Position != rOther.m_Position || m_Heading != rOther.m_Heading); + } + + /** + * In place Pose2 add. + */ + inline void operator += (const Pose2& rOther) + { + m_Position += rOther.m_Position; + m_Heading = math::NormalizeAngle(m_Heading + rOther.m_Heading); + } + + /** + * Binary Pose2 add + * @param rOther + * @return Pose2 sum + */ + inline Pose2 operator + (const Pose2& rOther) const + { + return Pose2(m_Position + rOther.m_Position, math::NormalizeAngle(m_Heading + rOther.m_Heading)); + } + + /** + * Binary Pose2 subtract + * @param rOther + * @return Pose2 difference + */ + inline Pose2 operator - (const Pose2& rOther) const + { + return Pose2(m_Position - rOther.m_Position, math::NormalizeAngle(m_Heading - rOther.m_Heading)); + } + + /** + * Read pose from input stream + * @param rStream input stream + */ + friend inline std::istream& operator >> (std::istream& rStream, const Pose2& /*rPose*/) + { + // Implement me!! + return rStream; + } + + /** + * Write this pose onto output stream + * @param rStream output stream + * @param rPose to read + */ + friend inline std::ostream& operator << (std::ostream& rStream, const Pose2& rPose) + { + rStream << rPose.m_Position.GetX() << " " << rPose.m_Position.GetY() << " " << rPose.m_Heading; + return rStream; + } + + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + ar & BOOST_SERIALIZATION_NVP(m_Position); + ar & BOOST_SERIALIZATION_NVP(m_Heading); + } + + private: + Vector2 m_Position; + + kt_double m_Heading; + }; // Pose2 + + /** + * Type declaration of Pose2 vector + */ + typedef std::vector< Pose2 > Pose2Vector; + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + /** + * Defines a position and orientation in 3-dimensional space. + * Karto uses a right-handed coordinate system with X, Y as the 2-D ground plane and X is forward and Y is left. + * Values in Vector3 used to define position must have units of meters. + * The value of angle when defining orientation in two dimensions must be in units of radians. + * The definition of orientation in three dimensions uses quaternions. + */ + class Pose3 + { + public: + /** + * Default constructor + */ + Pose3() + { + } + + /** + * Create a new Pose3 object from the given position. + * @param rPosition position vector in three space. + */ + Pose3(const Vector3& rPosition) + : m_Position(rPosition) + { + } + + /** + * Create a new Pose3 object from the given position and orientation. + * @param rPosition position vector in three space. + * @param rOrientation quaternion orientation in three space. + */ + Pose3(const Vector3& rPosition, const karto::Quaternion& rOrientation) + : m_Position(rPosition) + , m_Orientation(rOrientation) + { + } + + /** + * Copy constructor + */ + Pose3(const Pose3& rOther) + : m_Position(rOther.m_Position) + , m_Orientation(rOther.m_Orientation) + { + } + + /** + * Constructs a Pose3 object from a Pose2. + */ + Pose3(const Pose2& rPose) + { + m_Position = Vector3(rPose.GetX(), rPose.GetY(), 0.0); + m_Orientation.FromEulerAngles(rPose.GetHeading(), 0.0, 0.0); + } + + public: + /** + * Get the position of the pose as a 3D vector as const. Values have units of meters. + * @return 3-dimensional position vector as const + */ + inline const Vector3& GetPosition() const + { + return m_Position; + } + + /** + * Set the position of the pose as a 3D vector. Values have units of meters. + * @return 3-dimensional position vector + */ + inline void SetPosition(const Vector3& rPosition) + { + m_Position = rPosition; + } + + /** + * Get the orientation quaternion of the pose as const. + * @return orientation quaternion as const + */ + inline const Quaternion& GetOrientation() const + { + return m_Orientation; + } + + /** + * Get the orientation quaternion of the pose. + * @return orientation quaternion + */ + inline void SetOrientation(const Quaternion& rOrientation) + { + m_Orientation = rOrientation; + } + + /** + * Returns a string representation of this pose + * @return string representation of this pose + */ + inline std::string ToString() + { + std::stringstream converter; + converter.precision(std::numeric_limits::digits10); + + converter << GetPosition() << " " << GetOrientation(); + + return converter.str(); + } + + public: + /** + * Assignment operator + */ + inline Pose3& operator = (const Pose3& rOther) + { + m_Position = rOther.m_Position; + m_Orientation = rOther.m_Orientation; + + return *this; + } + + /** + * Equality operator + */ + inline kt_bool operator == (const Pose3& rOther) const + { + return (m_Position == rOther.m_Position && m_Orientation == rOther.m_Orientation); + } + + /** + * Inequality operator + */ + inline kt_bool operator != (const Pose3& rOther) const + { + return (m_Position != rOther.m_Position || m_Orientation != rOther.m_Orientation); + } + + /** + * Write Pose3 onto output stream + * @param rStream output stream + * @param rPose to write + */ + friend inline std::ostream& operator << (std::ostream& rStream, const Pose3& rPose) + { + rStream << rPose.GetPosition() << ", " << rPose.GetOrientation(); + return rStream; + } + + /** + * Read Pose3 from input stream + * @param rStream input stream + */ + friend inline std::istream& operator >> (std::istream& rStream, const Pose3& /*rPose*/) + { + // Implement me!! + return rStream; + } + + private: + Vector3 m_Position; + Quaternion m_Orientation; + }; // Pose3 + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + /** + * Defines a Matrix 3 x 3 class. + */ + class Matrix3 + { + public: + /** + * Default constructor + */ + Matrix3() + { + Clear(); + } + + /** + * Copy constructor + */ + inline Matrix3(const Matrix3& rOther) + { + memcpy(m_Matrix, rOther.m_Matrix, 9*sizeof(kt_double)); + } + + public: + /** + * Sets this matrix to identity matrix + */ + void SetToIdentity() + { + memset(m_Matrix, 0, 9*sizeof(kt_double)); + + for (kt_int32s i = 0; i < 3; i++) + { + m_Matrix[i][i] = 1.0; + } + } + + /** + * Sets this matrix to zero matrix + */ + void Clear() + { + memset(m_Matrix, 0, 9*sizeof(kt_double)); + } + + /** + * Sets this matrix to be the rotation matrix of rotation around given axis + * @param x x-coordinate of axis + * @param y y-coordinate of axis + * @param z z-coordinate of axis + * @param radians amount of rotation + */ + void FromAxisAngle(kt_double x, kt_double y, kt_double z, const kt_double radians) + { + kt_double cosRadians = cos(radians); + kt_double sinRadians = sin(radians); + kt_double oneMinusCos = 1.0 - cosRadians; + + kt_double xx = x * x; + kt_double yy = y * y; + kt_double zz = z * z; + + kt_double xyMCos = x * y * oneMinusCos; + kt_double xzMCos = x * z * oneMinusCos; + kt_double yzMCos = y * z * oneMinusCos; + + kt_double xSin = x * sinRadians; + kt_double ySin = y * sinRadians; + kt_double zSin = z * sinRadians; + + m_Matrix[0][0] = xx * oneMinusCos + cosRadians; + m_Matrix[0][1] = xyMCos - zSin; + m_Matrix[0][2] = xzMCos + ySin; + + m_Matrix[1][0] = xyMCos + zSin; + m_Matrix[1][1] = yy * oneMinusCos + cosRadians; + m_Matrix[1][2] = yzMCos - xSin; + + m_Matrix[2][0] = xzMCos - ySin; + m_Matrix[2][1] = yzMCos + xSin; + m_Matrix[2][2] = zz * oneMinusCos + cosRadians; + } + + /** + * Returns transposed version of this matrix + * @return transposed matrix + */ + Matrix3 Transpose() const + { + Matrix3 transpose; + + for (kt_int32u row = 0; row < 3; row++) + { + for (kt_int32u col = 0; col < 3; col++) + { + transpose.m_Matrix[row][col] = m_Matrix[col][row]; + } + } + + return transpose; + } + + /** + * Returns the inverse of the matrix + */ + Matrix3 Inverse() const + { + Matrix3 kInverse = *this; + kt_bool haveInverse = InverseFast(kInverse, 1e-14); + if (haveInverse == false) + { + assert(false); + } + return kInverse; + } + + /** + * Internal helper method for inverse matrix calculation + * This code is lifted from the OgreMatrix3 class!! + */ + kt_bool InverseFast(Matrix3& rkInverse, kt_double fTolerance = KT_TOLERANCE) const + { + // Invert a 3x3 using cofactors. This is about 8 times faster than + // the Numerical Recipes code which uses Gaussian elimination. + rkInverse.m_Matrix[0][0] = m_Matrix[1][1]*m_Matrix[2][2] - m_Matrix[1][2]*m_Matrix[2][1]; + rkInverse.m_Matrix[0][1] = m_Matrix[0][2]*m_Matrix[2][1] - m_Matrix[0][1]*m_Matrix[2][2]; + rkInverse.m_Matrix[0][2] = m_Matrix[0][1]*m_Matrix[1][2] - m_Matrix[0][2]*m_Matrix[1][1]; + rkInverse.m_Matrix[1][0] = m_Matrix[1][2]*m_Matrix[2][0] - m_Matrix[1][0]*m_Matrix[2][2]; + rkInverse.m_Matrix[1][1] = m_Matrix[0][0]*m_Matrix[2][2] - m_Matrix[0][2]*m_Matrix[2][0]; + rkInverse.m_Matrix[1][2] = m_Matrix[0][2]*m_Matrix[1][0] - m_Matrix[0][0]*m_Matrix[1][2]; + rkInverse.m_Matrix[2][0] = m_Matrix[1][0]*m_Matrix[2][1] - m_Matrix[1][1]*m_Matrix[2][0]; + rkInverse.m_Matrix[2][1] = m_Matrix[0][1]*m_Matrix[2][0] - m_Matrix[0][0]*m_Matrix[2][1]; + rkInverse.m_Matrix[2][2] = m_Matrix[0][0]*m_Matrix[1][1] - m_Matrix[0][1]*m_Matrix[1][0]; + + kt_double fDet = m_Matrix[0][0]*rkInverse.m_Matrix[0][0] + + m_Matrix[0][1]*rkInverse.m_Matrix[1][0] + + m_Matrix[0][2]*rkInverse.m_Matrix[2][0]; + + if (fabs(fDet) <= fTolerance) + { + return false; + } + + kt_double fInvDet = 1.0/fDet; + for (size_t row = 0; row < 3; row++) + { + for (size_t col = 0; col < 3; col++) + { + rkInverse.m_Matrix[row][col] *= fInvDet; + } + } + + return true; + } + + /** + * Returns a string representation of this matrix + * @return string representation of this matrix + */ + inline std::string ToString() const + { + std::stringstream converter; + converter.precision(std::numeric_limits::digits10); + + for (int row = 0; row < 3; row++) + { + for (int col = 0; col < 3; col++) + { + converter << m_Matrix[row][col] << " "; + } + } + + return converter.str(); + } + + public: + /** + * Assignment operator + */ + inline Matrix3& operator = (const Matrix3& rOther) + { + memcpy(m_Matrix, rOther.m_Matrix, 9*sizeof(kt_double)); + return *this; + } + + /** + * Matrix element access, allows use of construct mat(r, c) + * @param row + * @param column + * @return reference to mat(r,c) + */ + inline kt_double& operator()(kt_int32u row, kt_int32u column) + { + return m_Matrix[row][column]; + } + + /** + * Read-only matrix element access, allows use of construct mat(r, c) + * @param row + * @param column + * @return mat(r,c) + */ + inline kt_double operator()(kt_int32u row, kt_int32u column) const + { + return m_Matrix[row][column]; + } + + /** + * Binary Matrix3 multiplication. + * @param rOther + * @return Matrix3 product + */ + Matrix3 operator * (const Matrix3& rOther) const + { + Matrix3 product; + + for (size_t row = 0; row < 3; row++) + { + for (size_t col = 0; col < 3; col++) + { + product.m_Matrix[row][col] = m_Matrix[row][0]*rOther.m_Matrix[0][col] + + m_Matrix[row][1]*rOther.m_Matrix[1][col] + + m_Matrix[row][2]*rOther.m_Matrix[2][col]; + } + } + + return product; + } + + /** + * Matrix3 and Pose2 multiplication - matrix * pose [3x3 * 3x1 = 3x1] + * @param rPose2 + * @return Pose2 product + */ + inline Pose2 operator * (const Pose2& rPose2) const + { + Pose2 pose2; + + pose2.SetX(m_Matrix[0][0] * rPose2.GetX() + m_Matrix[0][1] * + rPose2.GetY() + m_Matrix[0][2] * rPose2.GetHeading()); + pose2.SetY(m_Matrix[1][0] * rPose2.GetX() + m_Matrix[1][1] * + rPose2.GetY() + m_Matrix[1][2] * rPose2.GetHeading()); + pose2.SetHeading(m_Matrix[2][0] * rPose2.GetX() + m_Matrix[2][1] * + rPose2.GetY() + m_Matrix[2][2] * rPose2.GetHeading()); + + return pose2; + } + + /** + * In place Matrix3 add. + * @param rkMatrix + */ + inline void operator += (const Matrix3& rkMatrix) + { + for (kt_int32u row = 0; row < 3; row++) + { + for (kt_int32u col = 0; col < 3; col++) + { + m_Matrix[row][col] += rkMatrix.m_Matrix[row][col]; + } + } + } + + /** + * Write Matrix3 onto output stream + * @param rStream output stream + * @param rMatrix to write + */ + friend inline std::ostream& operator << (std::ostream& rStream, const Matrix3& rMatrix) + { + rStream << rMatrix.ToString(); + return rStream; + } + + private: + kt_double m_Matrix[3][3]; + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + ar & BOOST_SERIALIZATION_NVP(m_Matrix); + } + }; // Matrix3 + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + /** + * Defines a general Matrix class. + */ + class Matrix + { + public: + /** + * Constructs a matrix of size rows x columns + */ + Matrix(kt_int32u rows, kt_int32u columns) + : m_Rows(rows) + , m_Columns(columns) + , m_pData(NULL) + { + Allocate(); + + Clear(); + } + + /** + * Destructor + */ + virtual ~Matrix() + { + delete [] m_pData; + } + + public: + /** + * Set all entries to 0 + */ + void Clear() + { + if (m_pData != NULL) + { + memset(m_pData, 0, sizeof(kt_double) * m_Rows * m_Columns); + } + } + + /** + * Gets the number of rows of the matrix + * @return nubmer of rows + */ + inline kt_int32u GetRows() const + { + return m_Rows; + } + + /** + * Gets the number of columns of the matrix + * @return nubmer of columns + */ + inline kt_int32u GetColumns() const + { + return m_Columns; + } + + /** + * Returns a reference to the entry at (row,column) + * @param row + * @param column + * @return reference to entry at (row,column) + */ + inline kt_double& operator()(kt_int32u row, kt_int32u column) + { + RangeCheck(row, column); + + return m_pData[row + column * m_Rows]; + } + + /** + * Returns a const reference to the entry at (row,column) + * @param row + * @param column + * @return const reference to entry at (row,column) + */ + inline const kt_double& operator()(kt_int32u row, kt_int32u column) const + { + RangeCheck(row, column); + + return m_pData[row + column * m_Rows]; + } + + private: + /** + * Allocate space for the matrix + */ + void Allocate() + { + try + { + if (m_pData != NULL) + { + delete[] m_pData; + } + + m_pData = new kt_double[m_Rows * m_Columns]; + } + catch (const std::bad_alloc& ex) + { + throw Exception("Matrix allocation error"); + } + + if (m_pData == NULL) + { + throw Exception("Matrix allocation error"); + } + } + + /** + * Checks if (row,column) is a valid entry into the matrix + * @param row + * @param column + */ + inline void RangeCheck(kt_int32u row, kt_int32u column) const + { + if (math::IsUpTo(row, m_Rows) == false) + { + throw Exception("Matrix - RangeCheck ERROR!!!!"); + } + + if (math::IsUpTo(column, m_Columns) == false) + { + throw Exception("Matrix - RangeCheck ERROR!!!!"); + } + } + + private: + kt_int32u m_Rows; + kt_int32u m_Columns; + + kt_double* m_pData; + }; // Matrix + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + /** + * Defines a bounding box in 2-dimensional real space. + */ + class BoundingBox2 + { + public: + /* + * Default constructor + */ + BoundingBox2() + : m_Minimum(999999999999999999.99999, 999999999999999999.99999) + , m_Maximum(-999999999999999999.99999, -999999999999999999.99999) + { + } + + public: + /** + * Get bounding box minimum + */ + inline const Vector2& GetMinimum() const + { + return m_Minimum; + } + + /** + * Set bounding box minimum + */ + inline void SetMinimum(const Vector2& mMinimum) + { + m_Minimum = mMinimum; + } + + /** + * Get bounding box maximum + */ + inline const Vector2& GetMaximum() const + { + return m_Maximum; + } + + /** + * Set bounding box maximum + */ + inline void SetMaximum(const Vector2& rMaximum) + { + m_Maximum = rMaximum; + } + + /** + * Get the size of the bounding box + */ + inline Size2 GetSize() const + { + Vector2 size = m_Maximum - m_Minimum; + + return Size2(size.GetX(), size.GetY()); + } + + /** + * Add vector to bounding box + */ + inline void Add(const Vector2& rPoint) + { + m_Minimum.MakeFloor(rPoint); + m_Maximum.MakeCeil(rPoint); + } + + /** + * Add other bounding box to bounding box + */ + inline void Add(const BoundingBox2& rBoundingBox) + { + Add(rBoundingBox.GetMinimum()); + Add(rBoundingBox.GetMaximum()); + } + + /** + * Whether the given point is in the bounds of this box + * @param rPoint + * @return in bounds? + */ + inline kt_bool IsInBounds(const Vector2& rPoint) const + { + return (math::InRange(rPoint.GetX(), m_Minimum.GetX(), m_Maximum.GetX()) && + math::InRange(rPoint.GetY(), m_Minimum.GetY(), m_Maximum.GetY())); + } + + /** + * Serialization: class BoundingBox2 + */ + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + ar & BOOST_SERIALIZATION_NVP(m_Minimum); + ar & BOOST_SERIALIZATION_NVP(m_Maximum); + } + + private: + Vector2 m_Minimum; + Vector2 m_Maximum; + }; // BoundingBox2 + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + /** + * Implementation of a Pose2 transform + */ + class Transform + { + public: + /** + * Constructs a transformation from the origin to the given pose + * @param rPose pose + */ + Transform(const Pose2& rPose) + { + SetTransform(Pose2(), rPose); + } + + /** + * Constructs a transformation from the first pose to the second pose + * @param rPose1 first pose + * @param rPose2 second pose + */ + Transform(const Pose2& rPose1, const Pose2& rPose2) + { + SetTransform(rPose1, rPose2); + } + + public: + /** + * Transforms the pose according to this transform + * @param rSourcePose pose to transform from + * @return transformed pose + */ + inline Pose2 TransformPose(const Pose2& rSourcePose) + { + Pose2 newPosition = m_Transform + m_Rotation * rSourcePose; + kt_double angle = math::NormalizeAngle(rSourcePose.GetHeading() + m_Transform.GetHeading()); + + return Pose2(newPosition.GetPosition(), angle); + } + + /** + * Inverse transformation of the pose according to this transform + * @param rSourcePose pose to transform from + * @return transformed pose + */ + inline Pose2 InverseTransformPose(const Pose2& rSourcePose) + { + Pose2 newPosition = m_InverseRotation * (rSourcePose - m_Transform); + kt_double angle = math::NormalizeAngle(rSourcePose.GetHeading() - m_Transform.GetHeading()); + + // components of transform + return Pose2(newPosition.GetPosition(), angle); + } + + private: + /** + * Sets this to be the transformation from the first pose to the second pose + * @param rPose1 first pose + * @param rPose2 second pose + */ + void SetTransform(const Pose2& rPose1, const Pose2& rPose2) + { + if (rPose1 == rPose2) + { + m_Rotation.SetToIdentity(); + m_InverseRotation.SetToIdentity(); + m_Transform = Pose2(); + return; + } + + // heading transformation + m_Rotation.FromAxisAngle(0, 0, 1, rPose2.GetHeading() - rPose1.GetHeading()); + m_InverseRotation.FromAxisAngle(0, 0, 1, rPose1.GetHeading() - rPose2.GetHeading()); + + // position transformation + Pose2 newPosition; + if (rPose1.GetX() != 0.0 || rPose1.GetY() != 0.0) + { + newPosition = rPose2 - m_Rotation * rPose1; + } + else + { + newPosition = rPose2; + } + + m_Transform = Pose2(newPosition.GetPosition(), rPose2.GetHeading() - rPose1.GetHeading()); + } + + private: + // pose transformation + Pose2 m_Transform; + + Matrix3 m_Rotation; + Matrix3 m_InverseRotation; + + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + ar & BOOST_SERIALIZATION_NVP(m_Transform); + ar & BOOST_SERIALIZATION_NVP(m_Rotation); + ar & BOOST_SERIALIZATION_NVP(m_InverseRotation); + } + }; // Transform + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + /** + * Enumerated type for valid LaserRangeFinder types + */ + typedef enum + { + LaserRangeFinder_Custom = 0, + + LaserRangeFinder_Sick_LMS100 = 1, + LaserRangeFinder_Sick_LMS200 = 2, + LaserRangeFinder_Sick_LMS291 = 3, + + LaserRangeFinder_Hokuyo_UTM_30LX = 4, + LaserRangeFinder_Hokuyo_URG_04LX = 5 + } LaserRangeFinderType; + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + /** + * Abstract base class for Parameters + */ + class AbstractParameter + { + + public: + AbstractParameter() + { + } + /** + * Constructs a parameter with the given name + * @param rName + * @param pParameterManger + */ + AbstractParameter(const std::string& rName, ParameterManager* pParameterManger = NULL) + : m_Name(rName) + { + // if parameter manager is provided add myself to it! + if (pParameterManger != NULL) + { + pParameterManger->Add(this); + } + } + + /** + * Constructs a parameter with the given name and description + * @param rName + * @param rDescription + * @param pParameterManger + */ + AbstractParameter(const std::string& rName, + const std::string& rDescription, + ParameterManager* pParameterManger = NULL) + : m_Name(rName) + , m_Description(rDescription) + { + // if parameter manager is provided add myself to it! + if (pParameterManger != NULL) + { + pParameterManger->Add(this); + } + } + + /** + * Destructor + */ + virtual ~AbstractParameter() + { + } + + public: + /** + * Gets the name of this object + * @return name + */ + inline const std::string& GetName() const + { + return m_Name; + } + + /** + * Returns the parameter description + * @return parameter description + */ + inline const std::string& GetDescription() const + { + return m_Description; + } + + /** + * Get parameter value as string. + * @return value as string + */ + virtual const std::string GetValueAsString() const = 0; + + /** + * Set parameter value from string. + * @param rStringValue value as string + */ + virtual void SetValueFromString(const std::string& rStringValue) = 0; + + /** + * Clones the parameter + * @return clone + */ + virtual AbstractParameter* Clone() = 0; + + public: + /** + * Write this parameter onto output stream + * @param rStream output stream + * @param rParameter + */ + friend std::ostream& operator << (std::ostream& rStream, const AbstractParameter& rParameter) + { + rStream.precision(6); + rStream.flags(std::ios::fixed); + + rStream << rParameter.GetName() << " = " << rParameter.GetValueAsString(); + return rStream; + } + + private: + std::string m_Name; + std::string m_Description; + /** + * Serialization: class AbstractParameter + */ + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + ar & BOOST_SERIALIZATION_NVP(m_Name); + ar & BOOST_SERIALIZATION_NVP(m_Description); + } + }; // AbstractParameter + BOOST_SERIALIZATION_ASSUME_ABSTRACT(AbstractParameter) + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + /** + * Parameter class + */ + template + class Parameter : public AbstractParameter + { + public: + /** + * Parameter with given name and value + * @param rName + * @param value + * @param pParameterManger + */ + Parameter() + { + } + Parameter(const std::string& rName, T value, ParameterManager* pParameterManger = NULL) + : AbstractParameter(rName, pParameterManger) + , m_Value(value) + { + } + + /** + * Parameter with given name, description and value + * @param rName + * @param rDescription + * @param value + * @param pParameterManger + */ + Parameter(const std::string& rName, + const std::string& rDescription, + T value, + ParameterManager* pParameterManger = NULL) + : AbstractParameter(rName, rDescription, pParameterManger) + , m_Value(value) + { + } + + /** + * Destructor + */ + virtual ~Parameter() + { + } + + public: + /** + * Gets value of parameter + * @return parameter value + */ + inline const T& GetValue() const + { + return m_Value; + } + + /** + * Sets value of parameter + * @param rValue + */ + inline void SetValue(const T& rValue) + { + m_Value = rValue; + } + + /** + * Gets value of parameter as string + * @return string version of value + */ + virtual const std::string GetValueAsString() const + { + std::stringstream converter; + converter << m_Value; + return converter.str(); + } + + /** + * Sets value of parameter from string + * @param rStringValue + */ + virtual void SetValueFromString(const std::string& rStringValue) + { + std::stringstream converter; + converter.str(rStringValue); + converter >> m_Value; + } + + /** + * Clone this parameter + * @return clone of this parameter + */ + virtual Parameter* Clone() + { + return new Parameter(GetName(), GetDescription(), GetValue()); + } + + public: + /** + * Assignment operator + */ + Parameter& operator = (const Parameter& rOther) + { + m_Value = rOther.m_Value; + + return *this; + } + + /** + * Sets the value of this parameter to given value + */ + T operator = (T value) + { + m_Value = value; + + return m_Value; + } + + /** + * Serialization: class Parameter + */ + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(AbstractParameter); + ar & BOOST_SERIALIZATION_NVP(m_Value); + } + + protected: + /** + * Parameter value + */ + T m_Value; + }; // Parameter + BOOST_SERIALIZATION_ASSUME_ABSTRACT(Parameter) + + template<> + inline void Parameter::SetValueFromString(const std::string& rStringValue) + { + int precision = std::numeric_limits::digits10; + std::stringstream converter; + converter.precision(precision); + + converter.str(rStringValue); + + m_Value = 0.0; + converter >> m_Value; + } + + template<> + inline const std::string Parameter::GetValueAsString() const + { + std::stringstream converter; + converter.precision(std::numeric_limits::digits10); + converter << m_Value; + return converter.str(); + } + + template<> + inline void Parameter::SetValueFromString(const std::string& rStringValue) + { + if (rStringValue == "true" || rStringValue == "TRUE") + { + m_Value = true; + } + else + { + m_Value = false; + } + } + + template<> + inline const std::string Parameter::GetValueAsString() const + { + if (m_Value == true) + { + return "true"; + } + + return "false"; + } + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + /** + * Parameter enum class + */ + class ParameterEnum : public Parameter + { + typedef std::map EnumMap; + + public: + /** + * Construct a Parameter object with name and value + * @param rName parameter name + * @param value of parameter + * @param pParameterManger + */ + ParameterEnum(const std::string& rName, kt_int32s value, ParameterManager* pParameterManger = NULL) + : Parameter(rName, value, pParameterManger) + { + } + ParameterEnum() + { + } + + /** + * Destructor + */ + virtual ~ParameterEnum() + { + } + + public: + /** + * Return a clone of this instance + * @return clone + */ + virtual Parameter* Clone() + { + ParameterEnum* pEnum = new ParameterEnum(GetName(), GetValue()); + + pEnum->m_EnumDefines = m_EnumDefines; + + return pEnum; + } + + /** + * Set parameter value from string. + * @param rStringValue value as string + */ + virtual void SetValueFromString(const std::string& rStringValue) + { + if (m_EnumDefines.find(rStringValue) != m_EnumDefines.end()) + { + m_Value = m_EnumDefines[rStringValue]; + } + else + { + std::string validValues; + + const_forEach(EnumMap, &m_EnumDefines) + { + validValues += iter->first + ", "; + } + + throw Exception("Unable to set enum: " + rStringValue + ". Valid values are: " + validValues); + } + } + + /** + * Get parameter value as string. + * @return value as string + */ + virtual const std::string GetValueAsString() const + { + const_forEach(EnumMap, &m_EnumDefines) + { + if (iter->second == m_Value) + { + return iter->first; + } + } + + throw Exception("Unable to lookup enum"); + } + + /** + * Defines the enum with the given name as having the given value + * @param value + * @param rName + */ + void DefineEnumValue(kt_int32s value, const std::string& rName) + { + if (m_EnumDefines.find(rName) == m_EnumDefines.end()) + { + m_EnumDefines[rName] = value; + } + else + { + std::cerr << "Overriding enum value: " << m_EnumDefines[rName] << " with " << value << std::endl; + + m_EnumDefines[rName] = value; + + assert(false); + } + } + + public: + /** + * Assignment operator + */ + ParameterEnum& operator = (const ParameterEnum& rOther) + { + SetValue(rOther.GetValue()); + + return *this; + } + + /** + * Assignment operator + */ + kt_int32s operator = (kt_int32s value) + { + SetValue(value); + + return m_Value; + } + + private: + EnumMap m_EnumDefines; + + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Parameter); + ar & BOOST_SERIALIZATION_NVP(m_EnumDefines); + } + }; // ParameterEnum + BOOST_SERIALIZATION_ASSUME_ABSTRACT(ParameterEnum) + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + /** + * Set of parameters + */ + class Parameters : public Object + { + public: + // @cond EXCLUDE + KARTO_Object(Parameters) + // @endcond + + public: + /** + * Parameters + * @param rName + */ + Parameters() + { + } + Parameters(const std::string& rName) + : Object(rName) + { + } + + /** + * Destructor + */ + virtual ~Parameters() + { + } + + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Object); + } + + private: + Parameters(const Parameters&); + const Parameters& operator=(const Parameters&); + }; // Parameters + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + class SensorData; + + /** + * Abstract Sensor base class + */ + class KARTO_EXPORT Sensor : public Object + { + + /** + * Serialization: class Sensor + */ + public: + Sensor() + { + } + // @cond EXCLUDE + KARTO_Object(Sensor) + // @endcond + + protected: + /** + * Construct a Sensor + * @param rName sensor name + */ + Sensor(const Name& rName); + + public: + /** + * Destructor + */ + virtual ~Sensor(); + + public: + /** + * Gets this range finder sensor's offset + * @return offset pose + */ + inline const Pose2& GetOffsetPose() const + { + return m_pOffsetPose->GetValue(); + } + + /** + * Sets this range finder sensor's offset + * @param rPose + */ + inline void SetOffsetPose(const Pose2& rPose) + { + m_pOffsetPose->SetValue(rPose); + } + + /** + * Validates sensor + * @return true if valid + */ + virtual kt_bool Validate() = 0; + + /** + * Validates sensor data + * @param pSensorData sensor data + * @return true if valid + */ + virtual kt_bool Validate(SensorData* pSensorData) = 0; + + private: + /** + * Restrict the copy constructor + */ + Sensor(const Sensor&); + + /** + * Restrict the assignment operator + */ + const Sensor& operator=(const Sensor&); + + private: + /** + * Sensor offset pose + */ + Parameter* m_pOffsetPose; + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Object); + ar & BOOST_SERIALIZATION_NVP(m_pOffsetPose); + } + }; // Sensor + BOOST_SERIALIZATION_ASSUME_ABSTRACT(Sensor) + /** + * Type declaration of Sensor vector + */ + typedef std::vector SensorVector; + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + /** + * Type declaration of map + */ + typedef std::map SensorManagerMap; + + /** + * Manages sensors + */ + class KARTO_EXPORT SensorManager + { + public: + /** + * Constructor + */ + SensorManager() + { + } + + /** + * Destructor + */ + virtual ~SensorManager() + { + } + + public: + /** + * Get singleton instance of SensorManager + */ + static SensorManager* GetInstance(); + + public: + /** + * Registers a sensor by it's name. The Sensor name must be unique, if not sensor is not registered + * unless override is set to true + * @param pSensor sensor to register + * @param override + * @return true if sensor is registered with SensorManager, false if Sensor name is not unique + */ + void RegisterSensor(Sensor* pSensor, kt_bool override = false) + { + Validate(pSensor); + + if ((m_Sensors.find(pSensor->GetName()) != m_Sensors.end()) && !override) + { + throw Exception("Cannot register sensor: already registered: [" + + pSensor->GetName().ToString() + + "] (Consider setting 'override' to true)"); + } + + std::cout << "Registering sensor: [" << pSensor->GetName().ToString() << "]" << std::endl; + + m_Sensors[pSensor->GetName()] = pSensor; + } + + /** + * Unregisters the given sensor + * @param pSensor sensor to unregister + */ + void UnregisterSensor(Sensor* pSensor) + { + Validate(pSensor); + + if (m_Sensors.find(pSensor->GetName()) != m_Sensors.end()) + { + std::cout << "Unregistering sensor: " << pSensor->GetName().ToString() << std::endl; + + m_Sensors.erase(pSensor->GetName()); + } + else + { + throw Exception("Cannot unregister sensor: not registered: [" + pSensor->GetName().ToString() + "]"); + } + } + + /** + * Gets the sensor with the given name + * @param rName name of sensor + * @return sensor + */ + Sensor* GetSensorByName(const Name& rName) + { + if (m_Sensors.find(rName) != m_Sensors.end()) + { + return m_Sensors[rName]; + } + + throw Exception("Sensor not registered: [" + rName.ToString() + "] (Did you add the sensor to the Dataset?)"); + } + + /** + * Gets the sensor with the given name + * @param rName name of sensor + * @return sensor + */ + template + T* GetSensorByName(const Name& rName) + { + Sensor* pSensor = GetSensorByName(rName); + + return dynamic_cast(pSensor); + } + + /** + * Gets all registered sensors + * @return vector of all registered sensors + */ + SensorVector GetAllSensors() + { + SensorVector sensors; + + forEach(SensorManagerMap, &m_Sensors) + { + sensors.push_back(iter->second); + } + + return sensors; + } + + protected: + /** + * Checks that given sensor is not NULL and has non-empty name + * @param pSensor sensor to validate + */ + static void Validate(Sensor* pSensor) + { + if (pSensor == NULL) + { + throw Exception("Invalid sensor: NULL"); + } + else if (pSensor->GetName().ToString() == "") + { + throw Exception("Invalid sensor: nameless"); + } + } + + protected: + /** + * Sensor map + */ + SensorManagerMap m_Sensors; + + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + ar & BOOST_SERIALIZATION_NVP(m_Sensors); + } + }; + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + /** + * Sensor that provides pose information relative to world coordinates. + * + * The user can set the offset pose of the drive sensor. If no value is provided by the user the default is no offset, + * i.e, the sensor is initially at the world origin, oriented along the positive z axis. + */ + class Drive : public Sensor + { + public: + // @cond EXCLUDE + KARTO_Object(Drive) + // @endcond + + public: + /** + * Constructs a Drive object + */ + Drive(const std::string& rName) + : Sensor(rName) + { + } + /** + * Destructor + */ + virtual ~Drive() + { + } + + public: + virtual kt_bool Validate() + { + return true; + } + + virtual kt_bool Validate(SensorData* pSensorData) + { + if (pSensorData == NULL) + { + return false; + } + + return true; + } + + private: + Drive(const Drive&); + const Drive& operator=(const Drive&); + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Sensor); + } + }; // class Drive + + BOOST_SERIALIZATION_ASSUME_ABSTRACT(Drive) + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + class LocalizedRangeScan; + class CoordinateConverter; + + /** + * The LaserRangeFinder defines a laser sensor that provides the pose offset position of a localized range scan relative to the robot. + * The user can set an offset pose for the sensor relative to the robot coordinate system. If no value is provided + * by the user, the sensor is set to be at the origin of the robot coordinate system. + * The LaserRangeFinder contains parameters for physical laser sensor used by the mapper for scan matching + * Also contains information about the maximum range of the sensor and provides a threshold + * for limiting the range of readings. + * The optimal value for the range threshold depends on the angular resolution of the scan and + * the desired map resolution. RangeThreshold should be set as large as possible while still + * providing "solid" coverage between consecutive range readings. The diagram below illustrates + * the relationship between map resolution and the range threshold. + */ + class KARTO_EXPORT LaserRangeFinder : public Sensor + { + public: + LaserRangeFinder() + { + } + // @cond EXCLUDE + KARTO_Object(LaserRangeFinder) + // @endcond + + public: + /** + * Destructor + */ + virtual ~LaserRangeFinder() + { + } + + public: + /** + * Gets this range finder sensor's minimum range + * @return minimum range + */ + inline kt_double GetMinimumRange() const + { + return m_pMinimumRange->GetValue(); + } + + /** + * Sets this range finder sensor's minimum range + * @param minimumRange + */ + inline void SetMinimumRange(kt_double minimumRange) + { + m_pMinimumRange->SetValue(minimumRange); + + SetRangeThreshold(GetRangeThreshold()); + } + + /** + * Gets this range finder sensor's maximum range + * @return maximum range + */ + inline kt_double GetMaximumRange() const + { + return m_pMaximumRange->GetValue(); + } + + /** + * Sets this range finder sensor's maximum range + * @param maximumRange + */ + inline void SetMaximumRange(kt_double maximumRange) + { + m_pMaximumRange->SetValue(maximumRange); + + SetRangeThreshold(GetRangeThreshold()); + } + + /** + * Gets the range threshold + * @return range threshold + */ + inline kt_double GetRangeThreshold() const + { + return m_pRangeThreshold->GetValue(); + } + + /** + * Sets the range threshold + * @param rangeThreshold + */ + inline void SetRangeThreshold(kt_double rangeThreshold) + { + // make sure rangeThreshold is within laser range finder range + m_pRangeThreshold->SetValue(math::Clip(rangeThreshold, GetMinimumRange(), GetMaximumRange())); + + if (math::DoubleEqual(GetRangeThreshold(), rangeThreshold) == false) + { + std::cout << "Info: clipped range threshold to be within minimum and maximum range!" << std::endl; + } + } + + /** + * Gets this range finder sensor's minimum angle + * @return minimum angle + */ + inline kt_double GetMinimumAngle() const + { + return m_pMinimumAngle->GetValue(); + } + + /** + * Sets this range finder sensor's minimum angle + * @param minimumAngle + */ + inline void SetMinimumAngle(kt_double minimumAngle) + { + m_pMinimumAngle->SetValue(minimumAngle); + + Update(); + } + + /** + * Gets this range finder sensor's maximum angle + * @return maximum angle + */ + inline kt_double GetMaximumAngle() const + { + return m_pMaximumAngle->GetValue(); + } + + /** + * Sets this range finder sensor's maximum angle + * @param maximumAngle + */ + inline void SetMaximumAngle(kt_double maximumAngle) + { + m_pMaximumAngle->SetValue(maximumAngle); + + Update(); + } + + /** + * Gets this range finder sensor's angular resolution + * @return angular resolution + */ + inline kt_double GetAngularResolution() const + { + return m_pAngularResolution->GetValue(); + } + + /** + * Sets this range finder sensor's angular resolution + * @param angularResolution + */ + inline void SetAngularResolution(kt_double angularResolution) + { + if (m_pType->GetValue() == LaserRangeFinder_Custom) + { + m_pAngularResolution->SetValue(angularResolution); + } + else if (m_pType->GetValue() == LaserRangeFinder_Sick_LMS100) + { + if (math::DoubleEqual(angularResolution, math::DegreesToRadians(0.25))) + { + m_pAngularResolution->SetValue(math::DegreesToRadians(0.25)); + } + else if (math::DoubleEqual(angularResolution, math::DegreesToRadians(0.50))) + { + m_pAngularResolution->SetValue(math::DegreesToRadians(0.50)); + } + else + { + std::stringstream stream; + stream << "Invalid resolution for Sick LMS100: "; + stream << angularResolution; + throw Exception(stream.str()); + } + } + else if (m_pType->GetValue() == LaserRangeFinder_Sick_LMS200 || + m_pType->GetValue() == LaserRangeFinder_Sick_LMS291) + { + if (math::DoubleEqual(angularResolution, math::DegreesToRadians(0.25))) + { + m_pAngularResolution->SetValue(math::DegreesToRadians(0.25)); + } + else if (math::DoubleEqual(angularResolution, math::DegreesToRadians(0.50))) + { + m_pAngularResolution->SetValue(math::DegreesToRadians(0.50)); + } + else if (math::DoubleEqual(angularResolution, math::DegreesToRadians(1.00))) + { + m_pAngularResolution->SetValue(math::DegreesToRadians(1.00)); + } + else + { + std::stringstream stream; + stream << "Invalid resolution for Sick LMS291: "; + stream << angularResolution; + throw Exception(stream.str()); + } + } + else + { + throw Exception("Can't set angular resolution, please create a LaserRangeFinder of type Custom"); + } + + Update(); + } + + /** + * Return Laser type + */ + inline kt_int32s GetType() + { + return m_pType->GetValue(); + } + + /** + * Gets the number of range readings each localized range scan must contain to be a valid scan. + * @return number of range readings + */ + inline kt_int32u GetNumberOfRangeReadings() const + { + return m_NumberOfRangeReadings; + } + + + /** + * Gets if this range finder sensor is 360° laser + * @return is360Laser + */ + inline kt_bool GetIs360Laser() const + { + return m_pIs360Laser->GetValue(); + } + + /** + * Sets if this range finder sensor is 360° laser + * @param is360Laser + */ + inline void SetIs360Laser(bool is_360_laser) + { + m_pIs360Laser->SetValue(is_360_laser); + + Update(); + } + + + virtual kt_bool Validate() + { + Update(); + + if (math::InRange(GetRangeThreshold(), GetMinimumRange(), GetMaximumRange()) == false) + { + std::cout << "Please set range threshold to a value between [" + << GetMinimumRange() << ";" << GetMaximumRange() << "]" << std::endl; + return false; + } + + return true; + } + + virtual kt_bool Validate(SensorData* pSensorData); + + /** + * Get point readings (potentially scale readings if given coordinate converter is not null) + * @param pLocalizedRangeScan + * @param pCoordinateConverter + * @param ignoreThresholdPoints + * @param flipY + */ + const PointVectorDouble GetPointReadings(LocalizedRangeScan* pLocalizedRangeScan, + CoordinateConverter* pCoordinateConverter, + kt_bool ignoreThresholdPoints = true, + kt_bool flipY = false) const; + + public: + /** + * Create a laser range finder of the given type and ID + * @param type + * @param rName name of sensor - if no name is specified default name will be assigned + * @return laser range finder + */ + static LaserRangeFinder* CreateLaserRangeFinder(LaserRangeFinderType type, const Name& rName) + { + LaserRangeFinder* pLrf = NULL; + + switch (type) + { + // see http://www.hizook.com/files/publications/SICK_LMS100.pdf + // set range threshold to 18m + case LaserRangeFinder_Sick_LMS100: + { + pLrf = new LaserRangeFinder((rName.GetName() != "") ? rName : Name("Sick LMS 100")); + + // Sensing range is 18 meters (at 10% reflectivity, max range of 20 meters), with an error of about 20mm + pLrf->m_pMinimumRange->SetValue(0.0); + pLrf->m_pMaximumRange->SetValue(20.0); + + // 270 degree range, 50 Hz + pLrf->m_pMinimumAngle->SetValue(math::DegreesToRadians(-135)); + pLrf->m_pMaximumAngle->SetValue(math::DegreesToRadians(135)); + + // 0.25 degree angular resolution + pLrf->m_pAngularResolution->SetValue(math::DegreesToRadians(0.25)); + + pLrf->m_NumberOfRangeReadings = 1081; + } + break; + + // see http://www.hizook.com/files/publications/SICK_LMS200-291_Tech_Info.pdf + // set range threshold to 10m + case LaserRangeFinder_Sick_LMS200: + { + pLrf = new LaserRangeFinder((rName.GetName() != "") ? rName : Name("Sick LMS 200")); + + // Sensing range is 80 meters + pLrf->m_pMinimumRange->SetValue(0.0); + pLrf->m_pMaximumRange->SetValue(80.0); + + // 180 degree range, 75 Hz + pLrf->m_pMinimumAngle->SetValue(math::DegreesToRadians(-90)); + pLrf->m_pMaximumAngle->SetValue(math::DegreesToRadians(90)); + + // 0.5 degree angular resolution + pLrf->m_pAngularResolution->SetValue(math::DegreesToRadians(0.5)); + + pLrf->m_NumberOfRangeReadings = 361; + } + break; + + // see http://www.hizook.com/files/publications/SICK_LMS200-291_Tech_Info.pdf + // set range threshold to 30m + case LaserRangeFinder_Sick_LMS291: + { + pLrf = new LaserRangeFinder((rName.GetName() != "") ? rName : Name("Sick LMS 291")); + + // Sensing range is 80 meters + pLrf->m_pMinimumRange->SetValue(0.0); + pLrf->m_pMaximumRange->SetValue(80.0); + + // 180 degree range, 75 Hz + pLrf->m_pMinimumAngle->SetValue(math::DegreesToRadians(-90)); + pLrf->m_pMaximumAngle->SetValue(math::DegreesToRadians(90)); + + // 0.5 degree angular resolution + pLrf->m_pAngularResolution->SetValue(math::DegreesToRadians(0.5)); + + pLrf->m_NumberOfRangeReadings = 361; + } + break; + + // see http://www.hizook.com/files/publications/Hokuyo_UTM_LaserRangeFinder_LIDAR.pdf + // set range threshold to 30m + case LaserRangeFinder_Hokuyo_UTM_30LX: + { + pLrf = new LaserRangeFinder((rName.GetName() != "") ? rName : Name("Hokuyo UTM-30LX")); + + // Sensing range is 30 meters + pLrf->m_pMinimumRange->SetValue(0.1); + pLrf->m_pMaximumRange->SetValue(30.0); + + // 270 degree range, 40 Hz + pLrf->m_pMinimumAngle->SetValue(math::DegreesToRadians(-135)); + pLrf->m_pMaximumAngle->SetValue(math::DegreesToRadians(135)); + + // 0.25 degree angular resolution + pLrf->m_pAngularResolution->SetValue(math::DegreesToRadians(0.25)); + + pLrf->m_NumberOfRangeReadings = 1081; + } + break; + + // see http://www.hizook.com/files/publications/HokuyoURG_Datasheet.pdf + // set range threshold to 3.5m + case LaserRangeFinder_Hokuyo_URG_04LX: + { + pLrf = new LaserRangeFinder((rName.GetName() != "") ? rName : Name("Hokuyo URG-04LX")); + + // Sensing range is 4 meters. It has detection problems when + // scanning absorptive surfaces (such as black trimming). + pLrf->m_pMinimumRange->SetValue(0.02); + pLrf->m_pMaximumRange->SetValue(4.0); + + // 240 degree range, 10 Hz + pLrf->m_pMinimumAngle->SetValue(math::DegreesToRadians(-120)); + pLrf->m_pMaximumAngle->SetValue(math::DegreesToRadians(120)); + + // 0.352 degree angular resolution + pLrf->m_pAngularResolution->SetValue(math::DegreesToRadians(0.352)); + + pLrf->m_NumberOfRangeReadings = 751; + } + break; + + case LaserRangeFinder_Custom: + { + pLrf = new LaserRangeFinder((rName.GetName() != "") ? rName : Name("User-Defined LaserRangeFinder")); + + // Sensing range is 80 meters. + pLrf->m_pMinimumRange->SetValue(0.0); + pLrf->m_pMaximumRange->SetValue(80.0); + + // 180 degree range + pLrf->m_pMinimumAngle->SetValue(math::DegreesToRadians(-90)); + pLrf->m_pMaximumAngle->SetValue(math::DegreesToRadians(90)); + + // 1.0 degree angular resolution + pLrf->m_pAngularResolution->SetValue(math::DegreesToRadians(1.0)); + + pLrf->m_NumberOfRangeReadings = 181; + } + break; + } + + if (pLrf != NULL) + { + pLrf->m_pType->SetValue(type); + + Pose2 defaultOffset; + pLrf->SetOffsetPose(defaultOffset); + } + + return pLrf; + } + + private: + /** + * Constructs a LaserRangeFinder object with given ID + */ + LaserRangeFinder(const Name& rName) + : Sensor(rName) + , m_NumberOfRangeReadings(0) + { + m_pMinimumRange = new Parameter("MinimumRange", 0.0, GetParameterManager()); + m_pMaximumRange = new Parameter("MaximumRange", 80.0, GetParameterManager()); + + m_pMinimumAngle = new Parameter("MinimumAngle", -KT_PI_2, GetParameterManager()); + m_pMaximumAngle = new Parameter("MaximumAngle", KT_PI_2, GetParameterManager()); + + m_pAngularResolution = new Parameter("AngularResolution", + math::DegreesToRadians(1), + GetParameterManager()); + + m_pRangeThreshold = new Parameter("RangeThreshold", 12.0, GetParameterManager()); + + m_pIs360Laser = new Parameter("Is360DegreeLaser", false, GetParameterManager()); + + m_pType = new ParameterEnum("Type", LaserRangeFinder_Custom, GetParameterManager()); + m_pType->DefineEnumValue(LaserRangeFinder_Custom, "Custom"); + m_pType->DefineEnumValue(LaserRangeFinder_Sick_LMS100, "Sick_LMS100"); + m_pType->DefineEnumValue(LaserRangeFinder_Sick_LMS200, "Sick_LMS200"); + m_pType->DefineEnumValue(LaserRangeFinder_Sick_LMS291, "Sick_LMS291"); + m_pType->DefineEnumValue(LaserRangeFinder_Hokuyo_UTM_30LX, "Hokuyo_UTM_30LX"); + m_pType->DefineEnumValue(LaserRangeFinder_Hokuyo_URG_04LX, "Hokuyo_URG_04LX"); + } + + /** + * Set the number of range readings based on the minimum and + * maximum angles of the sensor and the angular resolution + */ + void Update() + { + int residual = 1; + if (GetIs360Laser()) + { + // residual is 0 by 360 lidar conventions + residual = 0; + } + m_NumberOfRangeReadings = static_cast(math::Round((GetMaximumAngle() - + GetMinimumAngle()) + / GetAngularResolution()) + residual); + } + + private: + LaserRangeFinder(const LaserRangeFinder&); + const LaserRangeFinder& operator=(const LaserRangeFinder&); + + private: + // sensor m_Parameters + Parameter* m_pMinimumAngle; + Parameter* m_pMaximumAngle; + + Parameter* m_pAngularResolution; + + Parameter* m_pMinimumRange; + Parameter* m_pMaximumRange; + + Parameter* m_pRangeThreshold; + + Parameter* m_pIs360Laser; + + ParameterEnum* m_pType; + + kt_int32u m_NumberOfRangeReadings; + + // static std::string LaserRangeFinderTypeNames[6]; + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + if (Archive::is_loading::value) + { + m_pMinimumRange = new Parameter("MinimumRange", 0.0, GetParameterManager()); + m_pMaximumRange = new Parameter("MaximumRange", 80.0, GetParameterManager()); + + m_pMinimumAngle = new Parameter("MinimumAngle", -KT_PI_2, GetParameterManager()); + m_pMaximumAngle = new Parameter("MaximumAngle", KT_PI_2, GetParameterManager()); + + m_pAngularResolution = new Parameter("AngularResolution", + math::DegreesToRadians(1), + GetParameterManager()); + + m_pRangeThreshold = new Parameter("RangeThreshold", 12.0, GetParameterManager()); + + m_pIs360Laser = new Parameter("Is360DegreeLaser", false, GetParameterManager()); + + m_pType = new ParameterEnum("Type", LaserRangeFinder_Custom, GetParameterManager()); + } + + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Sensor); + ar & BOOST_SERIALIZATION_NVP(m_pMinimumAngle); + ar & BOOST_SERIALIZATION_NVP(m_pMaximumAngle); + ar & BOOST_SERIALIZATION_NVP(m_pAngularResolution); + ar & BOOST_SERIALIZATION_NVP(m_pMinimumRange); + ar & BOOST_SERIALIZATION_NVP(m_pMaximumRange); + ar & BOOST_SERIALIZATION_NVP(m_pRangeThreshold); + ar & BOOST_SERIALIZATION_NVP(m_pIs360Laser); + ar & BOOST_SERIALIZATION_NVP(m_pType); + ar & BOOST_SERIALIZATION_NVP(m_NumberOfRangeReadings); + } + }; // LaserRangeFinder + BOOST_SERIALIZATION_ASSUME_ABSTRACT(LaserRangeFinder) + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + /** + * Enumerated type for valid grid cell states + */ + typedef enum + { + GridStates_Unknown = 0, + GridStates_Occupied = 100, + GridStates_Free = 255 + } GridStates; + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + /** + * The CoordinateConverter class is used to convert coordinates between world and grid coordinates + * In world coordinates 1.0 = 1 meter where 1 in grid coordinates = 1 pixel! + * Default scale for coordinate converter is 20 that converters to 1 pixel = 0.05 meter + */ + class CoordinateConverter + { + public: + /** + * Default constructor + */ + CoordinateConverter() + : m_Scale(20.0) + { + } + + public: + /** + * Scales the value + * @param value + * @return scaled value + */ + inline kt_double Transform(kt_double value) + { + return value * m_Scale; + } + + /** + * Converts the point from world coordinates to grid coordinates + * @param rWorld world coordinate + * @param flipY + * @return grid coordinate + */ + inline Vector2 WorldToGrid(const Vector2& rWorld, kt_bool flipY = false) const + { + kt_double gridX = (rWorld.GetX() - m_Offset.GetX()) * m_Scale; + kt_double gridY = 0.0; + + if (flipY == false) + { + gridY = (rWorld.GetY() - m_Offset.GetY()) * m_Scale; + } + else + { + gridY = (m_Size.GetHeight() / m_Scale - rWorld.GetY() + m_Offset.GetY()) * m_Scale; + } + + return Vector2(static_cast(math::Round(gridX)), static_cast(math::Round(gridY))); + } + + /** + * Converts the point from grid coordinates to world coordinates + * @param rGrid world coordinate + * @param flipY + * @return world coordinate + */ + inline Vector2 GridToWorld(const Vector2& rGrid, kt_bool flipY = false) const + { + kt_double worldX = m_Offset.GetX() + rGrid.GetX() / m_Scale; + kt_double worldY = 0.0; + + if (flipY == false) + { + worldY = m_Offset.GetY() + rGrid.GetY() / m_Scale; + } + else + { + worldY = m_Offset.GetY() + (m_Size.GetHeight() - rGrid.GetY()) / m_Scale; + } + + return Vector2(worldX, worldY); + } + + /** + * Gets the scale + * @return scale + */ + inline kt_double GetScale() const + { + return m_Scale; + } + + /** + * Sets the scale + * @param scale + */ + inline void SetScale(kt_double scale) + { + m_Scale = scale; + } + + /** + * Gets the offset + * @return offset + */ + inline const Vector2& GetOffset() const + { + return m_Offset; + } + + /** + * Sets the offset + * @param rOffset + */ + inline void SetOffset(const Vector2& rOffset) + { + m_Offset = rOffset; + } + + /** + * Sets the size + * @param rSize + */ + inline void SetSize(const Size2& rSize) + { + m_Size = rSize; + } + + /** + * Gets the size + * @return size + */ + inline const Size2& GetSize() const + { + return m_Size; + } + + /** + * Gets the resolution + * @return resolution + */ + inline kt_double GetResolution() const + { + return 1.0 / m_Scale; + } + + /** + * Sets the resolution + * @param resolution + */ + inline void SetResolution(kt_double resolution) + { + m_Scale = 1.0 / resolution; + } + + /** + * Gets the bounding box + * @return bounding box + */ + inline BoundingBox2 GetBoundingBox() const + { + BoundingBox2 box; + + kt_double minX = GetOffset().GetX(); + kt_double minY = GetOffset().GetY(); + kt_double maxX = minX + GetSize().GetWidth() * GetResolution(); + kt_double maxY = minY + GetSize().GetHeight() * GetResolution(); + + box.SetMinimum(GetOffset()); + box.SetMaximum(Vector2(maxX, maxY)); + return box; + } + + private: + Size2 m_Size; + kt_double m_Scale; + + Vector2 m_Offset; + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + ar & BOOST_SERIALIZATION_NVP(m_Size); + ar & BOOST_SERIALIZATION_NVP(m_Scale); + ar & BOOST_SERIALIZATION_NVP(m_Offset); + } + + }; // CoordinateConverter + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + /** + * Defines a grid class + */ + template + class Grid + { + public: + /** + * Creates a grid of given size and resolution + * @param width + * @param height + * @param resolution + * @return grid pointer + */ + Grid() + { + } + static Grid* CreateGrid(kt_int32s width, kt_int32s height, kt_double resolution) + { + Grid* pGrid = new Grid(width, height); + + pGrid->GetCoordinateConverter()->SetScale(1.0 / resolution); + + return pGrid; + } + + /** + * Destructor + */ + virtual ~Grid() + { + if (m_pData) + { + delete [] m_pData; + } + if (m_pCoordinateConverter) + { + delete m_pCoordinateConverter; + } + } + + public: + /** + * Clear out the grid data + */ + void Clear() + { + memset(m_pData, 0, GetDataSize() * sizeof(T)); + } + + /** + * Returns a clone of this grid + * @return grid clone + */ + Grid* Clone() + { + Grid* pGrid = CreateGrid(GetWidth(), GetHeight(), GetResolution()); + pGrid->GetCoordinateConverter()->SetOffset(GetCoordinateConverter()->GetOffset()); + + memcpy(pGrid->GetDataPointer(), GetDataPointer(), GetDataSize()); + + return pGrid; + } + + /** + * Resizes the grid (deletes all old data) + * @param width + * @param height + */ + virtual void Resize(kt_int32s width, kt_int32s height) + { + m_Width = width; + m_Height = height; + m_WidthStep = math::AlignValue(width, 8); + + if (m_pData != NULL) + { + delete[] m_pData; + m_pData = NULL; + } + + try + { + m_pData = new T[GetDataSize()]; + + if (m_pCoordinateConverter == NULL) + { + m_pCoordinateConverter = new CoordinateConverter(); + } + + m_pCoordinateConverter->SetSize(Size2(width, height)); + } + catch(...) + { + m_pData = NULL; + + m_Width = 0; + m_Height = 0; + m_WidthStep = 0; + } + + Clear(); + } + + /** + * Checks whether the given coordinates are valid grid indices + * @param rGrid + */ + inline kt_bool IsValidGridIndex(const Vector2& rGrid) const + { + return (math::IsUpTo(rGrid.GetX(), m_Width) && math::IsUpTo(rGrid.GetY(), m_Height)); + } + + /** + * Gets the index into the data pointer of the given grid coordinate + * @param rGrid + * @param boundaryCheck default value is true + * @return grid index + */ + virtual kt_int32s GridIndex(const Vector2& rGrid, kt_bool boundaryCheck = true) const + { + if (boundaryCheck == true) + { + if (IsValidGridIndex(rGrid) == false) + { + std::stringstream error; + error << "Index " << rGrid << " out of range. Index must be between [0; " + << m_Width << ") and [0; " << m_Height << ")"; + throw Exception(error.str()); + } + } + + kt_int32s index = rGrid.GetX() + (rGrid.GetY() * m_WidthStep); + + if (boundaryCheck == true) + { + assert(math::IsUpTo(index, GetDataSize())); + } + + return index; + } + + /** + * Gets the grid coordinate from an index + * @param index + * @return grid coordinate + */ + Vector2 IndexToGrid(kt_int32s index) const + { + Vector2 grid; + + grid.SetY(index / m_WidthStep); + grid.SetX(index - grid.GetY() * m_WidthStep); + + return grid; + } + + /** + * Converts the point from world coordinates to grid coordinates + * @param rWorld world coordinate + * @param flipY + * @return grid coordinate + */ + inline Vector2 WorldToGrid(const Vector2& rWorld, kt_bool flipY = false) const + { + return GetCoordinateConverter()->WorldToGrid(rWorld, flipY); + } + + /** + * Converts the point from grid coordinates to world coordinates + * @param rGrid world coordinate + * @param flipY + * @return world coordinate + */ + inline Vector2 GridToWorld(const Vector2& rGrid, kt_bool flipY = false) const + { + return GetCoordinateConverter()->GridToWorld(rGrid, flipY); + } + + /** + * Gets pointer to data at given grid coordinate + * @param rGrid grid coordinate + * @return grid point + */ + T* GetDataPointer(const Vector2& rGrid) + { + kt_int32s index = GridIndex(rGrid, true); + return m_pData + index; + } + + /** + * Gets pointer to data at given grid coordinate + * @param rGrid grid coordinate + * @return grid point + */ + T* GetDataPointer(const Vector2& rGrid) const + { + kt_int32s index = GridIndex(rGrid, true); + return m_pData + index; + } + + /** + * Gets the width of the grid + * @return width of the grid + */ + inline kt_int32s GetWidth() const + { + return m_Width; + }; + + /** + * Gets the height of the grid + * @return height of the grid + */ + inline kt_int32s GetHeight() const + { + return m_Height; + }; + + /** + * Get the size as a Size2 + * @return size of the grid + */ + inline const Size2 GetSize() const + { + return Size2(m_Width, m_Height); + } + + /** + * Gets the width step in bytes + * @return width step + */ + inline kt_int32s GetWidthStep() const + { + return m_WidthStep; + } + + /** + * Gets the grid data pointer + * @return data pointer + */ + inline T* GetDataPointer() + { + return m_pData; + } + + /** + * Gets const grid data pointer + * @return data pointer + */ + inline T* GetDataPointer() const + { + return m_pData; + } + + /** + * Gets the allocated grid size in bytes + * @return data size + */ + inline kt_int32s GetDataSize() const + { + return m_WidthStep * m_Height; + } + + /** + * Get value at given grid coordinate + * @param rGrid grid coordinate + * @return value + */ + inline T GetValue(const Vector2& rGrid) const + { + kt_int32s index = GridIndex(rGrid); + return m_pData[index]; + } + + /** + * Gets the coordinate converter for this grid + * @return coordinate converter + */ + inline CoordinateConverter* GetCoordinateConverter() const + { + return m_pCoordinateConverter; + } + + /** + * Gets the resolution + * @return resolution + */ + inline kt_double GetResolution() const + { + return GetCoordinateConverter()->GetResolution(); + } + + /** + * Gets the grids bounding box + * @return bounding box + */ + inline BoundingBox2 GetBoundingBox() const + { + return GetCoordinateConverter()->GetBoundingBox(); + } + + /** + * Increments all the grid cells from (x0, y0) to (x1, y1); + * if applicable, apply f to each cell traced + * @param x0 + * @param y0 + * @param x1 + * @param y1 + * @param f + */ + void TraceLine(kt_int32s x0, kt_int32s y0, kt_int32s x1, kt_int32s y1, Functor* f = NULL) + { + kt_bool steep = abs(y1 - y0) > abs(x1 - x0); + if (steep) + { + std::swap(x0, y0); + std::swap(x1, y1); + } + if (x0 > x1) + { + std::swap(x0, x1); + std::swap(y0, y1); + } + + kt_int32s deltaX = x1 - x0; + kt_int32s deltaY = abs(y1 - y0); + kt_int32s error = 0; + kt_int32s ystep; + kt_int32s y = y0; + + if (y0 < y1) + { + ystep = 1; + } + else + { + ystep = -1; + } + + kt_int32s pointX; + kt_int32s pointY; + for (kt_int32s x = x0; x <= x1; x++) + { + if (steep) + { + pointX = y; + pointY = x; + } + else + { + pointX = x; + pointY = y; + } + + error += deltaY; + + if (2 * error >= deltaX) + { + y += ystep; + error -= deltaX; + } + + Vector2 gridIndex(pointX, pointY); + if (IsValidGridIndex(gridIndex)) + { + kt_int32s index = GridIndex(gridIndex, false); + T* pGridPointer = GetDataPointer(); + pGridPointer[index]++; + + if (f != NULL) + { + (*f)(index); + } + } + } + } + + protected: + /** + * Constructs grid of given size + * @param width + * @param height + */ + Grid(kt_int32s width, kt_int32s height) + : m_pData(NULL) + , m_pCoordinateConverter(NULL) + { + Resize(width, height); + } + + private: + kt_int32s m_Width; // width of grid + kt_int32s m_Height; // height of grid + kt_int32s m_WidthStep; // 8 bit aligned width of grid + T* m_pData; // grid data + + // coordinate converter to convert between world coordinates and grid coordinates + CoordinateConverter* m_pCoordinateConverter; + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + ar & BOOST_SERIALIZATION_NVP(m_Width); + ar & BOOST_SERIALIZATION_NVP(m_Height); + ar & BOOST_SERIALIZATION_NVP(m_WidthStep); + ar & BOOST_SERIALIZATION_NVP(m_pCoordinateConverter); + + + if (Archive::is_loading::value) + { + m_pData = new T[m_WidthStep * m_Height]; + } + ar & boost::serialization::make_array(m_pData, m_WidthStep * m_Height); + } + + }; // Grid + BOOST_SERIALIZATION_ASSUME_ABSTRACT(Grid) + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + /** + * For making custom data + */ + class CustomData : public Object + { + public: + // @cond EXCLUDE + KARTO_Object(CustomData) + // @endcond + + public: + /** + * Constructor + */ + CustomData() + : Object() + { + } + + /** + * Destructor + */ + virtual ~CustomData() + { + } + + public: + /** + * Write out this custom data as a string + * @return string representation of this data object + */ + virtual const std::string Write() const = 0; + + /** + * Read in this custom data from a string + * @param rValue string representation of this data object + */ + virtual void Read(const std::string& rValue) = 0; + + private: + CustomData(const CustomData&); + const CustomData& operator=(const CustomData&); + + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Object); + } + }; + BOOST_SERIALIZATION_ASSUME_ABSTRACT(CustomData) + + /** + * Type declaration of CustomData vector + */ + typedef std::vector CustomDataVector; + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + /** + * SensorData is a base class for all sensor data + */ + class KARTO_EXPORT SensorData : public Object + { + public: + // @cond EXCLUDE + KARTO_Object(SensorData) + // @endcond + + public: + + SensorData(){} + /** + * Destructor + */ + virtual ~SensorData(); + + public: + /** + * Gets sensor data id + * @return sensor id + */ + inline kt_int32s GetStateId() const + { + return m_StateId; + } + + /** + * Sets sensor data id + * @param stateId id + */ + inline void SetStateId(kt_int32s stateId) + { + m_StateId = stateId; + } + + /** + * Gets sensor unique id + * @return unique id + */ + inline kt_int32s GetUniqueId() const + { + return m_UniqueId; + } + + /** + * Sets sensor unique id + * @param uniqueId + */ + inline void SetUniqueId(kt_int32u uniqueId) + { + m_UniqueId = uniqueId; + } + + /** + * Gets sensor data time + * @return time + */ + inline kt_double GetTime() const + { + return m_Time; + } + + /** + * Sets sensor data time + * @param time + */ + inline void SetTime(kt_double time) + { + m_Time = time; + } + + /** + * Get the sensor that created this sensor data + * @return sensor + */ + inline const Name& GetSensorName() const + { + return m_SensorName; + } + + /** + * Add a CustomData object to sensor data + * @param pCustomData + */ + inline void AddCustomData(CustomData* pCustomData) + { + m_CustomData.push_back(pCustomData); + } + + /** + * Get all custom data objects assigned to sensor data + * @return CustomDataVector& + */ + inline const CustomDataVector& GetCustomData() const + { + return m_CustomData; + } + + protected: + /** + * Construct a SensorData object with a sensor name + */ + SensorData(const Name& rSensorName); + + private: + /** + * Restrict the copy constructor + */ + SensorData(const SensorData&); + + /** + * Restrict the assignment operator + */ + const SensorData& operator=(const SensorData&); + + private: + /** + * ID unique to individual sensor + */ + kt_int32s m_StateId; + + /** + * ID unique across all sensor data + */ + kt_int32s m_UniqueId; + + /** + * Sensor that created this sensor data + */ + Name m_SensorName; + + /** + * Time the sensor data was created + */ + kt_double m_Time; + + CustomDataVector m_CustomData; + + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + ar & BOOST_SERIALIZATION_NVP(m_StateId); + ar & BOOST_SERIALIZATION_NVP(m_UniqueId); + ar & BOOST_SERIALIZATION_NVP(m_SensorName); + ar & BOOST_SERIALIZATION_NVP(m_Time); + ar & BOOST_SERIALIZATION_NVP(m_CustomData); + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Object); + } +}; + + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + /** + * Type declaration of range readings vector + */ + typedef std::vector RangeReadingsVector; + + /** + * LaserRangeScan representing the range readings from a laser range finder sensor. + */ + class LaserRangeScan : public SensorData + { + public: + // @cond EXCLUDE + KARTO_Object(LaserRangeScan) + // @endcond + + public: + /** + * Constructs a scan from the given sensor with the given readings + * @param rSensorName + */ + LaserRangeScan(const Name& rSensorName) + : SensorData(rSensorName) + , m_pRangeReadings(NULL) + , m_NumberOfRangeReadings(0) + { + } + + LaserRangeScan() + { + } + + /** + * Constructs a scan from the given sensor with the given readings + * @param rSensorName + * @param rRangeReadings + */ + LaserRangeScan(const Name& rSensorName, const RangeReadingsVector& rRangeReadings) + : SensorData(rSensorName) + , m_pRangeReadings(NULL) + , m_NumberOfRangeReadings(0) + { + assert(rSensorName.ToString() != ""); + + SetRangeReadings(rRangeReadings); + } + + /** + * Destructor + */ + virtual ~LaserRangeScan() + { + delete [] m_pRangeReadings; + m_pRangeReadings = nullptr; + } + + public: + /** + * Gets the range readings of this scan + * @return range readings of this scan + */ + inline kt_double* GetRangeReadings() const + { + return m_pRangeReadings; + } + + inline RangeReadingsVector GetRangeReadingsVector() const + { + return RangeReadingsVector(m_pRangeReadings, m_pRangeReadings + m_NumberOfRangeReadings); + } + + /** + * Sets the range readings for this scan + * @param rRangeReadings + */ + inline void SetRangeReadings(const RangeReadingsVector& rRangeReadings) + { + // ignore for now! XXXMAE BUGUBUG 05/21/2010 << TODO(lucbettaieb): What the heck is this?? + // if (rRangeReadings.size() != GetNumberOfRangeReadings()) + // { + // std::stringstream error; + // error << "Given number of readings (" << rRangeReadings.size() + // << ") does not match expected number of range finder (" << GetNumberOfRangeReadings() << ")"; + // throw Exception(error.str()); + // } + + if (!rRangeReadings.empty()) + { + if (rRangeReadings.size() != m_NumberOfRangeReadings) + { + // delete old readings + delete [] m_pRangeReadings; + + // store size of array! + m_NumberOfRangeReadings = static_cast(rRangeReadings.size()); + + // allocate range readings + m_pRangeReadings = new kt_double[m_NumberOfRangeReadings]; + } + + // copy readings + kt_int32u index = 0; + const_forEach(RangeReadingsVector, &rRangeReadings) + { + m_pRangeReadings[index++] = *iter; + } + } + else + { + delete [] m_pRangeReadings; + m_pRangeReadings = NULL; + } + } + + /** + * Gets the laser range finder sensor that generated this scan + * @return laser range finder sensor of this scan + */ + inline LaserRangeFinder* GetLaserRangeFinder() const + { + return SensorManager::GetInstance()->GetSensorByName(GetSensorName()); + } + + /** + * Gets the number of range readings + * @return number of range readings + */ + inline kt_int32u GetNumberOfRangeReadings() const + { + return m_NumberOfRangeReadings; + } + + private: + LaserRangeScan(const LaserRangeScan&); + const LaserRangeScan& operator=(const LaserRangeScan&); + + private: + kt_double* m_pRangeReadings; + kt_int32u m_NumberOfRangeReadings; + + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + ar & BOOST_SERIALIZATION_NVP(m_NumberOfRangeReadings); + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(SensorData); + + if (Archive::is_loading::value) + { + m_pRangeReadings = new kt_double[m_NumberOfRangeReadings]; + } + ar & boost::serialization::make_array(m_pRangeReadings, m_NumberOfRangeReadings); + } + }; // LaserRangeScan + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + /** + * DrivePose representing the pose value of a drive sensor. + */ + class DrivePose : public SensorData + { + public: + // @cond EXCLUDE + KARTO_Object(DrivePose) + // @endcond + + public: + /** + * Constructs a pose of the given drive sensor + * @param rSensorName + */ + DrivePose(const Name& rSensorName) + : SensorData(rSensorName) + { + } + + /** + * Destructor + */ + virtual ~DrivePose() + { + } + + public: + /** + * Gets the odometric pose of this scan + * @return odometric pose of this scan + */ + inline const Pose3& GetOdometricPose() const + { + return m_OdometricPose; + } + + /** + * Sets the odometric pose of this scan + * @param rPose + */ + inline void SetOdometricPose(const Pose3& rPose) + { + m_OdometricPose = rPose; + } + + private: + DrivePose(const DrivePose&); + const DrivePose& operator=(const DrivePose&); + + private: + /** + * Odometric pose of robot + */ + Pose3 m_OdometricPose; + }; // class DrivePose + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + /** + * The LocalizedRangeScan contains range data from a single sweep of a laser range finder sensor + * in a two-dimensional space and position information. The odometer position is the position + * reported by the robot when the range data was recorded. The corrected position is the position + * calculated by the mapper (or localizer) + */ + class LocalizedRangeScan : public LaserRangeScan + { + public: + // @cond EXCLUDE + KARTO_Object(LocalizedRangeScan) + // @endcond + + public: + /** + * Constructs a range scan from the given range finder with the given readings + */ + LocalizedRangeScan(const Name& rSensorName, const RangeReadingsVector& rReadings) + : LaserRangeScan(rSensorName, rReadings) + , m_IsDirty(true) + { + } + + LocalizedRangeScan() + {} + + /** + * Destructor + */ + virtual ~LocalizedRangeScan() + { + } + + private: + mutable std::shared_mutex m_Lock; + + public: + /** + * Gets the odometric pose of this scan + * @return odometric pose of this scan + */ + inline const Pose2& GetOdometricPose() const + { + return m_OdometricPose; + } + + /** + * Sets the odometric pose of this scan + * @param rPose + */ + inline void SetOdometricPose(const Pose2& rPose) + { + m_OdometricPose = rPose; + } + + /** + * Gets the (possibly corrected) robot pose at which this scan was taken. The corrected robot pose of the scan + * is usually set by an external module such as a localization or mapping module when it is determined + * that the original pose was incorrect. The external module will set the correct pose based on + * additional sensor data and any context information it has. If the pose has not been corrected, + * a call to this method returns the same pose as GetOdometricPose(). + * @return corrected pose + */ + inline const Pose2& GetCorrectedPose() const + { + return m_CorrectedPose; + } + + /** + * Moves the scan by moving the robot pose to the given location. + * @param rPose new pose of the robot of this scan + */ + inline void SetCorrectedPose(const Pose2& rPose) + { + m_CorrectedPose = rPose; + + m_IsDirty = true; + } + + /** + * Moves the scan by moving the robot pose to the given location and update point readings. + * @param rPose new pose of the robot of this scan + */ + inline void SetCorrectedPoseAndUpdate(const Pose2& rPose) + { + SetCorrectedPose(rPose); + + Update(); + } + + /** + * Gets barycenter of point readings + */ + inline const Pose2& GetBarycenterPose() const + { + std::shared_lock lock(m_Lock); + if (m_IsDirty) + { + // throw away constness and do an update! + lock.unlock(); + std::unique_lock uniqueLock(m_Lock); + const_cast(this)->Update(); + } + + return m_BarycenterPose; + } + + inline void SetBarycenterPose(Pose2& bcenter) + { + m_BarycenterPose = bcenter; + } + + /** + * Gets barycenter if the given parameter is true, otherwise returns the scanner pose + * @param useBarycenter + * @return barycenter if parameter is true, otherwise scanner pose + */ + inline Pose2 GetReferencePose(kt_bool useBarycenter) const + { + std::shared_lock lock(m_Lock); + if (m_IsDirty) + { + // throw away constness and do an update! + lock.unlock(); + std::unique_lock uniqueLock(m_Lock); + const_cast(this)->Update(); + } + + return useBarycenter ? GetBarycenterPose() : GetSensorPose(); + } + + /** + * Computes the position of the sensor + * @return scan pose + */ + inline Pose2 GetSensorPose() const + { + return GetSensorAt(m_CorrectedPose); + } + + inline void SetIsDirty(kt_bool& rIsDirty) + { + m_IsDirty = rIsDirty; + } + + /** + * Computes the robot pose given the corrected scan pose + * @param rScanPose pose of the sensor + */ + void SetSensorPose(const Pose2& rScanPose) + { + m_CorrectedPose = GetCorrectedAt(rScanPose); + + Update(); + } + + /** + * Computes the position of the sensor if the robot were at the given pose + * @param rPose + * @return sensor pose + */ + inline Pose2 GetSensorAt(const Pose2& rPose) const + { + return Transform(rPose).TransformPose(GetLaserRangeFinder()->GetOffsetPose()); + } + + /** + * @brief Computes the pose of the robot if the sensor were at the given pose + * @param sPose sensor pose + * @return robot pose + */ + inline Pose2 GetCorrectedAt(const Pose2& sPose) const + { + Pose2 deviceOffsetPose2 = GetLaserRangeFinder()->GetOffsetPose(); + kt_double offsetLength = deviceOffsetPose2.GetPosition().Length(); + kt_double offsetHeading = deviceOffsetPose2.GetHeading(); + kt_double angleoffset = atan2(deviceOffsetPose2.GetY(), deviceOffsetPose2.GetX()); + kt_double correctedHeading = math::NormalizeAngle(sPose.GetHeading()); + Pose2 worldSensorOffset = Pose2(offsetLength * cos(correctedHeading + angleoffset - offsetHeading), + offsetLength * sin(correctedHeading + angleoffset - offsetHeading), + offsetHeading); + + return sPose - worldSensorOffset; + } + + /** + * Gets the bounding box of this scan + * @return bounding box of this scan + */ + inline const BoundingBox2& GetBoundingBox() const + { + std::shared_lock lock(m_Lock); + if (m_IsDirty) + { + // throw away constness and do an update! + lock.unlock(); + std::unique_lock uniqueLock(m_Lock); + const_cast(this)->Update(); + } + + return m_BoundingBox; + } + + inline void SetBoundingBox(BoundingBox2& bbox) + { + m_BoundingBox = bbox; + } + + /** + * Get point readings in local coordinates + */ + inline const PointVectorDouble& GetPointReadings(kt_bool wantFiltered = false) const + { + std::shared_lock lock(m_Lock); + if (m_IsDirty) + { + // throw away constness and do an update! + lock.unlock(); + std::unique_lock uniqueLock(m_Lock); + const_cast(this)->Update(); + } + + if (wantFiltered == true) + { + return m_PointReadings; + } + else + { + return m_UnfilteredPointReadings; + } + } + + inline void SetPointReadings(PointVectorDouble& points, kt_bool setFiltered = false) + { + if (setFiltered) + { + m_PointReadings = points; + } + else + { + m_UnfilteredPointReadings = points; + } + } + + private: + /** + * Compute point readings based on range readings + * Only range readings within [minimum range; range threshold] are returned + */ + virtual void Update() + { + LaserRangeFinder* pLaserRangeFinder = GetLaserRangeFinder(); + + if (pLaserRangeFinder != NULL) + { + m_PointReadings.clear(); + m_UnfilteredPointReadings.clear(); + + kt_double rangeThreshold = pLaserRangeFinder->GetRangeThreshold(); + kt_double minimumAngle = pLaserRangeFinder->GetMinimumAngle(); + kt_double angularResolution = pLaserRangeFinder->GetAngularResolution(); + Pose2 scanPose = GetSensorPose(); + + // compute point readings + Vector2 rangePointsSum; + kt_int32u beamNum = 0; + for (kt_int32u i = 0; i < pLaserRangeFinder->GetNumberOfRangeReadings(); i++, beamNum++) + { + kt_double rangeReading = GetRangeReadings()[i]; + if (!math::InRange(rangeReading, pLaserRangeFinder->GetMinimumRange(), rangeThreshold)) + { + kt_double angle = scanPose.GetHeading() + minimumAngle + beamNum * angularResolution; + + Vector2 point; + point.SetX(scanPose.GetX() + (rangeReading * cos(angle))); + point.SetY(scanPose.GetY() + (rangeReading * sin(angle))); + + m_UnfilteredPointReadings.push_back(point); + continue; + } + + kt_double angle = scanPose.GetHeading() + minimumAngle + beamNum * angularResolution; + + Vector2 point; + point.SetX(scanPose.GetX() + (rangeReading * cos(angle))); + point.SetY(scanPose.GetY() + (rangeReading * sin(angle))); + + m_PointReadings.push_back(point); + m_UnfilteredPointReadings.push_back(point); + + rangePointsSum += point; + } + + // compute barycenter + kt_double nPoints = static_cast(m_PointReadings.size()); + if (nPoints != 0.0) + { + Vector2 averagePosition = Vector2(rangePointsSum / nPoints); + m_BarycenterPose = Pose2(averagePosition, 0.0); + } + else + { + m_BarycenterPose = scanPose; + } + + // calculate bounding box of scan + m_BoundingBox = BoundingBox2(); + m_BoundingBox.Add(scanPose.GetPosition()); + forEach(PointVectorDouble, &m_PointReadings) + { + m_BoundingBox.Add(*iter); + } + } + + m_IsDirty = false; + } + + /** + * Serialization: class LocalizedRangeScan + */ + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + ar & BOOST_SERIALIZATION_NVP(m_OdometricPose); + ar & BOOST_SERIALIZATION_NVP(m_CorrectedPose); + ar & BOOST_SERIALIZATION_NVP(m_BarycenterPose); + ar & BOOST_SERIALIZATION_NVP(m_PointReadings); + ar & BOOST_SERIALIZATION_NVP(m_UnfilteredPointReadings); + ar & BOOST_SERIALIZATION_NVP(m_BoundingBox); + ar & BOOST_SERIALIZATION_NVP(m_IsDirty); + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(LaserRangeScan); + } + + + private: + LocalizedRangeScan(const LocalizedRangeScan&); + const LocalizedRangeScan& operator=(const LocalizedRangeScan&); + + private: + /** + * Odometric pose of robot + */ + Pose2 m_OdometricPose; + + /** + * Corrected pose of robot calculated by mapper (or localizer) + */ + Pose2 m_CorrectedPose; + + protected: + /** + * Average of all the point readings + */ + Pose2 m_BarycenterPose; + + /** + * Vector of point readings + */ + PointVectorDouble m_PointReadings; + + /** + * Vector of unfiltered point readings + */ + PointVectorDouble m_UnfilteredPointReadings; + + /** + * Bounding box of localized range scan + */ + BoundingBox2 m_BoundingBox; + + /** + * Internal flag used to update point readings, barycenter and bounding box + */ + kt_bool m_IsDirty; + }; // LocalizedRangeScan + + /** + * Type declaration of LocalizedRangeScan vector + */ + typedef std::vector LocalizedRangeScanVector; + typedef std::map LocalizedRangeScanMap; +//////////////////////////////////////////////////////////////////////////////////////// +//////////////////////////////////////////////////////////////////////////////////////// +//////////////////////////////////////////////////////////////////////////////////////// + + /** + * The LocalizedRangeScanWithPoints is an extension of the LocalizedRangeScan with precomputed points. + */ + class LocalizedRangeScanWithPoints : public LocalizedRangeScan + { + public: + // @cond EXCLUDE + KARTO_Object(LocalizedRangeScanWithPoints) + // @endcond + + public: + /** + * Constructs a range scan from the given range finder with the given readings. Precomptued points should be + * in the robot frame. + */ + LocalizedRangeScanWithPoints(const Name& rSensorName, const RangeReadingsVector& rReadings, + const PointVectorDouble& rPoints) + : m_Points(rPoints), + LocalizedRangeScan(rSensorName, rReadings) + { + } + + /** + * Destructor + */ + virtual ~LocalizedRangeScanWithPoints() + { + } + + private: + /** + * Update the points based on the latest sensor pose. + */ + void Update() + { + m_PointReadings.clear(); + m_UnfilteredPointReadings.clear(); + + Pose2 scanPose = GetSensorPose(); + Pose2 robotPose = GetCorrectedPose(); + + // update point readings + Vector2 rangePointsSum; + for (kt_int32u i = 0; i < m_Points.size(); i++) + { + // check if this has a NaN + if (!std::isfinite(m_Points[i].GetX()) || !std::isfinite(m_Points[i].GetY())) + { + Vector2 point(m_Points[i].GetX(), m_Points[i].GetY()); + m_UnfilteredPointReadings.push_back(point); + + continue; + } + + // transform into world coords + Pose2 pointPose(m_Points[i].GetX(), m_Points[i].GetY(), 0); + Pose2 result = Transform(robotPose).TransformPose(pointPose); + Vector2 point(result.GetX(), result.GetY()); + + m_PointReadings.push_back(point); + m_UnfilteredPointReadings.push_back(point); + + rangePointsSum += point; + } + + // compute barycenter + kt_double nPoints = static_cast(m_PointReadings.size()); + if (nPoints != 0.0) + { + Vector2 averagePosition = Vector2(rangePointsSum / nPoints); + m_BarycenterPose = Pose2(averagePosition, 0.0); + } + else + { + m_BarycenterPose = scanPose; + } + + // calculate bounding box of scan + m_BoundingBox = BoundingBox2(); + m_BoundingBox.Add(scanPose.GetPosition()); + forEach(PointVectorDouble, &m_PointReadings) + { + m_BoundingBox.Add(*iter); + } + + m_IsDirty = false; + } + + private: + LocalizedRangeScanWithPoints(const LocalizedRangeScanWithPoints&); + const LocalizedRangeScanWithPoints& operator=(const LocalizedRangeScanWithPoints&); + + private: + const PointVectorDouble m_Points; + }; // LocalizedRangeScanWithPoints + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + class OccupancyGrid; + + class KARTO_EXPORT CellUpdater : public Functor + { + public: + CellUpdater(OccupancyGrid* pGrid) + : m_pOccupancyGrid(pGrid) + { + } + + /** + * Updates the cell at the given index based on the grid's hits and pass counters + * @param index + */ + virtual void operator() (kt_int32u index); + + private: + OccupancyGrid* m_pOccupancyGrid; + }; // CellUpdater + + /** + * Occupancy grid definition. See GridStates for possible grid values. + */ + class OccupancyGrid : public Grid + { + friend class CellUpdater; + friend class IncrementalOccupancyGrid; + + public: + /** + * Constructs an occupancy grid of given size + * @param width + * @param height + * @param rOffset + * @param resolution + */ + OccupancyGrid(kt_int32s width, kt_int32s height, const Vector2& rOffset, kt_double resolution) + : Grid(width, height) + , m_pCellPassCnt(Grid::CreateGrid(0, 0, resolution)) + , m_pCellHitsCnt(Grid::CreateGrid(0, 0, resolution)) + , m_pCellUpdater(NULL) + { + m_pCellUpdater = new CellUpdater(this); + + if (karto::math::DoubleEqual(resolution, 0.0)) + { + throw Exception("Resolution cannot be 0"); + } + + m_pMinPassThrough = new Parameter("MinPassThrough", 2); + m_pOccupancyThreshold = new Parameter("OccupancyThreshold", 0.1); + + GetCoordinateConverter()->SetScale(1.0 / resolution); + GetCoordinateConverter()->SetOffset(rOffset); + } + + /** + * Destructor + */ + virtual ~OccupancyGrid() + { + delete m_pCellUpdater; + + delete m_pCellPassCnt; + delete m_pCellHitsCnt; + + delete m_pMinPassThrough; + delete m_pOccupancyThreshold; + } + + public: + /** + * Create an occupancy grid from the given scans using the given resolution + * @param rScans + * @param resolution + */ + static OccupancyGrid* CreateFromScans(const LocalizedRangeScanVector& rScans, kt_double resolution) + { + if (rScans.empty()) + { + return NULL; + } + + kt_int32s width, height; + Vector2 offset; + ComputeDimensions(rScans, resolution, width, height, offset); + OccupancyGrid* pOccupancyGrid = new OccupancyGrid(width, height, offset, resolution); + pOccupancyGrid->CreateFromScans(rScans); + + return pOccupancyGrid; + } + + /** + * Make a clone + * @return occupancy grid clone + */ + OccupancyGrid* Clone() const + { + OccupancyGrid* pOccupancyGrid = new OccupancyGrid(GetWidth(), + GetHeight(), + GetCoordinateConverter()->GetOffset(), + 1.0 / GetCoordinateConverter()->GetScale()); + memcpy(pOccupancyGrid->GetDataPointer(), GetDataPointer(), GetDataSize()); + + pOccupancyGrid->GetCoordinateConverter()->SetSize(GetCoordinateConverter()->GetSize()); + pOccupancyGrid->m_pCellPassCnt = m_pCellPassCnt->Clone(); + pOccupancyGrid->m_pCellHitsCnt = m_pCellHitsCnt->Clone(); + + return pOccupancyGrid; + } + + /** + * Check if grid point is free + * @param rPose + * @return whether the cell at the given point is free space + */ + virtual kt_bool IsFree(const Vector2& rPose) const + { + kt_int8u* pOffsets = reinterpret_cast(GetDataPointer(rPose)); + if (*pOffsets == GridStates_Free) + { + return true; + } + + return false; + } + + /** + * Casts a ray from the given point (up to the given max range) + * and returns the distance to the closest obstacle + * @param rPose2 + * @param maxRange + * @return distance to closest obstacle + */ + virtual kt_double RayCast(const Pose2& rPose2, kt_double maxRange) const + { + double scale = GetCoordinateConverter()->GetScale(); + + kt_double x = rPose2.GetX(); + kt_double y = rPose2.GetY(); + kt_double theta = rPose2.GetHeading(); + + kt_double sinTheta = sin(theta); + kt_double cosTheta = cos(theta); + + kt_double xStop = x + maxRange * cosTheta; + kt_double xSteps = 1 + fabs(xStop - x) * scale; + + kt_double yStop = y + maxRange * sinTheta; + kt_double ySteps = 1 + fabs(yStop - y) * scale; + + kt_double steps = math::Maximum(xSteps, ySteps); + kt_double delta = maxRange / steps; + kt_double distance = delta; + + for (kt_int32u i = 1; i < steps; i++) + { + kt_double x1 = x + distance * cosTheta; + kt_double y1 = y + distance * sinTheta; + + Vector2 gridIndex = WorldToGrid(Vector2(x1, y1)); + if (IsValidGridIndex(gridIndex) && IsFree(gridIndex)) + { + distance = (i + 1) * delta; + } + else + { + break; + } + } + + return (distance < maxRange)? distance : maxRange; + } + + /** + * Sets the minimum number of beams that must pass through a cell before it + * will be considered to be occupied or unoccupied. + * This prevents stray beams from messing up the map. + */ + void SetMinPassThrough(kt_int32u count) + { + m_pMinPassThrough->SetValue(count); + } + + /** + * Sets the minimum ratio of beams hitting cell to beams passing through + * cell for cell to be marked as occupied. + */ + void SetOccupancyThreshold(kt_double thresh) + { + m_pOccupancyThreshold->SetValue(thresh); + } + + protected: + /** + * Get cell hit grid + * @return Grid* + */ + virtual Grid* GetCellHitsCounts() + { + return m_pCellHitsCnt; + } + + /** + * Get cell pass grid + * @return Grid* + */ + virtual Grid* GetCellPassCounts() + { + return m_pCellPassCnt; + } + + protected: + /** + * Calculate grid dimensions from localized range scans + * @param rScans + * @param resolution + * @param rWidth + * @param rHeight + * @param rOffset + */ + static void ComputeDimensions(const LocalizedRangeScanVector& rScans, + kt_double resolution, + kt_int32s& rWidth, + kt_int32s& rHeight, + Vector2& rOffset) + { + BoundingBox2 boundingBox; + + const_forEach(LocalizedRangeScanVector, &rScans) + { + if (*iter == nullptr) + { + continue; + } + + boundingBox.Add((*iter)->GetBoundingBox()); + } + + kt_double scale = 1.0 / resolution; + Size2 size = boundingBox.GetSize(); + + rWidth = static_cast(math::Round(size.GetWidth() * scale)); + rHeight = static_cast(math::Round(size.GetHeight() * scale)); + rOffset = boundingBox.GetMinimum(); + } + + /** + * Create grid using scans + * @param rScans + */ + virtual void CreateFromScans(const LocalizedRangeScanVector& rScans) + { + m_pCellPassCnt->Resize(GetWidth(), GetHeight()); + m_pCellPassCnt->GetCoordinateConverter()->SetOffset(GetCoordinateConverter()->GetOffset()); + + m_pCellHitsCnt->Resize(GetWidth(), GetHeight()); + m_pCellHitsCnt->GetCoordinateConverter()->SetOffset(GetCoordinateConverter()->GetOffset()); + + const_forEach(LocalizedRangeScanVector, &rScans) + { + if (*iter == nullptr) + { + continue; + } + + LocalizedRangeScan* pScan = *iter; + AddScan(pScan); + } + + Update(); + } + + /** + * Adds the scan's information to this grid's counters (optionally + * update the grid's cells' occupancy status) + * @param pScan + * @param doUpdate whether to update the grid's cell's occupancy status + * @return returns false if an endpoint fell off the grid, otherwise true + */ + virtual kt_bool AddScan(LocalizedRangeScan* pScan, kt_bool doUpdate = false) + { + LaserRangeFinder* laserRangeFinder = pScan->GetLaserRangeFinder(); + kt_double rangeThreshold = laserRangeFinder->GetRangeThreshold(); + kt_double maxRange = laserRangeFinder->GetMaximumRange(); + kt_double minRange = laserRangeFinder->GetMinimumRange(); + + Vector2 scanPosition = pScan->GetSensorPose().GetPosition(); + // get scan point readings + const PointVectorDouble& rPointReadings = pScan->GetPointReadings(false); + + kt_bool isAllInMap = true; + + // draw lines from scan position to all point readings + int pointIndex = 0; + const_forEachAs(PointVectorDouble, &rPointReadings, pointsIter) + { + Vector2 point = *pointsIter; + kt_double rangeReading = pScan->GetRangeReadings()[pointIndex]; + kt_bool isEndPointValid = rangeReading < (rangeThreshold - KT_TOLERANCE); + + if (rangeReading <= minRange || rangeReading >= maxRange || std::isnan(rangeReading)) + { + // ignore these readings + pointIndex++; + continue; + } + else if (rangeReading >= rangeThreshold) + { + // trace up to range reading + kt_double ratio = rangeThreshold / rangeReading; + kt_double dx = point.GetX() - scanPosition.GetX(); + kt_double dy = point.GetY() - scanPosition.GetY(); + point.SetX(scanPosition.GetX() + ratio * dx); + point.SetY(scanPosition.GetY() + ratio * dy); + } + + kt_bool isInMap = RayTrace(scanPosition, point, isEndPointValid, doUpdate); + if (!isInMap) + { + isAllInMap = false; + } + + pointIndex++; + } + + return isAllInMap; + } + + /** + * Traces a beam from the start position to the end position marking + * the bookkeeping arrays accordingly. + * @param rWorldFrom start position of beam + * @param rWorldTo end position of beam + * @param isEndPointValid is the reading within the range threshold? + * @param doUpdate whether to update the cells' occupancy status immediately + * @return returns false if an endpoint fell off the grid, otherwise true + */ + virtual kt_bool RayTrace(const Vector2& rWorldFrom, + const Vector2& rWorldTo, + kt_bool isEndPointValid, + kt_bool doUpdate = false) + { + assert(m_pCellPassCnt != NULL && m_pCellHitsCnt != NULL); + + Vector2 gridFrom = m_pCellPassCnt->WorldToGrid(rWorldFrom); + Vector2 gridTo = m_pCellPassCnt->WorldToGrid(rWorldTo); + + CellUpdater* pCellUpdater = doUpdate ? m_pCellUpdater : NULL; + m_pCellPassCnt->TraceLine(gridFrom.GetX(), gridFrom.GetY(), gridTo.GetX(), gridTo.GetY(), pCellUpdater); + + // for the end point + if (isEndPointValid) + { + if (m_pCellPassCnt->IsValidGridIndex(gridTo)) + { + kt_int32s index = m_pCellPassCnt->GridIndex(gridTo, false); + + kt_int32u* pCellPassCntPtr = m_pCellPassCnt->GetDataPointer(); + kt_int32u* pCellHitCntPtr = m_pCellHitsCnt->GetDataPointer(); + + // increment cell pass through and hit count + pCellPassCntPtr[index]++; + pCellHitCntPtr[index]++; + + if (doUpdate) + { + (*m_pCellUpdater)(index); + } + } + } + + return m_pCellPassCnt->IsValidGridIndex(gridTo); + } + + /** + * Updates a single cell's value based on the given counters + * @param pCell + * @param cellPassCnt + * @param cellHitCnt + */ + virtual void UpdateCell(kt_int8u* pCell, kt_int32u cellPassCnt, kt_int32u cellHitCnt) + { + if (cellPassCnt > m_pMinPassThrough->GetValue()) + { + kt_double hitRatio = static_cast(cellHitCnt) / static_cast(cellPassCnt); + + if (hitRatio > m_pOccupancyThreshold->GetValue()) + { + *pCell = GridStates_Occupied; + } + else + { + *pCell = GridStates_Free; + } + } + } + + /** + * Update the grid based on the values in m_pCellHitsCnt and m_pCellPassCnt + */ + virtual void Update() + { + assert(m_pCellPassCnt != NULL && m_pCellHitsCnt != NULL); + + // clear grid + Clear(); + + // set occupancy status of cells + kt_int8u* pDataPtr = GetDataPointer(); + kt_int32u* pCellPassCntPtr = m_pCellPassCnt->GetDataPointer(); + kt_int32u* pCellHitCntPtr = m_pCellHitsCnt->GetDataPointer(); + + kt_int32u nBytes = GetDataSize(); + for (kt_int32u i = 0; i < nBytes; i++, pDataPtr++, pCellPassCntPtr++, pCellHitCntPtr++) + { + UpdateCell(pDataPtr, *pCellPassCntPtr, *pCellHitCntPtr); + } + } + + /** + * Resizes the grid (deletes all old data) + * @param width + * @param height + */ + virtual void Resize(kt_int32s width, kt_int32s height) + { + Grid::Resize(width, height); + m_pCellPassCnt->Resize(width, height); + m_pCellHitsCnt->Resize(width, height); + } + + protected: + /** + * Counters of number of times a beam passed through a cell + */ + Grid* m_pCellPassCnt; + + /** + * Counters of number of times a beam ended at a cell + */ + Grid* m_pCellHitsCnt; + + private: + /** + * Restrict the copy constructor + */ + OccupancyGrid(const OccupancyGrid&); + + /** + * Restrict the assignment operator + */ + const OccupancyGrid& operator=(const OccupancyGrid&); + + private: + CellUpdater* m_pCellUpdater; + + //////////////////////////////////////////////////////////// + // NOTE: These two values are dependent on the resolution. If the resolution is too small, + // then not many beams will hit the cell! + + // Number of beams that must pass through a cell before it will be considered to be occupied + // or unoccupied. This prevents stray beams from messing up the map. + Parameter* m_pMinPassThrough; + + // Minimum ratio of beams hitting cell to beams passing through cell for cell to be marked as occupied + Parameter* m_pOccupancyThreshold; + }; // OccupancyGrid + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + /** + * Dataset info + * Contains title, author and other information about the dataset + */ + class DatasetInfo : public Object + { + public: + // @cond EXCLUDE + KARTO_Object(DatasetInfo) + // @endcond + + public: + DatasetInfo() + : Object() + { + m_pTitle = new Parameter("Title", "", GetParameterManager()); + m_pAuthor = new Parameter("Author", "", GetParameterManager()); + m_pDescription = new Parameter("Description", "", GetParameterManager()); + m_pCopyright = new Parameter("Copyright", "", GetParameterManager()); + } + + virtual ~DatasetInfo() + { + } + + public: + /** + * Dataset title + */ + const std::string& GetTitle() const + { + return m_pTitle->GetValue(); + } + + /** + * Dataset author(s) + */ + const std::string& GetAuthor() const + { + return m_pAuthor->GetValue(); + } + + /** + * Dataset description + */ + const std::string& GetDescription() const + { + return m_pDescription->GetValue(); + } + + /** + * Dataset copyrights + */ + const std::string& GetCopyright() const + { + return m_pCopyright->GetValue(); + } + + /** + * Serialization: class DatasetInfo + */ + + private: + DatasetInfo(const DatasetInfo&); + const DatasetInfo& operator=(const DatasetInfo&); + + private: + Parameter* m_pTitle; + Parameter* m_pAuthor; + Parameter* m_pDescription; + Parameter* m_pCopyright; + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Object); + ar & BOOST_SERIALIZATION_NVP(*m_pTitle); + ar & BOOST_SERIALIZATION_NVP(*m_pAuthor); + ar & BOOST_SERIALIZATION_NVP(*m_pDescription); + ar & BOOST_SERIALIZATION_NVP(*m_pCopyright); + } + }; // class DatasetInfo + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + /** + * Karto dataset. Stores LaserRangeFinders and LocalizedRangeScans and manages memory of allocated LaserRangeFinders + * and LocalizedRangeScans + */ + class Dataset + { + public: + /** + * Default constructor + */ + Dataset() + : m_pDatasetInfo(NULL) + { + } + + /** + * Destructor + */ + virtual ~Dataset() + { + Clear(); + } + + /** + * Save dataset to file + * @param filename + */ + void SaveToFile(const std::string& filename) + { + printf("Save To File\n"); + std::ofstream ofs(filename.c_str()); + boost::archive::binary_oarchive oa(ofs, boost::archive::no_codecvt); + oa << BOOST_SERIALIZATION_NVP(*this); + } + + /** + * Load dataset from file + * @param filename + */ + void LoadFromFile(const std::string& filename) + { + printf("Load From File\n"); + std::ifstream ifs(filename.c_str()); + boost::archive::binary_iarchive ia(ifs, boost::archive::no_codecvt); //no second arg? + ia >> BOOST_SERIALIZATION_NVP(*this); + } + + public: + /** + * Adds object to this dataset + * @param pObject + */ + void Add(Object* pObject, kt_bool overrideSensorName = false) + { + if (pObject != NULL) + { + if (dynamic_cast(pObject)) + { + Sensor* pSensor = dynamic_cast(pObject); + if (pSensor != NULL) + { + m_SensorNameLookup[pSensor->GetName()] = pSensor; + karto::SensorManager::GetInstance()->RegisterSensor(pSensor, overrideSensorName); + } + + m_Lasers.push_back(pObject); + } + else if (dynamic_cast(pObject)) + { + SensorData* pSensorData = dynamic_cast(pObject); + m_Data.insert({pSensorData->GetUniqueId(), pSensorData}); + } + else if (dynamic_cast(pObject)) + { + m_pDatasetInfo = dynamic_cast(pObject); + } + else + { + std::cout << "Did not save object of non-data and non-sensor type" << std::endl; + } + } + } + + /** + * Get sensor states + * @return sensor state + */ + inline const ObjectVector& GetLasers() const + { + return m_Lasers; + } + + /** + * Get data states + * @return data state + */ + inline const DataMap& GetData() const + { + return m_Data; + } + + /** + * Remove data + * @param index to remove + */ + inline void RemoveData(LocalizedRangeScan* scan) + { + auto iterator = m_Data.find(scan->GetUniqueId()); + if (iterator != m_Data.end()) + { + delete iterator->second; + iterator->second = nullptr; + m_Data.erase(iterator); + } + else + { + std::cout << "Failed to remove data. Pointer to LocalizedRangeScan could not be found in dataset. " + << "Most likely different pointer address but same object TODO STEVE." << std::endl; + } + } + + /** + * Get dataset info + * @return dataset info + */ + inline DatasetInfo* GetDatasetInfo() + { + return m_pDatasetInfo; + } + + /** + * Delete all stored data + */ + virtual void Clear() + { + for (std::map::iterator iter = m_SensorNameLookup.begin(); iter != m_SensorNameLookup.end(); ++iter) + { + karto::SensorManager::GetInstance()->UnregisterSensor(iter->second); + } + + forEach(ObjectVector, &m_Lasers) + { + if(*iter) + { + delete *iter; + *iter = NULL; + } + } + + for (auto iter = m_Data.begin(); iter != m_Data.end(); ++iter) + { + if(iter->second) + { + delete iter->second; + iter->second = NULL; + } + } + + m_Lasers.clear(); + m_Data.clear(); + + if (m_pDatasetInfo != NULL) + { + delete m_pDatasetInfo; + m_pDatasetInfo = NULL; + } + } + + private: + std::map m_SensorNameLookup; + ObjectVector m_Lasers; + DataMap m_Data; + DatasetInfo* m_pDatasetInfo; + /** + * Serialization: class Dataset + */ + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + std::cout<<"**Serializing Dataset**\n"; + std::cout<<"Dataset <- m_SensorNameLookup\n"; + ar & BOOST_SERIALIZATION_NVP(m_SensorNameLookup); + std::cout<<"Dataset <- m_Data\n"; + ar & BOOST_SERIALIZATION_NVP(m_Data); + std::cout<<"Dataset <- m_Lasers\n"; + ar & BOOST_SERIALIZATION_NVP(m_Lasers); + std::cout<<"Dataset <- m_pDatasetInfo\n"; + ar & BOOST_SERIALIZATION_NVP(m_pDatasetInfo); + std::cout<<"**Finished serializing Dataset**\n"; + } + + }; // Dataset + BOOST_SERIALIZATION_ASSUME_ABSTRACT(Dataset) + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + /** + * An array that can be resized as long as the size + * does not exceed the initial capacity + */ + class LookupArray + { + public: + /** + * Constructs lookup array + */ + LookupArray() + : m_pArray(NULL) + , m_Capacity(0) + , m_Size(0) + { + } + + /** + * Destructor + */ + virtual ~LookupArray() + { + assert(m_pArray != NULL); + + delete[] m_pArray; + m_pArray = NULL; + } + + public: + /** + * Clear array + */ + void Clear() + { + memset(m_pArray, 0, sizeof(kt_int32s) * m_Capacity); + } + + /** + * Gets size of array + * @return array size + */ + kt_int32u GetSize() const + { + return m_Size; + } + + /** + * Sets size of array (resize if not big enough) + * @param size + */ + void SetSize(kt_int32u size) + { + assert(size != 0); + + if (size > m_Capacity) + { + if (m_pArray != NULL) + { + delete [] m_pArray; + } + m_Capacity = size; + m_pArray = new kt_int32s[m_Capacity]; + } + + m_Size = size; + } + + /** + * Gets reference to value at given index + * @param index + * @return reference to value at index + */ + inline kt_int32s& operator [] (kt_int32u index) + { + assert(index < m_Size); + + return m_pArray[index]; + } + + /** + * Gets value at given index + * @param index + * @return value at index + */ + inline kt_int32s operator [] (kt_int32u index) const + { + assert(index < m_Size); + + return m_pArray[index]; + } + + /** + * Gets array pointer + * @return array pointer + */ + inline kt_int32s* GetArrayPointer() + { + return m_pArray; + } + + /** + * Gets array pointer + * @return array pointer + */ + inline kt_int32s* GetArrayPointer() const + { + return m_pArray; + } + + private: + kt_int32s* m_pArray; + kt_int32u m_Capacity; + kt_int32u m_Size; + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + ar & BOOST_SERIALIZATION_NVP(m_Capacity); + ar & BOOST_SERIALIZATION_NVP(m_Size); + if (Archive::is_loading::value) + { + m_pArray = new kt_int32s[m_Capacity]; + } + ar & boost::serialization::make_array(m_pArray, m_Capacity); + + + } + }; // LookupArray + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + /** + * Create lookup tables for point readings at varying angles in grid. + * For each angle, grid indexes are calculated for each range reading. + * This is to speed up finding best angle/position for a localized range scan + * + * Used heavily in mapper and localizer. + * + * In the localizer, this is a huge speed up for calculating possible position. For each particle, + * a probability is calculated. The range scan is the same, but all grid indexes at all possible angles are + * calculated. So when calculating the particle probability at a specific angle, the index table is used + * to look up probability in probability grid! + * + */ + template + class GridIndexLookup + { + public: + /** + * Construct a GridIndexLookup with a grid + * @param pGrid + */ + GridIndexLookup() + { + } + GridIndexLookup(Grid* pGrid) + : m_pGrid(pGrid) + , m_Capacity(0) + , m_Size(0) + , m_ppLookupArray(NULL) + { + } + + /** + * Destructor + */ + virtual ~GridIndexLookup() + { + DestroyArrays(); + } + + public: + /** + * Gets the lookup array for a particular angle index + * @param index + * @return lookup array + */ + const LookupArray* GetLookupArray(kt_int32u index) const + { + assert(math::IsUpTo(index, m_Size)); + + return m_ppLookupArray[index]; + } + + /** + * Get angles + * @return std::vector& angles + */ + const std::vector& GetAngles() const + { + return m_Angles; + } + + /** + * Compute lookup table of the points of the given scan for the given angular space + * @param pScan the scan + * @param angleCenter + * @param angleOffset computes lookup arrays for the angles within this offset around angleStart + * @param angleResolution how fine a granularity to compute lookup arrays in the angular space + */ + void ComputeOffsets(LocalizedRangeScan* pScan, + kt_double angleCenter, + kt_double angleOffset, + kt_double angleResolution) + { + assert(angleOffset != 0.0); + assert(angleResolution != 0.0); + + kt_int32u nAngles = static_cast(math::Round(angleOffset * 2.0 / angleResolution) + 1); + SetSize(nAngles); + + ////////////////////////////////////////////////////// + // convert points into local coordinates of scan pose + + const PointVectorDouble& rPointReadings = pScan->GetPointReadings(); + + // compute transform to scan pose + Transform transform(pScan->GetSensorPose()); + + Pose2Vector localPoints; + const_forEach(PointVectorDouble, &rPointReadings) + { + // do inverse transform to get points in local coordinates + Pose2 vec = transform.InverseTransformPose(Pose2(*iter, 0.0)); + localPoints.push_back(vec); + } + + ////////////////////////////////////////////////////// + // create lookup array for different angles + kt_double angle = 0.0; + kt_double startAngle = angleCenter - angleOffset; + for (kt_int32u angleIndex = 0; angleIndex < nAngles; angleIndex++) + { + angle = startAngle + angleIndex * angleResolution; + ComputeOffsets(angleIndex, angle, localPoints, pScan); + } + // assert(math::DoubleEqual(angle, angleCenter + angleOffset)); + } + + private: + /** + * Compute lookup value of points for given angle + * @param angleIndex + * @param angle + * @param rLocalPoints + */ + void ComputeOffsets(kt_int32u angleIndex, kt_double angle, const Pose2Vector& rLocalPoints, LocalizedRangeScan* pScan) + { + m_ppLookupArray[angleIndex]->SetSize(static_cast(rLocalPoints.size())); + m_Angles.at(angleIndex) = angle; + + // set up point array by computing relative offsets to points readings + // when rotated by given angle + + const Vector2& rGridOffset = m_pGrid->GetCoordinateConverter()->GetOffset(); + + kt_double cosine = cos(angle); + kt_double sine = sin(angle); + + kt_int32u readingIndex = 0; + + kt_int32s* pAngleIndexPointer = m_ppLookupArray[angleIndex]->GetArrayPointer(); + + kt_double maxRange = pScan->GetLaserRangeFinder()->GetMaximumRange(); + + const_forEach(Pose2Vector, &rLocalPoints) + { + const Vector2& rPosition = iter->GetPosition(); + + if (std::isnan(pScan->GetRangeReadings()[readingIndex]) || std::isinf(pScan->GetRangeReadings()[readingIndex])) + { + pAngleIndexPointer[readingIndex] = INVALID_SCAN; + readingIndex++; + continue; + } + + + // counterclockwise rotation and that rotation is about the origin (0, 0). + Vector2 offset; + offset.SetX(cosine * rPosition.GetX() - sine * rPosition.GetY()); + offset.SetY(sine * rPosition.GetX() + cosine * rPosition.GetY()); + + // have to compensate for the grid offset when getting the grid index + Vector2 gridPoint = m_pGrid->WorldToGrid(offset + rGridOffset); + + // use base GridIndex to ignore ROI + kt_int32s lookupIndex = m_pGrid->Grid::GridIndex(gridPoint, false); + + pAngleIndexPointer[readingIndex] = lookupIndex; + + readingIndex++; + } + assert(readingIndex == rLocalPoints.size()); + } + + /** + * Sets size of lookup table (resize if not big enough) + * @param size + */ + void SetSize(kt_int32u size) + { + assert(size != 0); + + if (size > m_Capacity) + { + if (m_ppLookupArray != NULL) + { + DestroyArrays(); + } + + m_Capacity = size; + m_ppLookupArray = new LookupArray*[m_Capacity]; + for (kt_int32u i = 0; i < m_Capacity; i++) + { + m_ppLookupArray[i] = new LookupArray(); + } + } + + m_Size = size; + + m_Angles.resize(size); + } + + /** + * Delete the arrays + */ + void DestroyArrays() + { + if (m_ppLookupArray) + { + for (kt_int32u i = 0; i < m_Capacity; i++) + { + delete m_ppLookupArray[i]; + } + } + if (m_ppLookupArray) + { + delete[] m_ppLookupArray; + m_ppLookupArray = NULL; + } + } + + private: + Grid* m_pGrid; + + kt_int32u m_Capacity; + kt_int32u m_Size; + + LookupArray **m_ppLookupArray; + + // for sanity check + std::vector m_Angles; + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + ar & BOOST_SERIALIZATION_NVP(m_pGrid); + ar & BOOST_SERIALIZATION_NVP(m_Capacity); + ar & BOOST_SERIALIZATION_NVP(m_Size); + ar & BOOST_SERIALIZATION_NVP(m_Angles); + if (Archive::is_loading::value) + { + m_ppLookupArray = new LookupArray*[m_Capacity]; + for (kt_int32u i = 0; i < m_Capacity; i++) + { + m_ppLookupArray[i] = new LookupArray(); + } + } + ar & boost::serialization::make_array(m_ppLookupArray, m_Capacity); + } + }; // class GridIndexLookup + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + inline Pose2::Pose2(const Pose3& rPose) + : m_Position(rPose.GetPosition().GetX(), rPose.GetPosition().GetY()) + { + kt_double t1, t2; + + // calculates heading from orientation + rPose.GetOrientation().ToEulerAngles(m_Heading, t1, t2); + } + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + // @cond EXCLUDE + + template + inline void Object::SetParameter(const std::string& rName, T value) + { + AbstractParameter* pParameter = GetParameter(rName); + if (pParameter != NULL) + { + std::stringstream stream; + stream << value; + pParameter->SetValueFromString(stream.str()); + } + else + { + throw Exception("Parameter does not exist: " + rName); + } + } + + template<> + inline void Object::SetParameter(const std::string& rName, kt_bool value) + { + AbstractParameter* pParameter = GetParameter(rName); + if (pParameter != NULL) + { + pParameter->SetValueFromString(value ? "true" : "false"); + } + else + { + throw Exception("Parameter does not exist: " + rName); + } + } + + // @endcond + + /*@}*/ +}; // namespace karto + +BOOST_CLASS_EXPORT_KEY(karto::NonCopyable); +BOOST_CLASS_EXPORT_KEY(karto::Object); +BOOST_CLASS_EXPORT_KEY(karto::Sensor); +BOOST_CLASS_EXPORT_KEY(karto::Name); +BOOST_CLASS_EXPORT_KEY(karto::SensorData); +BOOST_CLASS_EXPORT_KEY(karto::LocalizedRangeScan); +BOOST_CLASS_EXPORT_KEY(karto::LaserRangeScan); +BOOST_CLASS_EXPORT_KEY(karto::LaserRangeFinder); +BOOST_CLASS_EXPORT_KEY(karto::CustomData); +BOOST_CLASS_EXPORT_KEY(karto::Module); +BOOST_CLASS_EXPORT_KEY(karto::Rectangle2); +BOOST_CLASS_EXPORT_KEY(karto::CoordinateConverter); +BOOST_CLASS_EXPORT_KEY(karto::Dataset); +BOOST_CLASS_EXPORT_KEY(karto::SensorManager); +BOOST_CLASS_EXPORT_KEY(karto::Size2); +BOOST_CLASS_EXPORT_KEY(karto::GridIndexLookup); +BOOST_CLASS_EXPORT_KEY(karto::LookupArray); +BOOST_CLASS_EXPORT_KEY(karto::AbstractParameter); +BOOST_CLASS_EXPORT_KEY(karto::ParameterEnum); +BOOST_CLASS_EXPORT_KEY(karto::Parameters); +BOOST_CLASS_EXPORT_KEY(karto::ParameterManager); +BOOST_CLASS_EXPORT_KEY(karto::Parameter); +BOOST_CLASS_EXPORT_KEY(karto::Parameter); +BOOST_CLASS_EXPORT_KEY(karto::Parameter); +BOOST_CLASS_EXPORT_KEY(karto::Parameter); +BOOST_CLASS_EXPORT_KEY(karto::Parameter); +BOOST_CLASS_EXPORT_KEY(karto::Parameter); + +#endif // karto_sdk_KARTO_H diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox/lib/karto_sdk/include/karto_sdk/Macros.h b/Localizations/Packages/slam_toolbox/slam_toolbox/lib/karto_sdk/include/karto_sdk/Macros.h new file mode 100644 index 0000000..0d397c9 --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox/lib/karto_sdk/include/karto_sdk/Macros.h @@ -0,0 +1,127 @@ +/* + * Copyright 2010 SRI International + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with this program. If not, see . + */ + +#ifndef karto_sdk_MACROS_H +#define karto_sdk_MACROS_H + +//////////////////////////////////////////////////////////////////////////////////////// +//////////////////////////////////////////////////////////////////////////////////////// +//////////////////////////////////////////////////////////////////////////////////////// + +/** + * Karto defines for handling deprecated code + */ +#ifndef KARTO_DEPRECATED +# if defined(__GNUC__) && (__GNUC__ >= 4 || (__GNUC__ == 3 && __GNUC_MINOR__>=1)) +# define KARTO_DEPRECATED __attribute__((deprecated)) +# elif defined(__INTEL) || defined(_MSC_VER) +# define KARTO_DEPRECATED __declspec(deprecated) +# else +# define KARTO_DEPRECATED +# endif +#endif + +//////////////////////////////////////////////////////////////////////////////////////// +//////////////////////////////////////////////////////////////////////////////////////// +//////////////////////////////////////////////////////////////////////////////////////// + +/** + * Karto defines for windows dynamic build + */ +#if defined(_MSC_VER) || defined(__CYGWIN__) || defined(__MINGW32__) || defined( __BCPLUSPLUS__) || defined( __MWERKS__) +# if defined( _LIB ) || defined( KARTO_STATIC ) || defined( STATIC_BUILD ) +# define KARTO_EXPORT +# else +# ifdef KARTO_DYNAMIC +# define KARTO_EXPORT __declspec(dllexport) +# else +# define KARTO_EXPORT __declspec(dllimport) +# endif // KARTO_DYNAMIC +# endif +#else +# define KARTO_EXPORT +#endif + +//////////////////////////////////////////////////////////////////////////////////////// +//////////////////////////////////////////////////////////////////////////////////////// +//////////////////////////////////////////////////////////////////////////////////////// + +/** + * Helper defines for std iterator loops + */ +#define forEach( listtype, list ) \ + for ( listtype::iterator iter = (list)->begin(); iter != (list)->end(); ++iter ) + +#define forEachAs( listtype, list, iter ) \ + for ( listtype::iterator iter = (list)->begin(); iter != (list)->end(); ++iter ) + +#define const_forEach( listtype, list ) \ + for ( listtype::const_iterator iter = (list)->begin(); iter != (list)->end(); ++iter ) + +#define const_forEachAs( listtype, list, iter ) \ + for ( listtype::const_iterator iter = (list)->begin(); iter != (list)->end(); ++iter ) + +#define forEachR( listtype, list ) \ + for ( listtype::reverse_iterator iter = (list)->rbegin(); iter != (list)->rend(); ++iter ) + +#define const_forEachR( listtype, list ) \ + for ( listtype::const_reverse_iterator iter = (list)->rbegin(); iter != (list)->rend(); ++iter ) + + +//////////////////////////////////////////////////////////////////////////////////////// +//////////////////////////////////////////////////////////////////////////////////////// +//////////////////////////////////////////////////////////////////////////////////////// + +/** + * Disable annoying compiler warnings + */ + +#if defined(__INTEL) || defined(_MSC_VER) + +// Disable the warning: 'identifier' : class 'type' needs to have dll-interface to be used by clients of class 'type2' +#pragma warning(disable:4251) + +#endif + +#ifdef __INTEL_COMPILER + +// Disable the warning: conditional expression is constant +#pragma warning(disable:4127) + +// Disable the warning: 'identifier' : unreferenced formal parameter +#pragma warning(disable:4100) + +// remark #383: value copied to temporary, reference to temporary used +#pragma warning(disable:383) + +// remark #981: operands are evaluated in unspecified order +// disabled -> completely pointless if the functions do not have side effects +#pragma warning(disable:981) + +// remark #1418: external function definition with no prior declaration +#pragma warning(disable:1418) + +// remark #1572: floating-point equality and inequality comparisons are unreliable +// disabled -> everyone knows it, the parser passes this problem deliberately to the user +#pragma warning(disable:1572) + +// remark #10121: +#pragma warning(disable:10121) + +#endif // __INTEL_COMPILER + +#endif // karto_sdk_MACROS_H diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox/lib/karto_sdk/include/karto_sdk/Mapper.h b/Localizations/Packages/slam_toolbox/slam_toolbox/lib/karto_sdk/include/karto_sdk/Mapper.h new file mode 100644 index 0000000..2f87fb3 --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox/lib/karto_sdk/include/karto_sdk/Mapper.h @@ -0,0 +1,2470 @@ +/* + * Copyright 2010 SRI International + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with this program. If not, see . + */ + +#ifndef karto_sdk_MAPPER_H +#define karto_sdk_MAPPER_H + +#include +#include +#include +#include + +#include + +#include "tbb/parallel_for.h" +#include "tbb/parallel_do.h" +#include "tbb/blocked_range.h" +#include +#include + +#include + +#include "nanoflann_adaptors.h" + +namespace karto +{ + //////////////////////////////////////////////////////////////////////////////////////// + // Listener classes + + /** + * Abstract class to listen to mapper general messages + */ + class MapperListener + { + public: + /** + * Called with general message + */ + virtual void Info(const std::string& /*rInfo*/) {}; + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + } + }; + BOOST_SERIALIZATION_ASSUME_ABSTRACT(MapperListener) + /** + * Abstract class to listen to mapper debug messages + */ + class MapperDebugListener + { + public: + /** + * Called with debug message + */ + virtual void Debug(const std::string& /*rInfo*/) {}; + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + } + }; + BOOST_SERIALIZATION_ASSUME_ABSTRACT(MapperDebugListener) + /** + * Abstract class to listen to mapper loop closure messages + */ + class MapperLoopClosureListener : public MapperListener + { + public: + /** + * Called when checking for loop closures + */ + virtual void LoopClosureCheck(const std::string& /*rInfo*/) {}; + + /** + * Called when loop closure is starting + */ + virtual void BeginLoopClosure(const std::string& /*rInfo*/) {}; + + /** + * Called when loop closure is over + */ + virtual void EndLoopClosure(const std::string& /*rInfo*/) {}; + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + } + }; // MapperLoopClosureListener + BOOST_SERIALIZATION_ASSUME_ABSTRACT(MapperLoopClosureListener) + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + /** + * Class for edge labels + */ + class EdgeLabel + { + public: + /** + * Default constructor + */ + EdgeLabel() + { + } + + /** + * Destructor + */ + virtual ~EdgeLabel() + { + } + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + } + }; // EdgeLabel + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + // A LinkInfo object contains the requisite information for the "spring" + // that links two scans together--the pose difference and the uncertainty + // (represented by a covariance matrix). + class LinkInfo : public EdgeLabel + { + public: + /** + * Constructs a link between the given poses + * @param rPose1 + * @param rPose2 + * @param rCovariance + */ + LinkInfo() + { + } + LinkInfo(const Pose2& rPose1, const Pose2& rPose2, const Matrix3& rCovariance) + { + Update(rPose1, rPose2, rCovariance); + } + + /** + * Destructor + */ + virtual ~LinkInfo() + { + } + + public: + /** + * Changes the link information to be the given parameters + * @param rPose1 + * @param rPose2 + * @param rCovariance + */ + void Update(const Pose2& rPose1, const Pose2& rPose2, const Matrix3& rCovariance) + { + m_Pose1 = rPose1; + m_Pose2 = rPose2; + + // transform second pose into the coordinate system of the first pose + Transform transform(rPose1, Pose2()); + m_PoseDifference = transform.TransformPose(rPose2); + + // transform covariance into reference of first pose + Matrix3 rotationMatrix; + rotationMatrix.FromAxisAngle(0, 0, 1, -rPose1.GetHeading()); + + m_Covariance = rotationMatrix * rCovariance * rotationMatrix.Transpose(); + } + + /** + * Gets the first pose + * @return first pose + */ + inline const Pose2& GetPose1() + { + return m_Pose1; + } + + /** + * Gets the second pose + * @return second pose + */ + inline const Pose2& GetPose2() + { + return m_Pose2; + } + + /** + * Gets the pose difference + * @return pose difference + */ + inline const Pose2& GetPoseDifference() + { + return m_PoseDifference; + } + + /** + * Gets the link covariance + * @return link covariance + */ + inline const Matrix3& GetCovariance() + { + return m_Covariance; + } + + private: + Pose2 m_Pose1; + Pose2 m_Pose2; + Pose2 m_PoseDifference; + Matrix3 m_Covariance; + + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(EdgeLabel); + ar & BOOST_SERIALIZATION_NVP(m_Pose1); + ar & BOOST_SERIALIZATION_NVP(m_Pose2); + ar & BOOST_SERIALIZATION_NVP(m_PoseDifference); + ar & BOOST_SERIALIZATION_NVP(m_Covariance); + } + }; // LinkInfo + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + template + class Edge; + + /** + * Represents an object in a graph + */ + template + class Vertex + { + friend class Edge; + + public: + /** + * Constructs a vertex representing the given object + * @param pObject + */ + Vertex() + : m_pObject(NULL), m_Score(1.0) + { + } + Vertex(T* pObject) + : m_pObject(pObject), m_Score(1.0) + { + } + + /** + * Destructor + */ + virtual ~Vertex() + { + } + + /** + * Gets edges adjacent to this vertex + * @return adjacent edges + */ + inline const std::vector*>& GetEdges() const + { + return m_Edges; + } + + /** + * Removes an edge at a position + */ + inline void RemoveEdge(const int& idx) + { + m_Edges[idx] = NULL; + m_Edges.erase(m_Edges.begin() + idx); + return; + } + + /** + * Gets score for vertex + * @return score + */ + inline const double GetScore() const + { + return m_Score; + } + + /** + * Sets score for vertex + * @return adjacent edges + */ + inline void SetScore(const double score) + { + m_Score = score; + } + + /** + * Gets the object associated with this vertex + * @return the object + */ + inline T* GetObject() const + { + return m_pObject; + } + + /** + * Deletes the object held by this vertex + */ + inline void RemoveObject() + { + m_pObject = NULL; + } + + /** + * Gets a vector of the vertices adjacent to this vertex + * @return adjacent vertices + */ + std::vector*> GetAdjacentVertices() const + { + std::vector*> vertices; + + const_forEach(typename std::vector*>, &m_Edges) + { + Edge* pEdge = *iter; + + if (pEdge == NULL) + { + continue; + } + + // check both source and target because we have a undirected graph + if (pEdge->GetSource() != this) + { + vertices.push_back(pEdge->GetSource()); + } + + if (pEdge->GetTarget() != this) + { + vertices.push_back(pEdge->GetTarget()); + } + } + + return vertices; + } + + private: + /** + * Adds the given edge to this vertex's edge list + * @param pEdge edge to add + */ + inline void AddEdge(Edge* pEdge) + { + m_Edges.push_back(pEdge); + } + + T* m_pObject; + std::vector*> m_Edges; + kt_double m_Score; + + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + ar & BOOST_SERIALIZATION_NVP(m_pObject); + ar & BOOST_SERIALIZATION_NVP(m_Edges); + ar & BOOST_SERIALIZATION_NVP(m_Score); + } + }; // Vertex + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + /** + * Represents an edge in a graph + */ + template + class Edge + { + public: + /** + * Constructs an edge from the source to target vertex + * @param pSource + * @param pTarget + */ + Edge() + : m_pSource(NULL) + , m_pTarget(NULL) + , m_pLabel(NULL) + { + } + Edge(Vertex* pSource, Vertex* pTarget) + : m_pSource(pSource) + , m_pTarget(pTarget) + , m_pLabel(NULL) + { + m_pSource->AddEdge(this); + m_pTarget->AddEdge(this); + } + + /** + * Destructor + */ + virtual ~Edge() + { + m_pSource = NULL; + m_pTarget = NULL; + + if (m_pLabel != NULL) + { + delete m_pLabel; + m_pLabel = NULL; + } + } + + public: + /** + * Gets the source vertex + * @return source vertex + */ + inline Vertex* GetSource() const + { + return m_pSource; + } + + /** + * Gets the target vertex + * @return target vertex + */ + inline Vertex* GetTarget() const + { + return m_pTarget; + } + + /** + * Gets the link info + * @return link info + */ + inline EdgeLabel* GetLabel() + { + return m_pLabel; + } + + /** + * Sets the link payload + * @param pLabel + */ + inline void SetLabel(EdgeLabel* pLabel) + { + m_pLabel = pLabel; + } + + private: + Vertex* m_pSource; + Vertex* m_pTarget; + EdgeLabel* m_pLabel; + + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + ar & BOOST_SERIALIZATION_NVP(m_pSource); + ar & BOOST_SERIALIZATION_NVP(m_pTarget); + ar & BOOST_SERIALIZATION_NVP(m_pLabel); + } + }; // class Edge + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + /** + * Visitor class + */ + template + class Visitor + { + public: + /** + * Applies the visitor to the vertex + * @param pVertex + * @return true if the visitor accepted the vertex, false otherwise + */ + virtual kt_bool Visit(Vertex* pVertex) = 0; + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + } + }; // Visitor + BOOST_SERIALIZATION_ASSUME_ABSTRACT(Visitor) + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + template + class Graph; + + /** + * Graph traversal algorithm + */ + template + class GraphTraversal + { + public: + GraphTraversal() + { + } + GraphTraversal(Graph* pGraph) + : m_pGraph(pGraph) + { + } + + virtual ~GraphTraversal() + { + } + + public: + + virtual std::vector TraverseForScans(Vertex* pStartVertex, Visitor* pVisitor) = 0; + virtual std::vector*> TraverseForVertices(Vertex* pStartVertex, Visitor* pVisitor) = 0; + + protected: + Graph* m_pGraph; + + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + ar & BOOST_SERIALIZATION_NVP(m_pGraph); + } + }; // GraphTraversal + BOOST_SERIALIZATION_ASSUME_ABSTRACT(GraphTraversal) + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + /** + * Graph + */ + template + class Graph + { + public: + /** + * Maps names to vector of vertices + */ + typedef std::map*> > VertexMap; + + public: + /** + * Default constructor + */ + Graph() + { + } + + /** + * Destructor + */ + virtual ~Graph() + { + Clear(); + } + + public: + /** + * Adds and indexes the given vertex into the map using the given name + * @param rName + * @param pVertex + */ + inline void AddVertex(const Name& rName, Vertex* pVertex) + { + m_Vertices[rName].insert({pVertex->GetObject()->GetStateId(), pVertex}); + } + + /** + * Removes a given vertex into the map using the given name + * @param rName + * @param pVertex + */ + inline void RemoveVertex(const Name& rName, const int& idx) + { + std::map* >::iterator it = m_Vertices[rName].find(idx); + if (it != m_Vertices[rName].end()) + { + it->second = NULL; + m_Vertices[rName].erase(it); + } + else + { + std::cout << "RemoveVertex: Failed to remove vertex " << idx + << " because it doesnt exist in m_Vertices." << std::endl; + } + } + + /** + * Adds an edge to the graph + * @param pEdge + */ + inline void AddEdge(Edge* pEdge) + { + m_Edges.push_back(pEdge); + } + + /** + * Removes an edge to the graph + * @param pEdge + */ + inline void RemoveEdge(const int& idx) + { + m_Edges[idx] = NULL; + m_Edges.erase(m_Edges.begin() + idx); + } + + + /** + * Deletes the graph data + */ + void Clear() + { + forEachAs(typename VertexMap, &m_Vertices, indexIter) + { + // delete each vertex + typename std::map*>::iterator iter; + for (iter = indexIter->second.begin(); iter != indexIter->second.end(); ++iter) + { + delete iter->second; + iter->second = nullptr; + } + } + m_Vertices.clear(); + + forEach(typename std::vector*>, &m_Edges) + { + delete *iter; + *iter = nullptr; + } + m_Edges.clear(); + } + + /** + * Gets the edges of this graph + * @return graph edges + */ + inline const std::vector*>& GetEdges() const + { + return m_Edges; + } + + /** + * Gets the vertices of this graph + * @return graph vertices + */ + inline const VertexMap& GetVertices() const + { + return m_Vertices; + } + + protected: + /** + * Map of names to vector of vertices + */ + VertexMap m_Vertices; + + /** + * Edges of this graph + */ + std::vector*> m_Edges; + /** + * Serialization: class Graph + */ + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + std::cout << "Graph <- m_Edges; "; + ar & BOOST_SERIALIZATION_NVP(m_Edges); + std::cout << "Graph <- m_Vertices\n"; + ar & BOOST_SERIALIZATION_NVP(m_Vertices); + } + }; // Graph + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + class Mapper; + class ScanMatcher; + + /** + * Graph for graph SLAM algorithm + */ + class KARTO_EXPORT MapperGraph : public Graph + { + public: + /** + * Graph for graph SLAM + * @param pMapper + * @param rangeThreshold + */ + MapperGraph(Mapper* pMapper, kt_double rangeThreshold); + MapperGraph() + { + } + /** + * Destructor + */ + virtual ~MapperGraph(); + + public: + /** + * Adds a vertex representing the given scan to the graph + * @param pScan + */ + Vertex* AddVertex(LocalizedRangeScan* pScan); + + /** + * Creates an edge between the source scan vertex and the target scan vertex if it + * does not already exist; otherwise return the existing edge + * @param pSourceScan + * @param pTargetScan + * @param rIsNewEdge set to true if the edge is new + * @return edge between source and target scan vertices + */ + Edge* AddEdge(LocalizedRangeScan* pSourceScan, + LocalizedRangeScan* pTargetScan, + kt_bool& rIsNewEdge); + + /** + * Link scan to last scan and nearby chains of scans + * @param pScan + * @param rCovariance uncertainty of match + */ + void AddEdges(LocalizedRangeScan* pScan, const Matrix3& rCovariance); + + /** + * Tries to close a loop using the given scan with the scans from the given device + * @param pScan + * @param rSensorName + */ + kt_bool TryCloseLoop(LocalizedRangeScan* pScan, const Name& rSensorName); + + /** + * Optimizes scan poses + */ + void CorrectPoses(); + + /** + * Find "nearby" (no further than given distance away) scans through graph links + * @param pScan + * @param maxDistance + */ + LocalizedRangeScanVector FindNearLinkedScans(LocalizedRangeScan* pScan, kt_double maxDistance); + + /** + * Find "nearby" (no further than given distance away) vertices through graph links + * @param pScan + * @param maxDistance + */ + std::vector*> FindNearLinkedVertices(LocalizedRangeScan* pScan, kt_double maxDistance); + + /** + * Find "nearby" (no further than given distance away) scans through graph links + * @param pScan + * @param maxDistance + */ + LocalizedRangeScanVector FindNearByScans(Name name, const Pose2 refPose, kt_double maxDistance); + + /** + * Find "nearby" (no further than given distance away) vertices through KD-tree + * @param pScan + * @param maxDistance + */ + std::vector*> FindNearByVertices(Name name, const Pose2 refPose, kt_double maxDistance); + + /** + * Find closest scan to pose + * @param pScan + */ + Vertex* FindNearByScan(Name name, const Pose2 refPose); + + /** + * Gets the graph's scan matcher + * @return scan matcher + */ + inline ScanMatcher* GetLoopScanMatcher() const + { + return m_pLoopScanMatcher; + } + + /** + * Create new scan matcher for graph + * @param rangeThreshold + */ + void UpdateLoopScanMatcher(kt_double rangeThreshold); + + private: + /** + * Gets the vertex associated with the given scan + * @param pScan + * @return vertex of scan + */ + inline Vertex* GetVertex(LocalizedRangeScan* pScan) + { + Name rName = pScan->GetSensorName(); + std::map* >::iterator it = m_Vertices[rName].find(pScan->GetStateId()); + if (it != m_Vertices[rName].end()) + { + return it->second; + } + else + { + std::cout << "GetVertex: Failed to get vertex, idx " << pScan->GetStateId() << + " is not in m_Vertices." << std::endl; + return nullptr; + } + } + + /** + * Finds the closest scan in the vector to the given pose + * @param rScans + * @param rPose + */ + LocalizedRangeScan* GetClosestScanToPose(const LocalizedRangeScanVector& rScans, const Pose2& rPose) const; + + /** + * Adds an edge between the two scans and labels the edge with the given mean and covariance + * @param pFromScan + * @param pToScan + * @param rMean + * @param rCovariance + */ + void LinkScans(LocalizedRangeScan* pFromScan, + LocalizedRangeScan* pToScan, + const Pose2& rMean, + const Matrix3& rCovariance); + + /** + * Find nearby chains of scans and link them to scan if response is high enough + * @param pScan + * @param rMeans + * @param rCovariances + */ + void LinkNearChains(LocalizedRangeScan* pScan, Pose2Vector& rMeans, std::vector& rCovariances); + + /** + * Link the chain of scans to the given scan by finding the closest scan in the chain to the given scan + * @param rChain + * @param pScan + * @param rMean + * @param rCovariance + */ + void LinkChainToScan(const LocalizedRangeScanVector& rChain, + LocalizedRangeScan* pScan, + const Pose2& rMean, + const Matrix3& rCovariance); + + /** + * Find chains of scans that are close to given scan + * @param pScan + * @return chains of scans + */ + std::vector FindNearChains(LocalizedRangeScan* pScan); + + /** + * Compute mean of poses weighted by covariances + * @param rMeans + * @param rCovariances + * @return weighted mean + */ + Pose2 ComputeWeightedMean(const Pose2Vector& rMeans, const std::vector& rCovariances) const; + + /** + * Tries to find a chain of scan from the given device starting at the + * given scan index that could possibly close a loop with the given scan + * @param pScan + * @param rSensorName + * @param rStartNum + * @return chain that can possibly close a loop with given scan + */ + LocalizedRangeScanVector FindPossibleLoopClosure(LocalizedRangeScan* pScan, + const Name& rSensorName, + kt_int32u& rStartNum); + + private: + /** + * Mapper of this graph + */ + Mapper* m_pMapper; + + /** + * Scan matcher for loop closures + */ + ScanMatcher* m_pLoopScanMatcher; + + /** + * Traversal algorithm to find near linked scans + */ + GraphTraversal* m_pTraversal; + + /** + * Serialization: class MapperGraph + */ + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + std::cout << "MapperGraph <- Graph; "; + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Graph); + std::cout << "MapperGraph <- m_pMapper; "; + ar & BOOST_SERIALIZATION_NVP(m_pMapper); + std::cout << "MapperGraph <- m_pLoopScanMatcher; "; + ar & BOOST_SERIALIZATION_NVP(m_pLoopScanMatcher); + std::cout << "MapperGraph <- m_pTraversal\n"; + ar & BOOST_SERIALIZATION_NVP(m_pTraversal); + } + + }; // MapperGraph + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + /** + * Graph optimization algorithm + */ + class ScanSolver + { + public: + /** + * Vector of id-pose pairs + */ + typedef std::vector > IdPoseVector; + + /** + * Default constructor + */ + ScanSolver() + { + } + + /** + * Destructor + */ + virtual ~ScanSolver() + { + } + + public: + /** + * Solve! + */ + virtual void Compute() = 0; + + /** + * Get corrected poses after optimization + * @return optimized poses + */ + virtual const IdPoseVector& GetCorrections() const = 0; + + /** + * Adds a node to the solver + */ + virtual void AddNode(Vertex* /*pVertex*/) + { + } + + /** + * Removes a node from the solver + */ + virtual void RemoveNode(kt_int32s /*id*/) + { + } + + /** + * Adds a constraint to the solver + */ + virtual void AddConstraint(Edge* /*pEdge*/) + { + } + + /** + * Removes a constraint from the solver + */ + virtual void RemoveConstraint(kt_int32s /*sourceId*/, kt_int32s /*targetId*/) + { + } + + /** + * Resets the solver + */ + virtual void Clear() + { + } + + /** + * Resets the solver for reinitialization + */ + virtual void Reset() + { + } + + /** + * Get graph stored + */ + virtual std::unordered_map* getGraph() + { + std::cout << "getGraph method not implemented for this solver type. Graph visualization unavailable." << std::endl; + return nullptr; + } + + /** + * Modify a node's pose + */ + virtual void ModifyNode(const int& unique_id, Eigen::Vector3d pose) + { + std::cout << "ModifyNode method not implemented for this solver type. Manual loop closure unavailable." << std::endl; + }; + /** + * Get node's yaw + */ + virtual void GetNodeOrientation(const int& unique_id, double& pose) + { + std::cout << "GetNodeOrientation method not implemented for this solver type." << std::endl; + }; + + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + } + }; // ScanSolver + BOOST_SERIALIZATION_ASSUME_ABSTRACT(ScanSolver) + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + /** + * Implementation of a correlation grid used for scan matching + */ + class CorrelationGrid : public Grid + { + public: + /** + * Destructor + */ + virtual ~CorrelationGrid() + { + if (m_pKernel) + { + delete [] m_pKernel; + } + + } + + public: + /** + * Create a correlation grid of given size and parameters + * @param width + * @param height + * @param resolution + * @param smearDeviation + * @return correlation grid + */ + CorrelationGrid() + { + } + static CorrelationGrid* CreateGrid(kt_int32s width, + kt_int32s height, + kt_double resolution, + kt_double smearDeviation) + { + assert(resolution != 0.0); + + // +1 in case of roundoff + kt_int32u borderSize = GetHalfKernelSize(smearDeviation, resolution) + 1; + + CorrelationGrid* pGrid = new CorrelationGrid(width, height, borderSize, resolution, smearDeviation); + + return pGrid; + } + + /** + * Gets the index into the data pointer of the given grid coordinate + * @param rGrid + * @param boundaryCheck + * @return grid index + */ + virtual kt_int32s GridIndex(const Vector2& rGrid, kt_bool boundaryCheck = true) const + { + kt_int32s x = rGrid.GetX() + m_Roi.GetX(); + kt_int32s y = rGrid.GetY() + m_Roi.GetY(); + + return Grid::GridIndex(Vector2(x, y), boundaryCheck); + } + + /** + * Get the Region Of Interest (ROI) + * @return region of interest + */ + inline const Rectangle2& GetROI() const + { + return m_Roi; + } + + /** + * Sets the Region Of Interest (ROI) + * @param roi + */ + inline void SetROI(const Rectangle2& roi) + { + m_Roi = roi; + } + + /** + * Smear cell if the cell at the given point is marked as "occupied" + * @param rGridPoint + */ + inline void SmearPoint(const Vector2& rGridPoint) + { + assert(m_pKernel != NULL); + + int gridIndex = GridIndex(rGridPoint); + if (GetDataPointer()[gridIndex] != GridStates_Occupied) + { + return; + } + + kt_int32s halfKernel = m_KernelSize / 2; + + // apply kernel + for (kt_int32s j = -halfKernel; j <= halfKernel; j++) + { + kt_int8u* pGridAdr = GetDataPointer(Vector2(rGridPoint.GetX(), rGridPoint.GetY() + j)); + + kt_int32s kernelConstant = (halfKernel) + m_KernelSize * (j + halfKernel); + + // if a point is on the edge of the grid, there is no problem + // with running over the edge of allowable memory, because + // the grid has margins to compensate for the kernel size + for (kt_int32s i = -halfKernel; i <= halfKernel; i++) + { + kt_int32s kernelArrayIndex = i + kernelConstant; + + kt_int8u kernelValue = m_pKernel[kernelArrayIndex]; + if (kernelValue > pGridAdr[i]) + { + // kernel value is greater, so set it to kernel value + pGridAdr[i] = kernelValue; + } + } + } + } + + protected: + /** + * Constructs a correlation grid of given size and parameters + * @param width + * @param height + * @param borderSize + * @param resolution + * @param smearDeviation + */ + CorrelationGrid(kt_int32u width, kt_int32u height, kt_int32u borderSize, + kt_double resolution, kt_double smearDeviation) + : Grid(width + borderSize * 2, height + borderSize * 2) + , m_SmearDeviation(smearDeviation) + , m_pKernel(NULL) + { + GetCoordinateConverter()->SetScale(1.0 / resolution); + + // setup region of interest + m_Roi = Rectangle2(borderSize, borderSize, width, height); + + // calculate kernel + CalculateKernel(); + } + + /** + * Sets up the kernel for grid smearing. + */ + virtual void CalculateKernel() + { + kt_double resolution = GetResolution(); + + assert(resolution != 0.0); + assert(m_SmearDeviation != 0.0); + + // min and max distance deviation for smearing; + // will smear for two standard deviations, so deviation must be at least 1/2 of the resolution + const kt_double MIN_SMEAR_DISTANCE_DEVIATION = 0.5 * resolution; + const kt_double MAX_SMEAR_DISTANCE_DEVIATION = 10 * resolution; + + // check if given value too small or too big + if (!math::InRange(m_SmearDeviation, MIN_SMEAR_DISTANCE_DEVIATION, MAX_SMEAR_DISTANCE_DEVIATION)) + { + std::stringstream error; + error << "Mapper Error: Smear deviation too small: Must be between " + << MIN_SMEAR_DISTANCE_DEVIATION + << " and " + << MAX_SMEAR_DISTANCE_DEVIATION; + throw std::runtime_error(error.str()); + } + + // NOTE: Currently assumes a two-dimensional kernel + + // +1 for center + m_KernelSize = 2 * GetHalfKernelSize(m_SmearDeviation, resolution) + 1; + + // allocate kernel + m_pKernel = new kt_int8u[m_KernelSize * m_KernelSize]; + if (m_pKernel == NULL) + { + throw std::runtime_error("Unable to allocate memory for kernel!"); + } + + // calculate kernel + kt_int32s halfKernel = m_KernelSize / 2; + for (kt_int32s i = -halfKernel; i <= halfKernel; i++) + { + for (kt_int32s j = -halfKernel; j <= halfKernel; j++) + { +#ifdef WIN32 + kt_double distanceFromMean = _hypot(i * resolution, j * resolution); +#else + kt_double distanceFromMean = hypot(i * resolution, j * resolution); +#endif + kt_double z = exp(-0.5 * pow(distanceFromMean / m_SmearDeviation, 2)); + + kt_int32u kernelValue = static_cast(math::Round(z * GridStates_Occupied)); + assert(math::IsUpTo(kernelValue, static_cast(255))); + + int kernelArrayIndex = (i + halfKernel) + m_KernelSize * (j + halfKernel); + m_pKernel[kernelArrayIndex] = static_cast(kernelValue); + } + } + } + + /** + * Computes the kernel half-size based on the smear distance and the grid resolution. + * Computes to two standard deviations to get 95% region and to reduce aliasing. + * @param smearDeviation + * @param resolution + * @return kernel half-size based on the parameters + */ + static kt_int32s GetHalfKernelSize(kt_double smearDeviation, kt_double resolution) + { + assert(resolution != 0.0); + + return static_cast(math::Round(2.0 * smearDeviation / resolution)); + } + + private: + /** + * The point readings are smeared by this value in X and Y to create a smoother response. + * Default value is 0.03 meters. + */ + kt_double m_SmearDeviation; + + // Size of one side of the kernel + kt_int32s m_KernelSize; + + // Cached kernel for smearing + kt_int8u* m_pKernel; + + // region of interest + Rectangle2 m_Roi; + /** + * Serialization: class CorrelationGrid + */ + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Grid); + ar & BOOST_SERIALIZATION_NVP(m_SmearDeviation); + ar & BOOST_SERIALIZATION_NVP(m_KernelSize); + if (Archive::is_loading::value) + { + m_pKernel = new kt_int8u[m_KernelSize * m_KernelSize]; + } + ar & boost::serialization::make_array(m_pKernel, m_KernelSize * m_KernelSize); + ar & BOOST_SERIALIZATION_NVP(m_Roi); + } + }; // CorrelationGrid + BOOST_SERIALIZATION_ASSUME_ABSTRACT(CorrelationGrid) + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + /** + * Scan matcher + */ + class KARTO_EXPORT ScanMatcher + { + public: + ScanMatcher() + { + } + /** + * Destructor + */ + virtual ~ScanMatcher(); + + public: + /** + * Parallelize scan matching + */ + void operator() (const kt_double& y) const; + + /** + * Create a scan matcher with the given parameters + */ + static ScanMatcher* Create(Mapper* pMapper, + kt_double searchSize, + kt_double resolution, + kt_double smearDeviation, + kt_double rangeThreshold); + + /** + * Match given scan against set of scans + * @param pScan scan being scan-matched + * @param rBaseScans set of scans whose points will mark cells in grid as being occupied + * @param rMean output parameter of mean (best pose) of match + * @param rCovariance output parameter of covariance of match + * @param doPenalize whether to penalize matches further from the search center + * @param doRefineMatch whether to do finer-grained matching if coarse match is good (default is true) + * @return strength of response + */ + template + kt_double MatchScan(LocalizedRangeScan* pScan, + const T& rBaseScans, + Pose2& rMean, Matrix3& rCovariance, + kt_bool doPenalize = true, + kt_bool doRefineMatch = true); + + /** + * Finds the best pose for the scan centering the search in the correlation grid + * at the given pose and search in the space by the vector and angular offsets + * in increments of the given resolutions + * @param pScan scan to match against correlation grid + * @param rSearchCenter the center of the search space + * @param rSearchSpaceOffset searches poses in the area offset by this vector around search center + * @param rSearchSpaceResolution how fine a granularity to search in the search space + * @param searchAngleOffset searches poses in the angles offset by this angle around search center + * @param searchAngleResolution how fine a granularity to search in the angular search space + * @param doPenalize whether to penalize matches further from the search center + * @param rMean output parameter of mean (best pose) of match + * @param rCovariance output parameter of covariance of match + * @param doingFineMatch whether to do a finer search after coarse search + * @return strength of response + */ + kt_double CorrelateScan(LocalizedRangeScan* pScan, + const Pose2& rSearchCenter, + const Vector2& rSearchSpaceOffset, + const Vector2& rSearchSpaceResolution, + kt_double searchAngleOffset, + kt_double searchAngleResolution, + kt_bool doPenalize, + Pose2& rMean, + Matrix3& rCovariance, + kt_bool doingFineMatch); + + /** + * Computes the positional covariance of the best pose + * @param rBestPose + * @param bestResponse + * @param rSearchCenter + * @param rSearchSpaceOffset + * @param rSearchSpaceResolution + * @param searchAngleResolution + * @param rCovariance + */ + void ComputePositionalCovariance(const Pose2& rBestPose, + kt_double bestResponse, + const Pose2& rSearchCenter, + const Vector2& rSearchSpaceOffset, + const Vector2& rSearchSpaceResolution, + kt_double searchAngleResolution, + Matrix3& rCovariance); + + /** + * Computes the angular covariance of the best pose + * @param rBestPose + * @param bestResponse + * @param rSearchCenter + * @param searchAngleOffset + * @param searchAngleResolution + * @param rCovariance + */ + void ComputeAngularCovariance(const Pose2& rBestPose, + kt_double bestResponse, + const Pose2& rSearchCenter, + kt_double searchAngleOffset, + kt_double searchAngleResolution, + Matrix3& rCovariance); + + /** + * Gets the correlation grid data (for debugging) + * @return correlation grid + */ + inline CorrelationGrid* GetCorrelationGrid() const + { + return m_pCorrelationGrid; + } + + private: + /** + * Marks cells where scans' points hit as being occupied + * @param rScans scans whose points will mark cells in grid as being occupied + * @param viewPoint do not add points that belong to scans "opposite" the view point + */ + void AddScans(const LocalizedRangeScanVector& rScans, Vector2 viewPoint); + void AddScans(const LocalizedRangeScanMap& rScans, Vector2 viewPoint); + + /** + * Marks cells where scans' points hit as being occupied. Can smear points as they are added. + * @param pScan scan whose points will mark cells in grid as being occupied + * @param viewPoint do not add points that belong to scans "opposite" the view point + * @param doSmear whether the points will be smeared + */ + void AddScan(LocalizedRangeScan* pScan, const Vector2& rViewPoint, kt_bool doSmear = true); + + /** + * Compute which points in a scan are on the same side as the given viewpoint + * @param pScan + * @param rViewPoint + * @return points on the same side + */ + PointVectorDouble FindValidPoints(LocalizedRangeScan* pScan, const Vector2& rViewPoint) const; + + /** + * Get response at given position for given rotation (only look up valid points) + * @param angleIndex + * @param gridPositionIndex + * @return response + */ + kt_double GetResponse(kt_int32u angleIndex, kt_int32s gridPositionIndex) const; + + protected: + /** + * Default constructor + */ + ScanMatcher(Mapper* pMapper) + : m_pMapper(pMapper) + , m_pCorrelationGrid(NULL) + , m_pSearchSpaceProbs(NULL) + , m_pGridLookup(NULL) + , m_pPoseResponse(NULL) + , m_doPenalize(false) + { + } + + private: + Mapper* m_pMapper; + CorrelationGrid* m_pCorrelationGrid; + Grid* m_pSearchSpaceProbs; + GridIndexLookup* m_pGridLookup; + std::pair* m_pPoseResponse; + std::vector m_xPoses; + std::vector m_yPoses; + Pose2 m_rSearchCenter; + kt_double m_searchAngleOffset; + kt_int32u m_nAngles; + kt_double m_searchAngleResolution; + kt_bool m_doPenalize; + + /** + * Serialization: class ScanMatcher + */ + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + ar & BOOST_SERIALIZATION_NVP(m_pMapper); + ar & BOOST_SERIALIZATION_NVP(m_pCorrelationGrid); + ar & BOOST_SERIALIZATION_NVP(m_pSearchSpaceProbs); + ar & BOOST_SERIALIZATION_NVP(m_pGridLookup); + ar & BOOST_SERIALIZATION_NVP(m_xPoses); + ar & BOOST_SERIALIZATION_NVP(m_yPoses); + ar & BOOST_SERIALIZATION_NVP(m_rSearchCenter); + ar & BOOST_SERIALIZATION_NVP(m_searchAngleResolution); + ar & BOOST_SERIALIZATION_NVP(m_nAngles); + ar & BOOST_SERIALIZATION_NVP(m_searchAngleResolution);; + ar & BOOST_SERIALIZATION_NVP(m_doPenalize); + + // Note - m_pPoseResponse is generally only ever defined within the + // execution of ScanMatcher::CorrelateScan and used as a temporary + // accumulator for multithreaded matching results. It would normally + // not make sense to serialize, but we don't want to break compatibility + // with previously serialized data. Gen some dummy data that we free + // immediately after so that we don't alloc here and leak. + kt_int32u poseResponseSize = + static_cast(m_xPoses.size() * m_yPoses.size() * m_nAngles); + + // We could check first if m_pPoseResponse == nullptr for good measure, but + // based on the codepaths it should always be freed and set to null outside of + // any execution of ScanMatcher::CorrelateScan, so go ahead and alloc here. + m_pPoseResponse = new std::pair[poseResponseSize]; + ar & boost::serialization::make_array>(m_pPoseResponse, + poseResponseSize); + + // Aaaand now, clean up the dummy data + delete[] m_pPoseResponse; + m_pPoseResponse = nullptr; + } + + }; // ScanMatcher + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + class ScanManager; + + /** + * Manages the devices for the mapper + */ + class KARTO_EXPORT MapperSensorManager //: public SensorManager // was commented out, works with it in, but I'll leave out for now... + { + typedef std::map ScanManagerMap; + + public: + /** + * Constructor + */ + MapperSensorManager(kt_int32u runningBufferMaximumSize, kt_double runningBufferMaximumDistance) + : m_RunningBufferMaximumSize(runningBufferMaximumSize) + , m_RunningBufferMaximumDistance(runningBufferMaximumDistance) + , m_NextScanId(0) + { + } + + MapperSensorManager(){ + } + + /** + * Destructor + */ + virtual ~MapperSensorManager() + { + Clear(); + } + + public: + /** + * Registers a sensor (with given name); do + * nothing if device already registered + * @param rSensorName + */ + void RegisterSensor(const Name& rSensorName); + + /** + * Gets scan from given sensor with given ID + * @param rSensorName + * @param scanIndex + * @return localized range scan + */ + LocalizedRangeScan* GetScan(const Name& rSensorName, kt_int32s scanIndex); + + /** + * Gets names of all sensors + * @return sensor names + */ + inline std::vector GetSensorNames() + { + std::vector deviceNames; + const_forEach(ScanManagerMap, &m_ScanManagers) + { + deviceNames.push_back(iter->first); + } + + return deviceNames; + } + + /** + * Gets last scan of given sensor + * @param rSensorName + * @return last localized range scan of sensor + */ + LocalizedRangeScan* GetLastScan(const Name& rSensorName); + + /** + * Sets the last scan of device of given scan + * @param pScan + */ + void SetLastScan(LocalizedRangeScan* pScan); + + /** + * Clears the laser scan of device + * @param pScan + */ + void ClearLastScan(LocalizedRangeScan* pScan); + + /** + * Clears the laser scan of device name + * @param pScan + */ + void ClearLastScan(const Name& name); + + /** + * Gets the scan with the given unique id + * @param id + * @return scan + */ + inline LocalizedRangeScan* GetScan(kt_int32s id) + { + std::map::iterator it = m_Scans.find(id); + if (it != m_Scans.end()) + { + return it->second; + } + else + { + std::cout << "GetScan: id " << id << + " does not exist in m_scans, cannot retrieve it." << std::endl; + return nullptr; + } + } + + /** + * Adds scan to scan vector of device that recorded scan + * @param pScan + */ + void AddScan(LocalizedRangeScan* pScan); + + /** + * Adds scan to running scans of device that recorded scan + * @param pScan + */ + void AddRunningScan(LocalizedRangeScan* pScan); + + /** + * Finds and replaces a scan from m_scans with NULL + * @param pScan + */ + void RemoveScan(LocalizedRangeScan* pScan); + + /** + * Gets scans of device + * @param rSensorName + * @return scans of device + */ + LocalizedRangeScanMap& GetScans(const Name& rSensorName); + + /** + * Gets running scans of device + * @param rSensorName + * @return running scans of device + */ + LocalizedRangeScanVector& GetRunningScans(const Name& rSensorName); + + /** + * Clears running scans of device + */ + void ClearRunningScans(const Name& rSensorName); + + /** + * Gets the running scan buffer of device + */ + kt_int32u GetRunningScanBufferSize(const Name& rSensorName); + + /** + * Sets the running scan buffer size for all devices + * @param rScanBufferSize + */ + void SetRunningScanBufferSize(kt_int32u rScanBufferSize); + + /** + * Sets the running scan buffer maximum distance for all devices + * @param rScanBufferMaxDistance + */ + void SetRunningScanBufferMaximumDistance(kt_double rScanBufferMaxDistance); + + /** + * Gets all scans of all devices + * @return all scans of all devices + */ + LocalizedRangeScanVector GetAllScans(); + + /** + * Deletes all scan managers of all devices + */ + void Clear(); + + private: + /** + * Get scan manager for localized range scan + * @return ScanManager + */ + inline ScanManager* GetScanManager(LocalizedRangeScan* pScan) + { + return GetScanManager(pScan->GetSensorName()); + } + + /** + * Get scan manager for id + * @param rSensorName + * @return ScanManager + */ + inline ScanManager* GetScanManager(const Name& rSensorName) + { + if (m_ScanManagers.find(rSensorName) != m_ScanManagers.end()) + { + return m_ScanManagers[rSensorName]; + } + + return NULL; + } + + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + std::cout << "MapperSensorManager <- m_ScanManagers; "; + ar & BOOST_SERIALIZATION_NVP(m_ScanManagers); + ar & BOOST_SERIALIZATION_NVP(m_RunningBufferMaximumSize); + ar & BOOST_SERIALIZATION_NVP(m_RunningBufferMaximumDistance); + ar & BOOST_SERIALIZATION_NVP(m_NextScanId); + std::cout << "MapperSensorManager <- m_Scans\n"; + ar & BOOST_SERIALIZATION_NVP(m_Scans); + } + + private: + // map from device ID to scan data + ScanManagerMap m_ScanManagers; + + kt_int32u m_RunningBufferMaximumSize; + kt_double m_RunningBufferMaximumDistance; + + kt_int32s m_NextScanId; + + std::map m_Scans; + }; // MapperSensorManager + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + /** + * Graph SLAM mapper. Creates a map given a set of LocalizedRangeScans + * The current Karto implementation is a proprietary, high-performance + * scan-matching algorithm that constructs a map from individual range scans and corrects for + * errors in the range and odometry data. + * + * The following parameters can be set on the Mapper. + * + * \a UseScanMatching (ParameterBool)\n + * When set to true, the mapper will use a scan matching algorithm. In most real-world situations + * this should be set to true so that the mapper algorithm can correct for noise and errors in + * odometry and scan data. In some simulator environments where the simulated scan and odometry + * data are very accurate, the scan matching algorithm can produce worse results. In those cases + * set to false to improve results. + * Default value is true. + * + * \a UseScanBarycenter (ParameterBool)\n + * + * \a UseResponseExpansion (ParameterBool)\n + * + * \a RangeThreshold (ParameterDouble - meters)\n + * The range threshold is used to truncate range scan distance measurement readings. The threshold should + * be set such that adjacent range readings in a scan will generally give "solid" coverage of objects. + * + * \image html doxygen/RangeThreshold.png + * \image latex doxygen/RangeThreshold.png "" width=3in + * + * Having solid coverage depends on the map resolution and the angular resolution of the range scan device. + * The following are the recommended threshold values for the corresponding map resolution and range finder + * resolution values: + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + *
Map ResolutionLaser Angular Resolution
1.0 degree0.5 degree0.25 degree
0.15.7m11.4m22.9m
0.052.8m5.7m11.4m
0.010.5m1.1m2.3m
+ * + * Note that the value of RangeThreshold should be adjusted taking into account the values of + * MinimumTravelDistance and MinimumTravelHeading (see also below). By incorporating scans + * into the map more frequently, the RangeThreshold value can be increased as the additional scans + * will "fill in" the gaps of objects at greater distances where there is less solid coverage. + * + * Default value is 12.0 (meters). + * + * \a MinimumTravelDistance (ParameterDouble - meters)\n + * Sets the minimum travel between scans. If a new scan's position is more than minimumDistance from + * the previous scan, the mapper will use the data from the new scan. Otherwise, it will discard the + * new scan if it also does not meet the minimum change in heading requirement. + * For performance reasons, generally it is a good idea to only process scans if the robot + * has moved a reasonable amount. + * Default value is 0.3 (meters). + * + * \a MinimumTravelHeading (ParameterDouble - radians)\n + * Sets the minimum heading change between scans. If a new scan's heading is more than minimumHeading from + * the previous scan, the mapper will use the data from the new scan. Otherwise, it will discard the + * new scan if it also does not meet the minimum travel distance requirement. + * For performance reasons, generally it is a good idea to only process scans if the robot + * has moved a reasonable amount. + * Default value is 0.08726646259971647 (radians) - 5 degrees. + * + * \a ScanBufferSize (ParameterIn32u - size)\n + * Scan buffer size is the length of the scan chain stored for scan matching. + * "ScanBufferSize" should be set to approximately "ScanBufferMaximumScanDistance" / "MinimumTravelDistance". + * The idea is to get an area approximately 20 meters long for scan matching. + * For example, if we add scans every MinimumTravelDistance = 0.3 meters, then "ScanBufferSize" + * should be 20 / 0.3 = 67.) + * Default value is 67. + * + * \a ScanBufferMaximumScanDistance (ParameterDouble - meters)\n + * Scan buffer maximum scan distance is the maximum distance between the first and last scans + * in the scan chain stored for matching. + * Default value is 20.0. + * + * \a CorrelationSearchSpaceDimension (ParameterDouble - meters)\n + * The size of the correlation grid used by the matcher. + * Default value is 0.3 meters which tells the matcher to use a 30cm x 30cm grid. + * + * \a CorrelationSearchSpaceResolution (ParameterDouble - meters)\n + * The resolution (size of a grid cell) of the correlation grid. + * Default value is 0.01 meters. + * + * \a CorrelationSearchSpaceSmearDeviation (ParameterDouble - meters)\n + * The robot position is smeared by this value in X and Y to create a smoother response. + * Default value is 0.03 meters. + * + * \a LinkMatchMinimumResponseFine (ParameterDouble - probability (>= 0.0, <= 1.0))\n + * Scans are linked only if the correlation response value is greater than this value. + * Default value is 0.4 + * + * \a LinkScanMaximumDistance (ParameterDouble - meters)\n + * Maximum distance between linked scans. Scans that are farther apart will not be linked + * regardless of the correlation response value. + * Default value is 6.0 meters. + * + * \a LoopSearchSpaceDimension (ParameterDouble - meters)\n + * Dimension of the correlation grid used by the loop closure detection algorithm + * Default value is 4.0 meters. + * + * \a LoopSearchSpaceResolution (ParameterDouble - meters)\n + * Coarse resolution of the correlation grid used by the matcher to determine a possible + * loop closure. + * Default value is 0.05 meters. + * + * \a LoopSearchSpaceSmearDeviation (ParameterDouble - meters)\n + * Smearing distance in the correlation grid used by the matcher to determine a possible + * loop closure match. + * Default value is 0.03 meters. + * + * \a LoopSearchMaximumDistance (ParameterDouble - meters)\n + * Scans less than this distance from the current position will be considered for a match + * in loop closure. + * Default value is 4.0 meters. + * + * \a LoopMatchMinimumChainSize (ParameterIn32s)\n + * When the loop closure detection finds a candidate it must be part of a large + * set of linked scans. If the chain of scans is less than this value we do not attempt + * to close the loop. + * Default value is 10. + * + * \a LoopMatchMaximumVarianceCoarse (ParameterDouble)\n + * The co-variance values for a possible loop closure have to be less than this value + * to consider a viable solution. This applies to the coarse search. + * Default value is 0.16. + * + * \a LoopMatchMinimumResponseCoarse (ParameterDouble - probability (>= 0.0, <= 1.0))\n + * If response is larger then this then initiate loop closure search at the coarse resolution. + * Default value is 0.7. + * + * \a LoopMatchMinimumResponseFine (ParameterDouble - probability (>= 0.0, <= 1.0))\n + * If response is larger then this then initiate loop closure search at the fine resolution. + * Default value is 0.5. + */ + + struct LocalizationScanVertex + { + LocalizationScanVertex(){return;}; + LocalizationScanVertex(const LocalizationScanVertex& obj){scan = obj.scan; vertex = obj.vertex;}; + LocalizedRangeScan* scan; + Vertex* vertex; + }; + + typedef std::queue LocalizationScanVertices; + + class KARTO_EXPORT Mapper : public Module + { + friend class MapperGraph; + friend class ScanMatcher; + + public: + /** + * Default constructor + */ + Mapper(); + + /** + * Constructor a mapper with a name + * @param rName mapper name + */ + Mapper(const std::string& rName); + + /** + * Destructor + */ + virtual ~Mapper(); + + public: + /** + * Allocate memory needed for mapping + * @param rangeThreshold + */ + void Initialize(kt_double rangeThreshold); + + /** + * Save map to file + * @param filename + */ + void SaveToFile(const std::string& filename); + + /** + * Load map from file + * @param filename + */ + void LoadFromFile(const std::string& filename); + + /** + * Resets the mapper. + * Deallocate memory allocated in Initialize() + */ + virtual void Reset(); + + /** + * Process a localized range scan for incorporation into the map. The scan must + * be identified with a range finder device. Once added to a map, the corrected pose information in the + * localized scan will be updated to the correct pose as determined by the mapper. + * + * @param pScan A localized range scan that has pose information associated directly with the scan data. The pose + * is that of the range device originating the scan. Note that the mapper will set corrected pose + * information in the scan object. + * + * @return true if the scan was added successfully, false otherwise + */ + virtual kt_bool Process(LocalizedRangeScan * pScan, Matrix3 * covariance = nullptr); + + /** + * Process an Object + */ + virtual kt_bool Process(Object* pObject); + + // processors + kt_bool ProcessAtDock(LocalizedRangeScan * pScan, Matrix3 * covariance = nullptr); + kt_bool ProcessAgainstNode(LocalizedRangeScan * pScan, const int & nodeId, Matrix3 * covariance = nullptr); + kt_bool ProcessAgainstNodesNearBy(LocalizedRangeScan* pScan, kt_bool addScanToLocalizationBuffer = false, Matrix3 * covariance = nullptr); + kt_bool ProcessLocalization(LocalizedRangeScan * pScan, Matrix3 * covariance = nullptr); + kt_bool RemoveNodeFromGraph(Vertex*); + void AddScanToLocalizationBuffer(LocalizedRangeScan* pScan, Vertex* scan_vertex); + void ClearLocalizationBuffer(); + + /** + * Returns all processed scans added to the mapper. + * NOTE: The returned scans have their corrected pose updated. + * @return list of scans received and processed by the mapper. If no scans have been processed, + * return an empty list. + */ + virtual const LocalizedRangeScanVector GetAllProcessedScans() const; + + /** + * Add a listener to mapper + * @param pListener + */ + void AddListener(MapperListener* pListener); + + /** + * Remove a listener to mapper + * @param pListener + */ + void RemoveListener(MapperListener* pListener); + + /** + * Set scan optimizer used by mapper when closing the loop + * @param pSolver + */ + void SetScanSolver(ScanSolver* pSolver); + + /** + * Gets scan optimizer used by mapper when closing the loop + * @return pSolver + */ + ScanSolver* getScanSolver(); + + /** + * Get scan link graph + * @return graph + */ + virtual MapperGraph* GetGraph() const; + + /** + * Gets the sequential scan matcher + * @return sequential scan matcher + */ + virtual ScanMatcher* GetSequentialScanMatcher() const; + + /** + * Gets the loop scan matcher + * @return loop scan matcher + */ + virtual ScanMatcher* GetLoopScanMatcher() const; + + /** + * Gets the device manager + * @return device manager + */ + inline MapperSensorManager* GetMapperSensorManager() const + { + return m_pMapperSensorManager; + } + + /** + * Tries to close a loop using the given scan with the scans from the given sensor + * @param pScan + * @param rSensorName + */ + inline kt_bool TryCloseLoop(LocalizedRangeScan* pScan, const Name& rSensorName) + { + return m_pGraph->TryCloseLoop(pScan, rSensorName); + } + + inline void CorrectPoses() + { + m_pGraph->CorrectPoses(); + } + + protected: + void InitializeParameters(); + + /** + * Test if the scan is "sufficiently far" from the last scan added. + * @param pScan scan to be checked + * @param pLastScan last scan added to mapper + * @return true if the scan is "sufficiently far" from the last scan added or + * the scan is the first scan to be added + */ + kt_bool HasMovedEnough(LocalizedRangeScan* pScan, LocalizedRangeScan* pLastScan) const; + + public: + ///////////////////////////////////////////// + // fire information for listeners!! + + /** + * Fire a general message to listeners + * @param rInfo + */ + void FireInfo(const std::string& rInfo) const; + + /** + * Fire a debug message to listeners + * @param rInfo + */ + void FireDebug(const std::string& rInfo) const; + + /** + * Fire a message upon checking for loop closure to listeners + * @param rInfo + */ + void FireLoopClosureCheck(const std::string& rInfo) const; + + /** + * Fire a message before loop closure to listeners + * @param rInfo + */ + void FireBeginLoopClosure(const std::string& rInfo) const; + + /** + * Fire a message after loop closure to listeners + * @param rInfo + */ + void FireEndLoopClosure(const std::string& rInfo) const; + + // FireRunningScansUpdated + + // FireCovarianceCalculated + + // FireLoopClosureChain + + private: + /** + * Restrict the copy constructor + */ + Mapper(const Mapper&); + + /** + * Restrict the assignment operator + */ + const Mapper& operator=(const Mapper&); + + public: + void SetUseScanMatching(kt_bool val) { m_pUseScanMatching->SetValue(val); } + + protected: + kt_bool m_Initialized; + kt_bool m_Deserialized; + + ScanMatcher* m_pSequentialScanMatcher; + + MapperSensorManager* m_pMapperSensorManager; + + MapperGraph* m_pGraph; + ScanSolver* m_pScanOptimizer; + LocalizationScanVertices m_LocalizationScanVertices; + + + std::vector m_Listeners; + + /** + * When set to true, the mapper will use a scan matching algorithm. In most real-world situations + * this should be set to true so that the mapper algorithm can correct for noise and errors in + * odometry and scan data. In some simulator environments where the simulated scan and odometry + * data are very accurate, the scan matching algorithm can produce worse results. In those cases + * set this to false to improve results. + * Default value is true. + */ + Parameter* m_pUseScanMatching; + + /** + * Default value is true. + */ + Parameter* m_pUseScanBarycenter; + + /** + * Sets the minimum time between scans. If a new scan's time stamp is + * longer than MinimumTimeInterval from the previously processed scan, + * the mapper will use the data from the new scan. Otherwise, it will + * discard the new scan if it also does not meet the minimum travel + * distance and heading requirements. For performance reasons, it is + * generally it is a good idea to only process scans if a reasonable + * amount of time has passed. This parameter is particularly useful + * when there is a need to process scans while the robot is stationary. + * Default value is 3600 (seconds), effectively disabling this parameter. + */ + Parameter* m_pMinimumTimeInterval; + + /** + * Sets the minimum travel between scans. If a new scan's position is more than minimumTravelDistance + * from the previous scan, the mapper will use the data from the new scan. Otherwise, it will discard the + * new scan if it also does not meet the minimum change in heading requirement. + * For performance reasons, generally it is a good idea to only process scans if the robot + * has moved a reasonable amount. + * Default value is 0.2 (meters). + */ + Parameter* m_pMinimumTravelDistance; + + /** + * Sets the minimum heading change between scans. If a new scan's heading is more than minimumTravelHeading + * from the previous scan, the mapper will use the data from the new scan. Otherwise, it will discard the + * new scan if it also does not meet the minimum travel distance requirement. + * For performance reasons, generally it is a good idea to only process scans if the robot + * has moved a reasonable amount. + * Default value is 10 degrees. + */ + Parameter* m_pMinimumTravelHeading; + + /** + * Scan buffer size is the length of the scan chain stored for scan matching. + * "scanBufferSize" should be set to approximately "scanBufferMaximumScanDistance" / "minimumTravelDistance". + * The idea is to get an area approximately 20 meters long for scan matching. + * For example, if we add scans every minimumTravelDistance == 0.3 meters, then "scanBufferSize" + * should be 20 / 0.3 = 67.) + * Default value is 67. + */ + Parameter* m_pScanBufferSize; + + /** + * Scan buffer maximum scan distance is the maximum distance between the first and last scans + * in the scan chain stored for matching. + * Default value is 20.0. + */ + Parameter* m_pScanBufferMaximumScanDistance; + + /** + * Scans are linked only if the correlation response value is greater than this value. + * Default value is 0.4 + */ + Parameter* m_pLinkMatchMinimumResponseFine; + + /** + * Maximum distance between linked scans. Scans that are farther apart will not be linked + * regardless of the correlation response value. + * Default value is 6.0 meters. + */ + Parameter* m_pLinkScanMaximumDistance; + + /** + * Enable/disable loop closure. + * Default is enabled. + */ + Parameter* m_pDoLoopClosing; + + /** + * Scans less than this distance from the current position will be considered for a match + * in loop closure. + * Default value is 4.0 meters. + */ + Parameter* m_pLoopSearchMaximumDistance; + + /** + * When the loop closure detection finds a candidate it must be part of a large + * set of linked scans. If the chain of scans is less than this value we do not attempt + * to close the loop. + * Default value is 10. + */ + Parameter* m_pLoopMatchMinimumChainSize; + + /** + * The co-variance values for a possible loop closure have to be less than this value + * to consider a viable solution. This applies to the coarse search. + * Default value is 0.16. + */ + Parameter* m_pLoopMatchMaximumVarianceCoarse; + + /** + * If response is larger then this, then initiate loop closure search at the coarse resolution. + * Default value is 0.7. + */ + Parameter* m_pLoopMatchMinimumResponseCoarse; + + /** + * If response is larger then this, then initiate loop closure search at the fine resolution. + * Default value is 0.7. + */ + Parameter* m_pLoopMatchMinimumResponseFine; + + ////////////////////////////////////////////////////////////////////////////// + // CorrelationParameters correlationParameters; + + /** + * The size of the search grid used by the matcher. + * Default value is 0.3 meters which tells the matcher to use a 30cm x 30cm grid. + */ + Parameter* m_pCorrelationSearchSpaceDimension; + + /** + * The resolution (size of a grid cell) of the correlation grid. + * Default value is 0.01 meters. + */ + Parameter* m_pCorrelationSearchSpaceResolution; + + /** + * The point readings are smeared by this value in X and Y to create a smoother response. + * Default value is 0.03 meters. + */ + Parameter* m_pCorrelationSearchSpaceSmearDeviation; + + + ////////////////////////////////////////////////////////////////////////////// + // CorrelationParameters loopCorrelationParameters; + + /** + * The size of the search grid used by the matcher. + * Default value is 0.3 meters which tells the matcher to use a 30cm x 30cm grid. + */ + Parameter* m_pLoopSearchSpaceDimension; + + /** + * The resolution (size of a grid cell) of the correlation grid. + * Default value is 0.01 meters. + */ + Parameter* m_pLoopSearchSpaceResolution; + + /** + * The point readings are smeared by this value in X and Y to create a smoother response. + * Default value is 0.03 meters. + */ + Parameter* m_pLoopSearchSpaceSmearDeviation; + + ////////////////////////////////////////////////////////////////////////////// + // ScanMatcherParameters; + + // Variance of penalty for deviating from odometry when scan-matching. + // The penalty is a multiplier (less than 1.0) is a function of the + // delta of the scan position being tested and the odometric pose + Parameter* m_pDistanceVariancePenalty; + Parameter* m_pAngleVariancePenalty; + + // The range of angles to search during a coarse search and a finer search + Parameter* m_pFineSearchAngleOffset; + Parameter* m_pCoarseSearchAngleOffset; + + // Resolution of angles to search during a coarse search + Parameter* m_pCoarseAngleResolution; + + // Minimum value of the penalty multiplier so scores do not + // become too small + Parameter* m_pMinimumAnglePenalty; + Parameter* m_pMinimumDistancePenalty; + + // whether to increase the search space if no good matches are initially found + Parameter* m_pUseResponseExpansion; + + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + std::cout << "Mapper <- Module\n"; + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Module); + ar & BOOST_SERIALIZATION_NVP(m_Initialized); + std::cout << "Mapper <- m_pSequentialScanMatcher\n"; + ar & BOOST_SERIALIZATION_NVP(m_pSequentialScanMatcher); + std::cout << "Mapper <- m_pGraph\n"; + ar & BOOST_SERIALIZATION_NVP(m_pGraph); + std::cout << "Mapper <- m_pMapperSensorManager\n"; + ar & BOOST_SERIALIZATION_NVP(m_pMapperSensorManager); + std::cout << "Mapper <- m_Listeners\n"; + ar & BOOST_SERIALIZATION_NVP(m_Listeners); + ar & BOOST_SERIALIZATION_NVP(m_pUseScanMatching); + ar & BOOST_SERIALIZATION_NVP(m_pUseScanBarycenter); + ar & BOOST_SERIALIZATION_NVP(m_pMinimumTimeInterval); + ar & BOOST_SERIALIZATION_NVP(m_pMinimumTravelDistance); + ar & BOOST_SERIALIZATION_NVP(m_pMinimumTravelHeading); + ar & BOOST_SERIALIZATION_NVP(m_pScanBufferSize); + ar & BOOST_SERIALIZATION_NVP(m_pScanBufferMaximumScanDistance); + ar & BOOST_SERIALIZATION_NVP(m_pLinkMatchMinimumResponseFine); + ar & BOOST_SERIALIZATION_NVP(m_pLinkScanMaximumDistance); + ar & BOOST_SERIALIZATION_NVP(m_pDoLoopClosing); + ar & BOOST_SERIALIZATION_NVP(m_pLoopSearchMaximumDistance); + ar & BOOST_SERIALIZATION_NVP(m_pLoopMatchMinimumChainSize); + ar & BOOST_SERIALIZATION_NVP(m_pLoopMatchMaximumVarianceCoarse); + ar & BOOST_SERIALIZATION_NVP(m_pLoopMatchMinimumResponseCoarse); + ar & BOOST_SERIALIZATION_NVP(m_pLoopMatchMinimumResponseFine); + ar & BOOST_SERIALIZATION_NVP(m_pCorrelationSearchSpaceDimension); + ar & BOOST_SERIALIZATION_NVP(m_pCorrelationSearchSpaceResolution); + ar & BOOST_SERIALIZATION_NVP(m_pCorrelationSearchSpaceSmearDeviation); + ar & BOOST_SERIALIZATION_NVP(m_pLoopSearchSpaceDimension); + ar & BOOST_SERIALIZATION_NVP(m_pLoopSearchSpaceResolution); + ar & BOOST_SERIALIZATION_NVP(m_pLoopSearchSpaceSmearDeviation); + ar & BOOST_SERIALIZATION_NVP(m_pDistanceVariancePenalty); + ar & BOOST_SERIALIZATION_NVP(m_pAngleVariancePenalty); + ar & BOOST_SERIALIZATION_NVP(m_pFineSearchAngleOffset); + ar & BOOST_SERIALIZATION_NVP(m_pCoarseSearchAngleOffset); + ar & BOOST_SERIALIZATION_NVP(m_pCoarseAngleResolution); + ar & BOOST_SERIALIZATION_NVP(m_pMinimumAnglePenalty); + ar & BOOST_SERIALIZATION_NVP(m_pMinimumDistancePenalty); + ar & BOOST_SERIALIZATION_NVP(m_pUseResponseExpansion); + std::cout << "**Finished serializing Mapper**\n"; + } + public: + /* Abstract methods for parameter setters and getters */ + + /* Getters */ + // General Parameters + bool getParamUseScanMatching(); + bool getParamUseScanBarycenter(); + double getParamMinimumTimeInterval(); + double getParamMinimumTravelDistance(); + double getParamMinimumTravelHeading(); + int getParamScanBufferSize(); + double getParamScanBufferMaximumScanDistance(); + double getParamLinkMatchMinimumResponseFine(); + double getParamLinkScanMaximumDistance(); + double getParamLoopSearchMaximumDistance(); + bool getParamDoLoopClosing(); + int getParamLoopMatchMinimumChainSize(); + double getParamLoopMatchMaximumVarianceCoarse(); + double getParamLoopMatchMinimumResponseCoarse(); + double getParamLoopMatchMinimumResponseFine(); + + // Correlation Parameters - Correlation Parameters + double getParamCorrelationSearchSpaceDimension(); + double getParamCorrelationSearchSpaceResolution(); + double getParamCorrelationSearchSpaceSmearDeviation(); + + // Correlation Parameters - Loop Closure Parameters + double getParamLoopSearchSpaceDimension(); + double getParamLoopSearchSpaceResolution(); + double getParamLoopSearchSpaceSmearDeviation(); + + // Scan Matcher Parameters + double getParamDistanceVariancePenalty(); + double getParamAngleVariancePenalty(); + double getParamFineSearchAngleOffset(); + double getParamCoarseSearchAngleOffset(); + double getParamCoarseAngleResolution(); + double getParamMinimumAnglePenalty(); + double getParamMinimumDistancePenalty(); + bool getParamUseResponseExpansion(); + + /* Setters */ + // General Parameters + void setParamUseScanMatching(bool b); + void setParamUseScanBarycenter(bool b); + void setParamMinimumTimeInterval(double d); + void setParamMinimumTravelDistance(double d); + void setParamMinimumTravelHeading(double d); + void setParamScanBufferSize(int i); + void setParamScanBufferMaximumScanDistance(double d); + void setParamLinkMatchMinimumResponseFine(double d); + void setParamLinkScanMaximumDistance(double d); + void setParamLoopSearchMaximumDistance(double d); + void setParamDoLoopClosing(bool b); + void setParamLoopMatchMinimumChainSize(int i); + void setParamLoopMatchMaximumVarianceCoarse(double d); + void setParamLoopMatchMinimumResponseCoarse(double d); + void setParamLoopMatchMinimumResponseFine(double d); + + // Correlation Parameters - Correlation Parameters + void setParamCorrelationSearchSpaceDimension(double d); + void setParamCorrelationSearchSpaceResolution(double d); + void setParamCorrelationSearchSpaceSmearDeviation(double d); + + // Correlation Parameters - Loop Closure Parameters + void setParamLoopSearchSpaceDimension(double d); + void setParamLoopSearchSpaceResolution(double d); + void setParamLoopSearchSpaceSmearDeviation(double d); + + // Scan Matcher Parameters + void setParamDistanceVariancePenalty(double d); + void setParamAngleVariancePenalty(double d); + void setParamFineSearchAngleOffset(double d); + void setParamCoarseSearchAngleOffset(double d); + void setParamCoarseAngleResolution(double d); + void setParamMinimumAnglePenalty(double d); + void setParamMinimumDistancePenalty(double d); + void setParamUseResponseExpansion(bool b); + }; + BOOST_SERIALIZATION_ASSUME_ABSTRACT(Mapper) +} // namespace karto + +#endif // karto_sdk_MAPPER_H diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox/lib/karto_sdk/include/karto_sdk/Math.h b/Localizations/Packages/slam_toolbox/slam_toolbox/lib/karto_sdk/include/karto_sdk/Math.h new file mode 100644 index 0000000..b19cef8 --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox/lib/karto_sdk/include/karto_sdk/Math.h @@ -0,0 +1,252 @@ +/* + * Copyright 2010 SRI International + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with this program. If not, see . + */ + +#ifndef karto_sdk_MATH_H +#define karto_sdk_MATH_H + +#include +#include +#include + +#include + +namespace karto +{ + /** + * Platform independent pi definitions + */ + const kt_double KT_PI = 3.14159265358979323846; // The value of PI + const kt_double KT_2PI = 6.28318530717958647692; // 2 * PI + const kt_double KT_PI_2 = 1.57079632679489661923; // PI / 2 + const kt_double KT_PI_180 = 0.01745329251994329577; // PI / 180 + const kt_double KT_180_PI = 57.29577951308232087685; // 180 / PI + + /** + * Lets define a small number! + */ + const kt_double KT_TOLERANCE = 1e-06; + + /** + * Lets define max value of kt_int32s (int32_t) to use it to mark invalid scans + */ + + const kt_int32s INVALID_SCAN = std::numeric_limits::max(); + + namespace math + { + /** + * Converts degrees into radians + * @param degrees + * @return radian equivalent of degrees + */ + inline kt_double DegreesToRadians(kt_double degrees) + { + return degrees * KT_PI_180; + } + + /** + * Converts radians into degrees + * @param radians + * @return degree equivalent of radians + */ + inline kt_double RadiansToDegrees(kt_double radians) + { + return radians * KT_180_PI; + } + + /** + * Square function + * @param value + * @return square of value + */ + template + inline T Square(T value) + { + return (value * value); + } + + /** + * Round function + * @param value + * @return rounds value to the nearest whole number (as double) + */ + inline kt_double Round(kt_double value) + { + return value >= 0.0 ? floor(value + 0.5) : ceil(value - 0.5); + } + + /** + * Binary minimum function + * @param value1 + * @param value2 + * @return the lesser of value1 and value2 + */ + template + inline const T& Minimum(const T& value1, const T& value2) + { + return value1 < value2 ? value1 : value2; + } + + /** + * Binary maximum function + * @param value1 + * @param value2 + * @return the greater of value1 and value2 + */ + template + inline const T& Maximum(const T& value1, const T& value2) + { + return value1 > value2 ? value1 : value2; + } + + /** + * Clips a number to the specified minimum and maximum values. + * @param n number to be clipped + * @param minValue minimum value + * @param maxValue maximum value + * @return the clipped value + */ + template + inline const T& Clip(const T& n, const T& minValue, const T& maxValue) + { + return Minimum(Maximum(n, minValue), maxValue); + } + + /** + * Checks whether two numbers are equal within a certain tolerance. + * @param a + * @param b + * @return true if a and b differ by at most a certain tolerance. + */ + inline kt_bool DoubleEqual(kt_double a, kt_double b) + { + double delta = a - b; + return delta < 0.0 ? delta >= -KT_TOLERANCE : delta <= KT_TOLERANCE; + } + + /** + * Checks whether value is in the range [0;maximum) + * @param value + * @param maximum + */ + template + inline kt_bool IsUpTo(const T& value, const T& maximum) + { + return (value >= 0 && value < maximum); + } + + /** + * Checks whether value is in the range [0;maximum) + * Specialized version for unsigned int (kt_int32u) + * @param value + * @param maximum + */ + template<> + inline kt_bool IsUpTo(const kt_int32u& value, const kt_int32u& maximum) + { + return (value < maximum); + } + + + /** + * Checks whether value is in the range [a;b] + * @param value + * @param a + * @param b + */ + template + inline kt_bool InRange(const T& value, const T& a, const T& b) + { + return (value >= a && value <= b); + } + + /** + * Normalizes angle to be in the range of [-pi, pi] + * @param angle to be normalized + * @return normalized angle + */ + inline kt_double NormalizeAngle(kt_double angle) + { + while (angle < -KT_PI) + { + if (angle < -KT_2PI) + { + angle += (kt_int32u)(angle / -KT_2PI) * KT_2PI; + } + else + { + angle += KT_2PI; + } + } + + while (angle > KT_PI) + { + if (angle > KT_2PI) + { + angle -= (kt_int32u)(angle / KT_2PI) * KT_2PI; + } + else + { + angle -= KT_2PI; + } + } + + assert(math::InRange(angle, -KT_PI, KT_PI)); + + return angle; + } + + /** + * Returns an equivalent angle to the first parameter such that the difference + * when the second parameter is subtracted from this new value is an angle + * in the normalized range of [-pi, pi], i.e. abs(minuend - subtrahend) <= pi. + * @param minuend + * @param subtrahend + * @return normalized angle + */ + inline kt_double NormalizeAngleDifference(kt_double minuend, kt_double subtrahend) + { + while (minuend - subtrahend < -KT_PI) + { + minuend += KT_2PI; + } + + while (minuend - subtrahend > KT_PI) + { + minuend -= KT_2PI; + } + + return minuend; + } + + /** + * Align a value to the alignValue. + * The alignValue should be the power of two (2, 4, 8, 16, 32 and so on) + * @param value + * @param alignValue + * @return aligned value + */ + template + inline T AlignValue(size_t value, size_t alignValue = 8) + { + return static_cast ((value + (alignValue - 1)) & ~(alignValue - 1)); + } + } // namespace math + +} // namespace karto + +#endif // karto_sdk_MATH_H diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox/lib/karto_sdk/include/karto_sdk/Types.h b/Localizations/Packages/slam_toolbox/slam_toolbox/lib/karto_sdk/include/karto_sdk/Types.h new file mode 100644 index 0000000..f0e3598 --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox/lib/karto_sdk/include/karto_sdk/Types.h @@ -0,0 +1,69 @@ +/* + * Copyright 2010 SRI International + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with this program. If not, see . + */ + +#ifndef karto_sdk_TYPES_H +#define karto_sdk_TYPES_H + +#include + +/** + * Karto type definition + * Ensures platform independent types for windows, linux and mac + */ +#if defined(_MSC_VER) + + typedef signed __int8 kt_int8s; + typedef unsigned __int8 kt_int8u; + + typedef signed __int16 kt_int16s; + typedef unsigned __int16 kt_int16u; + + typedef signed __int32 kt_int32s; + typedef unsigned __int32 kt_int32u; + + typedef signed __int64 kt_int64s; + typedef unsigned __int64 kt_int64u; + +#else + + #include + + typedef int8_t kt_int8s; + typedef uint8_t kt_int8u; + + typedef int16_t kt_int16s; + typedef uint16_t kt_int16u; + + typedef int32_t kt_int32s; + typedef uint32_t kt_int32u; + +#if defined(__LP64__) + typedef signed long kt_int64s; + typedef unsigned long kt_int64u; +#else + typedef signed long long kt_int64s; + typedef unsigned long long kt_int64u; +#endif + +#endif + +typedef bool kt_bool; +typedef char kt_char; +typedef float kt_float; +typedef double kt_double; + +#endif // karto_sdk_TYPES_H diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox/lib/karto_sdk/include/karto_sdk/nanoflann.hpp b/Localizations/Packages/slam_toolbox/slam_toolbox/lib/karto_sdk/include/karto_sdk/nanoflann.hpp new file mode 100644 index 0000000..3704ee6 --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox/lib/karto_sdk/include/karto_sdk/nanoflann.hpp @@ -0,0 +1,2039 @@ +/*********************************************************************** + * Software License Agreement (BSD License) + * + * Copyright 2008-2009 Marius Muja (mariusm@cs.ubc.ca). All rights reserved. + * Copyright 2008-2009 David G. Lowe (lowe@cs.ubc.ca). All rights reserved. + * Copyright 2011-2016 Jose Luis Blanco (joseluisblancoc@gmail.com). + * All rights reserved. + * + * THE BSD LICENSE + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR + * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES + * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY + * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF + * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + *************************************************************************/ + +/** \mainpage nanoflann C++ API documentation + * nanoflann is a C++ header-only library for building KD-Trees, mostly + * optimized for 2D or 3D point clouds. + * + * nanoflann does not require compiling or installing, just an + * #include in your code. + * + * See: + * - C++ API organized by modules + * - Online README + * - Doxygen + * documentation + */ + +#ifndef NANOFLANN_HPP_ +#define NANOFLANN_HPP_ + +#include +#include +#include +#include // for abs() +#include // for fwrite() +#include // for abs() +#include +#include // std::reference_wrapper +#include +#include + +/** Library version: 0xMmP (M=Major,m=minor,P=patch) */ +#define NANOFLANN_VERSION 0x130 + +// Avoid conflicting declaration of min/max macros in windows headers +#if !defined(NOMINMAX) && \ + (defined(_WIN32) || defined(_WIN32_) || defined(WIN32) || defined(_WIN64)) +#define NOMINMAX +#ifdef max +#undef max +#undef min +#endif +#endif + +namespace nanoflann { +/** @addtogroup nanoflann_grp nanoflann C++ library for ANN + * @{ */ + +/** the PI constant (required to avoid MSVC missing symbols) */ +template T pi_const() { + return static_cast(3.14159265358979323846); +} + +/** + * Traits if object is resizable and assignable (typically has a resize | assign + * method) + */ +template struct has_resize : std::false_type {}; + +template +struct has_resize().resize(1), 0)> + : std::true_type {}; + +template struct has_assign : std::false_type {}; + +template +struct has_assign().assign(1, 0), 0)> + : std::true_type {}; + +/** + * Free function to resize a resizable object + */ +template +inline typename std::enable_if::value, void>::type +resize(Container &c, const size_t nElements) { + c.resize(nElements); +} + +/** + * Free function that has no effects on non resizable containers (e.g. + * std::array) It raises an exception if the expected size does not match + */ +template +inline typename std::enable_if::value, void>::type +resize(Container &c, const size_t nElements) { + if (nElements != c.size()) + throw std::logic_error("Try to change the size of a std::array."); +} + +/** + * Free function to assign to a container + */ +template +inline typename std::enable_if::value, void>::type +assign(Container &c, const size_t nElements, const T &value) { + c.assign(nElements, value); +} + +/** + * Free function to assign to a std::array + */ +template +inline typename std::enable_if::value, void>::type +assign(Container &c, const size_t nElements, const T &value) { + for (size_t i = 0; i < nElements; i++) + c[i] = value; +} + +/** @addtogroup result_sets_grp Result set classes + * @{ */ +template +class KNNResultSet { +public: + typedef _DistanceType DistanceType; + typedef _IndexType IndexType; + typedef _CountType CountType; + +private: + IndexType *indices; + DistanceType *dists; + CountType capacity; + CountType count; + +public: + inline KNNResultSet(CountType capacity_) + : indices(0), dists(0), capacity(capacity_), count(0) {} + + inline void init(IndexType *indices_, DistanceType *dists_) { + indices = indices_; + dists = dists_; + count = 0; + if (capacity) + dists[capacity - 1] = (std::numeric_limits::max)(); + } + + inline CountType size() const { return count; } + + inline bool full() const { return count == capacity; } + + /** + * Called during search to add an element matching the criteria. + * @return true if the search should be continued, false if the results are + * sufficient + */ + inline bool addPoint(DistanceType dist, IndexType index) { + CountType i; + for (i = count; i > 0; --i) { +#ifdef NANOFLANN_FIRST_MATCH // If defined and two points have the same + // distance, the one with the lowest-index will be + // returned first. + if ((dists[i - 1] > dist) || + ((dist == dists[i - 1]) && (indices[i - 1] > index))) { +#else + if (dists[i - 1] > dist) { +#endif + if (i < capacity) { + dists[i] = dists[i - 1]; + indices[i] = indices[i - 1]; + } + } else + break; + } + if (i < capacity) { + dists[i] = dist; + indices[i] = index; + } + if (count < capacity) + count++; + + // tell caller that the search shall continue + return true; + } + + inline DistanceType worstDist() const { return dists[capacity - 1]; } +}; + +/** operator "<" for std::sort() */ +struct IndexDist_Sorter { + /** PairType will be typically: std::pair */ + template + inline bool operator()(const PairType &p1, const PairType &p2) const { + return p1.second < p2.second; + } +}; + +/** + * A result-set class used when performing a radius based search. + */ +template +class RadiusResultSet { +public: + typedef _DistanceType DistanceType; + typedef _IndexType IndexType; + +public: + const DistanceType radius; + + std::vector> &m_indices_dists; + + inline RadiusResultSet( + DistanceType radius_, + std::vector> &indices_dists) + : radius(radius_), m_indices_dists(indices_dists) { + init(); + } + + inline void init() { clear(); } + inline void clear() { m_indices_dists.clear(); } + + inline size_t size() const { return m_indices_dists.size(); } + + inline bool full() const { return true; } + + /** + * Called during search to add an element matching the criteria. + * @return true if the search should be continued, false if the results are + * sufficient + */ + inline bool addPoint(DistanceType dist, IndexType index) { + if (dist < radius) + m_indices_dists.push_back(std::make_pair(index, dist)); + return true; + } + + inline DistanceType worstDist() const { return radius; } + + /** + * Find the worst result (furtherest neighbor) without copying or sorting + * Pre-conditions: size() > 0 + */ + std::pair worst_item() const { + if (m_indices_dists.empty()) + throw std::runtime_error("Cannot invoke RadiusResultSet::worst_item() on " + "an empty list of results."); + typedef + typename std::vector>::const_iterator + DistIt; + DistIt it = std::max_element(m_indices_dists.begin(), m_indices_dists.end(), + IndexDist_Sorter()); + return *it; + } +}; + +/** @} */ + +/** @addtogroup loadsave_grp Load/save auxiliary functions + * @{ */ +template +void save_value(FILE *stream, const T &value, size_t count = 1) { + fwrite(&value, sizeof(value), count, stream); +} + +template +void save_value(FILE *stream, const std::vector &value) { + size_t size = value.size(); + fwrite(&size, sizeof(size_t), 1, stream); + fwrite(&value[0], sizeof(T), size, stream); +} + +template +void load_value(FILE *stream, T &value, size_t count = 1) { + size_t read_cnt = fread(&value, sizeof(value), count, stream); + if (read_cnt != count) { + throw std::runtime_error("Cannot read from file"); + } +} + +template void load_value(FILE *stream, std::vector &value) { + size_t size; + size_t read_cnt = fread(&size, sizeof(size_t), 1, stream); + if (read_cnt != 1) { + throw std::runtime_error("Cannot read from file"); + } + value.resize(size); + read_cnt = fread(&value[0], sizeof(T), size, stream); + if (read_cnt != size) { + throw std::runtime_error("Cannot read from file"); + } +} +/** @} */ + +/** @addtogroup metric_grp Metric (distance) classes + * @{ */ + +struct Metric {}; + +/** Manhattan distance functor (generic version, optimized for + * high-dimensionality data sets). Corresponding distance traits: + * nanoflann::metric_L1 \tparam T Type of the elements (e.g. double, float, + * uint8_t) \tparam _DistanceType Type of distance variables (must be signed) + * (e.g. float, double, int64_t) + */ +template +struct L1_Adaptor { + typedef T ElementType; + typedef _DistanceType DistanceType; + + const DataSource &data_source; + + L1_Adaptor(const DataSource &_data_source) : data_source(_data_source) {} + + inline DistanceType evalMetric(const T *a, const size_t b_idx, size_t size, + DistanceType worst_dist = -1) const { + DistanceType result = DistanceType(); + const T *last = a + size; + const T *lastgroup = last - 3; + size_t d = 0; + + /* Process 4 items with each loop for efficiency. */ + while (a < lastgroup) { + const DistanceType diff0 = + std::abs(a[0] - data_source.kdtree_get_pt(b_idx, d++)); + const DistanceType diff1 = + std::abs(a[1] - data_source.kdtree_get_pt(b_idx, d++)); + const DistanceType diff2 = + std::abs(a[2] - data_source.kdtree_get_pt(b_idx, d++)); + const DistanceType diff3 = + std::abs(a[3] - data_source.kdtree_get_pt(b_idx, d++)); + result += diff0 + diff1 + diff2 + diff3; + a += 4; + if ((worst_dist > 0) && (result > worst_dist)) { + return result; + } + } + /* Process last 0-3 components. Not needed for standard vector lengths. */ + while (a < last) { + result += std::abs(*a++ - data_source.kdtree_get_pt(b_idx, d++)); + } + return result; + } + + template + inline DistanceType accum_dist(const U a, const V b, const size_t) const { + return std::abs(a - b); + } +}; + +/** Squared Euclidean distance functor (generic version, optimized for + * high-dimensionality data sets). Corresponding distance traits: + * nanoflann::metric_L2 \tparam T Type of the elements (e.g. double, float, + * uint8_t) \tparam _DistanceType Type of distance variables (must be signed) + * (e.g. float, double, int64_t) + */ +template +struct L2_Adaptor { + typedef T ElementType; + typedef _DistanceType DistanceType; + + const DataSource &data_source; + + L2_Adaptor(const DataSource &_data_source) : data_source(_data_source) {} + + inline DistanceType evalMetric(const T *a, const size_t b_idx, size_t size, + DistanceType worst_dist = -1) const { + DistanceType result = DistanceType(); + const T *last = a + size; + const T *lastgroup = last - 3; + size_t d = 0; + + /* Process 4 items with each loop for efficiency. */ + while (a < lastgroup) { + const DistanceType diff0 = a[0] - data_source.kdtree_get_pt(b_idx, d++); + const DistanceType diff1 = a[1] - data_source.kdtree_get_pt(b_idx, d++); + const DistanceType diff2 = a[2] - data_source.kdtree_get_pt(b_idx, d++); + const DistanceType diff3 = a[3] - data_source.kdtree_get_pt(b_idx, d++); + result += diff0 * diff0 + diff1 * diff1 + diff2 * diff2 + diff3 * diff3; + a += 4; + if ((worst_dist > 0) && (result > worst_dist)) { + return result; + } + } + /* Process last 0-3 components. Not needed for standard vector lengths. */ + while (a < last) { + const DistanceType diff0 = *a++ - data_source.kdtree_get_pt(b_idx, d++); + result += diff0 * diff0; + } + return result; + } + + template + inline DistanceType accum_dist(const U a, const V b, const size_t) const { + return (a - b) * (a - b); + } +}; + +/** Squared Euclidean (L2) distance functor (suitable for low-dimensionality + * datasets, like 2D or 3D point clouds) Corresponding distance traits: + * nanoflann::metric_L2_Simple \tparam T Type of the elements (e.g. double, + * float, uint8_t) \tparam _DistanceType Type of distance variables (must be + * signed) (e.g. float, double, int64_t) + */ +template +struct L2_Simple_Adaptor { + typedef T ElementType; + typedef _DistanceType DistanceType; + + const DataSource &data_source; + + L2_Simple_Adaptor(const DataSource &_data_source) + : data_source(_data_source) {} + + inline DistanceType evalMetric(const T *a, const size_t b_idx, + size_t size) const { + DistanceType result = DistanceType(); + for (size_t i = 0; i < size; ++i) { + const DistanceType diff = a[i] - data_source.kdtree_get_pt(b_idx, i); + result += diff * diff; + } + return result; + } + + template + inline DistanceType accum_dist(const U a, const V b, const size_t) const { + return (a - b) * (a - b); + } +}; + +/** SO2 distance functor + * Corresponding distance traits: nanoflann::metric_SO2 + * \tparam T Type of the elements (e.g. double, float) + * \tparam _DistanceType Type of distance variables (must be signed) (e.g. + * float, double) orientation is constrained to be in [-pi, pi] + */ +template +struct SO2_Adaptor { + typedef T ElementType; + typedef _DistanceType DistanceType; + + const DataSource &data_source; + + SO2_Adaptor(const DataSource &_data_source) : data_source(_data_source) {} + + inline DistanceType evalMetric(const T *a, const size_t b_idx, + size_t size) const { + return accum_dist(a[size - 1], data_source.kdtree_get_pt(b_idx, size - 1), + size - 1); + } + + /** Note: this assumes that input angles are already in the range [-pi,pi] */ + template + inline DistanceType accum_dist(const U a, const V b, const size_t) const { + DistanceType result = DistanceType(), PI = pi_const(); + result = b - a; + if (result > PI) + result -= 2 * PI; + else if (result < -PI) + result += 2 * PI; + return result; + } +}; + +/** SO3 distance functor (Uses L2_Simple) + * Corresponding distance traits: nanoflann::metric_SO3 + * \tparam T Type of the elements (e.g. double, float) + * \tparam _DistanceType Type of distance variables (must be signed) (e.g. + * float, double) + */ +template +struct SO3_Adaptor { + typedef T ElementType; + typedef _DistanceType DistanceType; + + L2_Simple_Adaptor distance_L2_Simple; + + SO3_Adaptor(const DataSource &_data_source) + : distance_L2_Simple(_data_source) {} + + inline DistanceType evalMetric(const T *a, const size_t b_idx, + size_t size) const { + return distance_L2_Simple.evalMetric(a, b_idx, size); + } + + template + inline DistanceType accum_dist(const U a, const V b, const size_t idx) const { + return distance_L2_Simple.accum_dist(a, b, idx); + } +}; + +/** Metaprogramming helper traits class for the L1 (Manhattan) metric */ +struct metric_L1 : public Metric { + template struct traits { + typedef L1_Adaptor distance_t; + }; +}; +/** Metaprogramming helper traits class for the L2 (Euclidean) metric */ +struct metric_L2 : public Metric { + template struct traits { + typedef L2_Adaptor distance_t; + }; +}; +/** Metaprogramming helper traits class for the L2_simple (Euclidean) metric */ +struct metric_L2_Simple : public Metric { + template struct traits { + typedef L2_Simple_Adaptor distance_t; + }; +}; +/** Metaprogramming helper traits class for the SO3_InnerProdQuat metric */ +struct metric_SO2 : public Metric { + template struct traits { + typedef SO2_Adaptor distance_t; + }; +}; +/** Metaprogramming helper traits class for the SO3_InnerProdQuat metric */ +struct metric_SO3 : public Metric { + template struct traits { + typedef SO3_Adaptor distance_t; + }; +}; + +/** @} */ + +/** @addtogroup param_grp Parameter structs + * @{ */ + +/** Parameters (see README.md) */ +struct KDTreeSingleIndexAdaptorParams { + KDTreeSingleIndexAdaptorParams(size_t _leaf_max_size = 10) + : leaf_max_size(_leaf_max_size) {} + + size_t leaf_max_size; +}; + +/** Search options for KDTreeSingleIndexAdaptor::findNeighbors() */ +struct SearchParams { + /** Note: The first argument (checks_IGNORED_) is ignored, but kept for + * compatibility with the FLANN interface */ + SearchParams(int checks_IGNORED_ = 32, float eps_ = 0, bool sorted_ = true) + : checks(checks_IGNORED_), eps(eps_), sorted(sorted_) {} + + int checks; //!< Ignored parameter (Kept for compatibility with the FLANN + //!< interface). + float eps; //!< search for eps-approximate neighbours (default: 0) + bool sorted; //!< only for radius search, require neighbours sorted by + //!< distance (default: true) +}; +/** @} */ + +/** @addtogroup memalloc_grp Memory allocation + * @{ */ + +/** + * Allocates (using C's malloc) a generic type T. + * + * Params: + * count = number of instances to allocate. + * Returns: pointer (of type T*) to memory buffer + */ +template inline T *allocate(size_t count = 1) { + T *mem = static_cast(::malloc(sizeof(T) * count)); + return mem; +} + +/** + * Pooled storage allocator + * + * The following routines allow for the efficient allocation of storage in + * small chunks from a specified pool. Rather than allowing each structure + * to be freed individually, an entire pool of storage is freed at once. + * This method has two advantages over just using malloc() and free(). First, + * it is far more efficient for allocating small objects, as there is + * no overhead for remembering all the information needed to free each + * object or consolidating fragmented memory. Second, the decision about + * how long to keep an object is made at the time of allocation, and there + * is no need to track down all the objects to free them. + * + */ + +const size_t WORDSIZE = 16; +const size_t BLOCKSIZE = 8192; + +class PooledAllocator { + /* We maintain memory alignment to word boundaries by requiring that all + allocations be in multiples of the machine wordsize. */ + /* Size of machine word in bytes. Must be power of 2. */ + /* Minimum number of bytes requested at a time from the system. Must be + * multiple of WORDSIZE. */ + + size_t remaining; /* Number of bytes left in current block of storage. */ + void *base; /* Pointer to base of current block of storage. */ + void *loc; /* Current location in block to next allocate memory. */ + + void internal_init() { + remaining = 0; + base = NULL; + usedMemory = 0; + wastedMemory = 0; + } + +public: + size_t usedMemory; + size_t wastedMemory; + + /** + Default constructor. Initializes a new pool. + */ + PooledAllocator() { internal_init(); } + + /** + * Destructor. Frees all the memory allocated in this pool. + */ + ~PooledAllocator() { free_all(); } + + /** Frees all allocated memory chunks */ + void free_all() { + while (base != NULL) { + void *prev = + *(static_cast(base)); /* Get pointer to prev block. */ + ::free(base); + base = prev; + } + internal_init(); + } + + /** + * Returns a pointer to a piece of new memory of the given size in bytes + * allocated from the pool. + */ + void *malloc(const size_t req_size) { + /* Round size up to a multiple of wordsize. The following expression + only works for WORDSIZE that is a power of 2, by masking last bits of + incremented size to zero. + */ + const size_t size = (req_size + (WORDSIZE - 1)) & ~(WORDSIZE - 1); + + /* Check whether a new block must be allocated. Note that the first word + of a block is reserved for a pointer to the previous block. + */ + if (size > remaining) { + + wastedMemory += remaining; + + /* Allocate new storage. */ + const size_t blocksize = + (size + sizeof(void *) + (WORDSIZE - 1) > BLOCKSIZE) + ? size + sizeof(void *) + (WORDSIZE - 1) + : BLOCKSIZE; + + // use the standard C malloc to allocate memory + void *m = ::malloc(blocksize); + if (!m) { + fprintf(stderr, "Failed to allocate memory.\n"); + return NULL; + } + + /* Fill first word of new block with pointer to previous block. */ + static_cast(m)[0] = base; + base = m; + + size_t shift = 0; + // int size_t = (WORDSIZE - ( (((size_t)m) + sizeof(void*)) & + // (WORDSIZE-1))) & (WORDSIZE-1); + + remaining = blocksize - sizeof(void *) - shift; + loc = (static_cast(m) + sizeof(void *) + shift); + } + void *rloc = loc; + loc = static_cast(loc) + size; + remaining -= size; + + usedMemory += size; + + return rloc; + } + + /** + * Allocates (using this pool) a generic type T. + * + * Params: + * count = number of instances to allocate. + * Returns: pointer (of type T*) to memory buffer + */ + template T *allocate(const size_t count = 1) { + T *mem = static_cast(this->malloc(sizeof(T) * count)); + return mem; + } +}; +/** @} */ + +/** @addtogroup nanoflann_metaprog_grp Auxiliary metaprogramming stuff + * @{ */ + +/** Used to declare fixed-size arrays when DIM>0, dynamically-allocated vectors + * when DIM=-1. Fixed size version for a generic DIM: + */ +template struct array_or_vector_selector { + typedef std::array container_t; +}; +/** Dynamic size version */ +template struct array_or_vector_selector<-1, T> { + typedef std::vector container_t; +}; + +/** @} */ + +/** kd-tree base-class + * + * Contains the member functions common to the classes KDTreeSingleIndexAdaptor + * and KDTreeSingleIndexDynamicAdaptor_. + * + * \tparam Derived The name of the class which inherits this class. + * \tparam DatasetAdaptor The user-provided adaptor (see comments above). + * \tparam Distance The distance metric to use, these are all classes derived + * from nanoflann::Metric \tparam DIM Dimensionality of data points (e.g. 3 for + * 3D points) \tparam IndexType Will be typically size_t or int + */ + +template +class KDTreeBaseClass { + +public: + /** Frees the previously-built index. Automatically called within + * buildIndex(). */ + void freeIndex(Derived &obj) { + obj.pool.free_all(); + obj.root_node = NULL; + obj.m_size_at_index_build = 0; + } + + typedef typename Distance::ElementType ElementType; + typedef typename Distance::DistanceType DistanceType; + + /*--------------------- Internal Data Structures --------------------------*/ + struct Node { + /** Union used because a node can be either a LEAF node or a non-leaf node, + * so both data fields are never used simultaneously */ + union { + struct leaf { + IndexType left, right; //!< Indices of points in leaf node + } lr; + struct nonleaf { + int divfeat; //!< Dimension used for subdivision. + DistanceType divlow, divhigh; //!< The values used for subdivision. + } sub; + } node_type; + Node *child1, *child2; //!< Child nodes (both=NULL mean its a leaf node) + }; + + typedef Node *NodePtr; + + struct Interval { + ElementType low, high; + }; + + /** + * Array of indices to vectors in the dataset. + */ + std::vector vind; + + NodePtr root_node; + + size_t m_leaf_max_size; + + size_t m_size; //!< Number of current points in the dataset + size_t m_size_at_index_build; //!< Number of points in the dataset when the + //!< index was built + int dim; //!< Dimensionality of each data point + + /** Define "BoundingBox" as a fixed-size or variable-size container depending + * on "DIM" */ + typedef + typename array_or_vector_selector::container_t BoundingBox; + + /** Define "distance_vector_t" as a fixed-size or variable-size container + * depending on "DIM" */ + typedef typename array_or_vector_selector::container_t + distance_vector_t; + + /** The KD-tree used to find neighbours */ + + BoundingBox root_bbox; + + /** + * Pooled memory allocator. + * + * Using a pooled memory allocator is more efficient + * than allocating memory directly when there is a large + * number small of memory allocations. + */ + PooledAllocator pool; + + /** Returns number of points in dataset */ + size_t size(const Derived &obj) const { return obj.m_size; } + + /** Returns the length of each point in the dataset */ + size_t veclen(const Derived &obj) { + return static_cast(DIM > 0 ? DIM : obj.dim); + } + + /// Helper accessor to the dataset points: + inline ElementType dataset_get(const Derived &obj, size_t idx, + int component) const { + return obj.dataset.kdtree_get_pt(idx, component); + } + + /** + * Computes the inde memory usage + * Returns: memory used by the index + */ + size_t usedMemory(Derived &obj) { + return obj.pool.usedMemory + obj.pool.wastedMemory + + obj.dataset.kdtree_get_point_count() * + sizeof(IndexType); // pool memory and vind array memory + } + + void computeMinMax(const Derived &obj, IndexType *ind, IndexType count, + int element, ElementType &min_elem, + ElementType &max_elem) { + min_elem = dataset_get(obj, ind[0], element); + max_elem = dataset_get(obj, ind[0], element); + for (IndexType i = 1; i < count; ++i) { + ElementType val = dataset_get(obj, ind[i], element); + if (val < min_elem) + min_elem = val; + if (val > max_elem) + max_elem = val; + } + } + + /** + * Create a tree node that subdivides the list of vecs from vind[first] + * to vind[last]. The routine is called recursively on each sublist. + * + * @param left index of the first vector + * @param right index of the last vector + */ + NodePtr divideTree(Derived &obj, const IndexType left, const IndexType right, + BoundingBox &bbox) { + NodePtr node = obj.pool.template allocate(); // allocate memory + + /* If too few exemplars remain, then make this a leaf node. */ + if ((right - left) <= static_cast(obj.m_leaf_max_size)) { + node->child1 = node->child2 = NULL; /* Mark as leaf node. */ + node->node_type.lr.left = left; + node->node_type.lr.right = right; + + // compute bounding-box of leaf points + for (int i = 0; i < (DIM > 0 ? DIM : obj.dim); ++i) { + bbox[i].low = dataset_get(obj, obj.vind[left], i); + bbox[i].high = dataset_get(obj, obj.vind[left], i); + } + for (IndexType k = left + 1; k < right; ++k) { + for (int i = 0; i < (DIM > 0 ? DIM : obj.dim); ++i) { + if (bbox[i].low > dataset_get(obj, obj.vind[k], i)) + bbox[i].low = dataset_get(obj, obj.vind[k], i); + if (bbox[i].high < dataset_get(obj, obj.vind[k], i)) + bbox[i].high = dataset_get(obj, obj.vind[k], i); + } + } + } else { + IndexType idx; + int cutfeat; + DistanceType cutval; + middleSplit_(obj, &obj.vind[0] + left, right - left, idx, cutfeat, cutval, + bbox); + + node->node_type.sub.divfeat = cutfeat; + + BoundingBox left_bbox(bbox); + left_bbox[cutfeat].high = cutval; + node->child1 = divideTree(obj, left, left + idx, left_bbox); + + BoundingBox right_bbox(bbox); + right_bbox[cutfeat].low = cutval; + node->child2 = divideTree(obj, left + idx, right, right_bbox); + + node->node_type.sub.divlow = left_bbox[cutfeat].high; + node->node_type.sub.divhigh = right_bbox[cutfeat].low; + + for (int i = 0; i < (DIM > 0 ? DIM : obj.dim); ++i) { + bbox[i].low = std::min(left_bbox[i].low, right_bbox[i].low); + bbox[i].high = std::max(left_bbox[i].high, right_bbox[i].high); + } + } + + return node; + } + + void middleSplit_(Derived &obj, IndexType *ind, IndexType count, + IndexType &index, int &cutfeat, DistanceType &cutval, + const BoundingBox &bbox) { + const DistanceType EPS = static_cast(0.00001); + ElementType max_span = bbox[0].high - bbox[0].low; + for (int i = 1; i < (DIM > 0 ? DIM : obj.dim); ++i) { + ElementType span = bbox[i].high - bbox[i].low; + if (span > max_span) { + max_span = span; + } + } + ElementType max_spread = -1; + cutfeat = 0; + for (int i = 0; i < (DIM > 0 ? DIM : obj.dim); ++i) { + ElementType span = bbox[i].high - bbox[i].low; + if (span > (1 - EPS) * max_span) { + ElementType min_elem, max_elem; + computeMinMax(obj, ind, count, i, min_elem, max_elem); + ElementType spread = max_elem - min_elem; + ; + if (spread > max_spread) { + cutfeat = i; + max_spread = spread; + } + } + } + // split in the middle + DistanceType split_val = (bbox[cutfeat].low + bbox[cutfeat].high) / 2; + ElementType min_elem, max_elem; + computeMinMax(obj, ind, count, cutfeat, min_elem, max_elem); + + if (split_val < min_elem) + cutval = min_elem; + else if (split_val > max_elem) + cutval = max_elem; + else + cutval = split_val; + + IndexType lim1, lim2; + planeSplit(obj, ind, count, cutfeat, cutval, lim1, lim2); + + if (lim1 > count / 2) + index = lim1; + else if (lim2 < count / 2) + index = lim2; + else + index = count / 2; + } + + /** + * Subdivide the list of points by a plane perpendicular on axe corresponding + * to the 'cutfeat' dimension at 'cutval' position. + * + * On return: + * dataset[ind[0..lim1-1]][cutfeat]cutval + */ + void planeSplit(Derived &obj, IndexType *ind, const IndexType count, + int cutfeat, DistanceType &cutval, IndexType &lim1, + IndexType &lim2) { + /* Move vector indices for left subtree to front of list. */ + IndexType left = 0; + IndexType right = count - 1; + for (;;) { + while (left <= right && dataset_get(obj, ind[left], cutfeat) < cutval) + ++left; + while (right && left <= right && + dataset_get(obj, ind[right], cutfeat) >= cutval) + --right; + if (left > right || !right) + break; // "!right" was added to support unsigned Index types + std::swap(ind[left], ind[right]); + ++left; + --right; + } + /* If either list is empty, it means that all remaining features + * are identical. Split in the middle to maintain a balanced tree. + */ + lim1 = left; + right = count - 1; + for (;;) { + while (left <= right && dataset_get(obj, ind[left], cutfeat) <= cutval) + ++left; + while (right && left <= right && + dataset_get(obj, ind[right], cutfeat) > cutval) + --right; + if (left > right || !right) + break; // "!right" was added to support unsigned Index types + std::swap(ind[left], ind[right]); + ++left; + --right; + } + lim2 = left; + } + + DistanceType computeInitialDistances(const Derived &obj, + const ElementType *vec, + distance_vector_t &dists) const { + assert(vec); + DistanceType distsq = DistanceType(); + + for (int i = 0; i < (DIM > 0 ? DIM : obj.dim); ++i) { + if (vec[i] < obj.root_bbox[i].low) { + dists[i] = obj.distance.accum_dist(vec[i], obj.root_bbox[i].low, i); + distsq += dists[i]; + } + if (vec[i] > obj.root_bbox[i].high) { + dists[i] = obj.distance.accum_dist(vec[i], obj.root_bbox[i].high, i); + distsq += dists[i]; + } + } + return distsq; + } + + void save_tree(Derived &obj, FILE *stream, NodePtr tree) { + save_value(stream, *tree); + if (tree->child1 != NULL) { + save_tree(obj, stream, tree->child1); + } + if (tree->child2 != NULL) { + save_tree(obj, stream, tree->child2); + } + } + + void load_tree(Derived &obj, FILE *stream, NodePtr &tree) { + tree = obj.pool.template allocate(); + load_value(stream, *tree); + if (tree->child1 != NULL) { + load_tree(obj, stream, tree->child1); + } + if (tree->child2 != NULL) { + load_tree(obj, stream, tree->child2); + } + } + + /** Stores the index in a binary file. + * IMPORTANT NOTE: The set of data points is NOT stored in the file, so when + * loading the index object it must be constructed associated to the same + * source of data points used while building it. See the example: + * examples/saveload_example.cpp \sa loadIndex */ + void saveIndex_(Derived &obj, FILE *stream) { + save_value(stream, obj.m_size); + save_value(stream, obj.dim); + save_value(stream, obj.root_bbox); + save_value(stream, obj.m_leaf_max_size); + save_value(stream, obj.vind); + save_tree(obj, stream, obj.root_node); + } + + /** Loads a previous index from a binary file. + * IMPORTANT NOTE: The set of data points is NOT stored in the file, so the + * index object must be constructed associated to the same source of data + * points used while building the index. See the example: + * examples/saveload_example.cpp \sa loadIndex */ + void loadIndex_(Derived &obj, FILE *stream) { + load_value(stream, obj.m_size); + load_value(stream, obj.dim); + load_value(stream, obj.root_bbox); + load_value(stream, obj.m_leaf_max_size); + load_value(stream, obj.vind); + load_tree(obj, stream, obj.root_node); + } +}; + +/** @addtogroup kdtrees_grp KD-tree classes and adaptors + * @{ */ + +/** kd-tree static index + * + * Contains the k-d trees and other information for indexing a set of points + * for nearest-neighbor matching. + * + * The class "DatasetAdaptor" must provide the following interface (can be + * non-virtual, inlined methods): + * + * \code + * // Must return the number of data poins + * inline size_t kdtree_get_point_count() const { ... } + * + * + * // Must return the dim'th component of the idx'th point in the class: + * inline T kdtree_get_pt(const size_t idx, const size_t dim) const { ... } + * + * // Optional bounding-box computation: return false to default to a standard + * bbox computation loop. + * // Return true if the BBOX was already computed by the class and returned + * in "bb" so it can be avoided to redo it again. + * // Look at bb.size() to find out the expected dimensionality (e.g. 2 or 3 + * for point clouds) template bool kdtree_get_bbox(BBOX &bb) const + * { + * bb[0].low = ...; bb[0].high = ...; // 0th dimension limits + * bb[1].low = ...; bb[1].high = ...; // 1st dimension limits + * ... + * return true; + * } + * + * \endcode + * + * \tparam DatasetAdaptor The user-provided adaptor (see comments above). + * \tparam Distance The distance metric to use: nanoflann::metric_L1, + * nanoflann::metric_L2, nanoflann::metric_L2_Simple, etc. \tparam DIM + * Dimensionality of data points (e.g. 3 for 3D points) \tparam IndexType Will + * be typically size_t or int + */ +template +class KDTreeSingleIndexAdaptor + : public KDTreeBaseClass< + KDTreeSingleIndexAdaptor, + Distance, DatasetAdaptor, DIM, IndexType> { +public: + /** Deleted copy constructor*/ + KDTreeSingleIndexAdaptor( + const KDTreeSingleIndexAdaptor + &) = delete; + + /** + * The dataset used by this index + */ + const DatasetAdaptor &dataset; //!< The source of our data + + const KDTreeSingleIndexAdaptorParams index_params; + + Distance distance; + + typedef typename nanoflann::KDTreeBaseClass< + nanoflann::KDTreeSingleIndexAdaptor, + Distance, DatasetAdaptor, DIM, IndexType> + BaseClassRef; + + typedef typename BaseClassRef::ElementType ElementType; + typedef typename BaseClassRef::DistanceType DistanceType; + + typedef typename BaseClassRef::Node Node; + typedef Node *NodePtr; + + typedef typename BaseClassRef::Interval Interval; + /** Define "BoundingBox" as a fixed-size or variable-size container depending + * on "DIM" */ + typedef typename BaseClassRef::BoundingBox BoundingBox; + + /** Define "distance_vector_t" as a fixed-size or variable-size container + * depending on "DIM" */ + typedef typename BaseClassRef::distance_vector_t distance_vector_t; + + /** + * KDTree constructor + * + * Refer to docs in README.md or online in + * https://github.com/jlblancoc/nanoflann + * + * The KD-Tree point dimension (the length of each point in the datase, e.g. 3 + * for 3D points) is determined by means of: + * - The \a DIM template parameter if >0 (highest priority) + * - Otherwise, the \a dimensionality parameter of this constructor. + * + * @param inputData Dataset with the input features + * @param params Basically, the maximum leaf node size + */ + KDTreeSingleIndexAdaptor(const int dimensionality, + const DatasetAdaptor &inputData, + const KDTreeSingleIndexAdaptorParams ¶ms = + KDTreeSingleIndexAdaptorParams()) + : dataset(inputData), index_params(params), distance(inputData) { + BaseClassRef::root_node = NULL; + BaseClassRef::m_size = dataset.kdtree_get_point_count(); + BaseClassRef::m_size_at_index_build = BaseClassRef::m_size; + BaseClassRef::dim = dimensionality; + if (DIM > 0) + BaseClassRef::dim = DIM; + BaseClassRef::m_leaf_max_size = params.leaf_max_size; + + // Create a permutable array of indices to the input vectors. + init_vind(); + } + + /** + * Builds the index + */ + void buildIndex() { + BaseClassRef::m_size = dataset.kdtree_get_point_count(); + BaseClassRef::m_size_at_index_build = BaseClassRef::m_size; + init_vind(); + this->freeIndex(*this); + BaseClassRef::m_size_at_index_build = BaseClassRef::m_size; + if (BaseClassRef::m_size == 0) + return; + computeBoundingBox(BaseClassRef::root_bbox); + BaseClassRef::root_node = + this->divideTree(*this, 0, BaseClassRef::m_size, + BaseClassRef::root_bbox); // construct the tree + } + + /** \name Query methods + * @{ */ + + /** + * Find set of nearest neighbors to vec[0:dim-1]. Their indices are stored + * inside the result object. + * + * Params: + * result = the result object in which the indices of the + * nearest-neighbors are stored vec = the vector for which to search the + * nearest neighbors + * + * \tparam RESULTSET Should be any ResultSet + * \return True if the requested neighbors could be found. + * \sa knnSearch, radiusSearch + */ + template + bool findNeighbors(RESULTSET &result, const ElementType *vec, + const SearchParams &searchParams) const { + assert(vec); + if (this->size(*this) == 0) + return false; + if (!BaseClassRef::root_node) + throw std::runtime_error( + "[nanoflann] findNeighbors() called before building the index."); + float epsError = 1 + searchParams.eps; + + distance_vector_t + dists; // fixed or variable-sized container (depending on DIM) + auto zero = static_cast(0); + assign(dists, (DIM > 0 ? DIM : BaseClassRef::dim), + zero); // Fill it with zeros. + DistanceType distsq = this->computeInitialDistances(*this, vec, dists); + searchLevel(result, vec, BaseClassRef::root_node, distsq, dists, + epsError); // "count_leaf" parameter removed since was neither + // used nor returned to the user. + return result.full(); + } + + /** + * Find the "num_closest" nearest neighbors to the \a query_point[0:dim-1]. + * Their indices are stored inside the result object. \sa radiusSearch, + * findNeighbors \note nChecks_IGNORED is ignored but kept for compatibility + * with the original FLANN interface. \return Number `N` of valid points in + * the result set. Only the first `N` entries in `out_indices` and + * `out_distances_sq` will be valid. Return may be less than `num_closest` + * only if the number of elements in the tree is less than `num_closest`. + */ + size_t knnSearch(const ElementType *query_point, const size_t num_closest, + IndexType *out_indices, DistanceType *out_distances_sq, + const int /* nChecks_IGNORED */ = 10) const { + nanoflann::KNNResultSet resultSet(num_closest); + resultSet.init(out_indices, out_distances_sq); + this->findNeighbors(resultSet, query_point, nanoflann::SearchParams()); + return resultSet.size(); + } + + /** + * Find all the neighbors to \a query_point[0:dim-1] within a maximum radius. + * The output is given as a vector of pairs, of which the first element is a + * point index and the second the corresponding distance. Previous contents of + * \a IndicesDists are cleared. + * + * If searchParams.sorted==true, the output list is sorted by ascending + * distances. + * + * For a better performance, it is advisable to do a .reserve() on the vector + * if you have any wild guess about the number of expected matches. + * + * \sa knnSearch, findNeighbors, radiusSearchCustomCallback + * \return The number of points within the given radius (i.e. indices.size() + * or dists.size() ) + */ + size_t + radiusSearch(const ElementType *query_point, const DistanceType &radius, + std::vector> &IndicesDists, + const SearchParams &searchParams) const { + RadiusResultSet resultSet(radius, IndicesDists); + const size_t nFound = + radiusSearchCustomCallback(query_point, resultSet, searchParams); + if (searchParams.sorted) + std::sort(IndicesDists.begin(), IndicesDists.end(), IndexDist_Sorter()); + return nFound; + } + + /** + * Just like radiusSearch() but with a custom callback class for each point + * found in the radius of the query. See the source of RadiusResultSet<> as a + * start point for your own classes. \sa radiusSearch + */ + template + size_t radiusSearchCustomCallback( + const ElementType *query_point, SEARCH_CALLBACK &resultSet, + const SearchParams &searchParams = SearchParams()) const { + this->findNeighbors(resultSet, query_point, searchParams); + return resultSet.size(); + } + + /** @} */ + +public: + /** Make sure the auxiliary list \a vind has the same size than the current + * dataset, and re-generate if size has changed. */ + void init_vind() { + // Create a permutable array of indices to the input vectors. + BaseClassRef::m_size = dataset.kdtree_get_point_count(); + if (BaseClassRef::vind.size() != BaseClassRef::m_size) + BaseClassRef::vind.resize(BaseClassRef::m_size); + for (size_t i = 0; i < BaseClassRef::m_size; i++) + BaseClassRef::vind[i] = i; + } + + void computeBoundingBox(BoundingBox &bbox) { + resize(bbox, (DIM > 0 ? DIM : BaseClassRef::dim)); + if (dataset.kdtree_get_bbox(bbox)) { + // Done! It was implemented in derived class + } else { + const size_t N = dataset.kdtree_get_point_count(); + if (!N) + throw std::runtime_error("[nanoflann] computeBoundingBox() called but " + "no data points found."); + for (int i = 0; i < (DIM > 0 ? DIM : BaseClassRef::dim); ++i) { + bbox[i].low = bbox[i].high = this->dataset_get(*this, 0, i); + } + for (size_t k = 1; k < N; ++k) { + for (int i = 0; i < (DIM > 0 ? DIM : BaseClassRef::dim); ++i) { + if (this->dataset_get(*this, k, i) < bbox[i].low) + bbox[i].low = this->dataset_get(*this, k, i); + if (this->dataset_get(*this, k, i) > bbox[i].high) + bbox[i].high = this->dataset_get(*this, k, i); + } + } + } + } + + /** + * Performs an exact search in the tree starting from a node. + * \tparam RESULTSET Should be any ResultSet + * \return true if the search should be continued, false if the results are + * sufficient + */ + template + bool searchLevel(RESULTSET &result_set, const ElementType *vec, + const NodePtr node, DistanceType mindistsq, + distance_vector_t &dists, const float epsError) const { + /* If this is a leaf node, then do check and return. */ + if ((node->child1 == NULL) && (node->child2 == NULL)) { + // count_leaf += (node->lr.right-node->lr.left); // Removed since was + // neither used nor returned to the user. + DistanceType worst_dist = result_set.worstDist(); + for (IndexType i = node->node_type.lr.left; i < node->node_type.lr.right; + ++i) { + const IndexType index = BaseClassRef::vind[i]; // reorder... : i; + DistanceType dist = distance.evalMetric( + vec, index, (DIM > 0 ? DIM : BaseClassRef::dim)); + if (dist < worst_dist) { + if (!result_set.addPoint(dist, BaseClassRef::vind[i])) { + // the resultset doesn't want to receive any more points, we're done + // searching! + return false; + } + } + } + return true; + } + + /* Which child branch should be taken first? */ + int idx = node->node_type.sub.divfeat; + ElementType val = vec[idx]; + DistanceType diff1 = val - node->node_type.sub.divlow; + DistanceType diff2 = val - node->node_type.sub.divhigh; + + NodePtr bestChild; + NodePtr otherChild; + DistanceType cut_dist; + if ((diff1 + diff2) < 0) { + bestChild = node->child1; + otherChild = node->child2; + cut_dist = distance.accum_dist(val, node->node_type.sub.divhigh, idx); + } else { + bestChild = node->child2; + otherChild = node->child1; + cut_dist = distance.accum_dist(val, node->node_type.sub.divlow, idx); + } + + /* Call recursively to search next level down. */ + if (!searchLevel(result_set, vec, bestChild, mindistsq, dists, epsError)) { + // the resultset doesn't want to receive any more points, we're done + // searching! + return false; + } + + DistanceType dst = dists[idx]; + mindistsq = mindistsq + cut_dist - dst; + dists[idx] = cut_dist; + if (mindistsq * epsError <= result_set.worstDist()) { + if (!searchLevel(result_set, vec, otherChild, mindistsq, dists, + epsError)) { + // the resultset doesn't want to receive any more points, we're done + // searching! + return false; + } + } + dists[idx] = dst; + return true; + } + +public: + /** Stores the index in a binary file. + * IMPORTANT NOTE: The set of data points is NOT stored in the file, so when + * loading the index object it must be constructed associated to the same + * source of data points used while building it. See the example: + * examples/saveload_example.cpp \sa loadIndex */ + void saveIndex(FILE *stream) { this->saveIndex_(*this, stream); } + + /** Loads a previous index from a binary file. + * IMPORTANT NOTE: The set of data points is NOT stored in the file, so the + * index object must be constructed associated to the same source of data + * points used while building the index. See the example: + * examples/saveload_example.cpp \sa loadIndex */ + void loadIndex(FILE *stream) { this->loadIndex_(*this, stream); } + +}; // class KDTree + +/** kd-tree dynamic index + * + * Contains the k-d trees and other information for indexing a set of points + * for nearest-neighbor matching. + * + * The class "DatasetAdaptor" must provide the following interface (can be + * non-virtual, inlined methods): + * + * \code + * // Must return the number of data poins + * inline size_t kdtree_get_point_count() const { ... } + * + * // Must return the dim'th component of the idx'th point in the class: + * inline T kdtree_get_pt(const size_t idx, const size_t dim) const { ... } + * + * // Optional bounding-box computation: return false to default to a standard + * bbox computation loop. + * // Return true if the BBOX was already computed by the class and returned + * in "bb" so it can be avoided to redo it again. + * // Look at bb.size() to find out the expected dimensionality (e.g. 2 or 3 + * for point clouds) template bool kdtree_get_bbox(BBOX &bb) const + * { + * bb[0].low = ...; bb[0].high = ...; // 0th dimension limits + * bb[1].low = ...; bb[1].high = ...; // 1st dimension limits + * ... + * return true; + * } + * + * \endcode + * + * \tparam DatasetAdaptor The user-provided adaptor (see comments above). + * \tparam Distance The distance metric to use: nanoflann::metric_L1, + * nanoflann::metric_L2, nanoflann::metric_L2_Simple, etc. \tparam DIM + * Dimensionality of data points (e.g. 3 for 3D points) \tparam IndexType Will + * be typically size_t or int + */ +template +class KDTreeSingleIndexDynamicAdaptor_ + : public KDTreeBaseClass, + Distance, DatasetAdaptor, DIM, IndexType> { +public: + /** + * The dataset used by this index + */ + const DatasetAdaptor &dataset; //!< The source of our data + + KDTreeSingleIndexAdaptorParams index_params; + + std::vector &treeIndex; + + Distance distance; + + typedef typename nanoflann::KDTreeBaseClass< + nanoflann::KDTreeSingleIndexDynamicAdaptor_, + Distance, DatasetAdaptor, DIM, IndexType> + BaseClassRef; + + typedef typename BaseClassRef::ElementType ElementType; + typedef typename BaseClassRef::DistanceType DistanceType; + + typedef typename BaseClassRef::Node Node; + typedef Node *NodePtr; + + typedef typename BaseClassRef::Interval Interval; + /** Define "BoundingBox" as a fixed-size or variable-size container depending + * on "DIM" */ + typedef typename BaseClassRef::BoundingBox BoundingBox; + + /** Define "distance_vector_t" as a fixed-size or variable-size container + * depending on "DIM" */ + typedef typename BaseClassRef::distance_vector_t distance_vector_t; + + /** + * KDTree constructor + * + * Refer to docs in README.md or online in + * https://github.com/jlblancoc/nanoflann + * + * The KD-Tree point dimension (the length of each point in the datase, e.g. 3 + * for 3D points) is determined by means of: + * - The \a DIM template parameter if >0 (highest priority) + * - Otherwise, the \a dimensionality parameter of this constructor. + * + * @param inputData Dataset with the input features + * @param params Basically, the maximum leaf node size + */ + KDTreeSingleIndexDynamicAdaptor_( + const int dimensionality, const DatasetAdaptor &inputData, + std::vector &treeIndex_, + const KDTreeSingleIndexAdaptorParams ¶ms = + KDTreeSingleIndexAdaptorParams()) + : dataset(inputData), index_params(params), treeIndex(treeIndex_), + distance(inputData) { + BaseClassRef::root_node = NULL; + BaseClassRef::m_size = 0; + BaseClassRef::m_size_at_index_build = 0; + BaseClassRef::dim = dimensionality; + if (DIM > 0) + BaseClassRef::dim = DIM; + BaseClassRef::m_leaf_max_size = params.leaf_max_size; + } + + /** Assignment operator definiton */ + KDTreeSingleIndexDynamicAdaptor_ + operator=(const KDTreeSingleIndexDynamicAdaptor_ &rhs) { + KDTreeSingleIndexDynamicAdaptor_ tmp(rhs); + std::swap(BaseClassRef::vind, tmp.BaseClassRef::vind); + std::swap(BaseClassRef::m_leaf_max_size, tmp.BaseClassRef::m_leaf_max_size); + std::swap(index_params, tmp.index_params); + std::swap(treeIndex, tmp.treeIndex); + std::swap(BaseClassRef::m_size, tmp.BaseClassRef::m_size); + std::swap(BaseClassRef::m_size_at_index_build, + tmp.BaseClassRef::m_size_at_index_build); + std::swap(BaseClassRef::root_node, tmp.BaseClassRef::root_node); + std::swap(BaseClassRef::root_bbox, tmp.BaseClassRef::root_bbox); + std::swap(BaseClassRef::pool, tmp.BaseClassRef::pool); + return *this; + } + + /** + * Builds the index + */ + void buildIndex() { + BaseClassRef::m_size = BaseClassRef::vind.size(); + this->freeIndex(*this); + BaseClassRef::m_size_at_index_build = BaseClassRef::m_size; + if (BaseClassRef::m_size == 0) + return; + computeBoundingBox(BaseClassRef::root_bbox); + BaseClassRef::root_node = + this->divideTree(*this, 0, BaseClassRef::m_size, + BaseClassRef::root_bbox); // construct the tree + } + + /** \name Query methods + * @{ */ + + /** + * Find set of nearest neighbors to vec[0:dim-1]. Their indices are stored + * inside the result object. + * + * Params: + * result = the result object in which the indices of the + * nearest-neighbors are stored vec = the vector for which to search the + * nearest neighbors + * + * \tparam RESULTSET Should be any ResultSet + * \return True if the requested neighbors could be found. + * \sa knnSearch, radiusSearch + */ + template + bool findNeighbors(RESULTSET &result, const ElementType *vec, + const SearchParams &searchParams) const { + assert(vec); + if (this->size(*this) == 0) + return false; + if (!BaseClassRef::root_node) + return false; + float epsError = 1 + searchParams.eps; + + // fixed or variable-sized container (depending on DIM) + distance_vector_t dists; + // Fill it with zeros. + assign(dists, (DIM > 0 ? DIM : BaseClassRef::dim), + static_cast(0)); + DistanceType distsq = this->computeInitialDistances(*this, vec, dists); + searchLevel(result, vec, BaseClassRef::root_node, distsq, dists, + epsError); // "count_leaf" parameter removed since was neither + // used nor returned to the user. + return result.full(); + } + + /** + * Find the "num_closest" nearest neighbors to the \a query_point[0:dim-1]. + * Their indices are stored inside the result object. \sa radiusSearch, + * findNeighbors \note nChecks_IGNORED is ignored but kept for compatibility + * with the original FLANN interface. \return Number `N` of valid points in + * the result set. Only the first `N` entries in `out_indices` and + * `out_distances_sq` will be valid. Return may be less than `num_closest` + * only if the number of elements in the tree is less than `num_closest`. + */ + size_t knnSearch(const ElementType *query_point, const size_t num_closest, + IndexType *out_indices, DistanceType *out_distances_sq, + const int /* nChecks_IGNORED */ = 10) const { + nanoflann::KNNResultSet resultSet(num_closest); + resultSet.init(out_indices, out_distances_sq); + this->findNeighbors(resultSet, query_point, nanoflann::SearchParams()); + return resultSet.size(); + } + + /** + * Find all the neighbors to \a query_point[0:dim-1] within a maximum radius. + * The output is given as a vector of pairs, of which the first element is a + * point index and the second the corresponding distance. Previous contents of + * \a IndicesDists are cleared. + * + * If searchParams.sorted==true, the output list is sorted by ascending + * distances. + * + * For a better performance, it is advisable to do a .reserve() on the vector + * if you have any wild guess about the number of expected matches. + * + * \sa knnSearch, findNeighbors, radiusSearchCustomCallback + * \return The number of points within the given radius (i.e. indices.size() + * or dists.size() ) + */ + size_t + radiusSearch(const ElementType *query_point, const DistanceType &radius, + std::vector> &IndicesDists, + const SearchParams &searchParams) const { + RadiusResultSet resultSet(radius, IndicesDists); + const size_t nFound = + radiusSearchCustomCallback(query_point, resultSet, searchParams); + if (searchParams.sorted) + std::sort(IndicesDists.begin(), IndicesDists.end(), IndexDist_Sorter()); + return nFound; + } + + /** + * Just like radiusSearch() but with a custom callback class for each point + * found in the radius of the query. See the source of RadiusResultSet<> as a + * start point for your own classes. \sa radiusSearch + */ + template + size_t radiusSearchCustomCallback( + const ElementType *query_point, SEARCH_CALLBACK &resultSet, + const SearchParams &searchParams = SearchParams()) const { + this->findNeighbors(resultSet, query_point, searchParams); + return resultSet.size(); + } + + /** @} */ + +public: + void computeBoundingBox(BoundingBox &bbox) { + resize(bbox, (DIM > 0 ? DIM : BaseClassRef::dim)); + + if (dataset.kdtree_get_bbox(bbox)) { + // Done! It was implemented in derived class + } else { + const size_t N = BaseClassRef::m_size; + if (!N) + throw std::runtime_error("[nanoflann] computeBoundingBox() called but " + "no data points found."); + for (int i = 0; i < (DIM > 0 ? DIM : BaseClassRef::dim); ++i) { + bbox[i].low = bbox[i].high = + this->dataset_get(*this, BaseClassRef::vind[0], i); + } + for (size_t k = 1; k < N; ++k) { + for (int i = 0; i < (DIM > 0 ? DIM : BaseClassRef::dim); ++i) { + if (this->dataset_get(*this, BaseClassRef::vind[k], i) < bbox[i].low) + bbox[i].low = this->dataset_get(*this, BaseClassRef::vind[k], i); + if (this->dataset_get(*this, BaseClassRef::vind[k], i) > bbox[i].high) + bbox[i].high = this->dataset_get(*this, BaseClassRef::vind[k], i); + } + } + } + } + + /** + * Performs an exact search in the tree starting from a node. + * \tparam RESULTSET Should be any ResultSet + */ + template + void searchLevel(RESULTSET &result_set, const ElementType *vec, + const NodePtr node, DistanceType mindistsq, + distance_vector_t &dists, const float epsError) const { + /* If this is a leaf node, then do check and return. */ + if ((node->child1 == NULL) && (node->child2 == NULL)) { + // count_leaf += (node->lr.right-node->lr.left); // Removed since was + // neither used nor returned to the user. + DistanceType worst_dist = result_set.worstDist(); + for (IndexType i = node->node_type.lr.left; i < node->node_type.lr.right; + ++i) { + const IndexType index = BaseClassRef::vind[i]; // reorder... : i; + if (treeIndex[index] == -1) + continue; + DistanceType dist = distance.evalMetric( + vec, index, (DIM > 0 ? DIM : BaseClassRef::dim)); + if (dist < worst_dist) { + if (!result_set.addPoint( + static_cast(dist), + static_cast( + BaseClassRef::vind[i]))) { + // the resultset doesn't want to receive any more points, we're done + // searching! + return; // false; + } + } + } + return; + } + + /* Which child branch should be taken first? */ + int idx = node->node_type.sub.divfeat; + ElementType val = vec[idx]; + DistanceType diff1 = val - node->node_type.sub.divlow; + DistanceType diff2 = val - node->node_type.sub.divhigh; + + NodePtr bestChild; + NodePtr otherChild; + DistanceType cut_dist; + if ((diff1 + diff2) < 0) { + bestChild = node->child1; + otherChild = node->child2; + cut_dist = distance.accum_dist(val, node->node_type.sub.divhigh, idx); + } else { + bestChild = node->child2; + otherChild = node->child1; + cut_dist = distance.accum_dist(val, node->node_type.sub.divlow, idx); + } + + /* Call recursively to search next level down. */ + searchLevel(result_set, vec, bestChild, mindistsq, dists, epsError); + + DistanceType dst = dists[idx]; + mindistsq = mindistsq + cut_dist - dst; + dists[idx] = cut_dist; + if (mindistsq * epsError <= result_set.worstDist()) { + searchLevel(result_set, vec, otherChild, mindistsq, dists, epsError); + } + dists[idx] = dst; + } + +public: + /** Stores the index in a binary file. + * IMPORTANT NOTE: The set of data points is NOT stored in the file, so when + * loading the index object it must be constructed associated to the same + * source of data points used while building it. See the example: + * examples/saveload_example.cpp \sa loadIndex */ + void saveIndex(FILE *stream) { this->saveIndex_(*this, stream); } + + /** Loads a previous index from a binary file. + * IMPORTANT NOTE: The set of data points is NOT stored in the file, so the + * index object must be constructed associated to the same source of data + * points used while building the index. See the example: + * examples/saveload_example.cpp \sa loadIndex */ + void loadIndex(FILE *stream) { this->loadIndex_(*this, stream); } +}; + +/** kd-tree dynaimic index + * + * class to create multiple static index and merge their results to behave as + * single dynamic index as proposed in Logarithmic Approach. + * + * Example of usage: + * examples/dynamic_pointcloud_example.cpp + * + * \tparam DatasetAdaptor The user-provided adaptor (see comments above). + * \tparam Distance The distance metric to use: nanoflann::metric_L1, + * nanoflann::metric_L2, nanoflann::metric_L2_Simple, etc. \tparam DIM + * Dimensionality of data points (e.g. 3 for 3D points) \tparam IndexType Will + * be typically size_t or int + */ +template +class KDTreeSingleIndexDynamicAdaptor { +public: + typedef typename Distance::ElementType ElementType; + typedef typename Distance::DistanceType DistanceType; + +protected: + size_t m_leaf_max_size; + size_t treeCount; + size_t pointCount; + + /** + * The dataset used by this index + */ + const DatasetAdaptor &dataset; //!< The source of our data + + std::vector treeIndex; //!< treeIndex[idx] is the index of tree in which + //!< point at idx is stored. treeIndex[idx]=-1 + //!< means that point has been removed. + + KDTreeSingleIndexAdaptorParams index_params; + + int dim; //!< Dimensionality of each data point + + typedef KDTreeSingleIndexDynamicAdaptor_ + index_container_t; + std::vector index; + +public: + /** Get a const ref to the internal list of indices; the number of indices is + * adapted dynamically as the dataset grows in size. */ + const std::vector &getAllIndices() const { return index; } + +private: + /** finds position of least significant unset bit */ + int First0Bit(IndexType num) { + int pos = 0; + while (num & 1) { + num = num >> 1; + pos++; + } + return pos; + } + + /** Creates multiple empty trees to handle dynamic support */ + void init() { + typedef KDTreeSingleIndexDynamicAdaptor_ + my_kd_tree_t; + std::vector index_( + treeCount, my_kd_tree_t(dim /*dim*/, dataset, treeIndex, index_params)); + index = index_; + } + +public: + Distance distance; + + /** + * KDTree constructor + * + * Refer to docs in README.md or online in + * https://github.com/jlblancoc/nanoflann + * + * The KD-Tree point dimension (the length of each point in the datase, e.g. 3 + * for 3D points) is determined by means of: + * - The \a DIM template parameter if >0 (highest priority) + * - Otherwise, the \a dimensionality parameter of this constructor. + * + * @param inputData Dataset with the input features + * @param params Basically, the maximum leaf node size + */ + KDTreeSingleIndexDynamicAdaptor(const int dimensionality, + const DatasetAdaptor &inputData, + const KDTreeSingleIndexAdaptorParams ¶ms = + KDTreeSingleIndexAdaptorParams(), + const size_t maximumPointCount = 1000000000U) + : dataset(inputData), index_params(params), distance(inputData) { + treeCount = static_cast(std::log2(maximumPointCount)); + pointCount = 0U; + dim = dimensionality; + treeIndex.clear(); + if (DIM > 0) + dim = DIM; + m_leaf_max_size = params.leaf_max_size; + init(); + const size_t num_initial_points = dataset.kdtree_get_point_count(); + if (num_initial_points > 0) { + addPoints(0, num_initial_points - 1); + } + } + + /** Deleted copy constructor*/ + KDTreeSingleIndexDynamicAdaptor( + const KDTreeSingleIndexDynamicAdaptor &) = delete; + + /** Add points to the set, Inserts all points from [start, end] */ + void addPoints(IndexType start, IndexType end) { + size_t count = end - start + 1; + treeIndex.resize(treeIndex.size() + count); + for (IndexType idx = start; idx <= end; idx++) { + int pos = First0Bit(pointCount); + index[pos].vind.clear(); + treeIndex[pointCount] = pos; + for (int i = 0; i < pos; i++) { + for (int j = 0; j < static_cast(index[i].vind.size()); j++) { + index[pos].vind.push_back(index[i].vind[j]); + if (treeIndex[index[i].vind[j]] != -1) + treeIndex[index[i].vind[j]] = pos; + } + index[i].vind.clear(); + index[i].freeIndex(index[i]); + } + index[pos].vind.push_back(idx); + index[pos].buildIndex(); + pointCount++; + } + } + + /** Remove a point from the set (Lazy Deletion) */ + void removePoint(size_t idx) { + if (idx >= pointCount) + return; + treeIndex[idx] = -1; + } + + /** + * Find set of nearest neighbors to vec[0:dim-1]. Their indices are stored + * inside the result object. + * + * Params: + * result = the result object in which the indices of the + * nearest-neighbors are stored vec = the vector for which to search the + * nearest neighbors + * + * \tparam RESULTSET Should be any ResultSet + * \return True if the requested neighbors could be found. + * \sa knnSearch, radiusSearch + */ + template + bool findNeighbors(RESULTSET &result, const ElementType *vec, + const SearchParams &searchParams) const { + for (size_t i = 0; i < treeCount; i++) { + index[i].findNeighbors(result, &vec[0], searchParams); + } + return result.full(); + } +}; + +/** An L2-metric KD-tree adaptor for working with data directly stored in an + * Eigen Matrix, without duplicating the data storage. Each row in the matrix + * represents a point in the state space. + * + * Example of usage: + * \code + * Eigen::Matrix mat; + * // Fill out "mat"... + * + * typedef KDTreeEigenMatrixAdaptor< Eigen::Matrix > + * my_kd_tree_t; const int max_leaf = 10; my_kd_tree_t mat_index(mat, max_leaf + * ); mat_index.index->buildIndex(); mat_index.index->... \endcode + * + * \tparam DIM If set to >0, it specifies a compile-time fixed dimensionality + * for the points in the data set, allowing more compiler optimizations. \tparam + * Distance The distance metric to use: nanoflann::metric_L1, + * nanoflann::metric_L2, nanoflann::metric_L2_Simple, etc. + */ +template +struct KDTreeEigenMatrixAdaptor { + typedef KDTreeEigenMatrixAdaptor self_t; + typedef typename MatrixType::Scalar num_t; + typedef typename MatrixType::Index IndexType; + typedef + typename Distance::template traits::distance_t metric_t; + typedef KDTreeSingleIndexAdaptor + index_t; + + index_t *index; //! The kd-tree index for the user to call its methods as + //! usual with any other FLANN index. + + /// Constructor: takes a const ref to the matrix object with the data points + KDTreeEigenMatrixAdaptor(const size_t dimensionality, + const std::reference_wrapper &mat, + const int leaf_max_size = 10) + : m_data_matrix(mat) { + const auto dims = mat.get().cols(); + if (size_t(dims) != dimensionality) + throw std::runtime_error( + "Error: 'dimensionality' must match column count in data matrix"); + if (DIM > 0 && int(dims) != DIM) + throw std::runtime_error( + "Data set dimensionality does not match the 'DIM' template argument"); + index = + new index_t(static_cast(dims), *this /* adaptor */, + nanoflann::KDTreeSingleIndexAdaptorParams(leaf_max_size)); + index->buildIndex(); + } + +public: + /** Deleted copy constructor */ + KDTreeEigenMatrixAdaptor(const self_t &) = delete; + + ~KDTreeEigenMatrixAdaptor() { delete index; } + + const std::reference_wrapper m_data_matrix; + + /** Query for the \a num_closest closest points to a given point (entered as + * query_point[0:dim-1]). Note that this is a short-cut method for + * index->findNeighbors(). The user can also call index->... methods as + * desired. \note nChecks_IGNORED is ignored but kept for compatibility with + * the original FLANN interface. + */ + inline void query(const num_t *query_point, const size_t num_closest, + IndexType *out_indices, num_t *out_distances_sq, + const int /* nChecks_IGNORED */ = 10) const { + nanoflann::KNNResultSet resultSet(num_closest); + resultSet.init(out_indices, out_distances_sq); + index->findNeighbors(resultSet, query_point, nanoflann::SearchParams()); + } + + /** @name Interface expected by KDTreeSingleIndexAdaptor + * @{ */ + + const self_t &derived() const { return *this; } + self_t &derived() { return *this; } + + // Must return the number of data points + inline size_t kdtree_get_point_count() const { + return m_data_matrix.get().rows(); + } + + // Returns the dim'th component of the idx'th point in the class: + inline num_t kdtree_get_pt(const IndexType idx, size_t dim) const { + return m_data_matrix.get().coeff(idx, IndexType(dim)); + } + + // Optional bounding-box computation: return false to default to a standard + // bbox computation loop. + // Return true if the BBOX was already computed by the class and returned in + // "bb" so it can be avoided to redo it again. Look at bb.size() to find out + // the expected dimensionality (e.g. 2 or 3 for point clouds) + template bool kdtree_get_bbox(BBOX & /*bb*/) const { + return false; + } + + /** @} */ + +}; // end of KDTreeEigenMatrixAdaptor + /** @} */ + +/** @} */ // end of grouping +} // namespace nanoflann + +#endif /* NANOFLANN_HPP_ */ diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox/lib/karto_sdk/include/karto_sdk/nanoflann_adaptors.h b/Localizations/Packages/slam_toolbox/slam_toolbox/lib/karto_sdk/include/karto_sdk/nanoflann_adaptors.h new file mode 100644 index 0000000..1b67810 --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox/lib/karto_sdk/include/karto_sdk/nanoflann_adaptors.h @@ -0,0 +1,75 @@ +/* + * nanoflann_adaptors.h + * Copyright (c) 2018, Simbe Robotics + * Copyright (c) 2019, Samsung Research America + * + * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE + * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY + * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS + * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED. + * + * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO + * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS + * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND + * CONDITIONS. + * + */ + +/* Author: Steven Macenski */ + +#include "nanoflann.hpp" + +// And this is the "dataset to kd-tree" adaptor class: +template +struct VertexVectorPoseNanoFlannAdaptor +{ + const Derived &obj; //!< A const ref to the data set origin + + /// The constructor that sets the data set source + VertexVectorPoseNanoFlannAdaptor(const Derived &obj_) : obj(obj_) { } + + /// CRTP helper method + inline const Derived& derived() const { return obj; } + + // Must return the number of data points + inline size_t kdtree_get_point_count() const { return derived().size(); } + + // Returns the dim'th component of the idx'th point in the class: + // Since this is inlined and the "dim" argument is typically an immediate value, the + // "if/else's" are actually solved at compile time. + inline double kdtree_get_pt(const size_t idx, const size_t dim) const + { + if (dim == 0) return derived()[idx]->GetObject()->GetCorrectedPose().GetX(); + else return derived()[idx]->GetObject()->GetCorrectedPose().GetY(); + } + + // Optional bounding-box computation: return false to default to a standard bbox computation loop. + // Return true if the BBOX was already computed by the class and returned in "bb" so it can be avoided to redo it again. + // Look at bb.size() to find out the expected dimensionality (e.g. 2 or 3 for point clouds) + template + bool kdtree_get_bbox(BBOX& /*bb*/) const { return false; } + +}; // end of VertexVectorPoseNanoFlannAdaptor + +// And this is the "dataset to kd-tree" adaptor class: +template +struct VertexVectorScanCenterNanoFlannAdaptor +{ + const Derived &obj; + + VertexVectorScanCenterNanoFlannAdaptor(const Derived &obj_) : obj(obj_) { } + + inline const Derived& derived() const { return obj; } + + inline size_t kdtree_get_point_count() const { return derived().size(); } + + inline double kdtree_get_pt(const size_t idx, const size_t dim) const + { + if (dim == 0) return derived()[idx]->GetObject()->GetBarycenterPose().GetX(); + else return derived()[idx]->GetObject()->GetBarycenterPose().GetY(); + } + + template + bool kdtree_get_bbox(BBOX& /*bb*/) const { return false; } + +}; // end of VertexVectorScanCenterNanoFlannAdaptor diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox/lib/karto_sdk/package.xml b/Localizations/Packages/slam_toolbox/slam_toolbox/lib/karto_sdk/package.xml new file mode 100644 index 0000000..9a312ff --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox/lib/karto_sdk/package.xml @@ -0,0 +1,23 @@ + + + karto_sdk + 1.5.2 + Catkinized ROS packaging of the OpenKarto library + + Michael Ferguson + Luc Bettaieb + LGPLv3 + + catkin + + boost + tbb + libblas-dev + liblapack-dev + + boost + tbb + libblas-dev + liblapack-dev + + diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox/lib/karto_sdk/src/Karto.cpp b/Localizations/Packages/slam_toolbox/slam_toolbox/lib/karto_sdk/src/Karto.cpp new file mode 100644 index 0000000..70e7634 --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox/lib/karto_sdk/src/Karto.cpp @@ -0,0 +1,279 @@ +/* + * Copyright 2010 SRI International + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with this program. If not, see . + */ + +#include +#include +#include +#include +#include +#include +#include +#include "karto_sdk/Karto.h" +#include +BOOST_CLASS_EXPORT_IMPLEMENT(karto::NonCopyable); +BOOST_CLASS_EXPORT_IMPLEMENT(karto::Object); +BOOST_CLASS_EXPORT_IMPLEMENT(karto::Sensor); +BOOST_CLASS_EXPORT_IMPLEMENT(karto::SensorData); +BOOST_CLASS_EXPORT_IMPLEMENT(karto::Name); +BOOST_CLASS_EXPORT_IMPLEMENT(karto::LaserRangeScan); +BOOST_CLASS_EXPORT_IMPLEMENT(karto::LocalizedRangeScan); +BOOST_CLASS_EXPORT_IMPLEMENT(karto::CustomData); +BOOST_CLASS_EXPORT_IMPLEMENT(karto::Module); +BOOST_CLASS_EXPORT_IMPLEMENT(karto::LaserRangeFinder); +BOOST_CLASS_EXPORT_IMPLEMENT(karto::Dataset); +BOOST_CLASS_EXPORT_IMPLEMENT(karto::LookupArray); +BOOST_CLASS_EXPORT_IMPLEMENT(karto::SensorManager); +BOOST_CLASS_EXPORT_IMPLEMENT(karto::AbstractParameter); +BOOST_CLASS_EXPORT_IMPLEMENT(karto::ParameterEnum); +BOOST_CLASS_EXPORT_IMPLEMENT(karto::Parameters); +BOOST_CLASS_EXPORT_IMPLEMENT(karto::ParameterManager); +BOOST_CLASS_EXPORT_IMPLEMENT(karto::Parameter); +BOOST_CLASS_EXPORT_IMPLEMENT(karto::Parameter); +BOOST_CLASS_EXPORT_IMPLEMENT(karto::Parameter); +BOOST_CLASS_EXPORT_IMPLEMENT(karto::Parameter); +BOOST_CLASS_EXPORT_IMPLEMENT(karto::Parameter); +BOOST_CLASS_EXPORT_IMPLEMENT(karto::Parameter); +namespace karto +{ + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + SensorManager* SensorManager::GetInstance() + { + static Singleton sInstance; + return sInstance.Get(); + } + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + Object::Object() + : m_pParameterManager(new ParameterManager()) + { + } + + Object::Object(const Name& rName) + : m_Name(rName) + , m_pParameterManager(new ParameterManager()) + { + } + + Object::~Object() + { + delete m_pParameterManager; + m_pParameterManager = NULL; + } + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + Module::Module(const std::string& rName) + : Object(rName) + { + } + + Module::~Module() + { + } + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + Sensor::Sensor(const Name& rName) + : Object(rName) + { + m_pOffsetPose = new Parameter("OffsetPose", Pose2(), GetParameterManager()); + } + + Sensor::~Sensor() + { + } + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + SensorData::SensorData(const Name& rSensorName) + : Object() + , m_StateId(-1) + , m_UniqueId(-1) + , m_SensorName(rSensorName) + , m_Time(0.0) + { + } + + SensorData::~SensorData() + { + forEach(CustomDataVector, &m_CustomData) + { + delete *iter; + } + + m_CustomData.clear(); + } + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + void CellUpdater::operator() (kt_int32u index) + { + kt_int8u* pDataPtr = m_pOccupancyGrid->GetDataPointer(); + kt_int32u* pCellPassCntPtr = m_pOccupancyGrid->m_pCellPassCnt->GetDataPointer(); + kt_int32u* pCellHitCntPtr = m_pOccupancyGrid->m_pCellHitsCnt->GetDataPointer(); + + m_pOccupancyGrid->UpdateCell(&pDataPtr[index], pCellPassCntPtr[index], pCellHitCntPtr[index]); + } + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + std::ostream& operator << (std::ostream& rStream, Exception& rException) + { + rStream << "Error detect: " << std::endl; + rStream << " ==> error code: " << rException.GetErrorCode() << std::endl; + rStream << " ==> error message: " << rException.GetErrorMessage() << std::endl; + + return rStream; + } + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + const PointVectorDouble LaserRangeFinder::GetPointReadings(LocalizedRangeScan* pLocalizedRangeScan, + CoordinateConverter* pCoordinateConverter, + kt_bool ignoreThresholdPoints, + kt_bool flipY) const + { + PointVectorDouble pointReadings; + + Pose2 scanPose = pLocalizedRangeScan->GetSensorPose(); + + // compute point readings + kt_int32u beamNum = 0; + kt_double* pRangeReadings = pLocalizedRangeScan->GetRangeReadings(); + for (kt_int32u i = 0; i < m_NumberOfRangeReadings; i++, beamNum++) + { + kt_double rangeReading = pRangeReadings[i]; + + if (ignoreThresholdPoints) + { + if (!math::InRange(rangeReading, GetMinimumRange(), GetRangeThreshold())) + { + continue; + } + } + else + { + rangeReading = math::Clip(rangeReading, GetMinimumRange(), GetRangeThreshold()); + } + + kt_double angle = scanPose.GetHeading() + GetMinimumAngle() + beamNum * GetAngularResolution(); + + Vector2 point; + + point.SetX(scanPose.GetX() + (rangeReading * cos(angle))); + point.SetY(scanPose.GetY() + (rangeReading * sin(angle))); + + if (pCoordinateConverter != NULL) + { + Vector2 gridPoint = pCoordinateConverter->WorldToGrid(point, flipY); + point.SetX(gridPoint.GetX()); + point.SetY(gridPoint.GetY()); + } + + pointReadings.push_back(point); + } + + return pointReadings; + } + + kt_bool LaserRangeFinder::Validate(SensorData* pSensorData) + { + LaserRangeScan* pLaserRangeScan = dynamic_cast(pSensorData); + + // verify number of range readings in LaserRangeScan matches the number of expected range readings + // if (pLaserRangeScan->GetNumberOfRangeReadings() != GetNumberOfRangeReadings()) + // { + // std::cout << "LaserRangeScan contains " << pLaserRangeScan->GetNumberOfRangeReadings() + // << " range readings, expected " << GetNumberOfRangeReadings() << std::endl; + // return false; + // } + + return true; + } + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + void ParameterManager::Clear() + { + forEach(karto::ParameterVector, &m_Parameters) + { + delete *iter; + } + + m_Parameters.clear(); + + m_ParameterLookup.clear(); + } + + void ParameterManager::Add(AbstractParameter* pParameter) + { + if (pParameter != NULL && pParameter->GetName() != "") + { + if (m_ParameterLookup.find(pParameter->GetName()) == m_ParameterLookup.end()) + { + m_Parameters.push_back(pParameter); + + m_ParameterLookup[pParameter->GetName()] = pParameter; + } + else + { + m_ParameterLookup[pParameter->GetName()]->SetValueFromString(pParameter->GetValueAsString()); + + assert(false); + } + } + } + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + /* + std::string LaserRangeFinder::LaserRangeFinderTypeNames[6] = + { + "Custom", + + "Sick_LMS100", + "Sick_LMS200", + "Sick_LMS291", + + "Hokuyo_UTM_30LX", + "Hokuyo_URG_04LX" + }; + */ +} // namespace karto diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox/lib/karto_sdk/src/Mapper.cpp b/Localizations/Packages/slam_toolbox/slam_toolbox/lib/karto_sdk/src/Mapper.cpp new file mode 100644 index 0000000..1bc48db --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox/lib/karto_sdk/src/Mapper.cpp @@ -0,0 +1,3346 @@ +/* + * Copyright 2010 SRI International + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with this program. If not, see . + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "karto_sdk/Mapper.h" + +BOOST_CLASS_EXPORT(karto::MapperGraph); +BOOST_CLASS_EXPORT(karto::Graph); +BOOST_CLASS_EXPORT(karto::EdgeLabel); +BOOST_CLASS_EXPORT(karto::LinkInfo); +BOOST_CLASS_EXPORT(karto::Edge); +BOOST_CLASS_EXPORT(karto::Vertex); +BOOST_CLASS_EXPORT(karto::MapperSensorManager) +BOOST_CLASS_EXPORT(karto::Mapper) +namespace karto +{ + + // enable this for verbose debug information + // #define KARTO_DEBUG + + #define MAX_VARIANCE 500.0 + #define DISTANCE_PENALTY_GAIN 0.2 + #define ANGLE_PENALTY_GAIN 0.2 + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + /** + * Manages the scan data for a device + */ + class ScanManager + { + public: + /** + * Default constructor + */ + ScanManager(kt_int32u runningBufferMaximumSize, kt_double runningBufferMaximumDistance) + : m_pLastScan(NULL) + , m_RunningBufferMaximumSize(runningBufferMaximumSize) + , m_RunningBufferMaximumDistance(runningBufferMaximumDistance) + , m_NextStateId(0) + { + } + + ScanManager(){} + + /** + * Destructor + */ + virtual ~ScanManager() + { + Clear(); + } + + public: + /** + * Adds scan to vector of processed scans tagging scan with given unique id + * @param pScan + */ + inline void AddScan(LocalizedRangeScan* pScan, kt_int32s uniqueId) + { + // assign state id to scan + pScan->SetStateId(m_NextStateId); + + // assign unique id to scan + pScan->SetUniqueId(uniqueId); + + // add it to scan buffer + m_Scans.insert({pScan->GetStateId(), pScan}); + m_NextStateId++; + } + + /** + * Gets last scan + * @param deviceId + * @return last localized range scan + */ + inline LocalizedRangeScan* GetLastScan() + { + return m_pLastScan; + } + + /** + * Clears last scan + * @param deviceId + */ + inline void ClearLastScan() + { + m_pLastScan = NULL; + } + + /** + * Sets the last scan + * @param pScan + */ + void SetLastScan(LocalizedRangeScan* pScan) + { + m_pLastScan = pScan; + } + + /** + * Gets scans + * @return scans + */ + inline LocalizedRangeScanMap& GetScans() + { + return m_Scans; + } + + /** + * Gets running scans + * @return running scans + */ + inline LocalizedRangeScanVector& GetRunningScans() + { + return m_RunningScans; + } + + /** + * Gets running scan buffer size + * @return running scan buffer size + */ + inline kt_int32u& GetRunningScanBufferSize() + { + return m_RunningBufferMaximumSize; + } + + /** + * Sets running scan buffer size + * @param rScanBufferSize + */ + void SetRunningScanBufferSize(const kt_int32u & rScanBufferSize) + { + m_RunningBufferMaximumSize = rScanBufferSize; + } + + /** + * Sets running scan buffer maximum distance + * @param rScanBufferMaxDistance + */ + void SetRunningScanBufferMaximumDistance(const kt_int32u & rScanBufferMaxDistance) + { + m_RunningBufferMaximumDistance = rScanBufferMaxDistance; + } + + /** + * Adds scan to vector of running scans + * @param pScan + */ + void AddRunningScan(LocalizedRangeScan* pScan) + { + m_RunningScans.push_back(pScan); + + // vector has at least one element (first line of this function), so this is valid + Pose2 frontScanPose = m_RunningScans.front()->GetSensorPose(); + Pose2 backScanPose = m_RunningScans.back()->GetSensorPose(); + + // cap vector size and remove all scans from front of vector that are too far from end of vector + kt_double squaredDistance = frontScanPose.GetPosition().SquaredDistance(backScanPose.GetPosition()); + while (m_RunningScans.size() > m_RunningBufferMaximumSize || + squaredDistance > math::Square(m_RunningBufferMaximumDistance) - KT_TOLERANCE) + { + // remove front of running scans + m_RunningScans.erase(m_RunningScans.begin()); + + // recompute stats of running scans + frontScanPose = m_RunningScans.front()->GetSensorPose(); + backScanPose = m_RunningScans.back()->GetSensorPose(); + squaredDistance = frontScanPose.GetPosition().SquaredDistance(backScanPose.GetPosition()); + } + } + + /** + * Finds and replaces a scan from m_scans with NULL + * @param pScan + */ + void RemoveScan(LocalizedRangeScan* pScan) + { + LocalizedRangeScanMap::iterator it = m_Scans.find(pScan->GetStateId()); + if (it != m_Scans.end()) + { + it->second = NULL; + m_Scans.erase(it); + } + else + { + std::cout << "Remove Scan: Failed to find scan in m_Scans" << std::endl; + } + } + + /** + * Clears the vector of running scans + */ + void ClearRunningScans() + { + m_RunningScans.clear(); + } + + /** + * Deletes data of this buffered device + */ + void Clear() + { + m_Scans.clear(); + m_RunningScans.clear(); + } + + private: + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + ar & BOOST_SERIALIZATION_NVP(m_Scans); + ar & BOOST_SERIALIZATION_NVP(m_RunningScans); + ar & BOOST_SERIALIZATION_NVP(m_pLastScan); + ar & BOOST_SERIALIZATION_NVP(m_RunningBufferMaximumSize); + ar & BOOST_SERIALIZATION_NVP(m_RunningBufferMaximumDistance); + ar & BOOST_SERIALIZATION_NVP(m_NextStateId); + } + + private: + LocalizedRangeScanMap m_Scans; + LocalizedRangeScanVector m_RunningScans; + LocalizedRangeScan* m_pLastScan; + kt_int32u m_NextStateId; + + kt_int32u m_RunningBufferMaximumSize; + kt_double m_RunningBufferMaximumDistance; + }; // ScanManager + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + void MapperSensorManager::RegisterSensor(const Name& rSensorName) + { + if (GetScanManager(rSensorName) == NULL) + { + m_ScanManagers[rSensorName] = new ScanManager(m_RunningBufferMaximumSize, m_RunningBufferMaximumDistance); + } + } + + + /** + * Gets scan from given device with given ID + * @param rSensorName + * @param scanNum + * @return localized range scan + */ + LocalizedRangeScan* MapperSensorManager::GetScan(const Name& rSensorName, kt_int32s scanIndex) + { + ScanManager* pScanManager = GetScanManager(rSensorName); + if (pScanManager != NULL) + { + LocalizedRangeScanMap::iterator it = pScanManager->GetScans().find(scanIndex); + if (it != pScanManager->GetScans().end()) + { + return it->second; + } + else + { + return nullptr; + } + } + + assert(false); + return NULL; + } + + /** + * Gets last scan of given device + * @param pLaserRangeFinder + * @return last localized range scan of device + */ + inline LocalizedRangeScan* MapperSensorManager::GetLastScan(const Name& rSensorName) + { + RegisterSensor(rSensorName); + + return GetScanManager(rSensorName)->GetLastScan(); + } + + /** + * Sets the last scan of device of given scan + * @param pScan + */ + void MapperSensorManager::SetLastScan(LocalizedRangeScan* pScan) + { + GetScanManager(pScan)->SetLastScan(pScan); + } + + /** + * Clears the last scan of device of given scan + * @param pScan + */ + void MapperSensorManager::ClearLastScan(LocalizedRangeScan* pScan) + { + GetScanManager(pScan)->ClearLastScan(); + } + + /** + * Clears the last scan of device name + * @param pScan + */ + void MapperSensorManager::ClearLastScan(const Name& name) + { + GetScanManager(name)->ClearLastScan(); + } + + /** + * Adds scan to scan vector of device that recorded scan + * @param pScan + */ + void MapperSensorManager::AddScan(LocalizedRangeScan* pScan) + { + GetScanManager(pScan)->AddScan(pScan, m_NextScanId); + m_Scans.insert({m_NextScanId, pScan}); + m_NextScanId++; + } + + /** + * Adds scan to running scans of device that recorded scan + * @param pScan + */ + inline void MapperSensorManager::AddRunningScan(LocalizedRangeScan* pScan) + { + GetScanManager(pScan)->AddRunningScan(pScan); + } + + /** + * Finds and replaces a scan from m_Scans with NULL + * @param pScan + */ + void MapperSensorManager::RemoveScan(LocalizedRangeScan* pScan) + { + GetScanManager(pScan)->RemoveScan(pScan); + + LocalizedRangeScanMap::iterator it = m_Scans.find(pScan->GetUniqueId()); + if (it != m_Scans.end()) + { + it->second = NULL; + m_Scans.erase(it); + } + else + { + std::cout << "RemoveScan: Failed to find scan in m_Scans" << std::endl; + } + } + + /** + * Gets scans of device + * @param rSensorName + * @return scans of device + */ + inline LocalizedRangeScanMap& MapperSensorManager::GetScans(const Name& rSensorName) + { + return GetScanManager(rSensorName)->GetScans(); + } + + /** + * Gets running scans of device + * @param rSensorName + * @return running scans of device + */ + inline LocalizedRangeScanVector& MapperSensorManager::GetRunningScans(const Name& rSensorName) + { + return GetScanManager(rSensorName)->GetRunningScans(); + } + + void MapperSensorManager::ClearRunningScans(const Name& rSensorName) + { + GetScanManager(rSensorName)->ClearRunningScans(); + return; + } + + inline kt_int32u MapperSensorManager::GetRunningScanBufferSize(const Name& rSensorName) + { + return GetScanManager(rSensorName)->GetRunningScanBufferSize(); + } + + void MapperSensorManager::SetRunningScanBufferSize(kt_int32u rScanBufferSize) + { + m_RunningBufferMaximumSize = rScanBufferSize; + + std::vector names = GetSensorNames(); + for (uint i = 0; i != names.size(); i++) { + GetScanManager(names[i])->SetRunningScanBufferSize(rScanBufferSize); + } + } + + void MapperSensorManager::SetRunningScanBufferMaximumDistance(kt_double rScanBufferMaxDistance) + { + m_RunningBufferMaximumDistance = rScanBufferMaxDistance; + + std::vector names = GetSensorNames(); + for (uint i = 0; i != names.size(); i++) { + GetScanManager(names[i])->SetRunningScanBufferMaximumDistance(rScanBufferMaxDistance); + } + } + + /** + * Gets all scans of all devices + * @return all scans of all devices + */ + LocalizedRangeScanVector MapperSensorManager::GetAllScans() + { + LocalizedRangeScanVector scans; + + forEach(ScanManagerMap, &m_ScanManagers) + { + LocalizedRangeScanMap& rScans = iter->second->GetScans(); + + LocalizedRangeScanMap::iterator it; + for (it = rScans.begin(); it != rScans.end(); ++it) + { + scans.push_back(it->second); + } + } + + return scans; + } + + /** + * Deletes all scan managers of all devices + */ + void MapperSensorManager::Clear() + { +// SensorManager::Clear(); + + forEach(ScanManagerMap, &m_ScanManagers) + { + delete iter->second; + iter->second = nullptr; + } + + m_ScanManagers.clear(); + } + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + ScanMatcher::~ScanMatcher() + { + if (m_pCorrelationGrid) + { + delete m_pCorrelationGrid; + } + if (m_pSearchSpaceProbs) + { + delete m_pSearchSpaceProbs; + } + if (m_pGridLookup) + { + delete m_pGridLookup; + } + + } + + ScanMatcher* ScanMatcher::Create(Mapper* pMapper, kt_double searchSize, kt_double resolution, + kt_double smearDeviation, kt_double rangeThreshold) + { + // invalid parameters + if (resolution <= 0) + { + return NULL; + } + if (searchSize <= 0) + { + return NULL; + } + if (smearDeviation < 0) + { + return NULL; + } + if (rangeThreshold <= 0) + { + return NULL; + } + + assert(math::DoubleEqual(math::Round(searchSize / resolution), (searchSize / resolution))); + + // calculate search space in grid coordinates + kt_int32u searchSpaceSideSize = static_cast(math::Round(searchSize / resolution) + 1); + + // compute requisite size of correlation grid (pad grid so that scan points can't fall off the grid + // if a scan is on the border of the search space) + kt_int32u pointReadingMargin = static_cast(ceil(rangeThreshold / resolution)); + + kt_int32s gridSize = searchSpaceSideSize + 2 * pointReadingMargin; + + // create correlation grid + assert(gridSize % 2 == 1); + CorrelationGrid* pCorrelationGrid = CorrelationGrid::CreateGrid(gridSize, gridSize, resolution, smearDeviation); + + // create search space probabilities + Grid* pSearchSpaceProbs = Grid::CreateGrid(searchSpaceSideSize, + searchSpaceSideSize, resolution); + + ScanMatcher* pScanMatcher = new ScanMatcher(pMapper); + pScanMatcher->m_pCorrelationGrid = pCorrelationGrid; + pScanMatcher->m_pSearchSpaceProbs = pSearchSpaceProbs; + pScanMatcher->m_pGridLookup = new GridIndexLookup(pCorrelationGrid); + + return pScanMatcher; + } + + /** + * Match given scan against set of scans + * @param pScan scan being scan-matched + * @param rBaseScans set of scans whose points will mark cells in grid as being occupied + * @param rMean output parameter of mean (best pose) of match + * @param rCovariance output parameter of covariance of match + * @param doPenalize whether to penalize matches further from the search center + * @param doRefineMatch whether to do finer-grained matching if coarse match is good (default is true) + * @return strength of response + */ + template + kt_double ScanMatcher::MatchScan(LocalizedRangeScan* pScan, const T& rBaseScans, Pose2& rMean, + Matrix3& rCovariance, kt_bool doPenalize, kt_bool doRefineMatch) + { + /////////////////////////////////////// + // set scan pose to be center of grid + + // 1. get scan position + Pose2 scanPose = pScan->GetSensorPose(); + + // scan has no readings; cannot do scan matching + // best guess of pose is based off of adjusted odometer reading + if (pScan->GetNumberOfRangeReadings() == 0) + { + rMean = scanPose; + + // maximum covariance + rCovariance(0, 0) = MAX_VARIANCE; // XX + rCovariance(1, 1) = MAX_VARIANCE; // YY + rCovariance(2, 2) = 4 * math::Square(m_pMapper->m_pCoarseAngleResolution->GetValue()); // TH*TH + + return 0.0; + } + + // 2. get size of grid + Rectangle2 roi = m_pCorrelationGrid->GetROI(); + + // 3. compute offset (in meters - lower left corner) + Vector2 offset; + offset.SetX(scanPose.GetX() - (0.5 * (roi.GetWidth() - 1) * m_pCorrelationGrid->GetResolution())); + offset.SetY(scanPose.GetY() - (0.5 * (roi.GetHeight() - 1) * m_pCorrelationGrid->GetResolution())); + + // 4. set offset + m_pCorrelationGrid->GetCoordinateConverter()->SetOffset(offset); + + /////////////////////////////////////// + + // set up correlation grid + AddScans(rBaseScans, scanPose.GetPosition()); + + // compute how far to search in each direction + Vector2 searchDimensions(m_pSearchSpaceProbs->GetWidth(), m_pSearchSpaceProbs->GetHeight()); + Vector2 coarseSearchOffset(0.5 * (searchDimensions.GetX() - 1) * m_pCorrelationGrid->GetResolution(), + 0.5 * (searchDimensions.GetY() - 1) * m_pCorrelationGrid->GetResolution()); + + // a coarse search only checks half the cells in each dimension + Vector2 coarseSearchResolution(2 * m_pCorrelationGrid->GetResolution(), + 2 * m_pCorrelationGrid->GetResolution()); + + // actual scan-matching + kt_double bestResponse = CorrelateScan(pScan, scanPose, coarseSearchOffset, coarseSearchResolution, + m_pMapper->m_pCoarseSearchAngleOffset->GetValue(), + m_pMapper->m_pCoarseAngleResolution->GetValue(), + doPenalize, rMean, rCovariance, false); + + if (m_pMapper->m_pUseResponseExpansion->GetValue() == true) + { + if (math::DoubleEqual(bestResponse, 0.0)) + { +#ifdef KARTO_DEBUG + std::cout << "Mapper Info: Expanding response search space!" << std::endl; +#endif + // try and increase search angle offset with 20 degrees and do another match + kt_double newSearchAngleOffset = m_pMapper->m_pCoarseSearchAngleOffset->GetValue(); + for (kt_int32u i = 0; i < 3; i++) + { + newSearchAngleOffset += math::DegreesToRadians(20); + + bestResponse = CorrelateScan(pScan, scanPose, coarseSearchOffset, coarseSearchResolution, + newSearchAngleOffset, m_pMapper->m_pCoarseAngleResolution->GetValue(), + doPenalize, rMean, rCovariance, false); + + if (math::DoubleEqual(bestResponse, 0.0) == false) + { + break; + } + } + +#ifdef KARTO_DEBUG + if (math::DoubleEqual(bestResponse, 0.0)) + { + std::cout << "Mapper Warning: Unable to calculate response!" << std::endl; + } +#endif + } + } + + if (doRefineMatch) + { + Vector2 fineSearchOffset(coarseSearchResolution * 0.5); + Vector2 fineSearchResolution(m_pCorrelationGrid->GetResolution(), m_pCorrelationGrid->GetResolution()); + bestResponse = CorrelateScan(pScan, rMean, fineSearchOffset, fineSearchResolution, + 0.5 * m_pMapper->m_pCoarseAngleResolution->GetValue(), + m_pMapper->m_pFineSearchAngleOffset->GetValue(), + doPenalize, rMean, rCovariance, true); + } + +#ifdef KARTO_DEBUG + std::cout << " BEST POSE = " << rMean << " BEST RESPONSE = " << bestResponse << ", VARIANCE = " + << rCovariance(0, 0) << ", " << rCovariance(1, 1) << std::endl; +#endif + assert(math::InRange(rMean.GetHeading(), -KT_PI, KT_PI)); + + return bestResponse; + } + + void ScanMatcher::operator() (const kt_double& y) const + { + kt_int32u poseResponseCounter; + kt_int32u x_pose; + kt_int32u y_pose = std::find(m_yPoses.begin(), m_yPoses.end(), y) - m_yPoses.begin(); + + const kt_int32u size_x = m_xPoses.size(); + + kt_double newPositionY = m_rSearchCenter.GetY() + y; + kt_double squareY = math::Square(y); + + for ( std::vector::const_iterator xIter = m_xPoses.begin(); xIter != m_xPoses.end(); ++xIter ) + { + x_pose = std::distance(m_xPoses.begin(), xIter); + kt_double x = *xIter; + kt_double newPositionX = m_rSearchCenter.GetX() + x; + kt_double squareX = math::Square(x); + + Vector2 gridPoint = m_pCorrelationGrid->WorldToGrid(Vector2(newPositionX, newPositionY)); + kt_int32s gridIndex = m_pCorrelationGrid->GridIndex(gridPoint); + assert(gridIndex >= 0); + + kt_double angle = 0.0; + kt_double startAngle = m_rSearchCenter.GetHeading() - m_searchAngleOffset; + for (kt_int32u angleIndex = 0; angleIndex < m_nAngles; angleIndex++) + { + angle = startAngle + angleIndex * m_searchAngleResolution; + + kt_double response = GetResponse(angleIndex, gridIndex); + if (m_doPenalize && (math::DoubleEqual(response, 0.0) == false)) + { + // simple model (approximate Gaussian) to take odometry into account + kt_double squaredDistance = squareX + squareY; + kt_double distancePenalty = 1.0 - (DISTANCE_PENALTY_GAIN * + squaredDistance / m_pMapper->m_pDistanceVariancePenalty->GetValue()); + distancePenalty = math::Maximum(distancePenalty, m_pMapper->m_pMinimumDistancePenalty->GetValue()); + + kt_double squaredAngleDistance = math::Square(angle - m_rSearchCenter.GetHeading()); + kt_double anglePenalty = 1.0 - (ANGLE_PENALTY_GAIN * + squaredAngleDistance / m_pMapper->m_pAngleVariancePenalty->GetValue()); + anglePenalty = math::Maximum(anglePenalty, m_pMapper->m_pMinimumAnglePenalty->GetValue()); + + response *= (distancePenalty * anglePenalty); + } + + // store response and pose + poseResponseCounter = (y_pose*size_x + x_pose)*(m_nAngles) + angleIndex; + m_pPoseResponse[poseResponseCounter] = std::pair(response, Pose2(newPositionX, newPositionY, + math::NormalizeAngle(angle))); + } + } + return; + } + + /** + * Finds the best pose for the scan centering the search in the correlation grid + * at the given pose and search in the space by the vector and angular offsets + * in increments of the given resolutions + * @param rScan scan to match against correlation grid + * @param rSearchCenter the center of the search space + * @param rSearchSpaceOffset searches poses in the area offset by this vector around search center + * @param rSearchSpaceResolution how fine a granularity to search in the search space + * @param searchAngleOffset searches poses in the angles offset by this angle around search center + * @param searchAngleResolution how fine a granularity to search in the angular search space + * @param doPenalize whether to penalize matches further from the search center + * @param rMean output parameter of mean (best pose) of match + * @param rCovariance output parameter of covariance of match + * @param doingFineMatch whether to do a finer search after coarse search + * @return strength of response + */ + kt_double ScanMatcher::CorrelateScan(LocalizedRangeScan* pScan, const Pose2& rSearchCenter, + const Vector2& rSearchSpaceOffset, + const Vector2& rSearchSpaceResolution, + kt_double searchAngleOffset, kt_double searchAngleResolution, + kt_bool doPenalize, Pose2& rMean, Matrix3& rCovariance, kt_bool doingFineMatch) + { + assert(searchAngleResolution != 0.0); + + // setup lookup arrays + m_pGridLookup->ComputeOffsets(pScan, rSearchCenter.GetHeading(), searchAngleOffset, searchAngleResolution); + + // only initialize probability grid if computing positional covariance (during coarse match) + if (!doingFineMatch) + { + m_pSearchSpaceProbs->Clear(); + + // position search grid - finds lower left corner of search grid + Vector2 offset(rSearchCenter.GetPosition() - rSearchSpaceOffset); + m_pSearchSpaceProbs->GetCoordinateConverter()->SetOffset(offset); + } + + // calculate position arrays + + m_xPoses.clear(); + kt_int32u nX = static_cast(math::Round(rSearchSpaceOffset.GetX() * + 2.0 / rSearchSpaceResolution.GetX()) + 1); + kt_double startX = -rSearchSpaceOffset.GetX(); + for (kt_int32u xIndex = 0; xIndex < nX; xIndex++) + { + m_xPoses.push_back(startX + xIndex * rSearchSpaceResolution.GetX()); + } + assert(math::DoubleEqual(m_xPoses.back(), -startX)); + + m_yPoses.clear(); + kt_int32u nY = static_cast(math::Round(rSearchSpaceOffset.GetY() * + 2.0 / rSearchSpaceResolution.GetY()) + 1); + kt_double startY = -rSearchSpaceOffset.GetY(); + for (kt_int32u yIndex = 0; yIndex < nY; yIndex++) + { + m_yPoses.push_back(startY + yIndex * rSearchSpaceResolution.GetY()); + } + assert(math::DoubleEqual(m_yPoses.back(), -startY)); + + // calculate pose response array size + kt_int32u nAngles = static_cast(math::Round(searchAngleOffset * 2.0 / searchAngleResolution) + 1); + + kt_int32u poseResponseSize = static_cast(m_xPoses.size() * m_yPoses.size() * nAngles); + + // allocate array + m_pPoseResponse = new std::pair[poseResponseSize]; + + Vector2 startGridPoint = m_pCorrelationGrid->WorldToGrid(Vector2(rSearchCenter.GetX() + + startX, rSearchCenter.GetY() + startY)); + + // this isn't good but its the fastest way to iterate. Should clean up later. + m_rSearchCenter = rSearchCenter; + m_searchAngleOffset = searchAngleOffset; + m_nAngles = nAngles; + m_searchAngleResolution = searchAngleResolution; + m_doPenalize = doPenalize; + tbb::parallel_do(m_yPoses, (*this) ); + + // find value of best response (in [0; 1]) + kt_double bestResponse = -1; + for (kt_int32u i = 0; i < poseResponseSize; i++) + { + bestResponse = math::Maximum(bestResponse, m_pPoseResponse[i].first); + + // will compute positional covariance, save best relative probability for each cell + if (!doingFineMatch) + { + const Pose2& rPose = m_pPoseResponse[i].second; + Vector2 grid = m_pSearchSpaceProbs->WorldToGrid(rPose.GetPosition()); + kt_double* ptr; + + try + { + ptr = (kt_double*)(m_pSearchSpaceProbs->GetDataPointer(grid)); + } + catch(...) + { + throw std::runtime_error("Mapper FATAL ERROR - unable to get pointer in probability search!"); + } + + if (ptr == NULL) + { + throw std::runtime_error("Mapper FATAL ERROR - Index out of range in probability search!"); + } + + *ptr = math::Maximum(m_pPoseResponse[i].first, *ptr); + } + } + + // average all poses with same highest response + Vector2 averagePosition; + kt_double thetaX = 0.0; + kt_double thetaY = 0.0; + kt_int32s averagePoseCount = 0; + for (kt_int32u i = 0; i < poseResponseSize; i++) + { + if (math::DoubleEqual(m_pPoseResponse[i].first, bestResponse)) + { + averagePosition += m_pPoseResponse[i].second.GetPosition(); + + kt_double heading = m_pPoseResponse[i].second.GetHeading(); + thetaX += cos(heading); + thetaY += sin(heading); + + averagePoseCount++; + } + } + + Pose2 averagePose; + if (averagePoseCount > 0) + { + averagePosition /= averagePoseCount; + + thetaX /= averagePoseCount; + thetaY /= averagePoseCount; + + averagePose = Pose2(averagePosition, atan2(thetaY, thetaX)); + } + else + { + throw std::runtime_error("Mapper FATAL ERROR - Unable to find best position"); + } + + // delete pose response array + delete [] m_pPoseResponse; + m_pPoseResponse = nullptr; + +#ifdef KARTO_DEBUG + std::cout << "bestPose: " << averagePose << std::endl; + std::cout << "bestResponse: " << bestResponse << std::endl; +#endif + + if (!doingFineMatch) + { + ComputePositionalCovariance(averagePose, bestResponse, rSearchCenter, rSearchSpaceOffset, + rSearchSpaceResolution, searchAngleResolution, rCovariance); + } + else + { + ComputeAngularCovariance(averagePose, bestResponse, rSearchCenter, + searchAngleOffset, searchAngleResolution, rCovariance); + } + + rMean = averagePose; + +#ifdef KARTO_DEBUG + std::cout << "bestPose: " << averagePose << std::endl; +#endif + + if (bestResponse > 1.0) + { + bestResponse = 1.0; + } + + assert(math::InRange(bestResponse, 0.0, 1.0)); + assert(math::InRange(rMean.GetHeading(), -KT_PI, KT_PI)); + + return bestResponse; + } + + /** + * Computes the positional covariance of the best pose + * @param rBestPose + * @param bestResponse + * @param rSearchCenter + * @param rSearchSpaceOffset + * @param rSearchSpaceResolution + * @param searchAngleResolution + * @param rCovariance + */ + void ScanMatcher::ComputePositionalCovariance(const Pose2& rBestPose, kt_double bestResponse, + const Pose2& rSearchCenter, + const Vector2& rSearchSpaceOffset, + const Vector2& rSearchSpaceResolution, + kt_double searchAngleResolution, Matrix3& rCovariance) + { + // reset covariance to identity matrix + rCovariance.SetToIdentity(); + + // if best response is vary small return max variance + if (bestResponse < KT_TOLERANCE) + { + rCovariance(0, 0) = MAX_VARIANCE; // XX + rCovariance(1, 1) = MAX_VARIANCE; // YY + rCovariance(2, 2) = 4 * math::Square(searchAngleResolution); // TH*TH + + return; + } + + kt_double accumulatedVarianceXX = 0; + kt_double accumulatedVarianceXY = 0; + kt_double accumulatedVarianceYY = 0; + kt_double norm = 0; + + kt_double dx = rBestPose.GetX() - rSearchCenter.GetX(); + kt_double dy = rBestPose.GetY() - rSearchCenter.GetY(); + + kt_double offsetX = rSearchSpaceOffset.GetX(); + kt_double offsetY = rSearchSpaceOffset.GetY(); + + kt_int32u nX = static_cast(math::Round(offsetX * 2.0 / rSearchSpaceResolution.GetX()) + 1); + kt_double startX = -offsetX; + assert(math::DoubleEqual(startX + (nX - 1) * rSearchSpaceResolution.GetX(), -startX)); + + kt_int32u nY = static_cast(math::Round(offsetY * 2.0 / rSearchSpaceResolution.GetY()) + 1); + kt_double startY = -offsetY; + assert(math::DoubleEqual(startY + (nY - 1) * rSearchSpaceResolution.GetY(), -startY)); + + for (kt_int32u yIndex = 0; yIndex < nY; yIndex++) + { + kt_double y = startY + yIndex * rSearchSpaceResolution.GetY(); + + for (kt_int32u xIndex = 0; xIndex < nX; xIndex++) + { + kt_double x = startX + xIndex * rSearchSpaceResolution.GetX(); + + Vector2 gridPoint = m_pSearchSpaceProbs->WorldToGrid(Vector2(rSearchCenter.GetX() + x, + rSearchCenter.GetY() + y)); + kt_double response = *(m_pSearchSpaceProbs->GetDataPointer(gridPoint)); + + // response is not a low response + if (response >= (bestResponse - 0.1)) + { + norm += response; + accumulatedVarianceXX += (math::Square(x - dx) * response); + accumulatedVarianceXY += ((x - dx) * (y - dy) * response); + accumulatedVarianceYY += (math::Square(y - dy) * response); + } + } + } + + if (norm > KT_TOLERANCE) + { + kt_double varianceXX = accumulatedVarianceXX / norm; + kt_double varianceXY = accumulatedVarianceXY / norm; + kt_double varianceYY = accumulatedVarianceYY / norm; + kt_double varianceTHTH = 4 * math::Square(searchAngleResolution); + + // lower-bound variances so that they are not too small; + // ensures that links are not too tight + kt_double minVarianceXX = 0.1 * math::Square(rSearchSpaceResolution.GetX()); + kt_double minVarianceYY = 0.1 * math::Square(rSearchSpaceResolution.GetY()); + varianceXX = math::Maximum(varianceXX, minVarianceXX); + varianceYY = math::Maximum(varianceYY, minVarianceYY); + + // increase variance for poorer responses + kt_double multiplier = 1.0 / bestResponse; + rCovariance(0, 0) = varianceXX * multiplier; + rCovariance(0, 1) = varianceXY * multiplier; + rCovariance(1, 0) = varianceXY * multiplier; + rCovariance(1, 1) = varianceYY * multiplier; + rCovariance(2, 2) = varianceTHTH; // this value will be set in ComputeAngularCovariance + } + + // if values are 0, set to MAX_VARIANCE + // values might be 0 if points are too sparse and thus don't hit other points + if (math::DoubleEqual(rCovariance(0, 0), 0.0)) + { + rCovariance(0, 0) = MAX_VARIANCE; + } + + if (math::DoubleEqual(rCovariance(1, 1), 0.0)) + { + rCovariance(1, 1) = MAX_VARIANCE; + } + } + + /** + * Computes the angular covariance of the best pose + * @param rBestPose + * @param bestResponse + * @param rSearchCenter + * @param rSearchAngleOffset + * @param searchAngleResolution + * @param rCovariance + */ + void ScanMatcher::ComputeAngularCovariance(const Pose2& rBestPose, + kt_double bestResponse, + const Pose2& rSearchCenter, + kt_double searchAngleOffset, + kt_double searchAngleResolution, + Matrix3& rCovariance) + { + // NOTE: do not reset covariance matrix + + // normalize angle difference + kt_double bestAngle = math::NormalizeAngleDifference(rBestPose.GetHeading(), rSearchCenter.GetHeading()); + + Vector2 gridPoint = m_pCorrelationGrid->WorldToGrid(rBestPose.GetPosition()); + kt_int32s gridIndex = m_pCorrelationGrid->GridIndex(gridPoint); + + kt_int32u nAngles = static_cast(math::Round(searchAngleOffset * 2 / searchAngleResolution) + 1); + + kt_double angle = 0.0; + kt_double startAngle = rSearchCenter.GetHeading() - searchAngleOffset; + + kt_double norm = 0.0; + kt_double accumulatedVarianceThTh = 0.0; + for (kt_int32u angleIndex = 0; angleIndex < nAngles; angleIndex++) + { + angle = startAngle + angleIndex * searchAngleResolution; + kt_double response = GetResponse(angleIndex, gridIndex); + + // response is not a low response + if (response >= (bestResponse - 0.1)) + { + norm += response; + accumulatedVarianceThTh += (math::Square(angle - bestAngle) * response); + } + } + assert(math::DoubleEqual(angle, rSearchCenter.GetHeading() + searchAngleOffset)); + + if (norm > KT_TOLERANCE) + { + if (accumulatedVarianceThTh < KT_TOLERANCE) + { + accumulatedVarianceThTh = math::Square(searchAngleResolution); + } + + accumulatedVarianceThTh /= norm; + } + else + { + accumulatedVarianceThTh = 1000 * math::Square(searchAngleResolution); + } + + rCovariance(2, 2) = accumulatedVarianceThTh; + } + + /** + * Marks cells where scans' points hit as being occupied + * @param rScans scans whose points will mark cells in grid as being occupied + * @param viewPoint do not add points that belong to scans "opposite" the view point + */ + void ScanMatcher::AddScans(const LocalizedRangeScanVector& rScans, Vector2 viewPoint) + { + m_pCorrelationGrid->Clear(); + + // add all scans to grid + const_forEach(LocalizedRangeScanVector, &rScans) + { + if (*iter == NULL) + { + continue; + } + + AddScan(*iter, viewPoint); + } + } + + /** + * Marks cells where scans' points hit as being occupied + * @param rScans scans whose points will mark cells in grid as being occupied + * @param viewPoint do not add points that belong to scans "opposite" the view point + */ + void ScanMatcher::AddScans(const LocalizedRangeScanMap& rScans, Vector2 viewPoint) + { + m_pCorrelationGrid->Clear(); + + // add all scans to grid + const_forEach(LocalizedRangeScanMap, &rScans) + { + if (iter->second == NULL) + { + continue; + } + + AddScan(iter->second, viewPoint); + } + } + + /** + * Marks cells where scans' points hit as being occupied. Can smear points as they are added. + * @param pScan scan whose points will mark cells in grid as being occupied + * @param viewPoint do not add points that belong to scans "opposite" the view point + * @param doSmear whether the points will be smeared + */ + void ScanMatcher::AddScan(LocalizedRangeScan* pScan, const Vector2& rViewPoint, kt_bool doSmear) + { + PointVectorDouble validPoints = FindValidPoints(pScan, rViewPoint); + + // put in all valid points + const_forEach(PointVectorDouble, &validPoints) + { + Vector2 gridPoint = m_pCorrelationGrid->WorldToGrid(*iter); + if (!math::IsUpTo(gridPoint.GetX(), m_pCorrelationGrid->GetROI().GetWidth()) || + !math::IsUpTo(gridPoint.GetY(), m_pCorrelationGrid->GetROI().GetHeight())) + { + // point not in grid + continue; + } + + int gridIndex = m_pCorrelationGrid->GridIndex(gridPoint); + + // set grid cell as occupied + if (m_pCorrelationGrid->GetDataPointer()[gridIndex] == GridStates_Occupied) + { + // value already set + continue; + } + + m_pCorrelationGrid->GetDataPointer()[gridIndex] = GridStates_Occupied; + + // smear grid + if (doSmear == true) + { + m_pCorrelationGrid->SmearPoint(gridPoint); + } + } + } + + /** + * Compute which points in a scan are on the same side as the given viewpoint + * @param pScan + * @param rViewPoint + * @return points on the same side + */ + PointVectorDouble ScanMatcher::FindValidPoints(LocalizedRangeScan* pScan, const Vector2& rViewPoint) const + { + const PointVectorDouble& rPointReadings = pScan->GetPointReadings(); + + // points must be at least 10 cm away when making comparisons of inside/outside of viewpoint + const kt_double minSquareDistance = math::Square(0.1); // in m^2 + + // this iterator lags from the main iterator adding points only when the points are on + // the same side as the viewpoint + PointVectorDouble::const_iterator trailingPointIter = rPointReadings.begin(); + PointVectorDouble validPoints; + + Vector2 firstPoint; + kt_bool firstTime = true; + const_forEach(PointVectorDouble, &rPointReadings) + { + Vector2 currentPoint = *iter; + + if (firstTime && !std::isnan(currentPoint.GetX()) && !std::isnan(currentPoint.GetY())) + { + firstPoint = currentPoint; + firstTime = false; + } + + Vector2 delta = firstPoint - currentPoint; + if (delta.SquaredLength() > minSquareDistance) + { + // This compute the Determinant (viewPoint FirstPoint, viewPoint currentPoint) + // Which computes the direction of rotation, if the rotation is counterclock + // wise then we are looking at data we should keep. If it's negative rotation + // we should not included in in the matching + // have enough distance, check viewpoint + double a = rViewPoint.GetY() - firstPoint.GetY(); + double b = firstPoint.GetX() - rViewPoint.GetX(); + double c = firstPoint.GetY() * rViewPoint.GetX() - firstPoint.GetX() * rViewPoint.GetY(); + double ss = currentPoint.GetX() * a + currentPoint.GetY() * b + c; + + // reset beginning point + firstPoint = currentPoint; + + if (ss < 0.0) // wrong side, skip and keep going + { + trailingPointIter = iter; + } + else + { + for (; trailingPointIter != iter; ++trailingPointIter) + { + validPoints.push_back(*trailingPointIter); + } + } + } + } + + return validPoints; + } + + /** + * Get response at given position for given rotation (only look up valid points) + * @param angleIndex + * @param gridPositionIndex + * @return response + */ + kt_double ScanMatcher::GetResponse(kt_int32u angleIndex, kt_int32s gridPositionIndex) const + { + kt_double response = 0.0; + + // add up value for each point + kt_int8u* pByte = m_pCorrelationGrid->GetDataPointer() + gridPositionIndex; + + const LookupArray* pOffsets = m_pGridLookup->GetLookupArray(angleIndex); + assert(pOffsets != NULL); + + // get number of points in offset list + kt_int32u nPoints = pOffsets->GetSize(); + if (nPoints == 0) + { + return response; + } + + // calculate response + kt_int32s* pAngleIndexPointer = pOffsets->GetArrayPointer(); + for (kt_int32u i = 0; i < nPoints; i++) + { + // ignore points that fall off the grid + kt_int32s pointGridIndex = gridPositionIndex + pAngleIndexPointer[i]; + if (!math::IsUpTo(pointGridIndex, m_pCorrelationGrid->GetDataSize()) || pAngleIndexPointer[i] == INVALID_SCAN) + { + continue; + } + + // uses index offsets to efficiently find location of point in the grid + response += pByte[pAngleIndexPointer[i]]; + } + + // normalize response + response /= (nPoints * GridStates_Occupied); + assert(fabs(response) <= 1.0); + + return response; + } + + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + template + class BreadthFirstTraversal : public GraphTraversal + { + public: + /** + * Constructs a breadth-first traverser for the given graph + */ + BreadthFirstTraversal() + { + } + BreadthFirstTraversal(Graph* pGraph) + : GraphTraversal(pGraph) + { + } + + /** + * Destructor + */ + virtual ~BreadthFirstTraversal() + { + } + + public: + /** + * Traverse the graph starting with the given vertex; applies the visitor to visited nodes + * @param pStartVertex + * @param pVisitor + * @return visited vertice scans + */ + virtual std::vector TraverseForScans(Vertex* pStartVertex, Visitor* pVisitor) + { + std::vector*> validVertices = TraverseForVertices(pStartVertex, pVisitor); + + std::vector objects; + forEach(typename std::vector*>, &validVertices) + { + objects.push_back((*iter)->GetObject()); + } + + return objects; + } + + /** + * Traverse the graph starting with the given vertex; applies the visitor to visited nodes + * @param pStartVertex + * @param pVisitor + * @return visited vertices + */ + virtual std::vector*> TraverseForVertices(Vertex* pStartVertex, Visitor* pVisitor) + { + std::queue*> toVisit; + std::set*> seenVertices; + std::vector*> validVertices; + + toVisit.push(pStartVertex); + seenVertices.insert(pStartVertex); + + do + { + Vertex* pNext = toVisit.front(); + toVisit.pop(); + + if (pNext != NULL && pVisitor->Visit(pNext)) + { + // vertex is valid, explore neighbors + validVertices.push_back(pNext); + + std::vector*> adjacentVertices = pNext->GetAdjacentVertices(); + forEach(typename std::vector*>, &adjacentVertices) + { + Vertex* pAdjacent = *iter; + + // adjacent vertex has not yet been seen, add to queue for processing + if (seenVertices.find(pAdjacent) == seenVertices.end()) + { + toVisit.push(pAdjacent); + seenVertices.insert(pAdjacent); + } + } + } + } while (toVisit.empty() == false); + + return validVertices; + } + + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(GraphTraversal); + } + }; // class BreadthFirstTraversal + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + class NearScanVisitor : public Visitor + { + public: + NearScanVisitor(LocalizedRangeScan* pScan, kt_double maxDistance, kt_bool useScanBarycenter) + : m_MaxDistanceSquared(math::Square(maxDistance)) + , m_UseScanBarycenter(useScanBarycenter) + { + m_CenterPose = pScan->GetReferencePose(m_UseScanBarycenter); + } + + virtual kt_bool Visit(Vertex* pVertex) + { + try + { + LocalizedRangeScan* pScan = pVertex->GetObject(); + Pose2 pose = pScan->GetReferencePose(m_UseScanBarycenter); + kt_double squaredDistance = pose.GetPosition().SquaredDistance(m_CenterPose.GetPosition()); + return (squaredDistance <= m_MaxDistanceSquared - KT_TOLERANCE); + } + catch(...) + { + // relocalization vertex elements missing + std::cout << "Unable to visit valid vertex elements!" << std::endl; + return false; + } + } + + protected: + Pose2 m_CenterPose; + kt_double m_MaxDistanceSquared; + kt_bool m_UseScanBarycenter; + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Visitor); + ar & BOOST_SERIALIZATION_NVP(m_CenterPose); + ar & BOOST_SERIALIZATION_NVP(m_MaxDistanceSquared); + ar & BOOST_SERIALIZATION_NVP(m_UseScanBarycenter); + } + }; // NearScanVisitor + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + class NearPoseVisitor : public Visitor + { + public: + NearPoseVisitor(Pose2 refPose, kt_double maxDistance, kt_bool useScanBarycenter) + : m_MaxDistanceSquared(math::Square(maxDistance)) + , m_UseScanBarycenter(useScanBarycenter) + { + m_CenterPose = refPose; + } + + virtual kt_bool Visit(Vertex* pVertex) + { + LocalizedRangeScan* pScan = pVertex->GetObject(); + + Pose2 pose = pScan->GetReferencePose(m_UseScanBarycenter); + + kt_double squaredDistance = pose.GetPosition().SquaredDistance(m_CenterPose.GetPosition()); + return (squaredDistance <= m_MaxDistanceSquared - KT_TOLERANCE); + } + + protected: + Pose2 m_CenterPose; + kt_double m_MaxDistanceSquared; + kt_bool m_UseScanBarycenter; + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int version) + { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Visitor); + ar & BOOST_SERIALIZATION_NVP(m_CenterPose); + ar & BOOST_SERIALIZATION_NVP(m_MaxDistanceSquared); + ar & BOOST_SERIALIZATION_NVP(m_UseScanBarycenter); + } + }; // NearPoseVisitor + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + + MapperGraph::MapperGraph(Mapper* pMapper, kt_double rangeThreshold) + : m_pMapper(pMapper) + { + m_pLoopScanMatcher = ScanMatcher::Create(pMapper, m_pMapper->m_pLoopSearchSpaceDimension->GetValue(), + m_pMapper->m_pLoopSearchSpaceResolution->GetValue(), + m_pMapper->m_pLoopSearchSpaceSmearDeviation->GetValue(), rangeThreshold); + assert(m_pLoopScanMatcher); + + m_pTraversal = new BreadthFirstTraversal(this); + } + + MapperGraph::~MapperGraph() + { + if (m_pLoopScanMatcher) + { + delete m_pLoopScanMatcher; + m_pLoopScanMatcher = NULL; + } + if (m_pTraversal) + { + delete m_pTraversal; + m_pTraversal = NULL; + } + } + + Vertex* MapperGraph::AddVertex(LocalizedRangeScan* pScan) + { + assert(pScan); + + if (pScan != NULL) + { + Vertex* pVertex = new Vertex(pScan); + Graph::AddVertex(pScan->GetSensorName(), pVertex); + if (m_pMapper->m_pScanOptimizer != NULL) + { + m_pMapper->m_pScanOptimizer->AddNode(pVertex); + } + return pVertex; + } + + return nullptr; + } + + void MapperGraph::AddEdges(LocalizedRangeScan* pScan, const Matrix3& rCovariance) + { + MapperSensorManager* pSensorManager = m_pMapper->m_pMapperSensorManager; + + const Name rSensorName = pScan->GetSensorName(); + + // link to previous scan + kt_int32s previousScanNum = pScan->GetStateId() - 1; + if (pSensorManager->GetLastScan(rSensorName) != NULL) + { + assert(previousScanNum >= 0); + LocalizedRangeScan* pPrevScan = pSensorManager->GetScan(rSensorName, previousScanNum); + if (!pPrevScan) + { + return; + } + LinkScans(pPrevScan, pScan, pScan->GetSensorPose(), rCovariance); + } + + Pose2Vector means; + std::vector covariances; + + // first scan (link to first scan of other robots) + if (pSensorManager->GetLastScan(rSensorName) == NULL) + { + assert(pSensorManager->GetScans(rSensorName).size() == 1); + + std::vector deviceNames = pSensorManager->GetSensorNames(); + forEach(std::vector, &deviceNames) + { + const Name& rCandidateSensorName = *iter; + + // skip if candidate device is the same or other device has no scans + if ((rCandidateSensorName == rSensorName) || (pSensorManager->GetScans(rCandidateSensorName).empty())) + { + continue; + } + + Pose2 bestPose; + Matrix3 covariance; + kt_double response = m_pMapper->m_pSequentialScanMatcher->MatchScan(pScan, + pSensorManager->GetScans(rCandidateSensorName), + bestPose, covariance); + LinkScans(pScan, pSensorManager->GetScan(rCandidateSensorName, 0), bestPose, covariance); + + // only add to means and covariances if response was high "enough" + if (response > m_pMapper->m_pLinkMatchMinimumResponseFine->GetValue()) + { + means.push_back(bestPose); + covariances.push_back(covariance); + } + } + } + else + { + // link to running scans + Pose2 scanPose = pScan->GetSensorPose(); + means.push_back(scanPose); + covariances.push_back(rCovariance); + LinkChainToScan(pSensorManager->GetRunningScans(rSensorName), pScan, scanPose, rCovariance); + } + + // link to other near chains (chains that include new scan are invalid) + LinkNearChains(pScan, means, covariances); + + if (!means.empty()) + { + pScan->SetSensorPose(ComputeWeightedMean(means, covariances)); + } + } + + kt_bool MapperGraph::TryCloseLoop(LocalizedRangeScan* pScan, const Name& rSensorName) + { + kt_bool loopClosed = false; + + kt_int32u scanIndex = 0; + + LocalizedRangeScanVector candidateChain = FindPossibleLoopClosure(pScan, rSensorName, scanIndex); + + while (!candidateChain.empty()) + { + Pose2 bestPose; + Matrix3 covariance; + kt_double coarseResponse = m_pLoopScanMatcher->MatchScan(pScan, candidateChain, + bestPose, covariance, false, false); + + std::stringstream stream; + stream << "COARSE RESPONSE: " << coarseResponse + << " (> " << m_pMapper->m_pLoopMatchMinimumResponseCoarse->GetValue() << ")" + << std::endl; + stream << " var: " << covariance(0, 0) << ", " << covariance(1, 1) + << " (< " << m_pMapper->m_pLoopMatchMaximumVarianceCoarse->GetValue() << ")"; + + m_pMapper->FireLoopClosureCheck(stream.str()); + + if ((coarseResponse > m_pMapper->m_pLoopMatchMinimumResponseCoarse->GetValue()) && + (covariance(0, 0) < m_pMapper->m_pLoopMatchMaximumVarianceCoarse->GetValue()) && + (covariance(1, 1) < m_pMapper->m_pLoopMatchMaximumVarianceCoarse->GetValue())) + { + LocalizedRangeScan tmpScan(pScan->GetSensorName(), pScan->GetRangeReadingsVector()); + tmpScan.SetUniqueId(pScan->GetUniqueId()); + tmpScan.SetTime(pScan->GetTime()); + tmpScan.SetStateId(pScan->GetStateId()); + tmpScan.SetCorrectedPose(pScan->GetCorrectedPose()); + tmpScan.SetSensorPose(bestPose); // This also updates OdometricPose. + kt_double fineResponse = m_pMapper->m_pSequentialScanMatcher->MatchScan(&tmpScan, candidateChain, + bestPose, covariance, false); + + std::stringstream stream1; + stream1 << "FINE RESPONSE: " << fineResponse << " (>" + << m_pMapper->m_pLoopMatchMinimumResponseFine->GetValue() << ")" << std::endl; + m_pMapper->FireLoopClosureCheck(stream1.str()); + + if (fineResponse < m_pMapper->m_pLoopMatchMinimumResponseFine->GetValue()) + { + m_pMapper->FireLoopClosureCheck("REJECTED!"); + } + else + { + m_pMapper->FireBeginLoopClosure("Closing loop..."); + + pScan->SetSensorPose(bestPose); + LinkChainToScan(candidateChain, pScan, bestPose, covariance); + CorrectPoses(); + + m_pMapper->FireEndLoopClosure("Loop closed!"); + + loopClosed = true; + } + } + + candidateChain = FindPossibleLoopClosure(pScan, rSensorName, scanIndex); + } + + return loopClosed; + } + + LocalizedRangeScan* MapperGraph::GetClosestScanToPose(const LocalizedRangeScanVector& rScans, + const Pose2& rPose) const + { + LocalizedRangeScan* pClosestScan = NULL; + kt_double bestSquaredDistance = DBL_MAX; + + const_forEach(LocalizedRangeScanVector, &rScans) + { + Pose2 scanPose = (*iter)->GetReferencePose(m_pMapper->m_pUseScanBarycenter->GetValue()); + + kt_double squaredDistance = rPose.GetPosition().SquaredDistance(scanPose.GetPosition()); + if (squaredDistance < bestSquaredDistance) + { + bestSquaredDistance = squaredDistance; + pClosestScan = *iter; + } + } + + return pClosestScan; + } + + Edge* MapperGraph::AddEdge(LocalizedRangeScan* pSourceScan, + LocalizedRangeScan* pTargetScan, kt_bool& rIsNewEdge) + { + std::map* >::iterator v1 = m_Vertices[pSourceScan->GetSensorName()].find(pSourceScan->GetStateId()); + std::map* >::iterator v2 = m_Vertices[pTargetScan->GetSensorName()].find(pTargetScan->GetStateId()); + + if (v1 == m_Vertices[pSourceScan->GetSensorName()].end() || + v2 == m_Vertices[pSourceScan->GetSensorName()].end()) + { + std::cout << "AddEdge: At least one vertex is invalid." << std::endl; + return NULL; + } + + // see if edge already exists + const_forEach(std::vector*>, &(v1->second->GetEdges())) + { + Edge* pEdge = *iter; + + if (pEdge->GetTarget() == v2->second) + { + rIsNewEdge = false; + return pEdge; + } + } + + Edge* pEdge = new Edge(v1->second, v2->second); + Graph::AddEdge(pEdge); + rIsNewEdge = true; + return pEdge; + } + + void MapperGraph::LinkScans(LocalizedRangeScan* pFromScan, LocalizedRangeScan* pToScan, + const Pose2& rMean, const Matrix3& rCovariance) + { + kt_bool isNewEdge = true; + Edge* pEdge = AddEdge(pFromScan, pToScan, isNewEdge); + + if (pEdge == NULL) + { + return; + } + + // only attach link information if the edge is new + if (isNewEdge == true) + { + pEdge->SetLabel(new LinkInfo(pFromScan->GetCorrectedPose(), pToScan->GetCorrectedAt(rMean), rCovariance)); + if (m_pMapper->m_pScanOptimizer != NULL) + { + m_pMapper->m_pScanOptimizer->AddConstraint(pEdge); + } + } + } + + void MapperGraph::LinkNearChains(LocalizedRangeScan* pScan, Pose2Vector& rMeans, std::vector& rCovariances) + { + const std::vector nearChains = FindNearChains(pScan); + const_forEach(std::vector, &nearChains) + { + if (iter->size() < m_pMapper->m_pLoopMatchMinimumChainSize->GetValue()) + { + continue; + } + + Pose2 mean; + Matrix3 covariance; + // match scan against "near" chain + kt_double response = m_pMapper->m_pSequentialScanMatcher->MatchScan(pScan, *iter, mean, covariance, false); + if (response > m_pMapper->m_pLinkMatchMinimumResponseFine->GetValue() - KT_TOLERANCE) + { + rMeans.push_back(mean); + rCovariances.push_back(covariance); + LinkChainToScan(*iter, pScan, mean, covariance); + } + } + } + + void MapperGraph::LinkChainToScan(const LocalizedRangeScanVector& rChain, LocalizedRangeScan* pScan, + const Pose2& rMean, const Matrix3& rCovariance) + { + Pose2 pose = pScan->GetReferencePose(m_pMapper->m_pUseScanBarycenter->GetValue()); + + LocalizedRangeScan* pClosestScan = GetClosestScanToPose(rChain, pose); + assert(pClosestScan != NULL); + + Pose2 closestScanPose = pClosestScan->GetReferencePose(m_pMapper->m_pUseScanBarycenter->GetValue()); + + kt_double squaredDistance = pose.GetPosition().SquaredDistance(closestScanPose.GetPosition()); + if (squaredDistance < math::Square(m_pMapper->m_pLinkScanMaximumDistance->GetValue()) + KT_TOLERANCE) + { + LinkScans(pClosestScan, pScan, rMean, rCovariance); + } + } + + std::vector MapperGraph::FindNearChains(LocalizedRangeScan* pScan) + { + std::vector nearChains; + + Pose2 scanPose = pScan->GetReferencePose(m_pMapper->m_pUseScanBarycenter->GetValue()); + + // to keep track of which scans have been added to a chain + LocalizedRangeScanVector processed; + + const LocalizedRangeScanVector nearLinkedScans = FindNearLinkedScans(pScan, + m_pMapper->m_pLinkScanMaximumDistance->GetValue()); + const_forEach(LocalizedRangeScanVector, &nearLinkedScans) + { + LocalizedRangeScan* pNearScan = *iter; + + if (pNearScan == pScan) + { + continue; + } + + // scan has already been processed, skip + if (find(processed.begin(), processed.end(), pNearScan) != processed.end()) + { + continue; + } + + processed.push_back(pNearScan); + + // build up chain + kt_bool isValidChain = true; + std::list chain; + + // add scans before current scan being processed + for (kt_int32s candidateScanNum = pNearScan->GetStateId() - 1; candidateScanNum >= 0; candidateScanNum--) + { + LocalizedRangeScan* pCandidateScan = m_pMapper->m_pMapperSensorManager->GetScan(pNearScan->GetSensorName(), + candidateScanNum); + + // chain is invalid--contains scan being added + if (pCandidateScan == pScan) + { + isValidChain = false; + } + + // probably removed in localization mode + if (pCandidateScan == NULL) + { + continue; + } + + Pose2 candidatePose = pCandidateScan->GetReferencePose(m_pMapper->m_pUseScanBarycenter->GetValue()); + kt_double squaredDistance = scanPose.GetPosition().SquaredDistance(candidatePose.GetPosition()); + + if (squaredDistance < math::Square(m_pMapper->m_pLinkScanMaximumDistance->GetValue()) + KT_TOLERANCE) + { + chain.push_front(pCandidateScan); + processed.push_back(pCandidateScan); + } + else + { + break; + } + } + + chain.push_back(pNearScan); + + // add scans after current scan being processed + kt_int32u end = static_cast(m_pMapper->m_pMapperSensorManager->GetScans(pNearScan->GetSensorName()).size()); + for (kt_int32u candidateScanNum = pNearScan->GetStateId() + 1; candidateScanNum < end; candidateScanNum++) + { + LocalizedRangeScan* pCandidateScan = m_pMapper->m_pMapperSensorManager->GetScan(pNearScan->GetSensorName(), + candidateScanNum); + + if (pCandidateScan == pScan) + { + isValidChain = false; + } + + // probably removed in localization mode + if (pCandidateScan == NULL) + { + continue; + } + + Pose2 candidatePose = pCandidateScan->GetReferencePose(m_pMapper->m_pUseScanBarycenter->GetValue());; + kt_double squaredDistance = scanPose.GetPosition().SquaredDistance(candidatePose.GetPosition()); + + if (squaredDistance < math::Square(m_pMapper->m_pLinkScanMaximumDistance->GetValue()) + KT_TOLERANCE) + { + chain.push_back(pCandidateScan); + processed.push_back(pCandidateScan); + } + else + { + break; + } + } + + if (isValidChain) + { + // change list to vector + LocalizedRangeScanVector tempChain; + std::copy(chain.begin(), chain.end(), std::inserter(tempChain, tempChain.begin())); + // add chain to collection + nearChains.push_back(tempChain); + } + } + + return nearChains; + } + + LocalizedRangeScanVector MapperGraph::FindNearLinkedScans(LocalizedRangeScan* pScan, kt_double maxDistance) + { + NearScanVisitor* pVisitor = new NearScanVisitor(pScan, maxDistance, m_pMapper->m_pUseScanBarycenter->GetValue()); + LocalizedRangeScanVector nearLinkedScans = m_pTraversal->TraverseForScans(GetVertex(pScan), pVisitor); + delete pVisitor; + + return nearLinkedScans; + } + + std::vector*> MapperGraph::FindNearLinkedVertices(LocalizedRangeScan* pScan, kt_double maxDistance) + { + NearScanVisitor* pVisitor = new NearScanVisitor(pScan, maxDistance, m_pMapper->m_pUseScanBarycenter->GetValue()); + std::vector*> nearLinkedVertices = m_pTraversal->TraverseForVertices(GetVertex(pScan), pVisitor); + delete pVisitor; + + return nearLinkedVertices; + } + + LocalizedRangeScanVector MapperGraph::FindNearByScans(Name name, const Pose2 refPose, kt_double maxDistance) + { + NearPoseVisitor* pVisitor = new NearPoseVisitor(refPose, maxDistance, m_pMapper->m_pUseScanBarycenter->GetValue()); + + Vertex* closestVertex = FindNearByScan(name, refPose); + + LocalizedRangeScanVector nearLinkedScans = m_pTraversal->TraverseForScans(closestVertex, pVisitor); + delete pVisitor; + + return nearLinkedScans; + } + + std::vector*> MapperGraph::FindNearByVertices(Name name, const Pose2 refPose, kt_double maxDistance) + { + VertexMap vertexMap = GetVertices(); + std::map*>& vertices = vertexMap[name]; + + std::vector*> vertices_to_search; + std::map*>::iterator it; + for (it = vertices.begin(); it != vertices.end(); ++it) + { + if (it->second) + { + vertices_to_search.push_back(it->second); + } + } + + const size_t dim = 2; + + typedef VertexVectorPoseNanoFlannAdaptor* > > P2KD; + const P2KD p2kd(vertices_to_search); + + typedef nanoflann::KDTreeSingleIndexAdaptor , P2KD, dim> my_kd_tree_t; + + my_kd_tree_t index(dim, p2kd, nanoflann::KDTreeSingleIndexAdaptorParams(10) ); + index.buildIndex(); + + std::vector > ret_matches; + const double query_pt[2] = {refPose.GetX(), refPose.GetY()}; + nanoflann::SearchParams params; + const size_t num_results = index.radiusSearch(&query_pt[0], maxDistance, ret_matches, params); + + std::vector*> rtn_vertices; + rtn_vertices.reserve(ret_matches.size()); + for (uint i = 0; i != ret_matches.size(); i++) + { + rtn_vertices.push_back(vertices_to_search[ret_matches[i].first]); + } + return rtn_vertices; + } + + Vertex* MapperGraph::FindNearByScan(Name name, const Pose2 refPose) + { + VertexMap vertexMap = GetVertices(); + std::map*>& vertices = vertexMap[name]; + + std::vector*> vertices_to_search; + std::map*>::iterator it; + for (it = vertices.begin(); it != vertices.end(); ++it) + { + if (it->second) + { + vertices_to_search.push_back(it->second); + } + } + + size_t num_results = 1; + const size_t dim = 2; + + typedef VertexVectorPoseNanoFlannAdaptor* > > P2KD; + const P2KD p2kd(vertices_to_search); + + typedef nanoflann::KDTreeSingleIndexAdaptor , P2KD, dim> my_kd_tree_t; + + my_kd_tree_t index(dim, p2kd, nanoflann::KDTreeSingleIndexAdaptorParams(10) ); + index.buildIndex(); + + std::vector ret_index(num_results); + std::vector out_dist_sqr(num_results); + const double query_pt[2] = {refPose.GetX(), refPose.GetY()}; + num_results = index.knnSearch(&query_pt[0], num_results, &ret_index[0], &out_dist_sqr[0]); + + if (num_results > 0) + { + return vertices_to_search[ret_index[0]]; + } + else + { + return NULL; + } + } + + Pose2 MapperGraph::ComputeWeightedMean(const Pose2Vector& rMeans, const std::vector& rCovariances) const + { + assert(rMeans.size() == rCovariances.size()); + + // compute sum of inverses and create inverse list + std::vector inverses; + inverses.reserve(rCovariances.size()); + + Matrix3 sumOfInverses; + const_forEach(std::vector, &rCovariances) + { + Matrix3 inverse = iter->Inverse(); + inverses.push_back(inverse); + + sumOfInverses += inverse; + } + Matrix3 inverseOfSumOfInverses = sumOfInverses.Inverse(); + + // compute weighted mean + Pose2 accumulatedPose; + kt_double thetaX = 0.0; + kt_double thetaY = 0.0; + + Pose2Vector::const_iterator meansIter = rMeans.begin(); + const_forEach(std::vector, &inverses) + { + Pose2 pose = *meansIter; + kt_double angle = pose.GetHeading(); + thetaX += cos(angle); + thetaY += sin(angle); + + Matrix3 weight = inverseOfSumOfInverses * (*iter); + accumulatedPose += weight * pose; + + ++meansIter; + } + + thetaX /= rMeans.size(); + thetaY /= rMeans.size(); + accumulatedPose.SetHeading(atan2(thetaY, thetaX)); + + return accumulatedPose; + } + + LocalizedRangeScanVector MapperGraph::FindPossibleLoopClosure(LocalizedRangeScan* pScan, + const Name& rSensorName, + kt_int32u& rStartNum) + { + LocalizedRangeScanVector chain; // return value + + Pose2 pose = pScan->GetReferencePose(m_pMapper->m_pUseScanBarycenter->GetValue()); + + // possible loop closure chain should not include close scans that have a + // path of links to the scan of interest + const LocalizedRangeScanVector nearLinkedScans = + FindNearLinkedScans(pScan, m_pMapper->m_pLoopSearchMaximumDistance->GetValue()); + + kt_int32u nScans = static_cast(m_pMapper->m_pMapperSensorManager->GetScans(rSensorName).size()); + for (; rStartNum < nScans; rStartNum++) + { + LocalizedRangeScan* pCandidateScan = m_pMapper->m_pMapperSensorManager->GetScan(rSensorName, rStartNum); + + if (pCandidateScan == NULL) + { + continue; + } + + Pose2 candidateScanPose = pCandidateScan->GetReferencePose(m_pMapper->m_pUseScanBarycenter->GetValue()); + + kt_double squaredDistance = candidateScanPose.GetPosition().SquaredDistance(pose.GetPosition()); + if (squaredDistance < math::Square(m_pMapper->m_pLoopSearchMaximumDistance->GetValue()) + KT_TOLERANCE) + { + // a linked scan cannot be in the chain + if (find(nearLinkedScans.begin(), nearLinkedScans.end(), pCandidateScan) != nearLinkedScans.end()) + { + chain.clear(); + } + else + { + chain.push_back(pCandidateScan); + } + } + else + { + // return chain if it is long "enough" + if (chain.size() >= m_pMapper->m_pLoopMatchMinimumChainSize->GetValue()) + { + return chain; + } + else + { + chain.clear(); + } + } + } + + return chain; + } + + void MapperGraph::CorrectPoses() + { + // optimize scans! + ScanSolver* pSolver = m_pMapper->m_pScanOptimizer; + if (pSolver != NULL) + { + pSolver->Compute(); + + const_forEach(ScanSolver::IdPoseVector, &pSolver->GetCorrections()) + { + LocalizedRangeScan* scan = m_pMapper->m_pMapperSensorManager->GetScan(iter->first); + if (scan == NULL) + { + continue; + } + scan->SetCorrectedPoseAndUpdate(iter->second); + } + + pSolver->Clear(); + } + } + + void MapperGraph::UpdateLoopScanMatcher(kt_double rangeThreshold) + { + if (m_pLoopScanMatcher) { + delete m_pLoopScanMatcher; + } + m_pLoopScanMatcher = ScanMatcher::Create(m_pMapper, + m_pMapper->m_pLoopSearchSpaceDimension->GetValue(), + m_pMapper->m_pLoopSearchSpaceResolution->GetValue(), + m_pMapper->m_pLoopSearchSpaceSmearDeviation->GetValue(), rangeThreshold); + assert(m_pLoopScanMatcher); + } + + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////// + + /** + * Default constructor + */ + Mapper::Mapper() + : Module("Mapper"), + m_Initialized(false), + m_Deserialized(false), + m_pSequentialScanMatcher(NULL), + m_pMapperSensorManager(NULL), + m_pGraph(NULL), + m_pScanOptimizer(NULL) + { + InitializeParameters(); + } + + /** + * Default constructor + */ + Mapper::Mapper(const std::string & rName) + : Module(rName), + m_Initialized(false), + m_Deserialized(false), + m_pSequentialScanMatcher(NULL), + m_pMapperSensorManager(NULL), + m_pGraph(NULL), + m_pScanOptimizer(NULL) + { + InitializeParameters(); + } + + /** + * Destructor + */ + Mapper::~Mapper() + { + Reset(); + + delete m_pMapperSensorManager; + } + + void Mapper::InitializeParameters() + { + m_pUseScanMatching = new Parameter( + "UseScanMatching", + "When set to true, the mapper will use a scan matching algorithm. " + "In most real-world situations this should be set to true so that the " + "mapper algorithm can correct for noise and errors in odometry and " + "scan data. In some simulator environments where the simulated scan " + "and odometry data are very accurate, the scan matching algorithm can " + "produce worse results. In those cases set this to false to improve " + "results.", + true, + GetParameterManager()); + + m_pUseScanBarycenter = new Parameter( + "UseScanBarycenter", + "Use the barycenter of scan endpoints to define distances between " + "scans.", + true, GetParameterManager()); + + m_pMinimumTimeInterval = new Parameter( + "MinimumTimeInterval", + "Sets the minimum time between scans. If a new scan's time stamp is " + "longer than MinimumTimeInterval from the previously processed scan, " + "the mapper will use the data from the new scan. Otherwise, it will " + "discard the new scan if it also does not meet the minimum travel " + "distance and heading requirements. For performance reasons, it is " + "generally it is a good idea to only process scans if a reasonable " + "amount of time has passed. This parameter is particularly useful " + "when there is a need to process scans while the robot is stationary.", + 3600, GetParameterManager()); + + m_pMinimumTravelDistance = new Parameter( + "MinimumTravelDistance", + "Sets the minimum travel between scans. If a new scan's position is " + "more than minimumTravelDistance from the previous scan, the mapper " + "will use the data from the new scan. Otherwise, it will discard the " + "new scan if it also does not meet the minimum change in heading " + "requirement. For performance reasons, generally it is a good idea to " + "only process scans if the robot has moved a reasonable amount.", + 0.2, GetParameterManager()); + + m_pMinimumTravelHeading = new Parameter( + "MinimumTravelHeading", + "Sets the minimum heading change between scans. If a new scan's " + "heading is more than MinimumTravelHeading from the previous scan, the " + "mapper will use the data from the new scan. Otherwise, it will " + "discard the new scan if it also does not meet the minimum travel " + "distance requirement. For performance reasons, generally it is a good " + "idea to only process scans if the robot has moved a reasonable " + "amount.", + math::DegreesToRadians(10), GetParameterManager()); + + m_pScanBufferSize = new Parameter( + "ScanBufferSize", + "Scan buffer size is the length of the scan chain stored for scan " + "matching. \"ScanBufferSize\" should be set to approximately " + "\"ScanBufferMaximumScanDistance\" / \"MinimumTravelDistance\". The " + "idea is to get an area approximately 20 meters long for scan " + "matching. For example, if we add scans every MinimumTravelDistance == " + "0.3 meters, then \"scanBufferSize\" should be 20 / 0.3 = 67.)", + 70, GetParameterManager()); + + m_pScanBufferMaximumScanDistance = new Parameter( + "ScanBufferMaximumScanDistance", + "Scan buffer maximum scan distance is the maximum distance between the " + "first and last scans in the scan chain stored for matching.", + 20.0, GetParameterManager()); + + m_pLinkMatchMinimumResponseFine = new Parameter( + "LinkMatchMinimumResponseFine", + "Scans are linked only if the correlation response value is greater " + "than this value.", + 0.8, GetParameterManager()); + + m_pLinkScanMaximumDistance = new Parameter( + "LinkScanMaximumDistance", + "Maximum distance between linked scans. Scans that are farther apart " + "will not be linked regardless of the correlation response value.", + 10.0, GetParameterManager()); + + m_pLoopSearchMaximumDistance = new Parameter( + "LoopSearchMaximumDistance", + "Scans less than this distance from the current position will be " + "considered for a match in loop closure.", + 4.0, GetParameterManager()); + + m_pDoLoopClosing = new Parameter( + "DoLoopClosing", + "Enable/disable loop closure.", + true, GetParameterManager()); + + m_pLoopMatchMinimumChainSize = new Parameter( + "LoopMatchMinimumChainSize", + "When the loop closure detection finds a candidate it must be part of " + "a large set of linked scans. If the chain of scans is less than this " + "value we do not attempt to close the loop.", + 10, GetParameterManager()); + + m_pLoopMatchMaximumVarianceCoarse = new Parameter( + "LoopMatchMaximumVarianceCoarse", + "The co-variance values for a possible loop closure have to be less " + "than this value to consider a viable solution. This applies to the " + "coarse search.", + math::Square(0.4), GetParameterManager()); + + m_pLoopMatchMinimumResponseCoarse = new Parameter( + "LoopMatchMinimumResponseCoarse", + "If response is larger then this, then initiate loop closure search at " + "the coarse resolution.", + 0.8, GetParameterManager()); + + m_pLoopMatchMinimumResponseFine = new Parameter( + "LoopMatchMinimumResponseFine", + "If response is larger then this, then initiate loop closure search at " + "the fine resolution.", + 0.8, GetParameterManager()); + + ////////////////////////////////////////////////////////////////////////////// + // CorrelationParameters correlationParameters; + + m_pCorrelationSearchSpaceDimension = new Parameter( + "CorrelationSearchSpaceDimension", + "The size of the search grid used by the matcher. The search grid will " + "have the size CorrelationSearchSpaceDimension * " + "CorrelationSearchSpaceDimension", + 0.3, GetParameterManager()); + + m_pCorrelationSearchSpaceResolution = new Parameter( + "CorrelationSearchSpaceResolution", + "The resolution (size of a grid cell) of the correlation grid.", + 0.01, GetParameterManager()); + + m_pCorrelationSearchSpaceSmearDeviation = new Parameter( + "CorrelationSearchSpaceSmearDeviation", + "The point readings are smeared by this value in X and Y to create a " + "smoother response.", + 0.03, GetParameterManager()); + + + ////////////////////////////////////////////////////////////////////////////// + // CorrelationParameters loopCorrelationParameters; + + m_pLoopSearchSpaceDimension = new Parameter( + "LoopSearchSpaceDimension", + "The size of the search grid used by the matcher.", + 8.0, GetParameterManager()); + + m_pLoopSearchSpaceResolution = new Parameter( + "LoopSearchSpaceResolution", + "The resolution (size of a grid cell) of the correlation grid.", + 0.05, GetParameterManager()); + + m_pLoopSearchSpaceSmearDeviation = new Parameter( + "LoopSearchSpaceSmearDeviation", + "The point readings are smeared by this value in X and Y to create a " + "smoother response.", + 0.03, GetParameterManager()); + + ////////////////////////////////////////////////////////////////////////////// + // ScanMatcherParameters; + + m_pDistanceVariancePenalty = new Parameter( + "DistanceVariancePenalty", + "Variance of penalty for deviating from odometry when scan-matching. " + "The penalty is a multiplier (less than 1.0) is a function of the " + "delta of the scan position being tested and the odometric pose.", + math::Square(0.3), GetParameterManager()); + + m_pAngleVariancePenalty = new Parameter( + "AngleVariancePenalty", + "See DistanceVariancePenalty.", + math::Square(math::DegreesToRadians(20)), GetParameterManager()); + + m_pFineSearchAngleOffset = new Parameter( + "FineSearchAngleOffset", + "The range of angles to search during a fine search.", + math::DegreesToRadians(0.2), GetParameterManager()); + + m_pCoarseSearchAngleOffset = new Parameter( + "CoarseSearchAngleOffset", + "The range of angles to search during a coarse search.", + math::DegreesToRadians(20), GetParameterManager()); + + m_pCoarseAngleResolution = new Parameter( + "CoarseAngleResolution", + "Resolution of angles to search during a coarse search.", + math::DegreesToRadians(2), GetParameterManager()); + + m_pMinimumAnglePenalty = new Parameter( + "MinimumAnglePenalty", + "Minimum value of the angle penalty multiplier so scores do not become " + "too small.", + 0.9, GetParameterManager()); + + m_pMinimumDistancePenalty = new Parameter( + "MinimumDistancePenalty", + "Minimum value of the distance penalty multiplier so scores do not " + "become too small.", + 0.5, GetParameterManager()); + + m_pUseResponseExpansion = new Parameter( + "UseResponseExpansion", + "Whether to increase the search space if no good matches are initially " + "found.", + false, GetParameterManager()); + } + /* Adding in getters and setters here for easy parameter access */ + + // General Parameters + + bool Mapper::getParamUseScanMatching() + { + return static_cast(m_pUseScanMatching->GetValue()); + } + + bool Mapper::getParamUseScanBarycenter() + { + return static_cast(m_pUseScanBarycenter->GetValue()); + } + + double Mapper::getParamMinimumTimeInterval() + { + return static_cast(m_pMinimumTimeInterval->GetValue()); + } + + double Mapper::getParamMinimumTravelDistance() + { + return static_cast(m_pMinimumTravelDistance->GetValue()); + } + + double Mapper::getParamMinimumTravelHeading() + { + return math::RadiansToDegrees(static_cast(m_pMinimumTravelHeading->GetValue())); + } + + int Mapper::getParamScanBufferSize() + { + return static_cast(m_pScanBufferSize->GetValue()); + } + + double Mapper::getParamScanBufferMaximumScanDistance() + { + return static_cast(m_pScanBufferMaximumScanDistance->GetValue()); + } + + double Mapper::getParamLinkMatchMinimumResponseFine() + { + return static_cast(m_pLinkMatchMinimumResponseFine->GetValue()); + } + + double Mapper::getParamLinkScanMaximumDistance() + { + return static_cast(m_pLinkScanMaximumDistance->GetValue()); + } + + double Mapper::getParamLoopSearchMaximumDistance() + { + return static_cast(m_pLoopSearchMaximumDistance->GetValue()); + } + + bool Mapper::getParamDoLoopClosing() + { + return static_cast(m_pDoLoopClosing->GetValue()); + } + + int Mapper::getParamLoopMatchMinimumChainSize() + { + return static_cast(m_pLoopMatchMinimumChainSize->GetValue()); + } + + double Mapper::getParamLoopMatchMaximumVarianceCoarse() + { + return static_cast(std::sqrt(m_pLoopMatchMaximumVarianceCoarse->GetValue())); + } + + double Mapper::getParamLoopMatchMinimumResponseCoarse() + { + return static_cast(m_pLoopMatchMinimumResponseCoarse->GetValue()); + } + + double Mapper::getParamLoopMatchMinimumResponseFine() + { + return static_cast(m_pLoopMatchMinimumResponseFine->GetValue()); + } + + // Correlation Parameters - Correlation Parameters + + double Mapper::getParamCorrelationSearchSpaceDimension() + { + return static_cast(m_pCorrelationSearchSpaceDimension->GetValue()); + } + + double Mapper::getParamCorrelationSearchSpaceResolution() + { + return static_cast(m_pCorrelationSearchSpaceResolution->GetValue()); + } + + double Mapper::getParamCorrelationSearchSpaceSmearDeviation() + { + return static_cast(m_pCorrelationSearchSpaceSmearDeviation->GetValue()); + } + + // Correlation Parameters - Loop Correlation Parameters + + double Mapper::getParamLoopSearchSpaceDimension() + { + return static_cast(m_pLoopSearchSpaceDimension->GetValue()); + } + + double Mapper::getParamLoopSearchSpaceResolution() + { + return static_cast(m_pLoopSearchSpaceResolution->GetValue()); + } + + double Mapper::getParamLoopSearchSpaceSmearDeviation() + { + return static_cast(m_pLoopSearchSpaceSmearDeviation->GetValue()); + } + + // ScanMatcher Parameters + + double Mapper::getParamDistanceVariancePenalty() + { + return std::sqrt(static_cast(m_pDistanceVariancePenalty->GetValue())); + } + + double Mapper::getParamAngleVariancePenalty() + { + return std::sqrt(static_cast(m_pAngleVariancePenalty->GetValue())); + } + + double Mapper::getParamFineSearchAngleOffset() + { + return static_cast(m_pFineSearchAngleOffset->GetValue()); + } + + double Mapper::getParamCoarseSearchAngleOffset() + { + return static_cast(m_pCoarseSearchAngleOffset->GetValue()); + } + + double Mapper::getParamCoarseAngleResolution() + { + return static_cast(m_pCoarseAngleResolution->GetValue()); + } + + double Mapper::getParamMinimumAnglePenalty() + { + return static_cast(m_pMinimumAnglePenalty->GetValue()); + } + + double Mapper::getParamMinimumDistancePenalty() + { + return static_cast(m_pMinimumDistancePenalty->GetValue()); + } + + bool Mapper::getParamUseResponseExpansion() + { + return static_cast(m_pUseResponseExpansion->GetValue()); + } + + /* Setters for parameters */ + // General Parameters + void Mapper::setParamUseScanMatching(bool b) + { + m_pUseScanMatching->SetValue((kt_bool)b); + } + + void Mapper::setParamUseScanBarycenter(bool b) + { + m_pUseScanBarycenter->SetValue((kt_bool)b); + } + + void Mapper::setParamMinimumTimeInterval(double d) + { + m_pMinimumTimeInterval->SetValue((kt_double)d); + } + + void Mapper::setParamMinimumTravelDistance(double d) + { + m_pMinimumTravelDistance->SetValue((kt_double)d); + } + + void Mapper::setParamMinimumTravelHeading(double d) + { + m_pMinimumTravelHeading->SetValue((kt_double)d); + } + + void Mapper::setParamScanBufferSize(int i) + { + m_pScanBufferSize->SetValue((kt_int32u)i); + } + + void Mapper::setParamScanBufferMaximumScanDistance(double d) + { + m_pScanBufferMaximumScanDistance->SetValue((kt_double)d); + } + + void Mapper::setParamLinkMatchMinimumResponseFine(double d) + { + m_pLinkMatchMinimumResponseFine->SetValue((kt_double)d); + } + + void Mapper::setParamLinkScanMaximumDistance(double d) + { + m_pLinkScanMaximumDistance->SetValue((kt_double)d); + } + + void Mapper::setParamLoopSearchMaximumDistance(double d) + { + m_pLoopSearchMaximumDistance->SetValue((kt_double)d); + } + + void Mapper::setParamDoLoopClosing(bool b) + { + m_pDoLoopClosing->SetValue((kt_bool)b); + } + + void Mapper::setParamLoopMatchMinimumChainSize(int i) + { + m_pLoopMatchMinimumChainSize->SetValue((kt_int32u)i); + } + + void Mapper::setParamLoopMatchMaximumVarianceCoarse(double d) + { + m_pLoopMatchMaximumVarianceCoarse->SetValue((kt_double)math::Square(d)); + } + + void Mapper::setParamLoopMatchMinimumResponseCoarse(double d) + { + m_pLoopMatchMinimumResponseCoarse->SetValue((kt_double)d); + } + + void Mapper::setParamLoopMatchMinimumResponseFine(double d) + { + m_pLoopMatchMinimumResponseFine->SetValue((kt_double)d); + } + + // Correlation Parameters - Correlation Parameters + void Mapper::setParamCorrelationSearchSpaceDimension(double d) + { + m_pCorrelationSearchSpaceDimension->SetValue((kt_double)d); + } + + void Mapper::setParamCorrelationSearchSpaceResolution(double d) + { + m_pCorrelationSearchSpaceResolution->SetValue((kt_double)d); + } + + void Mapper::setParamCorrelationSearchSpaceSmearDeviation(double d) + { + m_pCorrelationSearchSpaceSmearDeviation->SetValue((kt_double)d); + } + + + // Correlation Parameters - Loop Closure Parameters + void Mapper::setParamLoopSearchSpaceDimension(double d) + { + m_pLoopSearchSpaceDimension->SetValue((kt_double)d); + } + + void Mapper::setParamLoopSearchSpaceResolution(double d) + { + m_pLoopSearchSpaceResolution->SetValue((kt_double)d); + } + + void Mapper::setParamLoopSearchSpaceSmearDeviation(double d) + { + m_pLoopSearchSpaceSmearDeviation->SetValue((kt_double)d); + } + + + // Scan Matcher Parameters + void Mapper::setParamDistanceVariancePenalty(double d) + { + m_pDistanceVariancePenalty->SetValue((kt_double)math::Square(d)); + } + + void Mapper::setParamAngleVariancePenalty(double d) + { + m_pAngleVariancePenalty->SetValue((kt_double)math::Square(d)); + } + + void Mapper::setParamFineSearchAngleOffset(double d) + { + m_pFineSearchAngleOffset->SetValue((kt_double)d); + } + + void Mapper::setParamCoarseSearchAngleOffset(double d) + { + m_pCoarseSearchAngleOffset->SetValue((kt_double)d); + } + + void Mapper::setParamCoarseAngleResolution(double d) + { + m_pCoarseAngleResolution->SetValue((kt_double)d); + } + + void Mapper::setParamMinimumAnglePenalty(double d) + { + m_pMinimumAnglePenalty->SetValue((kt_double)d); + } + + void Mapper::setParamMinimumDistancePenalty(double d) + { + m_pMinimumDistancePenalty->SetValue((kt_double)d); + } + + void Mapper::setParamUseResponseExpansion(bool b) + { + m_pUseResponseExpansion->SetValue((kt_bool)b); + } + + + + void Mapper::Initialize(kt_double rangeThreshold) + { + if (m_Initialized) + { + return; + } + // create sequential scan and loop matcher, update if deserialized + + if (m_pSequentialScanMatcher) { + delete m_pSequentialScanMatcher; + } + m_pSequentialScanMatcher = ScanMatcher::Create(this, + m_pCorrelationSearchSpaceDimension->GetValue(), + m_pCorrelationSearchSpaceResolution->GetValue(), + m_pCorrelationSearchSpaceSmearDeviation->GetValue(), + rangeThreshold); + assert(m_pSequentialScanMatcher); + + if (m_Deserialized) { + m_pMapperSensorManager->SetRunningScanBufferSize(m_pScanBufferSize->GetValue()); + m_pMapperSensorManager->SetRunningScanBufferMaximumDistance(m_pScanBufferMaximumScanDistance->GetValue()); + + m_pGraph->UpdateLoopScanMatcher(rangeThreshold); + } else { + m_pMapperSensorManager = new MapperSensorManager(m_pScanBufferSize->GetValue(), + m_pScanBufferMaximumScanDistance->GetValue()); + + m_pGraph = new MapperGraph(this, rangeThreshold); + } + + m_Initialized = true; + } + + void Mapper::SaveToFile(const std::string& filename) + { + printf("Save To File %s \n", filename.c_str()); + std::ofstream ofs(filename.c_str()); + boost::archive::binary_oarchive oa(ofs, boost::archive::no_codecvt); + oa << BOOST_SERIALIZATION_NVP(*this); + } + + void Mapper::LoadFromFile(const std::string& filename) + { + printf("Load From File %s \n", filename.c_str()); + std::ifstream ifs(filename.c_str()); + boost::archive::binary_iarchive ia(ifs, boost::archive::no_codecvt); + ia >> BOOST_SERIALIZATION_NVP(*this); + m_Deserialized = true; + m_Initialized = false; + } + + void Mapper::Reset() + { + if (m_pSequentialScanMatcher) + { + delete m_pSequentialScanMatcher; + m_pSequentialScanMatcher = NULL; + } + if (m_pGraph) + { + delete m_pGraph; + m_pGraph = NULL; + } + if (m_pMapperSensorManager) + { + delete m_pMapperSensorManager; + m_pMapperSensorManager = NULL; + } + m_Initialized = false; + m_Deserialized = false; + while (!m_LocalizationScanVertices.empty()) + { + m_LocalizationScanVertices.pop(); + } + } + + kt_bool Mapper::Process(Object* /*pObject*/) + { + return true; + } + + kt_bool Mapper::Process(LocalizedRangeScan* pScan, Matrix3 * covariance) + { + if (pScan != NULL) + { + karto::LaserRangeFinder* pLaserRangeFinder = pScan->GetLaserRangeFinder(); + + // validate scan + if (pLaserRangeFinder == NULL || pScan == NULL || pLaserRangeFinder->Validate(pScan) == false) + { + return false; + } + + if (m_Initialized == false) + { + // initialize mapper with range threshold from device + Initialize(pLaserRangeFinder->GetRangeThreshold()); + } + + // get last scan + LocalizedRangeScan* pLastScan = m_pMapperSensorManager->GetLastScan(pScan->GetSensorName()); + + // update scans corrected pose based on last correction + if (pLastScan != NULL) + { + Transform lastTransform(pLastScan->GetOdometricPose(), pLastScan->GetCorrectedPose()); + pScan->SetCorrectedPose(lastTransform.TransformPose(pScan->GetOdometricPose())); + } + + // test if scan is outside minimum boundary or if heading is larger then minimum heading + if (!HasMovedEnough(pScan, pLastScan)) + { + return false; + } + + Matrix3 cov; + cov.SetToIdentity(); + + // correct scan (if not first scan) + if (m_pUseScanMatching->GetValue() && pLastScan != NULL) + { + Pose2 bestPose; + m_pSequentialScanMatcher->MatchScan(pScan, + m_pMapperSensorManager->GetRunningScans(pScan->GetSensorName()), + bestPose, + cov); + pScan->SetSensorPose(bestPose); + if (covariance) { + *covariance = cov; + } + } + + // add scan to buffer and assign id + m_pMapperSensorManager->AddScan(pScan); + + if (m_pUseScanMatching->GetValue()) + { + // add to graph + m_pGraph->AddVertex(pScan); + m_pGraph->AddEdges(pScan, cov); + + m_pMapperSensorManager->AddRunningScan(pScan); + + if (m_pDoLoopClosing->GetValue()) + { + std::vector deviceNames = m_pMapperSensorManager->GetSensorNames(); + const_forEach(std::vector, &deviceNames) + { + m_pGraph->TryCloseLoop(pScan, *iter); + } + } + } + + m_pMapperSensorManager->SetLastScan(pScan); + + return true; + } + + return false; + } + + kt_bool Mapper::ProcessAgainstNodesNearBy(LocalizedRangeScan* pScan, kt_bool addScanToLocalizationBuffer, Matrix3 * covariance) + { + if (pScan != NULL) + { + karto::LaserRangeFinder* pLaserRangeFinder = pScan->GetLaserRangeFinder(); + + // validate scan + if (pLaserRangeFinder == NULL || pScan == NULL || + pLaserRangeFinder->Validate(pScan) == false) + { + return false; + } + + if (m_Initialized == false) + { + // initialize mapper with range threshold from device + Initialize(pLaserRangeFinder->GetRangeThreshold()); + } + + Vertex* closetVertex = m_pGraph->FindNearByScan( + pScan->GetSensorName(), pScan->GetOdometricPose()); + LocalizedRangeScan* pLastScan = NULL; + if (closetVertex) + { + pLastScan = m_pMapperSensorManager->GetScan(pScan->GetSensorName(), + closetVertex->GetObject()->GetStateId()); + m_pMapperSensorManager->ClearRunningScans(pScan->GetSensorName()); + m_pMapperSensorManager->AddRunningScan(pLastScan); + m_pMapperSensorManager->SetLastScan(pLastScan); + } + + Matrix3 cov; + cov.SetToIdentity(); + + // correct scan (if not first scan) + if (m_pUseScanMatching->GetValue() && pLastScan != NULL) + { + Pose2 bestPose; + m_pSequentialScanMatcher->MatchScan(pScan, + m_pMapperSensorManager->GetRunningScans(pScan->GetSensorName()), + bestPose, + cov); + pScan->SetSensorPose(bestPose); + } + + pScan->SetOdometricPose(pScan->GetCorrectedPose()); + + if (covariance) { + *covariance = cov; + } + + // add scan to buffer and assign id + m_pMapperSensorManager->AddScan(pScan); + + Vertex* scan_vertex = NULL; + if (m_pUseScanMatching->GetValue()) + { + scan_vertex = m_pGraph->AddVertex(pScan); + // add to graph + m_pGraph->AddVertex(pScan); + m_pGraph->AddEdges(pScan, cov); + + m_pMapperSensorManager->AddRunningScan(pScan); + + if (m_pDoLoopClosing->GetValue()) + { + std::vector deviceNames = + m_pMapperSensorManager->GetSensorNames(); + const_forEach(std::vector, &deviceNames) + { + m_pGraph->TryCloseLoop(pScan, *iter); + } + } + } + + m_pMapperSensorManager->SetLastScan(pScan); + + if (addScanToLocalizationBuffer) + { + AddScanToLocalizationBuffer(pScan, scan_vertex); + } + + return true; + } + + return false; + } + + kt_bool Mapper::ProcessLocalization(LocalizedRangeScan * pScan, Matrix3 * covariance) + { + if (pScan == NULL) + { + return false; + } + + karto::LaserRangeFinder* pLaserRangeFinder = pScan->GetLaserRangeFinder(); + + // validate scan + if (pLaserRangeFinder == NULL || pScan == NULL || + pLaserRangeFinder->Validate(pScan) == false) + { + return false; + } + + if (m_Initialized == false) + { + // initialize mapper with range threshold from device + Initialize(pLaserRangeFinder->GetRangeThreshold()); + } + + // get last scan + LocalizedRangeScan* pLastScan = m_pMapperSensorManager->GetLastScan( + pScan->GetSensorName()); + + // update scans corrected pose based on last correction + if (pLastScan != NULL) + { + Transform lastTransform(pLastScan->GetOdometricPose(), + pLastScan->GetCorrectedPose()); + pScan->SetCorrectedPose(lastTransform.TransformPose( + pScan->GetOdometricPose())); + } + + // test if scan is outside minimum boundary + // or if heading is larger then minimum heading + if (!HasMovedEnough(pScan, pLastScan)) + { + return false; + } + + Matrix3 cov; + cov.SetToIdentity(); + + // correct scan (if not first scan) + if (m_pUseScanMatching->GetValue() && pLastScan != NULL) + { + Pose2 bestPose; + m_pSequentialScanMatcher->MatchScan(pScan, + m_pMapperSensorManager->GetRunningScans(pScan->GetSensorName()), + bestPose, + cov); + pScan->SetSensorPose(bestPose); + if (covariance) { + *covariance = cov; + } + } + + // add scan to buffer and assign id + m_pMapperSensorManager->AddScan(pScan); + + Vertex* scan_vertex = NULL; + if (m_pUseScanMatching->GetValue()) + { + // add to graph + scan_vertex = m_pGraph->AddVertex(pScan); + m_pGraph->AddEdges(pScan, cov); + + m_pMapperSensorManager->AddRunningScan(pScan); + + if (m_pDoLoopClosing->GetValue()) + { + std::vector deviceNames = m_pMapperSensorManager->GetSensorNames(); + const_forEach(std::vector, &deviceNames) + { + m_pGraph->TryCloseLoop(pScan, *iter); + } + } + } + + m_pMapperSensorManager->SetLastScan(pScan); + AddScanToLocalizationBuffer(pScan, scan_vertex); + + return true; + } + + void Mapper::AddScanToLocalizationBuffer(LocalizedRangeScan * pScan, Vertex * scan_vertex) + { + + // generate the info to store and later decay, outside of dataset + LocalizationScanVertex lsv; + lsv.scan = pScan; + lsv.vertex = scan_vertex; + m_LocalizationScanVertices.push(lsv); + + if (m_LocalizationScanVertices.size() > getParamScanBufferSize()) + { + LocalizationScanVertex& oldLSV = m_LocalizationScanVertices.front(); + RemoveNodeFromGraph(oldLSV.vertex); + + // delete node and scans + // free hat! + // No need to delete from m_scans as those pointers will be freed memory + oldLSV.vertex->RemoveObject(); + m_pMapperSensorManager->RemoveScan(oldLSV.scan); + if (oldLSV.scan) + { + delete oldLSV.scan; + oldLSV.scan = NULL; + } + + m_LocalizationScanVertices.pop(); + } + } + + void Mapper::ClearLocalizationBuffer() + { + while (!m_LocalizationScanVertices.empty()) + { + LocalizationScanVertex& oldLSV = m_LocalizationScanVertices.front(); + RemoveNodeFromGraph(oldLSV.vertex); + oldLSV.vertex->RemoveObject(); + m_pMapperSensorManager->RemoveScan(oldLSV.scan); + if (oldLSV.scan) + { + delete oldLSV.scan; + oldLSV.scan = NULL; + } + + m_LocalizationScanVertices.pop(); + } + + std::vector names = m_pMapperSensorManager->GetSensorNames(); + for (uint i = 0; i != names.size(); i++) + { + m_pMapperSensorManager->ClearRunningScans(names[i]); + m_pMapperSensorManager->ClearLastScan(names[i]); + } + + return; + } + + kt_bool Mapper::RemoveNodeFromGraph(Vertex* vertex_to_remove) + { + // 1) delete edges in adjacent vertices, graph, and optimizer + std::vector*> adjVerts = + vertex_to_remove->GetAdjacentVertices(); + for (int i = 0; i != adjVerts.size(); i++) + { + std::vector*> adjEdges = adjVerts[i]->GetEdges(); + bool found = false; + for (int j=0; j!=adjEdges.size(); j++) + { + if (adjEdges[j]->GetTarget() == vertex_to_remove || + adjEdges[j]->GetSource() == vertex_to_remove) + { + adjVerts[i]->RemoveEdge(j); + m_pScanOptimizer->RemoveConstraint( + adjEdges[j]->GetSource()->GetObject()->GetUniqueId(), + adjEdges[j]->GetTarget()->GetObject()->GetUniqueId()); + std::vector*> edges = m_pGraph->GetEdges(); + std::vector*>::iterator edgeGraphIt = + std::find(edges.begin(), edges.end(), adjEdges[j]); + + if (edgeGraphIt == edges.end()) + { + std::cout << "Edge not found in graph to remove!" << std::endl; + continue; + } + + int posEdge = edgeGraphIt - edges.begin(); + m_pGraph->RemoveEdge(posEdge); // remove from graph + delete *edgeGraphIt; // free hat! + *edgeGraphIt = NULL; + found = true; + } + } + if (!found) + { + std::cout << "Failed to find any edge in adj. vertex" << + " with a matching vertex to current!" << std::endl; + } + } + + // 2) delete vertex from optimizer + m_pScanOptimizer->RemoveNode(vertex_to_remove->GetObject()->GetUniqueId()); + + // 3) delete from vertex map + std::map*> > + vertexMap = m_pGraph->GetVertices(); + std::map*> graphVertices = + vertexMap[vertex_to_remove->GetObject()->GetSensorName()]; + std::map*>::iterator + vertexGraphIt = graphVertices.find(vertex_to_remove->GetObject()->GetStateId()); + if (vertexGraphIt != graphVertices.end()) + { + m_pGraph->RemoveVertex(vertex_to_remove->GetObject()->GetSensorName(), + vertexGraphIt->second->GetObject()->GetStateId()); + } + else + { + std::cout << "Vertex not found in graph to remove!" << std::endl; + return false; + } + + return true; + } + + kt_bool Mapper::ProcessAgainstNode( + LocalizedRangeScan * pScan, + const int & nodeId, + Matrix3 * covariance) + { + if (pScan != NULL) + { + karto::LaserRangeFinder* pLaserRangeFinder = pScan->GetLaserRangeFinder(); + + // validate scan + if (pLaserRangeFinder == NULL || pScan == NULL || + pLaserRangeFinder->Validate(pScan) == false) + { + return false; + } + + if (m_Initialized == false) + { + // initialize mapper with range threshold from device + Initialize(pLaserRangeFinder->GetRangeThreshold()); + } + + // If we're matching against a node from an older mapping session + // lets get the first scan as the last scan and populate running scans + // with the first few from that run as well. + LocalizedRangeScan* pLastScan = + m_pMapperSensorManager->GetScan(pScan->GetSensorName(), nodeId); + m_pMapperSensorManager->ClearRunningScans(pScan->GetSensorName()); + m_pMapperSensorManager->AddRunningScan(pLastScan); + m_pMapperSensorManager->SetLastScan(pLastScan); + + Matrix3 cov; + cov.SetToIdentity(); + + // correct scan (if not first scan) + if (m_pUseScanMatching->GetValue() && pLastScan != NULL) + { + Pose2 bestPose; + m_pSequentialScanMatcher->MatchScan(pScan, + m_pMapperSensorManager->GetRunningScans(pScan->GetSensorName()), + bestPose, + cov); + pScan->SetSensorPose(bestPose); + } + + pScan->SetOdometricPose(pScan->GetCorrectedPose()); + if (covariance) { + *covariance = cov; + } + + // add scan to buffer and assign id + m_pMapperSensorManager->AddScan(pScan); + + if (m_pUseScanMatching->GetValue()) + { + // add to graph + m_pGraph->AddVertex(pScan); + m_pGraph->AddEdges(pScan, cov); + + m_pMapperSensorManager->AddRunningScan(pScan); + + if (m_pDoLoopClosing->GetValue()) + { + std::vector deviceNames = + m_pMapperSensorManager->GetSensorNames(); + const_forEach(std::vector, &deviceNames) + { + m_pGraph->TryCloseLoop(pScan, *iter); + } + } + } + + m_pMapperSensorManager->SetLastScan(pScan); + + return true; + } + + return false; + } + + kt_bool Mapper::ProcessAtDock(LocalizedRangeScan * pScan, Matrix3 * covariance) + { + // Special case of processing against node where node is the starting point + return ProcessAgainstNode(pScan, 0, covariance); + } + + /** + * Is the scan sufficiently far from the last scan? + * @param pScan + * @param pLastScan + * @return true if the scans are sufficiently far + */ + kt_bool Mapper::HasMovedEnough(LocalizedRangeScan* pScan, LocalizedRangeScan* pLastScan) const + { + // test if first scan + if (pLastScan == NULL) + { + return true; + } + + // test if enough time has passed + kt_double timeInterval = pScan->GetTime() - pLastScan->GetTime(); + if (timeInterval >= m_pMinimumTimeInterval->GetValue()) + { + return true; + } + + Pose2 lastScannerPose = pLastScan->GetSensorAt(pLastScan->GetOdometricPose()); + Pose2 scannerPose = pScan->GetSensorAt(pScan->GetOdometricPose()); + + // test if we have turned enough + kt_double deltaHeading = math::NormalizeAngle(scannerPose.GetHeading() - lastScannerPose.GetHeading()); + if (fabs(deltaHeading) >= m_pMinimumTravelHeading->GetValue()) + { + return true; + } + + // test if we have moved enough + kt_double squaredTravelDistance = lastScannerPose.GetPosition().SquaredDistance(scannerPose.GetPosition()); + if (squaredTravelDistance >= math::Square(m_pMinimumTravelDistance->GetValue()) - KT_TOLERANCE) + { + return true; + } + + return false; + } + + /** + * Gets all the processed scans + * @return all scans + */ + const LocalizedRangeScanVector Mapper::GetAllProcessedScans() const + { + LocalizedRangeScanVector allScans; + + if (m_pMapperSensorManager != NULL) + { + allScans = m_pMapperSensorManager->GetAllScans(); + } + + return allScans; + } + + /** + * Adds a listener + * @param pListener + */ + void Mapper::AddListener(MapperListener* pListener) + { + m_Listeners.push_back(pListener); + } + + /** + * Removes a listener + * @param pListener + */ + void Mapper::RemoveListener(MapperListener* pListener) + { + std::vector::iterator iter = std::find(m_Listeners.begin(), m_Listeners.end(), pListener); + if (iter != m_Listeners.end()) + { + m_Listeners.erase(iter); + } + } + + void Mapper::FireInfo(const std::string& rInfo) const + { + const_forEach(std::vector, &m_Listeners) + { + (*iter)->Info(rInfo); + } + } + + void Mapper::FireDebug(const std::string& rInfo) const + { + const_forEach(std::vector, &m_Listeners) + { + MapperDebugListener* pListener = dynamic_cast(*iter); + + if (pListener != NULL) + { + pListener->Debug(rInfo); + } + } + } + + void Mapper::FireLoopClosureCheck(const std::string& rInfo) const + { + const_forEach(std::vector, &m_Listeners) + { + MapperLoopClosureListener* pListener = dynamic_cast(*iter); + + if (pListener != NULL) + { + pListener->LoopClosureCheck(rInfo); + } + } + } + + void Mapper::FireBeginLoopClosure(const std::string& rInfo) const + { + const_forEach(std::vector, &m_Listeners) + { + MapperLoopClosureListener* pListener = dynamic_cast(*iter); + + if (pListener != NULL) + { + pListener->BeginLoopClosure(rInfo); + } + } + } + + void Mapper::FireEndLoopClosure(const std::string& rInfo) const + { + const_forEach(std::vector, &m_Listeners) + { + MapperLoopClosureListener* pListener = dynamic_cast(*iter); + + if (pListener != NULL) + { + pListener->EndLoopClosure(rInfo); + } + } + } + + void Mapper::SetScanSolver(ScanSolver* pScanOptimizer) + { + m_pScanOptimizer = pScanOptimizer; + } + + ScanSolver* Mapper::getScanSolver() + { + return m_pScanOptimizer; + } + + MapperGraph* Mapper::GetGraph() const + { + return m_pGraph; + } + + ScanMatcher* Mapper::GetSequentialScanMatcher() const + { + return m_pSequentialScanMatcher; + } + + ScanMatcher* Mapper::GetLoopScanMatcher() const + { + return m_pGraph->GetLoopScanMatcher(); + } +} // namespace karto +BOOST_CLASS_EXPORT(karto::BreadthFirstTraversal) diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox/package.xml b/Localizations/Packages/slam_toolbox/slam_toolbox/package.xml new file mode 100644 index 0000000..4c89241 --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox/package.xml @@ -0,0 +1,106 @@ + + + slam_toolbox + + This package provides a sped up improved slam karto with updated SDK and visualization and modification toolsets + + 1.5.7 + Steve Macenski + Steve Macenski + LGPL + + catkin + + slam_toolbox_msgs + cmake_modules + pluginlib + eigen + message_filters + nav_msgs + rosconsole + roscpp + sparse_bundle_adjustment + sensor_msgs + tf2_ros + tf + tf2 + visualization_msgs + std_srvs + interactive_markers + std_msgs + suitesparse + liblapack-dev + libceres-dev + libg2o + tf2_geometry_msgs + tbb + libqt5-core + libqt5-widgets + qtbase5-dev + map_server + karto_sdk + + slam_toolbox_msgs + cmake_modules + pluginlib + eigen + message_filters + nav_msgs + rosconsole + roscpp + sparse_bundle_adjustment + sensor_msgs + tf2_ros + tf + tf2 + visualization_msgs + std_srvs + interactive_markers + std_msgs + suitesparse + liblapack-dev + libceres-dev + libg2o + tf2_geometry_msgs + tbb + libqt5-core + libqt5-widgets + qtbase5-dev + map_server + karto_sdk + + slam_toolbox_msgs + eigen + pluginlib + message_filters + nav_msgs + rosconsole + roscpp + sparse_bundle_adjustment + sensor_msgs + tf + tf2 + tf2_ros + visualization_msgs + message_runtime + std_srvs + interactive_markers + std_msgs + suitesparse + liblapack-dev + libceres-dev + libg2o + tf2_geometry_msgs + tbb + libqt5-core + libqt5-widgets + map_server + karto_sdk + + gtest + + + + + + diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox/snap/generate_snap b/Localizations/Packages/slam_toolbox/slam_toolbox/snap/generate_snap new file mode 100644 index 0000000..422e498 --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox/snap/generate_snap @@ -0,0 +1,21 @@ +#!/bin/bash + +# make workspace +mkdir -p snap_ws/src +cd snap_ws && catkin_init_workspace +cd src + +# add all the necessary things to it +git clone -b melodic-devel https://github.com/SteveMacenski/slam_toolbox.git +cd ../ + +# move snap and hooks to right place +mkdir snap +cp -r src/slam_toolbox/snap/snapcraft.yaml snap + +# build the snap +export SNAPCRAFT_BUILD_ENVIRONMENT_MEMORY=4g +snapcraft + +# go back to root +cd ../ diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox/snap/snapcraft.yaml b/Localizations/Packages/slam_toolbox/slam_toolbox/snap/snapcraft.yaml new file mode 100644 index 0000000..e03e5dc --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox/snap/snapcraft.yaml @@ -0,0 +1,47 @@ +name: slam-toolbox-melodic +version: '1.1.3' +summary: Slam Toolbox +description: | + A toolbox for building high quality maps of large spaces with a LIDAR. + +grade: stable +confinement: classic +base: core18 +type: app + +apps: + slam-toolbox-online-sync: + command: roslaunch --wait slam_toolbox online_sync.launch + environment: + LD_LIBRARY_PATH: $LD_LIBRARY_PATH:$SNAP/usr/lib/x86_64-linux-gnu # pluginlib (lapack, blas) + plugs: [network, network-bind] + + slam-toolbox-online-async: + command: roslaunch --wait slam_toolbox online_async.launch + environment: + LD_LIBRARY_PATH: $LD_LIBRARY_PATH:$SNAP/usr/lib/x86_64-linux-gnu # pluginlib (lapack, blas) + plugs: [network, network-bind] + + slam-toolbox-offline: + command: roslaunch --wait slam_toolbox offline.launch + environment: + LD_LIBRARY_PATH: $LD_LIBRARY_PATH:$SNAP/usr/lib/x86_64-linux-gnu # pluginlib (lapack, blas) + plugs: [network, network-bind] + + slam-toolbox-localization: + command: roslaunch --wait slam_toolbox localization.launch + environment: + LD_LIBRARY_PATH: $LD_LIBRARY_PATH:$SNAP/usr/lib/x86_64-linux-gnu # pluginlib (lapack, blas) + plugs: [network, network-bind] + +parts: + workspace: + build-packages: [qt5-default, libgoogle-glog-dev, liblapack-dev, libblas-dev, libpthread-stubs0-dev] + stage-packages: [qt5-default, libgoogle-glog-dev, liblapack-dev, libblas-dev, libpthread-stubs0-dev] + plugin: catkin + source-space: src + include-roscore: true + source: . + catkin-ros-master-uri: http://localhost:11311 + catkin-packages: [slam_toolbox] + recursive-rosinstall: false diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox/solver_plugins.xml b/Localizations/Packages/slam_toolbox/slam_toolbox/solver_plugins.xml new file mode 100644 index 0000000..181de02 --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox/solver_plugins.xml @@ -0,0 +1,23 @@ + + + SPA Optimizer for karto + + + + + + G2O Optimizer for karto + + + + + + Ceres Optimizer for karto + + + + \ No newline at end of file diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox/solvers/ceres_solver.cpp b/Localizations/Packages/slam_toolbox/slam_toolbox/solvers/ceres_solver.cpp new file mode 100644 index 0000000..38e96c0 --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox/solvers/ceres_solver.cpp @@ -0,0 +1,443 @@ +/* + * Copyright 2018 Simbe Robotics, Inc. + * Author: Steve Macenski (stevenmacenski@gmail.com) + */ + +#include "ceres_solver.hpp" +#include + +#include "ros/console.h" +#include + +PLUGINLIB_EXPORT_CLASS(solver_plugins::CeresSolver, karto::ScanSolver) + +namespace solver_plugins +{ + +/*****************************************************************************/ +CeresSolver::CeresSolver() : + nodes_(new std::unordered_map()), + blocks_(new std::unordered_map()), + problem_(NULL), was_constant_set_(false) +/*****************************************************************************/ +{ + ros::NodeHandle nh("~"); + std::string solver_type, preconditioner_type, dogleg_type, + trust_strategy, loss_fn, mode; + nh.getParam("ceres_linear_solver", solver_type); + nh.getParam("ceres_preconditioner", preconditioner_type); + nh.getParam("ceres_dogleg_type", dogleg_type); + nh.getParam("ceres_trust_strategy", trust_strategy); + nh.getParam("ceres_loss_function", loss_fn); + nh.getParam("mode", mode); + nh.getParam("debug_logging", debug_logging_); + + corrections_.clear(); + first_node_ = nodes_->end(); + + // formulate problem + angle_local_parameterization_ = AngleLocalParameterization::Create(); + + // choose loss function default squared loss (NULL) + loss_function_ = NULL; + if (loss_fn == "HuberLoss") + { + ROS_INFO("CeresSolver: Using HuberLoss loss function."); + loss_function_ = new ceres::HuberLoss(0.7); + } + else if (loss_fn == "CauchyLoss") + { + ROS_INFO("CeresSolver: Using CauchyLoss loss function."); + loss_function_ = new ceres::CauchyLoss(0.7); + } + + // choose linear solver default CHOL + options_.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY; + if (solver_type == "SPARSE_SCHUR") + { + ROS_INFO("CeresSolver: Using SPARSE_SCHUR solver."); + options_.linear_solver_type = ceres::SPARSE_SCHUR; + } + else if (solver_type == "ITERATIVE_SCHUR") + { + ROS_INFO("CeresSolver: Using ITERATIVE_SCHUR solver."); + options_.linear_solver_type = ceres::ITERATIVE_SCHUR; + } + else if (solver_type == "CGNR") + { + ROS_INFO("CeresSolver: Using CGNR solver."); + options_.linear_solver_type = ceres::CGNR; + } + + // choose preconditioner default Jacobi + options_.preconditioner_type = ceres::JACOBI; + if (preconditioner_type == "IDENTITY") + { + ROS_INFO("CeresSolver: Using IDENTITY preconditioner."); + options_.preconditioner_type = ceres::IDENTITY; + } + else if (preconditioner_type == "SCHUR_JACOBI") + { + ROS_INFO("CeresSolver: Using SCHUR_JACOBI preconditioner."); + options_.preconditioner_type = ceres::SCHUR_JACOBI; + } + + if (options_.preconditioner_type == ceres::CLUSTER_JACOBI || + options_.preconditioner_type == ceres::CLUSTER_TRIDIAGONAL) + { + //default canonical view is O(n^2) which is unacceptable for + // problems of this size + options_.visibility_clustering_type = ceres::SINGLE_LINKAGE; + } + + // choose trust region strategy default LM + options_.trust_region_strategy_type = ceres::LEVENBERG_MARQUARDT; + if (trust_strategy == "DOGLEG") + { + ROS_INFO("CeresSolver: Using DOGLEG trust region strategy."); + options_.trust_region_strategy_type = ceres::DOGLEG; + } + + // choose dogleg type default traditional + if(options_.trust_region_strategy_type == ceres::DOGLEG) + { + options_.dogleg_type = ceres::TRADITIONAL_DOGLEG; + if (dogleg_type == "SUBSPACE_DOGLEG") + { + ROS_INFO("CeresSolver: Using SUBSPACE_DOGLEG dogleg type."); + options_.dogleg_type = ceres::SUBSPACE_DOGLEG; + } + } + + // a typical ros map is 5cm, this is 0.001, 50x the resolution + options_.function_tolerance = 1e-3; + options_.gradient_tolerance = 1e-6; + options_.parameter_tolerance = 1e-3; + + options_.sparse_linear_algebra_library_type = ceres::SUITE_SPARSE; + options_.max_num_consecutive_invalid_steps = 3; + options_.max_consecutive_nonmonotonic_steps = + options_.max_num_consecutive_invalid_steps; + options_.num_threads = 50; + options_.use_nonmonotonic_steps = true; + options_.jacobi_scaling = true; + + options_.min_relative_decrease = 1e-3; + + options_.initial_trust_region_radius = 1e4; + options_.max_trust_region_radius = 1e8; + options_.min_trust_region_radius = 1e-16; + + options_.min_lm_diagonal = 1e-6; + options_.max_lm_diagonal = 1e32; + + if(options_.linear_solver_type == ceres::SPARSE_NORMAL_CHOLESKY) + { + options_.dynamic_sparsity = true; + } + + if (mode == std::string("localization")) + { + // doubles the memory footprint, but lets us remove contraints faster + options_problem_.enable_fast_removal = true; + } + + problem_ = new ceres::Problem(options_problem_); + + return; +} + +/*****************************************************************************/ +CeresSolver::~CeresSolver() +/*****************************************************************************/ +{ + if ( loss_function_ != NULL) + { + delete loss_function_; + } + if (nodes_ != NULL) + { + delete nodes_; + } + if (problem_ != NULL) + { + delete problem_; + } +} + +/*****************************************************************************/ +void CeresSolver::Compute() +/*****************************************************************************/ +{ + boost::mutex::scoped_lock lock(nodes_mutex_); + + if (nodes_->size() == 0) + { + ROS_ERROR("CeresSolver: Ceres was called when there are no nodes." + " This shouldn't happen."); + return; + } + + // populate contraint for static initial pose + if (!was_constant_set_ && first_node_ != nodes_->end()) + { + ROS_DEBUG("CeresSolver: Setting first node as a constant pose:" + "%0.2f, %0.2f, %0.2f.", first_node_->second(0), + first_node_->second(1), first_node_->second(2)); + problem_->SetParameterBlockConstant(&first_node_->second(0)); + problem_->SetParameterBlockConstant(&first_node_->second(1)); + problem_->SetParameterBlockConstant(&first_node_->second(2)); + was_constant_set_ = !was_constant_set_; + } + + const ros::Time start_time = ros::Time::now(); + ceres::Solver::Summary summary; + ceres::Solve(options_, problem_, &summary); + if (debug_logging_) + { + std::cout << summary.FullReport() << '\n'; + } + + if (!summary.IsSolutionUsable()) + { + ROS_WARN("CeresSolver: " + "Ceres could not find a usable solution to optimize."); + return; + } + + // store corrected poses + if (!corrections_.empty()) + { + corrections_.clear(); + } + corrections_.reserve(nodes_->size()); + karto::Pose2 pose; + ConstGraphIterator iter = nodes_->begin(); + for ( iter; iter != nodes_->end(); ++iter ) + { + pose.SetX(iter->second(0)); + pose.SetY(iter->second(1)); + pose.SetHeading(iter->second(2)); + corrections_.push_back(std::make_pair(iter->first, pose)); + } + + return; +} + +/*****************************************************************************/ +const karto::ScanSolver::IdPoseVector& CeresSolver::GetCorrections() const +/*****************************************************************************/ +{ + return corrections_; +} + +/*****************************************************************************/ +void CeresSolver::Clear() +/*****************************************************************************/ +{ + corrections_.clear(); +} + +/*****************************************************************************/ +void CeresSolver::Reset() +/*****************************************************************************/ +{ + boost::mutex::scoped_lock lock(nodes_mutex_); + + corrections_.clear(); + was_constant_set_ = false; + + if (problem_) + { + delete problem_; + } + + if (nodes_) + { + delete nodes_; + } + + if (blocks_) + { + delete blocks_; + } + + nodes_ = new std::unordered_map(); + blocks_ = new std::unordered_map(); + problem_ = new ceres::Problem(options_problem_); + first_node_ = nodes_->end(); + + angle_local_parameterization_ = AngleLocalParameterization::Create(); +} + +/*****************************************************************************/ +void CeresSolver::AddNode(karto::Vertex* pVertex) +/*****************************************************************************/ +{ + // store nodes + if (!pVertex) + { + return; + } + + karto::Pose2 pose = pVertex->GetObject()->GetCorrectedPose(); + Eigen::Vector3d pose2d(pose.GetX(), pose.GetY(), pose.GetHeading()); + + const int id = pVertex->GetObject()->GetUniqueId(); + + boost::mutex::scoped_lock lock(nodes_mutex_); + nodes_->insert(std::pair(id,pose2d)); + + if (nodes_->size() == 1) + { + first_node_ = nodes_->find(id); + } +} + +/*****************************************************************************/ +void CeresSolver::AddConstraint(karto::Edge* pEdge) +/*****************************************************************************/ +{ + // get IDs in graph for this edge + boost::mutex::scoped_lock lock(nodes_mutex_); + + if (!pEdge) + { + return; + } + + const int node1 = pEdge->GetSource()->GetObject()->GetUniqueId(); + GraphIterator node1it = nodes_->find(node1); + const int node2 = pEdge->GetTarget()->GetObject()->GetUniqueId(); + GraphIterator node2it = nodes_->find(node2); + + if (node1it == nodes_->end() || + node2it == nodes_->end() || node1it == node2it) + { + ROS_WARN("CeresSolver: Failed to add constraint, could not find nodes."); + return; + } + + // extract transformation + karto::LinkInfo* pLinkInfo = (karto::LinkInfo*)(pEdge->GetLabel()); + karto::Pose2 diff = pLinkInfo->GetPoseDifference(); + Eigen::Vector3d pose2d(diff.GetX(), diff.GetY(), diff.GetHeading()); + + karto::Matrix3 precisionMatrix = pLinkInfo->GetCovariance().Inverse(); + Eigen::Matrix3d information; + information(0, 0) = precisionMatrix(0, 0); + information(0, 1) = information(1, 0) = precisionMatrix(0, 1); + information(0, 2) = information(2, 0) = precisionMatrix(0, 2); + information(1, 1) = precisionMatrix(1, 1); + information(1, 2) = information(2, 1) = precisionMatrix(1, 2); + information(2, 2) = precisionMatrix(2, 2); + Eigen::Matrix3d sqrt_information = information.llt().matrixU(); + + // populate residual and parameterization for heading normalization + ceres::CostFunction* cost_function = PoseGraph2dErrorTerm::Create(pose2d(0), + pose2d(1), pose2d(2), sqrt_information); + ceres::ResidualBlockId block = problem_->AddResidualBlock( + cost_function, loss_function_, + &node1it->second(0), &node1it->second(1), &node1it->second(2), + &node2it->second(0), &node2it->second(1), &node2it->second(2)); + problem_->SetParameterization(&node1it->second(2), + angle_local_parameterization_); + problem_->SetParameterization(&node2it->second(2), + angle_local_parameterization_); + + blocks_->insert(std::pair( + GetHash(node1, node2), block)); + return; +} + +/*****************************************************************************/ +void CeresSolver::RemoveNode(kt_int32s id) +/*****************************************************************************/ +{ + boost::mutex::scoped_lock lock(nodes_mutex_); + GraphIterator nodeit = nodes_->find(id); + if (nodeit != nodes_->end()) + { + if (problem_->HasParameterBlock(&nodeit->second(0)) && + problem_->HasParameterBlock(&nodeit->second(1)) && + problem_->HasParameterBlock(&nodeit->second(2))) + { + problem_->RemoveParameterBlock(&nodeit->second(0)); + problem_->RemoveParameterBlock(&nodeit->second(1)); + problem_->RemoveParameterBlock(&nodeit->second(2)); + ROS_DEBUG("RemoveNode: Removed node id %d", nodeit->first); + } + else + { + ROS_DEBUG("RemoveNode: Failed to remove parameter blocks for node id %d", nodeit->first); + } + nodes_->erase(nodeit); + } + else + { + ROS_ERROR("RemoveNode: Failed to find node matching id %i", (int)id); + } +} + +/*****************************************************************************/ +void CeresSolver::RemoveConstraint(kt_int32s sourceId, kt_int32s targetId) +/*****************************************************************************/ +{ + boost::mutex::scoped_lock lock(nodes_mutex_); + std::unordered_map::iterator it_a = + blocks_->find(GetHash(sourceId, targetId)); + std::unordered_map::iterator it_b = + blocks_->find(GetHash(targetId, sourceId)); + if (it_a != blocks_->end()) + { + problem_->RemoveResidualBlock(it_a->second); + blocks_->erase(it_a); + } + else if (it_b != blocks_->end()) + { + problem_->RemoveResidualBlock(it_b->second); + blocks_->erase(it_b); + } + else + { + ROS_ERROR("RemoveConstraint: Failed to find residual block for %i %i", + (int)sourceId, (int)targetId); + } +} + +/*****************************************************************************/ +void CeresSolver::ModifyNode(const int& unique_id, Eigen::Vector3d pose) +/*****************************************************************************/ +{ + boost::mutex::scoped_lock lock(nodes_mutex_); + GraphIterator it = nodes_->find(unique_id); + if (it != nodes_->end()) + { + double yaw_init = it->second(2); + it->second = pose; + it->second(2) += yaw_init; + } +} + +/*****************************************************************************/ +void CeresSolver::GetNodeOrientation(const int& unique_id, double& pose) +/*****************************************************************************/ +{ + boost::mutex::scoped_lock lock(nodes_mutex_); + GraphIterator it = nodes_->find(unique_id); + if (it != nodes_->end()) + { + pose = it->second(2); + } +} + +/*****************************************************************************/ +std::unordered_map* CeresSolver::getGraph() +/*****************************************************************************/ +{ + boost::mutex::scoped_lock lock(nodes_mutex_); + return nodes_; +} + +} // end namespace diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox/solvers/ceres_solver.hpp b/Localizations/Packages/slam_toolbox/slam_toolbox/solvers/ceres_solver.hpp new file mode 100644 index 0000000..9c0f060 --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox/solvers/ceres_solver.hpp @@ -0,0 +1,72 @@ +/* + * Copyright 2018 Simbe Robotics, Inc. + * Author: Steve Macenski (stevenmacenski@gmail.com) + */ + +#ifndef KARTO_CERESSOLVER_H +#define KARTO_CERESSOLVER_H + +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "../include/slam_toolbox/toolbox_types.hpp" +#include "ceres_utils.h" + +namespace solver_plugins +{ + +using namespace ::toolbox_types; + +class CeresSolver : public karto::ScanSolver +{ +public: + CeresSolver(); + virtual ~CeresSolver(); + +public: + virtual const karto::ScanSolver::IdPoseVector& GetCorrections() const; //Get corrected poses after optimization + virtual void Compute(); //Solve + virtual void Clear(); //Resets the corrections + virtual void Reset(); //Resets the solver plugin clean + + virtual void AddNode(karto::Vertex* pVertex); //Adds a node to the solver + virtual void AddConstraint(karto::Edge* pEdge); //Adds a constraint to the solver + virtual std::unordered_map* getGraph(); //Get graph stored + virtual void RemoveNode(kt_int32s id); //Removes a node from the solver correction table + virtual void RemoveConstraint(kt_int32s sourceId, kt_int32s targetId); // Removes constraints from the optimization problem + + virtual void ModifyNode(const int& unique_id, Eigen::Vector3d pose); // change a node's pose + virtual void GetNodeOrientation(const int& unique_id, double& pose); // get a node's current pose yaw + +private: + // karto + karto::ScanSolver::IdPoseVector corrections_; + + // ceres + ceres::Solver::Options options_; + ceres::Problem::Options options_problem_; + ceres::LossFunction* loss_function_; + ceres::Problem* problem_; + ceres::LocalParameterization* angle_local_parameterization_; + bool was_constant_set_, debug_logging_; + + // graph + std::unordered_map* nodes_; + std::unordered_map* blocks_; + std::unordered_map::iterator first_node_; + boost::mutex nodes_mutex_; +}; + +} + +#endif diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox/solvers/ceres_utils.h b/Localizations/Packages/slam_toolbox/slam_toolbox/solvers/ceres_utils.h new file mode 100644 index 0000000..08b5f62 --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox/solvers/ceres_utils.h @@ -0,0 +1,103 @@ +/* + * Copyright 2018 Simbe Robotics + * Author: Steve Macenski + */ + +#include +#include +#include +#include + +/*****************************************************************************/ +/*****************************************************************************/ +/*****************************************************************************/ +inline std::size_t GetHash(const int& x, const int& y) +{ + return ((std::hash()(x) ^ (std::hash()(y) << 1)) >> 1); +} + +/*****************************************************************************/ +/*****************************************************************************/ +/*****************************************************************************/ + +// Normalizes the angle in radians between [-pi and pi). +template inline T NormalizeAngle(const T& angle_radians) +{ + T two_pi(2.0 * M_PI); + return angle_radians - two_pi * ceres::floor((angle_radians + T(M_PI)) / two_pi); +} + +/*****************************************************************************/ +/*****************************************************************************/ +/*****************************************************************************/ + +class AngleLocalParameterization +{ + public: + template + bool operator()(const T* theta_radians, const T* delta_theta_radians, T* theta_radians_plus_delta) const + { + *theta_radians_plus_delta = NormalizeAngle(*theta_radians + *delta_theta_radians); + return true; + } + + static ceres::LocalParameterization* Create() + { + return (new ceres::AutoDiffLocalParameterization); + } +}; + +/*****************************************************************************/ +/*****************************************************************************/ +/*****************************************************************************/ + +template +Eigen::Matrix RotationMatrix2D(T yaw_radians) +{ + const T cos_yaw = ceres::cos(yaw_radians); + const T sin_yaw = ceres::sin(yaw_radians); + Eigen::Matrix rotation; + rotation << cos_yaw, -sin_yaw, sin_yaw, cos_yaw; + return rotation; +} + +/*****************************************************************************/ +/*****************************************************************************/ +/*****************************************************************************/ + +class PoseGraph2dErrorTerm +{ + public: + PoseGraph2dErrorTerm(double x_ab, double y_ab, double yaw_ab_radians, const Eigen::Matrix3d& sqrt_information) + : p_ab_(x_ab, y_ab), yaw_ab_radians_(yaw_ab_radians), sqrt_information_(sqrt_information) + { + } + + template + bool operator()(const T* const x_a, const T* const y_a, const T* const yaw_a, const T* const x_b, const T* const y_b, const T* const yaw_b, T* residuals_ptr) const + { + const Eigen::Matrix p_a(*x_a, *y_a); + const Eigen::Matrix p_b(*x_b, *y_b); + Eigen::Map > residuals_map(residuals_ptr); + residuals_map.template head<2>() = RotationMatrix2D(*yaw_a).transpose() * (p_b - p_a) - p_ab_.cast(); + residuals_map(2) = NormalizeAngle((*yaw_b - *yaw_a) - static_cast(yaw_ab_radians_)); + // Scale the residuals by the square root information matrix to account for the measurement uncertainty. + residuals_map = sqrt_information_.template cast() * residuals_map; + return true; + } + + static ceres::CostFunction* Create(double x_ab, double y_ab, double yaw_ab_radians, const Eigen::Matrix3d& sqrt_information) + { + return (new ceres::AutoDiffCostFunction(new PoseGraph2dErrorTerm(x_ab, y_ab, yaw_ab_radians, sqrt_information))); + } + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + private: + // The position of B relative to A in the A frame. + const Eigen::Vector2d p_ab_; + // The orientation of frame B relative to frame A. + const double yaw_ab_radians_; + // The inverse square root of the measurement covariance matrix. + const Eigen::Matrix3d sqrt_information_; +}; + diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox/solvers/deprecated/GTSAM_solver.cpp b/Localizations/Packages/slam_toolbox/slam_toolbox/solvers/deprecated/GTSAM_solver.cpp new file mode 100644 index 0000000..e93609c --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox/solvers/deprecated/GTSAM_solver.cpp @@ -0,0 +1,186 @@ +/********************************************************************* +* +* Copyright (c) 2017, Saurav Agarwal +* All rights reserved. +* +*********************************************************************/ + +/* Authors: Saurav Agarwal */ +/* Modified: Steve Macenski */ + +#include +#include +#include +#include "GTSAM_solver.hpp" +#include + +PLUGINLIB_EXPORT_CLASS(solver_plugins::GTSAMSolver, karto::ScanSolver) + +namespace solver_plugins +{ + +/*****************************************************************************/ +GTSAMSolver::GTSAMSolver() +/*****************************************************************************/ +{ + using namespace gtsam; + + // add the prior on the first node which is known + noiseModel::Diagonal::shared_ptr priorNoise = + noiseModel::Diagonal::Sigmas(Vector3(1e-6, 1e-6, 1e-8)); + + graph_.emplace_shared >(0, Pose2(0, 0, 0), priorNoise); + +} + +/*****************************************************************************/ +GTSAMSolver::~GTSAMSolver() +/*****************************************************************************/ +{ + +} + +/*****************************************************************************/ +void GTSAMSolver::Clear() +/*****************************************************************************/ +{ + corrections_.clear(); +} + +/*****************************************************************************/ +const karto::ScanSolver::IdPoseVector& GTSAMSolver::GetCorrections() const +/*****************************************************************************/ +{ + return corrections_; +} + +/*****************************************************************************/ +void GTSAMSolver::Compute() +/*****************************************************************************/ +{ + using namespace gtsam; + + corrections_.clear(); + + graphNodes_.clear(); + + LevenbergMarquardtParams parameters; + + // Stop iterating once the change in error between steps is less than this value + parameters.relativeErrorTol = 1e-5; + + // Do not perform more than N iteration steps + parameters.maxIterations = 500; + + // Create the optimizer ... + LevenbergMarquardtOptimizer optimizer(graph_, initialGuess_, parameters); + + // ... and optimize + Values result = optimizer.optimize(); + + Values::ConstFiltered viewPose2 = result.filter(); + + // put values into corrections container + for(const Values::ConstFiltered::KeyValuePair& key_value: viewPose2) + { + + karto::Pose2 pose(key_value.value.x(), key_value.value.y(), + key_value.value.theta()); + + corrections_.push_back(std::make_pair(key_value.key, pose)); + + graphNodes_.push_back(Eigen::Vector2d(key_value.value.x(), + key_value.value.y())); + } +} + +/*****************************************************************************/ +void GTSAMSolver::AddNode(karto::Vertex* pVertex) +/*****************************************************************************/ +{ + using namespace gtsam; + + karto::Pose2 odom = pVertex->GetObject()->GetCorrectedPose(); + + initialGuess_.insert(pVertex->GetObject()->GetUniqueId(), + Pose2( odom.GetX(), odom.GetY(), odom.GetHeading() )); + + graphNodes_.push_back(Eigen::Vector2d(odom.GetX(), odom.GetY())); + + ROS_DEBUG("[gtsam] Adding node %d.", pVertex->GetObject()->GetUniqueId()); + +} + +/*****************************************************************************/ +void GTSAMSolver::AddConstraint(karto::Edge* pEdge) +/*****************************************************************************/ +{ + using namespace gtsam; + + // Set source and target + int sourceID = pEdge->GetSource()->GetObject()->GetUniqueId(); + + int targetID = pEdge->GetTarget()->GetObject()->GetUniqueId(); + + // Set the measurement (poseGraphEdge distance between vertices) + karto::LinkInfo* pLinkInfo = (karto::LinkInfo*)(pEdge->GetLabel()); + + karto::Pose2 diff = pLinkInfo->GetPoseDifference(); + + // Set the covariance of the measurement + karto::Matrix3 precisionMatrix = pLinkInfo->GetCovariance(); + + Eigen::Matrix cov; + + cov(0,0) = precisionMatrix(0,0); + + cov(0,1) = cov(1,0) = precisionMatrix(0,1); + + cov(0,2) = cov(2,0) = precisionMatrix(0,2); + + cov(1,1) = precisionMatrix(1,1); + + cov(1,2) = cov(2,1) = precisionMatrix(1,2); + + cov(2,2) = precisionMatrix(2,2); + + noiseModel::Gaussian::shared_ptr model = noiseModel::Diagonal::Covariance(cov); + + // Add odometry factors + // Create odometry (Between) factors between consecutive poses + graph_.emplace_shared >(sourceID, targetID, + Pose2(diff.GetX(), diff.GetY(), diff.GetHeading()), model); + + // Add the constraint to the optimizer + ROS_DEBUG("[gtsam] Adding Edge from node %d to node %d.", sourceID, targetID); + +} + +/*****************************************************************************/ +void GTSAMSolver::getGraph(std::vector& nodes) +/*****************************************************************************/ +{ + nodes = graphNodes_; + // using namespace gtsam; + // double *data1 = new double[3]; + // double *data2 = new double[3]; + // for (SparseOptimizer::EdgeSet::iterator it = + // optimizer_.edges().begin(); it != optimizer_.edges().end(); ++it) + // { + // EdgeSE2* e = dynamic_cast(*it); + // if(e) + // { + // VertexSE2* v1 = dynamic_cast(e->vertices()[0]); + // v1->getEstimateData(data1); + // Eigen::Vector2d poseFrom(data1[0], data1[1]); + // VertexSE2* v2 = dynamic_cast(e->vertices()[1]); + // v2->getEstimateData(data2); + // Eigen::Vector2d poseTo(data2[0], data2[1]); + // edges.push_back(std::make_pair(poseFrom, poseTo)); + // } + // } + // delete data1; + // delete data2; +} + +} // end namespace \ No newline at end of file diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox/solvers/deprecated/GTSAM_solver.hpp b/Localizations/Packages/slam_toolbox/slam_toolbox/solvers/deprecated/GTSAM_solver.hpp new file mode 100644 index 0000000..78b52a6 --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox/solvers/deprecated/GTSAM_solver.hpp @@ -0,0 +1,93 @@ +/********************************************************************* +* +* Copyright (c) 2017, Saurav Agarwal +* All rights reserved. +* +*********************************************************************/ + +/* Authors: Saurav Agarwal */ +/* Modified: Steve Macenski */ + +#ifndef KARTO_GTSAMSolver_H +#define KARTO_GTSAMSolver_H + +#include +#include +#include +#include +#include +#include + +namespace solver_plugins +{ + +/** + * @brief Wrapper for G2O to interface with Open Karto + */ +class GTSAMSolver : public karto::ScanSolver +{ + public: + + GTSAMSolver(); + + virtual ~GTSAMSolver(); + + public: + + /** + * @brief Clear the vector of corrections + * @details Empty out previously computed corrections + */ + virtual void Clear(); + + /** + * @brief Solve the SLAM back-end + * @details Calls G2O to solve the SLAM back-end + */ + virtual void Compute(); + + /** + * @brief Get the vector of corrections + * @details Get the vector of corrections + * @return Vector with corrected poses + */ + virtual const karto::ScanSolver::IdPoseVector& GetCorrections() const; + + /** + * @brief Add a node to pose-graph + * @details Add a node which is a robot pose to the pose-graph + * + * @param pVertex the node to be added in + */ + virtual void AddNode(karto::Vertex* pVertex); + + /** + * @brief Add an edge constraint to pose-graph + * @details Adds a relative pose measurement constraint between two poses in the graph + * + * @param pEdge [description] + */ + virtual void AddConstraint(karto::Edge* pEdge); + + /** + * @brief Get the pose-graph + * @details Get the underlying graph from g2o, return the graph of constraints + * + * @param g the graph + */ + virtual void getGraph(std::vector& nodes); // std::vector > &edges); + + virtual void ModifyNode(const int& unique_id, const Eigen::Vector3d& pose); // change a node's pose + + private: + + karto::ScanSolver::IdPoseVector corrections_; + gtsam::NonlinearFactorGraph graph_; + gtsam::Values initialGuess_; + std::vector graphNodes_; +}; + +} // end namespace + +#endif // KARTO_GTSAMSolver_H + diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox/solvers/deprecated/g2o_solver.cpp b/Localizations/Packages/slam_toolbox/slam_toolbox/solvers/deprecated/g2o_solver.cpp new file mode 100644 index 0000000..71e093e --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox/solvers/deprecated/g2o_solver.cpp @@ -0,0 +1,234 @@ +/********************************************************************* +* +* Copyright (c) 2017, Saurav Agarwal +* All rights reserved. +* Modified: Steve Macenski (stevenmacenski@gmail.com) +* +*********************************************************************/ + +#include "g2o_solver.hpp" +#include "g2o/core/block_solver.h" +#include "g2o/core/factory.h" +#include "g2o/core/robust_kernel_impl.h" +#include "g2o/core/optimization_algorithm_factory.h" +#include "g2o/core/optimization_algorithm_levenberg.h" +#include "g2o/types/slam2d/types_slam2d.h" +#include "g2o/solvers/cholmod/linear_solver_cholmod.h" +#include "g2o/solvers/csparse/linear_solver_csparse.h" +#include +#include +#include + +PLUGINLIB_EXPORT_CLASS(solver_plugins::G2OSolver, karto::ScanSolver) + +namespace solver_plugins +{ + +typedef g2o::BlockSolver< g2o::BlockSolverTraits<-1, -1> > SlamBlockSolver; + +typedef g2o::LinearSolverCholmod SlamLinearSolver; +//typedef g2o::LinearSolverCSparse SlamLinearSolver; + +/*****************************************************************************/ +G2OSolver::G2OSolver() +/*****************************************************************************/ +{ + // Initialize the SparseOptimizer + auto linearSolver = g2o::make_unique(); + linearSolver->setBlockOrdering(false); + auto blockSolver = g2o::make_unique( + std::move(linearSolver)); + optimizer_.setAlgorithm(new g2o::OptimizationAlgorithmLevenberg( + std::move(blockSolver))); + + latestNodeID_ = 0; + useRobustKernel_ = true; +} + +/*****************************************************************************/ +G2OSolver::~G2OSolver() +/*****************************************************************************/ +{ + // destroy all the singletons + g2o::Factory::destroy(); + g2o::OptimizationAlgorithmFactory::destroy(); + g2o::HyperGraphActionLibrary::destroy(); +} + +/*****************************************************************************/ +void G2OSolver::Clear() +/*****************************************************************************/ +{ + corrections_.clear(); +} + +const karto::ScanSolver::IdPoseVector& G2OSolver::GetCorrections() const +{ + return corrections_; +} + +/*****************************************************************************/ +void G2OSolver::Compute() +/*****************************************************************************/ +{ + corrections_.clear(); + + // Fix the first node in the graph to hold the map in place + g2o::OptimizableGraph::Vertex* first = optimizer_.vertex(0); + + if(!first) + { + ROS_ERROR("[g2o] No Node with ID 0 found!"); + return; + } + + first->setFixed(true); + + // Do the graph optimization + const ros::Time start_time = ros::Time::now(); + optimizer_.initializeOptimization(); + int iter = optimizer_.optimize(500); + ROS_INFO("Loop Closure Solve time: %f seconds", + (ros::Time::now() - start_time).toSec()); + + if (iter <= 0) + { + ROS_ERROR("[g2o] Optimization failed, result might be invalid!"); + return; + } + + // Write the result so it can be used by the mapper + g2o::SparseOptimizer::VertexContainer nodes = optimizer_.activeVertices(); + for (g2o::SparseOptimizer::VertexContainer::const_iterator n = + nodes.begin(); n != nodes.end(); n++) + { + double estimate[3]; + if((*n)->getEstimateData(estimate)) + { + karto::Pose2 pose(estimate[0], estimate[1], estimate[2]); + corrections_.push_back(std::make_pair((*n)->id(), pose)); + } + else + { + ROS_ERROR("[g2o] Could not get estimated pose from Optimizer!"); + } + } +} + +/*****************************************************************************/ +void G2OSolver::AddNode(karto::Vertex* pVertex) +/*****************************************************************************/ +{ + + karto::Pose2 odom = pVertex->GetObject()->GetCorrectedPose(); + g2o::VertexSE2* poseVertex = new g2o::VertexSE2; + poseVertex->setEstimate(g2o::SE2(odom.GetX(), odom.GetY(), + odom.GetHeading())); + poseVertex->setId(pVertex->GetObject()->GetUniqueId()); + optimizer_.addVertex(poseVertex); + latestNodeID_ = pVertex->GetObject()->GetUniqueId(); + + ROS_DEBUG("[g2o] Adding node %d.", pVertex->GetObject()->GetUniqueId()); +} + +/*****************************************************************************/ +void G2OSolver::AddConstraint(karto::Edge* pEdge) +/*****************************************************************************/ +{ + // Create a new edge + g2o::EdgeSE2* odometry = new g2o::EdgeSE2; + + // Set source and target + int sourceID = pEdge->GetSource()->GetObject()->GetUniqueId(); + int targetID = pEdge->GetTarget()->GetObject()->GetUniqueId(); + odometry->vertices()[0] = optimizer_.vertex(sourceID); + odometry->vertices()[1] = optimizer_.vertex(targetID); + + if(odometry->vertices()[0] == NULL) + { + ROS_ERROR("[g2o] Source vertex with id %d does not exist!", sourceID); + delete odometry; + return; + } + + if(odometry->vertices()[0] == NULL) + { + ROS_ERROR("[g2o] Target vertex with id %d does not exist!", targetID); + delete odometry; + return; + } + + // Set the measurement (odometry distance between vertices) + karto::LinkInfo* pLinkInfo = (karto::LinkInfo*)(pEdge->GetLabel()); + karto::Pose2 diff = pLinkInfo->GetPoseDifference(); + g2o::SE2 measurement(diff.GetX(), diff.GetY(), diff.GetHeading()); + odometry->setMeasurement(measurement); + + // Set the covariance of the measurement + karto::Matrix3 precisionMatrix = pLinkInfo->GetCovariance().Inverse(); + Eigen::Matrix info; + + info(0,0) = precisionMatrix(0,0); + info(0,1) = info(1,0) = precisionMatrix(0,1); + info(0,2) = info(2,0) = precisionMatrix(0,2); + info(1,1) = precisionMatrix(1,1); + info(1,2) = info(2,1) = precisionMatrix(1,2); + info(2,2) = precisionMatrix(2,2); + + odometry->setInformation(info); + + if(useRobustKernel_) + { + g2o::RobustKernelDCS* rk = new g2o::RobustKernelDCS; + odometry->setRobustKernel(rk); + } + + // Add the constraint to the optimizer + ROS_DEBUG("[g2o] Adding Edge from node %d to node %d.", sourceID, targetID); + optimizer_.addEdge(odometry); +} + +/*****************************************************************************/ +void G2OSolver::getGraph(std::vector& nodes) +/*****************************************************************************/ +{ + double *data = new double[3]; + for (g2o::SparseOptimizer::VertexIDMap::iterator it = + optimizer_.vertices().begin(); it != optimizer_.vertices().end(); ++it) + { + g2o::VertexSE2* v = dynamic_cast(it->second); + if(v) + { + v->getEstimateData(data); + Eigen::Vector2d pose(data[0], data[1]); + nodes.push_back(pose); + } + } + delete data; + + //using namespace g2o; + //HyperGraph::VertexIDMap vertexMap = optimizer_.vertices(); + //HyperGraph::EdgeSet edgeSet = optimizer_.edges(); + + // double *data1 = new double[3]; + // double *data2 = new double[3]; + // for (SparseOptimizer::EdgeSet::iterator it = optimizer_.edges().begin(); + // it != optimizer_.edges().end(); ++it) + // { + // EdgeSE2* e = dynamic_cast(*it); + // if(e) + // { + // VertexSE2* v1 = dynamic_cast(e->vertices()[0]); + // v1->getEstimateData(data1); + // Eigen::Vector2d poseFrom(data1[0], data1[1]); + // VertexSE2* v2 = dynamic_cast(e->vertices()[1]); + // v2->getEstimateData(data2); + // Eigen::Vector2d poseTo(data2[0], data2[1]); + // edges.push_back(std::make_pair(poseFrom, poseTo)); + // } + // } + // delete data1; + // delete data2; +} + +} // end namespace \ No newline at end of file diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox/solvers/deprecated/g2o_solver.hpp b/Localizations/Packages/slam_toolbox/slam_toolbox/solvers/deprecated/g2o_solver.hpp new file mode 100644 index 0000000..f030626 --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox/solvers/deprecated/g2o_solver.hpp @@ -0,0 +1,102 @@ +/********************************************************************* +* +* Copyright (c) 2017, Saurav Agarwal +* All rights reserved. +* +*********************************************************************/ + + +#ifndef KARTO_G2OSolver_H +#define KARTO_G2OSolver_H + +#include +#include "g2o/core/sparse_optimizer.h" + +namespace solver_plugins +{ + +/** + * @brief Wrapper for G2O to interface with Open Karto + */ +class G2OSolver : public karto::ScanSolver +{ + public: + + G2OSolver(); + + virtual ~G2OSolver(); + + public: + + /** + * @brief Clear the vector of corrections + * @details Empty out previously computed corrections + */ + virtual void Clear(); + + /** + * @brief Solve the SLAM back-end + * @details Calls G2O to solve the SLAM back-end + */ + virtual void Compute(); + + /** + * @brief Get the vector of corrections + * @details Get the vector of corrections + * @return Vector with corrected poses + */ + virtual const karto::ScanSolver::IdPoseVector& GetCorrections() const; + + /** + * @brief Add a node to pose-graph + * @details Add a node which is a robot pose to the pose-graph + * + * @param pVertex the node to be added in + */ + virtual void AddNode(karto::Vertex* pVertex); + + /** + * @brief Add an edge constraint to pose-graph + * @details Adds a relative pose measurement constraint between two poses in the graph + * + * @param pEdge [description] + */ + virtual void AddConstraint(karto::Edge* pEdge); + + /** + * @brief Get the pose-graph + * @details Get the underlying graph from g2o, return the graph of constraints + * + * @param g the graph + */ + void getGraph(std::vector& nodes); // std::vector > &edges); + + /** + * @brief Use robust kernel in back-end + * @details Uses Dynamic Covariance scaling kernel in back-end + * + * @param flag variable, if true robust kernel will be used + */ + void useRobustKernel(bool flag) + { + useRobustKernel_ = flag; + } + + virtual void ModifyNode(const int& unique_id, const Eigen::Vector3d& pose); // change a node's pose + + private: + + karto::ScanSolver::IdPoseVector corrections_; + + g2o::SparseOptimizer optimizer_; + + int latestNodeID_; // ID of the latest added node, this is used internally in AddHeadingConstraint + + bool useRobustKernel_; + +}; + +} + +#endif // KARTO_G2OSolver_H + diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox/solvers/deprecated/spa_solver.cpp b/Localizations/Packages/slam_toolbox/slam_toolbox/solvers/deprecated/spa_solver.cpp new file mode 100644 index 0000000..269ff2b --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox/solvers/deprecated/spa_solver.cpp @@ -0,0 +1,126 @@ +/* + * Copyright 2010 SRI International + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with this program. If not, see . + */ + +#include "spa_solver.hpp" +#include + +#include "ros/console.h" +#include + +PLUGINLIB_EXPORT_CLASS(solver_plugins::SpaSolver, karto::ScanSolver) + +namespace solver_plugins +{ +typedef std::vector > NodeVector; + +/*****************************************************************************/ +SpaSolver::SpaSolver() +/*****************************************************************************/ +{ + +} + +/*****************************************************************************/ +SpaSolver::~SpaSolver() +/*****************************************************************************/ +{ + +} + +/*****************************************************************************/ +void SpaSolver::Clear() +/*****************************************************************************/ +{ + corrections.clear(); +} + +const karto::ScanSolver::IdPoseVector& SpaSolver::GetCorrections() const +{ + return corrections; +} + +/*****************************************************************************/ +void SpaSolver::Compute() +/*****************************************************************************/ +{ + corrections.clear(); + + const ros::Time start_time = ros::Time::now(); + m_Spa.doSPA(40, 1.0e-4, 1); + ROS_INFO("Loop Closure Solve time: %f seconds", + (ros::Time::now() - start_time).toSec()); + + NodeVector nodes = m_Spa.getNodes(); + forEach(NodeVector, &nodes) + { + karto::Pose2 pose(iter->trans(0), iter->trans(1), iter->arot); + corrections.push_back(std::make_pair(iter->nodeId, pose)); + } +} + +/*****************************************************************************/ +void SpaSolver::AddNode(karto::Vertex* pVertex) +/*****************************************************************************/ +{ + karto::Pose2 pose = pVertex->GetObject()->GetCorrectedPose(); + Eigen::Vector3d vector(pose.GetX(), pose.GetY(), pose.GetHeading()); + m_Spa.addNode(vector, pVertex->GetObject()->GetUniqueId()); +} + +/*****************************************************************************/ +void SpaSolver::AddConstraint(karto::Edge* pEdge) +/*****************************************************************************/ +{ + karto::LocalizedRangeScan* pSource = pEdge->GetSource()->GetObject(); + karto::LocalizedRangeScan* pTarget = pEdge->GetTarget()->GetObject(); + karto::LinkInfo* pLinkInfo = (karto::LinkInfo*)(pEdge->GetLabel()); + + karto::Pose2 diff = pLinkInfo->GetPoseDifference(); + Eigen::Vector3d mean(diff.GetX(), diff.GetY(), diff.GetHeading()); + + karto::Matrix3 precisionMatrix = pLinkInfo->GetCovariance().Inverse(); + Eigen::Matrix m; + m(0,0) = precisionMatrix(0,0); + m(0,1) = m(1,0) = precisionMatrix(0,1); + m(0,2) = m(2,0) = precisionMatrix(0,2); + m(1,1) = precisionMatrix(1,1); + m(1,2) = m(2,1) = precisionMatrix(1,2); + m(2,2) = precisionMatrix(2,2); + + m_Spa.addConstraint(pSource->GetUniqueId(), pTarget->GetUniqueId(), mean, m); +} + +/*****************************************************************************/ +void SpaSolver::getGraph(std::vector& g) +/*****************************************************************************/ +{ + std::vector raw_graph; + m_Spa.getGraph(raw_graph); + + g.reserve(raw_graph.size()/4); + + Eigen::Vector2d pose; + for (size_t i=0; i!=raw_graph.size()/4; i++) + { + pose(0) = raw_graph[4*i]; + pose(1) = raw_graph[4*i+1]; + g.push_back(pose); + } +} + + +} // end namespace \ No newline at end of file diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox/solvers/deprecated/spa_solver.hpp b/Localizations/Packages/slam_toolbox/slam_toolbox/solvers/deprecated/spa_solver.hpp new file mode 100644 index 0000000..7a1a7ec --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox/solvers/deprecated/spa_solver.hpp @@ -0,0 +1,61 @@ +/* + * Copyright 2010 SRI International + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with this program. If not, see . + */ + +#ifndef KARTO_SPASOLVER_H +#define KARTO_SPASOLVER_H + +#include + +#ifndef EIGEN_USE_NEW_STDVECTOR +#define EIGEN_USE_NEW_STDVECTOR +#endif // EIGEN_USE_NEW_STDVECTOR + +#include + +#include + +namespace solver_plugins +{ + +class SpaSolver : public karto::ScanSolver +{ +public: + SpaSolver(); + virtual ~SpaSolver(); + +public: + virtual void Clear(); + virtual void Compute(); + virtual const karto::ScanSolver::IdPoseVector& GetCorrections() const; + + virtual void AddNode(karto::Vertex* pVertex); + virtual void AddConstraint(karto::Edge* pEdge); + + virtual void getGraph(std::vector& g); + + virtual void ModifyNode(const int& unique_id, const Eigen::Vector3d& pose); + +private: + karto::ScanSolver::IdPoseVector corrections; + + sba::SysSPA2d m_Spa; +}; + +} + +#endif // KARTO_SPASOLVER_H + diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox/src/experimental/slam_toolbox_lifelong.cpp b/Localizations/Packages/slam_toolbox/slam_toolbox/src/experimental/slam_toolbox_lifelong.cpp new file mode 100644 index 0000000..cf17a72 --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox/src/experimental/slam_toolbox_lifelong.cpp @@ -0,0 +1,444 @@ +/* + * slam_toolbox + * Copyright (c) 2019, Samsung Research America + * + * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE + * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY + * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS + * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED. + * + * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO + * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS + * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND + * CONDITIONS. + * + */ + +/* Author: Steven Macenski */ + +#include "slam_toolbox/experimental/slam_toolbox_lifelong.hpp" + +namespace slam_toolbox +{ + +/*****************************************************************************/ +void LifelongSlamToolbox::checkIsNotNormalized(const double& value) +/*****************************************************************************/ +{ + if (value < 0.0 || value > 1.0) + { + ROS_FATAL("All stores and scales must be in range [0, 1]."); + exit(-1); + } +} + +/*****************************************************************************/ +LifelongSlamToolbox::LifelongSlamToolbox(ros::NodeHandle& nh) +: SlamToolbox(nh) +/*****************************************************************************/ +{ + loadPoseGraphByParams(nh); + nh.param("lifelong_search_use_tree", use_tree_, false); + nh.param("lifelong_minimum_score", iou_thresh_, 0.10); + nh.param("lifelong_iou_match", iou_match_, 0.85); + nh.param("lifelong_node_removal_score", removal_score_, 0.10); + nh.param("lifelong_overlap_score_scale", overlap_scale_, 0.5); + nh.param("lifelong_constraint_multiplier", constraint_scale_, 0.05); + nh.param("lifelong_nearby_penalty", nearby_penalty_, 0.001); + nh.param("lifelong_candidates_scale", candidates_scale_, 0.03); + + checkIsNotNormalized(iou_thresh_); + checkIsNotNormalized(constraint_scale_); + checkIsNotNormalized(removal_score_); + checkIsNotNormalized(overlap_scale_); + checkIsNotNormalized(iou_match_); + checkIsNotNormalized(nearby_penalty_); + checkIsNotNormalized(candidates_scale_); + + ROS_WARN("Lifelong mapping mode in SLAM Toolbox is considered " + "experimental and should be understood before proceeding. Please visit: " + "https://github.com/SteveMacenski/slam_toolbox/wiki/" + "Experimental-Lifelong-Mapping-Node for more information."); + + // in lifelong mode, we cannot have interactive mode enabled + enable_interactive_mode_ = false; +} + +/*****************************************************************************/ +void LifelongSlamToolbox::laserCallback( + const sensor_msgs::LaserScan::ConstPtr& scan) +/*****************************************************************************/ +{ + // no odom info + Pose2 pose; + if(!pose_helper_->getOdomPose(pose, scan->header.stamp)) + { + return; + } + + // ensure the laser can be used + LaserRangeFinder* laser = getLaser(scan); + + if(!laser) + { + ROS_WARN_THROTTLE(5., "Failed to create laser device for" + " %s; discarding scan", scan->header.frame_id.c_str()); + return; + } + + // LTS additional bounded node increase parameter (rate, or total for run or at all?) + // LTS pseudo-localization mode. If want to add a scan, but not deleting a scan, add to local buffer? + // LTS if (eval() && dont_add_more_scans) {addScan()} else {localization_add_scan()} + // LTS if (eval() && ctr / total < add_rate_scans) {addScan()} else {localization_add_scan()} + karto::LocalizedRangeScan* range_scan = addScan(laser, scan, pose); + evaluateNodeDepreciation(range_scan); + return; +} + +/*****************************************************************************/ +void LifelongSlamToolbox::evaluateNodeDepreciation( + LocalizedRangeScan* range_scan) +/*****************************************************************************/ +{ + if (range_scan) + { + boost::mutex::scoped_lock lock(smapper_mutex_); + + const BoundingBox2& bb = range_scan->GetBoundingBox(); + const Size2 bb_size = bb.GetSize(); + double radius = sqrt(bb_size.GetWidth()*bb_size.GetWidth() + + bb_size.GetHeight()*bb_size.GetHeight()) / 2.0; + Vertices near_scan_vertices = FindScansWithinRadius(range_scan, radius); + + ScoredVertices scored_verices = + computeScores(near_scan_vertices, range_scan); + + ScoredVertices::iterator it; + for (it = scored_verices.begin(); it != scored_verices.end(); ++it) + { + if (it->GetScore() < removal_score_) + { + ROS_INFO("Removing node %i from graph with score: %f and " + "old score: %f.", it->GetVertex()->GetObject()->GetUniqueId(), + it->GetScore(), it->GetVertex()->GetScore()); + removeFromSlamGraph(it->GetVertex()); + } + else + { + updateScoresSlamGraph(it->GetScore(), it->GetVertex()); + } + } + } + + return; +} + +/*****************************************************************************/ +Vertices LifelongSlamToolbox::FindScansWithinRadius( + LocalizedRangeScan* scan, const double& radius) +/*****************************************************************************/ +{ + // Using the tree will create a Kd-tree and find all neighbors in graph + // around the reference scan. Otherwise it will use the graph and + // access scans within radius that are connected with constraints to this + // node. + + if (use_tree_) + { + return + smapper_->getMapper()->GetGraph()->FindNearByVertices( + scan->GetSensorName(), scan->GetBarycenterPose(), radius); + } + else + { + return + smapper_->getMapper()->GetGraph()->FindNearLinkedVertices(scan, radius); + } +} + +/*****************************************************************************/ +double LifelongSlamToolbox::computeObjectiveScore( + const double& intersect_over_union, + const double& area_overlap, + const double& reading_overlap, + const int& num_constraints, + const double& initial_score, + const int& num_candidates) const +/*****************************************************************************/ +{ + // We have some useful metrics. lets make a new score + // intersect_over_union: The higher this score, the better aligned in scope these scans are + // area_overlap: The higher, the more area they share normalized by candidate size + // reading_overlap: The higher, the more readings of the new scan the candidate contains + // num_constraints: The lower, the less other nodes may rely on this candidate + // initial_score: Last score of this vertex before update + + // this is a really good fit and not from a loop closure, lets just decay + if (intersect_over_union > iou_match_ && num_constraints < 3) + { + return -1.0; + } + + // to be conservative, lets say the overlap is the lesser of the + // area and proportion of laser returns in the intersecting region. + double overlap = overlap_scale_ * std::min(area_overlap, reading_overlap); + + // if the num_constraints are high we want to stave off the decay, but not override it + double contraint_scale_factor = std::min(1.0, + std::max(0., constraint_scale_ * (num_constraints - 2))); + contraint_scale_factor = std::min(contraint_scale_factor, overlap); + + // + double candidates = num_candidates - 1; + double candidate_scale_factor = candidates_scale_ * candidates; + + // Give the initial score a boost proportional to the number of constraints + // Subtract the overlap amount, apply a penalty for just being nearby + // and scale the entire additional score by the number of candidates + double score = + initial_score * (1.0 + contraint_scale_factor) + - overlap + - nearby_penalty_; + + //score += (initial_score - score) * candidate_scale_factor; + + if (score > 1.0) + { + ROS_ERROR("Objective function calculated for vertex score (%0.4f)" + " greater than one! Thresholding to 1.0", score); + return 1.0; + } + + return score; +} + +/*****************************************************************************/ +double LifelongSlamToolbox::computeScore( + LocalizedRangeScan* reference_scan, + Vertex* candidate, + const double& initial_score, const int& num_candidates) +/*****************************************************************************/ +{ + double new_score = initial_score; + LocalizedRangeScan* candidate_scan = candidate->GetObject(); + + // compute metrics for information loss normalized + double iou = computeIntersectOverUnion(reference_scan, candidate_scan); + double area_overlap = computeAreaOverlapRatio(reference_scan, candidate_scan); + int num_constraints = candidate->GetEdges().size(); + double reading_overlap = computeReadingOverlapRatio(reference_scan, candidate_scan); + + bool critical_lynchpoint = candidate_scan->GetUniqueId() == 0 || + candidate_scan->GetUniqueId() == 1; + int id_diff = reference_scan->GetUniqueId() - candidate_scan->GetUniqueId(); + if (id_diff < smapper_->getMapper()->getParamScanBufferSize() || + critical_lynchpoint) + { + return initial_score; + } + + double score = computeObjectiveScore(iou, + area_overlap, + reading_overlap, + num_constraints, + initial_score, + num_candidates); + + ROS_INFO("Metric Scores: Initial: %f, IOU: %f," + " Area: %f, Num Con: %i, Reading: %f, outcome score: %f.", + initial_score, iou, area_overlap, num_constraints, reading_overlap, score); + return score; +} + +/*****************************************************************************/ +ScoredVertices +LifelongSlamToolbox::computeScores( + Vertices& near_scans, + LocalizedRangeScan* range_scan) +/*****************************************************************************/ +{ + ScoredVertices scored_vertices; + scored_vertices.reserve(near_scans.size()); + + // must have some minimum metric to utilize + // IOU will drop sharply with fitment, I'd advise not setting this value + // any higher than 0.15. Also check this is a linked constraint + // We want to do this early to get a better estimate of local candidates + ScanVector::iterator candidate_scan_it; + double iou = 0.0; + for (candidate_scan_it = near_scans.begin(); + candidate_scan_it != near_scans.end(); ) + { + iou = computeIntersectOverUnion(range_scan, + (*candidate_scan_it)->GetObject()); + if (iou < iou_thresh_ || (*candidate_scan_it)->GetEdges().size() < 2) + { + candidate_scan_it = near_scans.erase(candidate_scan_it); + } + else + { + ++candidate_scan_it; + } + } + + for (candidate_scan_it = near_scans.begin(); + candidate_scan_it != near_scans.end(); ++candidate_scan_it) + { + ScoredVertex scored_vertex((*candidate_scan_it), + computeScore(range_scan, (*candidate_scan_it), + (*candidate_scan_it)->GetScore(), near_scans.size())); + scored_vertices.push_back(scored_vertex); + } + return scored_vertices; +} + +/*****************************************************************************/ +void LifelongSlamToolbox::removeFromSlamGraph( + Vertex* vertex) +/*****************************************************************************/ +{ + smapper_->getMapper()->RemoveNodeFromGraph(vertex); + smapper_->getMapper()->GetMapperSensorManager()->RemoveScan( + vertex->GetObject()); + dataset_->RemoveData(vertex->GetObject()); + vertex->RemoveObject(); + delete vertex; + vertex = nullptr; + // LTS what do we do about the contraints that node had about it?Nothing?Transfer? +} + +/*****************************************************************************/ +void LifelongSlamToolbox::updateScoresSlamGraph(const double& score, + Vertex* vertex) +/*****************************************************************************/ +{ + // Saved in graph so it persists between sessions and runs + vertex->SetScore(score); +} + +/*****************************************************************************/ +bool LifelongSlamToolbox::deserializePoseGraphCallback( + slam_toolbox_msgs::DeserializePoseGraph::Request& req, + slam_toolbox_msgs::DeserializePoseGraph::Response& resp) +/*****************************************************************************/ +{ + if (req.match_type == procType::LOCALIZE_AT_POSE) + { + ROS_ERROR("Requested a localization deserialization " + "in non-localization mode."); + return false; + } + + return SlamToolbox::deserializePoseGraphCallback(req, resp); +} + +/*****************************************************************************/ +void LifelongSlamToolbox::computeIntersectBounds( + LocalizedRangeScan* s1, LocalizedRangeScan* s2, + double& x_l, double& x_u, double& y_l, double& y_u) +/*****************************************************************************/ +{ + Size2 bb1 = s1->GetBoundingBox().GetSize(); + Size2 bb2 = s2->GetBoundingBox().GetSize(); + Pose2 pose1 = s1->GetBarycenterPose(); + Pose2 pose2 = s2->GetBarycenterPose(); + + const double s1_upper_x = pose1.GetX() + (bb1.GetWidth() / 2.0); + const double s1_upper_y = pose1.GetY() + (bb1.GetHeight() / 2.0); + const double s1_lower_x = pose1.GetX() - (bb1.GetWidth() / 2.0); + const double s1_lower_y = pose1.GetY() - (bb1.GetHeight() / 2.0); + + const double s2_upper_x = pose2.GetX() + (bb2.GetWidth() / 2.0); + const double s2_upper_y = pose2.GetY() + (bb2.GetHeight() / 2.0); + const double s2_lower_x = pose2.GetX() - (bb2.GetWidth() / 2.0); + const double s2_lower_y = pose2.GetY() - (bb2.GetHeight() / 2.0); + + x_u = std::min(s1_upper_x, s2_upper_x); + y_u = std::min(s1_upper_y, s2_upper_y); + x_l = std::max(s1_lower_x, s2_lower_x); + y_l = std::max(s1_lower_y, s2_lower_y); + return; +} + +/*****************************************************************************/ +double LifelongSlamToolbox::computeIntersect(LocalizedRangeScan* s1, + LocalizedRangeScan* s2) +/*****************************************************************************/ +{ + double x_l, x_u, y_l, y_u; + computeIntersectBounds(s1, s2, x_l, x_u, y_l, y_u); + const double intersect = (y_u - y_l) * (x_u - x_l); + + if (intersect < 0.0) + { + return 0.0; + } + + return intersect; +} + +/*****************************************************************************/ +double LifelongSlamToolbox::computeIntersectOverUnion(LocalizedRangeScan* s1, + LocalizedRangeScan* s2) +/*****************************************************************************/ +{ + // this is a common metric in machine learning used to determine + // the fitment of a set of bounding boxes. Its response sharply + // drops by box matches. + + const double intersect = computeIntersect(s1, s2); + + Size2 bb1 = s1->GetBoundingBox().GetSize(); + Size2 bb2 = s2->GetBoundingBox().GetSize(); + const double uni = (bb1.GetWidth() * bb1.GetHeight()) + + (bb2.GetWidth() * bb2.GetHeight()) - intersect; + + return intersect / uni; +} + +/*****************************************************************************/ +double LifelongSlamToolbox::computeAreaOverlapRatio( + LocalizedRangeScan* ref_scan, + LocalizedRangeScan* candidate_scan) +/*****************************************************************************/ +{ + // ref scan is new scan, candidate scan is potential for decay + // so we want to find the ratio of space of the candidate scan + // the reference scan takes up + + double overlap_area = computeIntersect(ref_scan, candidate_scan); + Size2 bb_candidate = candidate_scan->GetBoundingBox().GetSize(); + const double candidate_area = + bb_candidate.GetHeight() * bb_candidate.GetWidth(); + + return overlap_area / candidate_area; +} + +/*****************************************************************************/ +double LifelongSlamToolbox::computeReadingOverlapRatio( + LocalizedRangeScan* ref_scan, + LocalizedRangeScan* candidate_scan) +/*****************************************************************************/ +{ + const PointVectorDouble& pts = candidate_scan->GetPointReadings(true); + const int num_pts = pts.size(); + + // get the bounds of the intersect area + double x_l, x_u, y_l, y_u; + computeIntersectBounds(ref_scan, candidate_scan, x_l, x_u, y_l, y_u); + + PointVectorDouble::const_iterator pt_it; + int inner_pts = 0; + for (pt_it = pts.begin(); pt_it != pts.end(); ++pt_it) + { + if (pt_it->GetX() < x_u && pt_it->GetX() > x_l && + pt_it->GetY() < y_u && pt_it->GetY() > y_l) + { + inner_pts++; + } + } + + return double(inner_pts) / double(num_pts); +} + +} // end namespace diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox/src/experimental/slam_toolbox_lifelong_node.cpp b/Localizations/Packages/slam_toolbox/slam_toolbox/src/experimental/slam_toolbox_lifelong_node.cpp new file mode 100644 index 0000000..a22cc36 --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox/src/experimental/slam_toolbox_lifelong_node.cpp @@ -0,0 +1,45 @@ +/* + * slam_toolbox + * Copyright (c) 2019, Samsung Research America + * + * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE + * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY + * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS + * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED. + * + * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO + * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS + * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND + * CONDITIONS. + * + */ + +/* Author: Steven Macenski */ + +#include "slam_toolbox/experimental/slam_toolbox_lifelong.hpp" + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "slam_toolbox"); + ros::NodeHandle nh("~"); + ros::spinOnce(); + + int stack_size; + if (nh.getParam("stack_size_to_use", stack_size)) + { + ROS_INFO("Node using stack size %i", (int)stack_size); + const rlim_t max_stack_size = stack_size; + struct rlimit stack_limit; + getrlimit(RLIMIT_STACK, &stack_limit); + if (stack_limit.rlim_cur < stack_size) + { + stack_limit.rlim_cur = stack_size; + } + setrlimit(RLIMIT_STACK, &stack_limit); + } + + slam_toolbox::LifelongSlamToolbox llst(nh); + + ros::spin(); + return 0; +} diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox/src/laser_utils.cpp b/Localizations/Packages/slam_toolbox/slam_toolbox/src/laser_utils.cpp new file mode 100644 index 0000000..1fb0f6c --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox/src/laser_utils.cpp @@ -0,0 +1,230 @@ +/* + * laser_utils + * Copyright (c) 2019, Samsung Research America + * + * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE + * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY + * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS + * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED. + * + * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO + * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS + * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND + * CONDITIONS. + * + */ + +/* Author: Steven Macenski */ + +#include "slam_toolbox/laser_utils.hpp" +#include + +namespace laser_utils +{ + +LaserMetadata::LaserMetadata() +{ +}; + +LaserMetadata::~LaserMetadata() +{ +} + +LaserMetadata::LaserMetadata(karto::LaserRangeFinder* lsr, bool invert) +{ + laser = lsr; + inverted = invert; +}; + +bool LaserMetadata::isInverted() const +{ + return inverted; +} + +karto::LaserRangeFinder* LaserMetadata::getLaser() +{ + return laser; +} + +void LaserMetadata::invertScan(sensor_msgs::LaserScan& scan) const +{ + sensor_msgs::LaserScan temp; + temp.intensities.reserve(scan.intensities.size()); + temp.ranges.reserve(scan.ranges.size()); + const bool has_intensities = scan.intensities.size() > 0 ? true : false; + + for (int i = scan.ranges.size(); i != 0; i--) + { + temp.ranges.push_back(scan.ranges[i]); + if (has_intensities) + { + temp.intensities.push_back(scan.intensities[i]); + } + } + + scan.ranges = temp.ranges; + scan.intensities = temp.intensities; + return; +}; + + +LaserAssistant::LaserAssistant(ros::NodeHandle& nh, + tf2_ros::Buffer* tf, const std::string& base_frame) + : nh_(nh), tf_(tf), base_frame_(base_frame) +{ +}; + +LaserAssistant::~LaserAssistant() +{ +}; + +LaserMetadata LaserAssistant::toLaserMetadata(sensor_msgs::LaserScan scan) +{ + scan_ = scan; + frame_ = scan_.header.frame_id; + + double mountingYaw; + bool inverted = isInverted(mountingYaw); + karto::LaserRangeFinder* laser = makeLaser(mountingYaw); + LaserMetadata laserMeta(laser, inverted); + return laserMeta; +}; + +karto::LaserRangeFinder* LaserAssistant::makeLaser(const double& mountingYaw) +{ + karto::LaserRangeFinder* laser = + karto::LaserRangeFinder::CreateLaserRangeFinder( + karto::LaserRangeFinder_Custom, karto::Name("Custom Described Lidar")); + laser->SetOffsetPose(karto::Pose2(laser_pose_.transform.translation.x, + laser_pose_.transform.translation.y, mountingYaw)); + laser->SetMinimumRange(scan_.range_min); + laser->SetMaximumRange(scan_.range_max); + laser->SetMinimumAngle(scan_.angle_min); + laser->SetMaximumAngle(scan_.angle_max); + laser->SetAngularResolution(scan_.angle_increment); + + bool is_360_lidar = false; + const float angular_range = std::fabs(scan_.angle_max - scan_.angle_min); + if (std::fabs(angular_range - 2.0 * M_PI) < (scan_.angle_increment - (std::numeric_limits::epsilon() * 2.0f*M_PI))) { + is_360_lidar = true; + } + + // Check if we have a 360 laser, but incorrectly setup as to produce + // measurements in range [0, 360] rather than appropriately as [0, 360) + if (angular_range > 6.10865 /*350 deg*/ && (angular_range / scan_.angle_increment) + 1 == scan_.ranges.size()) { + is_360_lidar = false; + } + + laser->SetIs360Laser(is_360_lidar); + + double max_laser_range = 25; + nh_.getParam("max_laser_range", max_laser_range); + if (max_laser_range > scan_.range_max) + { + ROS_WARN("maximum laser range setting (%.1f m) exceeds the capabilities " + "of the used Lidar (%.1f m)", + max_laser_range, scan_.range_max); + max_laser_range = scan_.range_max; + } + laser->SetRangeThreshold(max_laser_range); + return laser; +} + +bool LaserAssistant::isInverted(double& mountingYaw) +{ + // geometry_msgs::TransformStamped laser_ident; + // laser_ident.header.stamp = scan_.header.stamp; + // laser_ident.header.frame_id = frame_; + // laser_ident.transform.rotation.w = 1.0; + + // laser_pose_ = tf_->transform(laser_ident, base_frame_); + // mountingYaw = tf2::getYaw(laser_pose_.transform.rotation); + + // ROS_DEBUG("laser %s's pose wrt base: %.3f %.3f %.3f %.3f", + // frame_.c_str(), laser_pose_.transform.translation.x, + // laser_pose_.transform.translation.y, + // laser_pose_.transform.translation.z, mountingYaw); + + // geometry_msgs::Vector3Stamped laser_orient; + // laser_orient.vector.z = laser_orient.vector.y = 0.; + // laser_orient.vector.z = 1 + laser_pose_.transform.translation.z; + // laser_orient.header.stamp = scan_.header.stamp; + // laser_orient.header.frame_id = base_frame_; + // laser_orient = tf_->transform(laser_orient, frame_); + + // if (laser_orient.vector.z <= 0) + // { + // ROS_DEBUG("laser is mounted upside-down"); + // return true; + // } + + bool result = false; + try + { + laser_pose_ = tf_->lookupTransform(base_frame_, frame_, ros::Time(0)); + mountingYaw = tf2::getYaw(laser_pose_.transform.rotation); + ROS_INFO("Transform from %s to %s: x=%f, y=%f, z=%f, yaw=%f", + frame_.c_str(), + base_frame_.c_str(), + laser_pose_.transform.translation.x, + laser_pose_.transform.translation.y, + laser_pose_.transform.translation.z, + mountingYaw); + auto quaternion = laser_pose_.transform.rotation; + // ROS_INFO("Quaternion: x=%f, y=%f, z=%f, w=%f", + // quaternion.x, quaternion.y, quaternion.z, quaternion.w); + tf2::Quaternion q( + quaternion.x, + quaternion.y, + quaternion.z, + quaternion.w + ); + double roll, pitch, yaw; + tf2::Matrix3x3(q).getRPY(roll, pitch, yaw); + ROS_INFO("Laser orientation: Roll: %f, Pitch: %f, Yaw: %f", roll, pitch, yaw); + if (abs(pitch) > 3.13 && abs(pitch) < 3.15) + { + ROS_WARN("Laser is mounted upside-down"); + result = true; + } + else + { + ROS_INFO("Laser orientation seems to be correct."); + result = false; + } + } + catch (tf2::TransformException &ex) + { + ROS_WARN("[TransformException] Could not get transform: %s", ex.what()); + } + return result; +}; + +ScanHolder::ScanHolder(std::map& lasers) +: lasers_(lasers) +{ + current_scans_ = std::make_unique >(); +}; + +ScanHolder::~ScanHolder() +{ + current_scans_.reset(); +}; + +sensor_msgs::LaserScan ScanHolder::getCorrectedScan(const int& id) +{ + sensor_msgs::LaserScan scan = current_scans_->at(id); + const laser_utils::LaserMetadata& laser = lasers_[scan.header.frame_id]; + if (laser.isInverted()) + { + laser.invertScan(scan); + } + return scan; +}; + +void ScanHolder::addScan(const sensor_msgs::LaserScan scan) +{ + current_scans_->push_back(scan); +}; + +} // end namespace diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox/src/loop_closure_assistant.cpp b/Localizations/Packages/slam_toolbox/slam_toolbox/src/loop_closure_assistant.cpp new file mode 100644 index 0000000..1098ef3 --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox/src/loop_closure_assistant.cpp @@ -0,0 +1,365 @@ +/* + * loop_closure_assistant + * Copyright (c) 2019, Samsung Research America + * + * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE + * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY + * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS + * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED. + * + * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO + * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS + * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND + * CONDITIONS. + * + */ + +/* Author: Steven Macenski */ + +#include "slam_toolbox/loop_closure_assistant.hpp" + +namespace loop_closure_assistant +{ + +/*****************************************************************************/ +LoopClosureAssistant::LoopClosureAssistant( + ros::NodeHandle& node, + karto::Mapper* mapper, + laser_utils::ScanHolder* scan_holder, + PausedState& state, ProcessType & processor_type) +: mapper_(mapper), scan_holder_(scan_holder), + interactive_mode_(false), nh_(node), state_(state), + processor_type_(processor_type) +/*****************************************************************************/ +{ + node.setParam("paused_processing", false); + tfB_ = std::make_unique(); + ssClear_manual_ = node.advertiseService("clear_changes", + &LoopClosureAssistant::clearChangesCallback, this); + ssLoopClosure_ = node.advertiseService("manual_loop_closure", + &LoopClosureAssistant::manualLoopClosureCallback, this); + scan_publisher_ = node.advertise( + "karto_scan_visualization",10); + solver_ = mapper_->getScanSolver(); + interactive_server_ = + std::make_unique( + "slam_toolbox","",true); + ssInteractive_ = node.advertiseService("toggle_interactive_mode", + &LoopClosureAssistant::interactiveModeCallback,this); + node.setParam("interactive_mode", interactive_mode_); + marker_publisher_ = node.advertise( + "karto_graph_visualization",1); + node.param("map_frame", map_frame_, std::string("map")); + node.param("enable_interactive_mode", enable_interactive_mode_, false); +} + +/*****************************************************************************/ +void LoopClosureAssistant::setMapper(karto::Mapper * mapper) +/*****************************************************************************/ +{ + mapper_ = mapper; +} + +/*****************************************************************************/ +void LoopClosureAssistant::processInteractiveFeedback(const + visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback) +/*****************************************************************************/ +{ + if (processor_type_ != PROCESS) + { + ROS_ERROR_THROTTLE(5., + "Interactive mode is invalid outside processing mode."); + return; + } + + const int id = std::stoi(feedback->marker_name, nullptr, 10) - 1; + + // was depressed, something moved, and now released + if (feedback->event_type == + visualization_msgs::InteractiveMarkerFeedback::MOUSE_UP && + feedback->mouse_point_valid) + { + addMovedNodes(id, Eigen::Vector3d(feedback->mouse_point.x, + feedback->mouse_point.y, tf2::getYaw(feedback->pose.orientation))); + } + + // is currently depressed, being moved before release + if (feedback->event_type == + visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE) + { + // get scan + sensor_msgs::LaserScan scan = scan_holder_->getCorrectedScan(id); + + // get correct orientation + tf2::Quaternion quat(0.,0.,0.,1.0), msg_quat(0.,0.,0.,1.0); + double node_yaw, first_node_yaw; + solver_->GetNodeOrientation(id, node_yaw); + solver_->GetNodeOrientation(0, first_node_yaw); + tf2::Quaternion q1(0.,0.,0.,1.0); + q1.setEuler(0., 0., node_yaw - 3.14159); + tf2::Quaternion q2(0.,0.,0.,1.0); + q2.setEuler(0., 0., 3.14159); + quat *= q1; + quat *= q2; + + // interactive move + tf2::convert(feedback->pose.orientation, msg_quat); + quat *= msg_quat; + quat.normalize(); + + // create correct transform + tf2::Transform transform; + transform.setOrigin(tf2::Vector3(feedback->pose.position.x, + feedback->pose.position.y, 0.)); + transform.setRotation(quat); + + // publish the scan visualization with transform + geometry_msgs::TransformStamped msg; + tf2::convert(transform, msg.transform); + msg.child_frame_id = "karto_scan_visualization"; + msg.header.frame_id = feedback->header.frame_id; + msg.header.stamp = ros::Time::now(); + tfB_->sendTransform(msg); + + scan.header.frame_id = "karto_scan_visualization"; + scan.header.stamp = ros::Time::now(); + scan_publisher_.publish(scan); + } +} + +/*****************************************************************************/ +void LoopClosureAssistant::publishGraph() +/*****************************************************************************/ +{ + interactive_server_->clear(); + karto::MapperGraph * graph = mapper_->GetGraph(); + + if (!graph || graph->GetVertices().empty()) + { + return; + } + + using ConstVertexMapIterator = + karto::MapperGraph::VertexMap::const_iterator; + const karto::MapperGraph::VertexMap& vertices = graph->GetVertices(); + + int graph_size = 0; + for (ConstVertexMapIterator it = vertices.begin(); it != vertices.end(); ++it) + { + graph_size += it->second.size(); + } + ROS_DEBUG("Graph size: %i", graph_size); + + bool interactive_mode = false; + { + boost::mutex::scoped_lock lock(interactive_mutex_); + interactive_mode = interactive_mode_; + } + + const size_t current_marker_count = marker_array_.markers.size(); + marker_array_.markers.clear(); // restart the marker count + + visualization_msgs::Marker vertex_marker = + vis_utils::toVertexMarker(map_frame_, "slam_toolbox", 0.1); + + for (ConstVertexMapIterator outer_it = vertices.begin(); + outer_it != vertices.end(); ++outer_it) + { + using ConstVertexMapValueIterator = + karto::MapperGraph::VertexMap::mapped_type::const_iterator; + for (ConstVertexMapValueIterator inner_it = outer_it->second.begin(); + inner_it != outer_it->second.end(); ++inner_it) + { + karto::LocalizedRangeScan * scan = inner_it->second->GetObject(); + + vertex_marker.pose.position.x = scan->GetCorrectedPose().GetX(); + vertex_marker.pose.position.y = scan->GetCorrectedPose().GetY(); + + if (interactive_mode && enable_interactive_mode_) + { + // need a 1-to-1 mapping between marker IDs and + // scan unique IDs to process interactive feedback + vertex_marker.id = scan->GetUniqueId() + 1; + visualization_msgs::InteractiveMarker int_marker = + vis_utils::toInteractiveMarker(vertex_marker, 0.3); + interactive_server_->insert(int_marker, + boost::bind( + &LoopClosureAssistant::processInteractiveFeedback, + this, _1)); + } + else + { + // use monotonically increasing vertex marker IDs to + // make room for edge marker IDs + vertex_marker.id = marker_array_.markers.size(); + marker_array_.markers.push_back(vertex_marker); + } + } + } + + if (!interactive_mode) + { + using EdgeList = std::vector*>; + using ConstEdgeListIterator = EdgeList::const_iterator; + + visualization_msgs::Marker edge_marker = + vis_utils::toEdgeMarker(map_frame_, "slam_toolbox", 0.05); + + const EdgeList& edges = graph->GetEdges(); + for (ConstEdgeListIterator it = edges.begin(); it != edges.end(); ++it) + { + const karto::Edge * edge = *it; + karto::LocalizedRangeScan * source_scan = edge->GetSource()->GetObject(); + karto::LocalizedRangeScan * target_scan = edge->GetTarget()->GetObject(); + const karto::Pose2 source_pose = source_scan->GetCorrectedPose(); + const karto::Pose2 target_pose = target_scan->GetCorrectedPose(); + + edge_marker.id = marker_array_.markers.size(); + edge_marker.points[0].x = source_pose.GetX(); + edge_marker.points[0].y = source_pose.GetY(); + edge_marker.points[1].x = target_pose.GetX(); + edge_marker.points[1].y = target_pose.GetY(); + marker_array_.markers.push_back(edge_marker); + } + } + + const size_t next_marker_count = marker_array_.markers.size(); + + // append preexisting markers to force deletion + while (marker_array_.markers.size() < current_marker_count) + { + visualization_msgs::Marker deleted_marker; + deleted_marker.id = marker_array_.markers.size(); + deleted_marker.action = visualization_msgs::Marker::DELETE; + marker_array_.markers.push_back(deleted_marker); + } + + // if disabled, clears out old markers + interactive_server_->applyChanges(); + marker_publisher_.publish(marker_array_); + + // drop trailing deleted markers + marker_array_.markers.resize(next_marker_count); + return; +} + +/*****************************************************************************/ +bool LoopClosureAssistant::manualLoopClosureCallback( + slam_toolbox_msgs::LoopClosure::Request& req, + slam_toolbox_msgs::LoopClosure::Response& resp) +/*****************************************************************************/ +{ + if(!enable_interactive_mode_) + { + ROS_WARN("Called manual loop closure" + " with interactive mode disabled. Ignoring."); + return false; + } + + { + boost::mutex::scoped_lock lock(moved_nodes_mutex_); + + if (moved_nodes_.size() == 0) + { + ROS_WARN("No moved nodes to attempt manual loop closure."); + return true; + } + + ROS_INFO("LoopClosureAssistant: Attempting to manual " + "loop close with %i moved nodes.", (int)moved_nodes_.size()); + // for each in node map + std::map::const_iterator it = moved_nodes_.begin(); + for (it; it != moved_nodes_.end(); ++it) + { + moveNode(it->first, + Eigen::Vector3d(it->second(0),it->second(1), it->second(2))); + } + } + + // optimize + mapper_->CorrectPoses(); + + // update visualization and clear out nodes completed + publishGraph(); + clearMovedNodes(); + return true; +} + +/*****************************************************************************/ +bool LoopClosureAssistant::interactiveModeCallback( + slam_toolbox_msgs::ToggleInteractive::Request &req, + slam_toolbox_msgs::ToggleInteractive::Response &resp) +/*****************************************************************************/ +{ + if(!enable_interactive_mode_) + { + ROS_WARN("Called toggle interactive mode with " + "interactive mode disabled. Ignoring."); + return false; + } + + bool interactive_mode; + { + boost::mutex::scoped_lock lock_i(interactive_mutex_); + interactive_mode_ = !interactive_mode_; + interactive_mode = interactive_mode_; + nh_.setParam("interactive_mode", interactive_mode_); + } + + ROS_INFO("SlamToolbox: Toggling %s interactive mode.", + interactive_mode ? "on" : "off"); + publishGraph(); + clearMovedNodes(); + + // set state so we don't overwrite changes in rviz while loop closing + state_.set(PROCESSING, interactive_mode); + state_.set(VISUALIZING_GRAPH, interactive_mode); + nh_.setParam("paused_processing", interactive_mode); + return true; +} + +/*****************************************************************************/ +void LoopClosureAssistant::moveNode( + const int& id, const Eigen::Vector3d& pose) +/*****************************************************************************/ +{ + solver_->ModifyNode(id, pose); +} + +/*****************************************************************************/ +bool LoopClosureAssistant::clearChangesCallback( + slam_toolbox_msgs::Clear::Request& req, + slam_toolbox_msgs::Clear::Response& resp) +/*****************************************************************************/ +{ + if(!enable_interactive_mode_) + { + ROS_WARN("Called Clear changes with interactive mode disabled. Ignoring."); + return false; + } + + ROS_INFO("LoopClosureAssistant: Clearing manual loop closure nodes."); + publishGraph(); + clearMovedNodes(); + return true; +} + +/*****************************************************************************/ +void LoopClosureAssistant::clearMovedNodes() +/*****************************************************************************/ +{ + boost::mutex::scoped_lock lock(moved_nodes_mutex_); + moved_nodes_.clear(); +} + +/*****************************************************************************/ +void LoopClosureAssistant::addMovedNodes(const int& id, Eigen::Vector3d vec) +/*****************************************************************************/ +{ + ROS_INFO("LoopClosureAssistant: Node %i new manual loop closure " + "pose has been recorded.",id); + boost::mutex::scoped_lock lock(moved_nodes_mutex_); + moved_nodes_[id] = vec; +} + +} // end namespace diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox/src/map_saver.cpp b/Localizations/Packages/slam_toolbox/slam_toolbox/src/map_saver.cpp new file mode 100644 index 0000000..e43cbe6 --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox/src/map_saver.cpp @@ -0,0 +1,68 @@ +/* + * map_saver + * Copyright (c) 2019, Samsung Research America + * + * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE + * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY + * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS + * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED. + * + * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO + * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS + * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND + * CONDITIONS. + * + */ + +/* Author: Steven Macenski */ + +#include "slam_toolbox/map_saver.hpp" + +namespace map_saver +{ + +/*****************************************************************************/ +MapSaver::MapSaver(ros::NodeHandle & nh, const std::string& map_name) +: nh_(nh), map_name_(map_name), received_map_(false) +/*****************************************************************************/ +{ + server_ = nh_.advertiseService("save_map", &MapSaver::saveMapCallback, this); + sub_ = nh_.subscribe(map_name_, 1, &MapSaver::mapCallback, this); +} + +/*****************************************************************************/ +void MapSaver::mapCallback(const nav_msgs::OccupancyGrid& msg) +/*****************************************************************************/ +{ + received_map_ = true; +} + +/*****************************************************************************/ +bool MapSaver::saveMapCallback( + slam_toolbox_msgs::SaveMap::Request& req, + slam_toolbox_msgs::SaveMap::Response& resp) +/*****************************************************************************/ +{ + if (!received_map_) + { + ROS_WARN("Map Saver: Cannot save map, no map yet received on topic %s.", + map_name_.c_str()); + return false; + } + + const std::string name = req.name.data; + if (name != "") + { + ROS_INFO("SlamToolbox: Saving map as %s.", name.c_str()); + int rc = system(("rosrun map_server map_saver -f " + name).c_str()); + } + else + { + ROS_INFO("SlamToolbox: Saving map in current directory."); + int rc = system("rosrun map_server map_saver"); + } + ros::Duration(1.0).sleep(); + return true; +} + +} // end namespace \ No newline at end of file diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox/src/merge_maps_kinematic.cpp b/Localizations/Packages/slam_toolbox/slam_toolbox/src/merge_maps_kinematic.cpp new file mode 100644 index 0000000..b147ad1 --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox/src/merge_maps_kinematic.cpp @@ -0,0 +1,367 @@ +/* + * Author + * Copyright (c) 2018, Simbe Robotics, Inc. + * + * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE + * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY + * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS + * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED. + * + * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO + * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS + * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND + * CONDITIONS. + * + */ + +/* Author: Steven Macenski */ + +#include "slam_toolbox/merge_maps_kinematic.hpp" +#include "slam_toolbox/serialization.hpp" + +/*****************************************************************************/ +MergeMapsKinematic::MergeMapsKinematic() : num_submaps_(0), nh_("map_merging") +/*****************************************************************************/ +{ + ROS_INFO("MergeMapsKinematic: Starting up!"); + setup(); +} + +/*****************************************************************************/ +void MergeMapsKinematic::setup() +/*****************************************************************************/ +{ + nh_.param("resolution", resolution_, 0.05); + sstS_.push_back(nh_.advertise("/map", 1, true)); + sstmS_.push_back(nh_.advertise( + "/map_metadata", 1, true)); + ssMap_ = nh_.advertiseService("merge_submaps", + &MergeMapsKinematic::mergeMapCallback, this); + ssSubmap_ = nh_.advertiseService("add_submap", + &MergeMapsKinematic::addSubmapCallback, this); + + tfB_ = std::make_unique(); + + interactive_server_ = + std::make_unique( + "merge_maps_tool","",true); +} + +/*****************************************************************************/ +MergeMapsKinematic::~MergeMapsKinematic() +/*****************************************************************************/ +{ +} + +/*****************************************************************************/ +bool MergeMapsKinematic::addSubmapCallback( + slam_toolbox_msgs::AddSubmap::Request& req, + slam_toolbox_msgs::AddSubmap::Response& resp) +/*****************************************************************************/ +{ + std::unique_ptr mapper = std::make_unique(); + std::unique_ptr dataset = std::make_unique(); + + if (!serialization::read(req.filename, *mapper, *dataset)) + { + ROS_ERROR("addSubmapCallback: Failed to read " + "file: %s.", req.filename.c_str()); + return true; + } + + // we know the position because we put it there before any scans + karto::LaserRangeFinder* laser = dynamic_cast( + dataset->GetLasers()[0]); + dataset->Add(laser, true); + dataset_vec_.push_back(std::move(dataset)); + + if (lasers_.find(laser->GetName().GetName()) == lasers_.end()) + { + laser_utils::LaserMetadata laserMeta(laser, false); + lasers_[laser->GetName().GetName()] = laserMeta; + } + + karto::LocalizedRangeScanVector scans = mapper->GetAllProcessedScans(); + scans_vec_.push_back(scans); + num_submaps_++; + + // create and publish map with marker that will move the map around + sstS_.push_back(nh_.advertise( + "/map_"+std::to_string(num_submaps_), 1, true)); + sstmS_.push_back(nh_.advertise( + "/map_metadata_" + std::to_string(num_submaps_), 1, true)); + sleep(1.0); + + nav_msgs::GetMap::Response map; + nav_msgs::OccupancyGrid& og = map.map; + try + { + kartoToROSOccupancyGrid(scans, map); + } catch (const karto::Exception& e) + { + ROS_WARN("Failed to build grid to add submap, Exception: %s", + e.GetErrorMessage().c_str()); + return false; + } + + tf2::Transform transform; + transform.setIdentity(); + transform.setOrigin(tf2::Vector3(og.info.origin.position.x + + og.info.width * og.info.resolution / 2.0, + og.info.origin.position.y + og.info.height * og.info.resolution / 2.0, + 0.)); + og.info.origin.position.x = - (og.info.width * og.info.resolution / 2.0); + og.info.origin.position.y = - (og.info.height * og.info.resolution / 2.0); + og.header.stamp = ros::Time::now(); + og.header.frame_id = "map_"+std::to_string(num_submaps_); + sstS_[num_submaps_].publish(og); + sstmS_[num_submaps_].publish(og.info); + + geometry_msgs::TransformStamped msg; + tf2::convert(transform, msg.transform); + msg.child_frame_id = "/map_"+std::to_string(num_submaps_); + msg.header.frame_id = "/map"; + msg.header.stamp = ros::Time::now(); + tfB_->sendTransform(msg); + + submap_marker_transform_[num_submaps_] = + tf2::Transform(tf2::Quaternion(0.,0.,0.,1.0), + tf2::Vector3(0,0,0)); //no initial correction -- identity mat + submap_locations_[num_submaps_] = + Eigen::Vector3d(transform.getOrigin().getX(), + transform.getOrigin().getY(),0.); + + // create an interactive marker for the base of this frame and attach it + visualization_msgs::Marker m = + vis_utils::toVertexMarker("map", "merge_maps_tool", 2.0); + m.pose.position.x = transform.getOrigin().getX(); + m.pose.position.y = transform.getOrigin().getY(); + m.id = num_submaps_; + + visualization_msgs::InteractiveMarker int_marker = + vis_utils::toInteractiveMarker(m, 2.4); + interactive_server_->insert(int_marker, + boost::bind(&MergeMapsKinematic::processInteractiveFeedback, this, _1)); + interactive_server_->applyChanges(); + + ROS_INFO("Map %s was loaded into topic %s!", req.filename.c_str(), + ("/map_"+std::to_string(num_submaps_)).c_str()); + return true; +} + +/*****************************************************************************/ +karto::Pose2 MergeMapsKinematic::applyCorrection(const + karto::Pose2& pose, + const tf2::Transform& submap_correction) +/*****************************************************************************/ +{ + tf2::Transform pose_tf, pose_corr; + tf2::Quaternion q(0.,0.,0.,1.0); + q.setRPY(0., 0., pose.GetHeading()); + pose_tf.setOrigin(tf2::Vector3(pose.GetX(), pose.GetY(), 0.)); + pose_tf.setRotation(q); + pose_corr = submap_correction * pose_tf; + return karto::Pose2(pose_corr.getOrigin().x(), pose_corr.getOrigin().y(), + tf2::getYaw(pose_corr.getRotation())); +} + +/*****************************************************************************/ +karto::Vector2 MergeMapsKinematic::applyCorrection(const + karto::Vector2& pose, + const tf2::Transform& submap_correction) +/*****************************************************************************/ +{ + tf2::Transform pose_tf, pose_corr; + pose_tf.setOrigin(tf2::Vector3(pose.GetX(), pose.GetY(), 0.)); + pose_tf.setRotation(tf2::Quaternion(0.,0.,0.,1.0)); + pose_corr = submap_correction * pose_tf; + return karto::Vector2(pose_corr.getOrigin().x(), + pose_corr.getOrigin().y()); +} + +/*****************************************************************************/ +void MergeMapsKinematic::transformScan(LocalizedRangeScansIt iter, + tf2::Transform& submap_correction) +/*****************************************************************************/ +{ + // TRANSFORM BARYCENTERR POSE + const karto::Pose2 bary_center_pose = (*iter)->GetBarycenterPose(); + auto bary_center_pose_corr = + applyCorrection(bary_center_pose, submap_correction); + (*iter)->SetBarycenterPose(bary_center_pose_corr); + + // TRANSFORM BOUNDING BOX POSITIONS + karto::BoundingBox2 bbox = (*iter)->GetBoundingBox(); + const karto::Vector2 bbox_min_corr = + applyCorrection(bbox.GetMinimum(), submap_correction); + bbox.SetMinimum(bbox_min_corr); + const karto::Vector2 bbox_max_corr = + applyCorrection(bbox.GetMaximum(), submap_correction); + bbox.SetMaximum(bbox_max_corr); + (*iter)->SetBoundingBox(bbox); + + // TRANSFORM UNFILTERED POINTS USED + karto::PointVectorDouble UPR_vec = (*iter)->GetPointReadings(); + for(karto::PointVectorDouble::iterator it_upr = UPR_vec.begin(); + it_upr != UPR_vec.end(); ++it_upr) + { + const karto::Vector2 upr_corr = applyCorrection( + *it_upr, submap_correction); + it_upr->SetX(upr_corr.GetX()); + it_upr->SetY(upr_corr.GetY()); + } + (*iter)->SetPointReadings(UPR_vec); + + // TRANSFORM CORRECTED POSE + const karto::Pose2 corrected_pose = (*iter)->GetCorrectedPose(); + karto::Pose2 karto_robot_pose_corr = applyCorrection( + corrected_pose, submap_correction); + (*iter)->SetCorrectedPose(karto_robot_pose_corr); + kt_bool dirty = false; + (*iter)->SetIsDirty(dirty); + + // TRANSFORM ODOM POSE + karto::Pose2 odom_pose = (*iter)->GetOdometricPose(); + karto::Pose2 karto_robot_pose_odom = applyCorrection( + odom_pose, submap_correction); + (*iter)->SetOdometricPose(karto_robot_pose_odom); +} + +/*****************************************************************************/ +bool MergeMapsKinematic::mergeMapCallback( + slam_toolbox_msgs::MergeMaps::Request& req, + slam_toolbox_msgs::MergeMaps::Response& resp) +/*****************************************************************************/ +{ + ROS_INFO("Merging maps!"); + + // transform all the scans into the new global map coordinates + int id = 0; + karto::LocalizedRangeScanVector transformed_scans; + for(LocalizedRangeScansVecIt it_LRV = scans_vec_.begin(); + it_LRV != scans_vec_.end(); ++it_LRV) + { + id++; + for (LocalizedRangeScansIt iter = it_LRV->begin(); + iter != it_LRV->end(); ++iter) + { + tf2::Transform submap_correction = submap_marker_transform_[id]; + transformScan(iter, submap_correction); + transformed_scans.push_back((*iter)); + } + } + + // create the map + nav_msgs::GetMap::Response map; + try + { + kartoToROSOccupancyGrid(transformed_scans, map); + } catch (const karto::Exception& e) + { + ROS_WARN("Failed to build grid to merge maps together, Exception: %s", + e.GetErrorMessage().c_str()); + } + + // publish + map.map.header.stamp = ros::Time::now(); + map.map.header.frame_id = "map"; + sstS_[0].publish(map.map); + sstmS_[0].publish(map.map.info); + return true; +} + +/*****************************************************************************/ +void MergeMapsKinematic::kartoToROSOccupancyGrid( + const karto::LocalizedRangeScanVector& scans, + nav_msgs::GetMap::Response& map) +/*****************************************************************************/ +{ + karto::OccupancyGrid* occ_grid = NULL; + occ_grid = karto::OccupancyGrid::CreateFromScans(scans, resolution_); + if (!occ_grid) + { + ROS_INFO("MergeMapsKinematic: Could not make Karto occupancy grid."); + } + else + { + map.map.info.resolution = resolution_; + vis_utils::toNavMap(occ_grid, map.map); + } + + delete occ_grid; + return; +} + +/*****************************************************************************/ +void MergeMapsKinematic::processInteractiveFeedback(const + visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback) +/*****************************************************************************/ +{ + const int id = std::stoi(feedback->marker_name,nullptr,10); + + if (feedback->event_type == + visualization_msgs::InteractiveMarkerFeedback::MOUSE_UP && + feedback->mouse_point_valid) + { + tfScalar yaw = tf2::getYaw(feedback->pose.orientation); + tf2::Quaternion quat(0.,0.,0.,1.0); + tf2::convert(feedback->pose.orientation, quat); // relative + + tf2::Transform previous_submap_correction; + previous_submap_correction.setIdentity(); + previous_submap_correction.setOrigin(tf2::Vector3(submap_locations_[id](0), + submap_locations_[id](1), 0.)); + + // update internal knowledge of submap locations + submap_locations_[id] = Eigen::Vector3d(feedback->pose.position.x, + feedback->pose.position.y, + submap_locations_[id](2) + yaw); + + // add the map_N frame there + tf2::Transform new_submap_location; + new_submap_location.setOrigin(tf2::Vector3(submap_locations_[id](0), + submap_locations_[id](1), 0.)); + new_submap_location.setRotation(quat); + + geometry_msgs::TransformStamped msg; + tf2::convert(new_submap_location, msg.transform); + msg.child_frame_id = "/map_"+std::to_string(id); + msg.header.frame_id = "/map"; + msg.header.stamp = ros::Time::now(); + tfB_->sendTransform(msg); + + submap_marker_transform_[id] = submap_marker_transform_[id] * + previous_submap_correction.inverse() * new_submap_location; + } + + if (feedback->event_type == + visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE) + { + tfScalar yaw = tf2::getYaw(feedback->pose.orientation); + tf2::Quaternion quat(0.,0.,0.,1.0); + tf2::convert(feedback->pose.orientation, quat); // relative + + // add the map_N frame there + tf2::Transform new_submap_location; + new_submap_location.setOrigin(tf2::Vector3(feedback->pose.position.x, + feedback->pose.position.y, 0.)); + new_submap_location.setRotation(quat); + + geometry_msgs::TransformStamped msg; + tf2::convert(new_submap_location, msg.transform); + msg.child_frame_id = "/map_"+std::to_string(id); + msg.header.frame_id = "/map"; + msg.header.stamp = ros::Time::now(); + tfB_->sendTransform(msg); + } +} + +/*****************************************************************************/ +int main(int argc, char** argv) +/*****************************************************************************/ +{ + ros::init(argc, argv, "merge_maps_kinematic"); + MergeMapsKinematic mmk; + ros::spin(); + return 0; +} diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox/src/slam_mapper.cpp b/Localizations/Packages/slam_toolbox/slam_toolbox/src/slam_mapper.cpp new file mode 100644 index 0000000..d808d8f --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox/src/slam_mapper.cpp @@ -0,0 +1,289 @@ +/* + * slam_mapper + * Copyright (c) 2018, Simbe Robotics + * Copyright (c) 2018, Steve Macenski + * Copyright (c) 2019, Samsung Research America + * + * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE + * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY + * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS + * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED. + * + * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO + * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS + * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND + * CONDITIONS. + * + */ + +/* Author: Steven Macenski */ + +#include "slam_toolbox/slam_mapper.hpp" + +namespace mapper_utils +{ + +/*****************************************************************************/ +SMapper::SMapper() +/*****************************************************************************/ +{ + mapper_ = std::make_unique(); +} + +/*****************************************************************************/ +SMapper::~SMapper() +/*****************************************************************************/ +{ + mapper_.reset(); +} + +/*****************************************************************************/ +karto::Mapper* SMapper::getMapper() +/*****************************************************************************/ +{ + return mapper_.get(); +} + +/*****************************************************************************/ +void SMapper::setMapper(karto::Mapper* mapper) +/*****************************************************************************/ +{ + mapper_.reset(mapper); +} + +/*****************************************************************************/ +void SMapper::clearLocalizationBuffer() +/*****************************************************************************/ +{ + mapper_->ClearLocalizationBuffer(); +} + +/*****************************************************************************/ +karto::OccupancyGrid* SMapper::getOccupancyGrid(const double& resolution) +/*****************************************************************************/ +{ + karto::OccupancyGrid* occ_grid = nullptr; + return karto::OccupancyGrid::CreateFromScans(mapper_->GetAllProcessedScans(), + resolution); +} + +/*****************************************************************************/ +tf2::Transform SMapper::toTfPose(const karto::Pose2& pose) const +/*****************************************************************************/ +{ + tf2::Transform new_pose; + new_pose.setOrigin(tf2::Vector3(pose.GetX(), pose.GetY(), 0.)); + tf2::Quaternion q; + q.setRPY(0., 0., pose.GetHeading()); + new_pose.setRotation(q); + return new_pose; +}; + +/*****************************************************************************/ +karto::Pose2 SMapper::toKartoPose(const tf2::Transform& pose) const +/*****************************************************************************/ +{ + karto::Pose2 transformed_pose; + transformed_pose.SetX(pose.getOrigin().x()); + transformed_pose.SetY(pose.getOrigin().y()); + transformed_pose.SetHeading(tf2::getYaw(pose.getRotation())); + return transformed_pose; +}; + +/*****************************************************************************/ +void SMapper::configure(const ros::NodeHandle& nh) +/*****************************************************************************/ +{ + bool use_scan_matching; + if(nh.getParam("use_scan_matching", use_scan_matching)) + { + mapper_->setParamUseScanMatching(use_scan_matching); + } + + bool use_scan_barycenter; + if(nh.getParam("use_scan_barycenter", use_scan_barycenter)) + { + mapper_->setParamUseScanBarycenter(use_scan_barycenter); + } + + double minimum_travel_distance = 0.5; + if(nh.getParam("minimum_travel_distance", minimum_travel_distance)) + { + mapper_->setParamMinimumTravelDistance(minimum_travel_distance); + } + + double minimum_travel_heading; + if(nh.getParam("minimum_travel_heading", minimum_travel_heading)) + { + mapper_->setParamMinimumTravelHeading(minimum_travel_heading); + } + + int scan_buffer_size; + if(nh.getParam("scan_buffer_size", scan_buffer_size)) + { + mapper_->setParamScanBufferSize(scan_buffer_size); + } + + double scan_buffer_maximum_scan_distance; + if(nh.getParam("scan_buffer_maximum_scan_distance", + scan_buffer_maximum_scan_distance)) + { + mapper_->setParamScanBufferMaximumScanDistance(scan_buffer_maximum_scan_distance); + } + + double link_match_minimum_response_fine; + if(nh.getParam("link_match_minimum_response_fine", + link_match_minimum_response_fine)) + { + mapper_->setParamLinkMatchMinimumResponseFine(link_match_minimum_response_fine); + } + + double link_scan_maximum_distance; + if(nh.getParam("link_scan_maximum_distance", link_scan_maximum_distance)) + { + mapper_->setParamLinkScanMaximumDistance(link_scan_maximum_distance); + } + + double loop_search_maximum_distance; + if(nh.getParam("loop_search_maximum_distance", loop_search_maximum_distance)) + { + mapper_->setParamLoopSearchMaximumDistance(loop_search_maximum_distance); + } + + bool do_loop_closing; + if(nh.getParam("do_loop_closing", do_loop_closing)) + { + mapper_->setParamDoLoopClosing(do_loop_closing); + } + + int loop_match_minimum_chain_size; + if(nh.getParam("loop_match_minimum_chain_size", + loop_match_minimum_chain_size)) + { + mapper_->setParamLoopMatchMinimumChainSize(loop_match_minimum_chain_size); + } + + double loop_match_maximum_variance_coarse; + if(nh.getParam("loop_match_maximum_variance_coarse", + loop_match_maximum_variance_coarse)) + { + mapper_->setParamLoopMatchMaximumVarianceCoarse(loop_match_maximum_variance_coarse); + } + + double loop_match_minimum_response_coarse; + if(nh.getParam("loop_match_minimum_response_coarse", + loop_match_minimum_response_coarse)) + { + mapper_->setParamLoopMatchMinimumResponseCoarse(loop_match_minimum_response_coarse); + } + + double loop_match_minimum_response_fine; + if(nh.getParam("loop_match_minimum_response_fine", + loop_match_minimum_response_fine)) + { + mapper_->setParamLoopMatchMinimumResponseFine(loop_match_minimum_response_fine); + } + + // Setting Correlation Parameters + double correlation_search_space_dimension; + if(nh.getParam("correlation_search_space_dimension", + correlation_search_space_dimension)) + { + mapper_->setParamCorrelationSearchSpaceDimension(correlation_search_space_dimension); + } + + double correlation_search_space_resolution; + if(nh.getParam("correlation_search_space_resolution", + correlation_search_space_resolution)) + { + mapper_->setParamCorrelationSearchSpaceResolution(correlation_search_space_resolution); + } + + double correlation_search_space_smear_deviation; + if(nh.getParam("correlation_search_space_smear_deviation", + correlation_search_space_smear_deviation)) + { + mapper_->setParamCorrelationSearchSpaceSmearDeviation( + correlation_search_space_smear_deviation); + } + + // Setting Correlation Parameters, Loop Closure Parameters + double loop_search_space_dimension; + if(nh.getParam("loop_search_space_dimension", loop_search_space_dimension)) + { + mapper_->setParamLoopSearchSpaceDimension(loop_search_space_dimension); + } + + double loop_search_space_resolution; + if(nh.getParam("loop_search_space_resolution", loop_search_space_resolution)) + { + mapper_->setParamLoopSearchSpaceResolution(loop_search_space_resolution); + } + + double loop_search_space_smear_deviation; + if(nh.getParam("loop_search_space_smear_deviation", + loop_search_space_smear_deviation)) + { + mapper_->setParamLoopSearchSpaceSmearDeviation(loop_search_space_smear_deviation); + } + + // Setting Scan Matcher Parameters + double distance_variance_penalty; + if(nh.getParam("distance_variance_penalty", distance_variance_penalty)) + { + mapper_->setParamDistanceVariancePenalty(distance_variance_penalty); + } + + double angle_variance_penalty; + if(nh.getParam("angle_variance_penalty", angle_variance_penalty)) + { + mapper_->setParamAngleVariancePenalty(angle_variance_penalty); + } + + double fine_search_angle_offset; + if(nh.getParam("fine_search_angle_offset", fine_search_angle_offset)) + { + mapper_->setParamFineSearchAngleOffset(fine_search_angle_offset); + } + + double coarse_search_angle_offset; + if(nh.getParam("coarse_search_angle_offset", coarse_search_angle_offset)) + { + mapper_->setParamCoarseSearchAngleOffset(coarse_search_angle_offset); + } + + double coarse_angle_resolution; + if(nh.getParam("coarse_angle_resolution", coarse_angle_resolution)) + { + mapper_->setParamCoarseAngleResolution(coarse_angle_resolution); + } + + double minimum_angle_penalty; + if(nh.getParam("minimum_angle_penalty", minimum_angle_penalty)) + { + mapper_->setParamMinimumAnglePenalty(minimum_angle_penalty); + } + + double minimum_distance_penalty; + if(nh.getParam("minimum_distance_penalty", minimum_distance_penalty)) + { + mapper_->setParamMinimumDistancePenalty(minimum_distance_penalty); + } + + bool use_response_expansion; + if(nh.getParam("use_response_expansion", use_response_expansion)) + { + mapper_->setParamUseResponseExpansion(use_response_expansion); + } + return; +} + +/*****************************************************************************/ +void SMapper::Reset() +/*****************************************************************************/ +{ + mapper_->Reset(); + return; +} + +} // end namespace diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox/src/slam_toolbox_async.cpp b/Localizations/Packages/slam_toolbox/slam_toolbox/src/slam_toolbox_async.cpp new file mode 100644 index 0000000..87de834 --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox/src/slam_toolbox_async.cpp @@ -0,0 +1,75 @@ +/* + * slam_toolbox + * Copyright Work Modifications (c) 2018, Simbe Robotics, Inc. + * Copyright Work Modifications (c) 2019, Steve Macenski + * + * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE + * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY + * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS + * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED. + * + * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO + * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS + * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND + * CONDITIONS. + * + */ + +/* Author: Steven Macenski */ + +#include "slam_toolbox/slam_toolbox_async.hpp" + +namespace slam_toolbox +{ + +/*****************************************************************************/ +AsynchronousSlamToolbox::AsynchronousSlamToolbox(ros::NodeHandle& nh) +: SlamToolbox(nh) +/*****************************************************************************/ +{ + loadPoseGraphByParams(nh); +} + +/*****************************************************************************/ +void AsynchronousSlamToolbox::laserCallback( + const sensor_msgs::LaserScan::ConstPtr& scan) +/*****************************************************************************/ +{ + // no odom info + karto::Pose2 pose; + if(!pose_helper_->getOdomPose(pose, scan->header.stamp)) + { + return; + } + + // ensure the laser can be used + karto::LaserRangeFinder* laser = getLaser(scan); + + if(!laser) + { + ROS_WARN_THROTTLE(5., "Failed to create laser device for" + " %s; discarding scan", scan->header.frame_id.c_str()); + return; + } + + addScan(laser, scan, pose); + return; +} + +/*****************************************************************************/ +bool AsynchronousSlamToolbox::deserializePoseGraphCallback( + slam_toolbox_msgs::DeserializePoseGraph::Request& req, + slam_toolbox_msgs::DeserializePoseGraph::Response& resp) +/*****************************************************************************/ +{ + if (req.match_type == procType::LOCALIZE_AT_POSE) + { + ROS_ERROR("Requested a localization deserialization " + "in non-localization mode."); + return false; + } + + return SlamToolbox::deserializePoseGraphCallback(req, resp); +} + +} // end namespace diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox/src/slam_toolbox_async_node.cpp b/Localizations/Packages/slam_toolbox/slam_toolbox/src/slam_toolbox_async_node.cpp new file mode 100644 index 0000000..297b622 --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox/src/slam_toolbox_async_node.cpp @@ -0,0 +1,46 @@ +/* + * slam_toolbox + * Copyright Work Modifications (c) 2018, Simbe Robotics, Inc. + * Copyright Work Modifications (c) 2019, Steve Macenski + * + * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE + * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY + * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS + * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED. + * + * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO + * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS + * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND + * CONDITIONS. + * + */ + +/* Author: Steven Macenski */ + +#include "slam_toolbox/slam_toolbox_async.hpp" + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "slam_toolbox"); + ros::NodeHandle nh("~"); + ros::spinOnce(); + + int stack_size; + if (nh.getParam("stack_size_to_use", stack_size)) + { + ROS_INFO("Node using stack size %i", (int)stack_size); + const rlim_t max_stack_size = stack_size; + struct rlimit stack_limit; + getrlimit(RLIMIT_STACK, &stack_limit); + if (stack_limit.rlim_cur < stack_size) + { + stack_limit.rlim_cur = stack_size; + } + setrlimit(RLIMIT_STACK, &stack_limit); + } + + slam_toolbox::AsynchronousSlamToolbox sst(nh); + + ros::spin(); + return 0; +} diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox/src/slam_toolbox_common.cpp b/Localizations/Packages/slam_toolbox/slam_toolbox/src/slam_toolbox_common.cpp new file mode 100644 index 0000000..4b83689 --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox/src/slam_toolbox_common.cpp @@ -0,0 +1,850 @@ +/* + * slam_toolbox + * Copyright Work Modifications (c) 2018, Simbe Robotics, Inc. + * Copyright Work Modifications (c) 2019, Samsung Research America + * + * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE + * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY + * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS + * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED. + * + * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO + * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS + * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND + * CONDITIONS. + * + */ + + /* Orginal Author for slam_karto the original work was based on: Brian Gerkey */ + /* Author: Steven Macenski */ + +#include "slam_toolbox/slam_toolbox_common.hpp" +#include "slam_toolbox/serialization.hpp" + +namespace slam_toolbox +{ + + /*****************************************************************************/ + SlamToolbox::SlamToolbox(ros::NodeHandle& nh) + : solver_loader_("slam_toolbox", "karto::ScanSolver"), + processor_type_(PROCESS), + first_measurement_(true), + nh_(nh), + process_near_pose_(nullptr), + using_loop_(false), + initialized_(false) + /*****************************************************************************/ + { + if (!initialized_) + { + smapper_ = std::make_unique(); + dataset_ = std::make_unique(); + + setParams(nh_); + setROSInterfaces(nh_); + setSolver(nh_); + + laser_assistant_ = std::make_unique( + nh_, tf_.get(), base_frame_); + pose_helper_ = std::make_unique( + tf_.get(), base_frame_, odom_frame_); + scan_holder_ = std::make_unique(lasers_); + map_saver_ = std::make_unique(nh_, map_name_); + closure_assistant_ = + std::make_unique( + nh_, smapper_->getMapper(), scan_holder_.get(), state_, processor_type_); + + reprocessing_transform_.setIdentity(); + + double transform_publish_period; + nh_.param("transform_publish_period", transform_publish_period, 0.05); + using_loop_ = true; + threads_.push_back(std::make_unique( + boost::bind(&SlamToolbox::publishTransformLoop, + this, transform_publish_period))); + threads_.push_back(std::make_unique( + boost::bind(&SlamToolbox::publishVisualizations, this))); + + bool all_joinable = false; + while (ros::ok() && !all_joinable) + { + all_joinable = true; + for (auto& p : threads_) + { + if (!p->joinable()) + { + all_joinable = false; + break; + } + } + } + + ROS_INFO("All threads are joinable, proceeding with next steps."); + initialized_ = true; + } + } + + /*****************************************************************************/ + SlamToolbox::~SlamToolbox() + /*****************************************************************************/ + { + using_loop_ = false; + for (int i = 0; i != threads_.size(); i++) + { + threads_[i]->join(); + } + + smapper_.reset(); + dataset_.reset(); + closure_assistant_.reset(); + map_saver_.reset(); + pose_helper_.reset(); + laser_assistant_.reset(); + scan_holder_.reset(); + } + + /*****************************************************************************/ + void SlamToolbox::setSolver(ros::NodeHandle& private_nh_) + /*****************************************************************************/ + { + // Set solver to be used in loop closure + std::string solver_plugin; + if (!private_nh_.getParam("solver_plugin", solver_plugin)) + { + ROS_WARN("unable to find requested solver plugin, defaulting to SPA"); + solver_plugin = "solver_plugins::CeresSolver"; + } + try + { + solver_ = solver_loader_.createInstance(solver_plugin); + ROS_INFO("Using plugin %s", solver_plugin.c_str()); + } + catch (const pluginlib::PluginlibException& ex) + { + ROS_FATAL("Failed to create %s, is it registered and built? Exception: %s.", + solver_plugin.c_str(), ex.what()); + exit(1); + } + smapper_->getMapper()->SetScanSolver(solver_.get()); + } + + /*****************************************************************************/ + void SlamToolbox::setParams(ros::NodeHandle& private_nh) + /*****************************************************************************/ + { + map_to_odom_.setIdentity(); + private_nh.param("odom_frame", odom_frame_, std::string("odom")); + private_nh.param("map_frame", map_frame_, std::string("map")); + private_nh.param("base_frame", base_frame_, std::string("base_footprint")); + private_nh.param("resolution", resolution_, 0.05); + private_nh.param("map_name", map_name_, std::string("/map")); + private_nh.param("scan_topic", scan_topic_, std::string("/scan")); + private_nh.param("throttle_scans", throttle_scans_, 1); + private_nh.param("enable_interactive_mode", enable_interactive_mode_, false); + + double tmp_val; + private_nh.param("transform_timeout", tmp_val, 0.2); + transform_timeout_ = ros::Duration(tmp_val); + private_nh.param("tf_buffer_duration", tmp_val, 30.); + tf_buffer_dur_ = ros::Duration(tmp_val); + private_nh.param("minimum_time_interval", tmp_val, 0.5); + minimum_time_interval_ = ros::Duration(tmp_val); + + private_nh.param("position_covariance_scale", position_covariance_scale_, 1.0); + private_nh.param("yaw_covariance_scale", yaw_covariance_scale_, 1.0); + + bool debug = false; + if (private_nh.getParam("debug_logging", debug) && debug) + { + if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, + ros::console::levels::Debug)) + { + ros::console::notifyLoggerLevelsChanged(); + } + } + + smapper_->configure(private_nh); + private_nh.setParam("paused_new_measurements", false); + } + + /*****************************************************************************/ + void SlamToolbox::setROSInterfaces(ros::NodeHandle& node) + /*****************************************************************************/ + { + tf_ = std::make_unique(ros::Duration(tf_buffer_dur_)); + tfL_ = std::make_unique(*tf_); + tfB_ = std::make_unique(); + sst_ = node.advertise(map_name_, 1, true); + sstm_ = node.advertise(map_name_ + "_metadata", 1, true); + ssMap_ = node.advertiseService("dynamic_map", &SlamToolbox::mapCallback, this); + ssPauseMeasurements_ = node.advertiseService("pause_new_measurements", &SlamToolbox::pauseNewMeasurementsCallback, this); + ssSerialize_ = node.advertiseService("serialize_map", &SlamToolbox::serializePoseGraphCallback, this); + ssDesserialize_ = node.advertiseService("deserialize_map", &SlamToolbox::deserializePoseGraphCallback, this); + ssReset_ = node.advertiseService("reset", &SlamToolbox::resetCallback, this); + scan_filter_sub_ = std::make_unique >(node, scan_topic_, 5); + scan_filter_ = std::make_unique >(*scan_filter_sub_, *tf_, odom_frame_, 5, node); + scan_filter_->registerCallback(boost::bind(&SlamToolbox::laserCallback, this, _1)); + pose_pub_ = node.advertise("pose", 10, true); + } + + /*****************************************************************************/ + void SlamToolbox::publishTransformLoop(const double& transform_publish_period) + /*****************************************************************************/ + { + if (transform_publish_period == 0) + { + return; + } + + ros::Rate r(1.0 / transform_publish_period); + while (ros::ok() && using_loop_) + { + { + boost::mutex::scoped_lock lock(map_to_odom_mutex_); + geometry_msgs::TransformStamped msg; + tf2::convert(map_to_odom_, msg.transform); + msg.child_frame_id = odom_frame_; + msg.header.frame_id = map_frame_; + + msg.header.stamp = ros::Time::now() + transform_timeout_; + tfB_->sendTransform(msg); + } + r.sleep(); + } + } + + /*****************************************************************************/ + void SlamToolbox::publishVisualizations() + /*****************************************************************************/ + { + nav_msgs::OccupancyGrid& og = map_.map; + og.info.resolution = resolution_; + og.info.origin.position.x = 0.0; + og.info.origin.position.y = 0.0; + og.info.origin.position.z = 0.0; + og.info.origin.orientation.x = 0.0; + og.info.origin.orientation.y = 0.0; + og.info.origin.orientation.z = 0.0; + og.info.origin.orientation.w = 1.0; + og.header.frame_id = map_frame_; + + double map_update_interval; + if (!nh_.getParam("map_update_interval", map_update_interval)) + map_update_interval = 10.0; + ros::Rate r(1.0 / map_update_interval); + while (ros::ok() && using_loop_) + { + this->updateMap(); + if (!isPaused(VISUALIZING_GRAPH)) + { + boost::mutex::scoped_lock lock(smapper_mutex_); + closure_assistant_->publishGraph(); + } + r.sleep(); + } + } + + /*****************************************************************************/ + void SlamToolbox::loadPoseGraphByParams(ros::NodeHandle& nh) + /*****************************************************************************/ + { + std::string filename; + geometry_msgs::Pose2D pose; + bool dock = false; + if (shouldStartWithPoseGraph(filename, pose, dock)) + { + slam_toolbox_msgs::DeserializePoseGraph::Request req; + slam_toolbox_msgs::DeserializePoseGraph::Response resp; + req.initial_pose = pose; + req.filename = filename; + if (dock) + { + req.match_type = + slam_toolbox_msgs::DeserializePoseGraph::Request::START_AT_FIRST_NODE; + } + else + { + req.match_type = + slam_toolbox_msgs::DeserializePoseGraph::Request::START_AT_GIVEN_POSE; + } + deserializePoseGraphCallback(req, resp); + } + } + + /*****************************************************************************/ + bool SlamToolbox::shouldStartWithPoseGraph(std::string& filename, + geometry_msgs::Pose2D& pose, bool& start_at_dock) + /*****************************************************************************/ + { + // if given a map to load at run time, do it. + if (nh_.getParam("map_file_name", filename)) + { + std::vector read_pose; + if (nh_.getParam("map_start_pose", read_pose)) + { + start_at_dock = false; + if (read_pose.size() != 3) + { + ROS_ERROR("LocalizationSlamToolbox: Incorrect number of " + "arguments for map starting pose. Must be in format: " + "[x, y, theta]. Starting at the origin"); + pose.x = 0.; + pose.y = 0.; + pose.theta = 0.; + } + else + { + pose.x = read_pose[0]; + pose.y = read_pose[1]; + pose.theta = read_pose[2]; + } + } + else + { + nh_.getParam("map_start_at_dock", start_at_dock); + } + + return true; + } + + return false; + } + + /*****************************************************************************/ + karto::LaserRangeFinder* SlamToolbox::getLaser(const + sensor_msgs::LaserScan::ConstPtr& scan) + /*****************************************************************************/ + { + const std::string& frame = scan->header.frame_id; + if (lasers_.find(frame) == lasers_.end()) + { + try + { + lasers_[frame] = laser_assistant_->toLaserMetadata(*scan); + dataset_->Add(lasers_[frame].getLaser(), true); + } + catch (tf2::TransformException& e) + { + ROS_ERROR("Failed to compute laser pose, aborting initialization (%s)", + e.what()); + return nullptr; + } + } + + return lasers_[frame].getLaser(); + } + + /*****************************************************************************/ + bool SlamToolbox::updateMap() + /*****************************************************************************/ + { + if (sst_.getNumSubscribers() == 0) + { + return true; + } + boost::mutex::scoped_lock lock(smapper_mutex_); + karto::OccupancyGrid* occ_grid = smapper_->getOccupancyGrid(resolution_); + if (!occ_grid) + { + return false; + } + + vis_utils::toNavMap(occ_grid, map_.map); + + // publish map as current + map_.map.header.stamp = ros::Time::now(); + sst_.publish(map_.map); + sstm_.publish(map_.map.info); + + delete occ_grid; + occ_grid = nullptr; + return true; + } + + /*****************************************************************************/ + tf2::Stamped SlamToolbox::setTransformFromPoses( + const karto::Pose2& corrected_pose, + const karto::Pose2& karto_pose, + const ros::Time& t, + const bool& update_reprocessing_transform) + /*****************************************************************************/ + { + // Compute the map->odom transform + tf2::Stamped odom_to_map; + tf2::Quaternion q(0., 0., 0., 1.0); + q.setRPY(0., 0., corrected_pose.GetHeading()); + tf2::Stamped base_to_map( + tf2::Transform(q, tf2::Vector3(corrected_pose.GetX(), + corrected_pose.GetY(), 0.0)).inverse(), t, base_frame_); + try + { + geometry_msgs::TransformStamped base_to_map_msg, odom_to_map_msg; + tf2::convert(base_to_map, base_to_map_msg); + odom_to_map_msg = tf_->transform(base_to_map_msg, odom_frame_); + tf2::convert(odom_to_map_msg, odom_to_map); + } + catch (tf2::TransformException& e) + { + ROS_ERROR("Transform from base_link to odom failed: %s", e.what()); + return odom_to_map; + } + + // if we're continuing a previous session, we need to + // estimate the homogenous transformation between the old and new + // odometry frames and transform the new session + // into the older session's frame + if (update_reprocessing_transform) + { + tf2::Transform odom_to_base_serialized = base_to_map.inverse(); + tf2::Quaternion q1(0., 0., 0., 1.0); + q1.setRPY(0., 0., tf2::getYaw(odom_to_base_serialized.getRotation())); + odom_to_base_serialized.setRotation(q1); + tf2::Transform odom_to_base_current = smapper_->toTfPose(karto_pose); + reprocessing_transform_ = + odom_to_base_serialized * odom_to_base_current.inverse(); + } + + // set map to odom for our transformation thread to publish + boost::mutex::scoped_lock lock(map_to_odom_mutex_); + map_to_odom_ = tf2::Transform(tf2::Quaternion(odom_to_map.getRotation()), + tf2::Vector3(odom_to_map.getOrigin())).inverse(); + + return odom_to_map; + } + + /*****************************************************************************/ + karto::LocalizedRangeScan* SlamToolbox::getLocalizedRangeScan( + karto::LaserRangeFinder* laser, + const sensor_msgs::LaserScan::ConstPtr& scan, + karto::Pose2& karto_pose) + /*****************************************************************************/ + { + // Create a vector of doubles for karto + std::vector readings = laser_utils::scanToReadings( + *scan, lasers_[scan->header.frame_id].isInverted()); + + // transform by the reprocessing transform + tf2::Transform pose_original = smapper_->toTfPose(karto_pose); + tf2::Transform tf_pose_transformed = reprocessing_transform_ * pose_original; + karto::Pose2 transformed_pose = smapper_->toKartoPose(tf_pose_transformed); + + // create localized range scan + karto::LocalizedRangeScan* range_scan = new karto::LocalizedRangeScan( + laser->GetName(), readings); + range_scan->SetOdometricPose(transformed_pose); + range_scan->SetCorrectedPose(transformed_pose); + return range_scan; + } + + /*****************************************************************************/ + bool SlamToolbox::shouldProcessScan( + const sensor_msgs::LaserScan::ConstPtr& scan, + const karto::Pose2& pose) + /*****************************************************************************/ + { + static karto::Pose2 last_pose; + static ros::Time last_scan_time = ros::Time(0.); + static double min_dist2 = + smapper_->getMapper()->getParamMinimumTravelDistance() * + smapper_->getMapper()->getParamMinimumTravelDistance(); + + // we give it a pass on the first measurement to get the ball rolling + if (first_measurement_) + { + last_scan_time = scan->header.stamp; + last_pose = pose; + first_measurement_ = false; + return true; + } + + // we are in a paused mode, reject incomming information + if (isPaused(NEW_MEASUREMENTS)) + { + return false; + } + + // throttled out + if ((scan->header.seq % throttle_scans_) != 0) + { + return false; + } + + // not enough time + if (scan->header.stamp - last_scan_time < minimum_time_interval_) + { + return false; + } + + // check moved enough, within 10% for correction error + const double dist2 = last_pose.SquaredDistance(pose); + if (dist2 < 0.8 * min_dist2 || scan->header.seq < 5) + { + return false; + } + + last_pose = pose; + last_scan_time = scan->header.stamp; + + return true; + } + + /*****************************************************************************/ + karto::LocalizedRangeScan* SlamToolbox::addScan( + karto::LaserRangeFinder* laser, + PosedScan& scan_w_pose) + /*****************************************************************************/ + { + return addScan(laser, scan_w_pose.scan, scan_w_pose.pose); + } + + /*****************************************************************************/ + karto::LocalizedRangeScan* SlamToolbox::addScan( + karto::LaserRangeFinder* laser, + const sensor_msgs::LaserScan::ConstPtr& scan, + karto::Pose2& karto_pose) + /*****************************************************************************/ + { + // get our localized range scan + karto::LocalizedRangeScan* range_scan = getLocalizedRangeScan( + laser, scan, karto_pose); + + // Add the localized range scan to the smapper + boost::mutex::scoped_lock lock(smapper_mutex_); + bool processed = false, update_reprocessing_transform = false; + + karto::Matrix3 covariance; + covariance.SetToIdentity(); + + if (processor_type_ == PROCESS) + { + processed = smapper_->getMapper()->Process(range_scan, &covariance); + } + else if (processor_type_ == PROCESS_FIRST_NODE) + { + processed = smapper_->getMapper()->ProcessAtDock(range_scan, &covariance); + processor_type_ = PROCESS; + update_reprocessing_transform = true; + } + else if (processor_type_ == PROCESS_NEAR_REGION) + { + boost::mutex::scoped_lock l(pose_mutex_); + if (!process_near_pose_) + { + ROS_ERROR("Process near region called without a " + "valid region request. Ignoring scan."); + return nullptr; + } + range_scan->SetOdometricPose(*process_near_pose_); + range_scan->SetCorrectedPose(range_scan->GetOdometricPose()); + process_near_pose_.reset(nullptr); + processed = smapper_->getMapper()->ProcessAgainstNodesNearBy(range_scan, false, &covariance); + update_reprocessing_transform = true; + processor_type_ = PROCESS; + } + else + { + ROS_FATAL("SlamToolbox: No valid processor type set! Exiting."); + exit(-1); + } + + // if successfully processed, create odom to map transformation + // and add our scan to storage + if (processed) + { + if (enable_interactive_mode_) + { + scan_holder_->addScan(*scan); + } + + setTransformFromPoses(range_scan->GetCorrectedPose(), karto_pose, + scan->header.stamp, update_reprocessing_transform); + dataset_->Add(range_scan); + publishPose(range_scan->GetCorrectedPose(), covariance, scan->header.stamp); + } + else + { + delete range_scan; + range_scan = nullptr; + } + + return range_scan; + } + + /*****************************************************************************/ + void SlamToolbox::publishPose( + const karto::Pose2& pose, + const karto::Matrix3& cov, + const ros::Time& t) + /*****************************************************************************/ + { + geometry_msgs::PoseWithCovarianceStamped pose_msg; + pose_msg.header.stamp = t; + pose_msg.header.frame_id = map_frame_; + + tf2::Quaternion q(0., 0., 0., 1.0); + q.setRPY(0., 0., pose.GetHeading()); + tf2::Transform transform(q, tf2::Vector3(pose.GetX(), pose.GetY(), 0.0)); + tf2::toMsg(transform, pose_msg.pose.pose); + + pose_msg.pose.covariance[0] = cov(0, 0) * position_covariance_scale_; // x + pose_msg.pose.covariance[1] = cov(0, 1) * position_covariance_scale_; // xy + pose_msg.pose.covariance[6] = cov(1, 0) * position_covariance_scale_; // xy + pose_msg.pose.covariance[7] = cov(1, 1) * position_covariance_scale_; // y + pose_msg.pose.covariance[35] = cov(2, 2) * yaw_covariance_scale_; // yaw + + pose_pub_.publish(pose_msg); + } + + /*****************************************************************************/ + bool SlamToolbox::mapCallback( + nav_msgs::GetMap::Request& req, + nav_msgs::GetMap::Response& res) + /*****************************************************************************/ + { + if (map_.map.info.width && map_.map.info.height) + { + boost::mutex::scoped_lock lock(smapper_mutex_); + res = map_; + return true; + } + else + { + return false; + } + } + + /*****************************************************************************/ + bool SlamToolbox::pauseNewMeasurementsCallback( + slam_toolbox_msgs::Pause::Request& req, + slam_toolbox_msgs::Pause::Response& resp) + /*****************************************************************************/ + { + bool curr_state = isPaused(NEW_MEASUREMENTS); + state_.set(NEW_MEASUREMENTS, !curr_state); + + nh_.setParam("paused_new_measurements", !curr_state); + ROS_INFO("SlamToolbox: Toggled to %s", + !curr_state ? "pause taking new measurements." : + "actively taking new measurements."); + resp.status = true; + return true; + } + + /*****************************************************************************/ + bool SlamToolbox::isPaused(const PausedApplication& app) + /*****************************************************************************/ + { + return state_.get(app); + } + + /*****************************************************************************/ + bool SlamToolbox::serializePoseGraphCallback( + slam_toolbox_msgs::SerializePoseGraph::Request& req, + slam_toolbox_msgs::SerializePoseGraph::Response& resp) + /*****************************************************************************/ + { + std::string filename = req.filename; + + // if we're inside the snap, we need to write to commonly accessible space + if (snap_utils::isInSnap()) + { + filename = snap_utils::getSnapPath() + std::string("/") + filename; + } + + boost::mutex::scoped_lock lock(smapper_mutex_); + serialization::write(filename, *smapper_->getMapper(), *dataset_); + return true; + } + + /*****************************************************************************/ + void SlamToolbox::loadSerializedPoseGraph( + std::unique_ptr& mapper, + std::unique_ptr& dataset) + /*****************************************************************************/ + { + boost::mutex::scoped_lock lock(smapper_mutex_); + + solver_->Reset(); + + // add the nodes and constraints to the optimizer + VerticeMap mapper_vertices = mapper->GetGraph()->GetVertices(); + VerticeMap::iterator vertex_map_it = mapper_vertices.begin(); + for (vertex_map_it; vertex_map_it != mapper_vertices.end(); ++vertex_map_it) + { + ScanMap::iterator vertex_it = vertex_map_it->second.begin(); + for (vertex_it; vertex_it != vertex_map_it->second.end(); ++vertex_it) + { + if (vertex_it->second != nullptr) + { + solver_->AddNode(vertex_it->second); + } + } + } + + EdgeVector mapper_edges = mapper->GetGraph()->GetEdges(); + EdgeVector::iterator edges_it = mapper_edges.begin(); + for (edges_it; edges_it != mapper_edges.end(); ++edges_it) + { + if (*edges_it != nullptr) + { + solver_->AddConstraint(*edges_it); + } + } + + mapper->SetScanSolver(solver_.get()); + + // move the memory to our working dataset + smapper_->setMapper(mapper.release()); + smapper_->configure(nh_); + dataset_.reset(dataset.release()); + + closure_assistant_->setMapper(smapper_->getMapper()); + + if (!smapper_->getMapper()) + { + ROS_FATAL("loadSerializedPoseGraph: Could not properly load " + "a valid mapping object. Did you modify something by hand?"); + exit(-1); + } + + if (dataset_->GetLasers().size() < 1) + { + ROS_FATAL("loadSerializedPoseGraph: Cannot deserialize " + "dataset with no laser objects."); + exit(-1); + } + + // create a current laser sensor + karto::LaserRangeFinder* laser = + dynamic_cast( + dataset_->GetLasers()[0]); + karto::Sensor* pSensor = dynamic_cast(laser); + if (pSensor) + { + karto::SensorManager::GetInstance()->RegisterSensor(pSensor); + + while (ros::ok()) + { + ROS_INFO("Waiting for incoming scan to get metadata..."); + boost::shared_ptr scan = + ros::topic::waitForMessage( + scan_topic_, ros::Duration(1.0)); + if (scan) + { + ROS_INFO("Got scan!"); + try + { + lasers_[scan->header.frame_id] = + laser_assistant_->toLaserMetadata(*scan); + break; + } + catch (tf2::TransformException& e) + { + ROS_ERROR("Failed to compute laser pose, aborting continue mapping (%s)", + e.what()); + exit(-1); + } + } + } + } + else + { + ROS_ERROR("Invalid sensor pointer in dataset. Unable to register sensor."); + } + + solver_->Compute(); + + return; + } + + /*****************************************************************************/ + bool SlamToolbox::deserializePoseGraphCallback( + slam_toolbox_msgs::DeserializePoseGraph::Request& req, + slam_toolbox_msgs::DeserializePoseGraph::Response& resp) + /*****************************************************************************/ + { + if (req.match_type == slam_toolbox_msgs::DeserializePoseGraph::Request::UNSET) + { + ROS_ERROR("Deserialization called without valid processor type set. " + "Undefined behavior!"); + return false; + } + + std::string filename = req.filename; + + if (filename.empty()) + { + ROS_WARN("No map file given!"); + return true; + } + + // if we're inside the snap, we need to write to commonly accessible space + if (snap_utils::isInSnap()) + { + filename = snap_utils::getSnapPath() + std::string("/") + filename; + } + + std::unique_ptr dataset = std::make_unique(); + std::unique_ptr mapper = std::make_unique(); + + if (!serialization::read(filename, *mapper, *dataset)) + { + ROS_ERROR("DeserializePoseGraph: Failed to read " + "file: %s.", filename.c_str()); + return true; + } + ROS_DEBUG("DeserializePoseGraph: Successfully read file."); + + loadSerializedPoseGraph(mapper, dataset); + updateMap(); + + first_measurement_ = true; + boost::mutex::scoped_lock l(pose_mutex_); + switch (req.match_type) + { + case procType::START_AT_FIRST_NODE: + processor_type_ = PROCESS_FIRST_NODE; + break; + case procType::START_AT_GIVEN_POSE: + processor_type_ = PROCESS_NEAR_REGION; + process_near_pose_ = std::make_unique(req.initial_pose.x, + req.initial_pose.y, req.initial_pose.theta); + break; + case procType::LOCALIZE_AT_POSE: + processor_type_ = PROCESS_LOCALIZATION; + process_near_pose_ = std::make_unique(req.initial_pose.x, + req.initial_pose.y, req.initial_pose.theta); + break; + default: + ROS_FATAL("Deserialization called without valid processor type set."); + } + + return true; + } + + /*****************************************************************************/ + bool SlamToolbox::resetCallback( + slam_toolbox_msgs::Reset::Request& req, + slam_toolbox_msgs::Reset::Response& resp) + /*****************************************************************************/ + { + boost::mutex::scoped_lock lock(smapper_mutex_); + // Reset the map. + smapper_->Reset(); + smapper_->getMapper()->getScanSolver()->Reset(); + + // Ensure we will process the next available scan. + first_measurement_ = true; + + // Pause new measurements processing if requested. + if (req.pause_new_measurements) { + state_.set(NEW_MEASUREMENTS, true); + nh_.setParam("paused_new_measurements", true); + ROS_INFO("SlamToolbox: Toggled to pause taking new measurements after reset."); + } + + resp.result = true; + return true; + } + +} // end namespace diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox/src/slam_toolbox_localization.cpp b/Localizations/Packages/slam_toolbox/slam_toolbox/src/slam_toolbox_localization.cpp new file mode 100644 index 0000000..fc169b5 --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox/src/slam_toolbox_localization.cpp @@ -0,0 +1,232 @@ +/* + * slam_toolbox + * Copyright Work Modifications (c) 2019, Steve Macenski + * + * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE + * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY + * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS + * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED. + * + * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO + * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS + * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND + * CONDITIONS. + * + */ + +/* Author: Steven Macenski */ + +#include "slam_toolbox/slam_toolbox_localization.hpp" + +namespace slam_toolbox +{ + +/*****************************************************************************/ +LocalizationSlamToolbox::LocalizationSlamToolbox(ros::NodeHandle& nh) +: SlamToolbox(nh) +/*****************************************************************************/ +{ + processor_type_ = PROCESS_LOCALIZATION; + localization_pose_sub_ = nh.subscribe("/initialpose", 1, + &LocalizationSlamToolbox::localizePoseCallback, this); + clear_localization_ = nh.advertiseService( + "clear_localization_buffer", + &LocalizationSlamToolbox::clearLocalizationBuffer, this); + + std::string filename; + geometry_msgs::Pose2D pose; + bool dock = false; + if (shouldStartWithPoseGraph(filename, pose, dock)) + { + slam_toolbox_msgs::DeserializePoseGraph::Request req; + slam_toolbox_msgs::DeserializePoseGraph::Response resp; + req.initial_pose = pose; + req.filename = filename; + req.match_type = + slam_toolbox_msgs::DeserializePoseGraph::Request::LOCALIZE_AT_POSE; + if (dock) + { + ROS_ERROR("LocalizationSlamToolbox: Starting localization " + "at first node (dock) is correctly not supported."); + } + + deserializePoseGraphCallback(req, resp); + } + + // in localization mode, we cannot allow for interactive mode + enable_interactive_mode_ = false; + + // in localization mode, disable map saver + map_saver_.reset(); + return; +} + +/*****************************************************************************/ +bool LocalizationSlamToolbox::clearLocalizationBuffer( + std_srvs::Empty::Request& req, + std_srvs::Empty::Response& resp) +/*****************************************************************************/ +{ + boost::mutex::scoped_lock lock(smapper_mutex_); + ROS_INFO("LocalizationSlamToolbox: Clearing localization buffer."); + smapper_->clearLocalizationBuffer(); + return true; +} + +/*****************************************************************************/ +bool LocalizationSlamToolbox::serializePoseGraphCallback( + slam_toolbox_msgs::SerializePoseGraph::Request& req, + slam_toolbox_msgs::SerializePoseGraph::Response& resp) +/*****************************************************************************/ +{ + ROS_FATAL("LocalizationSlamToolbox: Cannot call serialize map " + "in localization mode!"); + return false; +} + +/*****************************************************************************/ +bool LocalizationSlamToolbox::deserializePoseGraphCallback( + slam_toolbox_msgs::DeserializePoseGraph::Request& req, + slam_toolbox_msgs::DeserializePoseGraph::Response& resp) +/*****************************************************************************/ +{ + if (req.match_type != procType::LOCALIZE_AT_POSE) + { + ROS_ERROR("Requested a non-localization deserialization " + "in localization mode."); + return false; + } + return SlamToolbox::deserializePoseGraphCallback(req, resp); +} + +/*****************************************************************************/ +void LocalizationSlamToolbox::laserCallback( + const sensor_msgs::LaserScan::ConstPtr& scan) +/*****************************************************************************/ +{ + // no odom info + Pose2 pose; + if(!pose_helper_->getOdomPose(pose, scan->header.stamp)) + { + return; + } + + // ensure the laser can be used + LaserRangeFinder* laser = getLaser(scan); + + if(!laser) + { + ROS_WARN_THROTTLE(5., "SynchronousSlamToolbox: Failed to create laser" + " device for %s; discarding scan", scan->header.frame_id.c_str()); + return; + } + + if (shouldProcessScan(scan, pose)) + { + addScan(laser, scan, pose); + } + + return; +} + +/*****************************************************************************/ +LocalizedRangeScan* LocalizationSlamToolbox::addScan( + LaserRangeFinder* laser, + const sensor_msgs::LaserScan::ConstPtr& scan, + Pose2& karto_pose) +/*****************************************************************************/ +{ + boost::mutex::scoped_lock l(pose_mutex_); + + if (PROCESS_LOCALIZATION && process_near_pose_) + { + processor_type_ = PROCESS_NEAR_REGION; + } + + LocalizedRangeScan* range_scan = getLocalizedRangeScan( + laser, scan, karto_pose); + + // Add the localized range scan to the smapper + boost::mutex::scoped_lock lock(smapper_mutex_); + bool processed = false, update_reprocessing_transform = false; + if (processor_type_ == PROCESS_NEAR_REGION) + { + if (!process_near_pose_) + { + ROS_ERROR("Process near region called without a " + "valid region request. Ignoring scan."); + return nullptr; + } + + // set our position to the requested pose and process + range_scan->SetOdometricPose(*process_near_pose_); + range_scan->SetCorrectedPose(range_scan->GetOdometricPose()); + process_near_pose_.reset(nullptr); + processed = smapper_->getMapper()->ProcessAgainstNodesNearBy(range_scan, true); + + // reset to localization mode + processor_type_ = PROCESS_LOCALIZATION; + update_reprocessing_transform = true; + } + else if (processor_type_ == PROCESS_LOCALIZATION) + { + processed = smapper_->getMapper()->ProcessLocalization(range_scan); + update_reprocessing_transform = false; + } + else + { + ROS_FATAL("LocalizationSlamToolbox: " + "No valid processor type set! Exiting."); + exit(-1); + } + + // if successfully processed, create odom to map transformation + if(!processed) + { + delete range_scan; + range_scan = nullptr; + } else { + // compute our new transform + setTransformFromPoses(range_scan->GetCorrectedPose(), karto_pose, + scan->header.stamp, update_reprocessing_transform); + } + + return range_scan; +} + +/*****************************************************************************/ +void LocalizationSlamToolbox::localizePoseCallback(const + geometry_msgs::PoseWithCovarianceStampedConstPtr& msg) +/*****************************************************************************/ +{ + if (processor_type_ != PROCESS_LOCALIZATION) + { + ROS_ERROR("LocalizePoseCallback: Cannot process localization command " + "if not in localization mode."); + return; + } + + boost::mutex::scoped_lock l(pose_mutex_); + if (process_near_pose_) + { + process_near_pose_.reset(new Pose2(msg->pose.pose.position.x, + msg->pose.pose.position.y, tf2::getYaw(msg->pose.pose.orientation))); + } + else + { + process_near_pose_ = std::make_unique(msg->pose.pose.position.x, + msg->pose.pose.position.y, tf2::getYaw(msg->pose.pose.orientation)); + } + + first_measurement_ = true; + + boost::mutex::scoped_lock lock(smapper_mutex_); + smapper_->clearLocalizationBuffer(); + + ROS_INFO("LocalizePoseCallback: Localizing to: (%0.2f %0.2f), theta=%0.2f", + msg->pose.pose.position.x, msg->pose.pose.position.y, + tf2::getYaw(msg->pose.pose.orientation)); + return; +} + +} // end namespace diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox/src/slam_toolbox_localization_node.cpp b/Localizations/Packages/slam_toolbox/slam_toolbox/src/slam_toolbox_localization_node.cpp new file mode 100644 index 0000000..931f346 --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox/src/slam_toolbox_localization_node.cpp @@ -0,0 +1,45 @@ +/* + * slam_toolbox + * Copyright Work Modifications (c) 2019, Steve Macenski + * + * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE + * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY + * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS + * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED. + * + * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO + * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS + * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND + * CONDITIONS. + * + */ + +/* Author: Steven Macenski */ + +#include "slam_toolbox/slam_toolbox_localization.hpp" + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "slam_toolbox"); + ros::NodeHandle nh("~"); + ros::spinOnce(); + + int stack_size; + if (nh.getParam("stack_size_to_use", stack_size)) + { + ROS_INFO("Node using stack size %i", (int)stack_size); + const rlim_t max_stack_size = stack_size; + struct rlimit stack_limit; + getrlimit(RLIMIT_STACK, &stack_limit); + if (stack_limit.rlim_cur < stack_size) + { + stack_limit.rlim_cur = stack_size; + } + setrlimit(RLIMIT_STACK, &stack_limit); + } + + slam_toolbox::LocalizationSlamToolbox sst(nh); + + ros::spin(); + return 0; +} diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox/src/slam_toolbox_sync.cpp b/Localizations/Packages/slam_toolbox/slam_toolbox/src/slam_toolbox_sync.cpp new file mode 100644 index 0000000..30bef8f --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox/src/slam_toolbox_sync.cpp @@ -0,0 +1,154 @@ +/* + * slam_toolbox + * Copyright Work Modifications (c) 2018, Simbe Robotics, Inc. + * Copyright Work Modifications (c) 2019, Steve Macenski + * + * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE + * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY + * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS + * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED. + * + * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO + * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS + * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND + * CONDITIONS. + * + */ + +/* Author: Steven Macenski */ + +#include "slam_toolbox/slam_toolbox_sync.hpp" + +namespace slam_toolbox +{ + +/*****************************************************************************/ +SynchronousSlamToolbox::SynchronousSlamToolbox(ros::NodeHandle& nh) +: SlamToolbox(nh) +/*****************************************************************************/ +{ + ssClear_ = nh.advertiseService("clear_queue", + &SynchronousSlamToolbox::clearQueueCallback, this); + + threads_.push_back(std::make_unique( + boost::bind(&SynchronousSlamToolbox::run, this))); + + loadPoseGraphByParams(nh); +} + +/*****************************************************************************/ +void SynchronousSlamToolbox::run() +/*****************************************************************************/ +{ + ros::Rate r(100); + while(ros::ok()) + { + if (!isPaused(PROCESSING)) + { + PosedScan scan_w_pose(nullptr, karto::Pose2()); // dummy, updated in critical section + bool queue_empty = true; + { + boost::mutex::scoped_lock lock(q_mutex_); + queue_empty = q_.empty(); + if(!queue_empty) + { + scan_w_pose = q_.front(); + q_.pop(); + + if (q_.size() > 10) + { + ROS_WARN_THROTTLE(10., "Queue size has grown to: %i. " + "Recommend stopping until message is gone if online mapping.", + (int)q_.size()); + } + } + } + if(!queue_empty){ + addScan(getLaser(scan_w_pose.scan), scan_w_pose); + continue; + } + } + + r.sleep(); + } +} + +/*****************************************************************************/ +void SynchronousSlamToolbox::laserCallback( + const sensor_msgs::LaserScan::ConstPtr& scan) +/*****************************************************************************/ +{ + // no odom info + karto::Pose2 pose; + if(!pose_helper_->getOdomPose(pose, scan->header.stamp)) + { + return; + } + + // ensure the laser can be used + karto::LaserRangeFinder* laser = getLaser(scan); + + if(!laser) + { + ROS_WARN_THROTTLE(5., "SynchronousSlamToolbox: Failed to create laser" + " device for %s; discarding scan", scan->header.frame_id.c_str()); + return; + } + + // if sync and valid, add to queue + if (shouldProcessScan(scan, pose)) + { + boost::mutex::scoped_lock lock(q_mutex_); + q_.push(PosedScan(scan, pose)); + } + + return; +} + +/*****************************************************************************/ +bool SynchronousSlamToolbox::clearQueueCallback( + slam_toolbox_msgs::ClearQueue::Request& req, + slam_toolbox_msgs::ClearQueue::Response& resp) +/*****************************************************************************/ +{ + ROS_INFO("SynchronousSlamToolbox: Clearing all queued scans to add to map."); + while(!q_.empty()) + { + q_.pop(); + } + resp.status = true; + return true; +} + +/*****************************************************************************/ +bool SynchronousSlamToolbox::deserializePoseGraphCallback( + slam_toolbox_msgs::DeserializePoseGraph::Request& req, + slam_toolbox_msgs::DeserializePoseGraph::Response& resp) +/*****************************************************************************/ +{ + if (req.match_type == procType::LOCALIZE_AT_POSE) + { + ROS_ERROR("Requested a localization deserialization " + "in non-localization mode."); + return false; + } + return SlamToolbox::deserializePoseGraphCallback(req, resp); +} + +/*****************************************************************************/ +bool SynchronousSlamToolbox::resetCallback( + slam_toolbox_msgs::Reset::Request& req, + slam_toolbox_msgs::Reset::Response& resp) +/*****************************************************************************/ +{ + { + boost::mutex::scoped_lock lock(q_mutex_); + // Clear the scan queue. + while (!q_.empty()) { + q_.pop(); + } + } + return SlamToolbox::resetCallback(req, resp); +} + +} // end namespace diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox/src/slam_toolbox_sync_node.cpp b/Localizations/Packages/slam_toolbox/slam_toolbox/src/slam_toolbox_sync_node.cpp new file mode 100644 index 0000000..81945f3 --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox/src/slam_toolbox_sync_node.cpp @@ -0,0 +1,46 @@ +/* + * slam_toolbox + * Copyright Work Modifications (c) 2018, Simbe Robotics, Inc. + * Copyright Work Modifications (c) 2019, Steve Macenski + * + * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE + * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY + * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS + * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED. + * + * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO + * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS + * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND + * CONDITIONS. + * + */ + +/* Author: Steven Macenski */ + +#include "slam_toolbox/slam_toolbox_sync.hpp" + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "slam_toolbox"); + ros::NodeHandle nh("~"); + ros::spinOnce(); + + int stack_size; + if (nh.getParam("stack_size_to_use", stack_size)) + { + ROS_INFO("Node using stack size %i", (int)stack_size); + const rlim_t max_stack_size = stack_size; + struct rlimit stack_limit; + getrlimit(RLIMIT_STACK, &stack_limit); + if (stack_limit.rlim_cur < stack_size) + { + stack_limit.rlim_cur = stack_size; + } + setrlimit(RLIMIT_STACK, &stack_limit); + } + + slam_toolbox::SynchronousSlamToolbox sst(nh); + + ros::spin(); + return 0; +} diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox/test/constraints_on_graph.dat b/Localizations/Packages/slam_toolbox/slam_toolbox/test/constraints_on_graph.dat new file mode 100644 index 0000000..93f44b6 --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox/test/constraints_on_graph.dat @@ -0,0 +1,8841 @@ +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 4 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 4 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 9 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 11 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 10 vertexes +3 constraints are in 7 vertexes +4 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 17 vertexes +3 constraints are in 7 vertexes +4 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 23 vertexes +3 constraints are in 7 vertexes +4 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 28 vertexes +3 constraints are in 8 vertexes +4 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 25 vertexes +3 constraints are in 15 vertexes +4 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 30 vertexes +3 constraints are in 17 vertexes +4 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 34 vertexes +3 constraints are in 19 vertexes +4 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 40 vertexes +3 constraints are in 19 vertexes +4 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 46 vertexes +3 constraints are in 19 vertexes +4 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 52 vertexes +3 constraints are in 19 vertexes +4 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 57 vertexes +3 constraints are in 20 vertexes +4 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 60 vertexes +3 constraints are in 21 vertexes +4 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 63 vertexes +3 constraints are in 21 vertexes +4 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 68 vertexes +3 constraints are in 21 vertexes +4 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 68 vertexes +3 constraints are in 26 vertexes +4 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 68 vertexes +3 constraints are in 27 vertexes +4 constraints are in 5 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 70 vertexes +3 constraints are in 26 vertexes +4 constraints are in 6 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 70 vertexes +3 constraints are in 26 vertexes +4 constraints are in 6 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 70 vertexes +3 constraints are in 29 vertexes +4 constraints are in 7 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 72 vertexes +3 constraints are in 31 vertexes +4 constraints are in 7 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 78 vertexes +3 constraints are in 31 vertexes +4 constraints are in 7 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 84 vertexes +3 constraints are in 31 vertexes +4 constraints are in 7 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 87 vertexes +3 constraints are in 34 vertexes +4 constraints are in 7 vertexes +5 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 91 vertexes +3 constraints are in 36 vertexes +4 constraints are in 7 vertexes +5 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 94 vertexes +3 constraints are in 36 vertexes +4 constraints are in 10 vertexes +5 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 99 vertexes +3 constraints are in 36 vertexes +4 constraints are in 10 vertexes +5 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 105 vertexes +3 constraints are in 36 vertexes +4 constraints are in 10 vertexes +5 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 111 vertexes +3 constraints are in 36 vertexes +4 constraints are in 10 vertexes +5 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 113 vertexes +3 constraints are in 36 vertexes +4 constraints are in 10 vertexes +5 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 112 vertexes +3 constraints are in 41 vertexes +4 constraints are in 10 vertexes +5 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 112 vertexes +3 constraints are in 46 vertexes +4 constraints are in 10 vertexes +5 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 117 vertexes +3 constraints are in 46 vertexes +4 constraints are in 10 vertexes +5 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 121 vertexes +3 constraints are in 48 vertexes +4 constraints are in 10 vertexes +5 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 121 vertexes +3 constraints are in 48 vertexes +4 constraints are in 10 vertexes +5 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 123 vertexes +3 constraints are in 50 vertexes +4 constraints are in 11 vertexes +5 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 125 vertexes +3 constraints are in 51 vertexes +4 constraints are in 11 vertexes +5 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 124 vertexes +3 constraints are in 52 vertexes +4 constraints are in 11 vertexes +5 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 128 vertexes +3 constraints are in 55 vertexes +4 constraints are in 11 vertexes +5 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 131 vertexes +3 constraints are in 57 vertexes +4 constraints are in 11 vertexes +5 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 135 vertexes +3 constraints are in 58 vertexes +4 constraints are in 11 vertexes +5 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 139 vertexes +3 constraints are in 58 vertexes +4 constraints are in 11 vertexes +5 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 145 vertexes +3 constraints are in 58 vertexes +4 constraints are in 11 vertexes +5 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 151 vertexes +3 constraints are in 58 vertexes +4 constraints are in 11 vertexes +5 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 157 vertexes +3 constraints are in 58 vertexes +4 constraints are in 11 vertexes +5 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 162 vertexes +3 constraints are in 58 vertexes +4 constraints are in 11 vertexes +5 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 167 vertexes +3 constraints are in 58 vertexes +4 constraints are in 11 vertexes +5 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 170 vertexes +3 constraints are in 61 vertexes +4 constraints are in 13 vertexes +5 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 173 vertexes +3 constraints are in 62 vertexes +4 constraints are in 15 vertexes +5 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 178 vertexes +3 constraints are in 64 vertexes +4 constraints are in 15 vertexes +5 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 182 vertexes +3 constraints are in 66 vertexes +4 constraints are in 15 vertexes +5 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 186 vertexes +3 constraints are in 68 vertexes +4 constraints are in 15 vertexes +5 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 190 vertexes +3 constraints are in 70 vertexes +4 constraints are in 15 vertexes +5 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 193 vertexes +3 constraints are in 73 vertexes +4 constraints are in 16 vertexes +5 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 197 vertexes +3 constraints are in 74 vertexes +4 constraints are in 17 vertexes +5 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 204 vertexes +3 constraints are in 75 vertexes +4 constraints are in 17 vertexes +5 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 203 vertexes +3 constraints are in 80 vertexes +4 constraints are in 18 vertexes +5 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 205 vertexes +3 constraints are in 83 vertexes +4 constraints are in 21 vertexes +5 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 207 vertexes +3 constraints are in 87 vertexes +4 constraints are in 21 vertexes +5 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 211 vertexes +3 constraints are in 88 vertexes +4 constraints are in 21 vertexes +5 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 214 vertexes +3 constraints are in 91 vertexes +4 constraints are in 22 vertexes +5 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 216 vertexes +3 constraints are in 93 vertexes +4 constraints are in 22 vertexes +5 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 222 vertexes +3 constraints are in 93 vertexes +4 constraints are in 22 vertexes +5 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 225 vertexes +3 constraints are in 96 vertexes +4 constraints are in 22 vertexes +5 constraints are in 4 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 231 vertexes +3 constraints are in 96 vertexes +4 constraints are in 22 vertexes +5 constraints are in 4 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 237 vertexes +3 constraints are in 97 vertexes +4 constraints are in 22 vertexes +5 constraints are in 4 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 239 vertexes +3 constraints are in 100 vertexes +4 constraints are in 22 vertexes +5 constraints are in 4 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 244 vertexes +3 constraints are in 102 vertexes +4 constraints are in 22 vertexes +5 constraints are in 4 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 246 vertexes +3 constraints are in 105 vertexes +4 constraints are in 22 vertexes +5 constraints are in 5 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 251 vertexes +3 constraints are in 105 vertexes +4 constraints are in 23 vertexes +5 constraints are in 5 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 258 vertexes +3 constraints are in 105 vertexes +4 constraints are in 23 vertexes +5 constraints are in 5 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 264 vertexes +3 constraints are in 105 vertexes +4 constraints are in 23 vertexes +5 constraints are in 5 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 270 vertexes +3 constraints are in 105 vertexes +4 constraints are in 23 vertexes +5 constraints are in 5 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 276 vertexes +3 constraints are in 105 vertexes +4 constraints are in 23 vertexes +5 constraints are in 5 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 283 vertexes +3 constraints are in 105 vertexes +4 constraints are in 23 vertexes +5 constraints are in 5 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 285 vertexes +3 constraints are in 105 vertexes +4 constraints are in 23 vertexes +5 constraints are in 5 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 286 vertexes +3 constraints are in 105 vertexes +4 constraints are in 23 vertexes +5 constraints are in 5 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 288 vertexes +3 constraints are in 108 vertexes +4 constraints are in 22 vertexes +5 constraints are in 6 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 288 vertexes +3 constraints are in 108 vertexes +4 constraints are in 22 vertexes +5 constraints are in 6 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 288 vertexes +3 constraints are in 108 vertexes +4 constraints are in 22 vertexes +5 constraints are in 6 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 289 vertexes +3 constraints are in 108 vertexes +4 constraints are in 22 vertexes +5 constraints are in 6 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 291 vertexes +3 constraints are in 108 vertexes +4 constraints are in 22 vertexes +5 constraints are in 6 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 293 vertexes +3 constraints are in 108 vertexes +4 constraints are in 22 vertexes +5 constraints are in 6 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 294 vertexes +3 constraints are in 111 vertexes +4 constraints are in 23 vertexes +5 constraints are in 6 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 297 vertexes +3 constraints are in 114 vertexes +4 constraints are in 23 vertexes +5 constraints are in 6 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 299 vertexes +3 constraints are in 115 vertexes +4 constraints are in 23 vertexes +5 constraints are in 6 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 298 vertexes +3 constraints are in 116 vertexes +4 constraints are in 23 vertexes +5 constraints are in 6 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 299 vertexes +3 constraints are in 120 vertexes +4 constraints are in 24 vertexes +5 constraints are in 6 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 299 vertexes +3 constraints are in 115 vertexes +4 constraints are in 23 vertexes +5 constraints are in 6 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 298 vertexes +3 constraints are in 116 vertexes +4 constraints are in 23 vertexes +5 constraints are in 6 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 299 vertexes +3 constraints are in 120 vertexes +4 constraints are in 24 vertexes +5 constraints are in 6 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 304 vertexes +3 constraints are in 120 vertexes +4 constraints are in 25 vertexes +5 constraints are in 6 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 311 vertexes +3 constraints are in 120 vertexes +4 constraints are in 25 vertexes +5 constraints are in 6 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 313 vertexes +3 constraints are in 122 vertexes +4 constraints are in 25 vertexes +5 constraints are in 6 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 318 vertexes +3 constraints are in 122 vertexes +4 constraints are in 25 vertexes +5 constraints are in 6 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 320 vertexes +3 constraints are in 122 vertexes +4 constraints are in 25 vertexes +5 constraints are in 6 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 321 vertexes +3 constraints are in 123 vertexes +4 constraints are in 25 vertexes +5 constraints are in 6 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 320 vertexes +3 constraints are in 124 vertexes +4 constraints are in 25 vertexes +5 constraints are in 6 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 321 vertexes +3 constraints are in 127 vertexes +4 constraints are in 25 vertexes +5 constraints are in 6 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 323 vertexes +3 constraints are in 130 vertexes +4 constraints are in 25 vertexes +5 constraints are in 6 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 328 vertexes +3 constraints are in 130 vertexes +4 constraints are in 25 vertexes +5 constraints are in 6 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 330 vertexes +3 constraints are in 130 vertexes +4 constraints are in 25 vertexes +5 constraints are in 6 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 332 vertexes +3 constraints are in 134 vertexes +4 constraints are in 25 vertexes +5 constraints are in 6 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 335 vertexes +3 constraints are in 135 vertexes +4 constraints are in 28 vertexes +5 constraints are in 6 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 339 vertexes +3 constraints are in 136 vertexes +4 constraints are in 29 vertexes +5 constraints are in 6 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 343 vertexes +3 constraints are in 138 vertexes +4 constraints are in 29 vertexes +5 constraints are in 6 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 346 vertexes +3 constraints are in 140 vertexes +4 constraints are in 29 vertexes +5 constraints are in 6 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 349 vertexes +3 constraints are in 141 vertexes +4 constraints are in 29 vertexes +5 constraints are in 5 vertexes +6 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 351 vertexes +3 constraints are in 143 vertexes +4 constraints are in 29 vertexes +5 constraints are in 5 vertexes +6 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 354 vertexes +3 constraints are in 146 vertexes +4 constraints are in 29 vertexes +5 constraints are in 4 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 360 vertexes +3 constraints are in 146 vertexes +4 constraints are in 29 vertexes +5 constraints are in 4 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 365 vertexes +3 constraints are in 146 vertexes +4 constraints are in 29 vertexes +5 constraints are in 4 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 367 vertexes +3 constraints are in 146 vertexes +4 constraints are in 29 vertexes +5 constraints are in 4 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 369 vertexes +3 constraints are in 151 vertexes +4 constraints are in 30 vertexes +5 constraints are in 4 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 368 vertexes +3 constraints are in 154 vertexes +4 constraints are in 30 vertexes +5 constraints are in 4 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 373 vertexes +3 constraints are in 154 vertexes +4 constraints are in 31 vertexes +5 constraints are in 4 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 377 vertexes +3 constraints are in 157 vertexes +4 constraints are in 31 vertexes +5 constraints are in 4 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 380 vertexes +3 constraints are in 158 vertexes +4 constraints are in 32 vertexes +5 constraints are in 4 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 386 vertexes +3 constraints are in 158 vertexes +4 constraints are in 32 vertexes +5 constraints are in 4 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 392 vertexes +3 constraints are in 158 vertexes +4 constraints are in 32 vertexes +5 constraints are in 4 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 397 vertexes +3 constraints are in 160 vertexes +4 constraints are in 32 vertexes +5 constraints are in 4 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 401 vertexes +3 constraints are in 162 vertexes +4 constraints are in 33 vertexes +5 constraints are in 4 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 405 vertexes +3 constraints are in 162 vertexes +4 constraints are in 33 vertexes +5 constraints are in 4 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 411 vertexes +3 constraints are in 162 vertexes +4 constraints are in 33 vertexes +5 constraints are in 4 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 412 vertexes +3 constraints are in 164 vertexes +4 constraints are in 33 vertexes +5 constraints are in 4 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 412 vertexes +3 constraints are in 168 vertexes +4 constraints are in 33 vertexes +5 constraints are in 4 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 416 vertexes +3 constraints are in 170 vertexes +4 constraints are in 33 vertexes +5 constraints are in 4 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 422 vertexes +3 constraints are in 170 vertexes +4 constraints are in 34 vertexes +5 constraints are in 4 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 424 vertexes +3 constraints are in 170 vertexes +4 constraints are in 34 vertexes +5 constraints are in 4 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 430 vertexes +3 constraints are in 170 vertexes +4 constraints are in 34 vertexes +5 constraints are in 4 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 436 vertexes +3 constraints are in 170 vertexes +4 constraints are in 34 vertexes +5 constraints are in 4 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 442 vertexes +3 constraints are in 170 vertexes +4 constraints are in 34 vertexes +5 constraints are in 4 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 444 vertexes +3 constraints are in 172 vertexes +4 constraints are in 34 vertexes +5 constraints are in 4 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 447 vertexes +3 constraints are in 172 vertexes +4 constraints are in 34 vertexes +5 constraints are in 4 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 454 vertexes +3 constraints are in 172 vertexes +4 constraints are in 34 vertexes +5 constraints are in 4 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 459 vertexes +3 constraints are in 173 vertexes +4 constraints are in 34 vertexes +5 constraints are in 4 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 464 vertexes +3 constraints are in 174 vertexes +4 constraints are in 34 vertexes +5 constraints are in 4 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 469 vertexes +3 constraints are in 175 vertexes +4 constraints are in 35 vertexes +5 constraints are in 4 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 472 vertexes +3 constraints are in 176 vertexes +4 constraints are in 35 vertexes +5 constraints are in 4 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 477 vertexes +3 constraints are in 176 vertexes +4 constraints are in 35 vertexes +5 constraints are in 4 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 479 vertexes +3 constraints are in 178 vertexes +4 constraints are in 35 vertexes +5 constraints are in 4 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 482 vertexes +3 constraints are in 179 vertexes +4 constraints are in 35 vertexes +5 constraints are in 4 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 484 vertexes +3 constraints are in 180 vertexes +4 constraints are in 35 vertexes +5 constraints are in 4 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 483 vertexes +3 constraints are in 184 vertexes +4 constraints are in 35 vertexes +5 constraints are in 4 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 488 vertexes +3 constraints are in 184 vertexes +4 constraints are in 35 vertexes +5 constraints are in 4 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 488 vertexes +3 constraints are in 187 vertexes +4 constraints are in 35 vertexes +5 constraints are in 4 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 487 vertexes +3 constraints are in 189 vertexes +4 constraints are in 37 vertexes +5 constraints are in 4 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 490 vertexes +3 constraints are in 191 vertexes +4 constraints are in 38 vertexes +5 constraints are in 4 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 491 vertexes +3 constraints are in 196 vertexes +4 constraints are in 38 vertexes +5 constraints are in 4 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 497 vertexes +3 constraints are in 196 vertexes +4 constraints are in 38 vertexes +5 constraints are in 4 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 501 vertexes +3 constraints are in 198 vertexes +4 constraints are in 38 vertexes +5 constraints are in 4 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 506 vertexes +3 constraints are in 200 vertexes +4 constraints are in 38 vertexes +5 constraints are in 4 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 511 vertexes +3 constraints are in 200 vertexes +4 constraints are in 38 vertexes +5 constraints are in 4 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 514 vertexes +3 constraints are in 200 vertexes +4 constraints are in 38 vertexes +5 constraints are in 4 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 516 vertexes +3 constraints are in 202 vertexes +4 constraints are in 38 vertexes +5 constraints are in 4 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 517 vertexes +3 constraints are in 203 vertexes +4 constraints are in 39 vertexes +5 constraints are in 4 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 519 vertexes +3 constraints are in 207 vertexes +4 constraints are in 39 vertexes +5 constraints are in 4 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 521 vertexes +3 constraints are in 210 vertexes +4 constraints are in 39 vertexes +5 constraints are in 4 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 524 vertexes +3 constraints are in 212 vertexes +4 constraints are in 39 vertexes +5 constraints are in 4 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 530 vertexes +3 constraints are in 212 vertexes +4 constraints are in 39 vertexes +5 constraints are in 4 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 536 vertexes +3 constraints are in 212 vertexes +4 constraints are in 39 vertexes +5 constraints are in 4 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 542 vertexes +3 constraints are in 212 vertexes +4 constraints are in 39 vertexes +5 constraints are in 4 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 544 vertexes +3 constraints are in 215 vertexes +4 constraints are in 40 vertexes +5 constraints are in 4 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 549 vertexes +3 constraints are in 216 vertexes +4 constraints are in 40 vertexes +5 constraints are in 4 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 555 vertexes +3 constraints are in 216 vertexes +4 constraints are in 40 vertexes +5 constraints are in 4 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 561 vertexes +3 constraints are in 216 vertexes +4 constraints are in 40 vertexes +5 constraints are in 4 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 568 vertexes +3 constraints are in 216 vertexes +4 constraints are in 40 vertexes +5 constraints are in 4 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 574 vertexes +3 constraints are in 216 vertexes +4 constraints are in 40 vertexes +5 constraints are in 4 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 578 vertexes +3 constraints are in 218 vertexes +4 constraints are in 40 vertexes +5 constraints are in 4 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 581 vertexes +3 constraints are in 218 vertexes +4 constraints are in 40 vertexes +5 constraints are in 4 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 582 vertexes +3 constraints are in 222 vertexes +4 constraints are in 40 vertexes +5 constraints are in 4 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 584 vertexes +3 constraints are in 226 vertexes +4 constraints are in 40 vertexes +5 constraints are in 4 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 587 vertexes +3 constraints are in 228 vertexes +4 constraints are in 40 vertexes +5 constraints are in 4 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 590 vertexes +3 constraints are in 228 vertexes +4 constraints are in 41 vertexes +5 constraints are in 4 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 596 vertexes +3 constraints are in 228 vertexes +4 constraints are in 41 vertexes +5 constraints are in 4 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 598 vertexes +3 constraints are in 228 vertexes +4 constraints are in 41 vertexes +5 constraints are in 4 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 598 vertexes +3 constraints are in 228 vertexes +4 constraints are in 41 vertexes +5 constraints are in 4 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 596 vertexes +3 constraints are in 235 vertexes +4 constraints are in 41 vertexes +5 constraints are in 4 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 599 vertexes +3 constraints are in 236 vertexes +4 constraints are in 41 vertexes +5 constraints are in 5 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 601 vertexes +3 constraints are in 239 vertexes +4 constraints are in 41 vertexes +5 constraints are in 5 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 607 vertexes +3 constraints are in 239 vertexes +4 constraints are in 41 vertexes +5 constraints are in 5 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 609 vertexes +3 constraints are in 239 vertexes +4 constraints are in 41 vertexes +5 constraints are in 5 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 614 vertexes +3 constraints are in 239 vertexes +4 constraints are in 41 vertexes +5 constraints are in 5 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 620 vertexes +3 constraints are in 239 vertexes +4 constraints are in 41 vertexes +5 constraints are in 5 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 628 vertexes +3 constraints are in 240 vertexes +4 constraints are in 41 vertexes +5 constraints are in 5 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 626 vertexes +3 constraints are in 245 vertexes +4 constraints are in 43 vertexes +5 constraints are in 5 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 632 vertexes +3 constraints are in 246 vertexes +4 constraints are in 43 vertexes +5 constraints are in 5 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 634 vertexes +3 constraints are in 249 vertexes +4 constraints are in 43 vertexes +5 constraints are in 5 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 639 vertexes +3 constraints are in 251 vertexes +4 constraints are in 43 vertexes +5 constraints are in 5 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 644 vertexes +3 constraints are in 251 vertexes +4 constraints are in 44 vertexes +5 constraints are in 5 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 648 vertexes +3 constraints are in 253 vertexes +4 constraints are in 44 vertexes +5 constraints are in 5 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 654 vertexes +3 constraints are in 253 vertexes +4 constraints are in 44 vertexes +5 constraints are in 5 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 658 vertexes +3 constraints are in 255 vertexes +4 constraints are in 44 vertexes +5 constraints are in 5 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 659 vertexes +3 constraints are in 255 vertexes +4 constraints are in 44 vertexes +5 constraints are in 5 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 659 vertexes +3 constraints are in 255 vertexes +4 constraints are in 44 vertexes +5 constraints are in 5 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 664 vertexes +3 constraints are in 255 vertexes +4 constraints are in 44 vertexes +5 constraints are in 5 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 666 vertexes +3 constraints are in 256 vertexes +4 constraints are in 44 vertexes +5 constraints are in 5 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 666 vertexes +3 constraints are in 257 vertexes +4 constraints are in 44 vertexes +5 constraints are in 5 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 668 vertexes +3 constraints are in 257 vertexes +4 constraints are in 44 vertexes +5 constraints are in 5 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 669 vertexes +3 constraints are in 258 vertexes +4 constraints are in 44 vertexes +5 constraints are in 5 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 669 vertexes +3 constraints are in 258 vertexes +4 constraints are in 44 vertexes +5 constraints are in 5 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 667 vertexes +3 constraints are in 261 vertexes +4 constraints are in 44 vertexes +5 constraints are in 5 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 666 vertexes +3 constraints are in 267 vertexes +4 constraints are in 45 vertexes +5 constraints are in 5 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 670 vertexes +3 constraints are in 269 vertexes +4 constraints are in 45 vertexes +5 constraints are in 5 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 674 vertexes +3 constraints are in 271 vertexes +4 constraints are in 46 vertexes +5 constraints are in 5 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 678 vertexes +3 constraints are in 273 vertexes +4 constraints are in 46 vertexes +5 constraints are in 5 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 683 vertexes +3 constraints are in 275 vertexes +4 constraints are in 45 vertexes +5 constraints are in 6 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 684 vertexes +3 constraints are in 278 vertexes +4 constraints are in 45 vertexes +5 constraints are in 6 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 686 vertexes +3 constraints are in 281 vertexes +4 constraints are in 45 vertexes +5 constraints are in 6 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 684 vertexes +3 constraints are in 284 vertexes +4 constraints are in 49 vertexes +5 constraints are in 6 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 687 vertexes +3 constraints are in 284 vertexes +4 constraints are in 49 vertexes +5 constraints are in 6 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 691 vertexes +3 constraints are in 286 vertexes +4 constraints are in 49 vertexes +5 constraints are in 6 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 696 vertexes +3 constraints are in 287 vertexes +4 constraints are in 50 vertexes +5 constraints are in 6 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 700 vertexes +3 constraints are in 287 vertexes +4 constraints are in 51 vertexes +5 constraints are in 6 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 701 vertexes +3 constraints are in 288 vertexes +4 constraints are in 51 vertexes +5 constraints are in 6 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 701 vertexes +3 constraints are in 288 vertexes +4 constraints are in 51 vertexes +5 constraints are in 6 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 703 vertexes +3 constraints are in 289 vertexes +4 constraints are in 52 vertexes +5 constraints are in 6 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 704 vertexes +3 constraints are in 295 vertexes +4 constraints are in 52 vertexes +5 constraints are in 6 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 706 vertexes +3 constraints are in 298 vertexes +4 constraints are in 52 vertexes +5 constraints are in 6 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 710 vertexes +3 constraints are in 300 vertexes +4 constraints are in 52 vertexes +5 constraints are in 6 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 713 vertexes +3 constraints are in 302 vertexes +4 constraints are in 53 vertexes +5 constraints are in 6 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 718 vertexes +3 constraints are in 304 vertexes +4 constraints are in 53 vertexes +5 constraints are in 6 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 722 vertexes +3 constraints are in 306 vertexes +4 constraints are in 53 vertexes +5 constraints are in 6 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 726 vertexes +3 constraints are in 308 vertexes +4 constraints are in 53 vertexes +5 constraints are in 6 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 730 vertexes +3 constraints are in 310 vertexes +4 constraints are in 54 vertexes +5 constraints are in 6 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 735 vertexes +3 constraints are in 309 vertexes +4 constraints are in 55 vertexes +5 constraints are in 6 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 736 vertexes +3 constraints are in 310 vertexes +4 constraints are in 55 vertexes +5 constraints are in 6 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 736 vertexes +3 constraints are in 310 vertexes +4 constraints are in 55 vertexes +5 constraints are in 6 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 737 vertexes +3 constraints are in 310 vertexes +4 constraints are in 55 vertexes +5 constraints are in 6 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 738 vertexes +3 constraints are in 310 vertexes +4 constraints are in 55 vertexes +5 constraints are in 6 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 739 vertexes +3 constraints are in 311 vertexes +4 constraints are in 54 vertexes +5 constraints are in 7 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 744 vertexes +3 constraints are in 312 vertexes +4 constraints are in 54 vertexes +5 constraints are in 7 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 746 vertexes +3 constraints are in 313 vertexes +4 constraints are in 54 vertexes +5 constraints are in 7 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 750 vertexes +3 constraints are in 315 vertexes +4 constraints are in 54 vertexes +5 constraints are in 7 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 753 vertexes +3 constraints are in 319 vertexes +4 constraints are in 54 vertexes +5 constraints are in 7 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 753 vertexes +3 constraints are in 324 vertexes +4 constraints are in 54 vertexes +5 constraints are in 7 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 751 vertexes +3 constraints are in 329 vertexes +4 constraints are in 54 vertexes +5 constraints are in 7 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 758 vertexes +3 constraints are in 329 vertexes +4 constraints are in 54 vertexes +5 constraints are in 8 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 760 vertexes +3 constraints are in 332 vertexes +4 constraints are in 54 vertexes +5 constraints are in 8 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 764 vertexes +3 constraints are in 334 vertexes +4 constraints are in 54 vertexes +5 constraints are in 8 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 769 vertexes +3 constraints are in 334 vertexes +4 constraints are in 55 vertexes +5 constraints are in 8 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 775 vertexes +3 constraints are in 334 vertexes +4 constraints are in 56 vertexes +5 constraints are in 8 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 777 vertexes +3 constraints are in 338 vertexes +4 constraints are in 55 vertexes +5 constraints are in 9 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 777 vertexes +3 constraints are in 338 vertexes +4 constraints are in 55 vertexes +5 constraints are in 9 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 775 vertexes +3 constraints are in 341 vertexes +4 constraints are in 56 vertexes +5 constraints are in 9 vertexes +6 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 777 vertexes +3 constraints are in 343 vertexes +4 constraints are in 55 vertexes +5 constraints are in 9 vertexes +6 constraints are in 3 vertexes +7 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 778 vertexes +3 constraints are in 345 vertexes +4 constraints are in 55 vertexes +5 constraints are in 10 vertexes +6 constraints are in 3 vertexes +7 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 779 vertexes +3 constraints are in 346 vertexes +4 constraints are in 56 vertexes +5 constraints are in 10 vertexes +6 constraints are in 3 vertexes +7 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 778 vertexes +3 constraints are in 348 vertexes +4 constraints are in 58 vertexes +5 constraints are in 11 vertexes +6 constraints are in 3 vertexes +7 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 780 vertexes +3 constraints are in 349 vertexes +4 constraints are in 58 vertexes +5 constraints are in 11 vertexes +6 constraints are in 3 vertexes +7 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 779 vertexes +3 constraints are in 349 vertexes +4 constraints are in 61 vertexes +5 constraints are in 11 vertexes +6 constraints are in 4 vertexes +7 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 780 vertexes +3 constraints are in 349 vertexes +4 constraints are in 61 vertexes +5 constraints are in 15 vertexes +6 constraints are in 4 vertexes +7 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 780 vertexes +3 constraints are in 350 vertexes +4 constraints are in 62 vertexes +5 constraints are in 17 vertexes +6 constraints are in 4 vertexes +7 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 780 vertexes +3 constraints are in 350 vertexes +4 constraints are in 66 vertexes +5 constraints are in 18 vertexes +6 constraints are in 5 vertexes +7 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 781 vertexes +3 constraints are in 350 vertexes +4 constraints are in 66 vertexes +5 constraints are in 18 vertexes +6 constraints are in 5 vertexes +7 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 782 vertexes +3 constraints are in 350 vertexes +4 constraints are in 66 vertexes +5 constraints are in 19 vertexes +6 constraints are in 7 vertexes +7 constraints are in 3 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 783 vertexes +3 constraints are in 351 vertexes +4 constraints are in 66 vertexes +5 constraints are in 22 vertexes +6 constraints are in 6 vertexes +7 constraints are in 4 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 783 vertexes +3 constraints are in 352 vertexes +4 constraints are in 66 vertexes +5 constraints are in 22 vertexes +6 constraints are in 6 vertexes +7 constraints are in 4 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 786 vertexes +3 constraints are in 353 vertexes +4 constraints are in 66 vertexes +5 constraints are in 21 vertexes +6 constraints are in 7 vertexes +7 constraints are in 4 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 790 vertexes +3 constraints are in 353 vertexes +4 constraints are in 67 vertexes +5 constraints are in 21 vertexes +6 constraints are in 7 vertexes +7 constraints are in 4 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 795 vertexes +3 constraints are in 353 vertexes +4 constraints are in 68 vertexes +5 constraints are in 21 vertexes +6 constraints are in 7 vertexes +7 constraints are in 4 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 801 vertexes +3 constraints are in 353 vertexes +4 constraints are in 69 vertexes +5 constraints are in 21 vertexes +6 constraints are in 7 vertexes +7 constraints are in 4 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 803 vertexes +3 constraints are in 353 vertexes +4 constraints are in 71 vertexes +5 constraints are in 21 vertexes +6 constraints are in 7 vertexes +7 constraints are in 4 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 807 vertexes +3 constraints are in 353 vertexes +4 constraints are in 71 vertexes +5 constraints are in 21 vertexes +6 constraints are in 7 vertexes +7 constraints are in 4 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 813 vertexes +3 constraints are in 354 vertexes +4 constraints are in 70 vertexes +5 constraints are in 22 vertexes +6 constraints are in 7 vertexes +7 constraints are in 4 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 818 vertexes +3 constraints are in 354 vertexes +4 constraints are in 70 vertexes +5 constraints are in 22 vertexes +6 constraints are in 7 vertexes +7 constraints are in 4 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 817 vertexes +3 constraints are in 358 vertexes +4 constraints are in 70 vertexes +5 constraints are in 22 vertexes +6 constraints are in 7 vertexes +7 constraints are in 4 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 817 vertexes +3 constraints are in 362 vertexes +4 constraints are in 71 vertexes +5 constraints are in 22 vertexes +6 constraints are in 7 vertexes +7 constraints are in 4 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 823 vertexes +3 constraints are in 362 vertexes +4 constraints are in 71 vertexes +5 constraints are in 22 vertexes +6 constraints are in 7 vertexes +7 constraints are in 4 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 827 vertexes +3 constraints are in 362 vertexes +4 constraints are in 71 vertexes +5 constraints are in 22 vertexes +6 constraints are in 7 vertexes +7 constraints are in 4 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 833 vertexes +3 constraints are in 362 vertexes +4 constraints are in 71 vertexes +5 constraints are in 22 vertexes +6 constraints are in 7 vertexes +7 constraints are in 4 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 835 vertexes +3 constraints are in 362 vertexes +4 constraints are in 71 vertexes +5 constraints are in 22 vertexes +6 constraints are in 7 vertexes +7 constraints are in 4 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 841 vertexes +3 constraints are in 363 vertexes +4 constraints are in 71 vertexes +5 constraints are in 22 vertexes +6 constraints are in 7 vertexes +7 constraints are in 4 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 838 vertexes +3 constraints are in 370 vertexes +4 constraints are in 72 vertexes +5 constraints are in 22 vertexes +6 constraints are in 7 vertexes +7 constraints are in 4 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 842 vertexes +3 constraints are in 372 vertexes +4 constraints are in 72 vertexes +5 constraints are in 22 vertexes +6 constraints are in 7 vertexes +7 constraints are in 4 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 845 vertexes +3 constraints are in 374 vertexes +4 constraints are in 72 vertexes +5 constraints are in 22 vertexes +6 constraints are in 7 vertexes +7 constraints are in 4 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 850 vertexes +3 constraints are in 375 vertexes +4 constraints are in 73 vertexes +5 constraints are in 22 vertexes +6 constraints are in 7 vertexes +7 constraints are in 4 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 853 vertexes +3 constraints are in 376 vertexes +4 constraints are in 74 vertexes +5 constraints are in 22 vertexes +6 constraints are in 7 vertexes +7 constraints are in 4 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 858 vertexes +3 constraints are in 376 vertexes +4 constraints are in 74 vertexes +5 constraints are in 22 vertexes +6 constraints are in 7 vertexes +7 constraints are in 4 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 861 vertexes +3 constraints are in 378 vertexes +4 constraints are in 74 vertexes +5 constraints are in 22 vertexes +6 constraints are in 7 vertexes +7 constraints are in 4 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 864 vertexes +3 constraints are in 380 vertexes +4 constraints are in 74 vertexes +5 constraints are in 22 vertexes +6 constraints are in 7 vertexes +7 constraints are in 4 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 864 vertexes +3 constraints are in 383 vertexes +4 constraints are in 74 vertexes +5 constraints are in 22 vertexes +6 constraints are in 7 vertexes +7 constraints are in 4 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 866 vertexes +3 constraints are in 383 vertexes +4 constraints are in 76 vertexes +5 constraints are in 22 vertexes +6 constraints are in 7 vertexes +7 constraints are in 4 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 867 vertexes +3 constraints are in 384 vertexes +4 constraints are in 76 vertexes +5 constraints are in 22 vertexes +6 constraints are in 7 vertexes +7 constraints are in 4 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 867 vertexes +3 constraints are in 384 vertexes +4 constraints are in 76 vertexes +5 constraints are in 22 vertexes +6 constraints are in 7 vertexes +7 constraints are in 4 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 868 vertexes +3 constraints are in 384 vertexes +4 constraints are in 76 vertexes +5 constraints are in 22 vertexes +6 constraints are in 7 vertexes +7 constraints are in 4 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 870 vertexes +3 constraints are in 383 vertexes +4 constraints are in 77 vertexes +5 constraints are in 22 vertexes +6 constraints are in 7 vertexes +7 constraints are in 4 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 868 vertexes +3 constraints are in 387 vertexes +4 constraints are in 77 vertexes +5 constraints are in 22 vertexes +6 constraints are in 7 vertexes +7 constraints are in 4 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 869 vertexes +3 constraints are in 387 vertexes +4 constraints are in 79 vertexes +5 constraints are in 23 vertexes +6 constraints are in 7 vertexes +7 constraints are in 4 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 876 vertexes +3 constraints are in 387 vertexes +4 constraints are in 79 vertexes +5 constraints are in 24 vertexes +6 constraints are in 7 vertexes +7 constraints are in 4 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 879 vertexes +3 constraints are in 388 vertexes +4 constraints are in 80 vertexes +5 constraints are in 24 vertexes +6 constraints are in 7 vertexes +7 constraints are in 4 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 884 vertexes +3 constraints are in 389 vertexes +4 constraints are in 80 vertexes +5 constraints are in 23 vertexes +6 constraints are in 8 vertexes +7 constraints are in 4 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 889 vertexes +3 constraints are in 388 vertexes +4 constraints are in 82 vertexes +5 constraints are in 23 vertexes +6 constraints are in 8 vertexes +7 constraints are in 4 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 891 vertexes +3 constraints are in 389 vertexes +4 constraints are in 83 vertexes +5 constraints are in 23 vertexes +6 constraints are in 8 vertexes +7 constraints are in 4 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 894 vertexes +3 constraints are in 391 vertexes +4 constraints are in 83 vertexes +5 constraints are in 23 vertexes +6 constraints are in 8 vertexes +7 constraints are in 4 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 898 vertexes +3 constraints are in 393 vertexes +4 constraints are in 82 vertexes +5 constraints are in 24 vertexes +6 constraints are in 8 vertexes +7 constraints are in 4 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 902 vertexes +3 constraints are in 394 vertexes +4 constraints are in 82 vertexes +5 constraints are in 24 vertexes +6 constraints are in 8 vertexes +7 constraints are in 4 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 908 vertexes +3 constraints are in 394 vertexes +4 constraints are in 82 vertexes +5 constraints are in 24 vertexes +6 constraints are in 8 vertexes +7 constraints are in 4 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 914 vertexes +3 constraints are in 394 vertexes +4 constraints are in 82 vertexes +5 constraints are in 24 vertexes +6 constraints are in 8 vertexes +7 constraints are in 4 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 917 vertexes +3 constraints are in 394 vertexes +4 constraints are in 82 vertexes +5 constraints are in 24 vertexes +6 constraints are in 8 vertexes +7 constraints are in 4 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 923 vertexes +3 constraints are in 394 vertexes +4 constraints are in 82 vertexes +5 constraints are in 24 vertexes +6 constraints are in 8 vertexes +7 constraints are in 4 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 929 vertexes +3 constraints are in 394 vertexes +4 constraints are in 82 vertexes +5 constraints are in 24 vertexes +6 constraints are in 8 vertexes +7 constraints are in 4 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 935 vertexes +3 constraints are in 394 vertexes +4 constraints are in 82 vertexes +5 constraints are in 24 vertexes +6 constraints are in 8 vertexes +7 constraints are in 4 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 941 vertexes +3 constraints are in 394 vertexes +4 constraints are in 82 vertexes +5 constraints are in 24 vertexes +6 constraints are in 8 vertexes +7 constraints are in 4 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 944 vertexes +3 constraints are in 394 vertexes +4 constraints are in 82 vertexes +5 constraints are in 24 vertexes +6 constraints are in 8 vertexes +7 constraints are in 4 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 944 vertexes +3 constraints are in 394 vertexes +4 constraints are in 82 vertexes +5 constraints are in 24 vertexes +6 constraints are in 8 vertexes +7 constraints are in 4 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 947 vertexes +3 constraints are in 395 vertexes +4 constraints are in 83 vertexes +5 constraints are in 24 vertexes +6 constraints are in 8 vertexes +7 constraints are in 4 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 951 vertexes +3 constraints are in 396 vertexes +4 constraints are in 83 vertexes +5 constraints are in 24 vertexes +6 constraints are in 8 vertexes +7 constraints are in 4 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 956 vertexes +3 constraints are in 397 vertexes +4 constraints are in 84 vertexes +5 constraints are in 24 vertexes +6 constraints are in 8 vertexes +7 constraints are in 4 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 960 vertexes +3 constraints are in 398 vertexes +4 constraints are in 83 vertexes +5 constraints are in 25 vertexes +6 constraints are in 8 vertexes +7 constraints are in 4 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 961 vertexes +3 constraints are in 399 vertexes +4 constraints are in 83 vertexes +5 constraints are in 25 vertexes +6 constraints are in 8 vertexes +7 constraints are in 4 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 964 vertexes +3 constraints are in 400 vertexes +4 constraints are in 82 vertexes +5 constraints are in 26 vertexes +6 constraints are in 7 vertexes +7 constraints are in 5 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 966 vertexes +3 constraints are in 400 vertexes +4 constraints are in 83 vertexes +5 constraints are in 26 vertexes +6 constraints are in 7 vertexes +7 constraints are in 5 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 965 vertexes +3 constraints are in 402 vertexes +4 constraints are in 84 vertexes +5 constraints are in 26 vertexes +6 constraints are in 7 vertexes +7 constraints are in 5 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 966 vertexes +3 constraints are in 403 vertexes +4 constraints are in 86 vertexes +5 constraints are in 26 vertexes +6 constraints are in 8 vertexes +7 constraints are in 5 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 970 vertexes +3 constraints are in 405 vertexes +4 constraints are in 86 vertexes +5 constraints are in 26 vertexes +6 constraints are in 8 vertexes +7 constraints are in 5 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 974 vertexes +3 constraints are in 407 vertexes +4 constraints are in 86 vertexes +5 constraints are in 26 vertexes +6 constraints are in 8 vertexes +7 constraints are in 5 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 978 vertexes +3 constraints are in 409 vertexes +4 constraints are in 85 vertexes +5 constraints are in 27 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 984 vertexes +3 constraints are in 410 vertexes +4 constraints are in 83 vertexes +5 constraints are in 29 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 987 vertexes +3 constraints are in 413 vertexes +4 constraints are in 83 vertexes +5 constraints are in 29 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 991 vertexes +3 constraints are in 415 vertexes +4 constraints are in 83 vertexes +5 constraints are in 29 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 995 vertexes +3 constraints are in 416 vertexes +4 constraints are in 82 vertexes +5 constraints are in 30 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 999 vertexes +3 constraints are in 418 vertexes +4 constraints are in 82 vertexes +5 constraints are in 30 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1003 vertexes +3 constraints are in 420 vertexes +4 constraints are in 82 vertexes +5 constraints are in 30 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1004 vertexes +3 constraints are in 423 vertexes +4 constraints are in 83 vertexes +5 constraints are in 31 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1010 vertexes +3 constraints are in 423 vertexes +4 constraints are in 83 vertexes +5 constraints are in 31 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1015 vertexes +3 constraints are in 423 vertexes +4 constraints are in 83 vertexes +5 constraints are in 31 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1019 vertexes +3 constraints are in 423 vertexes +4 constraints are in 83 vertexes +5 constraints are in 31 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1021 vertexes +3 constraints are in 423 vertexes +4 constraints are in 83 vertexes +5 constraints are in 31 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1023 vertexes +3 constraints are in 425 vertexes +4 constraints are in 83 vertexes +5 constraints are in 31 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1024 vertexes +3 constraints are in 425 vertexes +4 constraints are in 83 vertexes +5 constraints are in 31 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1023 vertexes +3 constraints are in 429 vertexes +4 constraints are in 83 vertexes +5 constraints are in 31 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1025 vertexes +3 constraints are in 429 vertexes +4 constraints are in 83 vertexes +5 constraints are in 31 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1027 vertexes +3 constraints are in 429 vertexes +4 constraints are in 84 vertexes +5 constraints are in 31 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1030 vertexes +3 constraints are in 429 vertexes +4 constraints are in 84 vertexes +5 constraints are in 31 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1031 vertexes +3 constraints are in 429 vertexes +4 constraints are in 84 vertexes +5 constraints are in 31 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1031 vertexes +3 constraints are in 429 vertexes +4 constraints are in 84 vertexes +5 constraints are in 31 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1031 vertexes +3 constraints are in 429 vertexes +4 constraints are in 84 vertexes +5 constraints are in 31 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 1033 vertexes +3 constraints are in 430 vertexes +4 constraints are in 84 vertexes +5 constraints are in 31 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 1033 vertexes +3 constraints are in 430 vertexes +4 constraints are in 84 vertexes +5 constraints are in 31 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1033 vertexes +3 constraints are in 431 vertexes +4 constraints are in 84 vertexes +5 constraints are in 31 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1034 vertexes +3 constraints are in 431 vertexes +4 constraints are in 84 vertexes +5 constraints are in 31 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1033 vertexes +3 constraints are in 435 vertexes +4 constraints are in 84 vertexes +5 constraints are in 31 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1035 vertexes +3 constraints are in 437 vertexes +4 constraints are in 84 vertexes +5 constraints are in 31 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 1041 vertexes +3 constraints are in 438 vertexes +4 constraints are in 85 vertexes +5 constraints are in 31 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 1045 vertexes +3 constraints are in 438 vertexes +4 constraints are in 86 vertexes +5 constraints are in 31 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1046 vertexes +3 constraints are in 439 vertexes +4 constraints are in 86 vertexes +5 constraints are in 31 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1051 vertexes +3 constraints are in 439 vertexes +4 constraints are in 87 vertexes +5 constraints are in 31 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1056 vertexes +3 constraints are in 439 vertexes +4 constraints are in 88 vertexes +5 constraints are in 31 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1060 vertexes +3 constraints are in 441 vertexes +4 constraints are in 88 vertexes +5 constraints are in 31 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1062 vertexes +3 constraints are in 441 vertexes +4 constraints are in 88 vertexes +5 constraints are in 31 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1062 vertexes +3 constraints are in 441 vertexes +4 constraints are in 88 vertexes +5 constraints are in 31 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1065 vertexes +3 constraints are in 441 vertexes +4 constraints are in 88 vertexes +5 constraints are in 31 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 1068 vertexes +3 constraints are in 444 vertexes +4 constraints are in 89 vertexes +5 constraints are in 31 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1070 vertexes +3 constraints are in 447 vertexes +4 constraints are in 89 vertexes +5 constraints are in 31 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1076 vertexes +3 constraints are in 447 vertexes +4 constraints are in 90 vertexes +5 constraints are in 31 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1082 vertexes +3 constraints are in 447 vertexes +4 constraints are in 90 vertexes +5 constraints are in 31 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1087 vertexes +3 constraints are in 447 vertexes +4 constraints are in 92 vertexes +5 constraints are in 31 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1093 vertexes +3 constraints are in 447 vertexes +4 constraints are in 92 vertexes +5 constraints are in 31 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1099 vertexes +3 constraints are in 447 vertexes +4 constraints are in 92 vertexes +5 constraints are in 31 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1101 vertexes +3 constraints are in 447 vertexes +4 constraints are in 92 vertexes +5 constraints are in 31 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1101 vertexes +3 constraints are in 447 vertexes +4 constraints are in 92 vertexes +5 constraints are in 31 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 1102 vertexes +3 constraints are in 448 vertexes +4 constraints are in 92 vertexes +5 constraints are in 31 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 1101 vertexes +3 constraints are in 450 vertexes +4 constraints are in 92 vertexes +5 constraints are in 31 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1103 vertexes +3 constraints are in 451 vertexes +4 constraints are in 92 vertexes +5 constraints are in 31 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1104 vertexes +3 constraints are in 451 vertexes +4 constraints are in 92 vertexes +5 constraints are in 31 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1105 vertexes +3 constraints are in 453 vertexes +4 constraints are in 92 vertexes +5 constraints are in 31 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1105 vertexes +3 constraints are in 453 vertexes +4 constraints are in 92 vertexes +5 constraints are in 31 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1108 vertexes +3 constraints are in 453 vertexes +4 constraints are in 92 vertexes +5 constraints are in 31 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1112 vertexes +3 constraints are in 453 vertexes +4 constraints are in 93 vertexes +5 constraints are in 31 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 1118 vertexes +3 constraints are in 454 vertexes +4 constraints are in 93 vertexes +5 constraints are in 31 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 1117 vertexes +3 constraints are in 456 vertexes +4 constraints are in 93 vertexes +5 constraints are in 31 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 1116 vertexes +3 constraints are in 460 vertexes +4 constraints are in 93 vertexes +5 constraints are in 31 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1117 vertexes +3 constraints are in 464 vertexes +4 constraints are in 92 vertexes +5 constraints are in 32 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1121 vertexes +3 constraints are in 465 vertexes +4 constraints are in 92 vertexes +5 constraints are in 33 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1125 vertexes +3 constraints are in 467 vertexes +4 constraints are in 92 vertexes +5 constraints are in 33 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1130 vertexes +3 constraints are in 467 vertexes +4 constraints are in 92 vertexes +5 constraints are in 33 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1135 vertexes +3 constraints are in 467 vertexes +4 constraints are in 92 vertexes +5 constraints are in 33 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1140 vertexes +3 constraints are in 467 vertexes +4 constraints are in 92 vertexes +5 constraints are in 33 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1142 vertexes +3 constraints are in 471 vertexes +4 constraints are in 92 vertexes +5 constraints are in 33 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1146 vertexes +3 constraints are in 473 vertexes +4 constraints are in 92 vertexes +5 constraints are in 33 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 1150 vertexes +3 constraints are in 476 vertexes +4 constraints are in 93 vertexes +5 constraints are in 33 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1150 vertexes +3 constraints are in 479 vertexes +4 constraints are in 93 vertexes +5 constraints are in 33 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1154 vertexes +3 constraints are in 481 vertexes +4 constraints are in 93 vertexes +5 constraints are in 33 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 1158 vertexes +3 constraints are in 482 vertexes +4 constraints are in 94 vertexes +5 constraints are in 33 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 1161 vertexes +3 constraints are in 484 vertexes +4 constraints are in 94 vertexes +5 constraints are in 33 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1164 vertexes +3 constraints are in 485 vertexes +4 constraints are in 95 vertexes +5 constraints are in 33 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 1169 vertexes +3 constraints are in 486 vertexes +4 constraints are in 95 vertexes +5 constraints are in 33 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1171 vertexes +3 constraints are in 487 vertexes +4 constraints are in 95 vertexes +5 constraints are in 33 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1174 vertexes +3 constraints are in 488 vertexes +4 constraints are in 94 vertexes +5 constraints are in 34 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1176 vertexes +3 constraints are in 488 vertexes +4 constraints are in 95 vertexes +5 constraints are in 34 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 1181 vertexes +3 constraints are in 491 vertexes +4 constraints are in 95 vertexes +5 constraints are in 34 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1183 vertexes +3 constraints are in 492 vertexes +4 constraints are in 95 vertexes +5 constraints are in 34 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1186 vertexes +3 constraints are in 492 vertexes +4 constraints are in 95 vertexes +5 constraints are in 34 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1188 vertexes +3 constraints are in 492 vertexes +4 constraints are in 95 vertexes +5 constraints are in 34 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 1189 vertexes +3 constraints are in 493 vertexes +4 constraints are in 95 vertexes +5 constraints are in 34 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 1189 vertexes +3 constraints are in 497 vertexes +4 constraints are in 97 vertexes +5 constraints are in 34 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1193 vertexes +3 constraints are in 498 vertexes +4 constraints are in 98 vertexes +5 constraints are in 34 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1194 vertexes +3 constraints are in 498 vertexes +4 constraints are in 98 vertexes +5 constraints are in 34 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1198 vertexes +3 constraints are in 498 vertexes +4 constraints are in 98 vertexes +5 constraints are in 34 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1200 vertexes +3 constraints are in 498 vertexes +4 constraints are in 98 vertexes +5 constraints are in 34 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1200 vertexes +3 constraints are in 502 vertexes +4 constraints are in 98 vertexes +5 constraints are in 34 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 1200 vertexes +3 constraints are in 505 vertexes +4 constraints are in 98 vertexes +5 constraints are in 34 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1201 vertexes +3 constraints are in 506 vertexes +4 constraints are in 98 vertexes +5 constraints are in 34 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1206 vertexes +3 constraints are in 506 vertexes +4 constraints are in 98 vertexes +5 constraints are in 34 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1210 vertexes +3 constraints are in 506 vertexes +4 constraints are in 98 vertexes +5 constraints are in 34 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1211 vertexes +3 constraints are in 506 vertexes +4 constraints are in 98 vertexes +5 constraints are in 34 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 1209 vertexes +3 constraints are in 513 vertexes +4 constraints are in 98 vertexes +5 constraints are in 34 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1211 vertexes +3 constraints are in 516 vertexes +4 constraints are in 98 vertexes +5 constraints are in 34 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1216 vertexes +3 constraints are in 516 vertexes +4 constraints are in 99 vertexes +5 constraints are in 34 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1219 vertexes +3 constraints are in 516 vertexes +4 constraints are in 99 vertexes +5 constraints are in 34 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1221 vertexes +3 constraints are in 518 vertexes +4 constraints are in 99 vertexes +5 constraints are in 34 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1228 vertexes +3 constraints are in 518 vertexes +4 constraints are in 99 vertexes +5 constraints are in 34 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1231 vertexes +3 constraints are in 520 vertexes +4 constraints are in 99 vertexes +5 constraints are in 34 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1237 vertexes +3 constraints are in 520 vertexes +4 constraints are in 99 vertexes +5 constraints are in 34 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 1244 vertexes +3 constraints are in 519 vertexes +4 constraints are in 100 vertexes +5 constraints are in 34 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1248 vertexes +3 constraints are in 520 vertexes +4 constraints are in 100 vertexes +5 constraints are in 34 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1252 vertexes +3 constraints are in 523 vertexes +4 constraints are in 99 vertexes +5 constraints are in 35 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1256 vertexes +3 constraints are in 523 vertexes +4 constraints are in 99 vertexes +5 constraints are in 35 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1258 vertexes +3 constraints are in 523 vertexes +4 constraints are in 99 vertexes +5 constraints are in 35 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1255 vertexes +3 constraints are in 531 vertexes +4 constraints are in 99 vertexes +5 constraints are in 35 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1259 vertexes +3 constraints are in 533 vertexes +4 constraints are in 100 vertexes +5 constraints are in 35 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 1264 vertexes +3 constraints are in 534 vertexes +4 constraints are in 101 vertexes +5 constraints are in 35 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1267 vertexes +3 constraints are in 535 vertexes +4 constraints are in 101 vertexes +5 constraints are in 35 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1270 vertexes +3 constraints are in 537 vertexes +4 constraints are in 101 vertexes +5 constraints are in 35 vertexes +6 constraints are in 7 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 1274 vertexes +3 constraints are in 539 vertexes +4 constraints are in 101 vertexes +5 constraints are in 34 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 1277 vertexes +3 constraints are in 541 vertexes +4 constraints are in 101 vertexes +5 constraints are in 34 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1276 vertexes +3 constraints are in 542 vertexes +4 constraints are in 101 vertexes +5 constraints are in 34 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1277 vertexes +3 constraints are in 546 vertexes +4 constraints are in 102 vertexes +5 constraints are in 34 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1281 vertexes +3 constraints are in 546 vertexes +4 constraints are in 103 vertexes +5 constraints are in 34 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 1283 vertexes +3 constraints are in 549 vertexes +4 constraints are in 105 vertexes +5 constraints are in 34 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 1283 vertexes +3 constraints are in 552 vertexes +4 constraints are in 107 vertexes +5 constraints are in 35 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 1283 vertexes +3 constraints are in 553 vertexes +4 constraints are in 109 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1282 vertexes +3 constraints are in 554 vertexes +4 constraints are in 111 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1285 vertexes +3 constraints are in 554 vertexes +4 constraints are in 113 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 1289 vertexes +3 constraints are in 555 vertexes +4 constraints are in 114 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 1290 vertexes +3 constraints are in 557 vertexes +4 constraints are in 115 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1291 vertexes +3 constraints are in 560 vertexes +4 constraints are in 115 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1294 vertexes +3 constraints are in 562 vertexes +4 constraints are in 115 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1300 vertexes +3 constraints are in 562 vertexes +4 constraints are in 115 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1304 vertexes +3 constraints are in 562 vertexes +4 constraints are in 115 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1306 vertexes +3 constraints are in 564 vertexes +4 constraints are in 115 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 1311 vertexes +3 constraints are in 567 vertexes +4 constraints are in 115 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 1313 vertexes +3 constraints are in 569 vertexes +4 constraints are in 115 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1315 vertexes +3 constraints are in 572 vertexes +4 constraints are in 115 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1318 vertexes +3 constraints are in 572 vertexes +4 constraints are in 116 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1318 vertexes +3 constraints are in 574 vertexes +4 constraints are in 116 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1318 vertexes +3 constraints are in 576 vertexes +4 constraints are in 118 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1320 vertexes +3 constraints are in 576 vertexes +4 constraints are in 119 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 1324 vertexes +3 constraints are in 579 vertexes +4 constraints are in 119 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 1326 vertexes +3 constraints are in 583 vertexes +4 constraints are in 119 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1326 vertexes +3 constraints are in 588 vertexes +4 constraints are in 119 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 1328 vertexes +3 constraints are in 589 vertexes +4 constraints are in 121 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1330 vertexes +3 constraints are in 592 vertexes +4 constraints are in 121 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 1336 vertexes +3 constraints are in 591 vertexes +4 constraints are in 123 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1340 vertexes +3 constraints are in 592 vertexes +4 constraints are in 123 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1346 vertexes +3 constraints are in 592 vertexes +4 constraints are in 123 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1352 vertexes +3 constraints are in 592 vertexes +4 constraints are in 123 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 1355 vertexes +3 constraints are in 595 vertexes +4 constraints are in 124 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1359 vertexes +3 constraints are in 596 vertexes +4 constraints are in 124 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1365 vertexes +3 constraints are in 596 vertexes +4 constraints are in 124 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1369 vertexes +3 constraints are in 596 vertexes +4 constraints are in 125 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 1372 vertexes +3 constraints are in 597 vertexes +4 constraints are in 125 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1372 vertexes +3 constraints are in 600 vertexes +4 constraints are in 125 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1376 vertexes +3 constraints are in 602 vertexes +4 constraints are in 125 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1379 vertexes +3 constraints are in 602 vertexes +4 constraints are in 125 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1381 vertexes +3 constraints are in 602 vertexes +4 constraints are in 125 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1382 vertexes +3 constraints are in 606 vertexes +4 constraints are in 125 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1388 vertexes +3 constraints are in 606 vertexes +4 constraints are in 125 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1392 vertexes +3 constraints are in 608 vertexes +4 constraints are in 126 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1398 vertexes +3 constraints are in 608 vertexes +4 constraints are in 126 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 1404 vertexes +3 constraints are in 609 vertexes +4 constraints are in 126 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1403 vertexes +3 constraints are in 612 vertexes +4 constraints are in 126 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1406 vertexes +3 constraints are in 612 vertexes +4 constraints are in 126 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1408 vertexes +3 constraints are in 614 vertexes +4 constraints are in 126 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1414 vertexes +3 constraints are in 614 vertexes +4 constraints are in 126 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1421 vertexes +3 constraints are in 614 vertexes +4 constraints are in 126 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1424 vertexes +3 constraints are in 616 vertexes +4 constraints are in 126 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1425 vertexes +3 constraints are in 616 vertexes +4 constraints are in 126 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 1425 vertexes +3 constraints are in 621 vertexes +4 constraints are in 126 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1429 vertexes +3 constraints are in 622 vertexes +4 constraints are in 126 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1433 vertexes +3 constraints are in 624 vertexes +4 constraints are in 126 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1437 vertexes +3 constraints are in 626 vertexes +4 constraints are in 126 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1441 vertexes +3 constraints are in 628 vertexes +4 constraints are in 126 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1444 vertexes +3 constraints are in 628 vertexes +4 constraints are in 126 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1445 vertexes +3 constraints are in 630 vertexes +4 constraints are in 126 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1449 vertexes +3 constraints are in 630 vertexes +4 constraints are in 126 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 1456 vertexes +3 constraints are in 631 vertexes +4 constraints are in 126 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1460 vertexes +3 constraints are in 632 vertexes +4 constraints are in 126 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 1462 vertexes +3 constraints are in 637 vertexes +4 constraints are in 126 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1467 vertexes +3 constraints are in 638 vertexes +4 constraints are in 126 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1470 vertexes +3 constraints are in 640 vertexes +4 constraints are in 126 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1473 vertexes +3 constraints are in 640 vertexes +4 constraints are in 126 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1478 vertexes +3 constraints are in 640 vertexes +4 constraints are in 126 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 1477 vertexes +3 constraints are in 645 vertexes +4 constraints are in 127 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1481 vertexes +3 constraints are in 646 vertexes +4 constraints are in 127 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1487 vertexes +3 constraints are in 646 vertexes +4 constraints are in 128 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1490 vertexes +3 constraints are in 646 vertexes +4 constraints are in 128 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 1493 vertexes +3 constraints are in 649 vertexes +4 constraints are in 128 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1497 vertexes +3 constraints are in 650 vertexes +4 constraints are in 128 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1501 vertexes +3 constraints are in 652 vertexes +4 constraints are in 128 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1504 vertexes +3 constraints are in 654 vertexes +4 constraints are in 128 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1507 vertexes +3 constraints are in 654 vertexes +4 constraints are in 128 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1512 vertexes +3 constraints are in 654 vertexes +4 constraints are in 128 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1513 vertexes +3 constraints are in 654 vertexes +4 constraints are in 128 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1513 vertexes +3 constraints are in 654 vertexes +4 constraints are in 128 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 1513 vertexes +3 constraints are in 657 vertexes +4 constraints are in 128 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 1514 vertexes +3 constraints are in 663 vertexes +4 constraints are in 128 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1516 vertexes +3 constraints are in 666 vertexes +4 constraints are in 128 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1520 vertexes +3 constraints are in 668 vertexes +4 constraints are in 128 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1524 vertexes +3 constraints are in 670 vertexes +4 constraints are in 128 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1529 vertexes +3 constraints are in 672 vertexes +4 constraints are in 128 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1533 vertexes +3 constraints are in 674 vertexes +4 constraints are in 128 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1539 vertexes +3 constraints are in 674 vertexes +4 constraints are in 128 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1545 vertexes +3 constraints are in 674 vertexes +4 constraints are in 128 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1551 vertexes +3 constraints are in 674 vertexes +4 constraints are in 128 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 1557 vertexes +3 constraints are in 675 vertexes +4 constraints are in 128 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1558 vertexes +3 constraints are in 678 vertexes +4 constraints are in 128 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1558 vertexes +3 constraints are in 684 vertexes +4 constraints are in 128 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1560 vertexes +3 constraints are in 686 vertexes +4 constraints are in 128 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1562 vertexes +3 constraints are in 686 vertexes +4 constraints are in 128 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1564 vertexes +3 constraints are in 686 vertexes +4 constraints are in 128 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1564 vertexes +3 constraints are in 686 vertexes +4 constraints are in 128 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1564 vertexes +3 constraints are in 686 vertexes +4 constraints are in 128 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1564 vertexes +3 constraints are in 690 vertexes +4 constraints are in 129 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1568 vertexes +3 constraints are in 692 vertexes +4 constraints are in 129 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 1572 vertexes +3 constraints are in 693 vertexes +4 constraints are in 130 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 1572 vertexes +3 constraints are in 695 vertexes +4 constraints are in 130 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 1572 vertexes +3 constraints are in 695 vertexes +4 constraints are in 130 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1570 vertexes +3 constraints are in 698 vertexes +4 constraints are in 130 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1572 vertexes +3 constraints are in 700 vertexes +4 constraints are in 130 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 1574 vertexes +3 constraints are in 701 vertexes +4 constraints are in 131 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1574 vertexes +3 constraints are in 704 vertexes +4 constraints are in 133 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1577 vertexes +3 constraints are in 706 vertexes +4 constraints are in 134 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1578 vertexes +3 constraints are in 706 vertexes +4 constraints are in 134 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1578 vertexes +3 constraints are in 706 vertexes +4 constraints are in 134 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 1579 vertexes +3 constraints are in 707 vertexes +4 constraints are in 134 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1578 vertexes +3 constraints are in 712 vertexes +4 constraints are in 135 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1584 vertexes +3 constraints are in 712 vertexes +4 constraints are in 136 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1588 vertexes +3 constraints are in 714 vertexes +4 constraints are in 136 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1592 vertexes +3 constraints are in 716 vertexes +4 constraints are in 136 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1598 vertexes +3 constraints are in 716 vertexes +4 constraints are in 136 vertexes +5 constraints are in 36 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1603 vertexes +3 constraints are in 717 vertexes +4 constraints are in 135 vertexes +5 constraints are in 37 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1605 vertexes +3 constraints are in 717 vertexes +4 constraints are in 135 vertexes +5 constraints are in 37 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1609 vertexes +3 constraints are in 720 vertexes +4 constraints are in 134 vertexes +5 constraints are in 38 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1613 vertexes +3 constraints are in 722 vertexes +4 constraints are in 134 vertexes +5 constraints are in 38 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1616 vertexes +3 constraints are in 724 vertexes +4 constraints are in 135 vertexes +5 constraints are in 38 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1620 vertexes +3 constraints are in 726 vertexes +4 constraints are in 135 vertexes +5 constraints are in 38 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 1624 vertexes +3 constraints are in 729 vertexes +4 constraints are in 135 vertexes +5 constraints are in 38 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 1628 vertexes +3 constraints are in 731 vertexes +4 constraints are in 135 vertexes +5 constraints are in 38 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1630 vertexes +3 constraints are in 734 vertexes +4 constraints are in 135 vertexes +5 constraints are in 38 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1635 vertexes +3 constraints are in 735 vertexes +4 constraints are in 134 vertexes +5 constraints are in 39 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1639 vertexes +3 constraints are in 737 vertexes +4 constraints are in 134 vertexes +5 constraints are in 39 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1643 vertexes +3 constraints are in 740 vertexes +4 constraints are in 133 vertexes +5 constraints are in 40 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1648 vertexes +3 constraints are in 741 vertexes +4 constraints are in 132 vertexes +5 constraints are in 41 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1652 vertexes +3 constraints are in 741 vertexes +4 constraints are in 133 vertexes +5 constraints are in 41 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 1655 vertexes +3 constraints are in 745 vertexes +4 constraints are in 133 vertexes +5 constraints are in 42 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1657 vertexes +3 constraints are in 748 vertexes +4 constraints are in 133 vertexes +5 constraints are in 42 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1662 vertexes +3 constraints are in 748 vertexes +4 constraints are in 134 vertexes +5 constraints are in 42 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1668 vertexes +3 constraints are in 748 vertexes +4 constraints are in 134 vertexes +5 constraints are in 42 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1671 vertexes +3 constraints are in 748 vertexes +4 constraints are in 134 vertexes +5 constraints are in 42 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1671 vertexes +3 constraints are in 748 vertexes +4 constraints are in 134 vertexes +5 constraints are in 42 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1673 vertexes +3 constraints are in 748 vertexes +4 constraints are in 134 vertexes +5 constraints are in 42 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1679 vertexes +3 constraints are in 748 vertexes +4 constraints are in 134 vertexes +5 constraints are in 42 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1685 vertexes +3 constraints are in 748 vertexes +4 constraints are in 134 vertexes +5 constraints are in 42 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1687 vertexes +3 constraints are in 750 vertexes +4 constraints are in 134 vertexes +5 constraints are in 42 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1691 vertexes +3 constraints are in 750 vertexes +4 constraints are in 134 vertexes +5 constraints are in 42 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1693 vertexes +3 constraints are in 750 vertexes +4 constraints are in 134 vertexes +5 constraints are in 42 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1699 vertexes +3 constraints are in 750 vertexes +4 constraints are in 134 vertexes +5 constraints are in 42 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 1700 vertexes +3 constraints are in 753 vertexes +4 constraints are in 134 vertexes +5 constraints are in 42 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 1704 vertexes +3 constraints are in 755 vertexes +4 constraints are in 134 vertexes +5 constraints are in 42 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 1707 vertexes +3 constraints are in 757 vertexes +4 constraints are in 134 vertexes +5 constraints are in 42 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1707 vertexes +3 constraints are in 760 vertexes +4 constraints are in 134 vertexes +5 constraints are in 42 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1713 vertexes +3 constraints are in 760 vertexes +4 constraints are in 134 vertexes +5 constraints are in 42 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 1718 vertexes +3 constraints are in 763 vertexes +4 constraints are in 134 vertexes +5 constraints are in 42 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1722 vertexes +3 constraints are in 764 vertexes +4 constraints are in 134 vertexes +5 constraints are in 42 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1724 vertexes +3 constraints are in 766 vertexes +4 constraints are in 134 vertexes +5 constraints are in 42 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1728 vertexes +3 constraints are in 768 vertexes +4 constraints are in 134 vertexes +5 constraints are in 42 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1733 vertexes +3 constraints are in 770 vertexes +4 constraints are in 134 vertexes +5 constraints are in 42 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1739 vertexes +3 constraints are in 770 vertexes +4 constraints are in 134 vertexes +5 constraints are in 42 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1741 vertexes +3 constraints are in 772 vertexes +4 constraints are in 134 vertexes +5 constraints are in 42 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1742 vertexes +3 constraints are in 772 vertexes +4 constraints are in 134 vertexes +5 constraints are in 42 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1743 vertexes +3 constraints are in 772 vertexes +4 constraints are in 134 vertexes +5 constraints are in 42 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1746 vertexes +3 constraints are in 772 vertexes +4 constraints are in 134 vertexes +5 constraints are in 42 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1747 vertexes +3 constraints are in 772 vertexes +4 constraints are in 134 vertexes +5 constraints are in 42 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 1745 vertexes +3 constraints are in 779 vertexes +4 constraints are in 134 vertexes +5 constraints are in 42 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1746 vertexes +3 constraints are in 782 vertexes +4 constraints are in 135 vertexes +5 constraints are in 42 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1751 vertexes +3 constraints are in 782 vertexes +4 constraints are in 136 vertexes +5 constraints are in 42 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1755 vertexes +3 constraints are in 784 vertexes +4 constraints are in 136 vertexes +5 constraints are in 42 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1761 vertexes +3 constraints are in 784 vertexes +4 constraints are in 137 vertexes +5 constraints are in 42 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 1767 vertexes +3 constraints are in 784 vertexes +4 constraints are in 137 vertexes +5 constraints are in 43 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1770 vertexes +3 constraints are in 785 vertexes +4 constraints are in 138 vertexes +5 constraints are in 43 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 1776 vertexes +3 constraints are in 784 vertexes +4 constraints are in 139 vertexes +5 constraints are in 43 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 1781 vertexes +3 constraints are in 785 vertexes +4 constraints are in 138 vertexes +5 constraints are in 44 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 1783 vertexes +3 constraints are in 789 vertexes +4 constraints are in 139 vertexes +5 constraints are in 44 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1786 vertexes +3 constraints are in 792 vertexes +4 constraints are in 139 vertexes +5 constraints are in 44 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1790 vertexes +3 constraints are in 794 vertexes +4 constraints are in 139 vertexes +5 constraints are in 44 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1793 vertexes +3 constraints are in 794 vertexes +4 constraints are in 139 vertexes +5 constraints are in 44 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1797 vertexes +3 constraints are in 796 vertexes +4 constraints are in 139 vertexes +5 constraints are in 44 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1800 vertexes +3 constraints are in 798 vertexes +4 constraints are in 139 vertexes +5 constraints are in 44 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 1807 vertexes +3 constraints are in 797 vertexes +4 constraints are in 140 vertexes +5 constraints are in 44 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1808 vertexes +3 constraints are in 800 vertexes +4 constraints are in 140 vertexes +5 constraints are in 44 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1809 vertexes +3 constraints are in 802 vertexes +4 constraints are in 140 vertexes +5 constraints are in 44 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 1810 vertexes +3 constraints are in 805 vertexes +4 constraints are in 140 vertexes +5 constraints are in 44 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1811 vertexes +3 constraints are in 806 vertexes +4 constraints are in 141 vertexes +5 constraints are in 44 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1811 vertexes +3 constraints are in 806 vertexes +4 constraints are in 141 vertexes +5 constraints are in 44 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1811 vertexes +3 constraints are in 806 vertexes +4 constraints are in 141 vertexes +5 constraints are in 44 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1811 vertexes +3 constraints are in 806 vertexes +4 constraints are in 141 vertexes +5 constraints are in 44 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1811 vertexes +3 constraints are in 806 vertexes +4 constraints are in 141 vertexes +5 constraints are in 44 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1811 vertexes +3 constraints are in 806 vertexes +4 constraints are in 141 vertexes +5 constraints are in 44 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1811 vertexes +3 constraints are in 806 vertexes +4 constraints are in 141 vertexes +5 constraints are in 44 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1811 vertexes +3 constraints are in 806 vertexes +4 constraints are in 141 vertexes +5 constraints are in 44 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1811 vertexes +3 constraints are in 806 vertexes +4 constraints are in 141 vertexes +5 constraints are in 44 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1811 vertexes +3 constraints are in 806 vertexes +4 constraints are in 141 vertexes +5 constraints are in 44 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1811 vertexes +3 constraints are in 806 vertexes +4 constraints are in 141 vertexes +5 constraints are in 44 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1811 vertexes +3 constraints are in 806 vertexes +4 constraints are in 141 vertexes +5 constraints are in 44 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1811 vertexes +3 constraints are in 806 vertexes +4 constraints are in 141 vertexes +5 constraints are in 44 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1811 vertexes +3 constraints are in 806 vertexes +4 constraints are in 141 vertexes +5 constraints are in 44 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1811 vertexes +3 constraints are in 806 vertexes +4 constraints are in 141 vertexes +5 constraints are in 44 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1811 vertexes +3 constraints are in 806 vertexes +4 constraints are in 141 vertexes +5 constraints are in 44 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1811 vertexes +3 constraints are in 806 vertexes +4 constraints are in 141 vertexes +5 constraints are in 44 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1811 vertexes +3 constraints are in 806 vertexes +4 constraints are in 141 vertexes +5 constraints are in 44 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1811 vertexes +3 constraints are in 806 vertexes +4 constraints are in 141 vertexes +5 constraints are in 44 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1811 vertexes +3 constraints are in 806 vertexes +4 constraints are in 141 vertexes +5 constraints are in 44 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1811 vertexes +3 constraints are in 806 vertexes +4 constraints are in 141 vertexes +5 constraints are in 44 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1811 vertexes +3 constraints are in 806 vertexes +4 constraints are in 141 vertexes +5 constraints are in 44 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1811 vertexes +3 constraints are in 806 vertexes +4 constraints are in 141 vertexes +5 constraints are in 44 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1811 vertexes +3 constraints are in 806 vertexes +4 constraints are in 141 vertexes +5 constraints are in 44 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1811 vertexes +3 constraints are in 806 vertexes +4 constraints are in 141 vertexes +5 constraints are in 44 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1811 vertexes +3 constraints are in 806 vertexes +4 constraints are in 141 vertexes +5 constraints are in 44 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1811 vertexes +3 constraints are in 806 vertexes +4 constraints are in 141 vertexes +5 constraints are in 44 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1811 vertexes +3 constraints are in 806 vertexes +4 constraints are in 141 vertexes +5 constraints are in 44 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1811 vertexes +3 constraints are in 806 vertexes +4 constraints are in 141 vertexes +5 constraints are in 44 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1811 vertexes +3 constraints are in 806 vertexes +4 constraints are in 141 vertexes +5 constraints are in 44 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1811 vertexes +3 constraints are in 806 vertexes +4 constraints are in 141 vertexes +5 constraints are in 44 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1811 vertexes +3 constraints are in 806 vertexes +4 constraints are in 141 vertexes +5 constraints are in 44 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1811 vertexes +3 constraints are in 806 vertexes +4 constraints are in 141 vertexes +5 constraints are in 44 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1811 vertexes +3 constraints are in 806 vertexes +4 constraints are in 141 vertexes +5 constraints are in 44 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1811 vertexes +3 constraints are in 806 vertexes +4 constraints are in 141 vertexes +5 constraints are in 44 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1811 vertexes +3 constraints are in 806 vertexes +4 constraints are in 141 vertexes +5 constraints are in 44 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1811 vertexes +3 constraints are in 810 vertexes +4 constraints are in 141 vertexes +5 constraints are in 44 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1815 vertexes +3 constraints are in 810 vertexes +4 constraints are in 141 vertexes +5 constraints are in 44 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1814 vertexes +3 constraints are in 816 vertexes +4 constraints are in 141 vertexes +5 constraints are in 44 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1820 vertexes +3 constraints are in 816 vertexes +4 constraints are in 141 vertexes +5 constraints are in 44 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1823 vertexes +3 constraints are in 820 vertexes +4 constraints are in 141 vertexes +5 constraints are in 44 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1825 vertexes +3 constraints are in 820 vertexes +4 constraints are in 141 vertexes +5 constraints are in 44 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1831 vertexes +3 constraints are in 820 vertexes +4 constraints are in 141 vertexes +5 constraints are in 44 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1837 vertexes +3 constraints are in 820 vertexes +4 constraints are in 141 vertexes +5 constraints are in 44 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1843 vertexes +3 constraints are in 820 vertexes +4 constraints are in 141 vertexes +5 constraints are in 44 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1846 vertexes +3 constraints are in 822 vertexes +4 constraints are in 141 vertexes +5 constraints are in 44 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1848 vertexes +3 constraints are in 822 vertexes +4 constraints are in 141 vertexes +5 constraints are in 44 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1848 vertexes +3 constraints are in 826 vertexes +4 constraints are in 141 vertexes +5 constraints are in 44 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 1850 vertexes +3 constraints are in 827 vertexes +4 constraints are in 143 vertexes +5 constraints are in 44 vertexes +6 constraints are in 8 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 1848 vertexes +3 constraints are in 833 vertexes +4 constraints are in 144 vertexes +5 constraints are in 44 vertexes +6 constraints are in 9 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1847 vertexes +3 constraints are in 834 vertexes +4 constraints are in 144 vertexes +5 constraints are in 44 vertexes +6 constraints are in 9 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1847 vertexes +3 constraints are in 834 vertexes +4 constraints are in 144 vertexes +5 constraints are in 44 vertexes +6 constraints are in 9 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 1848 vertexes +3 constraints are in 833 vertexes +4 constraints are in 148 vertexes +5 constraints are in 44 vertexes +6 constraints are in 9 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1847 vertexes +3 constraints are in 834 vertexes +4 constraints are in 148 vertexes +5 constraints are in 44 vertexes +6 constraints are in 9 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1852 vertexes +3 constraints are in 834 vertexes +4 constraints are in 148 vertexes +5 constraints are in 44 vertexes +6 constraints are in 9 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1858 vertexes +3 constraints are in 834 vertexes +4 constraints are in 148 vertexes +5 constraints are in 44 vertexes +6 constraints are in 9 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 1862 vertexes +3 constraints are in 837 vertexes +4 constraints are in 148 vertexes +5 constraints are in 44 vertexes +6 constraints are in 9 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 1864 vertexes +3 constraints are in 839 vertexes +4 constraints are in 148 vertexes +5 constraints are in 44 vertexes +6 constraints are in 9 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 1864 vertexes +3 constraints are in 839 vertexes +4 constraints are in 148 vertexes +5 constraints are in 44 vertexes +6 constraints are in 9 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1865 vertexes +3 constraints are in 841 vertexes +4 constraints are in 149 vertexes +5 constraints are in 45 vertexes +6 constraints are in 9 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1871 vertexes +3 constraints are in 841 vertexes +4 constraints are in 149 vertexes +5 constraints are in 45 vertexes +6 constraints are in 9 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1876 vertexes +3 constraints are in 843 vertexes +4 constraints are in 149 vertexes +5 constraints are in 45 vertexes +6 constraints are in 9 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1880 vertexes +3 constraints are in 843 vertexes +4 constraints are in 149 vertexes +5 constraints are in 45 vertexes +6 constraints are in 9 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1886 vertexes +3 constraints are in 843 vertexes +4 constraints are in 149 vertexes +5 constraints are in 45 vertexes +6 constraints are in 9 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1892 vertexes +3 constraints are in 843 vertexes +4 constraints are in 149 vertexes +5 constraints are in 45 vertexes +6 constraints are in 9 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1898 vertexes +3 constraints are in 843 vertexes +4 constraints are in 149 vertexes +5 constraints are in 45 vertexes +6 constraints are in 9 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1903 vertexes +3 constraints are in 843 vertexes +4 constraints are in 149 vertexes +5 constraints are in 45 vertexes +6 constraints are in 9 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1909 vertexes +3 constraints are in 843 vertexes +4 constraints are in 149 vertexes +5 constraints are in 45 vertexes +6 constraints are in 9 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1915 vertexes +3 constraints are in 843 vertexes +4 constraints are in 149 vertexes +5 constraints are in 45 vertexes +6 constraints are in 9 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1922 vertexes +3 constraints are in 843 vertexes +4 constraints are in 149 vertexes +5 constraints are in 45 vertexes +6 constraints are in 9 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1928 vertexes +3 constraints are in 843 vertexes +4 constraints are in 149 vertexes +5 constraints are in 45 vertexes +6 constraints are in 9 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1934 vertexes +3 constraints are in 843 vertexes +4 constraints are in 149 vertexes +5 constraints are in 45 vertexes +6 constraints are in 9 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1940 vertexes +3 constraints are in 843 vertexes +4 constraints are in 149 vertexes +5 constraints are in 45 vertexes +6 constraints are in 9 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1946 vertexes +3 constraints are in 843 vertexes +4 constraints are in 149 vertexes +5 constraints are in 45 vertexes +6 constraints are in 9 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1951 vertexes +3 constraints are in 843 vertexes +4 constraints are in 149 vertexes +5 constraints are in 45 vertexes +6 constraints are in 9 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1957 vertexes +3 constraints are in 843 vertexes +4 constraints are in 149 vertexes +5 constraints are in 45 vertexes +6 constraints are in 9 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1962 vertexes +3 constraints are in 843 vertexes +4 constraints are in 151 vertexes +5 constraints are in 45 vertexes +6 constraints are in 9 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1968 vertexes +3 constraints are in 843 vertexes +4 constraints are in 151 vertexes +5 constraints are in 45 vertexes +6 constraints are in 9 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1972 vertexes +3 constraints are in 843 vertexes +4 constraints are in 151 vertexes +5 constraints are in 45 vertexes +6 constraints are in 9 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1977 vertexes +3 constraints are in 843 vertexes +4 constraints are in 151 vertexes +5 constraints are in 45 vertexes +6 constraints are in 9 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1979 vertexes +3 constraints are in 847 vertexes +4 constraints are in 151 vertexes +5 constraints are in 45 vertexes +6 constraints are in 9 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1985 vertexes +3 constraints are in 847 vertexes +4 constraints are in 151 vertexes +5 constraints are in 45 vertexes +6 constraints are in 9 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1990 vertexes +3 constraints are in 847 vertexes +4 constraints are in 151 vertexes +5 constraints are in 45 vertexes +6 constraints are in 9 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1991 vertexes +3 constraints are in 849 vertexes +4 constraints are in 151 vertexes +5 constraints are in 45 vertexes +6 constraints are in 9 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + + + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 1997 vertexes +3 constraints are in 849 vertexes +4 constraints are in 151 vertexes +5 constraints are in 45 vertexes +6 constraints are in 9 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2003 vertexes +3 constraints are in 849 vertexes +4 constraints are in 151 vertexes +5 constraints are in 45 vertexes +6 constraints are in 9 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2007 vertexes +3 constraints are in 849 vertexes +4 constraints are in 151 vertexes +5 constraints are in 45 vertexes +6 constraints are in 9 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2011 vertexes +3 constraints are in 851 vertexes +4 constraints are in 151 vertexes +5 constraints are in 45 vertexes +6 constraints are in 9 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 2015 vertexes +3 constraints are in 852 vertexes +4 constraints are in 152 vertexes +5 constraints are in 45 vertexes +6 constraints are in 9 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2019 vertexes +3 constraints are in 853 vertexes +4 constraints are in 152 vertexes +5 constraints are in 45 vertexes +6 constraints are in 9 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2022 vertexes +3 constraints are in 853 vertexes +4 constraints are in 152 vertexes +5 constraints are in 45 vertexes +6 constraints are in 9 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2026 vertexes +3 constraints are in 853 vertexes +4 constraints are in 152 vertexes +5 constraints are in 45 vertexes +6 constraints are in 9 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2032 vertexes +3 constraints are in 853 vertexes +4 constraints are in 152 vertexes +5 constraints are in 45 vertexes +6 constraints are in 9 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2038 vertexes +3 constraints are in 853 vertexes +4 constraints are in 152 vertexes +5 constraints are in 45 vertexes +6 constraints are in 9 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2040 vertexes +3 constraints are in 857 vertexes +4 constraints are in 152 vertexes +5 constraints are in 45 vertexes +6 constraints are in 9 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2040 vertexes +3 constraints are in 861 vertexes +4 constraints are in 152 vertexes +5 constraints are in 45 vertexes +6 constraints are in 9 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2042 vertexes +3 constraints are in 865 vertexes +4 constraints are in 152 vertexes +5 constraints are in 45 vertexes +6 constraints are in 9 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2047 vertexes +3 constraints are in 865 vertexes +4 constraints are in 153 vertexes +5 constraints are in 45 vertexes +6 constraints are in 9 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2050 vertexes +3 constraints are in 865 vertexes +4 constraints are in 153 vertexes +5 constraints are in 45 vertexes +6 constraints are in 9 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2054 vertexes +3 constraints are in 865 vertexes +4 constraints are in 153 vertexes +5 constraints are in 45 vertexes +6 constraints are in 9 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2054 vertexes +3 constraints are in 865 vertexes +4 constraints are in 153 vertexes +5 constraints are in 45 vertexes +6 constraints are in 9 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 2052 vertexes +3 constraints are in 872 vertexes +4 constraints are in 153 vertexes +5 constraints are in 45 vertexes +6 constraints are in 9 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2054 vertexes +3 constraints are in 873 vertexes +4 constraints are in 154 vertexes +5 constraints are in 45 vertexes +6 constraints are in 9 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 2058 vertexes +3 constraints are in 874 vertexes +4 constraints are in 156 vertexes +5 constraints are in 45 vertexes +6 constraints are in 9 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2064 vertexes +3 constraints are in 873 vertexes +4 constraints are in 157 vertexes +5 constraints are in 45 vertexes +6 constraints are in 9 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2070 vertexes +3 constraints are in 873 vertexes +4 constraints are in 157 vertexes +5 constraints are in 45 vertexes +6 constraints are in 9 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2075 vertexes +3 constraints are in 873 vertexes +4 constraints are in 157 vertexes +5 constraints are in 45 vertexes +6 constraints are in 9 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2078 vertexes +3 constraints are in 875 vertexes +4 constraints are in 157 vertexes +5 constraints are in 45 vertexes +6 constraints are in 9 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 2081 vertexes +3 constraints are in 878 vertexes +4 constraints are in 157 vertexes +5 constraints are in 45 vertexes +6 constraints are in 9 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2082 vertexes +3 constraints are in 879 vertexes +4 constraints are in 157 vertexes +5 constraints are in 45 vertexes +6 constraints are in 9 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2084 vertexes +3 constraints are in 879 vertexes +4 constraints are in 161 vertexes +5 constraints are in 45 vertexes +6 constraints are in 9 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 2087 vertexes +3 constraints are in 882 vertexes +4 constraints are in 162 vertexes +5 constraints are in 45 vertexes +6 constraints are in 9 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2092 vertexes +3 constraints are in 881 vertexes +4 constraints are in 163 vertexes +5 constraints are in 45 vertexes +6 constraints are in 9 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2097 vertexes +3 constraints are in 883 vertexes +4 constraints are in 163 vertexes +5 constraints are in 45 vertexes +6 constraints are in 9 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2103 vertexes +3 constraints are in 883 vertexes +4 constraints are in 163 vertexes +5 constraints are in 45 vertexes +6 constraints are in 9 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2105 vertexes +3 constraints are in 887 vertexes +4 constraints are in 163 vertexes +5 constraints are in 45 vertexes +6 constraints are in 9 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2108 vertexes +3 constraints are in 889 vertexes +4 constraints are in 163 vertexes +5 constraints are in 45 vertexes +6 constraints are in 9 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2110 vertexes +3 constraints are in 889 vertexes +4 constraints are in 163 vertexes +5 constraints are in 45 vertexes +6 constraints are in 9 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2114 vertexes +3 constraints are in 891 vertexes +4 constraints are in 163 vertexes +5 constraints are in 45 vertexes +6 constraints are in 9 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2120 vertexes +3 constraints are in 891 vertexes +4 constraints are in 163 vertexes +5 constraints are in 45 vertexes +6 constraints are in 9 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2124 vertexes +3 constraints are in 891 vertexes +4 constraints are in 163 vertexes +5 constraints are in 45 vertexes +6 constraints are in 9 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2130 vertexes +3 constraints are in 891 vertexes +4 constraints are in 163 vertexes +5 constraints are in 45 vertexes +6 constraints are in 9 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2131 vertexes +3 constraints are in 891 vertexes +4 constraints are in 163 vertexes +5 constraints are in 45 vertexes +6 constraints are in 9 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2137 vertexes +3 constraints are in 891 vertexes +4 constraints are in 163 vertexes +5 constraints are in 45 vertexes +6 constraints are in 9 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2141 vertexes +3 constraints are in 891 vertexes +4 constraints are in 163 vertexes +5 constraints are in 45 vertexes +6 constraints are in 9 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2146 vertexes +3 constraints are in 891 vertexes +4 constraints are in 163 vertexes +5 constraints are in 45 vertexes +6 constraints are in 9 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2147 vertexes +3 constraints are in 893 vertexes +4 constraints are in 163 vertexes +5 constraints are in 45 vertexes +6 constraints are in 9 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 2151 vertexes +3 constraints are in 892 vertexes +4 constraints are in 164 vertexes +5 constraints are in 45 vertexes +6 constraints are in 9 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2151 vertexes +3 constraints are in 897 vertexes +4 constraints are in 165 vertexes +5 constraints are in 45 vertexes +6 constraints are in 9 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2152 vertexes +3 constraints are in 897 vertexes +4 constraints are in 165 vertexes +5 constraints are in 45 vertexes +6 constraints are in 9 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2152 vertexes +3 constraints are in 897 vertexes +4 constraints are in 165 vertexes +5 constraints are in 45 vertexes +6 constraints are in 9 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2154 vertexes +3 constraints are in 899 vertexes +4 constraints are in 164 vertexes +5 constraints are in 45 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 2159 vertexes +3 constraints are in 902 vertexes +4 constraints are in 164 vertexes +5 constraints are in 45 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2161 vertexes +3 constraints are in 905 vertexes +4 constraints are in 164 vertexes +5 constraints are in 45 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2165 vertexes +3 constraints are in 907 vertexes +4 constraints are in 164 vertexes +5 constraints are in 45 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2171 vertexes +3 constraints are in 907 vertexes +4 constraints are in 164 vertexes +5 constraints are in 45 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2176 vertexes +3 constraints are in 909 vertexes +4 constraints are in 164 vertexes +5 constraints are in 45 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2180 vertexes +3 constraints are in 911 vertexes +4 constraints are in 164 vertexes +5 constraints are in 45 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2186 vertexes +3 constraints are in 911 vertexes +4 constraints are in 164 vertexes +5 constraints are in 45 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2189 vertexes +3 constraints are in 913 vertexes +4 constraints are in 164 vertexes +5 constraints are in 45 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2190 vertexes +3 constraints are in 913 vertexes +4 constraints are in 164 vertexes +5 constraints are in 45 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2190 vertexes +3 constraints are in 913 vertexes +4 constraints are in 164 vertexes +5 constraints are in 45 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2190 vertexes +3 constraints are in 913 vertexes +4 constraints are in 164 vertexes +5 constraints are in 45 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2190 vertexes +3 constraints are in 913 vertexes +4 constraints are in 164 vertexes +5 constraints are in 45 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2190 vertexes +3 constraints are in 913 vertexes +4 constraints are in 164 vertexes +5 constraints are in 45 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2190 vertexes +3 constraints are in 913 vertexes +4 constraints are in 164 vertexes +5 constraints are in 45 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2192 vertexes +3 constraints are in 913 vertexes +4 constraints are in 164 vertexes +5 constraints are in 45 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2196 vertexes +3 constraints are in 913 vertexes +4 constraints are in 164 vertexes +5 constraints are in 45 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2202 vertexes +3 constraints are in 913 vertexes +4 constraints are in 164 vertexes +5 constraints are in 45 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2206 vertexes +3 constraints are in 913 vertexes +4 constraints are in 164 vertexes +5 constraints are in 45 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2210 vertexes +3 constraints are in 913 vertexes +4 constraints are in 164 vertexes +5 constraints are in 45 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2212 vertexes +3 constraints are in 917 vertexes +4 constraints are in 165 vertexes +5 constraints are in 45 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 2214 vertexes +3 constraints are in 916 vertexes +4 constraints are in 166 vertexes +5 constraints are in 45 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2214 vertexes +3 constraints are in 921 vertexes +4 constraints are in 166 vertexes +5 constraints are in 45 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2217 vertexes +3 constraints are in 923 vertexes +4 constraints are in 167 vertexes +5 constraints are in 45 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2224 vertexes +3 constraints are in 923 vertexes +4 constraints are in 167 vertexes +5 constraints are in 45 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 2231 vertexes +3 constraints are in 922 vertexes +4 constraints are in 168 vertexes +5 constraints are in 45 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2233 vertexes +3 constraints are in 925 vertexes +4 constraints are in 168 vertexes +5 constraints are in 45 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2235 vertexes +3 constraints are in 927 vertexes +4 constraints are in 168 vertexes +5 constraints are in 45 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2242 vertexes +3 constraints are in 927 vertexes +4 constraints are in 168 vertexes +5 constraints are in 45 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2248 vertexes +3 constraints are in 927 vertexes +4 constraints are in 168 vertexes +5 constraints are in 45 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2254 vertexes +3 constraints are in 927 vertexes +4 constraints are in 168 vertexes +5 constraints are in 45 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2256 vertexes +3 constraints are in 927 vertexes +4 constraints are in 168 vertexes +5 constraints are in 45 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2259 vertexes +3 constraints are in 927 vertexes +4 constraints are in 168 vertexes +5 constraints are in 45 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2260 vertexes +3 constraints are in 927 vertexes +4 constraints are in 168 vertexes +5 constraints are in 45 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 2261 vertexes +3 constraints are in 932 vertexes +4 constraints are in 168 vertexes +5 constraints are in 45 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 2264 vertexes +3 constraints are in 934 vertexes +4 constraints are in 168 vertexes +5 constraints are in 45 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2260 vertexes +3 constraints are in 942 vertexes +4 constraints are in 168 vertexes +5 constraints are in 46 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2262 vertexes +3 constraints are in 942 vertexes +4 constraints are in 168 vertexes +5 constraints are in 46 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2264 vertexes +3 constraints are in 942 vertexes +4 constraints are in 168 vertexes +5 constraints are in 46 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 2266 vertexes +3 constraints are in 943 vertexes +4 constraints are in 168 vertexes +5 constraints are in 46 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2268 vertexes +3 constraints are in 944 vertexes +4 constraints are in 168 vertexes +5 constraints are in 46 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2271 vertexes +3 constraints are in 944 vertexes +4 constraints are in 168 vertexes +5 constraints are in 46 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2271 vertexes +3 constraints are in 946 vertexes +4 constraints are in 168 vertexes +5 constraints are in 46 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2278 vertexes +3 constraints are in 946 vertexes +4 constraints are in 168 vertexes +5 constraints are in 46 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 2283 vertexes +3 constraints are in 947 vertexes +4 constraints are in 168 vertexes +5 constraints are in 46 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 2284 vertexes +3 constraints are in 951 vertexes +4 constraints are in 168 vertexes +5 constraints are in 46 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2288 vertexes +3 constraints are in 952 vertexes +4 constraints are in 168 vertexes +5 constraints are in 46 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 2290 vertexes +3 constraints are in 953 vertexes +4 constraints are in 168 vertexes +5 constraints are in 46 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2291 vertexes +3 constraints are in 956 vertexes +4 constraints are in 168 vertexes +5 constraints are in 46 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2295 vertexes +3 constraints are in 956 vertexes +4 constraints are in 168 vertexes +5 constraints are in 46 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2301 vertexes +3 constraints are in 956 vertexes +4 constraints are in 168 vertexes +5 constraints are in 46 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2304 vertexes +3 constraints are in 956 vertexes +4 constraints are in 168 vertexes +5 constraints are in 46 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 2303 vertexes +3 constraints are in 961 vertexes +4 constraints are in 170 vertexes +5 constraints are in 46 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2307 vertexes +3 constraints are in 962 vertexes +4 constraints are in 170 vertexes +5 constraints are in 46 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2313 vertexes +3 constraints are in 962 vertexes +4 constraints are in 170 vertexes +5 constraints are in 46 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2318 vertexes +3 constraints are in 962 vertexes +4 constraints are in 170 vertexes +5 constraints are in 46 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 2324 vertexes +3 constraints are in 963 vertexes +4 constraints are in 170 vertexes +5 constraints are in 46 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 2323 vertexes +3 constraints are in 966 vertexes +4 constraints are in 172 vertexes +5 constraints are in 47 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2325 vertexes +3 constraints are in 969 vertexes +4 constraints are in 172 vertexes +5 constraints are in 47 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2330 vertexes +3 constraints are in 971 vertexes +4 constraints are in 172 vertexes +5 constraints are in 47 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2336 vertexes +3 constraints are in 971 vertexes +4 constraints are in 172 vertexes +5 constraints are in 47 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2339 vertexes +3 constraints are in 973 vertexes +4 constraints are in 173 vertexes +5 constraints are in 47 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2344 vertexes +3 constraints are in 973 vertexes +4 constraints are in 173 vertexes +5 constraints are in 47 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 2346 vertexes +3 constraints are in 976 vertexes +4 constraints are in 173 vertexes +5 constraints are in 47 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2347 vertexes +3 constraints are in 977 vertexes +4 constraints are in 173 vertexes +5 constraints are in 47 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2350 vertexes +3 constraints are in 977 vertexes +4 constraints are in 173 vertexes +5 constraints are in 47 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2352 vertexes +3 constraints are in 977 vertexes +4 constraints are in 173 vertexes +5 constraints are in 47 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 2353 vertexes +3 constraints are in 978 vertexes +4 constraints are in 173 vertexes +5 constraints are in 47 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2351 vertexes +3 constraints are in 983 vertexes +4 constraints are in 175 vertexes +5 constraints are in 47 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2354 vertexes +3 constraints are in 983 vertexes +4 constraints are in 177 vertexes +5 constraints are in 47 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2357 vertexes +3 constraints are in 983 vertexes +4 constraints are in 177 vertexes +5 constraints are in 47 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2360 vertexes +3 constraints are in 983 vertexes +4 constraints are in 177 vertexes +5 constraints are in 47 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2362 vertexes +3 constraints are in 985 vertexes +4 constraints are in 177 vertexes +5 constraints are in 47 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2365 vertexes +3 constraints are in 987 vertexes +4 constraints are in 177 vertexes +5 constraints are in 47 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 2371 vertexes +3 constraints are in 988 vertexes +4 constraints are in 177 vertexes +5 constraints are in 47 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2374 vertexes +3 constraints are in 989 vertexes +4 constraints are in 177 vertexes +5 constraints are in 47 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2380 vertexes +3 constraints are in 989 vertexes +4 constraints are in 177 vertexes +5 constraints are in 47 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2382 vertexes +3 constraints are in 991 vertexes +4 constraints are in 177 vertexes +5 constraints are in 47 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2383 vertexes +3 constraints are in 991 vertexes +4 constraints are in 177 vertexes +5 constraints are in 47 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2384 vertexes +3 constraints are in 991 vertexes +4 constraints are in 177 vertexes +5 constraints are in 47 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2384 vertexes +3 constraints are in 991 vertexes +4 constraints are in 177 vertexes +5 constraints are in 47 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 2387 vertexes +3 constraints are in 994 vertexes +4 constraints are in 177 vertexes +5 constraints are in 47 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2389 vertexes +3 constraints are in 997 vertexes +4 constraints are in 177 vertexes +5 constraints are in 47 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2390 vertexes +3 constraints are in 999 vertexes +4 constraints are in 178 vertexes +5 constraints are in 47 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 2397 vertexes +3 constraints are in 998 vertexes +4 constraints are in 179 vertexes +5 constraints are in 47 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2400 vertexes +3 constraints are in 1000 vertexes +4 constraints are in 178 vertexes +5 constraints are in 48 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2404 vertexes +3 constraints are in 1002 vertexes +4 constraints are in 178 vertexes +5 constraints are in 48 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2409 vertexes +3 constraints are in 1004 vertexes +4 constraints are in 178 vertexes +5 constraints are in 48 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 2412 vertexes +3 constraints are in 1007 vertexes +4 constraints are in 178 vertexes +5 constraints are in 48 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 2414 vertexes +3 constraints are in 1010 vertexes +4 constraints are in 179 vertexes +5 constraints are in 49 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2416 vertexes +3 constraints are in 1012 vertexes +4 constraints are in 179 vertexes +5 constraints are in 50 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2423 vertexes +3 constraints are in 1012 vertexes +4 constraints are in 179 vertexes +5 constraints are in 50 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 2426 vertexes +3 constraints are in 1013 vertexes +4 constraints are in 182 vertexes +5 constraints are in 50 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2429 vertexes +3 constraints are in 1014 vertexes +4 constraints are in 183 vertexes +5 constraints are in 50 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 2434 vertexes +3 constraints are in 1015 vertexes +4 constraints are in 183 vertexes +5 constraints are in 50 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 2438 vertexes +3 constraints are in 1017 vertexes +4 constraints are in 183 vertexes +5 constraints are in 50 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2437 vertexes +3 constraints are in 1020 vertexes +4 constraints are in 183 vertexes +5 constraints are in 50 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 2440 vertexes +3 constraints are in 1023 vertexes +4 constraints are in 184 vertexes +5 constraints are in 50 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 2442 vertexes +3 constraints are in 1027 vertexes +4 constraints are in 186 vertexes +5 constraints are in 50 vertexes +6 constraints are in 10 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 2443 vertexes +3 constraints are in 1030 vertexes +4 constraints are in 185 vertexes +5 constraints are in 51 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2443 vertexes +3 constraints are in 1035 vertexes +4 constraints are in 186 vertexes +5 constraints are in 51 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2447 vertexes +3 constraints are in 1037 vertexes +4 constraints are in 186 vertexes +5 constraints are in 51 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2451 vertexes +3 constraints are in 1039 vertexes +4 constraints are in 186 vertexes +5 constraints are in 51 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 2456 vertexes +3 constraints are in 1042 vertexes +4 constraints are in 186 vertexes +5 constraints are in 51 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2460 vertexes +3 constraints are in 1043 vertexes +4 constraints are in 186 vertexes +5 constraints are in 51 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2464 vertexes +3 constraints are in 1045 vertexes +4 constraints are in 186 vertexes +5 constraints are in 51 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2470 vertexes +3 constraints are in 1045 vertexes +4 constraints are in 186 vertexes +5 constraints are in 51 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2477 vertexes +3 constraints are in 1045 vertexes +4 constraints are in 186 vertexes +5 constraints are in 51 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2483 vertexes +3 constraints are in 1045 vertexes +4 constraints are in 186 vertexes +5 constraints are in 51 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2488 vertexes +3 constraints are in 1045 vertexes +4 constraints are in 186 vertexes +5 constraints are in 51 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2495 vertexes +3 constraints are in 1045 vertexes +4 constraints are in 186 vertexes +5 constraints are in 51 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2496 vertexes +3 constraints are in 1049 vertexes +4 constraints are in 187 vertexes +5 constraints are in 51 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2501 vertexes +3 constraints are in 1050 vertexes +4 constraints are in 186 vertexes +5 constraints are in 52 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 2504 vertexes +3 constraints are in 1053 vertexes +4 constraints are in 186 vertexes +5 constraints are in 52 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2506 vertexes +3 constraints are in 1056 vertexes +4 constraints are in 186 vertexes +5 constraints are in 52 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 2511 vertexes +3 constraints are in 1057 vertexes +4 constraints are in 187 vertexes +5 constraints are in 52 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2514 vertexes +3 constraints are in 1059 vertexes +4 constraints are in 186 vertexes +5 constraints are in 53 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2521 vertexes +3 constraints are in 1059 vertexes +4 constraints are in 186 vertexes +5 constraints are in 53 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 2527 vertexes +3 constraints are in 1060 vertexes +4 constraints are in 186 vertexes +5 constraints are in 53 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 2531 vertexes +3 constraints are in 1062 vertexes +4 constraints are in 186 vertexes +5 constraints are in 53 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2534 vertexes +3 constraints are in 1065 vertexes +4 constraints are in 186 vertexes +5 constraints are in 53 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2538 vertexes +3 constraints are in 1067 vertexes +4 constraints are in 186 vertexes +5 constraints are in 53 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2542 vertexes +3 constraints are in 1069 vertexes +4 constraints are in 186 vertexes +5 constraints are in 53 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2547 vertexes +3 constraints are in 1069 vertexes +4 constraints are in 187 vertexes +5 constraints are in 53 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 2552 vertexes +3 constraints are in 1072 vertexes +4 constraints are in 187 vertexes +5 constraints are in 53 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 2554 vertexes +3 constraints are in 1074 vertexes +4 constraints are in 188 vertexes +5 constraints are in 53 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2554 vertexes +3 constraints are in 1075 vertexes +4 constraints are in 188 vertexes +5 constraints are in 53 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2557 vertexes +3 constraints are in 1075 vertexes +4 constraints are in 188 vertexes +5 constraints are in 53 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2562 vertexes +3 constraints are in 1075 vertexes +4 constraints are in 188 vertexes +5 constraints are in 53 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 2564 vertexes +3 constraints are in 1076 vertexes +4 constraints are in 188 vertexes +5 constraints are in 53 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2564 vertexes +3 constraints are in 1081 vertexes +4 constraints are in 189 vertexes +5 constraints are in 53 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2566 vertexes +3 constraints are in 1081 vertexes +4 constraints are in 189 vertexes +5 constraints are in 53 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2567 vertexes +3 constraints are in 1083 vertexes +4 constraints are in 189 vertexes +5 constraints are in 53 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2569 vertexes +3 constraints are in 1083 vertexes +4 constraints are in 189 vertexes +5 constraints are in 53 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2570 vertexes +3 constraints are in 1087 vertexes +4 constraints are in 190 vertexes +5 constraints are in 53 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2574 vertexes +3 constraints are in 1087 vertexes +4 constraints are in 190 vertexes +5 constraints are in 53 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 2572 vertexes +3 constraints are in 1094 vertexes +4 constraints are in 191 vertexes +5 constraints are in 53 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 2570 vertexes +3 constraints are in 1098 vertexes +4 constraints are in 192 vertexes +5 constraints are in 53 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 2570 vertexes +3 constraints are in 1098 vertexes +4 constraints are in 192 vertexes +5 constraints are in 53 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2569 vertexes +3 constraints are in 1099 vertexes +4 constraints are in 193 vertexes +5 constraints are in 53 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2569 vertexes +3 constraints are in 1099 vertexes +4 constraints are in 193 vertexes +5 constraints are in 53 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 2571 vertexes +3 constraints are in 1100 vertexes +4 constraints are in 193 vertexes +5 constraints are in 53 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2570 vertexes +3 constraints are in 1101 vertexes +4 constraints are in 193 vertexes +5 constraints are in 53 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2570 vertexes +3 constraints are in 1101 vertexes +4 constraints are in 193 vertexes +5 constraints are in 53 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 2572 vertexes +3 constraints are in 1102 vertexes +4 constraints are in 195 vertexes +5 constraints are in 53 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2574 vertexes +3 constraints are in 1103 vertexes +4 constraints are in 196 vertexes +5 constraints are in 53 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2578 vertexes +3 constraints are in 1103 vertexes +4 constraints are in 197 vertexes +5 constraints are in 53 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2580 vertexes +3 constraints are in 1105 vertexes +4 constraints are in 198 vertexes +5 constraints are in 53 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2584 vertexes +3 constraints are in 1105 vertexes +4 constraints are in 199 vertexes +5 constraints are in 53 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 2584 vertexes +3 constraints are in 1110 vertexes +4 constraints are in 200 vertexes +5 constraints are in 53 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2588 vertexes +3 constraints are in 1111 vertexes +4 constraints are in 201 vertexes +5 constraints are in 53 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2592 vertexes +3 constraints are in 1113 vertexes +4 constraints are in 201 vertexes +5 constraints are in 53 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2598 vertexes +3 constraints are in 1113 vertexes +4 constraints are in 201 vertexes +5 constraints are in 53 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 2603 vertexes +3 constraints are in 1114 vertexes +4 constraints are in 202 vertexes +5 constraints are in 53 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2606 vertexes +3 constraints are in 1117 vertexes +4 constraints are in 202 vertexes +5 constraints are in 53 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2613 vertexes +3 constraints are in 1117 vertexes +4 constraints are in 202 vertexes +5 constraints are in 53 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2619 vertexes +3 constraints are in 1117 vertexes +4 constraints are in 202 vertexes +5 constraints are in 53 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2625 vertexes +3 constraints are in 1117 vertexes +4 constraints are in 202 vertexes +5 constraints are in 53 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2629 vertexes +3 constraints are in 1117 vertexes +4 constraints are in 202 vertexes +5 constraints are in 53 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2628 vertexes +3 constraints are in 1121 vertexes +4 constraints are in 204 vertexes +5 constraints are in 53 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2632 vertexes +3 constraints are in 1123 vertexes +4 constraints are in 204 vertexes +5 constraints are in 53 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2635 vertexes +3 constraints are in 1123 vertexes +4 constraints are in 204 vertexes +5 constraints are in 53 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2635 vertexes +3 constraints are in 1125 vertexes +4 constraints are in 204 vertexes +5 constraints are in 53 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2638 vertexes +3 constraints are in 1126 vertexes +4 constraints are in 204 vertexes +5 constraints are in 54 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2641 vertexes +3 constraints are in 1126 vertexes +4 constraints are in 204 vertexes +5 constraints are in 54 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 2646 vertexes +3 constraints are in 1128 vertexes +4 constraints are in 203 vertexes +5 constraints are in 55 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2646 vertexes +3 constraints are in 1130 vertexes +4 constraints are in 206 vertexes +5 constraints are in 56 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2651 vertexes +3 constraints are in 1130 vertexes +4 constraints are in 206 vertexes +5 constraints are in 56 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2656 vertexes +3 constraints are in 1130 vertexes +4 constraints are in 206 vertexes +5 constraints are in 56 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2658 vertexes +3 constraints are in 1134 vertexes +4 constraints are in 206 vertexes +5 constraints are in 56 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2664 vertexes +3 constraints are in 1134 vertexes +4 constraints are in 206 vertexes +5 constraints are in 56 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2668 vertexes +3 constraints are in 1134 vertexes +4 constraints are in 206 vertexes +5 constraints are in 56 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2668 vertexes +3 constraints are in 1134 vertexes +4 constraints are in 206 vertexes +5 constraints are in 56 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2668 vertexes +3 constraints are in 1134 vertexes +4 constraints are in 206 vertexes +5 constraints are in 56 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 2670 vertexes +3 constraints are in 1135 vertexes +4 constraints are in 206 vertexes +5 constraints are in 56 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 2670 vertexes +3 constraints are in 1135 vertexes +4 constraints are in 206 vertexes +5 constraints are in 56 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2669 vertexes +3 constraints are in 1136 vertexes +4 constraints are in 206 vertexes +5 constraints are in 56 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2672 vertexes +3 constraints are in 1136 vertexes +4 constraints are in 206 vertexes +5 constraints are in 56 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2672 vertexes +3 constraints are in 1136 vertexes +4 constraints are in 206 vertexes +5 constraints are in 56 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2674 vertexes +3 constraints are in 1136 vertexes +4 constraints are in 206 vertexes +5 constraints are in 56 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 2676 vertexes +3 constraints are in 1137 vertexes +4 constraints are in 206 vertexes +5 constraints are in 56 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 2676 vertexes +3 constraints are in 1137 vertexes +4 constraints are in 206 vertexes +5 constraints are in 56 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 2675 vertexes +3 constraints are in 1139 vertexes +4 constraints are in 206 vertexes +5 constraints are in 56 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 2676 vertexes +3 constraints are in 1141 vertexes +4 constraints are in 206 vertexes +5 constraints are in 56 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2675 vertexes +3 constraints are in 1144 vertexes +4 constraints are in 208 vertexes +5 constraints are in 56 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 2676 vertexes +3 constraints are in 1148 vertexes +4 constraints are in 209 vertexes +5 constraints are in 57 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2677 vertexes +3 constraints are in 1149 vertexes +4 constraints are in 212 vertexes +5 constraints are in 57 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2683 vertexes +3 constraints are in 1149 vertexes +4 constraints are in 212 vertexes +5 constraints are in 57 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2688 vertexes +3 constraints are in 1151 vertexes +4 constraints are in 213 vertexes +5 constraints are in 57 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 2693 vertexes +3 constraints are in 1154 vertexes +4 constraints are in 213 vertexes +5 constraints are in 57 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 2693 vertexes +3 constraints are in 1158 vertexes +4 constraints are in 217 vertexes +5 constraints are in 57 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 2697 vertexes +3 constraints are in 1160 vertexes +4 constraints are in 218 vertexes +5 constraints are in 57 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 2695 vertexes +3 constraints are in 1164 vertexes +4 constraints are in 221 vertexes +5 constraints are in 57 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 2698 vertexes +3 constraints are in 1166 vertexes +4 constraints are in 222 vertexes +5 constraints are in 57 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2698 vertexes +3 constraints are in 1171 vertexes +4 constraints are in 222 vertexes +5 constraints are in 57 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2703 vertexes +3 constraints are in 1171 vertexes +4 constraints are in 223 vertexes +5 constraints are in 57 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2709 vertexes +3 constraints are in 1171 vertexes +4 constraints are in 223 vertexes +5 constraints are in 57 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2716 vertexes +3 constraints are in 1171 vertexes +4 constraints are in 223 vertexes +5 constraints are in 57 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2722 vertexes +3 constraints are in 1171 vertexes +4 constraints are in 223 vertexes +5 constraints are in 57 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2728 vertexes +3 constraints are in 1171 vertexes +4 constraints are in 223 vertexes +5 constraints are in 57 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2734 vertexes +3 constraints are in 1171 vertexes +4 constraints are in 223 vertexes +5 constraints are in 57 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2739 vertexes +3 constraints are in 1171 vertexes +4 constraints are in 223 vertexes +5 constraints are in 57 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2745 vertexes +3 constraints are in 1171 vertexes +4 constraints are in 224 vertexes +5 constraints are in 57 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2751 vertexes +3 constraints are in 1171 vertexes +4 constraints are in 224 vertexes +5 constraints are in 57 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2754 vertexes +3 constraints are in 1171 vertexes +4 constraints are in 224 vertexes +5 constraints are in 57 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2751 vertexes +3 constraints are in 1179 vertexes +4 constraints are in 224 vertexes +5 constraints are in 57 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2756 vertexes +3 constraints are in 1181 vertexes +4 constraints are in 224 vertexes +5 constraints are in 57 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2762 vertexes +3 constraints are in 1181 vertexes +4 constraints are in 224 vertexes +5 constraints are in 57 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2769 vertexes +3 constraints are in 1181 vertexes +4 constraints are in 224 vertexes +5 constraints are in 57 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 2 vertexes +2 constraints are in 2773 vertexes +3 constraints are in 1184 vertexes +4 constraints are in 224 vertexes +5 constraints are in 57 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes + +UpdateMap: Vertex count: +1 constraints are in 3 vertexes +2 constraints are in 2777 vertexes +3 constraints are in 1185 vertexes +4 constraints are in 224 vertexes +5 constraints are in 57 vertexes +6 constraints are in 11 vertexes +7 constraints are in 6 vertexes +9 constraints are in 2 vertexes diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox/test/lifelong_metrics_test.cpp b/Localizations/Packages/slam_toolbox/slam_toolbox/test/lifelong_metrics_test.cpp new file mode 100644 index 0000000..978ba9b --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox/test/lifelong_metrics_test.cpp @@ -0,0 +1,187 @@ +/* + * slam_toolbox + * Copyright (c) 2019, Steve Macenski + * + * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE + * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY + * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS + * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED. + * + * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO + * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS + * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND + * CONDITIONS. + * + */ + +/* Author: Steven Macenski */ + +#ifndef SLAM_TOOLBOX_SLAM_TOOLBOX_LIFELONG_TEST_H_ +#define SLAM_TOOLBOX_SLAM_TOOLBOX_LIFELONG_TEST_H_ + +#include +#include "slam_toolbox/experimental/slam_toolbox_lifelong.hpp" + +using namespace karto; + +// 3 potential test cases, t1 is used. +//t1 = IOU([3.5, 4.0, 3.0, 4.0], [3.5, 5.5, 3.0, 3.0]) == 6.0 +//t2 = IOU([4.5, 3.0, 5.0, 2.0], [4.5, 4.5, 3, 3]) == 3.0 +//t3 = IOU([4.5, 3.5, 3.0, 3.0], [2.5, 5.5, 3, 3]) == 1.0 + +namespace +{ + +TEST(LifelingMetricsTests, TestBounds) +{ + LocalizedRangeScan* s1 = new LocalizedRangeScan(); + LocalizedRangeScan* s2 = new LocalizedRangeScan(); + Pose2 p1 = Pose2(3.5, 4.0, 0.0); + Pose2 p2 = Pose2(3.5, 5.5, 0.0); + s1->SetBarycenterPose(p1); + s2->SetBarycenterPose(p2); + BoundingBox2 bb1, bb2; + bb1.SetMinimum(Vector2(2.0, 2.0)); + bb1.SetMaximum(Vector2(5.0, 6.0)); + bb2.SetMinimum(Vector2(2.0, 4.0)); + bb2.SetMaximum(Vector2(5.0, 7.0)); + s1->SetBoundingBox(bb1); + s2->SetBoundingBox(bb2); + PointVectorDouble pts; + pts.push_back(Vector2(3.0, 5.0)); //inside + pts.push_back(Vector2(3.0, 3.0)); //outside + s2->SetPointReadings(pts, true); + double x_l, x_u, y_l, y_u; + bool dirty = false; + s1->SetIsDirty(dirty); + s2->SetIsDirty(dirty); + slam_toolbox::LifelongSlamToolbox::computeIntersectBounds(s1, s2, x_l, x_u, y_l, y_u); + EXPECT_EQ(x_l, 2.0); + EXPECT_EQ(x_u, 5.0); + EXPECT_EQ(y_l, 4.0); + EXPECT_EQ(y_u, 6.0); + delete s1; + delete s2; +} + +TEST(LifelingMetricsTests, TestIntersect) +{ + LocalizedRangeScan* s1 = new LocalizedRangeScan(); + LocalizedRangeScan* s2 = new LocalizedRangeScan(); + Pose2 p1 = Pose2(3.5, 4.0, 0.0); + Pose2 p2 = Pose2(3.5, 5.5, 0.0); + s1->SetBarycenterPose(p1); + s2->SetBarycenterPose(p2); + BoundingBox2 bb1, bb2; + bb1.SetMinimum(Vector2(2.0, 2.0)); + bb1.SetMaximum(Vector2(5.0, 6.0)); + bb2.SetMinimum(Vector2(2.0, 4.0)); + bb2.SetMaximum(Vector2(5.0, 7.0)); + s1->SetBoundingBox(bb1); + s2->SetBoundingBox(bb2); + PointVectorDouble pts; + pts.push_back(Vector2(3.0, 5.0)); //inside + pts.push_back(Vector2(3.0, 3.0)); //outside + s2->SetPointReadings(pts, true); + bool dirty = false; + s1->SetIsDirty(dirty); + s2->SetIsDirty(dirty); + double intersect = slam_toolbox::LifelongSlamToolbox::computeIntersect(s1, s2); + EXPECT_EQ(intersect, 6.0); + delete s1; + delete s2; +} + +TEST(LifelingMetricsTests, TestIntersectOverUnion) +{ + LocalizedRangeScan* s1 = new LocalizedRangeScan(); + LocalizedRangeScan* s2 = new LocalizedRangeScan(); + Pose2 p1 = Pose2(3.5, 4.0, 0.0); + Pose2 p2 = Pose2(3.5, 5.5, 0.0); + s1->SetBarycenterPose(p1); + s2->SetBarycenterPose(p2); + BoundingBox2 bb1, bb2; + bb1.SetMinimum(Vector2(2.0, 2.0)); + bb1.SetMaximum(Vector2(5.0, 6.0)); + bb2.SetMinimum(Vector2(2.0, 4.0)); + bb2.SetMaximum(Vector2(5.0, 7.0)); + s1->SetBoundingBox(bb1); + s2->SetBoundingBox(bb2); + PointVectorDouble pts; + pts.push_back(Vector2(3.0, 5.0)); //inside + pts.push_back(Vector2(3.0, 3.0)); //outside + s2->SetPointReadings(pts, true); + bool dirty = false; + s1->SetIsDirty(dirty); + s2->SetIsDirty(dirty); + double intersect_over_union = slam_toolbox::LifelongSlamToolbox::computeIntersectOverUnion(s1, s2); + EXPECT_EQ(intersect_over_union, 0.4); + delete s1; + delete s2; +} + +TEST(LifelingMetricsTests, TestAreaOverlap) +{ + LocalizedRangeScan* s1 = new LocalizedRangeScan(); + LocalizedRangeScan* s2 = new LocalizedRangeScan(); + Pose2 p1 = Pose2(3.5, 4.0, 0.0); + Pose2 p2 = Pose2(3.5, 5.5, 0.0); + s1->SetBarycenterPose(p1); + s2->SetBarycenterPose(p2); + BoundingBox2 bb1, bb2; + bb1.SetMinimum(Vector2(2.0, 2.0)); + bb1.SetMaximum(Vector2(5.0, 6.0)); + bb2.SetMinimum(Vector2(2.0, 4.0)); + bb2.SetMaximum(Vector2(5.0, 7.0)); + s1->SetBoundingBox(bb1); + s2->SetBoundingBox(bb2); + PointVectorDouble pts; + pts.push_back(Vector2(3.0, 5.0)); //inside + pts.push_back(Vector2(3.0, 3.0)); //outside + s2->SetPointReadings(pts, true); + bool dirty = false; + s1->SetIsDirty(dirty); + s2->SetIsDirty(dirty); + double area = slam_toolbox::LifelongSlamToolbox::computeAreaOverlapRatio(s1, s2); + EXPECT_NEAR(area, 0.6666, 0.01); + delete s1; + delete s2; +} + +TEST(LifelingMetricsTests, TestPtOverlap) +{ + LocalizedRangeScan* s1 = new LocalizedRangeScan(); + LocalizedRangeScan* s2 = new LocalizedRangeScan(); + Pose2 p1 = Pose2(3.5, 4.0, 0.0); + Pose2 p2 = Pose2(3.5, 5.5, 0.0); + s1->SetBarycenterPose(p1); + s2->SetBarycenterPose(p2); + BoundingBox2 bb1, bb2; + bb1.SetMinimum(Vector2(2.0, 2.0)); + bb1.SetMaximum(Vector2(5.0, 6.0)); + bb2.SetMinimum(Vector2(2.0, 4.0)); + bb2.SetMaximum(Vector2(5.0, 7.0)); + s1->SetBoundingBox(bb1); + s2->SetBoundingBox(bb2); + PointVectorDouble pts; + pts.push_back(Vector2(3.0, 5.0)); //inside + pts.push_back(Vector2(3.0, 3.0)); //outside + s2->SetPointReadings(pts, true); + bool dirty = false; + s1->SetIsDirty(dirty); + s2->SetIsDirty(dirty); + double area = slam_toolbox::LifelongSlamToolbox::computeReadingOverlapRatio(s1, s2); + EXPECT_EQ(area, 0.5); + delete s1; + delete s2; +} + +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} + +#endif //SLAM_TOOLBOX_SLAM_TOOLBOX_LIFELONG_TEST_H_ diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox/test/process_constraints.py b/Localizations/Packages/slam_toolbox/slam_toolbox/test/process_constraints.py new file mode 100644 index 0000000..261a560 --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox/test/process_constraints.py @@ -0,0 +1,119 @@ +import math +import sys +import matplotlib.pyplot as plt +import random + +''' +Purpose: This is a simple graphing tool to find the relationship between number of nodes + and number of constraints over time. + +To use: add the following code to publishGraph and disable the optimizer printouts + + std::map*> > vertices = mapper_->GetGraph()->GetVertices(); + std::vector*>::const_iterator it; + std::map vertex_ctr; + for (it = vertices[mapper_->GetMapperSensorManager()->GetSensorNames()[0]].begin(); it != vertices[mapper_->GetMapperSensorManager()->GetSensorNames()[0]].end(); ++it) + { + int num = (*it)->GetEdges().size(); + if (vertex_ctr.find(num) == vertex_ctr.end()) + { + vertex_ctr[num] = 1; + } + vertex_ctr[num]++; + } + + std::cout << "UpdateMap: Vertex count: " << std::endl; + std::map::const_iterator it2; + for (it2 = vertex_ctr.begin(); it2 != vertex_ctr.end(); ++it2) + { + std::cout << it2->first << " constraints are in " << it2->second << " vertexes" << std::endl; + } + std::cout << std::endl; +''' + +def readFileToList(filename): + with open(filename) as fp: + line = fp.readline() + lines = [] + lines.append(line) + while line: + line = fp.readline() + lines.append(line) + return lines + +def getSingleSets(lines): + measurements = [] + measurement = [] + for line in lines: + if line == "\n": + continue + if "UpdateMap: Vertex count:" in line: + measurements.append(measurement) + measurement = [] + measurement.append(line) + return measurements[1:] + +def processForData(measurements): + measurements_out = [] + measurement_out = [] + for measurement in measurements: + for m in measurement: + txt = m.split(" ") + nums = [r for r in txt if r.isdigit()] + if not nums: + continue + measurement_out.append(nums) + measurements_out.append(measurement_out) + measurement_out = [] + return measurements_out + +def plotData(data): + #plot lines + + # give us the number of lines to create and unque items + max_size = 0 + keys = [] + for d in data: + for m in d: + if m[0] not in keys: + keys.append(m[0]) + + data_len = len(data) + + dat = {} + for k in keys: + dat[k] = [] + + for d in data: + local_keys = [] + for pt in d: + local_keys.append(pt[0]) + dat[pt[0]].append(int(pt[1])) + for k in keys: + if k not in local_keys: + dat[k].append(0) + + total_nodes = [] + for d in data: + summ = 0 + for pt in d: + summ = summ + int(pt[1]) + total_nodes.append(summ) + + for k in keys: + plt.plot( [10*i for i in range(0, len(dat[k]))], dat[k], marker='o', color=[random.random(),random.random(),random.random()], linewidth=2, label=k+' Constraints') + + plt.plot( [10*i for i in range(0, len(dat[k]))], total_nodes, marker='o', color=[random.random(),random.random(),random.random()], linewidth=2, label='Total Num. Nodes') + plt.legend() + plt.xlabel("time (s)") + plt.ylabel("Node count") + #plt.yscale("log") + plt.show() + +if __name__ == "__main__": + filename = sys.argv[1] + print ("reading file: " + filename) + listOfContents = readFileToList(filename) + listOfListOfMeasurements = getSingleSets(listOfContents) + data = processForData(listOfListOfMeasurements) + plotData(data) diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox_msgs/CMakeLists.txt b/Localizations/Packages/slam_toolbox/slam_toolbox_msgs/CMakeLists.txt new file mode 100644 index 0000000..b434c29 --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox_msgs/CMakeLists.txt @@ -0,0 +1,25 @@ +cmake_minimum_required(VERSION 3.5) +project(slam_toolbox_msgs) + +set(MSG_DEPS std_srvs std_msgs geometry_msgs) + +find_package(catkin REQUIRED COMPONENTS message_generation ${MSG_DEPS}) + +add_service_files(DIRECTORY srv + FILES + Pause.srv + ClearQueue.srv + ToggleInteractive.srv + Clear.srv + SaveMap.srv + LoopClosure.srv + MergeMaps.srv + AddSubmap.srv + DeserializePoseGraph.srv + SerializePoseGraph.srv + Reset.srv +) + +generate_messages(DEPENDENCIES ${MSG_DEPS}) + +catkin_package(CATKIN_DEPENDS message_runtime ${MSG_DEPS}) diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox_msgs/package.xml b/Localizations/Packages/slam_toolbox/slam_toolbox_msgs/package.xml new file mode 100644 index 0000000..a12063b --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox_msgs/package.xml @@ -0,0 +1,24 @@ + + slam_toolbox_msgs + + This package provides a sped up improved slam karto with updated SDK and visualization and modification toolsets + + 1.5.7 + Steve Macenski + Steve Macenski + LGPL + + catkin + + message_generation + + std_msgs + std_srvs + geometry_msgs + + message_runtime + + + + + diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox_msgs/srv/AddSubmap.srv b/Localizations/Packages/slam_toolbox/slam_toolbox_msgs/srv/AddSubmap.srv new file mode 100644 index 0000000..8111179 --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox_msgs/srv/AddSubmap.srv @@ -0,0 +1,2 @@ +string filename +--- \ No newline at end of file diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox_msgs/srv/Clear.srv b/Localizations/Packages/slam_toolbox/slam_toolbox_msgs/srv/Clear.srv new file mode 100644 index 0000000..38ae740 --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox_msgs/srv/Clear.srv @@ -0,0 +1,3 @@ + + +--- \ No newline at end of file diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox_msgs/srv/ClearQueue.srv b/Localizations/Packages/slam_toolbox/slam_toolbox_msgs/srv/ClearQueue.srv new file mode 100644 index 0000000..beeb12e --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox_msgs/srv/ClearQueue.srv @@ -0,0 +1,3 @@ + +--- +bool status \ No newline at end of file diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox_msgs/srv/DeserializePoseGraph.srv b/Localizations/Packages/slam_toolbox/slam_toolbox_msgs/srv/DeserializePoseGraph.srv new file mode 100644 index 0000000..480fea9 --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox_msgs/srv/DeserializePoseGraph.srv @@ -0,0 +1,12 @@ +int8 UNSET = 0 +int8 START_AT_FIRST_NODE = 1 +int8 START_AT_GIVEN_POSE = 2 +int8 LOCALIZE_AT_POSE = 3 + +# inital_pose should be Map -> base_frame (parameter, generally base_link) +# + +string filename +int8 match_type +geometry_msgs/Pose2D initial_pose +--- \ No newline at end of file diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox_msgs/srv/LoopClosure.srv b/Localizations/Packages/slam_toolbox/slam_toolbox_msgs/srv/LoopClosure.srv new file mode 100644 index 0000000..121cc31 --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox_msgs/srv/LoopClosure.srv @@ -0,0 +1,2 @@ + +--- \ No newline at end of file diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox_msgs/srv/MergeMaps.srv b/Localizations/Packages/slam_toolbox/slam_toolbox_msgs/srv/MergeMaps.srv new file mode 100644 index 0000000..121cc31 --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox_msgs/srv/MergeMaps.srv @@ -0,0 +1,2 @@ + +--- \ No newline at end of file diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox_msgs/srv/Pause.srv b/Localizations/Packages/slam_toolbox/slam_toolbox_msgs/srv/Pause.srv new file mode 100644 index 0000000..24054a6 --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox_msgs/srv/Pause.srv @@ -0,0 +1,4 @@ +# trigger pause toggle + +--- +bool status \ No newline at end of file diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox_msgs/srv/Reset.srv b/Localizations/Packages/slam_toolbox/slam_toolbox_msgs/srv/Reset.srv new file mode 100644 index 0000000..5e111f6 --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox_msgs/srv/Reset.srv @@ -0,0 +1,8 @@ +# Set this to 'true' to pause new measurements immediately after reset. +# Note: This is a set behaviour and not a toggle behaviour, contrary to Pause.srv service. +bool pause_new_measurements +--- +# Result code defintions +uint8 RESULT_SUCCESS=0 + +uint8 result diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox_msgs/srv/SaveMap.srv b/Localizations/Packages/slam_toolbox/slam_toolbox_msgs/srv/SaveMap.srv new file mode 100644 index 0000000..f383513 --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox_msgs/srv/SaveMap.srv @@ -0,0 +1,2 @@ +std_msgs/String name +--- diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox_msgs/srv/SerializePoseGraph.srv b/Localizations/Packages/slam_toolbox/slam_toolbox_msgs/srv/SerializePoseGraph.srv new file mode 100644 index 0000000..8111179 --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox_msgs/srv/SerializePoseGraph.srv @@ -0,0 +1,2 @@ +string filename +--- \ No newline at end of file diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox_msgs/srv/ToggleInteractive.srv b/Localizations/Packages/slam_toolbox/slam_toolbox_msgs/srv/ToggleInteractive.srv new file mode 100644 index 0000000..121cc31 --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox_msgs/srv/ToggleInteractive.srv @@ -0,0 +1,2 @@ + +--- \ No newline at end of file diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox_rviz/CMakeLists.txt b/Localizations/Packages/slam_toolbox/slam_toolbox_rviz/CMakeLists.txt new file mode 100644 index 0000000..dff8477 --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox_rviz/CMakeLists.txt @@ -0,0 +1,58 @@ +cmake_minimum_required(VERSION 3.5) +project(slam_toolbox_rviz) + +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +find_package(catkin REQUIRED + COMPONENTS + rviz + pluginlib + slam_toolbox_msgs +) +include_directories(${catkin_INCLUDE_DIRS}) + +add_definitions(-DQT_NO_KEYWORDS) + + +#include_directories(${EIGEN3_INCLUDE_DIRS}) +#add_definitions(${EIGEN3_DEFINITIONS}) + +catkin_package( + INCLUDE_DIRS + ${EIGEN3_INCLUDE_DIRS} + CATKIN_DEPENDS + rviz + slam_toolbox_msgs +) + +find_package(Qt5 REQUIRED Core Widgets) +set(QT_LIBRARIES Qt5::Widgets) +add_definitions(-DQT_NO_KEYWORDS) + +set(CMAKE_INCLUDE_CURRENT_DIR ON) +include_directories(${CMAKE_CURRENT_BINARY_DIR}) + +set(SOURCE_FILES + src/slam_toolbox_rviz_plugin.cpp +) + +qt_wrap_cpp(${PROJECT_NAME} MOC_FILES + src/slam_toolbox_rviz_plugin.h +) + +add_library(${PROJECT_NAME} ${SOURCE_FILES} ${MOC_FILES}) +target_link_libraries(${PROJECT_NAME} ${rviz_DEFAULT_PLUGIN_LIBRARIES} ${QT_LIBRARIES} ${catkin_LIBRARIES}) + +#### Install +install(TARGETS ${PROJECT_NAME} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +) + +install(FILES rviz_plugins.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +add_dependencies(slam_toolbox_rviz slam_toolbox_msgs_generate_messages_cpp) diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox_rviz/package.xml b/Localizations/Packages/slam_toolbox/slam_toolbox_rviz/package.xml new file mode 100644 index 0000000..6efb611 --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox_rviz/package.xml @@ -0,0 +1,27 @@ + + slam_toolbox_rviz + + This package provides a sped up improved slam karto with updated SDK and visualization and modification toolsets + + 1.5.7 + Steve Macenski + Steve Macenski + LGPL + + catkin + + rviz + slam_toolbox_msgs + qtbase5-dev + + + libqt5-core + libqt5-gui + libqt5-opengl + libqt5-widgets + + + + + + diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox_rviz/rviz_plugins.xml b/Localizations/Packages/slam_toolbox/slam_toolbox_rviz/rviz_plugins.xml new file mode 100644 index 0000000..38a62b6 --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox_rviz/rviz_plugins.xml @@ -0,0 +1,9 @@ + + + + Panel to assist in Mapping and SLAM with the slam_toolbox + + + diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox_rviz/src/slam_toolbox_rviz_plugin.cpp b/Localizations/Packages/slam_toolbox/slam_toolbox_rviz/src/slam_toolbox_rviz_plugin.cpp new file mode 100644 index 0000000..f2f4eee --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox_rviz/src/slam_toolbox_rviz_plugin.cpp @@ -0,0 +1,497 @@ +/* + * slam_toolbox + * Copyright (c) 2018, Simbe Robotics, Inc. + * + * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE + * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY + * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS + * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED. + * + * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO + * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS + * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND + * CONDITIONS. + * + */ + +/* Author: Steven Macenski */ + +// Header +#include "slam_toolbox_rviz_plugin.h" +// QT +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// ROS +#include +#include +PLUGINLIB_EXPORT_CLASS(slam_toolbox::SlamToolboxPlugin, rviz::Panel) + +namespace slam_toolbox +{ + +/*****************************************************************************/ +SlamToolboxPlugin::SlamToolboxPlugin(QWidget* parent): + rviz::Panel(parent), + _thread(NULL), + _match_type(PROCESS_FIRST_NODE_CMT) +/*****************************************************************************/ +{ + ros::NodeHandle nh; + bool paused_measure = false, interactive = false; + nh.getParam("/slam_toolbox/paused_new_measurements", paused_measure); + nh.getParam("/slam_toolbox/interactive_mode", interactive); + _serialize = nh.serviceClient("/slam_toolbox/serialize_map"); + _load_map = nh.serviceClient("/slam_toolbox/deserialize_map"); + _clearChanges = nh.serviceClient("/slam_toolbox/clear_changes"); + _saveChanges = nh.serviceClient("/slam_toolbox/manual_loop_closure"); + _saveMap = nh.serviceClient("/slam_toolbox/save_map"); + _clearQueue = nh.serviceClient("/slam_toolbox/clear_queue"); + _interactive = nh.serviceClient("/slam_toolbox/toggle_interactive_mode"); + _pause_measurements = nh.serviceClient("/slam_toolbox/pause_new_measurements"); + _load_submap_for_merging = nh.serviceClient("/map_merging/add_submap"); + _merge = nh.serviceClient("/map_merging/merge_submaps"); + + _initialposeSub = nh.subscribe("/initialpose", 10, &SlamToolboxPlugin::InitialPoseCallback, this); + + _vbox = new QVBoxLayout(); + _hbox1 = new QHBoxLayout(); + _hbox2 = new QHBoxLayout(); + _hbox3 = new QHBoxLayout(); + _hbox4 = new QHBoxLayout(); + _hbox5 = new QHBoxLayout(); + _hbox6 = new QHBoxLayout(); + _hbox7 = new QHBoxLayout(); + _hbox8 = new QHBoxLayout(); + _hbox9 = new QHBoxLayout(); + _hbox10 = new QHBoxLayout(); + + QFrame* _line = new QFrame(); + _line->setFrameShape(QFrame::HLine); + _line->setFrameShadow(QFrame::Sunken); + + _button1 = new QPushButton(this); + _button1->setText("Clear Changes"); + connect(_button1, SIGNAL(clicked()), this, SLOT(ClearChanges())); + _button2 = new QPushButton(this); + _button2->setText("Save Changes"); + connect(_button2, SIGNAL(clicked()), this, SLOT(SaveChanges())); + _button3 = new QPushButton(this); + _button3->setText("Save Map"); + connect(_button3, SIGNAL(clicked()), this, SLOT(SaveMap())); + _button4 = new QPushButton(this); + _button4->setText("Clear Measurement Queue"); + connect(_button4, SIGNAL(clicked()), this, SLOT(ClearQueue())); + _button5 = new QPushButton(this); + _button5->setText("Add Submap"); + connect(_button5, SIGNAL(clicked()), this, SLOT(LoadSubmap())); + _button6 = new QPushButton(this); + _button6->setText("Generate Map"); + connect(_button6, SIGNAL(clicked()), this, SLOT(GenerateMap())); + _button7 = new QPushButton(this); + _button7->setText("Serialize Map"); + connect(_button7, SIGNAL(clicked()), this, SLOT(SerializeMap())); + _button8 = new QPushButton(this); + _button8->setText("Deserialize Map"); + connect(_button8, SIGNAL(clicked()), this, SLOT(DeserializeMap())); + + _label1 = new QLabel(this); + _label1->setText("Interactive Mode"); + _label2 = new QLabel(this); + _label2->setText("Accept New Scans"); + _label4 = new QLabel(this); + _label4->setText("Merge Map Tool"); + _label4->setAlignment(Qt::AlignCenter); + _label5 = new QLabel(this); + _label5->setText("Create Map Tool"); + _label5->setAlignment(Qt::AlignCenter); + _label6 = new QLabel(this); + _label6->setText("X"); + _label6->setAlignment(Qt::AlignCenter); + _label7 = new QLabel(this); + _label7->setText("Y"); + _label7->setAlignment(Qt::AlignCenter); + _label8 = new QLabel(this); + _label8->setText("θ"); + _label8->setAlignment(Qt::AlignCenter); + + _check1 = new QCheckBox(); + _check1->setChecked(interactive); + connect(_check1, SIGNAL(stateChanged(int)), this, SLOT(InteractiveCb(int))); + _check2 = new QCheckBox(); + _check2->setChecked(!paused_measure); + connect(_check2, SIGNAL(stateChanged(int)), this, SLOT(PauseMeasurementsCb(int))); + _radio1 = new QRadioButton(tr("Start At Dock")); + _radio1->setChecked(true); + _radio2 = new QRadioButton(tr("Start At Pose Est.")); + _radio3 = new QRadioButton(tr("Start At Curr. Odom")); + _radio4 = new QRadioButton(tr("Localize")); + + connect(_radio1, SIGNAL(clicked()), this, SLOT(FirstNodeMatchCb())); + connect(_radio2, SIGNAL(clicked()), this, SLOT(PoseEstMatchCb())); + connect(_radio3, SIGNAL(clicked()), this, SLOT(CurEstMatchCb())); + connect(_radio4, SIGNAL(clicked()), this, SLOT(LocalizeCb())); + + _line1 = new QLineEdit(); + _line2 = new QLineEdit(); + _line3 = new QLineEdit(); + _line4 = new QLineEdit(); + _line5 = new QLineEdit(); + _line6 = new QLineEdit(); + _line7 = new QLineEdit(); + + _button1->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); + _button2->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); + _button3->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); + _button4->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); + _button5->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); + _button6->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); + _button7->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); + _button8->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); + _check1->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); + _check2->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); + _line1->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); + _line2->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); + _line3->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); + _line4->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); + _line5->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); + _line6->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); + _line7->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); + + _hbox1->addWidget(_check1); + _hbox1->addWidget(_label1); + _hbox1->addWidget(_check2); + _hbox1->addWidget(_label2); + + _hbox2->addWidget(_button1); + _hbox2->addWidget(_button2); + + _hbox3->addWidget(_button3); + _hbox3->addWidget(_line1); + + _hbox4->addWidget(_button4); + + _hbox5->addWidget(_button5); + _hbox5->addWidget(_line2); + + _hbox6->addWidget(_button6); + + _hbox7->addWidget(_button7); + _hbox7->addWidget(_line3); + + _hbox8->addWidget(_button8); + _hbox8->addWidget(_line4); + + _hbox9->addWidget(_radio1); + _hbox9->addWidget(_radio2); + _hbox9->addWidget(_radio3); + _hbox9->addWidget(_radio4); + _hbox9->addStretch(1); + + _hbox10->addWidget(_label6); + _hbox10->addWidget(_line5); + _hbox10->addWidget(_label7); + _hbox10->addWidget(_line6); + _hbox10->addWidget(_label8); + _hbox10->addWidget(_line7); + + _vbox->addWidget(_label5); + _vbox->addLayout(_hbox1); + _vbox->addLayout(_hbox2); + _vbox->addLayout(_hbox3); + _vbox->addLayout(_hbox7); + _vbox->addLayout(_hbox8); + _vbox->addLayout(_hbox9); + _vbox->addLayout(_hbox10); + _vbox->addLayout(_hbox4); + _vbox->addWidget(_line); + _vbox->addWidget(_label4); + _vbox->addLayout(_hbox5); + _vbox->addLayout(_hbox6); + + setLayout(_vbox); + + _thread = new std::thread(&SlamToolboxPlugin::updateCheckStateIfExternalChange, this); +} + +/*****************************************************************************/ +SlamToolboxPlugin::~SlamToolboxPlugin() +/*****************************************************************************/ +{ + if (_thread) + { + delete _thread; + } +} + +/*****************************************************************************/ +void SlamToolboxPlugin::InitialPoseCallback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg) +/*****************************************************************************/ +{ + _match_type = PROCESS_NEAR_REGION_CMT; + ROS_INFO("Setting initial pose from rviz; you can now deserialize a map given that pose."); + _radio2->setChecked(true); + _line5->setText(QString::number(msg->pose.pose.position.x, 'f', 2)); + _line6->setText(QString::number(msg->pose.pose.position.y, 'f', 2)); + tf::Quaternion q( + msg->pose.pose.orientation.x, + msg->pose.pose.orientation.y, + msg->pose.pose.orientation.z, + msg->pose.pose.orientation.w); + tf::Matrix3x3 m(q); + double roll, pitch, yaw; + m.getRPY(roll, pitch, yaw); + _line7->setText(QString::number(yaw, 'f', 2)); +} + +/*****************************************************************************/ +void SlamToolboxPlugin::SerializeMap() +/*****************************************************************************/ +{ + slam_toolbox_msgs::SerializePoseGraph msg; + msg.request.filename = _line3->text().toStdString(); + if (!_serialize.call(msg)) + { + ROS_WARN("SlamToolbox: Failed to serialize pose graph to file, is service running?"); + } +} + +/*****************************************************************************/ +void SlamToolboxPlugin::DeserializeMap() +/*****************************************************************************/ +{ + typedef slam_toolbox_msgs::DeserializePoseGraph::Request procType; + + slam_toolbox_msgs::DeserializePoseGraph msg; + msg.request.filename = _line4->text().toStdString(); + if (_match_type == PROCESS_FIRST_NODE_CMT) + { + msg.request.match_type = procType::START_AT_FIRST_NODE; + } + else if (_match_type == PROCESS_NEAR_REGION_CMT) + { + try + { + msg.request.match_type = procType::START_AT_GIVEN_POSE; + msg.request.initial_pose.x = std::stod(_line5->text().toStdString()); + msg.request.initial_pose.y = std::stod(_line6->text().toStdString()); + msg.request.initial_pose.theta = std::stod(_line7->text().toStdString()); + } + catch (const std::invalid_argument& ia) + { + ROS_WARN("Initial pose invalid."); + return; + } + } + else if (_match_type == LOCALIZE_CMT) + { + try + { + msg.request.match_type = procType::LOCALIZE_AT_POSE; + msg.request.initial_pose.x = std::stod(_line5->text().toStdString()); + msg.request.initial_pose.y = std::stod(_line6->text().toStdString()); + msg.request.initial_pose.theta = std::stod(_line7->text().toStdString()); + } + catch (const std::invalid_argument& ia) + { + ROS_WARN("Initial pose invalid."); + return; + } + } + else + { + ROS_WARN("No match type selected, cannot send request."); + return; + } + if (!_load_map.call(msg)) + { + ROS_WARN("SlamToolbox: Failed to deserialize mapper object " + "from file, is service running?"); + } +} + +/*****************************************************************************/ +void SlamToolboxPlugin::LoadSubmap() +/*****************************************************************************/ +{ + slam_toolbox_msgs::AddSubmap msg; + msg.request.filename = _line2->text().toStdString(); + if (!_load_submap_for_merging.call(msg)) + { + ROS_WARN("MergeMaps: Failed to load pose graph from file, is service running?"); + } +} +/*****************************************************************************/ +void SlamToolboxPlugin::GenerateMap() +/*****************************************************************************/ +{ + slam_toolbox_msgs::MergeMaps msg; + if (!_merge.call(msg)) + { + ROS_WARN("MergeMaps: Failed to merge maps, is service running?"); + } +} + +/*****************************************************************************/ +void SlamToolboxPlugin::ClearChanges() +/*****************************************************************************/ +{ + slam_toolbox_msgs::Clear msg; + if (!_clearChanges.call(msg)) + { + ROS_WARN("SlamToolbox: Failed to clear changes, is service running?"); + } +} + +/*****************************************************************************/ +void SlamToolboxPlugin::SaveChanges() +/*****************************************************************************/ +{ + slam_toolbox_msgs::LoopClosure msg; + + if (!_saveChanges.call(msg)) + { + ROS_WARN("SlamToolbox: Failed to save changes, is service running?"); + } +} + +/*****************************************************************************/ +void SlamToolboxPlugin::SaveMap() +/*****************************************************************************/ +{ + slam_toolbox_msgs::SaveMap msg; + msg.request.name.data = _line1->text().toStdString(); + if (!_saveMap.call(msg)) + { + ROS_WARN("SlamToolbox: Failed to save map as %s, is service running?", + msg.request.name.data.c_str()); + } +} + +/*****************************************************************************/ +void SlamToolboxPlugin::ClearQueue() +/*****************************************************************************/ +{ + slam_toolbox_msgs::ClearQueue msg; + if (!_clearQueue.call(msg)) + { + ROS_WARN("Failed to clear queue, is service running?"); + } +} + +/*****************************************************************************/ +void SlamToolboxPlugin::InteractiveCb(int state) +/*****************************************************************************/ +{ + slam_toolbox_msgs::ToggleInteractive msg; + if (!_interactive.call(msg)) + { + ROS_WARN("SlamToolbox: Failed to toggle interactive mode, is service running?"); + } +} + +/*****************************************************************************/ +void SlamToolboxPlugin::PauseMeasurementsCb(int state) +/*****************************************************************************/ +{ + slam_toolbox_msgs::Pause msg; + if (!_pause_measurements.call(msg)) + { + ROS_WARN("SlamToolbox: Failed to toggle pause measurements, is service running?"); + } +} + +/*****************************************************************************/ +void SlamToolboxPlugin::FirstNodeMatchCb() +/*****************************************************************************/ +{ + if (_radio1->isChecked() == Qt::Unchecked) + { + return; + } + else + { + _match_type = PROCESS_FIRST_NODE_CMT; + ROS_INFO("Processing at first node selected."); + } +} + +/*****************************************************************************/ +void SlamToolboxPlugin::PoseEstMatchCb() +/*****************************************************************************/ +{ + if (_radio2->isChecked() == Qt::Unchecked) + { + return; + } + else + { + _match_type = PROCESS_NEAR_REGION_CMT; + ROS_INFO("Processing at current pose estimate selected."); + } +} + +/*****************************************************************************/ +void SlamToolboxPlugin::CurEstMatchCb() +/*****************************************************************************/ +{ + if (_radio3->isChecked() == Qt::Unchecked) + { + return; + } + else + { + _match_type = PROCESS_CMT; + ROS_INFO("Processing at current odometry selected."); + } +} + + +/*****************************************************************************/ +void SlamToolboxPlugin::LocalizeCb() +/*****************************************************************************/ +{ + if (_radio4->isChecked() == Qt::Unchecked) + { + return; + } + else + { + _match_type = LOCALIZE_CMT; + ROS_INFO("Processing localization selected."); + } +} + + +/*****************************************************************************/ +void SlamToolboxPlugin::updateCheckStateIfExternalChange() +/*****************************************************************************/ +{ + ros::Rate r(1); //1 hz + ros::NodeHandle nh; + bool paused_measure = false, interactive = false; + while (ros::ok()) + { + nh.getParam("/slam_toolbox/paused_new_measurements", paused_measure); + nh.getParam("/slam_toolbox/interactive_mode", interactive); + + bool oldState = _check1->blockSignals(true); + _check1->setChecked(interactive); + _check1->blockSignals(oldState); + + oldState = _check2->blockSignals(true); + _check2->setChecked(!paused_measure); + _check2->blockSignals(oldState); + + r.sleep(); + } +} + +} // end namespace diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox_rviz/src/slam_toolbox_rviz_plugin.h b/Localizations/Packages/slam_toolbox/slam_toolbox_rviz/src/slam_toolbox_rviz_plugin.h new file mode 100644 index 0000000..a959a9c --- /dev/null +++ b/Localizations/Packages/slam_toolbox/slam_toolbox_rviz/src/slam_toolbox_rviz_plugin.h @@ -0,0 +1,159 @@ +/* + * slam_toolbox + * Copyright (c) 2018, Simbe Robotics, Inc. + * + * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE + * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY + * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS + * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED. + * + * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO + * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS + * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND + * CONDITIONS. + * + */ + +/* Author: Steven Macenski */ + +#ifndef SLAM_TOOLBOX_PANEL_H +#define SLAM_TOOLBOX_PANEL_H + +// ROS +#include +#include +// STL +#include +#include +// QT +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +// msgs +#include +#include "slam_toolbox_msgs/AddSubmap.h" +#include "slam_toolbox_msgs/Clear.h" +#include "slam_toolbox_msgs/ClearQueue.h" +#include "slam_toolbox_msgs/DeserializePoseGraph.h" +#include "slam_toolbox_msgs/LoopClosure.h" +#include "slam_toolbox_msgs/MergeMaps.h" +#include "slam_toolbox_msgs/Pause.h" +#include "slam_toolbox_msgs/SaveMap.h" +#include "slam_toolbox_msgs/SerializePoseGraph.h" +#include "slam_toolbox_msgs/ToggleInteractive.h" + +class QLineEdit; +class QSpinBox; +class QComboBox; + +#include + +namespace slam_toolbox +{ + +enum ContinueMappingType +{ + PROCESS_CMT = 0, + PROCESS_FIRST_NODE_CMT = 1, + PROCESS_NEAR_REGION_CMT = 2, + LOCALIZE_CMT = 3 +}; + +class SlamToolboxPlugin : public rviz::Panel +{ + Q_OBJECT + +public: + SlamToolboxPlugin(QWidget* parent = 0); + ~SlamToolboxPlugin(); + +public Q_SLOTS: +protected Q_SLOTS: + void ClearChanges(); + void SaveChanges(); + void SaveMap(); + void ClearQueue(); + void InteractiveCb(int state); + void PauseMeasurementsCb(int state); + void FirstNodeMatchCb(); + void PoseEstMatchCb(); + void CurEstMatchCb(); + void LocalizeCb(); + void LoadSubmap(); + void GenerateMap(); + void SerializeMap(); + void DeserializeMap(); + + void updateCheckStateIfExternalChange(); + +protected: + QVBoxLayout* _vbox; + QHBoxLayout* _hbox1; + QHBoxLayout* _hbox2; + QHBoxLayout* _hbox3; + QHBoxLayout* _hbox4; + QHBoxLayout* _hbox5; + QHBoxLayout* _hbox6; + QHBoxLayout* _hbox7; + QHBoxLayout* _hbox8; + QHBoxLayout* _hbox9; + QHBoxLayout* _hbox10; + + QPushButton* _button1; + QPushButton* _button2; + QPushButton* _button3; + QPushButton* _button4; + QPushButton* _button5; + QPushButton* _button6; + QPushButton* _button7; + QPushButton* _button8; + + QLineEdit* _line1; + QLineEdit* _line2; + QLineEdit* _line3; + QLineEdit* _line4; + QLineEdit* _line5; + QLineEdit* _line6; + QLineEdit* _line7; + + QCheckBox* _check1; + QCheckBox* _check2; + + QRadioButton* _radio1; + QRadioButton* _radio2; + QRadioButton* _radio3; + QRadioButton* _radio4; + + QLabel* _label1; + QLabel* _label2; + QLabel* _label4; + QLabel* _label5; + QLabel* _label6; + QLabel* _label7; + QLabel* _label8; + + QFrame* _line; + + ros::ServiceClient _clearChanges, _saveChanges, _saveMap, _clearQueue, _interactive, _pause_measurements, _load_submap_for_merging, _merge, _serialize, _load_map; + ros::Subscriber _initialposeSub; + + void InitialPoseCallback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& pose); + + std::thread* _thread; + + ContinueMappingType _match_type; +}; + +} // end namespace + +#endif diff --git a/Localizations/auto_setup.sh b/Localizations/auto_setup.sh new file mode 100755 index 0000000..f7932aa --- /dev/null +++ b/Localizations/auto_setup.sh @@ -0,0 +1,6 @@ +sudo apt-get -y install libsdl-dev +sudo apt-get -y install libsdl-image1.2-dev +sudo apt-get -y install ros-noetic-sparse-bundle-adjustment +sudo apt-get -y install libceres-dev +sudo apt-get -y install ros-noetic-map-server +sudo apt-get install -y ros-noetic-eigen-conversions diff --git a/Managerments/.$diff_drive_c.drawio.bkp b/Managerments/.$diff_drive_c.drawio.bkp new file mode 100755 index 0000000..6249490 --- /dev/null +++ b/Managerments/.$diff_drive_c.drawio.bkp @@ -0,0 +1,1606 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Managerments/CMakeLists.txt b/Managerments/CMakeLists.txt new file mode 100755 index 0000000..e428ca0 --- /dev/null +++ b/Managerments/CMakeLists.txt @@ -0,0 +1,201 @@ +cmake_minimum_required(VERSION 3.0.2) +project(managerments) + +## Compile as C++11, supported in ROS Kinetic and newer +# add_compile_options(-std=c++11) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a exec_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# std_msgs # Or other packages containing msgs +# ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( + INCLUDE_DIRS maps +# LIBRARIES managerments +# CATKIN_DEPENDS other_catkin_pkg +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( +# include +# ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +# add_library(${PROJECT_NAME} +# src/${PROJECT_NAME}/managerments.cpp +# ) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +## With catkin_make all packages are built within a single CMake context +## The recommended prefix ensures that target names across packages don't collide +# add_executable(${PROJECT_NAME}_node src/managerments_node.cpp) + +## Rename C++ executable without prefix +## The above recommended prefix causes long target names, the following renames the +## target back to the shorter version for ease of user use +## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" +# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +# target_link_libraries(${PROJECT_NAME}_node +# ${catkin_LIBRARIES} +# ) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# catkin_install_python(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html +# install(TARGETS ${PROJECT_NAME}_node +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark libraries for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html +# install(TARGETS ${PROJECT_NAME} +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +install(DIRECTORY + maps + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_managerments.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/Managerments/diff_drive_c.drawio b/Managerments/diff_drive_c.drawio new file mode 100755 index 0000000..b9be597 --- /dev/null +++ b/Managerments/diff_drive_c.drawio @@ -0,0 +1,1282 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Managerments/maps/505_room/505_room.pgm b/Managerments/maps/505_room/505_room.pgm new file mode 100755 index 0000000..5ecbcc4 Binary files /dev/null and b/Managerments/maps/505_room/505_room.pgm differ diff --git a/Managerments/maps/505_room/505_room.yaml b/Managerments/maps/505_room/505_room.yaml new file mode 100755 index 0000000..2be8e78 --- /dev/null +++ b/Managerments/maps/505_room/505_room.yaml @@ -0,0 +1,7 @@ +image: 505_room.pgm +resolution: 0.050000 +origin: [-1.723462, -5.370545, 0.000000] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 + diff --git a/Managerments/maps/A9A10/A9A10.png b/Managerments/maps/A9A10/A9A10.png new file mode 100644 index 0000000..7106bdf Binary files /dev/null and b/Managerments/maps/A9A10/A9A10.png differ diff --git a/Managerments/maps/A9A10/A9A10.yaml b/Managerments/maps/A9A10/A9A10.yaml new file mode 100644 index 0000000..37f717c --- /dev/null +++ b/Managerments/maps/A9A10/A9A10.yaml @@ -0,0 +1,6 @@ +image: A9A10.png +resolution: 0.05 +origin: [-58.885, -59.015, 0.0] +negate: 0 +occupied_thresh: 0.55 +free_thresh: 0.45 diff --git a/Managerments/maps/maze/maze.png b/Managerments/maps/maze/maze.png new file mode 100755 index 0000000..41d6fa1 Binary files /dev/null and b/Managerments/maps/maze/maze.png differ diff --git a/Managerments/maps/maze/maze.yaml b/Managerments/maps/maze/maze.yaml new file mode 100755 index 0000000..79220e7 --- /dev/null +++ b/Managerments/maps/maze/maze.yaml @@ -0,0 +1,6 @@ +image: maze.png +resolution: 0.05 +origin: [0.0, 0.0, 0.0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 diff --git a/Managerments/maps/maze/maze_virtual_walls.png b/Managerments/maps/maze/maze_virtual_walls.png new file mode 100755 index 0000000..e5dd8bb Binary files /dev/null and b/Managerments/maps/maze/maze_virtual_walls.png differ diff --git a/Managerments/maps/maze/maze_virtual_walls.yaml b/Managerments/maps/maze/maze_virtual_walls.yaml new file mode 100755 index 0000000..89320ac --- /dev/null +++ b/Managerments/maps/maze/maze_virtual_walls.yaml @@ -0,0 +1,6 @@ +image: maze_virtual_walls.png +resolution: 0.05 +origin: [0.0, 0.0, 0.0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 diff --git a/Managerments/maps/sehc_factory/sehc_factory.pgm b/Managerments/maps/sehc_factory/sehc_factory.pgm new file mode 100644 index 0000000..545a4c3 Binary files /dev/null and b/Managerments/maps/sehc_factory/sehc_factory.pgm differ diff --git a/Managerments/maps/sehc_factory/sehc_factory.png b/Managerments/maps/sehc_factory/sehc_factory.png new file mode 100644 index 0000000..bc12fb2 Binary files /dev/null and b/Managerments/maps/sehc_factory/sehc_factory.png differ diff --git a/Managerments/maps/sehc_factory/sehc_factory.yaml b/Managerments/maps/sehc_factory/sehc_factory.yaml new file mode 100644 index 0000000..a16ea1c --- /dev/null +++ b/Managerments/maps/sehc_factory/sehc_factory.yaml @@ -0,0 +1,7 @@ +image: sehc_factory.png +resolution: 0.050000 +origin: [-76.500015, -76.500015, 0.000000] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 + diff --git a/Managerments/maps/warehouse/critical_zones.png b/Managerments/maps/warehouse/critical_zones.png new file mode 100755 index 0000000..fb5a153 Binary files /dev/null and b/Managerments/maps/warehouse/critical_zones.png differ diff --git a/Managerments/maps/warehouse/critical_zones.yaml b/Managerments/maps/warehouse/critical_zones.yaml new file mode 100755 index 0000000..80a2da7 --- /dev/null +++ b/Managerments/maps/warehouse/critical_zones.yaml @@ -0,0 +1,7 @@ +image: critical_zones.png +resolution: 0.050000 +origin: [-51.224998, -51.224998, 0.000000] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 + diff --git a/Managerments/maps/warehouse/direction_zones.png b/Managerments/maps/warehouse/direction_zones.png new file mode 100755 index 0000000..4cead8a Binary files /dev/null and b/Managerments/maps/warehouse/direction_zones.png differ diff --git a/Managerments/maps/warehouse/direction_zones.yaml b/Managerments/maps/warehouse/direction_zones.yaml new file mode 100755 index 0000000..34d534a --- /dev/null +++ b/Managerments/maps/warehouse/direction_zones.yaml @@ -0,0 +1,7 @@ +image: direction_zones.png +resolution: 0.050000 +origin: [-51.224998, -51.224998, 0.000000] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 + diff --git a/Managerments/maps/warehouse/map_empty.png b/Managerments/maps/warehouse/map_empty.png new file mode 100755 index 0000000..e9cb54a Binary files /dev/null and b/Managerments/maps/warehouse/map_empty.png differ diff --git a/Managerments/maps/warehouse/map_empty.yaml b/Managerments/maps/warehouse/map_empty.yaml new file mode 100755 index 0000000..4045333 --- /dev/null +++ b/Managerments/maps/warehouse/map_empty.yaml @@ -0,0 +1,7 @@ +image: map_empty.png +resolution: 0.050000 +origin: [-51.224998, -51.224998, 0.000000] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 + diff --git a/Managerments/maps/warehouse/preferred_zones.yaml b/Managerments/maps/warehouse/preferred_zones.yaml new file mode 100755 index 0000000..7fc1e78 --- /dev/null +++ b/Managerments/maps/warehouse/preferred_zones.yaml @@ -0,0 +1,7 @@ +image: preferrred_zones.png +resolution: 0.050000 +origin: [-51.224998, -51.224998, 0.000000] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 + diff --git a/Managerments/maps/warehouse/preferrred_zones.png b/Managerments/maps/warehouse/preferrred_zones.png new file mode 100755 index 0000000..49ae274 Binary files /dev/null and b/Managerments/maps/warehouse/preferrred_zones.png differ diff --git a/Managerments/maps/warehouse/unpreferred_zones.yaml b/Managerments/maps/warehouse/unpreferred_zones.yaml new file mode 100755 index 0000000..5974235 --- /dev/null +++ b/Managerments/maps/warehouse/unpreferred_zones.yaml @@ -0,0 +1,7 @@ +image: unpreferrred_zones.png +resolution: 0.050000 +origin: [-51.224998, -51.224998, 0.000000] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 + diff --git a/Managerments/maps/warehouse/unpreferrred_zones.png b/Managerments/maps/warehouse/unpreferrred_zones.png new file mode 100755 index 0000000..88f8562 Binary files /dev/null and b/Managerments/maps/warehouse/unpreferrred_zones.png differ diff --git a/Managerments/maps/warehouse/virtual_walls.png b/Managerments/maps/warehouse/virtual_walls.png new file mode 100755 index 0000000..81346ab Binary files /dev/null and b/Managerments/maps/warehouse/virtual_walls.png differ diff --git a/Managerments/maps/warehouse/virtual_walls.yaml b/Managerments/maps/warehouse/virtual_walls.yaml new file mode 100755 index 0000000..9840d0d --- /dev/null +++ b/Managerments/maps/warehouse/virtual_walls.yaml @@ -0,0 +1,7 @@ +image: virtual_walls.png +resolution: 0.050000 +origin: [-51.224998, -51.224998, 0.000000] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 + diff --git a/Managerments/maps/warehouse/warehouse.pgm b/Managerments/maps/warehouse/warehouse.pgm new file mode 100755 index 0000000..01077c0 --- /dev/null +++ b/Managerments/maps/warehouse/warehouse.pgm @@ -0,0 +1,5 @@ +P5 +# CREATOR: map_saver.cpp 0.050 m/pix +2048 2048 +255 + \ No newline at end of file diff --git a/Managerments/maps/warehouse/warehouse.png b/Managerments/maps/warehouse/warehouse.png new file mode 100755 index 0000000..2069e81 Binary files /dev/null and b/Managerments/maps/warehouse/warehouse.png differ diff --git a/Managerments/maps/warehouse/warehouse.yaml b/Managerments/maps/warehouse/warehouse.yaml new file mode 100755 index 0000000..a394ba3 --- /dev/null +++ b/Managerments/maps/warehouse/warehouse.yaml @@ -0,0 +1,7 @@ +image: warehouse.png +resolution: 0.050000 +origin: [-51.224998, -51.224998, 0.000000] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 + diff --git a/Managerments/package.xml b/Managerments/package.xml new file mode 100755 index 0000000..2c00368 --- /dev/null +++ b/Managerments/package.xml @@ -0,0 +1,59 @@ + + + managerments + 0.0.0 + The managerments package + + + + + robotics + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + + + + + + + +